%====================================== 
%  Solver for a system of n first order 
%  Ordinary Differential Equations 
%  (initial value problem)
% 
%  Can be used for solving a system of n/2 second order ODE
% 
%  Demo version for 2D projectile motion
% 
%  AG: Last revision November 2020
%======================================
clear  % Clears command window
clc    % Clears command history
clf    % Removes anything in the figure window before simulation. 

% global constatns and variables 
global g rad Pi D yatm
g   = 9.80;             % free fall acceleration m/s^2
rad = 0.017533;         % degree to radians
Pi  = 3.1415926;        % Pi

% initialize arrays
n = 4;                  % number of equations
xf = zeros(1,n);
xi = zeros(1,n);

%* print the header and initial data
Date = today('datetime');

fprintf('      4-nd order a system of n ODEs \n')
fprintf('      Method: Runge-Kutta 4th order \n')
fprintf('            Date %s \n', Date)

% === projectile ===
m  =  0.011;                % mass of a projectile
rho = 1.25;                 % air density
C =   0.02;                 % aerodynamic coefficient
r =   0.005;                % radius of projectile
A =   Pi*r^2;               % cross sectional area
D = 0.5*C*rho*A/m;          % drag coefficient
yatm = 10000;               % for varaible air density

% === Initial data for motion ===
ti    =  0.0;              % initial value for variable t
v0    = 320.0;              % initial speed
angle =  45.0;              % initial angle (degrees)
x0    =  0.0;              % initial position in x
y0    =  0.0;              % initial position in y

% preparing for calculations
xi(1) = x0;                
xi(2) = y0;                
xi(3) = v0*cos(angle*rad);  % initial speed in x direction
xi(4) = v0*sin(angle*rad);  % initial speed in y direction
energy= m*g*xi(2) + 0.5*m*(xi(3)*xi(3)+xi(4)*xi(4)); % initial energy

% estimate distance and time if no air resistance and ven ground
% or y0 = yf
Range        = v0^2 * sin(2*angle*rad) /g;
TimeofFlight = 2*v0*sin(angle*rad) /g;
Ymax         = v0^2 * (sin(angle*rad))^2 /(2*g);

%print initial data and analytic results
fprintf(' v0     = %9.2f m/s\n',v0)
fprintf(' angle  = %9.2f deg\n',angle)
fprintf(' estimated time   = %9.2f sec\n',TimeofFlight)
fprintf(' estimated ymax   = %9.2f meters\n',Ymax)
fprintf(' estimated range  = %9.2f meters\n',Range)

% printing ithe header
fprintf...
('      t         x(t)       y(t)      vx(t)       vy(t)     energy \n')

% step size and max time
dt   = 0.1; 
tmax = 300.0;
%tmax = TimeofFlight;       % integrate from ti till tmax

%preparation to plot results
ip = 1;
tp(ip) = ti;
xp(ip) = xi(1);
yp(ip) = xi(2);
%analytic solution
xa(ip) = xi(1);
ya(ip) = xi(2);

%=== integration of ODE 
while (ti < tmax)
fprintf('%10.2e %10.2e %10.2e  %10.2e  %10.2e  %10.2e\n',...
    ti, xi(1), xi(2), xi(3), xi(4), energy);
    if  xi(2) < 0.0, break, end      % break if hitting ground y=0
    tf = ti + dt;
% =================================*/
   xf = RK4n(n, ti, tf, xi);
% =================================*/

% prepare for the next step
    ti = tf;
    for i = 1: n
        xi(i) = xf(i);
    end
% arrys for figures  
    ip=ip+1;
    tp(ip) = ti;
    xp(ip) = xi(1);
    yp(ip) = xi(2);
% analytic solution
    xa(ip) = xa(1) + v0*cos(angle*rad)*tf;
    ya(ip) = ya(1) + v0*sin(angle*rad)*tf - 0.5*g*tf^2;
    
% end arrays for fugures     
    energy = m*g*xi(2) + 0.5*m*((xi(3))^2+ (xi(4))^2);
end %end of while

% Linear interpolation for the time of flight
tFlight = tp(ip) - yp(ip)*(tp(ip)-tp(ip-1))/(yp(ip)-yp(ip-1));
xRange  = xp(ip) - yp(ip)*(xp(ip)-xp(ip-1))/(yp(ip)-yp(ip-1));
fprintf('\n tFlight  = %9.3f seconds\n',tFlight)
fprintf('  xRange  = %9.3f meters\n',xRange)


plot(xp,yp,'r-o',xa,ya,'b-o')
xlabel('distance')
ylabel('height')
title ('2D Projectile Motion')

%Range = Range*1.1;
%Ymax  = Ymax *1.1;
%plot(xp,yp,'r',xa,ya,'b')
%xlim([0 Range])
%ylim([0 Ymax])
%xlabel('distance')
%ylabel('height')
%title ('2D Projectile Motion')
%end of program

function [dx] = dnx(n, t, x)
% function for 2D projectile motion 
global g rad Pi D yatm
    Dc = D * exp((-1.0*x(2)/yatm));
    v = sqrt(  (x(3))^2+(x(4))^2  ); 
% /* first order */
    dx(1) = x(3);
    dx(2) = x(4);
% /* second order */
    dx(3) = 0.0 - Dc * x(3)*v;
    dx(4) = (-1.0)*g - Dc * x(4)*v;
end 

%*==========================================================
% RK4n Solution of a system of n first-order ODE
% method:      Runge-Kutta 4th-order
% written by: Alex Godunov
% last revision: November, 2020
% ----------------------------------------------------------
% call ... (supplied by a user)
%   dx = dnx(n, t, x) functions dx/dt  where dx and x are arrays size n 
% input ...
%   n     - number of first order equations
%   ti    - initial time
%   tf    - solution time
%   xi    - initial values (array size n)
% output ...
%   xf    - solutions (array size n)
% % ==========================================================*/
function [xf] = RK4n(n,ti, tf, xi)
% reserve arrays 
xf = zeros(1,n);
k1 = zeros(1,4);
k2 = zeros(1,4);
k3 = zeros(1,4);
k4 = zeros(1,4);
% calculations
h = tf-ti;
t = ti;
% k1
dx=dnx(n, t, xi);
for j = 1: n
    k1(j) = h*dx(j);
    x(j)  = xi(j) + k1(j)/2.0;  
end     
% //k2
dx = dnx(n, t+h/2.0, x);
for j = 1: n
    k2(j) = h*dx(j);
    x(j)  = xi(j) + k2(j)/2.0;  
end
%//k3
dx =  dnx(n, t+h/2.0, x);
for j = 1: n
    k3(j) = h*dx(j);
    x(j)  = xi(j) + k3(j);  
end    
%//k4 and result      
dx = dnx(n, t+h, x);
for j = 1: n
    k4(j) = h*dx(j);
    xf(j) = xi(j) + k1(j)/6.0+k2(j)/3.0+k3(j)/3.0+k4(j)/6.0;
end    
end % end of RK4n