% free link 
% Lagrange eom

clear; clc; close all
% dimension and basic data
L = 0.250;  % length of the bar (m)
d = 0.012;  % diameter of the bar (m)
R = d/2; % radius of the bar (m)
rho = 7800; % density of the material (kg/m^3)
m = pi*R^2*L*rho; % mass of the bar (kg)
g = 9.81; % gravitational acceleration (m/s^2)
% mass moment of inertia about mass center C of the bar
IC = m*(L^2+3*R^2)/12; % (kg m^2)
H = 0.18; % drop height (m)
syms q1(t) q2(t) q3(t)
% q1 x-coordinate of the end T
% q2 y-coordinate of the end T
% q3 angle of the bar with x-axis
omega_ = diff([0 0 q3(t)],t); % angular velocity of the bar
alpha_ = diff(omega_,t); % angular acceleration of the bar
% T contact point
rT_ = [q1(t) q2(t) 0];
vT_ = diff(rT_,t);
% C mass center
rC_ = rT_+0.5*[L*cos(q3(t)),L*sin(q3(t)),0];
vC_ = simplify(diff(rC_,t));
aC_ = simplify(diff(vC_,t));
% kinetic energy of the bar in general planar motion
T = 0.5*( m*vC_*vC_.'+IC*omega_*omega_.');
% dT/d(dq)
Tdq1 = deriv(T, diff(q1,t)); % dT/dq1'
Tdq2 = deriv(T, diff(q2,t)); % dT/dq2'
Tdq3 = deriv(T, diff(q3,t)); % dT/dq3'
% d(dT/d(dq))/dt
Tt1 = diff(Tdq1, t); % d(dT/dq1')/dt
Tt2 = diff(Tdq2, t); % d(dT/dq2')/dt
Tt3 = diff(Tdq3, t); % d(dT/dq3')/dt
% dT/dq
Tq1 = deriv(T, q1); % dT/dq1
Tq2 = deriv(T, q2); % dT/dq2
Tq3 = deriv(T, q3); % dT/dq3
% left hand side of Lagrange's eom
LHS1 = Tt1 - Tq1; % d(dT/dq1')/dt-dT/dq1
LHS2 = Tt2 - Tq2; % d(dT/dq2')/dt-dT/dq2
LHS3 = Tt3 - Tq3; % d(dT/dq3')/dt-dT/dq3
% partial derivatives 
rC_1_ = deriv(rC_, q1(t)); % drC_/dq1
rC_2_ = deriv(rC_, q2(t)); % drC_/dq2 
rC_3_ = deriv(rC_, q3(t)); % drC_/dq3 
rT_1_ = deriv(rT_, q1(t)); % drT_/dq1 
rT_2_ = deriv(rT_, q2(t)); % drT_/dq2 
rT_3_ = deriv(rT_, q3(t)); % drT_/dq3 
% generalized active forces Q
G_ = [0 -m*g 0];% gravity force
% generalized force corresponding to q1
Q1 = rC_1_*G_.'; % Q1 = G_.drC_/dq1
% generalized force corresponding to q2
Q2 = rC_2_*G_.'; % Q2 = G_.drC_/dq2
% generalized force corresponding to q3
Q3 = rC_3_*G_.'; % Q3 = G_.drC_/dq3
% Lagrange's eom corresponding to q1
Lagrange1 = LHS1 - Q1;
% Lagrange's eom corresponding to q2
Lagrange2 = LHS2 - Q2;
% Lagrange's eom corresponding to q3
Lagrange3 = LHS3 - Q3;
[V,S] = odeToVectorField(Lagrange1,Lagrange2,Lagrange3);
%  q3 -> Y[1]
% Dq3 -> Y[2]
%  q1 -> Y[3]
% Dq1 -> Y[4]
%  q2 -> Y[5]
% Dq2 -> Y[6]
syms Y
eomNE_ = matlabFunction(V,'vars', {t,Y});
time0 = [0 1];
% initial conditions
% starts from rest
q10=0;    % q1(0)
q20=H;    % q2(0)
q30=pi/4; % q3(0) dropping angle 
dq10=0;   % dq1(0)/dt
dq20=0;   % dq2(0)/dt
dq30=0;   % dq3(0)/dt
x0 = [q30 dq30 q10 dq10 q20 dq20];
option0 = odeset...
('RelTol',1e-3,'MaxStep',1e-3,'Events',@eventFL0);
% x(5)=q2=0
[tt0, xs0, t0, y0] = ...
    ode45(eomNE_,time0,x0,option0);
q3i  = y0(1);
dq3i = y0(2);
q1i  = y0(3);
dq1i = y0(4);
q2i  = y0(5);
dq2i = y0(6);
xi = [q3i dq3i q1i dq1i q2i dq2i];
% elastic impact
E1 = 200e9; % Young's modulus
nu = 0.29; % ratio
k1 = (2/(3*(1-nu^2))*E1*sqrt(R)); % elastic constant
% fprintf('k1 = %6.3e \n\n',k1)
delta = abs(q2(t));
% elastic normal contact force
Pe = k1*delta^1.5; 
mu = 0.1; % coefficient of friction
P_ = [mu*Pe, Pe, 0]; % total elastic contact force
% generalized active forces Q for impact phase
% generalized force corresponding to q1
% Q1 = G_.drC_/dq1+P_.drT_/dq1
Q1 = rC_1_*G_.'+rT_1_*P_.'; 
% generalized force corresponding to q2
% Q2 = G_.drC_/dq2+P_.drT_/dq2
Q2 = rC_2_*G_.'+rT_2_*P_.';
% generalized force corresponding to q3
% Q3 = G_.drC_/dq3+P_.drT_/dq3
Q3 = rC_3_*G_.'+rT_3_*P_.';
% Lagrange's eom corresponding to q1
Lagrange1 = LHS1 - Q1;
% Lagrange's eom corresponding to q2
Lagrange2 = LHS2 - Q2;
% Lagrange's eom corresponding to q3
Lagrange3 = LHS3 - Q3;
[V_,S] = odeToVectorField(Lagrange1,Lagrange2,Lagrange3);
eomI_ = matlabFunction(V_,'vars', {t,Y});
time0 = [0 1];
[tte, xsE, tE, yE] = ...
    ode45(eomI_,time0,xi,option0);
q1E  = yE(3);
dq1E = yE(4);
q2E  = yE(5);
dq2E = yE(6);
q3E  = yE(1);
dq3E = yE(2);
xE = [q1E dq1E q2E dq2E q3E dq3E];
pe = double(subs(Pe, q2(t), q2E));
q1se  = real(xsE(:,3));
dq1se = real(xsE(:,4));
q2se  = real(xsE(:,5));
dq2se = real(xsE(:,6));
q3se  = real(xsE(:,1));
dq3se = real(xsE(:,2));
q2max=max(abs(q2se));
Fen=subs(Pe,q2(t),q2se);
ql = {diff(q1(t),t), diff(q2(t),t), diff(q3(t),t),...
      q1(t), q2(t), q3(t)};    
qne = {dq1se, dq2se, dq3se,...
      q1se, q2se, q3se}; 
Te = subs(T,ql,qne);

function [value,isterminal,direction] = eventFL0(~,x)
value = x(5);
isterminal = 1;
direction = 0;
end

function fout = deriv(f, g)
syms t xx dxx
lg = {diff(g, t), g}; 
lx = {dxx, xx};
f1 = subs(f, lg, lx);
f2 = diff(f1, xx);
fout = subs(f2, lx, lg);
end


% ans =
% 
%     'q3 
%      Dq3 
%      q1 
%      Dq1 
%      q2 
%      Dq2 
%      '
% 
% initial conditions for impact
% q1  =   0.00 (m)
% dq1 =   0.00 (m/s)
% q2  =  -0.00 (m)
% dq2 =  -1.88 (m/s)
% q3  =   0.79 (rad)
% dq3 =   0.00 (rad/s)
% 
%  
% S =
%  
%  q3
% Dq3
%  q1
% Dq1
%  q2
% Dq2
%  
% end of  elastic phase
% q1  =  -0.00 (m)
% dq1 =  -1.99 (m/s)
% q2  =   0.00 (m)
% dq2 =   1.88 (m/s)
% q3  =   0.78 (rad)
% dq3 = -24.38 (rad/s)
% Fe = 7.863e-22 (N) 
%  
% ymax = 6.704e-05 (m) 
% 
% 
