clear;
clc;

m = 1.5;
M = 1;
n = 5;
% n=input('Number of nodes:');
H = zeros(4*n,4*n);
Hl = zeros(7);
theta = zeros(n,1);
l = zeros(n,1);

n2 = 5;
% n2=input('Number of nodes:');
H2 = zeros(4*n2,4*n2);
Hl2 = zeros(7);
theta2 = zeros(n2,1);
l2 = zeros(n2,1);

alphasum = zeros (n,1);
pb = 0;

timestep = input('timestep: ');
endtime = input('end time: ');

ng = n + n2 - 1;
length = zeros(n+1,n+1);
alpha = zeros (n,n);

%When particle combines
xnew = zeros(1,ng+1);
ynew = zeros(1,ng+1);

lnew = zeros(ng,1);
tnew = zeros(ng,1);
Hnew = zeros(4*ng,4*ng);
Hlnew = zeros(7);

combine = 0;

% 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);

% ALL PARTICLE NODES HAS TO BE IN CLOCKWISE DIRECTION!
x=[120,140,110,50,80,120];
y=[120,60,-30,50,70,120];
x2=[-120,-130,-50,5,-60,-120];
y2=[-100,-40,10,-85,-90,-100];
plot(x,y,x2,y2)

xmin = -800;
xmax = 800;
ymin = -800;
ymax = 800;

steps = endtime/timestep;
% Initial Calculation
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 i = 1:n+1
        for j = 1:n+1
            length (i,j) = sqrt((x(i) - x2(j))^2 + (y(i) - y2(j))^2);
        end 
    end
    
    for i=1:n2
            l2(i) = ((x2(i+1)-x2(i))^2+(y2(i+1)-y2(i))^2)^(1/2);
            theta2(i) = asin((y2(i+1)-y2(i))/l2(i));
            if x2(i+1) < x2(i)
                theta2(i) = pi - theta2(i);
            end
    end

    for i = 1:n
        for j = 1:n
            alpha (i,j) = 180/pi * acos (((length(i,j))^2 + (length(i,j+1))^2 - (l2(j,1))^2)/(2*length(i,j)*length(i,j+1)));
            alphasum (i,1) = alphasum (i,1) + alpha(i,j);
        end
    end

% 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 i = 1:n+1
        for j = 1:n+1
            length (i,j) = sqrt((x(i) - x2(j))^2 + (y(i) - y2(j))^2);
        end 
    end
    
    for i=1:n2
            l2(i) = ((x2(i+1)-x2(i))^2+(y2(i+1)-y2(i))^2)^(1/2);
            theta2(i) = asin((y2(i+1)-y2(i))/l2(i));
            if x2(i+1) < x2(i)
                theta2(i) = pi - theta2(i);
            end
    end
        

for i=1:n
    if alphasum(i) >= 358 && alphasum(i) <= 362
        pb = i;
        combine = 1;
    
        for j = 1:pb-1
            xnew(j) = x(j);
            ynew(j) = y(j);
        end
        for k = pb:n
            xnew(n2+k) = x(k+1);
            ynew(n2+k) = y(k+1);
        end
        W = zeros(1,n+1);
        for i = 1:n+1
            W(1,i) = length(pb,i);
        end

        Q = min(W);
        for i = 1:n+1
            if Q == W(i)
            MIN1 = i;
            W(i) = 10^23;
            end
        end

        R = min(W);
        for i = 1:n+1
            if R == W(i)
               MIN2 = i;
            end

        end

        if MIN1 >= MIN2
            for i=1:n-MIN1+1;
                xnew(pb+i-1) = x2(MIN1+i-1);
                ynew(pb+i-1) = y2(MIN1+i-1);
            end
            for i=1:MIN1-1;
                xnew(pb+(n-MIN1+i)) = x2(i);
                ynew(pb+(n-MIN1+i)) = y2(i);
            end
        else
            for i=1:n-MIN2+1;
                xnew(pb+i-1) = x2(MIN2+i-1);
                ynew(pb+i-1) = y2(MIN2+i-1);
            end
            for i=1:MIN2-1;
                xnew(pb+(n-MIN2+i)) = x2(i);
                ynew(pb+(n-MIN2+i)) = y2(i);
            end
        end
        
        for i=1:ng
        lnew(i) = ((xnew(i+1)-xnew(i))^2+(ynew(i+1)-ynew(i))^2)^(1/2);
        tnew(i) = asin((ynew(i+1)-ynew(i))/lnew(i));
            if xnew(i+1) < xnew(i)
                tnew(i) = pi - tnew(i);
            end
        end
        
        alphasum = zeros (n,1);
        
    elseif alphasum (i) == 0
        break
        
    else
        alphasum = zeros (n,1);
            for i = 1:n
                for j = 1:n
                    alpha (i,j) = 180/pi * acos (((length(i,j))^2 + (length(i,j+1))^2 - (l2(j,1))^2)/(2*length(i,j)*length(i,j+1)));
                    alphasum (i,1) = alphasum (i,1) + alpha(i,j);
                end
            end

    end
