clear;
clc;


m = 2;
M = 1;
n = 6;
% n=input('Number of nodes:');
H = zeros(4*n,4*n);
Hl = zeros(7);
theta = zeros(n,1);
l = zeros(n,1);
timestep = input('timestep: ');
endtime = input('end time: ');

% angle= 2*pi()/n;
% radius=200;
% for i=1:n
%     x(i)=radius*cos(angle*(i-1));
%     y(i)=radius*sin(angle*(i-1));
% end
%     
% x(n+1)=x(1);
% y(n+1)=y(1);

x=[100,150,50,50,20,80,100];
y=[100,60,10,80,120,150,100];
plot(x,y)

steps = endtime/timestep;

% Local matrix
for t = 1:steps;
    
    
for i=1:n
    l(i) = ((x(i+1)-x(i))^2+(y(i+1)-y(i))^2)^(1/2);
    theta(i) = asin((y(i+1)-y(i))/l(i));
    if x(i+1) < x(i)
        theta(i) = pi - theta(i);
    end
end

for j=1:n
    
    
    miu=(m*(l(j))^2)/(5*M);
    
    Hl(1,1)=l(j)/(6*m)*2*(sin(theta(j)))^2;
    Hl(1,2)=l(j)/(6*m)*-2*sin(theta(j))*cos(theta(j));
    Hl(1,3)=1/(6*m)*5*(sin(theta(j)));
    Hl(1,4)=l(j)/(6*m)*(sin(theta(j)))^2;
    Hl(1,5)=l(j)/(6*m)*-sin(theta(j))*cos(theta(j));
    Hl(1,6)=1/(6*m)*-1*(sin(theta(j)));
    Hl(1,7)=1/(6*m)*-4*(sin(theta(j)));
    
    Hl(2,1)=l(j)/(6*m)*-2*sin(theta(j))*cos(theta(j));
    Hl(2,2)=l(j)/(6*m)*2*(cos(theta(j)))^2;
    Hl(2,3)=1/(6*m)*-5*(cos(theta(j)));
    Hl(2,4)=l(j)/(6*m)*-sin(theta(j))*cos(theta(j));
    Hl(2,5)=l(j)/(6*m)*(cos(theta(j)))^2;
    Hl(2,6)=1/(6*m)*(cos(theta(j)));
    Hl(2,7)=1/(6*m)*4*(cos(theta(j)));
    
    Hl(3,1)=1/(6*m)*5*(sin(theta(j)));
    Hl(3,2)=1/(6*m)*-5*(cos(theta(j)));
    Hl(3,3)=1/(6*m*l(j))*(14+4*miu);
    Hl(3,4)=1/(6*m)*(sin(theta(j)));
    Hl(3,5)=1/(6*m)*-(cos(theta(j)));
    Hl(3,6)=1/(6*m*l(j))*(2-miu);
    Hl(3,7)=1/(6*m*l(j))*(-16+2*miu);
    
    Hl(4,1)=l(j)/(6*m)*(sin(theta(j)))^2;
    Hl(4,2)=l(j)/(6*m)*-sin(theta(j))*cos(theta(j));
    Hl(4,3)=1/(6*m)*(sin(theta(j)));
    Hl(4,4)=l(j)/(6*m)*2*(sin(theta(j)))^2;
    Hl(4,5)=l(j)/(6*m)*-2*sin(theta(j))*cos(theta(j));
    Hl(4,6)=1/(6*m)*-5*(sin(theta(j)));
    Hl(4,7)=1/(6*m)*4*(sin(theta(j)));
     
    Hl(5,1)=l(j)/(6*m)*-sin(theta(j))*cos(theta(j));
    Hl(5,2)=l(j)/(6*m)*(cos(theta(j)))^2;
    Hl(5,3)=1/(6*m)*-(cos(theta(j)));
    Hl(5,4)=l(j)/(6*m)*-2*sin(theta(j))*cos(theta(j));
    Hl(5,5)=l(j)/(6*m)*2*(cos(theta(j)))^2;
    Hl(5,6)=1/(6*m)*5*(cos(theta(j)));
    Hl(5,7)=1/(6*m)*-4*(cos(theta(j)));
    
    Hl(6,1)=1/(6*m)*-1*(sin(theta(j)));
    Hl(6,2)=1/(6*m)*(cos(theta(j)));
    Hl(6,3)=1/(6*m*l(j))*(2-miu);
    Hl(6,4)=1/(6*m)*-5*(sin(theta(j)));
    Hl(6,5)=1/(6*m)*5*(cos(theta(j)));
    Hl(6,6)=1/(6*m*l(j))*(14+4*miu);
    Hl(6,7)=1/(6*m*l(j))*(-16+2*miu);
    
    Hl(7,1)=1/(6*m)*-4*(sin(theta(j)));
    Hl(7,2)=1/(6*m)*4*(cos(theta(j)));
    Hl(7,3)=1/(6*m*l(j))*(-16+2*miu);
    Hl(7,4)=1/(6*m)*4*(sin(theta(j)));
    Hl(7,5)=1/(6*m)*-4*(cos(theta(j)));
    Hl(7,6)=1/(6*m*l(j))*(-16+2*miu);
    Hl(7,7)=1/(6*m*l(j))*(32+16*miu); 
  
    
 % Global Matrix
 
    if j<=1 % when n=1
       
      H(1:7,1:7) = H(1:7,1:7) + Hl(1:7,1:7); 
   
    elseif j>1 && j<=n-1 % when n=2...n-1
          
      H((4*j-4):(4*j-2),(4*j-4):(4*j-2)) = H((4*j-4):(4*j-2),(4*j-4):(4*j-2)) + Hl(1:3,1:3);
      H((4*j):(4*j+3),(4*j-4):(4*j-2)) = H((4*j):(4*j+3),(4*j-4):(4*j-2)) + Hl(4:7,1:3);
      H((4*j-4):(4*j-2),(4*j):(4*j+3)) = H((4*j-4):(4*j-2),(4*j):(4*j+3))+ Hl(1:3,4:7);
      H((4*j):(4*j+3),(4*j):(4*j+3)) = H((4*j):(4*j+3),(4*j):(4*j+3))+ Hl(4:7,4:7);
  
    end
    
    if j>n-1 && j<=n %when n=4
      H((4*j-4):(4*j-2),(4*j-4):(4*j-2)) = H((4*j-4):(4*j-2),(4*j-4):(4*j-2))+ Hl(1:3,1:3);
      H(1:3,(4*j-4):(4*j-2)) = H(1:3,(4*j-4):(4*j-2))+ Hl(4:6,1:3);   
      H((4*j-4):(4*j-2),1:3)=H((4*j-4):(4*j-2),1:3)+Hl(1:3,4:6);
      H(1:3,1:3)=H(1:3,1:3)+Hl(4:6,4:6);
