//
//  This is an adaptive 4th-order Runge-Kutta integrator.
//  See Numerical Recipes.
//

static (rk4, collapse);

ode4 = function( ydot, t0, tf, y0, output, tol, h, yscale ) 
{
  global (eps)

  TINY = 1e-4;

  //  Supply defaults.
  if (!exist (tol)) { tol = 1e-3; }
  if (!exist (h)) { h = abs( tf - t0 ) / 1000.0; }

  //  Initialize.
  t = t0;
  h = h * ( tf - t0 ) ./ abs( tf - t0 );
  y = y0[:];
  I = 2;

  // Save the initial  conditions

  if (!exist (output)) 
  {
    yout.[1] = [t, y.'];
  else
    yout.[1] = output(t,y).';
  }

  while ( (t-tf)*(tf-t0) < 0.0 ) 
  {

    dydt = ydot( t, y );
    if (!exist (yscale)) { scale = abs(y) + abs(h*dydt) + TINY; }

    //	If the step can overshoot end, cut the stepsize.

    if ( (t+h-tf)*(t+h-t0) > 0.0 ) { h = tf - t; }

    //	Take a step
    ys = y;

    while ( 1 ) 
    {
      //  Take two half steps.

      hh = 0.5 * h;
      yt = rk4( ydot, t, hh, ys, dydt );
      y =  rk4( ydot, t+hh, hh, yt, ydot( t+hh, yt ) );

      //  Check for step too small.

      if ( t+h == t ) 
      {
      	fprintf("stderr", "rlab: run time error: Step size too small in ode4\n");
      	yt = 0.0;
        errmax = 1.0;
        tf = t;
      else
	//  Take the full step.
      	yt = y - rk4( ydot, t, h, ys, dydt );
    
	//  Evaluate accuracy.
    	errmax = norm( abs( yt ) ./ scale, "I" ) / tol;
      }

      //  Success?
      if ( errmax <= 1.0 ) { break; }

      //  Try again.  Reduce step size, but by no more than
      //  a factor of about 50.

      if ( errmax > 4e6 ) 
      {
      	h = 0.02*h;
      else
        h = 0.9*h*errmax.^-0.25;
      }
    }

    t = t + h;

    //	Take care of 5th order.
    y = y + yt./15.0;

    //	Save output at the current point.

    if (!exist (output))
    {
      yout.[I] = [t, y.'];
    else
      yout.[I] = output( t, y ).';
    }
    I++;

    //	Determine next step size.
    if ( errmax > 6e-4 ) 
    {
      h = 0.9*h*errmax.^-0.2;
    else
      h = 4*h;
    }
  }
  return collapse (yout);
};

rk4 = function(ydot, t, h, y, dydt) 
{
  local( k1, k2, k3, k4 );

  k1 = h * dydt;
  k2 = h * ydot( t+h/2, y+k1/2 );
  k3 = h * ydot( t+h/2, y+k2/2 );
  k4 = h * ydot( t+h, y+k3 );

  return y + ( k1 + 2.0*k2 + 2.0*k3 + k4 ) / 6.0;
};

//
// Collapse a LIST of matrices into a single matrix.
//

collapse = function ( LIST )
{
  local (i, m, row, col);

  row = size (LIST.[members (LIST)[1]])[1];
  col = size (LIST.[members (LIST)[1]])[2];
  m = zeros (length (LIST)*row, col);

  for (i in 1:length (LIST))
  {
    m[i;] = LIST.[i];
  }
  return m;
};
