close all; clear all; clc;

%% Slide 6

syms m M g L cvv cth k K IG a zg zgdot zgdotdot z zdot ...
     zdotdot v vdot vdotdot th thdot thdotdot real

syms x1 x2 x3 x4 u real

% Kinetic Energy
xgdot = zdot+a*cos(th)*thdot;
ygdot = -a*sin(th)*thdot;
T = 0.5*m*zdot^2 + 0.5*M*(xgdot^2+ygdot^2) + 0.5*IG*thdot^2;

% Potential Energy
V = 0.5*k*(z-zg)^2 + 0.5*K*th^2 + M*g*a*cos(th);


%% Slide 7

% Nonconservative Forces
Pz = -cvv*(zdot-zgdot);
Pth = -cth*thdot;


%% Slide 10

% Lagrange Equations

% Pz
pTpzdot = diff(T,zdot);
ddtpTpzdot = diff(pTpzdot,z)*zdot + ...
             diff(pTpzdot,zdot)*zdotdot + ...
             diff(pTpzdot,th)*thdot + ...
             diff(pTpzdot,thdot)*thdotdot;
pTpz = diff(T,z);
pVpz = diff(V,z);

% Pth
pTpthdot = diff(T,thdot);
ddtpTpthdot = diff(pTpthdot,z)*zdot + ...
              diff(pTpthdot,zdot)*zdotdot + ...
              diff(pTpthdot,th)*thdot + ...
              diff(pTpthdot,thdot)*thdotdot;
pTpth = diff(T,th);
pVpth = diff(V,th);


%% Slide 12

fprintf('\n%s\n','The EOM in absolute coordinates.')

% The equation of motion for "z" (eqz = 0)
eqz = simplify(ddtpTpzdot - pTpz + pVpz - Pz);
pretty(eqz)

% The equation of motion for "th" (eqth = 0)
eqth = simplify(ddtpTpthdot - pTpth + pVpth - Pth);
pretty(eqth)


%% Slide 14

% Rewrite the EOM using relative coordinates (i.e., v = z - zg)
z = v + zg;
zdot = vdot + zgdot;
zdotdot = vdotdot + zgdotdot;

fprintf('\n%s\n','The EOM in relative coordinates.')

%The equation of motion for “v” (Eqv1=0)
Eqv1 = simplify(subs(eqz));
pretty(Eqv1)

%The equation of motion for “th” (Eqth1=0)
Eqth1 = simplify(subs(eqth));
pretty(Eqth1)


%% Slide 19

% Solve for vdotdot and thdotdot
% 	     for the state space formulation
Sol = solve(Eqv1,Eqth1,{vdotdot,thdotdot});
Sol.vdotdot = simplify(Sol.vdotdot);
Sol.thdotdot = simplify(Sol.thdotdot);

fv = subs(Sol.vdotdot,{v,vdot,th,thdot,zgdotdot}, {x1,x2,x3,x4,u});
fprintf('\n%s\n','fv')
pretty(fv)
fth = subs(Sol.thdotdot,{v,vdot,th,thdot, zgdotdot}, {x1,x2,x3,x4,u});
fprintf('\n%s\n','fth')
pretty(fth)


%% Slide 22

pfvpx1=subs(diff(fv,x1),{x1,x2,x3,x4,u},{0,0,0,0,0});
fprintf('\n%s\n','pfvpx1')
pretty(pfvpx1)
pfvpx2=subs(diff(fv,x2),{x1,x2,x3,x4,u},{0,0,0,0,0});
fprintf('\n%s\n','pfvpx2')
pretty(pfvpx2)
pfvpx3=subs(diff(fv,x3),{x1,x2,x3,x4,u},{0,0,0,0,0});
fprintf('\n%s\n','pfvpx3')
pretty(pfvpx3)
pfvpx4=subs(diff(fv,x4),{x1,x2,x3,x4,u},{0,0,0,0,0});
fprintf('\n%s\n','pfvpx4')
pretty(pfvpx4)
pfvpu=subs(diff(fv,u),{x1,x2,x3,x4,u},{0,0,0,0,0});
fprintf('\n%s\n','pfvpu')
pretty(pfvpu)
pfthpx1=subs(diff(fth,x1),{x1,x2,x3,x4,u},{0,0,0,0,0});
fprintf('\n%s\n','pfthpx1')
pretty(pfthpx1)
pfthpx2=subs(diff(fth,x2),{x1,x2,x3,x4,u},{0,0,0,0,0});
fprintf('\n%s\n','pfthpx2')
pretty(pfthpx2)
pfthpx3=subs(diff(fth,x3),{x1,x2,x3,x4,u},{0,0,0,0,0});
fprintf('\n%s\n','pfthpx3')
pretty(pfthpx3)
pfthpx4=subs(diff(fth,x4),{x1,x2,x3,x4,u},{0,0,0,0,0});
fprintf('\n%s\n','pfthpx4')
pretty(pfthpx4)
pfthpu=subs(diff(fth,u),{x1,x2,x3,x4,u},{0,0,0,0,0});
fprintf('\n%s\n','pfthpu')
pretty(pfthpu)

A = [0 1 0 0;
     pfvpx1 pfvpx2 pfvpx3 pfvpx4;
     0 0 0 1;
     pfthpx1 pfthpx2 pfthpx3 pfthpx4];
B = [0; pfvpu; 0; pfthpu];
fprintf('\n%s\n','The system is now in a linear state-space form,')
fprintf('%s\n\n','and can be used in Simulink models!!!')