% Dormand Prince

% We are creating a function that approximates values of ODEs using two linear
% combinations of seven different slopes. This function will approximate two
% values for each iteration, one of 4th order and one of 5th based on weighted
% averages. These values will be compared to increment size to determine the
% most accurate value possible

% Parameters
% ==========
%   f = f(t,y): The function f is the bivariate function (ODE) to be
%   approximated
%   t_rng: The range of input values
%   y0: The initial output value
%   h: The increment value between approximations.
%   eps_abs: The minimum absolute value of error required to terminate funtion
%
% Return Values
% =============
%   t_out: A row vector of n amount equally spaced values from t0 to tfinal
%   y_out: A row vector of n amount of values where:
%     y_out(1) = y0
%     y_out(i) approximates the remaining output values
%
% Description
% ==============
% Initialize weighted variables
% Define remaining variables
% Iterate until t final is found
% Calculate 7 DP slopes
% Estimate 4th and 5th order values
% Calculate scaling factor to interpret h
% Find next t value
% Determine if h is usable
% Determine y_out if h is acceptable
% If h doesnt work, half it
% Update counter

function [t_out, y_out] = dp45(f, t_rng, y0, h, eps_abs)

if ~isa( f, 'function_handle' )
        throw( MException( 'MATLAB:invalid_argument', ...
        'Invalid f: Not Funtion Handle' ) );
end
if ~all( size( t_rng ) == [1,2] )
        throw( MException( 'MATLAB:invalid_argument', ...
        'Invalid t_rng: Not a 3D column vector' ) );
end

if ~ismatrix(y0)
        throw( MException( 'MATLAB:invalid_argument', ...
        'Invalid y0: Not a column vector' ) );
end

[N,y0_col] = size(y0);
if ~(y0_col == 1)
        throw( MException( 'MATLAB:invalid_argument', ...
        'Invalid y0: Not a column vector ' ) );
end

if ~isscalar( h )
        throw( MException( 'MATLAB:invalid_argument', ...
        'Invalid h: Not a scalar' ) );
end

if ~isscalar( eps_abs ) || eps_abs < 0
        throw( MException( 'MATLAB:invalid_argument', ...
        'Invalid eps_abs: Not a positive scalar' ) );
end

% Initialize weighted variables
A = [ 0          0         0        0         0        0   0;
      1          0         0        0         0        0   0;
     1/4        3/4        0        0         0        0   0;
    11/9      -14/3      40/9       0         0        0   0;
  4843/1458 -3170/243  8056/729  -53/162      0        0   0;
  9017/3168  -355/33  46732/5247  49/176 -5103/18656   0   0;
    35/384       0      500/1113 125/192 -2187/6784  11/84 0]';

by = [5179/57600 0 7571/16695 393/640 -92097/339200 187/2100 1/40]';
bz = [  35/384   0  500/1113  125/192  -2187/6784    11/84    0 ]';

c = [0 1/5 3/10 4/5 8/9 1 1]';

% Define remaining variables
t_out(1) = t_rng(1);
y_out = y0;
K = zeros( N,7);
i = 1;

% Iterate until t final is found
while t_out(i) < t_rng(2)
    %Calculate 7 DP slopes
    for m = 1:7
         K(:, m) = f(t_out(i) + h*c(m), y_out(:, i) + h*c(m)*K*A(:, m) );
    end
    %Estimate 4th and 5th order values
    y_t = y_out(:,i) + h*K*by;
    z_t = y_out(:,i) + h*K*bz;

    e = norm(y_t - z_t);
    r = t_rng(2) - t_rng(1);

    % Calculate scaling factor to interpret h

    s = ((h*eps_abs)/((2*r*e)))^(1/4);
    %Find next t value
    %Determine if h is usable
    %Determine y_out if h is acceptable
    %If h doesnt work, half it
    %Update counter
    if s >= 2
        y_out(:,i+1) = z_t;
        t_out(i+1) = t_out(i) + h;
        h = h * 2;
        i = i + 1;
    elseif s >=1
        y_out(:,i+1) = z_t;
        t_out(i+1) = t_out(i) + h;
        i = i + 1;
    else
        h = h / 2;
    end

    if (t_out(i) + h) > t_rng(2)
        h = (t_rng(2) - t_out(i));
    end

end

end