#include "rkf45.h"
#define MACH_ACC  1.0e-15  /* Relative machine precision for real_8 */

rkf45(/* input:  */ n, f, y_prime, x0, y0, x1, epsilon, firsth, minh,
      /* output: */ y1, lasth, failure)

   integer
      n;		   /* number of equations */
   boolean
      (*f)();		   /* compute dy / dx and return true if ok */
   real_8
      y_prime[NEQN],	   /* dy / dx at x */
      x0,		   /* initial value of independent variable */
      y0[NEQN], 	   /* initial value of dependent   variable */
      x1,		   /* final   value of independent variable */
      epsilon[NEQN],	   /* integration tolerance, expressed as an
			      absolute value, not a fraction */
      firsth,		   /* initial step size */
      minh;		   /* smallest step size tolerated */
   real_8
      y1[NEQN], 	   /* final value of dependent variable */
      *lasth;		   /* last value of step size; recommended for
			      restart if desired */
   integer
      *failure; 	   /* 0 if ok; else failure mode */

    /*-----------------------------------------------------------------
    |  Runge-Kutta-Fehlberg integration of the system of n ordinary   |
    |  differential equations					      |
    |								      |
    |	 dy(x) [i]						      |
    |	 -----	    = y_prime(x) [i];				      |
    |	 dx							      |
    |								      |
    |	 y(x0) [i]  = y0 [i]; --> y(x1) [i] = y1 [i];		      |
    |								      |
    |		 (i = 1...n)					      |
    |								      |
    |  using the 5-stage fourth order Runge-Kutta method of Fehlberg. |
    |								      |
    |  The function f(n, x, y, y_prime) computes y_prime from the     |
    |  values of x and y(x) and returns true if the calculation is    |
    |  successful, else false.					      |
    |								      |
    |  In application rkf45 should be used in a series of steps       |
    |  to cover the desired total interval; the method will take      |
    |  finer steps within this interval as required.  Internally      |
    |  the step size is halved if the error estimate per unit step    |
    |  exceeds epsilon.   The step size is doubled if the error       |
    |  estimate is less than epsilon / 32, unless this exceeds the    |
    |  range available.  The process terminates if the step size      |
    |  becomes less than minh.	The variable "failure" is 0 if the    |
    |  integration is successful, 1 if the number of equations	      |
    |  exceeds NEQN, 2 if the step size became less than minh without |
    |  reaching the requested error tolerance, 3 if the derivative    |
    |  calculation failed.					      |
    |								      |
    |  Taken from An Introduction to Numerical Methods with Pascal    |
    |  p. 283 by L.V. Atkinson and P.J. Harley, (Addison Wesley).     |
    |								      |
    |  Modified 						      |
    |  (1) to protect the routine from bad step size and collapsed    |
    |	   x interval,						      |
    |  (2) to allow stepping in the  negative direction,	      |
    |  (3) to return several error status indications,		      |
    |  (4) to work in n-dimensions.				      |
    |								      |
    |			  14 January 1983			      |
    |			     H. L. Lynch			      |
    |								      |
    -----------------------------------------------------------------*/

