' ******************************************************************
' Numerical integration of a system of differential equations
' by the Runge-Kutta-Fehlberg (RKF) method.
' 
' Adapted from a Fortran-90 program available at:
' http://www.csit.fsu.edu/~burkardt/f_src/rkf45/rkf45.f90
' ******************************************************************

' ------------------------------------------------------------------
' Logical constants
' ------------------------------------------------------------------

#DEFINE True -1
#DEFINE False 0

' ------------------------------------------------------------------
' External functions
' ------------------------------------------------------------------

DECLARE FUNCTION Max(x AS DOUBLE, y AS DOUBLE) AS DOUBLE

DECLARE FUNCTION Min(x AS DOUBLE, y AS DOUBLE) AS DOUBLE

' ******************************************************************

SUB Fehl(f    AS SUB (t AS DOUBLE, y() AS DOUBLE, yp() AS DOUBLE), _
         neqn AS INTEGER, _
         y()  AS DOUBLE, _
         t    AS DOUBLE, _
         h    AS DOUBLE, _
         yp() AS DOUBLE, _
         f1() AS DOUBLE, _ 
         f2() AS DOUBLE, _ 
         f3() AS DOUBLE, _ 
         f4() AS DOUBLE, _ 
         f5() AS DOUBLE, _ 
         s()  AS DOUBLE)
' ------------------------------------------------------------------
'
'  Fehl takes one Fehlberg fourth-fifth order step (double precision).
'
'
'  Discussion:
'
'    This routine integrates a system of NEQN first order ordinary differential
'    equations of the form
'      dY(i)/dT = F(T,Y(1:NEQN))
'    where the initial values Y and the initial derivatives
'    YP are specified at the starting point T.
'
'    The routine advances the solution over the fixed step H and returns
'    the fifth order (sixth order accurate locally) solution
'    approximation at T+H in array S.
'
'    The formulas have been grouped to control loss of significance.
'    The routine should be called with an H not smaller than 13 units of
'    roundoff in T so that the various independent arguments can be
'    distinguished.
'
'  Modified:
'
'    27 March 2004
'
'  Author:
'
'    H A Watts and L F Shampine,
'    Sandia Laboratories,
'    Albuquerque, New Mexico.
'
'  Reference:
'
'    E. Fehlberg,
'    Low-order Classical Runge-Kutta Formulas with Stepsize Control,
'    NASA Technical Report R-315.
'
'    L F Shampine, H A Watts, S Davenport,
'    Solving Non-stiff Ordinary Differential Equations - The State of the Art,
'    SIAM Review,
'    Volume 18, pages 376-411, 1976.
'
'  Parameters:
'
'    Input, external F, a user-supplied subroutine to evaluate the
'    derivatives Y'(T), of the form:
'
'      sub f ( t as double, y() as double, yp() as double )
'
'    Input, NEQN, the number of equations to be integrated.
'
'    Input, Y(NEQN), the current value of the
'    dependent variable.
'
'    Input, T, the current value of the independent
'    variable.
'
'    Input, H, the step size to take.
'
'    Input, YP(NEQN), the current value of the
'    derivative of the dependent variable.
'
'    Output, F1(NEQN), F2(NEQN), F3(NEQN), F4(NEQN), F5(NEQN), 
'    derivative values needed for the computation.
'
'    Output, S(NEQN), the estimate of the solution at T+H.
' ------------------------------------------------------------------

  CONST C1 = 3# / 32#
  CONST C2 = 3# / 8#
  CONST C3 = 1# / 2197#
  CONST C4 = 12# / 13#
  CONST C5 = 1# / 4104#
  CONST C6 = 1# / 20520#
  CONST C7 = 1# / 7618050#
  
  DIM AS DOUBLE  ch
  DIM AS INTEGER i
  
  ch = 0.25 * h
  
  FOR i = 1 TO neqn
    f5(i) = y(i) + ch * yp(i)
  NEXT i

  f(t + ch, f5(), f1())

  ch = C1 * h

  FOR i = 1 TO neqn
    f5(i) = y(i) + ch * (yp(i) + 3.0 * f1(i))
  NEXT i
  
  f(t + C2 * h, f5(), f2())

  ch = C3 * h

  FOR i = 1 TO neqn
    f5(i) = y(i) + ch * (1932.0 * yp(i) + (7296.0 * f2(i) - 7200.0 * f1(i)))
  NEXT i

  f(t + C4 * h, f5(), f3())

  ch = C5 * h

  FOR i = 1 TO neqn
    f5(i) = y(i) + ch * ((8341.0 * yp(i) - 845.0 * f3(i)) _
              + (29440.0 * f2(i) - 32832.0 * f1(i)))
  NEXT i

  f(t + h, f5(), f4())

  ch = C6 * h

  FOR i = 1 TO neqn
    f1(i) = y(i) + ch * ((-6080.0 * yp(i) _
                 + (9295.0 * f3(i) - 5643.0 * f4(i))) _
                 + (41040.0 * f1(i) - 28352.0 * f2(i)))
  NEXT i

  f(t + 0.5 * h, f1(), f5())

