%% 多自由度系統瞬態響應分析,New-mark Beta，適用于非比例阻尼，非線性剛度，非線性阻尼;
%% Inputs:
% K Stiff matrix
% C Damping matrix, Structural Damping will be transformed to viscous
% damping.
% M Mass matrix
% fi_set Force excitation DOFs.
% Force Force matrix for every i_set
% R_set Output DOFs, Disp, Velo,Acc output in cell format.

%% mxl.2015-5-24
% 單位制不做規定；默認自由度序號為從1到N
% 在本程序中沒有考慮非線性剛度 K=K(t)，非線性阻尼C=C(t)這類問題，后續可以添加；
% 輸入默認為：x(0)=0,x'(0)=1;t 表示時間；
% 強迫位移，強迫速度和強迫加速度功能沒有考慮；
% 結構阻尼輸入的時候，轉換為等效粘性阻尼的功能還沒有添加；
% 輸出請求為位移，速度和加速度，不含應力；
clear
clc

dt=0.0001;
t=[0:dt:10]';% 延遲計算時間到15秒，可以看到明顯的數值阻尼
method='Newmark';% Runge-Kutta,Runge-Kutta
% M=0.2533;
% K0=10;
% C=0.592;
m1=2e2;m2=5e3;
k1=2e6;k2=1.5e6;
c1=1000;c2=2000;

M=diag([m1,m2]);
C=[
c1+c2,-c2;
-c2,c2;
];
K0=[
k1+k2,-k2;
-k2,k2;
];

fi_set=1;
Force=5*sin(pi*t/0.6);

N=size(M,1);

initial_disp=zeros(N,1);% You may change it.
initial_velo=[0;1];% You may change it.
initial_acc=[];% You may change it.

R_set=[1]';
% %---------------------------------------------------------------------%
% (1)Input check
% %---------------------------------------------------------------------%

if max(fi_set)>N
error('fi_set is wrongly set.');
end

if numel(fi_set)-size(Force,2)
error('Input force must keep in consistence.');
end

if isempty(dt)
error('You must specify or calculate time step.');
end

if strcmp(method,'Newmark')
if isempty(initial_disp)||isempty(initial_velo)
error('You must specify the initial states.');
end
end
if max(R_set)>N||isempty(R_set)
error('Response DOFs are badly set.')
end

%-----初始化輸出-----%
Nsteps=numel(t);
disp=zeros(Nsteps,numel(R_set));% 這是用來保存R_set 位移結果的；
velo=zeros(Nsteps,numel(R_set));% 這是用來保存R_set 速度結果的；
acc=zeros(Nsteps,numel(R_set));% 這是用來保存R_set 加速度結果的；
Fi=zeros(N,1);

update_disp=zeros(N,1);% 這是用來保存運算中全局位移結果的；
update_velo=zeros(N,1);% 這是用來保存運算中全局速度結果的；
update_acc=zeros(N,1);% 這是用來保存運算中全局加速度結果的；

Inv_M=inv(M);
% %---------------------------------------------------------------------%
% (2)New-Mark Beta
% gama=0.5;0.1667<=beta<=0.25,beta=0.25 is suggested.
% %---------------------------------------------------------------------%
gama=0.5;beta=0.25;
disp(['Gama= ',num2str(gama),'Beta= ',num2str(beta)]);
a2=1/beta/dt;a0=a2/dt;a1=a2*gama;
a3=1/2/beta-1;a4=gama/beta-1;a5=dt/2*(a4-1);

% if strcmp(method,'Newmark')
for loop=1:Nsteps
if loop>1

disp(loop,:)=initial_disp(R_set,1)';% 保存請求的計算結果
velo(loop,:)=initial_velo(R_set,1)';
acc(loop,:)=initial_acc(R_set,1)';
if loop==Nsteps
break;
end
%---we may update the K in the line in nonlinear problem----%
% K=Ki+a0*M+a1*C;

Fi=zeros(N,1);
for loopf=1:numel(fi_set)
Fi(fi_set(loopf),1)=Force(loop,loopf);
end
Fi=Fi+M*(a0*initial_disp+a2*initial_velo+a3*initial_acc)+...
C*(a1*initial_disp+a4*initial_velo+a5*initial_acc);

update_disp=K\Fi;
update_velo=a1*(update_disp-initial_disp)-a4*initial_velo-a5*initial_acc;
update_acc =a0*(update_disp-initial_disp)-a2*initial_velo-a3*initial_acc;

initial_disp=update_disp;% 將本次結果保存，下一次迭代時作為初值
initial_velo=update_velo;
initial_acc =update_acc;
else
for loopf=1:numel(fi_set)
Fi(fi_set(loopf),1)=Force(loop,loopf);
end