{      /* Runge-Kutta-Fehlberg rkf45 */

   real_8
      yt[NEQN], 		/* temporary storage for calc. kn */
      k1[NEQN], k2[NEQN],	/* interpolated devivative values */
      k3[NEQN], k4[NEQN],	/* interpolated derivative values */
      k5[NEQN], k6[NEQN],	/* interpolated derivative values */
      nearzero, 		/* within machine accuracy of end point */
      error,			/* error estimate */
      x,			/* curr. value of independent variable*/
      h,			/* current step size */
      ah,			/* absolute value of current step size*/
      tolerance;		/* temp. var. in error tolerance calc.*/
   /* enum { stepping, x1reached, htoosmall, bad_derivative }
      state;			   current state of program */
   static integer
       state, stepping=0, x1reached=1, htoosmall=2, bad_derivative=3;
   boolean
      good_deriv,		/* good derivative returned from f() */
      err_2_large,		/* need to reduce   step size */
      err_2_small,		/* need to increase step size */
      shortrange;		/* flag to prevent step beyond x1 and
				   return good estimated h */
   integer
      i;			/* dummy index for arrays */

   if (n > NEQN)     /* Attempt to over run internal arrays */
   {
      *failure = 1;
      return;
   }

   x = x0;
   for (i =0; i < n; ++i)
      y1[i] = y0[i];
   shortrange = false;
   state = stepping;
   h = firsth;

   /* check for bad input */

   if ((h * (x1 - x0)) < 0.0)	/* step is in wrong direction */
      h = -h;
   if ((((x1 - x0) / h) < 1) || (h == 0))    /* bad step size */
   {
      h = x1 - x0;
      if (fabs(h) < minh)      /* interval has collapsed */
	 return;
   }

   ah = fabs(h);
   nearzero = MACH_ACC * fabs(x1 - x0);

   do /* while state == stepping, checked at the end */
   {
      good_deriv = (*f)(n, x, y1, y_prime);
      for (i = 0; i < n; ++i)
	 k1[i] = y_prime[i];

      for (i = 0; i < n; ++i)
	 yt[i] = y1[i] + h * k1[i] / 4.0;
      good_deriv = (*f)(n, x + h / 4.0, yt, y_prime) && good_deriv;
      for (i = 0; i < n; ++i)
	 k2[i] = y_prime[i];

      for (i = 0; i < n; ++i)
	 yt[i] = y1[i] + h * (3.0 * k1[i] + 9.0 * k2[i]) / 32.0;
      good_deriv  = (*f)(n, x + 3.0 * h / 8.0, yt, y_prime)
		    && good_deriv;
      for (i = 0; i < n; ++i)
	 k3[i] = y_prime[i];

      for (i = 0; i < n; ++i)
	 yt[i] = y1[i] + h * (1932.0 * k1[i]
		 - 7200.0 * k2[i] + 7296.0 * k3[i]) / 2197.0;
      good_deriv  = (*f)(n, x + 12.0 * h / 13.0, yt, y_prime)
		    && good_deriv;
      for (i = 0; i < n; ++i)
	 k4[i] = y_prime[i];

      for (i = 0; i < n; ++i)
	 yt[i] = y1[i] + h * (439.0 * k1[i] / 216.0 - 8.0 * k2[i]
		 + 3680.0 * k3[i] / 513.0 - 845.0 * k4[i] / 4104.0);
      good_deriv  = (*f)(n, x + h, yt, y_prime) && good_deriv;
      for (i = 0; i < n; ++i)
	 k5[i] = y_prime[i];

      for (i = 0; i < n; ++i)
	 yt[i] = y1[i] + h * (-8.0 * k1[i] / 27.0 + 2.0 * k2[i]
		 - 3544.0 * k3[i] / 2565.0 + 1859.0 * k4[i] / 4104.0
		 - 11.0 * k5[i] / 40.0);
      good_deriv  = (*f)(n, x + h / 2.0, yt, y_prime) && good_deriv;
      for (i = 0; i < n; ++i)
	 k6[i] = y_prime[i];

      if (! good_deriv)
	 state = bad_derivative;

      /* Set up err_2_large = at least one error is too large;
		err_2_small = all errors are too small.
      */

      err_2_large = false;
      err_2_small = true;
      for (i = 0; i < n; ++i)
      {
	 error = ah * fabs(k1[i] / 360.0 - 128.0 * k3[i] / 4275.0
			   - 2197.0 * k4[i] / 75240.0
			   + k5[i] / 50.0 + 2 * k6[i] / 55.0);
	 tolerance = ah * epsilon[i];
	 err_2_large = (error > tolerance)	   || err_2_large;
	 err_2_small = (error <= tolerance / 32.0) && err_2_small;
      } /* end i loop */

      if (err_2_large)
      {
	 h = h / 2.0;
	 ah = fabs(h);
	 if (ah < minh)
	    state = htoosmall;
      }
      else
      {  /* Good result, take the step. */
	 x = x + h;
	 for (i = 0; i < n; ++i)
	 {
	    y1[i] = y1[i] + h * (25.0 * k1[i] / 216.0
		    + 1408.0 * k3[i] / 2565.0
		    + 2197.0 * k4[i] / 4104.0 - k5[i] / 5.0);
	 }
	 if (err_2_small)   /* increase h for future steps */
	 {
	    h = h * 2.0;
	    ah = fabs(h);
	 }
	 if (fabs(x1 - x) <= nearzero)
	    state = x1reached;
	 else
	 {
	    shortrange = (ah > fabs(x1 - x));
	    if (shortrange)
	    {
	       *lasth = h;
	       h = x1 - x;
	       ah = fabs(h);
	    }
	 }  /* end x1 not yet reached */
      }  /* end err not too large */
   }
   while (state == stepping);

   if (! shortrange)
      *lasth = h;
   if	   (state == x1reached)       *failure = 0;
   else if (state == htoosmall)       *failure = 2;
   else if (state == bad_derivative)  *failure = 3;
} /* end Runge-Kutta-Fehlberg rkf45 */