function [tout,yout,stats] = ode113(ydot,tspan,y0,options)
%ODE113	Solve non-stiff differential equations, variable order method.
%	[T,Y] = ODE113('ydot',TSPAN,Y0) with TSPAN = [T0 TFINAL] integrates
%	the system of first order differential equations y' = ydot(t,y) from
%	time T0 to TFINAL with initial conditions Y0.  Function ydot(t,y)
%	must return a column vector.  Each row in solution matrix Y
%	corresponds to a time returned in column vector T.  To obtain
%	solutions at the specific times T0, T1, ..., TFINAL (all increasing
%	or all decreasing), use TSPAN = [T0 T1 ... TFINAL].
%	
%	[T,Y] = ODE113('ydot',TSPAN,Y0,OPTIONS) solves as above with default
%	integration parameters replaced by values in OPTIONS, an argument
%	created with the ODESET function.  See ODESET for details.  Commonly
%	used options are scalar relative error tolerance 'rtol' (1e-3 by
%	default) and vector of absolute error tolerances 'atol' (all
%	components 1e-6 by default).
%	
%	It is possible to specify tspan, y0 and options in ydot.  If TSPAN
%	or Y0 is empty, or if ODE113 is invoked as ODE113('ydot'), ODE113
%	calls [tspan,y0,options] = ydot([],[]) to obtain any values not
%	supplied at the command line.  TYPE R3BODY to see how this is coded.
%	
%	As an example, the commands
%	
%	    options = odeset('rtol',1e-4,'atol',[1e-4 1e-4 1e-5]);
%	    ode113('rigid',[0 12],[0 1 1],options);
%	
%	solve the system y' = rigid(t,y) with relative error tolerance 1e-4
%	and absolute tolerances of 1e-4 for the first two components and
%	1e-5 for the third.  When called with no output arguments, as in
%	this example, ODE113 calls the default output function ODEPLOT to
%	plot the solution as it is computed.
%	
%	See also
%	    other ODE solvers:   ODE45, ODE23, ODE15S, ODE23S
%	    options handling:    ODESET, ODEGET
%	    output functions:    ODEPLOT, ODEPHAS2, ODEPHAS3
%	    ydot examples:       R3BODY, RIGID, TWOBODY, VDPNS

%	ODE113 is a fully variable step size, PECE implementation in terms
%	of modified divided differences of the Adams-Bashforth-Moulton
%	family of formulas of orders 1-12.  The natural "free" interpolants
%	are used.  Local extrapolation is done.  Details are to be found in
%	The MATLAB ODE Suite, L. F. Shampine and M. W. Reichelt, Rept. 94-6,
%	Math. Dept., SMU, Dallas, TX, 1994.

%	Mark W. Reichelt and Lawrence F. Shampine, 6-13-94
%	Copyright (c) 1984-95 by The MathWorks, Inc.

false = 0;
true = ~false;

nsteps = 0; 				% stats
nfailed = 0; 				% stats
nfevals = 0; 				% stats
npds = 0; 				% stats
ndecomps = 0; 				% stats
nsolves = 0; 				% stats

if nargin == 1
  tspan = []; y0 = []; options = [];
elseif nargin == 2
  y0 = []; options = [];
elseif nargin == 3
  options = [];
end

% Get default tspan and y0 from ydot if none are specified.
if (length(tspan) == 0) | (length(y0) == 0)
  [ydot_tspan,ydot_y0,ydot_options] = feval(ydot,[],[]);
  if length(tspan) == 0
    tspan = ydot_tspan;
  end
  if length(y0) == 0
    y0 = ydot_y0;
  end
  if length(options) == 0
    options = ydot_options;
  else
    options = odeset(ydot_options,options);
  end
end

% Test that tspan is internally consistent.
tspan = tspan(:);
ntspan = length(tspan);
if ntspan == 1
  t = 0;
  next = 1;
else
  t = tspan(1);
  next = 2;
end
tfinal = tspan(ntspan);
if t == tfinal
  error('The last entry in tspan must be different from the first entry.');
end
tdir = sign(tfinal - t);
if any(tdir * (tspan(2:ntspan) - tspan(1:ntspan-1)) <= 0)
  error('The entries in tspan must strictly increase or decrease.');
end

y = y0(:);
neq = length(y);

