function [ t, y ] = rk4(odefunc, tspan, yinit, stepSize)
%
%   [ t, y ] = rk4(func, tspan, yinit, stepSize)
%
% Implements the classical RK4 time integration for an ODE IVP
%   with fixed stepsize.
%
% The first three input parameters and both output parameters
%   are the same as used for Matlab's ODE suite.
%
% Parameters:
%    odefunc    Function with prototype dy = f(t,y).
%    tspan      Two element row vector specifying start and end times.
%    yinit      Initial condition column vector.
%    stepSize   Fixed stepsize to be used in the forward Euler steps.
%
%    t          Column vector of sample times.
%    y          Matrix of solution values at sample times.
%                 Each row corresponds to the solution value at the
%                 corresponding sample time in t.
%
% Ian Mitchell for CPSC 303, 2006/03/24.

% Section 1: Set up the outputs.
numSteps = ceil((tspan(2) - tspan(1)) / stepSize) + 1;
t = zeros(numSteps, 1);
y = zeros(numSteps, length(yinit));

% Section 2: Initial conditions.
tNow = tspan(1);
yNow = yinit;

% Section3: Record the step.
step = 1;
t(step) = tNow;
y(step,:) = yNow';

% Integrate.
for step = 2 : numSteps

  % Section 4: The size of the final step may need to be adjusted to hit
  %   the end of the integration interval.
  if(step < numSteps)
    h = stepSize;
  else
    h = tspan(2) - tNow;
  end
  
  % Section 5: Make new step.
  tHalf = tNow + 0.5 * h;
  Y1 = yNow;
  Y2 = yNow + 0.5 * h * feval(odefunc, tNow, Y1);
  Y3 = yNow + 0.5 * h * feval(odefunc, tHalf, Y2);
  Y4 = yNow + h * feval(odefunc, tHalf, Y3);
  yNow = yNow + (1/6) * h * (feval(odefunc, tNow, Y1) + ...
                             2 * feval(odefunc, tHalf, Y2) + ...
                             2 * feval(odefunc, tHalf, Y3) + ...
                             feval(odefunc, tNow + h, Y4));
  tNow = tNow + h;

  % Section 6: Record the step.
  t(step) = tNow;
  y(step,:) = yNow';

end