end
   

    if combine == 1
        
        for i=1:ng
        lnew(i) = ((xnew(i+1)-xnew(i))^2+(ynew(i+1)-ynew(i))^2)^(1/2);
        tnew(i) = asin((ynew(i+1)-ynew(i))/lnew(i));
            if xnew(i+1) < xnew(i)
                tnew(i) = pi - tnew(i);
            end
        end
           
        
        for j=1:ng
    
            miu=(m*(lnew(j))^2)/(5*M);

            Hlnew(1,1)=lnew(j)/(6*m)*2*(sin(tnew(j)))^2;
            Hlnew(1,2)=lnew(j)/(6*m)*-2*sin(tnew(j))*cos(tnew(j));
            Hlnew(1,3)=1/(6*m)*5*(sin(tnew(j)));
            Hlnew(1,4)=lnew(j)/(6*m)*(sin(tnew(j)))^2;
            Hlnew(1,5)=lnew(j)/(6*m)*-sin(tnew(j))*cos(tnew(j));
            Hlnew(1,6)=1/(6*m)*-1*(sin(tnew(j)));
            Hlnew(1,7)=1/(6*m)*-4*(sin(tnew(j)));

            Hlnew(2,1)=lnew(j)/(6*m)*-2*sin(tnew(j))*cos(tnew(j));
            Hlnew(2,2)=lnew(j)/(6*m)*2*(cos(tnew(j)))^2;
            Hlnew(2,3)=1/(6*m)*-5*(cos(tnew(j)));
            Hlnew(2,4)=lnew(j)/(6*m)*-sin(tnew(j))*cos(tnew(j));
            Hlnew(2,5)=lnew(j)/(6*m)*(cos(tnew(j)))^2;
            Hlnew(2,6)=1/(6*m)*(cos(tnew(j)));
            Hlnew(2,7)=1/(6*m)*4*(cos(tnew(j)));

            Hlnew(3,1)=1/(6*m)*5*(sin(tnew(j)));
            Hlnew(3,2)=1/(6*m)*-5*(cos(tnew(j)));
            Hlnew(3,3)=1/(6*m*lnew(j))*(14+4*miu);
            Hlnew(3,4)=1/(6*m)*(sin(tnew(j)));
            Hlnew(3,5)=1/(6*m)*-(cos(tnew(j)));
            Hlnew(3,6)=1/(6*m*lnew(j))*(2-miu);
            Hlnew(3,7)=1/(6*m*lnew(j))*(-16+2*miu);

            Hlnew(4,1)=lnew(j)/(6*m)*(sin(tnew(j)))^2;
            Hlnew(4,2)=lnew(j)/(6*m)*-sin(tnew(j))*cos(tnew(j));
            Hlnew(4,3)=1/(6*m)*(sin(tnew(j)));
            Hlnew(4,4)=lnew(j)/(6*m)*2*(sin(tnew(j)))^2;
            Hlnew(4,5)=lnew(j)/(6*m)*-2*sin(tnew(j))*cos(tnew(j));
            Hlnew(4,6)=1/(6*m)*-5*(sin(tnew(j)));
            Hlnew(4,7)=1/(6*m)*4*(sin(tnew(j)));

            Hlnew(5,1)=lnew(j)/(6*m)*-sin(tnew(j))*cos(tnew(j));
            Hlnew(5,2)=lnew(j)/(6*m)*(cos(tnew(j)))^2;
            Hlnew(5,3)=1/(6*m)*-(cos(tnew(j)));
            Hlnew(5,4)=lnew(j)/(6*m)*-2*sin(tnew(j))*cos(tnew(j));
            Hlnew(5,5)=lnew(j)/(6*m)*2*(cos(tnew(j)))^2;
            Hlnew(5,6)=1/(6*m)*5*(cos(tnew(j)));
            Hlnew(5,7)=1/(6*m)*-4*(cos(tnew(j)));

            Hlnew(6,1)=1/(6*m)*-1*(sin(tnew(j)));
            Hlnew(6,2)=1/(6*m)*(cos(tnew(j)));
            Hlnew(6,3)=1/(6*m*lnew(j))*(2-miu);
            Hlnew(6,4)=1/(6*m)*-5*(sin(tnew(j)));
            Hlnew(6,5)=1/(6*m)*5*(cos(tnew(j)));
            Hlnew(6,6)=1/(6*m*lnew(j))*(14+4*miu);
            Hlnew(6,7)=1/(6*m*lnew(j))*(-16+2*miu);

            Hlnew(7,1)=1/(6*m)*-4*(sin(tnew(j)));
            Hlnew(7,2)=1/(6*m)*4*(cos(tnew(j)));
            Hlnew(7,3)=1/(6*m*lnew(j))*(-16+2*miu);
            Hlnew(7,4)=1/(6*m)*4*(sin(tnew(j)));
            Hlnew(7,5)=1/(6*m)*-4*(cos(tnew(j)));
            Hlnew(7,6)=1/(6*m*lnew(j))*(-16+2*miu);
            Hlnew(7,7)=1/(6*m*lnew(j))*(32+16*miu); 


         % Global Matrix

            if j<=1 % when ng=1

              Hnew(1:7,1:7) = Hnew(1:7,1:7) + Hlnew(1:7,1:7); 

            elseif j>1 && j<=ng-1 % when ng=2...ng-1

              Hnew((4*j-4):(4*j-2),(4*j-4):(4*j-2)) = Hnew((4*j-4):(4*j-2),(4*j-4):(4*j-2)) + Hlnew(1:3,1:3);
              Hnew((4*j):(4*j+3),(4*j-4):(4*j-2)) = Hnew((4*j):(4*j+3),(4*j-4):(4*j-2)) + Hlnew(4:7,1:3);
              Hnew((4*j-4):(4*j-2),(4*j):(4*j+3)) = Hnew((4*j-4):(4*j-2),(4*j):(4*j+3))+ Hlnew(1:3,4:7);
              Hnew((4*j):(4*j+3),(4*j):(4*j+3)) = Hnew((4*j):(4*j+3),(4*j):(4*j+3))+ Hlnew(4:7,4:7);

            end

            if j>ng-1 && j<=ng %when ng=4
              Hnew((4*j-4):(4*j-2),(4*j-4):(4*j-2)) = Hnew((4*j-4):(4*j-2),(4*j-4):(4*j-2))+ Hlnew(1:3,1:3);
              Hnew(1:3,(4*j-4):(4*j-2)) = Hnew(1:3,(4*j-4):(4*j-2))+ Hlnew(4:6,1:3);   
              Hnew((4*j-4):(4*j-2),1:3)=Hnew((4*j-4):(4*j-2),1:3)+Hlnew(1:3,4:6);
              Hnew(1:3,1:3)=Hnew(1:3,1:3)+Hlnew(4:6,4:6);
        % Jm(4) values
              Hnew(4*ng,(4*j-4):(4*j-2))=Hnew(4*ng,(4*j-4):(4*j-2))+Hlnew(7,1:3);
              Hnew(4*ng,1:3)=Hnew(4*ng,1:3)+Hlnew(7,4:6);
              Hnew(1:3,4*ng)=Hnew(1:3,4*ng)+Hlnew(4:6,7);
              Hnew((4*j-4):(4*j-2),4*ng)=Hnew((4*j-4):(4*j-2),4*ng)+Hlnew(1:3,7);
              Hnew(4*ng,4*ng)=Hlnew(7,7);

            end
        end 

    % Force Matrix

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


    Fnew(1,1) = gamma*cos((tnew(1)))+dgdt*-sin((tnew(1)))+(lnew(1)/2)*(gI-gII)*-sin((tnew(1))) + gamma*-cos((tnew(ng)))+dgdt*sin((tnew(ng)))+(lnew(ng)/2)*(gI-gII)*-sin((tnew(ng)));
    Fnew(2,1) = gamma*sin((tnew(1)))+dgdt*cos((tnew(1)))+(lnew(1)/2)*(gI-gII)*cos((tnew(1))) + gamma*-sin((tnew(ng)))+dgdt*-cos((tnew(ng)))+(lnew(ng)/2)*(gI-gII)*cos((tnew(ng)));
    Fnew(3,1) = 0;
    Fnew(4*ng,1) = 0;

    for a= 1:ng-1
        Fnew(4*a,1) = gamma*-cos((tnew(a)))+dgdt*sin((tnew(a)))+(lnew(a)/2)*(gI-gII)*-sin((tnew(a))) + gamma*cos((tnew(a+1)))+dgdt*-sin((tnew(a+1)))+(lnew(a+1)/2)*(gI-gII)*-sin((tnew(a+1)));
        Fnew(4*a+1,1) = gamma*-sin((tnew(a)))+dgdt*-cos((tnew(a)))+(lnew(a)/2)*(gI-gII)*cos((tnew(a))) + gamma*sin((tnew(a+1)))+dgdt*cos((tnew(a+1)))+(lnew(a+1)/2)*(gI-gII)*cos((tnew(a+1)));
        Fnew(4*a+2,1) = 0;
        Fnew(4*a+3,1) = 0;
    end

     x_dotnew = Hnew\Fnew;
     
     xnew(1) = x_dotnew(1,1)*timestep+xnew(1);
     ynew(1) = x_dotnew(2,1)*timestep+ynew(1);
     
     for k = 1:ng-1
         xnew(k+1) = x_dotnew(4*k,1)*timestep+xnew(k+1);
         ynew(k+1) = x_dotnew(4*k+1,1)*timestep+ynew(k+1);
     end
     
     xnew(ng+1) = x_dotnew(1,1)*timestep+xnew(ng+1);
     ynew(ng+1) = x_dotnew(2,1)*timestep+ynew(ng+1);
    
     % store current frame for animation
     Framenew(t) = grabplotnew(xnew,ynew,xmin,xmax,ymin,ymax);
        
    elseif combine == 0
    
        %First Particle
        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

             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);

        %Second Particle
        for i=1:n2
            l2(i) = ((x2(i+1)-x2(i))^2+(y2(i+1)-y2(i))^2)^(1/2);
            theta2(i) = asin((y2(i+1)-y2(i))/l2(i));
            if x2(i+1) < x2(i)
                theta2(i) = pi - theta2(i);
            end
        end
        
        for j=1:n2


            miu=(m*(l2(j))^2)/(5*M);

            Hl2(1,1)=l2(j)/(6*m)*2*(sin(theta2(j)))^2;
            Hl2(1,2)=l2(j)/(6*m)*-2*sin(theta2(j))*cos(theta2(j));
            Hl2(1,3)=1/(6*m)*5*(sin(theta2(j)));
            Hl2(1,4)=l2(j)/(6*m)*(sin(theta2(j)))^2;
            Hl2(1,5)=l2(j)/(6*m)*-sin(theta2(j))*cos(theta2(j));
            Hl2(1,6)=1/(6*m)*-1*(sin(theta2(j)));
            Hl2(1,7)=1/(6*m)*-4*(sin(theta2(j)));

            Hl2(2,1)=l2(j)/(6*m)*-2*sin(theta2(j))*cos(theta2(j));
            Hl2(2,2)=l2(j)/(6*m)*2*(cos(theta2(j)))^2;
            Hl2(2,3)=1/(6*m)*-5*(cos(theta2(j)));
            Hl2(2,4)=l2(j)/(6*m)*-sin(theta2(j))*cos(theta2(j));
            Hl2(2,5)=l2(j)/(6*m)*(cos(theta2(j)))^2;
            Hl2(2,6)=1/(6*m)*(cos(theta2(j)));
            Hl2(2,7)=1/(6*m)*4*(cos(theta2(j)));

            Hl2(3,1)=1/(6*m)*5*(sin(theta2(j)));
            Hl2(3,2)=1/(6*m)*-5*(cos(theta2(j)));
            Hl2(3,3)=1/(6*m*l2(j))*(14+4*miu);
            Hl2(3,4)=1/(6*m)*(sin(theta2(j)));
            Hl2(3,5)=1/(6*m)*-(cos(theta2(j)));
            Hl2(3,6)=1/(6*m*l2(j))*(2-miu);
            Hl2(3,7)=1/(6*m*l2(j))*(-16+2*miu);

            Hl2(4,1)=l2(j)/(6*m)*(sin(theta2(j)))^2;
            Hl2(4,2)=l2(j)/(6*m)*-sin(theta2(j))*cos(theta2(j));
            Hl2(4,3)=1/(6*m)*(sin(theta2(j)));
            Hl2(4,4)=l2(j)/(6*m)*2*(sin(theta2(j)))^2;
            Hl2(4,5)=l2(j)/(6*m)*-2*sin(theta2(j))*cos(theta2(j));
            Hl2(4,6)=1/(6*m)*-5*(sin(theta2(j)));
            Hl2(4,7)=1/(6*m)*4*(sin(theta2(j)));

            Hl2(5,1)=l2(j)/(6*m)*-sin(theta2(j))*cos(theta2(j));
            Hl2(5,2)=l2(j)/(6*m)*(cos(theta2(j)))^2;
            Hl2(5,3)=1/(6*m)*-(cos(theta2(j)));
            Hl2(5,4)=l2(j)/(6*m)*-2*sin(theta2(j))*cos(theta2(j));
            Hl2(5,5)=l2(j)/(6*m)*2*(cos(theta2(j)))^2;
            Hl2(5,6)=1/(6*m)*5*(cos(theta2(j)));
            Hl2(5,7)=1/(6*m)*-4*(cos(theta2(j)));

            Hl2(6,1)=1/(6*m)*-1*(sin(theta2(j)));
            Hl2(6,2)=1/(6*m)*(cos(theta2(j)));
            Hl2(6,3)=1/(6*m*l2(j))*(2-miu);
            Hl2(6,4)=1/(6*m)*-5*(sin(theta2(j)));
            Hl2(6,5)=1/(6*m)*5*(cos(theta2(j)));
            Hl2(6,6)=1/(6*m*l2(j))*(14+4*miu);
            Hl2(6,7)=1/(6*m*l2(j))*(-16+2*miu);

            Hl2(7,1)=1/(6*m)*-4*(sin(theta2(j)));
            Hl2(7,2)=1/(6*m)*4*(cos(theta2(j)));
            Hl2(7,3)=1/(6*m*l2(j))*(-16+2*miu);
            Hl2(7,4)=1/(6*m)*4*(sin(theta2(j)));
            Hl2(7,5)=1/(6*m)*-4*(cos(theta2(j)));
            Hl2(7,6)=1/(6*m*l2(j))*(-16+2*miu);
            Hl2(7,7)=1/(6*m*l2(j))*(32+16*miu); 


         % Global Matrix

            if j<=1 % when n2=1

              H2(1:7,1:7) = H2(1:7,1:7) + Hl2(1:7,1:7); 

            elseif j>1 && j<=n2-1 % when n2=2...n2-1

              H2((4*j-4):(4*j-2),(4*j-4):(4*j-2)) = H2((4*j-4):(4*j-2),(4*j-4):(4*j-2)) + Hl2(1:3,1:3);
              H2((4*j):(4*j+3),(4*j-4):(4*j-2)) = H2((4*j):(4*j+3),(4*j-4):(4*j-2)) + Hl2(4:7,1:3);
              H2((4*j-4):(4*j-2),(4*j):(4*j+3)) = H2((4*j-4):(4*j-2),(4*j):(4*j+3))+ Hl2(1:3,4:7);
              H2((4*j):(4*j+3),(4*j):(4*j+3)) = H2((4*j):(4*j+3),(4*j):(4*j+3))+ Hl2(4:7,4:7);

            end

            if j>n2-1 && j<=n2 %when n2=4
              H2((4*j-4):(4*j-2),(4*j-4):(4*j-2)) = H2((4*j-4):(4*j-2),(4*j-4):(4*j-2))+ Hl2(1:3,1:3);
              H2(1:3,(4*j-4):(4*j-2)) = H2(1:3,(4*j-4):(4*j-2))+ Hl2(4:6,1:3);   
              H2((4*j-4):(4*j-2),1:3)=H2((4*j-4):(4*j-2),1:3)+Hl2(1:3,4:6);
              H2(1:3,1:3)=H2(1:3,1:3)+Hl2(4:6,4:6);
        % Jm(4) values
              H2(4*n2,(4*j-4):(4*j-2))=H2(4*n2,(4*j-4):(4*j-2))+Hl2(7,1:3);
              H2(4*n2,1:3)=H2(4*n2,1:3)+Hl2(7,4:6);
              H2(1:3,4*n2)=H2(1:3,4*n2)+Hl2(4:6,7);
              H2((4*j-4):(4*j-2),4*n2)=H2((4*j-4):(4*j-2),4*n2)+Hl2(1:3,7);
              H2(4*n2,4*n2)=Hl2(7,7);

            end

        end 

        % Force Matrix

        F2 = zeros (4*n2,1);
        % Variables for forces
        gamma2 = 1;
        dgdt2 = 0;
        gI2 = 1.03;
        gII2 = 1;


        F2(1,1) = gamma2*cos((theta2(1)))+dgdt2*-sin((theta2(1)))+(l2(1)/2)*(gI2-gII2)*-sin((theta2(1))) + gamma2*-cos((theta2(n2)))+dgdt2*sin((theta2(n2)))+(l2(n2)/2)*(gI2-gII2)*-sin((theta2(n2)));
        F2(2,1) = gamma2*sin((theta2(1)))+dgdt2*cos((theta2(1)))+(l2(1)/2)*(gI2-gII2)*cos((theta2(1))) + gamma2*-sin((theta2(n2)))+dgdt2*-cos((theta2(n2)))+(l2(n2)/2)*(gI2-gII2)*cos((theta2(n2)));
        F2(3,1) = 0;
        F2(4*n2,1) = 0;

        for a= 1:n2-1
            F2(4*a,1) = gamma2*-cos((theta2(a)))+dgdt2*sin((theta2(a)))+(l2(a)/2)*(gI2-gII2)*-sin((theta2(a))) + gamma2*cos((theta2(a+1)))+dgdt2*-sin((theta2(a+1)))+(l2(a+1)/2)*(gI2-gII2)*-sin((theta2(a+1)));
            F2(4*a+1,1) = gamma2*-sin((theta2(a)))+dgdt2*-cos((theta2(a)))+(l2(a)/2)*(gI2-gII2)*cos((theta2(a))) + gamma2*sin((theta2(a+1)))+dgdt2*cos((theta2(a+1)))+(l2(a+1)/2)*(gI2-gII2)*cos((theta2(a+1)));
            F2(4*a+2,1) = 0;
            F2(4*a+3,1) = 0;
        end


             Hinv2 = inv(H2);
             x_dot2 = Hinv2*F2;

             x2(1) = x_dot2(1,1)*timestep+x2(1);
             y2(1) = x_dot2(2,1)*timestep+y2(1);

             for k = 1:n2-1
                 x2(k+1) = x_dot2(4*k,1)*timestep+x2(k+1);
                 y2(k+1) = x_dot2(4*k+1,1)*timestep+y2(k+1);
             end

             x2(n2+1) = x_dot2(1,1)*timestep+x2(n2+1);
             y2(n2+1) = x_dot2(2,1)*timestep+y2(n2+1);

             % store current frame for animation
             Frame(t) = grabplot(x,y,x2,y2,xmin,xmax,ymin,ymax);

    else
    
end   
 
end