% free bar impact
% impact force
% Fe = k1*delta^1.5; 
% NE 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);
aT_ = diff(vT_,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));

% gravity force
G_ = [0 -m*g 0];

% Newton Euler equations of motion
Neom =-m*aC_+G_;
Eeom =-IC*alpha_;

eqNE1 = Neom(1);
eqNE2 = Neom(2);
eqNE3 = Eeom(3);

[V,S] = odeToVectorField(eqNE1, eqNE2, eqNE3);
% S
% sprintf('%s \n', S)
% S =
%  
%  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);

fprintf('initial conditions for impact\n')
fprintf('q1  = %6.2f (m)\n',q1i)
fprintf('dq1 = %6.2f (m/s)\n',dq1i)
fprintf('q2  = %6.2f (m)\n',q2i)
fprintf('dq2 = %6.2f (m/s)\n',dq2i)
fprintf('q3  = %6.2f (rad)\n',q3i)
fprintf('dq3 = %6.2f (rad/s)\n',dq3i)
fprintf('\n')

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

% Newton-Euler eom
NeomE =-m*aC_ + G_ + P_;
EeomE =-IC*alpha_ + cross(rT_-rC_, P_);

eqNE1E = NeomE(1);
eqNE2E = NeomE(2);
eqNE3E = EeomE(3);

[V_,S] = odeToVectorField(eqNE1E, eqNE2E, eqNE3E);
% sprintf('%s \n', S)
% S =
%  
%  q3
% Dq3
%  q1
% Dq1
%  q2
% Dq2

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

fprintf('end of impact\n')
fprintf('q1  = %6.2f (m)\n',q1E)
fprintf('dq1 = %6.2f (m/s)\n',dq1E)
fprintf('q2  = %6.2f (m)\n',q2E)
fprintf('dq2 = %6.2f (m/s)\n',dq2E)
fprintf('q3  = %6.2f (rad)\n',q3E)
fprintf('dq3 = %6.2f (rad/s)\n',dq3E)

xE = [q1E dq1E q2E dq2E q3E dq3E];

pe = double(subs(Pe, q2(t), q2E));

fprintf('Fe = %6.3e (N) \n', pe);
fprintf(' \n'); 

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));
fprintf('ymax = %6.3e (m) \n\n',q2max)

Fen=subs(Pe,q2(t),q2se);

figure(1)
subplot(3,1,1)
plot(tte,q2se,'b','LineWidth',1.5),...
ylabel('q2 (m)'),grid,...
subplot(3,1,2)
plot(tte,dq2se,'b','LineWidth',1.5),...
ylabel('dq2/dt (m/s)'),grid;
subplot(3,1,3)
plot(tte,Fen,'b','LineWidth',1.5),
xlabel('t (s)'),ylabel('Fe (N)'),grid;

T = 0.5*(m*vC_*vC_.'+IC*omega_*omega_.');
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);

figure(2)
plot(tte,Te,'b','LineWidth',1.5),...
xlabel('t (s)'),ylabel('T (J)')
grid on
 
function [value,isterminal,direction] = eventFL0(~,x)

value = x(5);

isterminal = 1;

direction = 0;

end
% If isterminal vector is set to 1, integration will halt 
% when a zero-crossing is detected.

% The elements of the direction vector are -1, 1, or 0, 
% specifying that the corresponding event must be decreasing, 
% increasing, or that any crossing is to be detected



% Events ? The function is of the form
% [value,isterminal,direction] = events(t,y)
% value, isterminal, and direction are vectors for which the ith element corresponds to the ith event function: 
% value(i) is the value of the ith event function.

% isterminal(i) = 1 if the integration is to terminate at a zero of this event function, otherwise, 0. 

% direction(i) = 0 if all zeros are to be located (the default), +1 if only zeros where the event function is increasing, and -1 if only zeros where the event function is decreasing. 

% If you specify an events function and events are detected, the solver returns three additional outputs:


% 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)
% 
% end of impact
% 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)
