/*
  diffsys - integrate an ODE by Bullirsch-Stoer method
  
  program from:

  R. Bulirsch and J. Stoer, "Numerical Treatment of Ordinary
  Differential Equations by Extrapolation Methods", Numerische
  Mathematik 8, 1-13 (1966).

  translated from algol to C by Jim Van Zandt <jrv@vanzandt.mv.com>
*/

#include <stdio.h>
#include <math.h>
#include <errno.h>

int diffsys(int n, double *x, double *y, double h0, double eps, double *s,
	    double *hnext, void (*f)(double x, double *y, double *dy));

/*
  diffsys performs one integration step with a stepsize h <= H0 for a
  system of N first order ordinary differential equations of the form
  dz/dx = f(x,z), the righthand side of which is calculated by the
  user supplied function f.  The program takes the first of the
  numbers H0, H0/2, H0/4 ... as step size h for which no more than 9
  extrapolation steps are needed to obtain a sufficiently accurate
  result. 

  X and Y are the initial values.  After leaving the function, the
  original values of X and Y are replaced by X+h and y(X+h)
  respectively.  Also H0 may be changed (automatic step size
  correction).  If HNEXT is non-null, it is set to the (presumably)
  optimal step size for the next integration step.  The array S and
  the constant EPS are used to control the accuracy of the computed
  values: The function returns if for all i, two successive values for
  Y[i] differ by EPS*S[i] at most.  EPS should be not smaller than
  10^(-D+3) where the machine maintains D significant digits.  For the
  first integration step it is advisable to set S[i]=0.  After
  leaving, the array S is changed to S[i] = max(S[i], fabs(Y[i](z)), x
  <= z <= x+h).  */
int diffsys(int n, double *xx, double *y, double h0, double eps, double *s,
	    double *hnext, 
	    void (*f)(double t, double *y, double *dy))
{
  double x, a, b, b1, c, g, u, v, ta, fc;
  int i, j, k, kk, jj, l, m, r, sr;
  double ya[n], yl[n], ym[n], dy[n], dz[n], dt[n][7], d[7];
  double yg[8][n], yh[8][n];
  int konv, bo, bh, fin;

  x = *xx;
  (*f)(x, y, dz); 
  bh = fin = 0;
  for (i=0; i<n; i++) ya[i] = y[i];

  do
    {
      a = h0 + x; fc = 1.5; bo = 0; m = 1; r = 2; sr = 3; jj = -1;
      for (j=0; j<=9; j++)
	{
	  if (bo) {d[1] = 16./9; d[3] = 64./9; d[5] = 256./9;}
	  else {d[1] = 9./4; d[3] = 9; d[5] = 36;}
	  if (j>2) konv = 1; else konv = 0;
	  if (j>6) {l = 6; d[6] = 64; fc *= .6;}
	  else {l = j; d[l] = m*m;}
	  m *= 2; g = h0/m; b = g*2;
	  if (bh && (j < 8))
	    for (i=0; i<n; i++) {ym[i] = yh[j][i]; yl[i] = yg[j][i];}
	  else
	    {
	      kk = (m - 2)/2; m--;
	      for (i=0; i<n; i++) {yl[i] = ya[i]; ym[i] = ya[i] + g*dz[i];}
	      for (k=1; k<=m; k++)
		{
		  (*f)(x + k*g, ym, dy);
		  for (i=0; i<n; i++)
		    {
		      u = yl[i] + b*dy[i]; yl[i] = ym[i]; ym[i] = u;
		      u = fabs(u); if (u > s[i]) s[i] = u;
		    }
		  if ((k == kk) && (k != 2))
		    {
		      jj++;
		      for (i=0; i<n; i++)
			{
			  yh[jj][i] = ym[i]; yg[jj][i] = yl[i];
			}
		    }
		}
	    }
	  (*f)(a, ym, dy);
	  for (i=0; i<n; i++)
	    {
	      v = dt[i][0]; ta = c = dt[i][0] = (ym[i] + yl[i] + g*dy[i])/2;
	      for (k=1; k<=l; k++)
		{
		  b1 = d[k]*v; b = b1 - c; u = v;
		  if (b != 0) {b = (c-v)/b; u = c*b; c = b1*b;}
		  v = dt[i][k]; dt[i][k] = u; ta = u + ta;
		}
	      if (fabs(y[i] - ta) > eps*s[i]) konv = 0; 
	      y[i] = ta;
	    }
	  if (konv) {*hnext = h0*fc; *xx = a; return fin;} /* normal return */
	  d[2] = 4; d[4] = 16; bo = !bo; m = r; r = sr; sr = m*2;
	}
      bh = !bh; fin = 1; h0 /= 2;
    } while (x + h0 != x);	/* check for underflow */
  return -EDOM;			/* none of the standard error codes
                                   really seems appropriate */
}

