function [x,y] = ode2d(dfun,xspan,y0,AbsTol,RelTol)
% [x,y] = ode2d(dfun,xspan,y0,AbsTol,RelTol)
%
% Integrates ODE system from xspan(1) to xspan(2) by the
% explicit midpoint formula with step doubling,
% automatically regulating step size so that
%
%    ErrEst < AbsTol + RelTol*norm(y,inf).
%
% Inputs:
%   dfun    string that names a function M-file for derivatives
%   xspan   two-component vector -- initial & final values of 
%           independent variable
%   y0      initial value(s) of dependent variable(s)
%   AbsTol,RelTol [optional] error tolerances
%   
% Outputs:
%   x       mesh of automatically chosen pts in xspan
%   y       solution of DE (Euler approximation) on the mesh x
% 
% Demo I (requires lnhde.m):
%   xspan = [0 2*pi]; y0=0;
%   [x,y] = ode2d('lnhde',xspan,y0);
%   yex   = (25/13)*cos(x) + (5/13)*sin(x) - (25/13)*exp(-5*x);
%   figure(1), plot(x,y,'b',x,yex,'r')
%   title('Solution of dy/dx = -5y + 10 cos(x) by Midpoint Formula')
%   figure(2), plot(x,yex-y)
%   title(sprintf('Maximum error is %8.2e',norm(yex-y,inf)))
%
% Demo II (requires orbit.m):
%   xspan = [0 6.19216933]; y0 = [1.2; 0; 0; -1.04935750983031990726];
%   AbsTol = 1e-5; RelTol = 1e-5;
%   [x,y] = ode2d('orbit',xspan,y0,AbsTol,RelTol);
%   figure(1), plot(y(:,1),y(:,2),'m')
%   title(sprintf('Figure 8 orbit. Error @end %8.2e',norm(y0-y(length(x),:)')))
%   
% Standard error control on this problem produces
% 593 successful steps
% 85 failed attempts
% 1829 function evaluations
%   Beat it!

if nargin < 5
  RelTol = 1e-3;
  if nargin < 4
    AbsTol = 1e-6;
  end
end

x0 = xspan(1);             % Initial point
xf = xspan(2);             % Final point
dr = sign(xf-x0);          % Direction of integration
n  = 1;                    % Number of steps completed
y0 = y0(:);                % Column vector
[neq one] = size(y0);      % Determine # of dep. vars.
x  = [x0; zeros(32,1)];    % Allocate space for mesh
y  = [y0'; zeros(32,neq)]; % ... and solution
f0 = feval(dfun,x0,y0);    % Initial derivative
nde   = 1;                 % Count derivative evaluations
nfail = 0;                 % Count failed steps
init  = 1;                 % Boolean: starting integration

p     = 2;                 % Integrator order
kI    = 1.0/(p+1);         % Controller gains for step size
kP    = 0.0/(p+1);         % kI=1/(p+1), kP=0 is standard

% MAIN LOOP: Take steps one at a time until end of interval reached.

while dr*(xf-x(n)) > 0
  if init

%   First Step: Estimate 3rd derivative bound with
%    steps 1/8, 1/64, 1/512 interval

    halfM2 = sqrt(eps);
    for p = 1:3
      h = (xf-x0)/8^p;
      y1 = y0 + h*f0;
      f1 = feval(dfun,x0+h,y1); nde=nde+1;
      y2 = y1 + h*f1;
      f2 = feval(dfun,x0+2*h,y2); nde=nde+1;
      E = norm( h*(f2-2*f1+f0), inf);
      halfM2 = max([halfM2 E/h^3]);
    end

%   Select initial stepsize -- choose a step that
%   is expected to pass the error test, but is at
%   most 1/8 of the integration interval.

    h = (0.8*(AbsTol+RelTol*norm(y1,inf))/halfM2)^(1/3);
    h = min([h abs(xf-x0)/8]);
    h = dr*h;
    hfn = h*f0;
    init = 0;
  else

%   Not first step.
%   Select step based on error estimate in step just completed.

    Tol    = AbsTol+RelTol*norm(y(n,:),inf);
    if Est < Tol & exist('OldEst')
      up   = (0.8*Tol/Est)^kI * (OldEst/Est)^kP;
    else
      up   = (0.8*Tol/Est)^(1/(p+1));
    end
    h      = up*h;
    hfn    = up*hfn;
    OldEst = Est;
  end

% Attempt two Midpoint steps and a double step, estimate error

  k11  = hfn;
  k12  = h*feval(dfun,x(n)+1/2*h,y(n,:)'+1/2*k11);
  ynp1 = y(n,:)'+k12;                % First Midpoint step
  k21  = h*feval(dfun,x(n)+  h,ynp1);
  k22  = h*feval(dfun,x(n)+3/2*h,ynp1   +1/2*k21);
  ynp2 = ynp1   +k22;                % Second Midpoint step
  kd2  = h*feval(dfun,x(n)+h,y(n,:)'+k11);
  ynpd = y(n,:)'+2*kd2;              % Dble Midpoint step 
  nde  = nde+4;                      % Count deriv evals

  Est = norm(ynpd-ynp2,inf) / 6;

  if Est < AbsTol+RelTol*norm(y(n,:),inf)

%   Passed error test -- accept step, advance solution
%   [use local extrapolation and evaluate derivative
%    at the corrected point]

    locerr   = (ynp2 - ynpd) / 6;
    x(n+1)   = x(n) + h;
    y(n+1,:) = (ynp1 + locerr)';
    x(n+2)   = x(n) + 2*h;
    y(n+2,:) = (ynp2 + 2*locerr)';
    n        = n+2;
    hfn      = h*feval(dfun,x(n),y(n,:)');
    nde      = nde + 1;
    
    if mod(n,32)==0                % Allocate more space for solution
      x = [x; zeros(32,1)];
      y = [y; zeros(32,neq)];
    end
  else
    
% Failed error test; will retry with smaller h
    nfail = nfail+1;               
  end
end

% Now dr*(xf-x(n)) <= 0.
% Interpolate to deliver solution at final point

if dr*(xf-x(n-1)) <= 0
  nf = n-1;        % Last step is [x(n-2),xf]
  X0 = x(n-2);
  Y0 = y(n-2,:)';
  X1 = x(n-1);
  Y1 = y(n-1,:)';
else
  nf = n;        % Last step is [x(n-1),xf]
  X0 = x(n-1);
  Y0 = y(n-1,:)';
  X1 = x(n);
  Y1 = y(n,:)';
end

% Create divided difference table for Hermite cubic interpolation.

Y00   = feval(dfun,X0,Y0);   % First differences
Y01   = (Y1-Y0)/(X1-X0);
Y11   = feval(dfun,X1,Y1);
nde   = nde+2;

Y001  = (Y01-Y00)/(X1-X0);   % Second differences
Y011  = (Y11-Y01)/(X1-X0);

Y0011 = (Y011-Y001)/(X1-X0); % Third difference

% Interpolate at xf. Nested multiplication.

yf    = ((Y0011*(xf-X1)+Y001)*(xf-X0)+Y00)*(xf-X0)+Y0;

x(nf)   = xf;           % Insert final point.
y(nf,:) = yf';

x       = x(1:nf);      % Delete unused array space
y       = y(1:nf,:);

% Print Stats
disp(sprintf('%d successful steps',n))
disp(sprintf('%d failed attempts',nfail))
disp(sprintf('%d function evaluations',nde))