%{
Initial value problem for a second-order ODE
Alex G. March 2022
%}

clear;
clc;
close;

tolerance = 1.e-6;
StepsMax  = 1000;
% chosing versions
KeyA = 1;       % Step size-control: 0 same step, 1 step-size control
KeyM = 2;       % Method: 1 - Explicit Euler, 2 - RKF 45
if KeyA == 1
  KeyM = 2;
end

Method(1) = "Explicit Euler";
Method(2) = "RKF45";
Control(1)= "No step-control";
Control(2)= "With step-control";

fprintf('Solving second-order ODE \n')
fprintf('Method:    %s \n',Method(KeyM))
fprintf('Step-size: %s \n',Control(KeyA+1))


% 1. initial conditions
ti = 0.0;
xi = 0.0;
yi = 1.0;

%%ei = (yi.^2 + xi^2)/2;   % energy for a harmonic oscillator

% 2: "time" step and max time
dt =   0.4;
tmax = 6*pi;

% 3: getting ready for the main loop
i    = 1;
step = 1;
t(i) = ti;
x(i) = xi;
y(i) = yi;

% 4a: the MAIN part with step-size control
if KeyA == 0
  while (ti < tmax)
    i = i+1;
    tf = ti + dt;
    if KeyM == 1
      [xf,yf]         = euler2(ti,xi,yi,tf);
    elseif KeyM == 2
      [xf,yf,erx,ery] = RKF52 (ti,xi,yi,tf);
    end
      t(i) = tf;
      x(i) = xf;
      y(i) = yf;
      ti = tf;
      xi = xf;
      yi = yf;
    step = step + 1;
    if step > StepsMax
      fprintf(' Max number of steps exceeded at tf = %8.4f \n',tf)
      break
    end
  end
else
% 4b: the MAIN part (same step-size)
  while (ti < tmax)
    tf = ti + dt;
    [xf,yf,erx,ery] = RKF52(ti,xi,yi,tf);
    er5 = max(abs(erx),abs(ery));
    if (er5 <= tolerance)
      i = i + 1;
      t(i) = tf;
      x(i) = xf;
      y(i) = yf;
      ti = tf;
      xi = xf;
      yi = yf;
      dt = min(0.9*dt*(abs(tolerance/er5)).^(1/5),2.0*dt); 
    else
      dt = max(0.9*dt*(abs(tolerance/er5)).^(1/5),0.5*dt);
    end
    step = step + 1;
    if step > StepsMax
      fprintf(' Max number of steps exceeded at tf = %8.4f \n',tf)
      break
    end
  end
end
% END of the main loop

% figure (1)
figure('DefaultAxesFontSize',14)
plot(t,x,'b-o',t,y,'r-o','LineWidth',1.0)
legend('x(t)','v(t)')
xlabel('t')
ylabel('x(t) and v(y)')
axis([0 tmax -1.2 1.2])
grid

% check for energy if needed (harmonic oscilltor)
% % for j = 1:i
%   e(j) = 0.5*(x(i).^2+(y(i).^2));
% end
% % figure (2)
% plot(t,e)


function dx = dx(t,x,v)
%--------------------------------------
% function dx/dt
%--------------------------------------
	dx = v +t*0.0 +x*0.0;
end
function dy = dy(t,x,v)
%--------------------------------------
% function d2x/dt2 (second equation)
%--------------------------------------
% simple harmonic oscillator 
k=1.0;
m=1.0;
  dy = (-1.0*k/m)*x +t*0.0-v*0.2;    
end


function [xf,yf,erx,ery] = RKF52(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