% Jm(4) values
      H(4*n,(4*j-4):(4*j-2))=H(4*n,(4*j-4):(4*j-2))+Hl(7,1:3);
      H(4*n,1:3)=H(4*n,1:3)+Hl(7,4:6);
      H(1:3,4*n)=H(1:3,4*n)+Hl(4:6,7);
      H((4*j-4):(4*j-2),4*n)=H((4*j-4):(4*j-2),4*n)+Hl(1:3,7);
      H(4*n,4*n)=Hl(7,7);
       
    end
 
end 

% Force Matrix

F = zeros (4*n,1);
% Variables for forces
gamma = 1;
dgdt = 0;
gI = 1.03;
gII = 1;


F(1,1) = gamma*cos((theta(1)))+dgdt*-sin((theta(1)))+(l(1)/2)*(gI-gII)*-sin((theta(1))) + gamma*-cos((theta(n)))+dgdt*sin((theta(n)))+(l(n)/2)*(gI-gII)*-sin((theta(n)));
F(2,1) = gamma*sin((theta(1)))+dgdt*cos((theta(1)))+(l(1)/2)*(gI-gII)*cos((theta(1))) + gamma*-sin((theta(n)))+dgdt*-cos((theta(n)))+(l(n)/2)*(gI-gII)*cos((theta(n)));
F(3,1) = 0;
F(4*n,1) = 0;

for a= 1:n-1
    F(4*a,1) = gamma*-cos((theta(a)))+dgdt*sin((theta(a)))+(l(a)/2)*(gI-gII)*-sin((theta(a))) + gamma*cos((theta(a+1)))+dgdt*-sin((theta(a+1)))+(l(a+1)/2)*(gI-gII)*-sin((theta(a+1)));
    F(4*a+1,1) = gamma*-sin((theta(a)))+dgdt*-cos((theta(a)))+(l(a)/2)*(gI-gII)*cos((theta(a))) + gamma*sin((theta(a+1)))+dgdt*cos((theta(a+1)))+(l(a+1)/2)*(gI-gII)*cos((theta(a+1)));
    F(4*a+2,1) = 0;
    F(4*a+3,1) = 0;
end

xmin = -500;
xmax = 500;
ymin = -500;
ymax = 500;

     Hinv = inv(H);
     x_dot = Hinv*F;
     
     x(1) = x_dot(1,1)*timestep+x(1);
     y(1) = x_dot(2,1)*timestep+y(1);
     
     for k = 1:n-1
         x(k+1) = x_dot(4*k,1)*timestep+x(k+1);
         y(k+1) = x_dot(4*k+1,1)*timestep+y(k+1);
     end
     
     x(n+1) = x_dot(1,1)*timestep+x(n+1);
     y(n+1) = x_dot(2,1)*timestep+y(n+1);
    
     % store current frame for animation
     Frame(t) = grabplot1(x,y,xmin,xmax,ymin,ymax);

end