%% Double Pendulum 
% Rosa Nguyen
% EPS 109 Project
% Using Runge Kutta to solve the ODE
% This simulation is with a pendulum that has the weight at the end of the
% rod as opposed to the mass being evenly distributed across the rod.

clear
clc

global L1 L2 m1 m2 g
L1=6;
L2=5;
m1=5;
m2=5;
g=9.81;

% set initial conditions
theta1_0=pi/4;
dtheta1=0;
theta2_0=-pi/8;
dtheta2=5;
% positions are at the end of each rod
x1_0=L1*sin(theta1_0);
y1_0=-L1*cos(theta1_0);
x2_0=L2*sin(theta2_0)+x1_0; % so the position of the second rod depends on the extent of the first rod, thus it is added
y2_0=-L2*cos(theta2_0)+y1_0;

%y column vector
y=[theta1_0;dtheta1;theta2_0;dtheta2];

% time
t    =  0.0;
tmax = 20;
dt   =  0.02;
n    = tmax/dt; % number of steps

tt = zeros(n+1,1);
yt = zeros(n+1,size(y,1));
tt(1)   = t;
yt(1,:) = y';

%initial plot
x1=[0,x1_0];
y1=[0,y1_0];
x2=[x1_0,x2_0];
y2=[y1_0,y2_0];
plot(0,0,'d',x1,y1,'b',x2,y2,'k',x1,y1,x2,y2,'*','LineWidth',2)
axis([-12.5,12.5,-12.5,12.5])
for i=1:n
    f1 = ode2(t     , y                );
    f2 = ode2(t+dt/2, y+f1 .*  (dt/2)  );
    f3 = ode2(t+dt/2, y+f2 .*  (dt/2)  );
    f4 = ode2(t+dt  , y+f3 .*   dt     );
   
    y = y + (f1 + f2.*2 + f3.*2 + f4) .* (dt/6);
    
    t = t+dt;
    tt(i+1)   = t;  % this records the time at each step
    yt(i+1,:) = y'; % this records the vector Y at each step --> Yt(:,1) has all x coordinates
    x1=[0,L1*sin(yt(i+1,1))];
    x2=[L1*sin(yt(i+1,1)),L1*sin(yt(i+1,1))+L2*sin(yt(i+1,3))];
    y1=[0,-L1*cos(yt(i+1,1))];
    y2=[-L1*cos(yt(i+1,1)),-L1*cos(yt(i+1,1))-L2*cos(yt(i+1,3))];
    plot(0,0,'d',x1,y1,'b',x2,y2,'k',x1,y1,x2,y2,'*','LineWidth',2)
    axis([-12.5,12.5,-12.5,12.5])
    pause(.01)
frame = getframe(1);
im = frame2im(frame);
[imind,cm] = rgb2ind(im,256);
if i == 1;
imwrite(imind,cm,'double_pendulum','gif', 'Loopcount',inf);
else
imwrite(imind,cm,'double_pendulum','gif','WriteMode','append');
end
% gif creation found at: http://www.mathworks.com/support/solutions/en/data/1-48KECO/?solution=1-48KECO
end

%% Other sources
% http://myphysicslab.com/dbl_pendulum.html
% http://en.wikipedia.org/wiki/Double_pendulum
% http://scienceworld.wolfram.com/physics/DoublePendulum.html
