% Control3_3.m
% Solution to problem 3 of control3. 2023/05/05
% J.M.Soler, May 2023

% Set parameters
clear
mass = 80;         % jumper mass (kg)
dt = 1e-2;         % integration time interval (s)
tmax = 25;         % simulation time (s)
r0 = [ 0; 1; 0] ;  % initial position (m)
v0 = [ 3; 1; 4 ];  % initial velocity (m/s)

% Integrate trajectory
t = 0:dt:tmax;
[r,v,a] = newtonRK_JMS( @JumperForce_JMS, mass, r0, v0, dt, tmax );

% Plot trajectory
figure(11)
nt = numel(t);
it = 1:10:nt;      % plot point every 0.1 s (10 time steps)
plot3(r(1,it),r(2,it),r(3,it),'o-')
axis equal
xlabel('x (m)')
ylabel('y (m)')
zlabel('z (m)')
grid on
set(gca,'FontSize',15)

% Find energy and works
for it = 1:nt
  [ftot,fel(:,it),ffr(:,it),ep] = JumperForce_JMS( r(:,it), v(:,it), t(it) );
  e(it) = ep + 0.5*mass*sum(v(:,it).^2);   % total mecanical energy
end
wel = integral( t, sum(fel.*v) );             % work of elastic force
wfr = integral( t, sum(ffr.*v) );             % work of friction

% Plot energy and works
figure(12)
plot( t,e, t,wel, t,wfr, t,e-wfr, 'LineWidth',2 )
legend('E_{tot}','W_{el}','W_{fr}','E_{tot}+W_{fr}')
xlabel('time (s)')
ylabel('energy (J)')
grid on
set(gca,'FontSize',15)
