%{
Simple pendulum with dissipative and driven forces
as initial value problem for a second-order ODE
Alex G. March 2022
%}

clear;
clc;
close;

% function hadles
dx = @dx1;
dv = @dv1;
dvx =@dv2;

global mass L omega0 alpha force omega
% physics data
mass =  1.0;            % mass
L    = 10.0;            % lenght
g    = 10.0;            % free-fall acceleration
alpha=  2.0;
force=  0.0;
omega0 = sqrt(g/L);     % natural angular frequency
omega=  0.697*omega0; 
T0   = 2.0*pi/omega0;   % natural period

% if calculating with step-size control
tolerance = 1.e-5;
StepsMax  = 100000;

% 1. initial conditions
ti = 0.0;
xi = 0.2;
vi = 0.0;
xi2 = xi;
vi2 = vi;

% 2: "time" step and max time
dt =   0.05;
tmax = 24*pi;

% 3: getting ready for the main loop
i    = 1;
step = 1;
t(i) = ti;
x(i) = xi;
v(i) = vi;
x2(i) = xi;
v2(i) = vi;

while (ti < tmax)

  tf = ti + dt;
  i = i + 1;
%   alpha = 0.2;
%   force = 0.7;
%   omega = 0.66*omega0; 

  [xf,vf,erx,ery] = RKF52(dx,dv,ti,xi,vi,tf);
  er5 = max(abs(erx),abs(ery));
  x(i) = xf;
  v(i) = vf;
  xi = xf;
  vi = vf;

%   alpha = 0.2;
%   force = 0.7;
%   omega = 0.66*omega0; 
  [xf2,vf2,erx,ery] = RKF52(dx,dvx,ti,xi2,vi2,tf);  
  x2(i) = xf2;
  v2(i) = vf2;
  xi2 = xf2;
  vi2 = vf2;

  t(i) = tf;
  ti = tf;

  step = step + 1;
  if step > StepsMax
    fprintf(' Max number of steps exceeded at tf = %8.4f \n',tf)
    break
  end
end
% END of the main loop

% figure (1)
figure('DefaultAxesFontSize',14)
%plot(t,x,'r',t,x2,'b','LineWidth',1.0)
plot(t,x,'r','LineWidth',1.0)
title('Amplitude')
%  legend('x0=1.0','x0=1.0001')
xlabel('t')
ylabel('amplitude')
% axis([0 tmax -1.2 1.2])
grid


% % figure (1)
% figure('DefaultAxesFontSize',14)
% %plot(t,x,'r',t,x2,'b','LineWidth',1.0)
% plot(t,x2,'r','LineWidth',1.0)
% title('Amplitude')
% %  legend('x0=1.0','x0=1.0001')
% xlabel('t')
% ylabel('amplitude')
% % axis([0 tmax -1.2 1.2])
% grid


figure('DefaultAxesFontSize',14)
%plot(x,v,'r',x2,v2,'b','LineWidth',1.0)
plot(x,v,'r','LineWidth',1.0)
title('Phase space')
% legend('Dissipation','Harmonic')
xlabel('amplitude')
ylabel('velocity')
%axis([0 tmax -1.2 1.2])
grid

% figure('DefaultAxesFontSize',14)
% %plot(x,v,'r',x2,v2,'b','LineWidth',1.0)
% plot(x2,v2,'r','LineWidth',1.0)
% title('Phase space')
% % legend('Dissipation','Harmonic')
% xlabel('amplitude')
% ylabel('velocity')
% %axis([0 tmax -1.2 1.2])
% grid

function dx = dx1(t,x,v)
%--------------------------------------
% function dx/dt
%--------------------------------------
  dx = v + t*0.0 + x*0.0;
end
function dv = dv1(t,x,v)
%--------------------------------------
% function d2x/dt2 
%--------------------------------------
global mass L omega0 alpha force omega
  dv = (-1.0*omega0^2)*sin(x) - (alpha/mass)*v + (force/mass)*cos(omega*t);
