% Program 
% Double pendulum
% Dynamics 
% Newton-Euler equations of motion

fprintf('Newton-Euler equations of motion \n'); fprintf(' \n');

clear all; clc; close;
format long;

global L1 L2 yc q10 q20 deltar

L1 = 1; 
L2 = 1; 
g = 9.807;  
R = 0.01;
rho = 7800;
m1 = pi*R^2*L1*rho;
m2 = pi*R^2*L2*rho;

q10 = pi/6;
q20 = pi/3;

omega10 = 2;  
omega20 = 3;

t = sym('t','real');
q1 = sym('q1(t)');
q2 = sym('q2(t)');

xB = L1*cos(q1); 
yB = L1*sin(q1); 
rB = [xB yB 0]; 
rC1 = rB/2;

xD = xB+L2*cos(q2); 
yD = yB+L2*sin(q2); 
rD = [xD yD 0];
vD = diff(rD,t);

rC2 = (rB + rD)/2; 
vC2 = diff(rC2,t); aC2 = diff(vC2,t);

omega1 = [0 0 diff(q1,t)]; alpha1 = diff(omega1,t);
omega2 = [0 0 diff(q2,t)]; alpha2 = diff(omega2,t);

slist = {diff(q1,t,2), diff(q2,t,2),...
         diff(q1,t), diff(q2,t), q1, q2};
     
list0 = {diff(q1,t,2), diff(q2,t,2),...
         omega10, omega20, q10, q20}; 
     
vD0 = eval(subs(vD, slist, list0));

fprintf('vD0 = [ %g, %g, %g ] [m/s]\n', vD0);

G1 = [0 m1*g 0]; 
G2 = [0 m2*g 0];
IC1 = m1*L1^2/12; IA = IC1 + m1*(L1/2)^2; 
IC2 = m2*L2^2/12;

% elastic compression
 
E1 = 200e9;
Sy = 1.12e9;
nu = 0.33;
Ep = (2*(1-nu^2)/E1)^-1;
C = 1.295*exp(.736*nu);
yc = (((pi*C*Sy)/(2*Ep))^2)*R;
k1 = (2/(3*(1-nu^2))*E1*sqrt(R));
 
delta = yD - (L1*sin(q10) + L2*sin(q20));

mu = 0.3; % coeff. of friction

Fe = k1*delta^1.5;

% sgn = vD(1)/abs(vD(1)); 

sgn = sign(vD0(1));

Ffe = - mu*sgn*Fe;

Pe = [Ffe  -Fe  0];

F21e = -m2*aC2 + G2 +Pe;
EqAe = -IA*alpha1 + cross(rB, F21e) + cross(rC1, G1);
Eq2e = -IC2*alpha2 + cross(rB - rC2, -F21e) + cross(rD - rC2, Pe);

nlist = {'ddq1', 'ddq2', 'x(2)', 'x(4)', 'x(1)','x(3)'};

vDt = subs(vD(1), slist, nlist); 
vDn = subs(vD(2), slist, nlist); 

eq1e = subs(EqAe(3), slist, nlist); 
eq2e = subs(Eq2e(3), slist, nlist);

sole = solve(eq1e, eq2e,'ddq1, ddq2');
dx2e = sole.ddq1; 
dx4e = sole.ddq2;
 
dx2dte = char(dx2e); 
dx4dte = char(dx4e);

fid = fopen('RRe.m','w+');
fprintf(fid,'function dx = RRe(t,x)\n');
fprintf(fid,'dx = zeros(4,1);\n');
fprintf(fid,'dx(1) = x(2);\n');
fprintf(fid,'dx(2) = ');
fprintf(fid,dx2dte);
fprintf(fid,';\n');
fprintf(fid,'dx(3) = x(4);\n');
fprintf(fid,'dx(4) = ');
fprintf(fid,dx4dte);
fprintf(fid,';');
fclose(fid);
cd(pwd);

optione = odeset('RelTol',1e-3,'MaxStep',1e-3,'Events', @eventRRe);

t0 = 0;  tf = .3; time = [0 tf];

x0 = [q10 omega10 q20 omega20]; % initial conditions

[tte, xse, te, yee] = ode45(@RRe, time, x0, optione);

q1e = yee(1);
omega1e = yee(2);
q2e = yee(3);
omega2e = yee(4);

liste = {'ddq1', 'ddq2', omega1e, omega2e, q1e, q2e};

deltae = eval(subs(delta, slist, liste));
vDe = eval(subs(vD, slist, liste));
Pee = eval(subs(Pe, slist, liste)); 
delta0=deltae-1.9*yc;

