%% Pendulum
% by JA Riousset ©2021
% email: jriousset@fit.edu
% Table of Contents
% Initiation
% Environment parameters
%         You can modify the value of each of these parameters for your needs.
% Pendulum characteristics
%         You can modify the value of each of these parameters for your needs.
%         You will NOT need to change any of the calculated parameters.
% Initial conditions
%         You can modify the value of each of these parameters for your needs.
% ODE solver
%         There is NO change required in this section.
% Plots
%         There is NO change required to this section.
% Animator
%         There is NO change required to this section.
% Pendulum
%         There is NO change required to this section.
% Equation of motion
%         There is NO change required to this section.
% Forces
%     Fluid drag
%         There is NO change required to this section.
%     Weight
%         There is NO change required to this section.
%     Centrifugal pseudo-force
%         There is NO change required to this section.
%     Stokes's drag
%         You will need to modify it.
%     Tension
%         There is NO change required to this section.
% Conversion to Cartesian coordinates
%         There is NO change required to this section.
% Runge-Kutta solver
%         There is NO change required to this section.
%
%% Initiation
% The following commands clear up the computer memory.

clearvars
close all
clc

%
%% Environment parameters
% The structure \tt env contains all the variables of the external evironment, namely:
% \tt g: the gravitational acceleration in \rm m/s^2.
% \tt rho: the mass density of the atmosphere \rho, in \rm kg/m^3.
% \tt mu: the fluid viscosity of the atmosphere \mu in \rm Pa\cdot s=kg/m/s.
%% You can modify the value of each of these parameters for your needs.

env.g   = 9.81; % m/s^2, Earth
env.rho = 1.21; % kg/m^3, Earth
env.mu  = 0;  % Pa*s

%
%% Pendulum characteristics
% The structure \tt pdl contains all the variables of the pendulum, namely:
% \tt l: the length of the string in \rm m.
% \tt r: the radius of the pendulum in \rm m.
% \tt rho: the mass density of the pendulum \rho, in \rm kg/m^3.
% \tt Cd: the drag coefficient of the pendulum C_{\rm d}.
%% You can modify the value of each of these parameters for your needs.

l   = 1;      % m
R   = .02132; % m, golf ball
rho = 1124.5; % kg/m^3, golf ball
Cd  = 0;      % unitless

% The function \tt pendulum (defined at the end of the script) automatically calculate the following values to the structure \tt pdl:
% \tt A: the reference area (e.g., the cross-section) A=\pi R^2 in \rm m^2.
% \tt V: the volume of the pendulum V=\frac{4\pi R^3}{3} in \rm m^3.
% \tt m: the mass of the pendulum m=\rho V in \rm kg.
% \tt w0: the proper or "natural" angular frequency of the pendulum \Omega_0=\sqrt{\frac{g}{l}} in \rm rad/s.
% \tt T%: the period of the natural oscillations T=\frac{2\pi}{\omega_0} in \rm s.
%% You will NOT need to change any of the calculated parameters.
% They are automatically calculated when the function \tt pendulum is called.

pdl = pendulum(rho,R,l,Cd,env)

%
%% Initial conditions
% In this section, you will define the initial condition of the experiments.
% \tt t0: Initial date of the experiment t_0 in \rm s.
% \tt tf: Final date of the experiment t_{\rm f} in \rm s.
% \tt N: the the number of times to record the pendulum state.
% \tt tspan: The total duration of the experiment t_{\rm span}\in[t_0,t_{\rm f}].
% \tt theta0: The angular position \theta_0 in \rm rad at t_0. (\tt deg2rad converts the value from degrees to radians).
% \tt omega0: The initial angular velocity of the pendulum \omega_0 in \rm rad/s at t_0.
%% You can modify the value of each of these parameters for your needs.

t0     = 0;
tf     = 5*pdl.T;
N      = 1001;
tspan  = linspace(t0,tf,N);
theta0 = deg2rad(15);
omega0 = 0;