' Ready to compute the approximate solution at T+H.

  ch = C7 * h

  FOR i = 1 TO neqn
    s(i) = y(i) + ch * ((902880.0 * yp(i) _
                + (3855735.0 * f3(i) - 1371249.0 * f4(i))) _
                + (3953664.0 * f2(i) + 277020.0 * f5(i)))
  NEXT i
END SUB

SUB RKF45(      f      AS SUB (t AS DOUBLE, y() AS DOUBLE, yp() AS DOUBLE), _ 
                y()    AS DOUBLE, _ 
                yp()   AS DOUBLE, _ 
          BYREF t      AS DOUBLE, _ 
                tout   AS DOUBLE, _ 
                relerr AS DOUBLE, _
                abserr AS DOUBLE, _
          BYREF flag   AS INTEGER)
' ------------------------------------------------------------------
'
'  RKF45 carries out the Runge-Kutta-Fehlberg method (double precision).
'
'
'  Discussion:
'
'    This routine is primarily designed to solve non-stiff and mildly stiff
'    differential equations when derivative evaluations are inexpensive.
'    It should generally not be used when the user is demanding
'    high accuracy.
'
'    This routine integrates a system of NEQN first-order ordinary differential
'    equations of the form:
'
'      dY(i)/dT = F(T,Y(1),Y(2),...,Y(NEQN))
'
'    where the Y(1:NEQN) are given at T.
'
'    Typically the subroutine is used to integrate from T to TOUT but it
'    can be used as a one-step integrator to advance the solution a
'    single step in the direction of TOUT.  On return, the parameters in
'    the call list are set for continuing the integration.  The user has
'    only to call again (and perhaps define a new value for TOUT).
'
'    Before the first call, the user must
'
'    * supply the subroutine F(T,Y,YP) to evaluate the right hand side;
'      and declare F in an EXTERNAL statement;
'
'    * initialize the parameters:
'      NEQN, Y(1:NEQN), T, TOUT, RELERR, ABSERR, FLAG.
'      In particular, T should initially be the starting point for integration,
'      Y should be the value of the initial conditions, and FLAG should
'      normally be +1.
'
'    Normally, the user only sets the value of FLAG before the first call, and
'    thereafter, the program manages the value.  On the first call, FLAG should
'    normally be +1 (or -1 for single step mode.)  On normal return, FLAG will
'    have been reset by the program to the value of 2 (or -2 in single
'    step mode), and the user can continue to call the routine with that
'    value of FLAG.
'
'    (When the input magnitude of FLAG is 1, this indicates to the program
'    that it is necessary to do some initialization work.  An input magnitude
'    of 2 lets the program know that that initialization can be skipped,
'    and that useful information was computed earlier.)
'
'    The routine returns with all the information needed to continue
'    the integration.  If the integration reached TOUT, the user need only
'    define a new TOUT and call again.  In the one-step integrator
'    mode, returning with FLAG = -2, the user must keep in mind that
'    each step taken is in the direction of the current TOUT.  Upon
'    reaching TOUT, indicated by the output value of FLAG switching to 2,
'    the user must define a new TOUT and reset FLAG to -2 to continue
'    in the one-step integrator mode.
'
'    In some cases, an error or difficulty occurs during a call.  In that case,
'    the output value of FLAG is used to indicate that there is a problem
'    that the user must address.  These values include:
'
'    * 3, integration was not completed because the input value of RELERR, the
'      relative error tolerance, was too small.  RELERR has been increased
'      appropriately for continuing.  If the user accepts the output value of
'      RELERR, then simply reset FLAG to 2 and continue.
'
'    * 4, integration was not completed because more than MAXNFE derivative
'      evaluations were needed.  This is approximately (MAXNFE/6) steps.
'      The user may continue by simply calling again.  The function counter
'      will be reset to 0, and another MAXNFE function evaluations are allowed.
'
'    * 5, integration was not completed because the solution vanished,
'      making a pure relative error test impossible.  The user must use
'      a non-zero ABSERR to continue.  Using the one-step integration mode
'      for one step is a good way to proceed.
'
'    * 6, integration was not completed because the requested accuracy
'      could not be achieved, even using the smallest allowable stepsize.
'      The user must increase the error tolerances ABSERR or RELERR before
'      continuing.  It is also necessary to reset FLAG to 2 (or -2 when
'      the one-step integration mode is being used).  The occurrence of
'      FLAG = 6 indicates a trouble spot.  The solution is changing
'      rapidly, or a singularity may be present.  It often is inadvisable
'      to continue.
'
'    * 7, it is likely that this routine is inefficient for solving
'      this problem.  Too much output is restricting the natural stepsize
'      choice.  The user should use the one-step integration mode with
'      the stepsize determined by the code.  If the user insists upon
'      continuing the integration, reset FLAG to 2 before calling
'      again.  Otherwise, execution will be terminated.
'
'    * 8, invalid input parameters, indicates one of the following:
'      NEQN <= 0;
'      T = TOUT and |FLAG| /= 1;
'      RELERR < 0 or ABSERR < 0;
'      FLAG == 0  or FLAG < -2 or 8 < FLAG.
'
'  Modified:
'
'    27 March 2004
'
'  Author:
'
'    H A Watts and L F Shampine,
'    Sandia Laboratories,
'    Albuquerque, New Mexico.
'
'  Reference:
'
'    E. Fehlberg,
'    Low-order Classical Runge-Kutta Formulas with Stepsize Control,
'    NASA Technical Report R-315.
'
'    L F Shampine, H A Watts, S Davenport,
'    Solving Non-stiff Ordinary Differential Equations - The State of the Art,
'    SIAM Review,
'    Volume 18, pages 376-411, 1976.
'
'  Parameters:
'
'    Input, external F, a user-supplied subroutine to evaluate the
'    derivatives Y'(T), of the form:
'
'      sub f ( t as double, y() as double, yp() as double )
'
'    Input, NEQN, the number of equations to be integrated.
'
'    Input/output, Y(NEQN), the current solution vector at T.
'
'    Input/output, YP(NEQN), the current value of the
'    derivative of the dependent variable.  The user should not set or alter
'    this information'
'
'    Input/output, T, the current value of the independent
'    variable.
'
'    Input, TOUT, the output point at which solution is
'    desired.  TOUT = T is allowed on the first call only, in which case
'    the routine returns with FLAG = 2 if continuation is possible.
'
'    Input, RELERR, ABSERR, the relative and absolute
'    error tolerances for the local error test.  At each step the code
'    requires:
'      abs ( local error ) <= RELERR * abs ( Y ) + ABSERR
'    for each component of the local error and the solution vector Y.
'    RELERR cannot be "too small".  If the routine believes RELERR has been
'    set too small, it will reset RELERR to an acceptable value and return
'    immediately for user action.
'
'    Input/output, FLAG, indicator for status of integration.
'    On the first call, set FLAG to +1 for normal use, or to -1 for single
'    step mode.  On return, a value of 2 or -2 indicates normal progress,
'    while any other value indicates a problem that should be addressed.
' ------------------------------------------------------------------
  
  CONST remin  = 1.0E-12
  CONST maxnfe = 3000  
  CONST eps    = 2.220446049250313E-16
  
  DIM AS INTEGER neqn, hfaild, k, mflag, outp
  
  DIM AS DOUBLE ae, dt, ee, eeoet, esttol, et
  DIM AS DOUBLE hmin, relerr_min, s, scale, tol, toln, ypk

  STATIC AS INTEGER flag_save = -1000
  STATIC AS INTEGER init      = -1000
  STATIC AS INTEGER kflag     = -1000
  STATIC AS INTEGER kop       = -1
  STATIC AS INTEGER nfe       = -1
  
  STATIC AS DOUBLE relerr_save = -1.0
  STATIC AS DOUBLE abserr_save = -1.0  
  STATIC AS DOUBLE h           = -1.0
  
  neqn = UBOUND(y)
  
  DIM AS DOUBLE f1(1 TO neqn)
  DIM AS DOUBLE f2(1 TO neqn)
  DIM AS DOUBLE f3(1 TO neqn)
  DIM AS DOUBLE f4(1 TO neqn)
  DIM AS DOUBLE f5(1 TO neqn)
  
