%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Compute trajectory of double pendulum
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

tspan		= [0:0.01:10];			% time boundaries, note the syntax for explicit time step
theta10			= pi/2;			% initial angle theta1
theta1_dot0		= 0;			% initial velocity theta1_dot
theta20			= pi/2;			% initial angle theta2
theta2_dot0		= 0;			% initial velocity theta2_dot

%initial state 
x0		= [theta10;theta1_dot0;theta20;theta2_dot0];

%the only real parameter in those equations is 'g/l', that we
%declare as a global veriable:
global g_L 
g_L=10;

%Call to the matlab solver: 
[t,x]	= ode45('f',tspan, x0);


%shotcuts for components of vector state
x1	= x(:,1);
x2	= x(:,2);
x3	= x(:,3);
x4	= x(:,4);


%let's plot the 2 positions on one plot
plot(t,x1,t,x3);
xlabel('time in s'); ylabel('positions (rad)'); 
legend('\theta_1', '\theta_2');
title('double pendulum');


%Let's produce an animated plot: 
figure
axis([-2 2 -2 2]);
s1 = sin(x1(1));c1=cos(x1(1));s2=sin(x2(1));c2=cos(x2(1));
link1 = line('xdata', [0, s1], 'ydata', [0, -c1]);
link2 = line('xdata', [s1, s1+s2], 'ydata', [-c1, -(c1+c2)]);
for i=1:length(x1)
  s1 = sin(x1(i));c1=cos(x1(i));s2=sin(x3(i));c2=cos(x3(i));
  set(link1,'xdata',  [0, s1], 'ydata', [0, -c1]);
  set(link2,'xdata', [s1, s1+s2], 'ydata', [-c1, -(c1+c2)]);
  drawnow;
end