%
%% ODE solver
% This section uses the initial conditions \tt X0, pendulum properties \tt pdl, and environmental parameters \tt  env to output the state vector \tt X at the time \tt t. The state vector is then decomposed into \tt theta and \tt omega, to give \theta and \omega at the instants t.
%% There is NO change required in this section.

X0         = [theta0,omega0];
[t,X,sols] = pendulumS(tspan,X0,pdl,env);
theta      = X(1,:);
omega      = X(2,:); 

%
%% Plots
% This section displays the results of your experiment based on the input of the previous sections.
% Top center: instant of observations t in \rm s.
% Top left: x, and y-components of Stoke's (blue), fluid drag (red), and weight (green) forces.
% Top right: position \vec{r} [x,y] in \rm m, and velocity \vec{v}=[V_x,V_y] in \rm m/s.
% Bottom left:  x, and y-components of centrifugal pseudo-force \vec{f}_{\rm c} and tension force \vec{T}.
% Bottom right: angular position \theta and \omega, in \rm^\circ and \rm rad/s.
%% There is NO change required to this section.

animator(sols,pdl,env)
figure;
yyaxis left;
plot(t, theta, '-');
ylabel('\theta (rad)');

yyaxis right;
plot(t, omega, '-');
ylabel('\omega (rad/s)');

grid on;
title('Closed Path in Phase Space');
xlabel('t (s)');
figure;
plot(theta, omega, '-');
xlabel('\theta (rad)');
ylabel('\omega (rad/s)');

%
%% Animator
% This function is the "black box" behind the fancy animated graphics.
%% There is NO change required to this section.

function []=animator(ode_sols,pdl,env)
[theta,omega,x,y,vx,vy,fcx,fcy,Tx,Ty,Wx,Wy,Dx,Dy,Sx,Sy] = cyl2cart(ode_sols,pdl,env);

figure;
ax = subplot(1,1,1);
quiver([0 0],[0 0],[1 0],[0 1],'LineWidth',2,"Color",'k');
ax.View            = [90 90];
ax.XLabel.String   = "x (m)";
ax.YLabel.String   = "y (m)";
ax.XAxisLocation   = "origin";
ax.YAxisLocation   = "origin";
ax.DataAspectRatio = [1 1 1];
ax.TickDir         = "both";
ax.XMinorTick      = "on";
ax.YMinorTick      = "on";
ax.Box             = "off";

hold on;
% Pendulum state vector
fanimator(ax,@(t) plot([0 x(t)],[0 y(t)],'ko-','MarkerFaceColor','k','LineWidth',1));
fanimator(ax,@(t) rectangle('Position',[x(t)-pdl.R y(t)-pdl.R 2*pdl.R 2*pdl.R],'Curvature', [1 1]));
%fanimator(ax,@(t) quiver(x(t),y(t), vx(t), vy(t),'k-'));

%% Forces & pseudo forces
fanimator(ax,@(t) quiver(x(t),y(t), Wx(t), Wy(t),'g-'));
fanimator(ax,@(t) quiver(x(t),y(t), Tx(t), Ty(t),'b-'));
fanimator(ax,@(t) quiver(x(t),y(t),fcx(t),fcy(t),'r:'));
fanimator(ax,@(t) quiver(x(t),y(t), Dx(t), Dy(t),'r-'));
fanimator(ax,@(t) quiver(x(t),y(t), Sx(t), Sy(t),'b-'));