fprintf(' \n');
fprintf('elastic compression \n');
fprintf('te =  %g  [s] \n', te);
fprintf('q1e =  %g  [rad] =  %g [degrees] \n', q1e, q1e*180/pi);
fprintf('omega1e =  %g  [rad/s] \n', omega1e);
fprintf('q2e =  %g  [rad] =  %g [degrees] \n', q2e, q2e*180/pi);
fprintf('omega2e =  %g  [rad/s] \n', omega2e); 
fprintf('deltae = %g  [m] \n', deltae);
fprintf('delta0 = deltae-1.9*yc = %g  [m] \n', delta0);
fprintf('vDe = [ %g, %g, %g ] [m/s]\n', vDe);
fprintf('Pe = [ %g, %g, %g ] [N]\n', Pee);
fprintf(' \n');

% q1se = xse(:,1); 
% omega1es = xse(:,2);
% q2se = xse(:,3); 
% omega2es = xse(:,4); 

% figure(1)
% subplot(2,1,1),plot(tte,q1se*180/pi,'r'),...
% xlabel('t (s)'),ylabel('q1 (deg)'),grid,...
% subplot(2,1,2),plot(tte,q2se*180/pi,'b'),...
% xlabel('t (s)'),ylabel('q2 (rad/s)'),grid;
% 
% figure(2)
% subplot(2,1,1),plot(tte,omega1es,'r'),...
% xlabel('t (s)'),ylabel('\omega 1 (rad/s)'),grid,...
% subplot(2,1,2),plot(tte,omega2es,'b'),...
% xlabel('t (s)'),ylabel('\omega 2 (rad/s)'),grid;

% elasto-plastic compression 

% phase I until the tangential velocity vt stops

ey = Sy/Ep;
B = 0.14*exp(23*ey);
A = sqrt(R*delta*(delta/(1.9*yc))^B);
HS = 2.84-.92*(1-cos(pi*A / R));
Pc = (4/3)*(R/Ep)^2*(pi*C*Sy/2)^3;
y = (delta/yc);

Fep = Pc*(exp((-1/4)*y^(5/12))*y^(3/2)+4/C*HS *(1-exp((-1/25)*y^(5/9)))*y);

% Fepe = eval(subs(vpa(-Fep,9), {q1, q2}, {q1e, q2e}))

Ffs = - mu*sign(vDe(1))*Fep;

Ps = [ Ffs  -Fep  0];

F21s = -m2*aC2 + G2 + Ps;
EqAs = -IA*alpha1 + cross(rB, F21s) + cross(rC1, G1);
Eq2s = -IC2*alpha2 + cross(rB - rC2, -F21s) + cross(rD - rC2, Ps);

eq1s = subs(EqAs(3), slist, nlist); 
eq2s = subs(Eq2s(3), slist, nlist);

sols = solve(eq1s, eq2s,'ddq1, ddq2');
dx2s = sols.ddq1; 
dx4s = sols.ddq2;
 
dx2dts = char(dx2s); 
dx4dts = char(dx4s);

fid = fopen('RRs.m','w+');
fprintf(fid,'function dx = RRs(t,x)\n');
fprintf(fid,'dx = zeros(4,1);\n');
fprintf(fid,'dx(1) = x(2);\n');
fprintf(fid,'dx(2) = ');
fprintf(fid,dx2dts);
fprintf(fid,';\n');
fprintf(fid,'dx(3) = x(4);\n');
fprintf(fid,'dx(4) = ');
fprintf(fid,dx4dts);
fprintf(fid,';');
fclose(fid);
cd(pwd);

options = odeset('RelTol',1e-3,'InitialStep',1e-10,'MaxStep',1e-8,'Events',@eventRRs);

times = [te tf];

xs = [q1e omega1e q2e omega2e]; % initial conditions

[tts, xss, ts, yes] = ode45(@RRs, times, xs, options);

q1s = yes(1);
omega1s = yes(2);
q2s = yes(3);
omega2s = yes(4);

lists = {'ddq1', 'ddq2', omega1s, omega2s, q1s, q2s};

deltas = eval(subs(delta, slist, lists));

vDs = eval(subs(vD, slist, lists));

Psn = eval(subs(vpa(Ps,9), {q1, q2}, {q1s, q2s}));

fprintf(' \n');
fprintf('elasto plastic compression: phase I vt=0 \n');
fprintf('ts =  %g  [s] \n', ts);
fprintf('q1s =  %g  [rad] =  %g [degrees] \n', q1s, q1s*180/pi);
fprintf('omega1s =  %g  [rad/s] \n', omega1s);
fprintf('q2s =  %g  [rad] =  %g [degrees] \n', q2s, q2s*180/pi);
fprintf('omega2s =  %g  [rad/s] \n', omega2s); 
fprintf('deltas = %g  [m] \n', deltas);
fprintf('vDs = [ %g, %g, %g ] [m/s]\n', vDs);
fprintf('Peps = [ %g, %g, %g ] [N]\n', Psn);
fprintf(' \n');