disp(loop,:)=initial_disp(R_set,1)';
velo(loop,:)=initial_velo(R_set,1)';
if isempty(initial_acc)
initial_acc=Inv_M*(Fi-C*initial_velo-K0*initial_disp);
acc(loop,:)=initial_acc(R_set,1)';
else
acc(loop,:)=initial_acc(R_set,1)';
end

K=K0+a0*M+a1*C;% 對于線性系統，以后K都不會變

Fi=Fi+M*(a0*initial_disp+a2*initial_velo+a3*initial_acc)+...
C*(a1*initial_disp+a4*initial_velo+a5*initial_acc);

update_disp=K\Fi;
update_velo=a1*(update_disp-initial_disp)-a4*initial_velo-a5*initial_acc;
update_acc =a0*(update_disp-initial_disp)-a2*initial_velo-a3*initial_acc;

initial_disp=update_disp;% 將本次結果保存，下一次迭代時作為初值
initial_velo=update_velo;
initial_acc =update_acc;
end

end % end of for
% end % end of if
subplot(2,1,1)
plot(t,disp)
hold on
plot(t,velo(:,1),'r')
hold off
legend('disp','velo')
title('Newmark')
% axis(gca,[0,max(t),-2e-5, 4e-5])
% %---------------------------------------------------------------------%
% (3)Runge-Kutta,注意R-K 可以應用于一般非線性的求解，因此很厲害的；
% 請參考盛宏玉書上308頁內容。
% %---------------------------------------------------------------------%
% R-K 方法要求 輸入剛度，質量，阻尼，載荷，以及非線性方程的形式；
% 初始條件只需給定初始位移和初始速度，初始加速度可以由此計算得到
disp=zeros(Nsteps,numel(R_set));% 這是用來保存R_set 位移結果的；
velo=zeros(Nsteps,numel(R_set));% 這是用來保存R_set 速度結果的；
acc=zeros(Nsteps,numel(R_set));% 這是用來保存R_set 加速度結果的；
% if strcmp(method,'Runge-Kutta')

% A=[
% zeros(size(M)),eye(N);
% -Inv_M*K,-inv_M*C;
% ];
initial_disp=zeros(N,1);% You may change it.
initial_velo=[0;1];% You may change it.
initial_acc=[];% You may change it.

for loop=1:Nsteps
if loop>1

if loop==Nsteps
break;
end

Fn1=zeros(N,1);
for loopf=1:numel(fi_set)
Fn1(fi_set(loopf),1)=Force(loop,loopf);
end

Kd1=Inv_M*(-K0*initial_disp-C*initial_velo+Fn1);% 這就是一般非線性的表達式，可以另外寫為一個函數單獨調用
Kd2=Inv_M*(-K0*(initial_disp+0.5*dt*initial_velo)-C*(initial_velo+0.5*dt*Kd1)+0.5*(Fn1+Fn0));
Kd3=Inv_M*(-K0*(initial_disp+0.5*dt*initial_velo+0.25*dt^2*Kd1)-C*(initial_velo+0.5*dt*Kd2)+0.5*(Fn0+Fn1));
Kd4=Inv_M*(-K0*(initial_disp+dt*initial_velo+0.5*dt^2*Kd2)-C*(initial_velo+dt*Kd3)+Fn1);

Fn0=Fn1;% 保存上一次的載荷，兩個時間步之間的就用平均值

update_disp=initial_disp+dt*initial_velo+(dt^2)/6*(Kd1+Kd2+Kd3); % Kdi是第二組R-K法系數
update_velo=initial_velo+dt/6*(Kd1+2*Kd2+2*Kd3+Kd4);
update_acc =Kd1;

initial_disp=update_disp;% 將本次結果保存，下一次迭代時作為初值
initial_velo=update_velo;
initial_acc=update_acc;

disp(loop,:)=initial_disp(R_set,1)';% 保存輸出
velo(loop,:)=initial_velo(R_set,1)';
acc(loop,:)=initial_acc(R_set,1)';

else

disp(loop,:)=initial_disp(R_set,1)';
velo(loop,:)=initial_velo(R_set,1)';
if isempty(initial_acc)
Fn0=zeros(N,1);
for loopf=1:numel(fi_set)
Fn0(fi_set(loopf),1)=Force(loop,loopf);
end

initial_acc=Inv_M*(Fn0-C*initial_velo-K0*initial_disp);
acc(loop,:)=initial_acc(R_set,1)';
else
acc(loop,:)=initial_acc(R_set,1)';
end

end

end

% end

subplot(2,1,2)
plot(t,disp)
hold on
plot(t,velo(:,1),'r')
hold off
legend('disp','velo')
title('Runge-Kutta')
% axis(gca,[0,max(t),-2e-5, 4e-5])
% %---------------------------------------------------------------------%
% (4)Post-Processing
% %---------------------------------------------------------------------%

% %---------------------------------------------------------------------%
% (5)
% %---------------------------------------------------------------------%