%% labels
xlim = ax.XLim;
ylim = ax.YLim;
fanimator(ax,@(t) text(xlim(1),      0,"t = "     +num2str(t,'%4.2f')+" s"                                      ,'HorizontalAlignment','center','VerticalAlignment','bottom','Color','k'));
fanimator(ax,@(t) text(xlim(1),ylim(2),"r = ["    +num2str(x(t)  ,'%+4.2f')+" "+num2str( y(t) ,'%+4.2f')+"] m"  ,'HorizontalAlignment','right' ,'VerticalAlignment','bottom','Color','k'));
fanimator(ax,@(t) text(xlim(1),ylim(2),"V = ["    +num2str(vx(t) ,'%+4.2f')+" "+num2str(vy(t) ,'%+4.2f')+"] m/s",'HorizontalAlignment','right' ,'VerticalAlignment','top'   ,'Color','k'));
fanimator(ax,@(t) text(xlim(1),ylim(1),"D = ["    +num2str(Dx(t) ,'%+4.2f')+" "+num2str(Dy(t) ,'%+4.2f')+"] N"  ,'HorizontalAlignment','left'  ,'VerticalAlignment','top'   ,'Color','r'));
fanimator(ax,@(t) text(xlim(1),ylim(1),"S = ["    +num2str(Sx(t) ,'%+4.2f')+" "+num2str(Sy(t) ,'%+4.2f')+"] N"  ,'HorizontalAlignment','left'  ,'VerticalAlignment','bottom','Color','b'));
fanimator(ax,@(t) text(      0,ylim(1),"W = ["    +num2str(Wx(t) ,'%+4.2f')+" "+num2str(Wy(t) ,'%+4.2f')+"] N"  ,'HorizontalAlignment','left'  ,'VerticalAlignment','top'   ,'Color','g'));
fanimator(ax,@(t) text(xlim(2),ylim(2),"\theta = "+num2str(rad2deg(theta(t)),'%+4.2f')+"^\circ"                 ,'HorizontalAlignment','right' ,'VerticalAlignment','top'   ,'Color','k'));
fanimator(ax,@(t) text(xlim(2),ylim(2),"\omega = "+num2str(omega(t),'%+4.2f')+" rad/s"                          ,'HorizontalAlignment','right' ,'VerticalAlignment','bottom','Color','k'));
fanimator(ax,@(t) text(xlim(2),ylim(1),"T = ["    +num2str(Tx(t) ,'%+4.2f')+" "+num2str(Ty(t) ,'%+4.2f')+"] N"  ,'HorizontalAlignment','left'  ,'VerticalAlignment','top'   ,'Color','b'));
fanimator(ax,@(t) text(xlim(2),ylim(1),"f_c = ["  +num2str(fcx(t),'%+4.2f')+" "+num2str(fcy(t),'%+4.2f')+"] N"  ,'HorizontalAlignment','left'  ,'VerticalAlignment','bottom','Color','r'));
hold off

playAnimation;
%writeAnimation(h,'pendulum.gif');
end

%% Pendulum
% The function \tt pendulum (defined at the end of the script) automatically calculates the following values and adds them to the structure \tt pdl:
% \tt A: the reference area (e.g., the cross-section) A=\pi R^2 in \rm m^2.
% \tt V: the volume of the pendulum V=\frac{4\pi R^3}{3} in \rm m^3.
% \tt m: the mass of the pendulum m=\rho V in \rm kg.
% \tt w0: the proper or "natural" angular frequency of the pendulum \Omega_0=\sqrt{\frac{g}{l}} in \rm rad/s.
% \tt T%: the period of the natural oscillations T=\frac{2\pi}{\omega_0} in \rm s.
%% There is NO change required to this section.

function pdl = pendulum(rho,R,l,Cd,env)
%% properties of a pendulum:
% * rho: mass density (kg/m^3)
% * R: radius (m)
% * l: length (m)
% * Cd: drag coefficient (unitless)
% * env: environmental parameters

g       = env.g;       % m/s^2
pdl.Cd  = Cd;          % sphere
pdl.rho = rho;         % kg/m^3
pdl.l   = l;           % m
pdl.R   = R;           % m
pdl.A   = pi*R^2;      % m^2
pdl.V   = 4/3*pi*R^3;  % m^3
pdl.m   = rho*pdl.V;   % kg
pdl.w0  = sqrt(g/l);   % rad/s
pdl.T   = 2*pi/pdl.w0; % s
end

%% Equation of motion
% This section is the implementation of Newton's second law \frac{d\vec{v}}{dt}=\frac{\sum\vec{F}}{m}.
%% There is NO change required to this section.

function dX = eom(~,X,pdl,env)
% Dynamic variables
theta = X(1); % rad
omega = X(2); % rad/s

% Pendulum
l   = pdl.l; % m
m   = pdl.m; % kg

