% compound pendulum with external force Fe
% Fe = k*delta^alpha; 
% NE eom

clear; clc; close all

% dimension and basic data
L = 0.50;  % length of link (m)
d = 0.01;  % diameter (m)
R = d/2;   % radius (m)
rho = 7800;% density (kg/m^3)
m = pi*R^2*L*rho; % mass (kg)
g = 9.81;  % gravitational acceleration (m/s^2)

alpha = 1.5;
k = 2000; % spring constant
% theta0 = -pi/6;

% generalized coordinate theta(t)
syms theta(t)

% angular velocity
omega_ = [0 0 diff(theta(t),t)];
% angular acceleration
alpha_ = diff(omega_,t);

c = cos(theta(t));
s = sin(theta(t));

% C center of mass
xC = (L/2)*c;
yC = (L/2)*s;
rC_ = [xC yC 0];

% A end of the link
xA = L*c;
yA = L*s;
rA_ = [xA yA 0];
vA_ = diff(rA_,t);

IC = m*(L^2+3*R^2)/12; % mass moment of inertia about C
IO = IC + m*(L/2)^2; % mass moment of inertia about O

G_ = [0 -m*g 0]; % force of gravity at C
MO_ = cross(rC_,G_); % MO = rC_ x G_
% eom for free fall
eq_ = -IO*alpha_ + MO_;

eqz = eq_(3);

[V,S] = odeToVectorField(eqz);
syms Y
eom0_ = matlabFunction(V,'vars', {t,Y});

time0 = [0 1];
x0 = [0 0]; % initial condition

option0 = ...
    odeset('RelTol',1e-3,'MaxStep',1e-3,'Events',@event1);

[tt0, xs0, t0, ye0] = ...
    ode45(eom0_,time0,x0,option0);

theta0 = ye0(1);
omega0 = ye0(2);

fprintf('initial conditions for contact\n')
fprintf('theta0 = %6.3g (rad) = %g (degrees)\n',...
    theta0, theta0*180/pi)
fprintf('omega0 = %6.3g (rad/s)\n', omega0)

qt = {diff(theta,t),theta};
list0 = {omega0, theta0} ;

vAn_ = subs(vA_, qt, list0);
% double(S) converts the symbolic object S to a numeric object
% converts the symbolic object vBn  to a numeric object
vA_  = double(vAn_); 
fprintf('vA_ = [%6.3g, %6.3g, %g] (m/s)\n', vA_)
fprintf('\n');

% elastic contact phase
% elastic displacement
delta = L*sin(theta(t))-L*sin(theta0);

% elastic force
alpha = 1.5;
Fe = k*abs(delta)^alpha;
Fe_ = [0, Fe, 0]; 

% MOe_ = rC_ x G_ + rA_ x Fe_
MOe_ = cross(rC_,G_) + cross(rA_,Fe_);

% eom for elastic contact
eqee_ = -IO*alpha_ + MOe_;

eqze = eqee_(3);

[V,S] = odeToVectorField(eqze);
eome_ = matlabFunction(V,'vars', {t,Y});

[tte, xse, te, yee] = ...
    ode45(eome_,time0, [theta0 omega0],option0);

thetaf = yee(1);
omegaf = yee(2);

fprintf('final conditions for contact\n')
fprintf('tf = %6.3g (s) \n',te);
fprintf('thetaf = %6.3g (rad) = %g (degrees)\n',...
    thetaf, thetaf*180/pi)
fprintf('omegaf = %6.3g (rad/s)\n', omegaf)
fprintf('\n')

thetan = real(xse(:,1));
omegan = real(xse(:,2));
deltan = L*(sin(thetan) ...
    -sin(theta0)*ones(length(thetan),1));
Fen = k*abs(deltan).^alpha;

thetam = min(thetan);
Fm = max(Fen);
fprintf('thetam = %6.3g (rad) = %6.3g (degrees)\n',...
    thetam, thetam*180/pi)
fprintf('Fm = %6.3g (N)\n', Fm)

%plots
figure(1)
subplot(3,1,1)
plot(tte,thetan*180/pi,'LineWidth',1.5)
ylabel('\theta (deg)'), grid 
subplot(3,1,2)
plot(tte,omegan,'LineWidth',1.5)
ylabel('\omega (rad/s)'),grid
subplot(3,1,3)
plot(tte,Fen,'LineWidth',1.5)
ylabel('Fe (N)'), xlabel('t (s)')
grid;

text_size = 12;
pin_scale_factor = 0.015;        

figure(2)
for i = 1:10:length(thetan)
clf

xAn = L*cos(thetan(i));
yAn = L*sin(thetan(i));

Ov = zeros(length(thetan(i)),1);
xO = Ov; yO = Ov; 

xlabel('x(m)'), ylabel('y(m)')

sf=0.5;
axis([-sf sf -sf sf])
grid on
hold on

% plotting joint O
plot_pin0(0,0,pin_scale_factor,0)

% axis auto
plot([xO xAn],[yO yAn],'k','LineWidth',1.5)

% label A
textAx = xAn+0.01;
textAy = yAn;
text(textAx, textAy,'A','FontSize',text_size)

% vector Fe
quiver(xAn,yAn,0,.15,0,'Color','r','LineWidth',2.5)
text(xAn+0.01,yAn+.07,'F_e',...
    'fontsize',14,'fontweight','b')

pause(0.2);   
end


function[value,ist,dir]=event1(~,x)
value = x(1)+pi/6;
ist = 1;
dir = 0;
end

% eof

% initial conditions for contact
% theta0 = -0.524 (rad) = -30 (degrees)
% omega0 =  -5.42 (rad/s)
% vA = [ -1.36,  -2.35, 0] (m/s)
% 
% final conditions for contact
% tf = 0.0633 (s) 
% thetaf = -0.524 (rad) = -30 (degrees)
% omegaf =   5.42 (rad/s)
% 
% thetam = -0.644 (rad) =  -36.9 (degrees)
% Fm =   22.5 (N)