' Check the input parameters.

  IF neqn < 1 OR relerr < 0 OR abserr < 0 OR _
    (flag = 0 OR flag > 8 OR flag < -2) THEN
      flag = 8
      EXIT SUB
  END IF

  mflag = ABS(flag)

' Is this a continuation call?

  IF mflag <> 1 THEN

    IF t = tout AND kflag <> 3 THEN
      flag = 8
      EXIT SUB
    END IF

    IF mflag = 2 THEN

      IF kflag = 3 THEN

        flag = flag_save
        mflag = ABS(flag)

      ELSEIF init = 0 THEN

        flag = flag_save

      ELSEIF kflag = 4 THEN

        nfe = 0

      ELSEIF kflag = 5 AND abserr = 0 THEN

        EXIT SUB

      ELSEIF kflag = 6 AND relerr <= relerr_save AND abserr <= abserr_save THEN

        EXIT SUB

      END IF

' FLAG = 3, 4, 5, 6, 7 or 8.

    ELSE

      IF flag = 3 THEN

        flag = flag_save
        IF kflag = 3 THEN 
          mflag = ABS(flag)
        END IF

      ELSEIF flag = 4 THEN

        nfe = 0
        flag = flag_save
        IF kflag = 3 THEN
          mflag = ABS(flag)
        END IF

      ELSEIF flag = 5 AND abserr > 0 THEN

        flag = flag_save
        IF kflag = 3 THEN
          mflag = ABS(flag)
        END IF