% Get options, and set defaults.
rtol = odeget(options,'rtol',1e-3);
atol = odeget(options,'atol',1e-6);
atol = atol(:);
if (rtol <= 0) | any(atol <= 0)  
  error('Tolerance rtol and all entries of atol must be positive.');
end
if rtol < 100 * eps 
  rtol = 100 * eps;
  fprintf('Warning: rtol has been increased to %e\n',rtol);
end
if length(atol) == 1
  atol = atol + zeros(neq,1);
elseif length(atol) ~= neq
  error(['Vector atol must be same length as y0 vector (' num2str(neq) ').']);
end
threshold = atol ./ rtol;

userhmax = abs(odeget(options,'hmax',0.1*abs(tfinal-t)));
hmax = min(userhmax, abs(tfinal-t));

hmin = 4 * eps * abs(t);
userhmin = abs(odeget(options,'hmin',hmin));
hmin = min(max(hmin, userhmin), hmax);

if nargout ~= 0
  outfun = odeget(options,'outfun');
else
  outfun = odeget(options,'outfun','odeplot');
end

refine = odeget(options,'refine',1);

printstats = odeget(options,'printstats',false);

if length(odeget(options,'mass')) ~= 0
  error('Solver does not handle mass matrices, M*y''.  See ODE15S or ODE23S.');
end

% Initialize the output function.
if length(outfun) == 0
  haveoutfun = false;
else
  haveoutfun = true;
  feval(outfun,[t tfinal],y,'init');
end

% Allocate memory if we're generating output.
if nargout ~= 0
  if (ntspan == 1) & (refine == 0) 	% only 1 output at tfinal
    nout = 0;
  else
    if ntspan > 2 			% output only at tspan points
      tout = zeros(ntspan,1);
      yout = zeros(ntspan,neq);
    else 				% alloc in chunks
      chunk = max(ceil(128 / neq),refine);
      tout = zeros(chunk,1);
      yout = zeros(chunk,neq);
    end
    nout = 1;
    tout(nout) = t;
    yout(nout,:) = y.';
  end
end

% Initialize method parameters.
maxk = 12;
two = 2 .^ (1:13)';
gstar = [ 0.5000;  0.0833;  0.0417;  0.0264;  ...
          0.0188;  0.0143;  0.0114;  0.00936; ...
          0.00789;  0.00679; 0.00592; 0.00524; 0.00468];

% Set the output flag.
if (ntspan > 2) | (refine == 0)
  outflag = 1; 				% output only at tspan points
elseif refine == 1
  outflag = 2; 				% computed points, no refinement
else
  outflag = 3; 				% computed points, with refinement
end

% Compute an initial step size h using y'(t).
yp = feval(ydot,t,y);
[m,n] = size(yp);
if n > 1
  error(['Function ' ydot '(t,y) must return a column vector.'])
elseif m ~= neq
  error(['Vector ' ydot '(t,y) must be same length as initial conditions.']);
end
nfevals = nfevals + 1; 			% stats
absh = min(hmax, abs(tspan(next) - t));
rh = norm(yp ./ (abs(y) + threshold),inf) / (0.25 * sqrt(rtol));
if absh * rh > 1
  absh = 1 / rh;
end
absh = max(absh, hmin);
h = tdir * absh;
k = 1;
K = 1;

% Initialize 
phi = zeros(neq,14);
phi(:,1) = yp;
psi = zeros(12,1);
alpha = zeros(12,1);
beta = zeros(12,1);
sig = zeros(13,1);
sig(1) = 1;
w = zeros(12,1);
v = zeros(12,1);
g = zeros(13,1);
g(1) = 1;
g(2) = 0.5;

hlast = 0;
klast = 0;
phase1 = true;

% THE MAIN LOOP

