%**************************************************************************
%*     Problem 5.3                                                        *
%*     filename: problem05_3.m                                            *
%*     Programed by Ryoichi Kawai for Computational Physics Course        *
%*     Revised on 02/07/2016.                                             *
%**************************************************************************
clear all;

% system parameters 
a = input('Enter value of a =>');

% Definition of four independent variables
% x1 = L
% x2 = dL/dt
% x3 = theta
% x4 = dtheta/dt = omega

% Right hand sides of ODEs.
F1 = @(X1,X2,X3,X4) X2;
F2 = @(X1,X2,X3,X4) X1*X4^2 - (X1-1) + a*cos(X3);
F3 = @(X1,X2,X3,X4) X4;
F4 = @(X1,X2,X3,X4) -X2*X4/X1 - a/X1*sin(X3);


% initial conditions
x1(1)=1.0;
x2(1)=0.0;
x3(1)=0.1;
x4(1)=0.0;
t(1)=0;
 
% control parameter
tmax=200;
N=10000;
h=tmax/N;

for n=1:N-1
    % 4th-order Runge-Kutta
    
    k11=F1(x1(n),x2(n),x3(n),x4(n));
    k21=F2(x1(n),x2(n),x3(n),x4(n));
    k31=F3(x1(n),x2(n),x3(n),x4(n));
    k41=F4(x1(n),x2(n),x3(n),x4(n));
    
    x1_mid = x1(n)+k11*h/2;
    x2_mid = x2(n)+k21*h/2;
    x3_mid = x3(n)+k31*h/2;
    x4_mid = x4(n)+k41*h/2;
    
    k12=F1(x1_mid,x2_mid,x3_mid,x4_mid);
    k22=F2(x1_mid,x2_mid,x3_mid,x4_mid);
    k32=F3(x1_mid,x2_mid,x3_mid,x4_mid);
    k42=F4(x1_mid,x2_mid,x3_mid,x4_mid);
    
    x1_mid = x1(n)+k12*h/2;
    x2_mid = x2(n)+k22*h/2;
    x3_mid = x3(n)+k32*h/2;
    x4_mid = x4(n)+k42*h/2;

    k13=F1(x1_mid,x2_mid,x3_mid,x4_mid);
    k23=F2(x1_mid,x2_mid,x3_mid,x4_mid);
    k33=F3(x1_mid,x2_mid,x3_mid,x4_mid);
    k43=F4(x1_mid,x2_mid,x3_mid,x4_mid);
    
    x1_end = x1(n)+k13*h;
    x2_end = x2(n)+k23*h;
    x3_end = x3(n)+k33*h;
    x4_end = x4(n)+k43*h;

    k14=F1(x1_end,x2_end,x3_end,x4_end);
    k24=F2(x1_end,x2_end,x3_end,x4_end);
    k34=F3(x1_end,x2_end,x3_end,x4_end);
    k44=F4(x1_end,x2_end,x3_end,x4_end);
    
    x1(n+1)=x1(n)+(k11+2*(k12+k13)+k14)*h/6;
    x2(n+1)=x2(n)+(k21+2*(k22+k23)+k24)*h/6;
    x3(n+1)=x3(n)+(k31+2*(k32+k33)+k34)*h/6;
    x4(n+1)=x4(n)+(k41+2*(k42+k43)+k44)*h/6;

    t(n+1)=t(1)+n*h;
end

% plot angular coordinate
subplot(1,3,1);
p=plot(t,x3);
xlabel('t');
ylabel(texlabel('theta'));
set(p(1),'Color','blue','Linewidth',2);

% plot angular coordinate
subplot(1,3,2);
q=plot(t,x1);
xlabel('t');
ylabel('length');
set(q(1),'Color','blue','Linewidth',2);

% trajectory of the bob in xy coordiates
x=x1.*sin(x3);
y=x1.*cos(x3)-1.0;
subplot(1,3,3);
plot(x,y);
xlabel('x');
ylabel('y');