' Integration cannot be continued because the user did not respond to
' the instructions pertaining to FLAG = 5, 6, 7 or 8.

      ELSE
        EXIT SUB
      END IF

    END IF

  END IF

' Save the input value of FLAG.
' Set the continuation flag KFLAG for subsequent input checking.

  flag_save = flag
  kflag = 0

' Save RELERR and ABSERR for checking input on subsequent calls.

  relerr_save = relerr
  abserr_save = abserr

' Restrict the relative error tolerance to be at least
'
'    2 * EPS + REMIN
'
' to avoid limiting precision difficulties arising from impossible
' accuracy requests.

  relerr_min = 2 * eps + remin

' Is the relative error tolerance too small?

  IF relerr < relerr_min THEN
    relerr = relerr_min
    flag = 3
    kflag = 3
    EXIT SUB
  END IF

  dt = tout - t

' Initialization:
'
' Set the initialization completion indicator, INIT;
' set the indicator for too many output points, KOP;
' evaluate the initial derivatives;
' set the counter for function evaluations, NFE;
' estimate the starting stepsize.

  IF mflag = 1 THEN

    init = 0
    kop = 0
    f(t, y(), yp())
    nfe = 1

    IF t = tout THEN
      flag = 2
      EXIT SUB
    END IF

  END IF

  IF init = 0 THEN

    init = 1
    h = ABS(dt)
    toln = 0

    FOR k = 1 TO neqn
      tol = relerr * ABS (y(k)) + abserr
      IF tol > 0 THEN
        toln = tol
        ypk = ABS(yp(k))
        IF tol < ypk * h^5 THEN
          h = (tol / ypk)^0.2
        END IF
      END IF
    NEXT k

    IF toln <= 0 THEN h = 0
    
    h = Max(h, 26 * eps * Max(ABS(t), ABS(dt)))
    flag_save = SGN(flag) * 2
  END IF

' Set the stepsize for integration in the direction from T to TOUT.

  h = SGN(dt) * ABS(h)

' Test to see if too may output points are being requested.

  IF 2 * ABS(dt) <= ABS(h) THEN kop = kop + 1

' Unnecessary frequency of output.

  IF kop = 100 THEN
    kop = 0
    flag = 7
    EXIT SUB
  END IF

' If we are too close to the output point, then simply extrapolate and return.

  IF ABS(dt) <= 26 * eps * ABS(t) THEN
    t = tout
    FOR k = 1 TO neqn
      y(k) = y(k) + dt * yp(k)
    NEXT k
    f(t, y(), yp())
    nfe = nfe + 1
    flag = 2
    EXIT SUB
  END IF