int evals=0;


#ifdef DEMO

#ifdef EXP

void f(double x, double y[], double dy[])
{
  dy[0] = -y[0];
  evals++;
}

int main()
{
  int n=1;
  double h0, hnext, eps, x, s[n], y[n];
  h0 = .5; eps = 1e-8;
  x = 0; y[0] = 1; s[0] = 0;
  do
    {
      s[0]=0;
      diffsys(n, &x, y, h0, eps, s, &hnext, f);
      printf("x=%5.2f  yest=%20.13g  relerr=%9.2g  evals=%d\n",
	     x, y[0], fabs((y[0]-exp(-x))/y[0]), evals);
    } while (x < 20);
  return 0;
}

#else /* !EXP */

void f(double t, double z[], double dz[])
{
  // An ODE arising from the restricted problem of three bodies
  // (earth-moon-spaceship), see: 
  // 
  // Fehlberg, E: Runge-Kutta type formulas of high-order accuracy
  // and their application to the numerical integration of the
  // restricted problem of three bodies.  Colloque international des
  // Techniques de Calcul Analogique et Numerique en Aeronautique a
  // Liege, 1963.
  //
  // Filippi, S.: Angenaherte Losung eines astronomischen
  // Drei-Korperproblems.  Teil II.  Electronische Datenverarbeitung,
  // Heft 6, 264-268 (1963).
  // 
  // According to Bulirsch and Stroer, 
  // the solution is a closed orbit with period T=6.192169331396...
  // This simulation suggests the period is       6.192169331317...
  // Over one orbit, the step size varies by more than two orders of
  // magnitude.

  double mu=1/82.45, mp=1-mu;
  double x=z[0], y=z[2];
  // 0   1   2   3
  // x   x'  y   y'
  dz[0] = z[1];
  dz[1] = x + 2*z[3]
    - mp*(x + mu)/pow((x+mu)*(x+mu) + y*y, 1.5) 
    - mu*(x - mp)/pow((x-mp)*(x-mp) + y*y, 1.5);
  dz[2] = z[3];
  dz[3] = y - 2*z[1]
    - mp*y/pow((x+mu)*(x+mu) + y*y, 1.5)
    - mu*y/pow((x-mp)*(x-mp) + y*y, 1.5);

  evals++;
}

int main()
{
  int n=4, steps=0, i;
  double h0=.2, hnext, t=0, tf=6.192169331396, s[n], tprev=0;
  double z[4] = {1.2, 0, 0, -1.04935750983};
  /* with this value of eps, this implementation reproduces the
     intermediate points shown in the figure in Bulirsch and Stroer's
     paper */
  double eps=9e-12;
  for (i=0; i<n; i++) s[i] = 0.;

  printf("#       t                x                 y        evals  steps   h0\n");
  do
    {
      diffsys(n, &t, z, h0, eps, s, &hnext, f);
      steps++;
      printf("%15.12f  %16.12f  %16.12f %5d  %3d  %6.4f\n",
	     t, z[0], z[2], evals, steps, t-tprev);
      h0 = hnext;
      tprev=t;
      if (t+h0 > tf) h0 = tf - t;
    } while (t < tf);
    printf("# x'=%16.12f y'=%16.12f period=%18.14f\n",
     z[1], z[3], t-z[2]/z[3]);
  return 0;
}
    
#endif /* EXP */
#endif /* DEMO */