% Movie of double pendulum EOM, dp.m
%
% CCK 2016-Sep-21, revised 2022-Apr-04

% Physical parameters
global L M g
L = 0.25   % (cm) Length of both pendulum segments
M = 0.0005   % (g) Mass of both pendulum bobs
g = 981   % (cm/s^2) Acceleration of gravity
h = 6.62607554e-27; % (erg s) Planck constant

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

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

% Setup for LSODE
lsode_options("integration method", "adams");
lsode_options("absolute tolerance", eps/10);
lsode_options("relative tolerance", eps);
lsode_options("maximum order", 8);
   % -1 means no limit (Adams goes up to 8). Energy is well conserved with max order 4-8,
   % but with more roundoff error at order 4 because more steps are required.
   % I suspect the energy conservation is a result of the way the ODEs are formulated, 
   % using canonical momenta conjugate to the angular variables. -CCK 2018-03-21
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 %%%%
% Want a perturbation commensurate with Heisenberg uncertainty principle. That is,
%     delta_theta1 * delta_p1 = h / 4 / pi
%     delta_theta2 * delta_p2 = h / 4 / pi
% Estimate typical values for theta and p.
typ_theta1 = 1               % Typical value for theta1
typ_theta2 = 1               % Typical value for theta2
typ_p1 = mean(abs(traj1p1))  % Typical value for p1
typ_p2 = mean(abs(traj1p2))  % Typical value for p2
% Work out QM uncertainty in theta and p, so that each will have an uncertainty
% proportional to the typical value. That is, we want:
%     delta_theta1 / typ_theta1 = delta_p1 / typ_p1
%     delta_theta2 / typ_theta2 = delta_p2 / typ_p2
delta_theta1 = sqrt( (typ_theta1/typ_p1)*h/4/pi )
delta_theta2 = sqrt( (typ_theta2/typ_p2)*h/4/pi )
delta_p1 = sqrt( (typ_p1/typ_theta1)*h/4/pi )
delta_p2 = sqrt( (typ_p2/typ_theta2)*h/4/pi )
% Check:
fractional_uncertainty_theta1 = delta_theta1/typ_theta1
fractional_uncertainty_theta2 = delta_theta2/typ_theta2
fractional_uncertainty_p1 = delta_p1/typ_p1
fractional_uncertainty_p2 = delta_p2/typ_p2
% Apply the perturbation:
state0 += [delta_theta1, delta_theta2, delta_p1, delta_p2] .* randn(1,4);

% 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);
traj2p1     = trajectory2(:,3);
traj2p2     = trajectory2(:,4);


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_2, traj 1','\theta_1, traj 2','\theta_2, traj 2','Location','NorthWest')
print('dpmovie_trajectories.pdf', '-dpdfwrite');

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


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

figure(4)
hold off
distances = sqrt( (traj2theta1-traj1theta1).^2 + (traj2theta2-traj1theta2).^2 +...
    ((traj2p1-traj1p1)/typ_p1).^2 + ((traj2p2-traj2p2)/typ_p2).^2 );
semilogy(times, distances)
xlabel('time (s)')
ylabel('phase space distance')
print('dpmovie_distance.pdf', '-dpdfwrite');


start_time = cputime; % Monitor movie rendering time

% Movie
system('rm /tmp/dp*.png')
system('mv ./dpmovie.mp4 ./dpmovie_previous.mp4')
figure(10)
for t=1:Nt

   % Fulcrum position:
   xf = 0;
   yf = 0;
   
   % Positions for pendulum bobs 1 and 2, in cm:
   traj1x1 = (  L*sin(traj1theta1(t)) );
   traj1y1 = ( -L*cos(traj1theta1(t)) );
   traj1x2 = (  L*sin(traj1theta2(t)) ) + traj1x1;
   traj1y2 = ( -L*cos(traj1theta2(t)) ) + traj1y1;
   % Same for trajectory 2
   traj2x1 = (  L*sin(traj2theta1(t)) );
   traj2y1 = ( -L*cos(traj2theta1(t)) );
   traj2x2 = (  L*sin(traj2theta2(t)) ) + traj2x1;
   traj2y2 = ( -L*cos(traj2theta2(t)) ) + traj2y1;
   
   hold off
   plot([xf],[yf], 'sb', 'markerfacecolor', 'b')                      % fulcrum
   title('Double Pendulum', 'fontsize', 12)
   xlabel('x (cm)', 'fontsize', 9)
   ylabel('y (cm)', 'fontsize', 9)
   hold on
   plot([xf,traj1x1,traj1x2],[yf,traj1y1,traj1y2],'-g', 'linewidth', 2) % both pendulum rods
   plot([traj1x1],[traj1y1], 'og', 'markersize', 10, 'markerfacecolor', 'g')    % bob 1
   plot([traj1x2],[traj1y2], 'og', 'markersize', 10, 'markerfacecolor', 'g')    % bob 2
   plot([xf,traj2x1,traj2x2],[yf,traj2y1,traj2y2],'-b', 'linewidth', 2) % both pendulum rods
   plot([traj2x1],[traj2y1], 'ob', 'markersize', 10, 'markerfacecolor', 'b')    % bob 1
   plot([traj2x2],[traj2y2], 'ob', 'markersize', 10, 'markerfacecolor', 'b')    % bob 2
   
   text( -2*L, 2*L, sprintf( 't = %05.3f s', times(t) ), 'fontsize', 9 ) % time label
   
   axis equal
   axis(L*[-2.2,2.2,-2.2,2.2])

   image_file_name = [ '/tmp/dp', sprintf('%05i',t), '.png' ];
   print(image_file_name, '-dpng');
endfor

% Now make the movie. ImageMagick and ffmpeg are required!
system('ffmpeg -r 60 -i /tmp/dp%05d.png -f mp4 -vcodec libx264 -pix_fmt yuv420p -qscale 1 ./dpmovie.mp4')

movie_rendering_time = cputime - start_time

