% -----------------Physical system -----------------------
%      M*ddXt + K*Xt = F
%            | 6.0    2.0 |
%   K =   | 2.0    4.0 |
%
%   M =  | 2.0    0.0 |
%           | 0.0    1.0 |
%
%    F =  | 10.0  |
%           |  23.0 |

% -----------------matlab computations -----------------------
clear all;
mm =2;
DeltaT=0.28;
num_step=92;
DAMPING_COEFF = 0.0;
nstep=100;

% stiffness matrix
K(1,1)=6.0;
K(1,2)=-2.0;
K(2,1)=-2.0;
K(2,2)=4.0;
 

% mass matrix
M(1,1)=2.0;
M(1,2)=0.0;
M(2,1)=0.0;
M(2,2)=1.0;

% force vector
F=0.0*[1:mm]';
F(1)=10.0;
F(2)=23.0;

% initial conditions
q0=0.0*[1:mm]';
dq0=0.0*[1:mm]';

[n,n]=size(M);
[n,m]=size(F);
gf=inv(M)*K;
[V,D]=eig(gf);
[lamda,k]=sort(diag(D));
V=V(:,k);
Factor=diag(V'*M*V);
Vnorm1=V*inv(sqrt(diag(Factor)));
% number of selected significant modes
nreduced=2;
Vnorm=Vnorm1(:,1:nreduced);
omega=diag(sqrt(Vnorm'*K*Vnorm));
Fnorm=Vnorm'*F;
kmat=lamda;
 
 

% newark integration method
a0 = (4.0+2.0*DAMPING_COEFF*DeltaT)/(DeltaT*DeltaT);
a1 = 4.0/(DeltaT*DeltaT)+(2.0*DAMPING_COEFF)/(DeltaT);
a2 = 4.0/(DeltaT)+DAMPING_COEFF;
a3 = 1.0;
a4 = 4.0/(DeltaT*DeltaT);
a5 = -a4;
a6 = a5*(DeltaT);
a7 = -1.0;
a8 = DeltaT/2.0;
a9 = (DeltaT*DeltaT)/4.0;
a10 = a9;

myForceplusDeltaT = Vnorm'*(F);
myForcer = 0.0*F;
Xt = 0.0*[1:nreduced]';
XtminusDeltaT = Xt;
XtplusDeltaT = Xt;
dXtplusDeltaT = Xt;
dXt = Xt;
ddXt = Vnorm'*F;
ddXtplusDeltaT = 0.0*F;
 

kmod = kmat+a0;
theta = 1.0;
 

% -------- start iteration -------------------------------------
for j = 1:num_step
    time(j)=DeltaT*j;
    ForceMod = myForcer + theta*(myForceplusDeltaT - myForcer) + (a1*Xt+a2*dXt+a3*ddXt);
    Solution = ForceMod./kmod;
    ddXtplusDeltaT = a4*Solution + a5*Xt + a6*dXt + a7*ddXt;
    dXtplusDeltaT = dXt + a8*(ddXt+ddXtplusDeltaT);
    XtplusDeltaT = Solution;
    myForceplusDeltaT = ddXtplusDeltaT + DAMPING_COEFF*dXtplusDeltaT + kmat.*XtplusDeltaT;
    XtminusDeltaT = Xt;
    Xt = XtplusDeltaT;
    dXt = dXtplusDeltaT;
    ddXt = ddXtplusDeltaT;
    myForcer = myForceplusDeltaT;
    XXt=Vnorm*Xt;
    FFt=Vnorm*ForceMod;
    pos1(j)=XXt(1);
    pos2(j)=XXt(2);
    myforce1(j)=FFt(1);
    myforce2(j)=FFt(2);
end

% --------- plot the results ------------------------
figure(1);
plot(time,pos1);
figure(2);
plot(time,pos2);
figure(3);
plot(time,myforce1);
figure(4);
plot(time,myforce2);