done = false;
while ~done
  
  hmin = max(userhmin, 4 * eps * abs(t));
  if 1.1*abs(h) >= abs(tfinal - t)
    h = tfinal - t;
    done = true;
  end
  
  % LOOP FOR ADVANCING ONE STEP.
  failed = 0;
  invwt = 1 ./ (abs(y) + threshold);
  while true

    % Compute coefficients of formulas for this step.  Avoid computing
    % those quantities not changed when step size is not changed.

    % ns is the number of steps taken with h, including the 
    % current one.  When k < ns, no coefficients change
    if h ~= hlast  
      ns = 0;
    end
    if ns <= klast 
      ns = ns + 1;
    end
    if k >= ns
      beta(ns) = 1;
      alpha(ns) = 1 / ns;
      temp1 = h * ns;
      sig(ns+1) = 1;
      for i = ns+1:k
        temp2 = psi(i-1);
        psi(i-1) = temp1;
        temp1 = temp2 + h;
	
	beta(i) = beta(i-1) * psi(i-1) / temp2;
        alpha(i) = h / temp1;
        sig(i+1) = i * alpha(i) * sig(i);
      end
      psi(k) = temp1;

      % Compute coefficients g.
      if ns == 1 			% Initialize v and set w
	v = 1 ./ (K .* (K + 1));
	w = v;
      else
        % If order was raised, update diagonal part of v.
        if k > klast
          v(k) = 1 / (k * (k+1));
          for j = 1:ns-2
            v(k-j) = v(k-j) - alpha(j+1) * v(k-j+1);
          end
        end
        % Update v and set w.
        for iq = 1:k+1-ns
          v(iq) = v(iq) - alpha(ns) * v(iq+1);
          w(iq) = v(iq);
        end
        g(ns+1) = w(1);
      end

      % Compute g in the work vector w.
      for i = ns+2:k+1
        for iq = 1:k+2-i
          w(iq) = w(iq) - alpha(i-1) * w(iq+1);
        end
        g(i) = w(1);
      end
    end   

    % Change phi to phi star.
    i = ns+1:k;
    phi(:,i) = phi(:,i) * diag(beta(i));

    % Predict solution and differences.
    phi(:,k+2) = phi(:,k+1);
    phi(:,k+1) = zeros(neq,1);
    p = zeros(neq,1);
    for i = k:-1:1
      p = p + g(i) * phi(:,i);
      phi(:,i) = phi(:,i) + phi(:,i+1);
    end
    p = y + h * p;
    tlast = t;
    t = tlast + h;
    absh = abs(h);
    yp = feval(ydot,t,p);
    nfevals = nfevals + 1;

    % Estimate errors at orders k, k-1, k-2.
    phikp1 = yp - phi(:,1);
    temp3 = norm(phikp1 .* invwt,inf);
    err = absh * (g(k) - g(k+1)) * temp3;
    erk = absh * sig(k+1) * gstar(k) * temp3;
    if k >= 2
      erkm1 = absh * sig(k) * gstar(k-1) * ...
	  norm((phi(:,k)+phikp1) .* invwt,inf);
    end
    if k >= 3
      erkm2 = absh * sig(k-1) * gstar(k-2) * ...
	  norm((phi(:,k-1)+phikp1) .* invwt,inf);
    end
    
    % Test if order should be lowered
    knew = k;
    if (k == 2) & (erkm1 <= 0.5*erk)
      knew = k - 1;
    end
    if (k > 2) & (max(erkm1,erkm2) <= erk)
      knew = k - 1;
    end
    
    % Test if step successful
    if err > rtol 			% Failed step
      nfailed = nfailed + 1; 		% stats
      if absh <= hmin
	fprintf('Step failure at %e with a minimum step size of %e\n', t, h);
	if nargout ~= 0
	  tout = tout(1:nout);
	  yout = yout(1:nout,:);
	  stats = [nsteps; nfailed; nfevals; npds; ndecomps; nsolves];
	end
	return;
      end
      
      % Restore t, phi, and psi.
      phase1 = false;
      t = tlast;
      for i = K
        phi(:,i) = (phi(:,i) - phi(:,i+1)) / beta(i);
      end
      for i = 2:k
        psi(i-1) = psi(i) - h;
      end

      failed = failed + 1;
      reduce = 0.5;
      if failed == 3
        knew = 1;
      elseif failed > 3
        reduce = min(0.5, sqrt(0.5*rtol/erk));
      end
      h = tdir * max(reduce * absh, hmin);
      k = knew;
      K = 1:k;
      done = false;
      
    else 				% Successful step
      break;
      
    end
  end

  klast = k;
  hlast = h;

  % Correct and evaluate.
  y = p + h * g(k+1) * phikp1;
  nsteps = nsteps + 1; 			% stats
  yp = feval(ydot,t,y);
  nfevals = nfevals + 1; 		% stats
  
  % Update differences for next step.
  phi(:,k+1) = yp - phi(:,1);
  phi(:,k+2) = phi(:,k+1) - phi(:,k+2);
  for i = K
    phi(:,i) = phi(:,i) + phi(:,k+1);
  end

  if (knew == k-1) | (k == maxk)
    phase1 = false;
  end

  % Select a new order.
  kold = k;
  if phase1 				% Always raise the order in phase1
    k = k + 1;
  elseif knew == k-1 			% Already decided to lower the order
    k = k - 1;
    erk = erkm1;
  elseif k+1 <= ns 			% Estimate error at higher order
    erkp1 = absh * gstar(k+1) * norm(phi(:,k+2) .* invwt,inf);
    if k == 1
      if erkp1 < 0.5*erk
        k = k + 1;
        erk = erkp1;
      end
    else
      if erkm1 <= min(erk,erkp1)
        k = k - 1;
        erk = erkm1;
      elseif (k < maxk) & (erkp1 < erk)
        k = k + 1;
        erk = erkp1;
      end
    end
  end
  if k ~= kold
    K = 1:k;
  end

  % Select a new step size.
  if phase1
    hnew = 2 * h;
  elseif 0.5*rtol >= erk*two(k+1)
    hnew = 2 * h;      
  elseif 0.5*rtol >= erk
    hnew = h;
  else
    reduce = (0.5 * rtol / erk)^(1 / (k+1));
    hnew = absh * max(0.5, min(0.9, reduce));
    hnew = tdir * max(hnew, hmin);
  end
  h = hnew;
  
  if nargout ~= 0
    oldnout = nout;
    if outflag == 2 			% computed points, no refinement
      nout = nout + 1;
      if nout > length(tout)
	tout = [tout; zeros(chunk,1)];
	yout = [yout; zeros(chunk,neq)];
      end
      tout(nout) = t;
      yout(nout,:) = y.';
    elseif outflag == 3 		% computed points, with refinement
      if nout + refine > length(tout)
	tout = [tout; zeros(chunk,1)]; 	% requires chunk >= refine
	yout = [yout; zeros(chunk,neq)];
      end
      dt = (t - tlast) / refine;
      for i = 1:refine-1
	nout = nout + 1;
	tout(nout) = tlast + i*dt;
	yout(nout,:) = ntrp113(t,y,tout(nout),klast,phi,psi).';
      end
      nout = nout + 1;
      tout(nout) = t;
      yout(nout,:) = y.';
    elseif outflag == 1 		% output only at tspan points
      while true
	if next > ntspan 
	  break;
	elseif tdir * (t - tspan(next)) < 0
	  break;
	end
	nout = nout + 1; 		% tout and yout are already allocated
	tout(nout) = tspan(next);
	yout(nout,:) = ntrp113(t,y,tspan(next),klast,phi,psi).';
	next = next + 1;
      end
    end
    
    if haveoutfun
      for i = oldnout+1:nout
	if feval(outfun,tout(i),yout(i,:).') == 1
	  tout = tout(1:nout);
	  yout = yout(1:nout,:);
	  stats = [nsteps; nfailed; nfevals; npds; ndecomps; nsolves];
	  return;
	end
      end
    end
    
  elseif haveoutfun
    if outflag == 2
      if feval(outfun,t,y) == 1
	return;
      end
    elseif outflag == 3 		% computed points, with refinement
      dt = (t - tlast) / refine;
      for i = 1:refine-1
	tinterp = tlast + i*dt;
	yinterp = ntrp113(t,y,tinterp,klast,phi,psi);
	if feval(outfun,tinterp,yinterp) == 1
	  return;
	end
      end
      if feval(outfun,t,y) == 1
	return;
      end
    elseif outflag == 1 		% output only at tspan points
      while true
	if next > ntspan 
	  break;
	elseif tdir * (t - tspan(next)) < 0
	  break;
	end
	yinterp = ntrp113(t,y,tspan(next),klast,phi,psi);
	if feval(outfun,tspan(next),yinterp) == 1
	  return;
	end
	next = next + 1;
      end
    end
  end
  
end

if printstats 				% print cost statistics
  fprintf('%g successful steps\n', nsteps);
  fprintf('%g failed attempts\n', nfailed);
  fprintf('%g calls to ydot\n', nfevals);
  fprintf('%g partial derivatives\n', npds);
  fprintf('%g LU decompositions\n', ndecomps);
  fprintf('%g solutions of linear systems\n', nsolves);
end

if nargout ~= 0
  tout = tout(1:nout);
  yout = yout(1:nout,:);
  stats = [nsteps; nfailed; nfevals; npds; ndecomps; nsolves];
end