end
function dv2 = dv2(t,x,v)
%--------------------------------------
% function d2x/dt2 
%--------------------------------------
global mass L omega0 alpha force omega
  dv2 = (-1.0*omega0^2)*x - (alpha/mass)*v + (force/mass)*cos(omega*t);
end


function [xf,yf,erx,ery] = RKF52(dx,dy,ti,xi,yi,tf)
%{
Solution of a single 2nd order ODE 
Method:  Runge-Kutta-Fehlberg RKF45
Alex G. March 2022
--------------------------------------------------
external functions ...
dx(t,x,y)- function dx/dt (supplied by a user)
dy(t,x,y)- function dy/dt (supplied by a user)
input ...
ti - initial time
xi  - initial value
yi  - initial value
tf  - time for a solution
output ...
xf  - solution at point tf
yf  - solution at point tf
erx and ery are error estimate
%}
h  = tf-ti;
k1x = h*dx(ti,xi,yi);
k1y = h*dy(ti,xi,yi);
k2x = h*dx(ti+h/4.0,  xi+k1x/4.0, yi+k1y/4.0);
k2y = h*dy(ti+h/4.0,  xi+k1x/4.0, yi+k1y/4.0);
k3x = h*dx(ti+3*h/8,  xi+3*k1x/32+9*k2x/32, yi+3*k1y/32+9*k2y/32);
k3y = h*dy(ti+3*h/8,  xi+3*k1x/32+9*k2x/32, yi+3*k1y/32+9*k2y/32);
k4x = h*dx(ti+12*h/13,xi+1932*k1x/2197-7200*k2x/2197+7296*k3x/2197, ...
                      yi+1932*k1y/2197-7200*k2y/2197+7296*k3y/2197);
k4y = h*dy(ti+12*h/13,xi+1932*k1x/2197-7200*k2x/2197+7296*k3x/2197, ...
                      yi+1932*k1y/2197-7200*k2y/2197+7296*k3y/2197);
k5x = h*dx(ti+h,      xi+439*k1x/216-8*k2x+3680*k3x/513-845*k4x/4104, ...
                      yi+439*k1y/216-8*k2y+3680*k3y/513-845*k4y/4104);
k5y = h*dy(ti+h,      xi+439*k1x/216-8*k2x+3680*k3x/513-845*k4x/4104, ...
                      yi+439*k1y/216-8*k2y+3680*k3y/513-845*k4y/4104);
k6x = h*dx(ti+h/2,    xi-8*k1x/27+2*k2x-3544*k3x/2565+1859*k4x/4104-11*k5x/40, ...
                      yi-8*k1y/27+2*k2y-3544*k3y/2565+1859*k4y/4104-11*k5y/40);
k6y = h*dy(ti+h/2,    xi-8*k1x/27+2*k2x-3544*k3x/2565+1859*k4x/4104-11*k5x/40, ...
                      yi-8*k1y/27+2*k2y-3544*k3y/2565+1859*k4y/4104-11*k5y/40);
xf = xi + 16*k1x/135+6656*k3x/12825+28561*k4x/56430-9*k5x/50+2*k6x/55;
yf = yi + 16*k1y/135+6656*k3y/12825+28561*k4y/56430-9*k5y/50+2*k6y/55;
erx = k1x/360-128*k3x/4275-2197*k4x/75240+k5x/50+2*k6x/55;
ery = k1y/360-128*k3y/4275-2197*k4y/75240+k5y/50+2*k6y/55;
end


function [xf,yf] = euler2(ti,xi,yi,tf)
%{
==================================================
Solution of a single 2nd order ODE
Method:  Simple Euler
Alex G. October 2020
--------------------------------------------------
external functions ...
dx(t,x,y) and dy(t,x,y) - functions supplied by a user
input ...
ti - initial time
xi  - initial position
tf  - time for a solution
output ...
xf  - solution at point tf
yf  - solution at point tf
==================================================
%}
xf = xi + dx(ti,xi,yi)*(tf-ti);
yf = yi + dy(ti,xi,yi)*(tf-ti);
end