% Equations=Lagrange(L,[theta,dtheta,ddtheta,x,dx,ddx]) 
% Calculate the equations for a triple damped pendulum

function compute_diff_system

syms t l1 l2 l3 g m1 m2 m3 k1 k2 k3 t1 t2 t3 dt1 dt2 dt3 ddt1 ddt2 ddt3 Dr f1 f2 f3;

% Kinetic Energy 
T=1/2*l1*l1*dt1*dt1*(m1+m2+m3)...
  +1/2*l2*l2*dt2*dt2*(m2+m3)...
	+1/2*l3*l3*dt3*dt3*m3...
  +l1*l2*dt1*dt2*cos(t1-t2)*(m2+m3)...
  +l2*l3*dt2*dt3*cos(t2-t3)*m3...
	+l1*l3*dt1*dt3*cos(t1-t3)*m3;

% Potential Energy 
V=g*(l1+l2+l3)*(m1+m2+m3)...
  -g*l1*(m1+m2+m3)*cos(t1)...
  -g*l2*(m2+m3)*cos(t2)...
  -g*l3*m3*cos(t3);

% Lagrangian 
L=T-V;

% Rayleigh Dissipation function
Dr=1/2*l1*l1*dt1*dt1*(k1+k2+k3)...
   +1/2*l2*l2*dt2*dt2*(k2+k3)...
   +1/2*l3*l3*dt3*dt3*k3...
   +l1*l2*dt1*dt2*cos(t1-t2)*(k2+k3)...
   +l2*l3*dt2*dt3*cos(t2-t3)*k3...
	 +l1*l3*dt1*dt3*cos(t1-t3)*k3;

% Derivative of Rayleigh function
f1=diff(Dr,dt1);
f2=diff(Dr,dt2);
f3=diff(Dr,dt3);

% Get Lagrange equations and solve for expressing theta_second
Equations=Lagrange(L,[t1,dt1,ddt1,t2,dt2,ddt2,t3,dt3,ddt3]);
[theta1_second theta2_second theta3_second]=solve(Equations(1)==-f1,Equations(2)==-f2,Equations(3)==-f3,ddt1,ddt2,ddt3);

% Simplify equations
theta1_second=simplify(theta1_second);
theta2_second=simplify(theta2_second);
theta3_second=simplify(theta3_second);

% Store equations into ascii file
str_theta1_second=char(theta1_second);
str_theta2_second=char(theta2_second);
str_theta3_second=char(theta3_second);
dlmwrite('diff_system.txt',str_theta1_second,'delimiter','');
dlmwrite('diff_system.txt',str_theta2_second,'-append','delimiter','','roffset',1);
dlmwrite('diff_system.txt',str_theta3_second,'-append','delimiter','','roffset',1);

end

% Lagrange function
function [M]=Lagrange(Lag,V)
syms t;
Var=length(V)/3;
Vt=V;
    for cont0=1:1:Var
        Vt(cont0*3-2)=strcat('f',num2str(cont0),'(t)');
        Vt(cont0*3-1)=diff(Vt((cont0*3)-2),t);
        Vt(cont0*3)=diff(Vt((cont0*3)-2),t,2);
    end
    for cont0=1:1:Var
        L1=simple(diff(Lag,V(cont0*3-1)));
        L2=simple(diff(Lag,V(cont0*3-2)));
        Dposx=L1;

        for cont=1:1:Var*3         
             Dposx=subs(Dposx,V(cont),Vt(cont));
        end
        L1=diff(Dposx,t);

        for cont=Var*3:-1:1         
             L1=subs(L1,Vt(cont),V(cont));
        end
        L1F=L1-L2;
        L1F=simple(expand(L1F));
        L1F=collect(L1F,Vt(cont0*3));
        M(cont0)=L1F;
    end
end