\ Numerical solution of a first-order ordinary
\ differential equation by 4th+5th -order Runge-Kutta
\ -Fehlberg algorithm, following Paul L. DeVries, "A First
\ Course in Computational Physics" (Wiley, New York, 1994),
\ p. 220ff.


\ ---------------------------------------------------
\     (c) Copyright 2004  Julian V. Noble.          \
\       Permission is granted by the author to      \
\       use this software for any application pro-  \
\       vided this copyright notice is preserved.   \
\ ---------------------------------------------------

\ This is an ANS Forth program requiring the
\   FLOAT, FLOAT EXT, FILE and TOOLS EXT wordsets.

\ Environmental dependencies:
\       Assumes independent floating point stack


\ Solves    dx/dt = f(x,t)  where x and f are scalars.

\ This version of the program does not use a FORmula TRANslator

FALSE [IF]
Example
    : fnb   ( f: x t -- t^2*exp[-x])
        FDUP  F*  FSWAP  FNEGATE  FEXP  F*
    ;

    use( fnb 0e0 0e0 5e0 0.1e0 eps: 1e-5 )runge
[THEN]

MARKER  -runge


INCLUDE vector1.f      \ for passing a function name as an argument

: eps:   ;      \ syntactic sugar

v: fdummy       \ place to put function name

: fvariables     0 DO  FVARIABLE  LOOP  ;

6 fvariables t tf epsilon delta MaxErr h
8 fvariables x xp k0 k1 k2 k3 k4 k5

\ ---------------------------- define constants
: /fconst   F/  FCONSTANT  ;

  1e0 4e0   FOVER FOVER     /fconst c1
                            /fconst a10

  3e0 8e0                   /fconst c2

  3e0 32e0                  /fconst a20
  9e0 32e0                  /fconst a21

  12e0 13e0                 /fconst c3

 1932e0 2197e0              /fconst a30
 -7200e0 2197e0             /fconst a31
 7296e0 2197e0              /fconst a32

 439e0 216e0                /fconst a40
 -8e0                     FCONSTANT a41
 3680e0 513e0               /fconst a42
 -845e0 4104e0              /fconst a43

 1e0 2e0                    /fconst c5

 -8e0 27e0                  /fconst a50
 2e0                      FCONSTANT a51
 -3544e0 2565e0             /fconst a52
 1859e0 4104e0              /fconst a53
 -11e0 40e0                 /fconst a54

 1e0 360e0                  /fconst b0
 -128e0 4275e0              /fconst b2
 -2197e0 75240e0            /fconst b3
 1e0 50e0                   /fconst b4
 2e0 55e0                   /fconst b5

 16e0 135e0                 /fconst d0
 6656e0 12825e0             /fconst d2
 28561e0 56430e0            /fconst d3
 -9e0 50e0                  /fconst d4
 2e0 55e0                   /fconst d5
\ -------------------- end constant definitions

: do_k0     \ f" k0 = h*fdummy(x,t)"
    h F@  x F@  t F@                    fdummy F*   k0 F!
;

: do_k1     \ f" k1 = h*fdummy(x+[a10]*k0,t+[c1]*h)"
    h F@  x F@  a10 k0 F@  F* F+
        t F@  c1 h F@  F* F+            fdummy F*   k1 F!
;

: do_k2     \ f" k2 = h*fdummy(x+[a20]*k0+[a21]*k1,t+[c2]*h)"
    h F@  x F@  a20 k0 F@  F* F+
                a21 k1 F@  F* F+
                t F@  c2 h F@  F* F+    fdummy F*   k2 F!
;

: do_k3     \ f" k3 = h*fdummy(x+[a30]*k0+[a31]*k1+[a32]*k2,t+[c3]*h)"
    h F@  x F@  a30 k0 F@  F* F+
                a31 k1 F@  F* F+
                a32 k2 F@  F* F+
                t F@  c3 h F@ F* F+     fdummy F*   k3 F!
;

: do_k4     \ f" k4 = h*fdummy(x+[a40]*k0+[a41]*k1+[a42]*k2+[a43]*k3,t+h)"
    h F@  x F@  a40 k0 F@  F* F+
                a41 k1 F@  F* F+
                a42 k2 F@  F* F+
                a43 k3 F@  F* F+
                t F@  h F@  F+          fdummy F*   k4 F!
;

: do_k5     \ f" k5 = h*fdummy(x+[a50]*k0+[a51]*k1+[a52]*k2+[a53]*k3+
            \        [a54]*k4,t+[c5]*h)"
    h F@  x F@  a50 k0 F@  F* F+
                a51 k1 F@  F* F+
                a52 k2 F@  F* F+
                a53 k3 F@  F* F+
                a54 k4 F@  F* F+
                t F@  c5 h F@  F* F+    fdummy F*   k5 F!
;

: step_too_big?
    \ f" delta = abs( [b0]*k0+[b2]*k2+[b3]*k3+[b4]*k4+[b5]*k5 )"
    b0 k0 F@  F*    b2 k2 F@  F* F+
                    b3 k3 F@  F* F+
                    b4 k4 F@  F* F+
                    b5 k5 F@  F* F+     FABS    delta F!

    \ f" MaxErr = abs(h)*epsilon"
    h F@    FABS  epsilon F@  F*                MaxErr F!
    delta F@  MaxErr F@  F>
;

: shrink    \ reduce step size
    \ f" h = max( 0.9*h*sqrt(sqrt(Maxerr/delta)) , [c1]*h)"
    0.9E0 h F@  Maxerr F@  delta F@  F/
    FSQRT FSQRT  F* F*   c1 h F@  F*
    FMAX  h F!      \ however, don't make h' < 0.25*h
;

: update
    \ f" x = x + [d0]*k0+[d2]*k2+[d3]*k3+[d4]*k4+[d5]*k5"
    x F@    d0 k0 F@  F* F+
            d2 k2 F@  F* F+
            d3 k3 F@  F* F+
            d4 k4 F@  F* F+
            d5 k5 F@  F* F+         x F!

    \ f" t = t+h"
    t F@  h F@  F+                  t F!
;

: expand    \ increase step size
    \ f" h = min( 0.9*h*sqrt(sqrt(Maxerr/delta)) , 4*h)"
    0.9E0 h F@  Maxerr F@  delta F@  F/
    FSQRT FSQRT  F* F*  .400000E1 h F@  F*
    FMIN  h F!      \ however, don't make h' > 4*h
;

: step
    do_k0   do_k1   do_k2   do_k3   do_k4   do_k5

    step_too_big?
    IF      shrink      \ reduce |h|
            RECURSE     \ redo with smaller |h|

    ELSE    \ error acceptable, update variables
            update
            expand      \ perhaps increase step size
    THEN
;

: initialize    ( xt --)  ( f: x0 t0 tf h epsilon -- )
    8 set-precision
    epsilon F!
    h F!
    tf F!
    t F!
    x F!
    defines fdummy      \ vector fn_name
;

: display   ( --)
    CR   t F@  FS.  5 SPACES   x F@  FS.
    5 SPACES
    \ f" ln(1+t^3/3)"
    t F@  FDUP  FDUP  F* F*  3e0 F/  1e0 F+   FLN  FS.
;

: done?  t F@  tf F@  F>  ;

: )runge   ( xt --)  ( f: t0 tf h -- )
    initialize
    BEGIN               \ solve
        display   step   done?
    UNTIL
        display
;