function [time, x_sol, x_drv, y_sol] = ode4u( dxdt, time, x0, u, cnstnt )
%  [time, x_sol, x_drv, y_sol] = ode4u( dxdt, time, x0, u, cnstnt )
%
% Solve a system of nonhomogeneous ordinary differential equations using the 
% 4th order Runge-Kutta method.  
%
%  Input Variable      Description                         
%  --------------      ----------- 
%     dxdt           : a function of the form [x_dot,y] = dxdt(t,x,u,cnstnt)
%                      which provides the state derivative given the time, t,
%                      the state, x, external forcing, u, and constants
%                      a vector system outputs, y, may also be returned
%     time           : 1-by-p vector of time values, uniformly increasing
%                      (or decreasing), at which the solution is computed
%     x0             : n-by-1 vector of initial state values, at time(1)
%     u              : m-by-p matrix of system forcing data for the ode  
%                      sampled at points in the 1-by-p vector time
%     cnstnt         : optional set of constants for the function dxdt
%
% Output Variable      Description
% --------------       -----------
%     time           : is returned, un-changed.
%     x_sol          : the solution to the differential equation at 
%                      times given in the 1-by-p vector time.
%     x_drv          : the derivitive of the solution 
%     y_sol          : additional system outputs 
%
% Henri Gavin, Civil and Environmental Engineering, Duke Univsersity
% WH Press, et al, Numerical Recipes in C, 1992, Section 16.1

 points = length(time);
 if nargin < 5, cnstnt = 0;                 end
 if nargin < 4, u = zeros(1,points);        end

 [dxdt1,y1] = feval( dxdt, 0, x0, u(:,1), cnstnt ); % compute initial output

 n = length(x0(:));
 m = length(y1(:));             % number of outputs

 x_sol = NaN(n,points);         % memory allocation for state history
 x_drv = NaN(n,points);         % memory allocation for state derivative history
 y_sol = NaN(m,points);         % memory allocation for output history
 
 x_sol(:,1) = x0(:);            % current state
 x_drv(:,1) = dxdt1(:);         % current rate
 y_sol(:,1) = y1(:);            % current output
 
 [ru,cu] = size(u);
 if ( cu < points ), u(:,cu+1:points) = 0; end % pad u with zeros to length of t

 for p = 1:points-1                  % advance the solution from p to p+1

    t   = time(p); 
    dt  = time(p+1)-t;               % the time step for this interval
    dt2 = dt/2;                      % half of the time step

    [dxdt2,y2] = feval( dxdt, t+dt2, x0+dxdt1*dt2, (u(:,p)+u(:,p+1))/2.0, cnstnt );
    [dxdt3,y3] = feval( dxdt, t+dt2, x0+dxdt2*dt2, (u(:,p)+u(:,p+1))/2.0, cnstnt );
    [dxdt4,y4] = feval( dxdt, t+dt,  x0+dxdt3*dt, u(:,p+1), cnstnt );

    x0 = x0  +  ( dxdt1 + 2*(dxdt2 + dxdt3) + dxdt4 ) * dt/6.0;   % update x0

    [dxdt1,y1] = feval( dxdt, time(p+1), x0, u(:,p+1), cnstnt );

    x_sol(:,p+1) = x0(:);            % current state
    x_drv(:,p+1) = dxdt1(:);         % current rate
    y_sol(:,p+1) = y1(:);            % current output

%   if max(abs(x0)) > 1e10, break; end % close enough to a floating point error
    if ~all(isfinite(x0)), break; end                  % a floating point error
 end
% ---------------------------------------------------------------------- ODE4U
% 2014, 2022-04-29, 2023-01-21