% elasto-plastic compression 

% phase II  vn stops

Ffep = -Ffs;
% Ffep = 0;
% Ffep = Ffs;

Pep = [Ffep  -Fep  0];

F21ep = -m2*aC2 + G2 + Pep;
EqAep = -IA*alpha1 + cross(rB, F21ep) + cross(rC1, G1);
Eq2ep = -IC2*alpha2 + cross(rB - rC2, -F21ep) + cross(rD - rC2, Pep);

eq1ep = subs(EqAep(3), slist, nlist); 
eq2ep = subs(Eq2ep(3), slist, nlist);

solep = solve(eq1ep, eq2ep,'ddq1, ddq2');
dx2ep = solep.ddq1; 
dx4ep = solep.ddq2;
 
dx2dtep = char(dx2ep); 
dx4dtep = char(dx4ep);

fid = fopen('RRep.m','w+');
fprintf(fid,'function dx = RRep(t,x)\n');
fprintf(fid,'dx = zeros(4,1);\n');
fprintf(fid,'dx(1) = x(2);\n');
fprintf(fid,'dx(2) = ');
fprintf(fid,dx2dtep);
fprintf(fid,';\n');
fprintf(fid,'dx(3) = x(4);\n');
fprintf(fid,'dx(4) = ');
fprintf(fid,dx4dtep);
fprintf(fid,';');
fclose(fid);
cd(pwd);

timeep = [ts tf];

xep = [q1s omega1s q2s omega2s]; % initial conditions

[ttm, xsm, tm, yem] = ode45(@RRep, timeep, xep, options);

q1m = yem(1);
omega1m = yem(2);
q2m = yem(3);
omega2m = yem(4);

listm = {'ddq1', 'ddq2', omega1m, omega2m, q1m, q2m};

deltam = eval(subs(delta, slist, listm));

vDm = eval(subs(vD, slist, listm));

Pem = eval(subs(vpa(Pep,9), {q1, q2}, {q1m, q2m})); 
  
fprintf(' \n');
fprintf('elasto plastic compression phase II vn=0 \n');
fprintf('tm =  %g  [s] \n', tm);
fprintf('q1m =  %g  [rad] =  %g [degrees] \n', q1m, q1m*180/pi);
fprintf('omega1m =  %g  [rad/s] \n', omega1m);
fprintf('q2m =  %g  [rad] =  %g [degrees] \n', q2m, q2m*180/pi);
fprintf('omega2m =  %g  [rad/s] \n', omega2m); 
fprintf('deltam = %g  [m] \n', deltam);
fprintf('vDm = [ %g, %g, %g ] [m/s]\n', vDm);
fprintf('Pm = [ %g, %g, %g ] [N]\n', Pem);
fprintf(' \n');

% restitution

Pm = -Pem(2);

deltams = deltam/yc;
deltar = deltam*(1.02*(1-((deltams+5.9)/6.9)^-0.54));
Rr = 1/(deltam-deltar)^3 * (3/4 * Pm/Ep)^2;
k1r = 2/(3*(1-nu^2))*E1*sqrt(Rr);

Fr = k1r*(delta-deltar)^1.5;

% pr = eval(subs(vpa(Fr,9), {q1, q2}, {q1m, q2m})); 

Ffr = - mu*sign(vDm(1))*Fr;

Pr = [Ffr  -Fr  0];

F21r = -m2*aC2 + G2 + Pr;
EqAr = -IA*alpha1 + cross(rB, F21r) + cross(rC1, G1);
Eq2r = -IC2*alpha2 + cross(rB - rC2, -F21r) + cross(rD - rC2, Pr);

eq1r = subs(EqAr(3), slist, nlist); 
eq2r = subs(Eq2r(3), slist, nlist);

solr = solve(eq1r, eq2r,'ddq1, ddq2');
dx2r = solr.ddq1; 
dx4r = solr.ddq2;
 
dx2dtr = char(dx2r); 
dx4dtr = char(dx4r);

fid = fopen('RRr.m','w+');
fprintf(fid,'function dx = RRr(t,x)\n');
fprintf(fid,'dx = zeros(4,1);\n');
fprintf(fid,'dx(1) = x(2);\n');
fprintf(fid,'dx(2) = ');
fprintf(fid,dx2dtr);
fprintf(fid,';\n');
fprintf(fid,'dx(3) = x(4);\n');
fprintf(fid,'dx(4) = ');
fprintf(fid,dx4dtr);
fprintf(fid,';');
fclose(fid);
cd(pwd);

optionr = odeset('RelTol',1e-6,'InitialStep',1e-10,'MaxStep',1e-8,'Events',@eventRRr);

timer = [tm tf];