' Initialize the output point indicator.

  outp = False

' To avoid premature underflow in the error tolerance function,
' scale the error tolerances.

  scale = 2 / relerr
  ae = scale * abserr

' Step by step integration.

  DO

    hfaild = False

' Set the smallest allowable stepsize.
'
    hmin = 26 * eps * ABS(t)

' Adjust the stepsize if necessary to hit the output point.
'
' Look ahead two steps to avoid drastic changes in the stepsize and
' thus lessen the impact of output points on the code.

    dt = tout - t

    IF 2 * ABS(h) > ABS(dt) THEN

' Will the next successful step complete the integration to the output point?

      IF ABS(dt) <= ABS(h) THEN
        outp = True
        h = dt
      ELSE
        h = 0.5 * dt
      END IF

    END IF

' Here begins the core integrator for taking a single step.
'
' The tolerances have been scaled to avoid premature underflow in
' computing the error tolerance function ET.
' To avoid problems with zero crossings, relative error is measured
' using the average of the magnitudes of the solution at the
' beginning and end of a step.
' The error estimate formula has been grouped to control loss of
' significance.
'
' To distinguish the various arguments, H is not permitted
' to become smaller than 26 units of roundoff in T.
' Practical limits on the change in the stepsize are enforced to
' smooth the stepsize selection process and to avoid excessive
' chattering on problems having discontinuities.
' To prevent unnecessary failures, the code uses 9/10 the stepsize
' it estimates will succeed.
'
' After a step failure, the stepsize is not allowed to increase for
' the next attempted step.  This makes the code more efficient on
' problems having discontinuities and more effective in general
' since local extrapolation is being used and extra caution seems
' warranted.
'
' Test the number of derivative function evaluations.
' If okay, try to advance the integration from T to T+H.

    DO

' Have we done too much work?

      IF maxnfe < nfe THEN
        flag = 4
        kflag = 4
        EXIT SUB
      END IF

' Advance an approximate solution over one step of length H.

      Fehl(f, neqn, y(), t, h, yp(), f1(), f2(), f3(), f4(), f5(), f1())
      nfe = nfe + 5

' Compute and test allowable tolerances versus local error estimates
' and remove scaling of tolerances.  The relative error is
' measured with respect to the average of the magnitudes of the
' solution at the beginning and end of the step.

      eeoet = 0

      FOR k = 1 TO neqn

        et = ABS(y(k)) + ABS(f1(k)) + ae

        IF et <= 0 THEN
          flag = 5
          EXIT SUB
        END IF

        ee = ABS((-2090 * yp(k) + (21970 * f3(k) - 15048 * f4(k))) _
                 + (22528 * f2(k) - 27360 * f5(k)))

        eeoet = Max(eeoet, ee / et)

      NEXT k

      esttol = ABS(h) * eeoet * scale / 752400

      IF esttol <= 1 THEN EXIT DO
                                                          
' Unsuccessful step. Reduce the stepsize, try again.
' The decrease is limited to a factor of 1/10.

      hfaild = True
      outp = False

      IF esttol < 59049 THEN
        s = 0.9 / esttol^0.2
      ELSE
        s = 0.1
      END IF

      h = s * h

      IF ABS(h) < hmin THEN
        flag = 6
        kflag = 6
        EXIT SUB
      END IF

    LOOP

' We exited the loop because we took a successful step.
' Store the solution for T+H, and evaluate the derivative there.

    t = t + h
    FOR k = 1 TO neqn
      y(k) = f1(k)
    NEXT k
    f(t, y(), yp())
    nfe = nfe + 1

' Choose the next stepsize.  The increase is limited to a factor of 5.
' If the step failed, the next stepsize is not allowed to increase.

    IF 0.0001889568 < esttol THEN
      s = 0.9 / esttol^0.2
    ELSE
      s = 5.0
    END IF

    IF hfaild THEN s = Min(s, 1.0)

    h = SGN(h) * Max(s * ABS(h), hmin)

'  End of core integrator
'
'  Should we take another step?

    IF outp THEN
      t = tout
      flag = 2
      EXIT SUB
    END IF

    IF flag <= 0 THEN EXIT DO

  LOOP

' One step integration mode.

  flag = -2

END SUB