% Force
W   = weight(pdl,env); % N
D   = drag(pdl,omega,env); % N
S   = Stokes(pdl,omega,env); % N
vth = l*omega;

% Angular acceleration
alpha = (-W*sin(theta) - D*sign(vth) - S*sign(vth))/(m*l); % rad/s^2

% output
dX   = [omega;alpha];
end

%% Forces
%% Fluid drag
% This function calculates the magnitude of the fluid drag D.
%% There is NO change required to this section.


function D = drag(pdl,w,env)
rho = env.rho;            % kg/m^2
l   = pdl.l;              % m
A   = pdl.A;              % m^2
Cd  = pdl.Cd;             % unitless
vth = l*w;                % m/s
D   = 1/2*rho*Cd*A*vth^2; % N
end

%% Weight
% This function calculates the magnitude of the weight W.
%% There is NO change required to this section.

function W = weight(pdl,env)
g = env.g; % m/s^2
m = pdl.m; % kg
W = m*g;   % N
end

%% Centrifugal pseudo-force
% This function calculates the magnitude of the centrifugal pseudoforce f_{\rm c}.
%% There is NO change required to this section.


function fc = centrifugal(pdl,w)
l  = pdl.l;   % m
m  = pdl.m;   % kg
fc = m*l*w^2; % N
end

%% Stokes's drag
% This function calculates the magnitude of Stokes's drag.
%% You will need to modify it.

function f = Stokes(pdl,w,env) 
f = 0; % N
end

%% Tension
% This function calculates the magnitude of the tension force T.
%% There is NO change required to this section.

function T = tension(pdl,th,w,env)
W  = weight(pdl,env);    % N
fc = centrifugal(pdl,w); % N
T  = W*cos(th) + fc;     % N
end

%% Conversion to Cartesian coordinates
% This function converts all forces from cylindrical to Cartesian coordinates.
%% There is NO change required to this section.

function [theta,omega,x,y,vx,vy,fcx,fcy,Tx,Ty,Wx,Wy,Dx,Dy,Sx,Sy] = cyl2cart(ode_sols,pdl,env)
l     = pdl.l;

% Dynamic Variables
theta = @(t) deval(ode_sols,t,1);
omega = @(t) deval(ode_sols,t,2);
vth   = @(t) l*omega(t);

% Forces
W   = weight(pdl,env); % N
D   = @(t) drag(pdl,omega(t),env); % N
S   = @(t) Stokes(pdl,omega(t),env); % N
fc  = @(t) centrifugal(pdl,omega(t)); % N
T   = @(t) tension(pdl,theta(t),omega(t),env); % N

% position
x   = @(t) l*cos(theta(t));
y   = @(t) l*sin(theta(t));
% speed & velocity
vx  = @(t)-vth(t)*sin(theta(t));
vy  = @(t) vth(t)*cos(theta(t));
% drag
Dx  = @(t)-D(t)*vx(t)/(vx(t)^2+vy(t)^2);
Dy  = @(t)-D(t)*vy(t)/(vx(t)^2+vy(t)^2);
% fluid friction
Sx  = @(t)-S(t)*vx(t)/(vx(t)^2+vy(t)^2);
Sy  = @(t)-S(t)*vy(t)/(vx(t)^2+vy(t)^2);
% weight
Wx  = @(t) W;
Wy  = @(t) 0;
% centrifugal force
fcx = @(t) fc(t)*cos(theta(t));
fcy = @(t) fc(t)*sin(theta(t));
% tension
Tx  = @(t)-T(t)*cos(theta(t));
Ty  = @(t)-T(t)*sin(theta(t));
end

%% Runge-Kutta solver
% This function solves the differential equation from Newton's second law using a Runge-Kutta solver.
%% There is NO change required to this section.

function [t,X,sols] = pendulumS(tspan,X0,pdl,env)
opts     = odeset('RelTol',1e-12,'AbsTol',1e-12);
sols     = ode45(@(t,X)eom(t,X,pdl,env),tspan,X0,opts);
t        = sols.x;
theta    = sols.y(1,:);
theta_t  = sols.y(2,:);
X        = [theta;theta_t];
end