% Test of double pendulum EOM, dp.m
%
% CCK 2016-Sep-20

% Physical parameters
global L M g
L = 1.0;   % (cm) Length of both pendulum segments
M = 0.1;   % (g) Mass of both pendulum bobs
g = 981;   % (m/s^2) Acceleration of gravity

% Time axis
fps = 60   % frames per second
times = 0:(1/fps):5; % Time points for trajectories
Nt = numel(times);

%%%% FIRST TRAJECTORY %%%%
% Initial state
state0 = (3/4)*[pi, pi, 0, 0];

% Setup for LSODE
lsode_options("integration method", "adams");
lsode_options("absolute tolerance", eps);
lsode_options("relative tolerance", eps);
lsode_options

% Integrate equations of motion
start_time = cputime; % Monitor LSODE execution time
trajectory1 = lsode(@dp, state0, times);
traj1_time = cputime - start_time

traj1theta1 = trajectory1(:,1);
traj1theta2 = trajectory1(:,2);
traj1p1     = trajectory1(:,3);
traj1p2     = trajectory1(:,4);

% Calculate energy
KineticEnergy = ( traj1p1.^2 + 2*traj1p2.^2 - 2*traj1p1.*traj1p2.*cos(traj1theta2-traj1theta1) ) ./ ...
         ( 2*M*L^2*(2 - cos(traj1theta2 - traj1theta1).^2) );
PotentialEnergy = -M*g*L*( 2*cos(traj1theta1) + cos(traj1theta2) );
Energy = KineticEnergy + PotentialEnergy;




%%%% PERTURBED TRAJECTORY %%%%
state0 += 1e-6*[randn, randn, 0, 0];

% Integrate equations of motion
start_time = cputime; % Monitor LSODE execution time
trajectory2 = lsode(@dp, state0, times);
traj2_time = cputime - start_time

traj2theta1 = trajectory2(:,1);
traj2theta2 = trajectory2(:,2);





figure(1)
hold off
plot(times, traj1theta1, 'g-', times, traj1theta2, 'g--')
hold on
plot(times, traj2theta1, 'b-', times, traj2theta2, 'b--')
xlabel('time (s)')
ylabel('pendulum angles (rad)')
legend('\theta_1, traj 1','\theta_1, traj 2','\theta_2, traj 1','\theta_2, traj 2')

figure(2)
hold off
plot(times, KineticEnergy, 'm-', times, PotentialEnergy, 'k-', times, Energy, 'g-')
xlabel('time (s)')
ylabel('energy (erg)')
legend('Kinetic','Potential','Total')

figure(3)
hold off
plot(times, (Energy/Energy(1))-1, 'g-')
xlabel('time (s)')
ylabel('fractional energy change since t=0')