xr = [q1m omega1m q2m omega2m]; % initial conditions

[ttr, xsr, tr, yer] = ode45(@RRr, timer, xr, optionr);

q1r = yer(1);
omega1r = yer(2);
q2r = yer(3);
omega2r = yer(4);

listr = {'ddq1', 'ddq2', omega1r, omega2r, q1r, q2r};

deltarn = eval(subs(delta, slist, listr));
vDr = eval(subs(vD, slist, listr));
Per = eval(subs(vpa(Pr,9), {q1, q2}, {q1r, q2r})); 

fprintf(' \n');
fprintf('restitution \n');
fprintf('tr =  %g  [s] \n', tr);
fprintf('q1r =  %g  [rad] =  %g [degrees] \n', q1r, q1r*180/pi);
fprintf('omega1r =  %g  [rad/s] \n', omega1r);
fprintf('q2r =  %g  [rad] =  %g [degrees] \n', q2r, q2r*180/pi);
fprintf('omega2r =  %g  [rad/s] \n', omega2r); 
fprintf('deltar = %g  [m] \n', deltarn);
fprintf('vDr = [ %g, %g, %g ] [m/s]\n', vDr);
fprintf('Pr = [ %g, %g, %g ] [N]\n', Per);
fprintf(' \n');



%%
%%
%%
%%

% elastic compression 
% te =  3.9382e-06  [s] 
% q1e =  0.523607  [rad] =  30.0005 [degrees] 
% omega1e =  2.00004  [rad/s] 
% q2e =  1.04721  [rad] =  60.0007 [degrees] 
% omega2e =  2.99855  [rad/s] 
% deltae = 1.27287e-05  [m] 
% delta0 = deltae-1.9*yc = 5.99039e-17  [m] 
% vDe = [ -3.59687, 3.23132, 0 ] [m/s]
% Pe = [ 203.85, -679.5, 0 ] [N]
%
%
% elasto plastic compression: phase I 
% [vt<0 ; vt=0]
%
% vt<0 => Ff>0
%
% ts =  0.00014919  [s] 
% q1s =  0.523894  [rad] =  30.0169 [degrees] 
% omega1s =  1.93247  [rad/s] 
% q2s =  1.04744  [rad] =  60.014 [degrees] 
% omega2s =  -1.11612  [rad/s] 
% deltas = 0.00037811  [m] 
% vDs = [ 7.56062e-14, 1.11546, 0 ] [m/s]
% Peps = [ 17114.1, -57047, 0 ] [N]
%  
%
% elasto plastic compression phase II vn=0 
% vt>0 => Ff<0
%
% tm =  0.000240913  [s] 
% q1m =  0.524016  [rad] =  30.0239 [degrees] 
% omega1m =  0.6982  [rad/s] 
% q2m =  1.04734  [rad] =  60.0079 [degrees] 
% omega2m =  -1.20932  [rad/s] 
% deltam = 0.000430481  [m] 
% vDm = [ 0.698031, -6.66134e-15, 0 ] [m/s]
% Pm = [ -19794, -65979.8, 0 ] [N]
%
%
% other scenarios
%
% Ff>0 
%
% elasto plastic compression phase II vn=0 
% tm =  0.000189159  [s] 
% q1m =  0.52397  [rad] =  30.0213 [degrees] 
% omega1m =  1.89645  [rad/s] 
% q2m =  1.04736  [rad] =  60.009 [degrees] 
% omega2m =  -3.28494  [rad/s] 
% deltam = 0.000400645  [m] 
% vDm = [ 1.89627, -4.92939e-14, 0 ] [m/s]
% Pm = [ 18269.2, -60897.2, 0 ] [N]
% (INCORECT) 
%
%
%
%
% Ff=0 
%
% elasto plastic compression phase II vn=0 
% tm =  0.000204836  [s] 
% q1m =  0.523991  [rad] =  30.0225 [degrees] 
% omega1m =  1.54659  [rad/s] 
% q2m =  1.04734  [rad] =  60.008 [degrees] 
% omega2m =  -2.67882  [rad/s] 
% deltam = 0.000409608  [m] 
% vDm = [ 1.54629, -2.22045e-16, 0 ] [m/s]
% Pm = [ 0, -62426.2, 0 ] [N]
% (INCORECT)  



% restitution 
% tr =  0.000398516  [s] 
% q1r =  0.523997  [rad] =  30.0228 [degrees] 
% omega1r =  -0.508288  [rad/s] 
% q2r =  1.04714  [rad] =  59.9965 [degrees] 
% omega2r =  -1.30229  [rad/s] 
% deltar = 0.000313591  [m] 
% vDr = [ 1.3821, -1.0913, 0 ] [m/s]
% Pr = [ -0.0046105, -0.0153683, 0 ] [N]