/* 
  Adams-Bashforth-Moulton integration formulas.

  Reference:
  Shampine, L. F. and M. K. Gordon, _Computer Solution of
  Ordinary Differential Equations_, W. H. Freeman, 1975.
 
  Program by Steve Moshier.
*/

# include <stdlib.h>
# include <stdio.h>

# include "ode_moshier.h"

/* 
  Divided differences
*/

#define N 19

/*
  Predictor coefficients
*/

double precof[N] = {
1.0,
1.0 / 2.0,
5.0 / 12.0,
3.0 / 8.0,
251.0 / 720.0,
95.0 / 288.0,
19087.0 / 60480.0,
5257.0 / 17280.0,
1070017.0 / 3628800.0,
25713.0 / 89600.0,
26842253.0 / 95800320.0,
4777223.0 / 17418240.0,
703604254357.0 / 2615348736000.0,
106364763817.0 / 402361344000.0,
1166309819657.0 / 4483454976000.0,
2.5221445e7 / 9.8402304e7,
8.092989203533249e15 / 3.201186852864e16,
8.5455477715379e13 / 3.4237292544e14,
1.2600467236042756559e19 / 5.109094217170944e19,
};

/*
  Corrector coefficients
*/
double corcof[N] = {
 1.0,
 -1.0 / 2.0,
 -1.0 / 12.0,
 -1.0 / 24.0,
 -19.0 / 720.0,
 -3.0 / 160.0,
 -863.0 / 60480.0,
 -275.0 / 24192.0,
 -33953.0 / 3628800.0,
 -8183.0 / 1036800.0,
 -3250433.0 / 479001600.0,
 -4671.0 / 788480.0,
 -13695779093.0 / 2615348736000.0,
 -2224234463.0 / 475517952000.0,
 -132282840127.0 / 31384184832000.0,
 -2639651053.0 / 689762304000.0,
 1.11956703448001e14 / 3.201186852864e16,
 5.0188465e7 / 1.5613165568e10,
 2.334028946344463e15 / 7.86014494949376e17,
};

/******************************************************************************/

void divdif ( double vec[], int k, double *diffn )

/******************************************************************************/
/* 
  Compute zeroth through kth backward differences
  of the data in the input array
*/
/*
  double vec[], input array of k+1 data items;
  int k;
  double *diffn, output array of ith differences;
*/
{
double diftbl[N];
double *p, *q;
double y;
int i, o;

/* Copy the given data (zeroth difference) into temp array
 */
p = diftbl;
q = vec;
for( i=0; i<=k; i++ )
	*p++ = *q++;

/* On the first outer loop, k-1 first differences are calculated.
 * These overwrite the original data in the temp array.
 */
o = k;
for( o=k; o>0; o-- )
	{
	p = diftbl;
	q = p;
	for( i=0; i<o; i++ )
		{
		y = *p++;
		*q++ = *p - y;
		}
	*diffn++ = *p; /* copy out the last (undifferenced) item */
#if DEBUG
	printf( "%.5e ", *p );
#endif
	}
#if DEBUG
	printf( "%.5e\n", *(q-1) );
#endif
*diffn++ = *(q-1);
}


/* Update array of differences, given new data value.
 * diffn is an array of k+1 differences, starting with the
 * zeroth difference (the previous original data value).
 */
/******************************************************************************/

void dupdate ( register double *diffn, int k, double f )

/******************************************************************************/
/*
  register double *diffn, input and output array of differences;
  int k, max order of differences;
  double f; new data point (zeroth difference) 
*/
{
double new, old;
int i;

new = f;
for( i=0; i<k; i++ )
	{
	old = *diffn;
	*diffn++ = new;
#if DEBUG
	printf( "%.5e ", new );
#endif
	new = new - old;
	}
#if DEBUG
	printf( "%.5e\n", new );
#endif
*diffn++ = new;
}




/* Evaluate the interpolating polynomial
 *
 *              (x - x )
 *                    n    1
 * P(x) = f  +  --------  D f   +  ...
 *         n       h         n
 *
 *     (x - x )(x - x   )...(x - x     )
 *           n       n-1          n+2-k    k-1
 *  +  ---------------------------------  D    f
 *                   k-1                        n
 *                  h     (k-1)!
 *
 *
 *         j
 *  where D denotes the jth backward difference, see dupdate(), and
 *
 *  f   =   f( x , y(x ) )  is the interpolated derivative y'(x ) .
 *   n          n     n                                        n
 *
 * The subroutine argument t is linearly scaled so that t = 1.0
 * will evaluate the polynomial at x = x_n + h,
 * t = 0.0 corresponds to x = x_n, etc.
 */

/******************************************************************************/

double difpol( double *diffn, int k, double t )

/******************************************************************************/
/*
  int k; differences go up to order k-1
  double t; scaled argument 
*/
{

double f, fac, s, u;
int i;

f = *diffn++; /* the zeroth difference = nth data point */
u = 1.0;
/*s = x/h - n;*/
s = 1.0;	/* to evaluate the polynomial at x = xn + h */
fac = 1.0;
for( i=1; i<k; i++ )
	{
	if( s == 0.0 )
		break;
	u *= s / fac;
	f += u  *  (*diffn++);
	fac += 1.0;
	s += 1.0;
	}
return( f );
}


/* Integrate the interpolating polynomial from x[n] to x[n+1]
 * to obtain the change in the integrated function from y[n] to y[n+1]
 * given by:
 *
 *                     k-1
 *                      -       i
 *  y    =  y   +   h   >   c  D  f    .
 *   n+1     n          -    i     n
 *                     i=0
 *
 *  This subroutine returns the summation term, not multiplied by h.
 *  The coefficients c_i of the integration formula are either
 *  precof[] or corcof[], given above.
 */
/******************************************************************************/

double intpol( double *diffn, double *coeffs, int k )

/******************************************************************************/
/*
  double *diffn; array of backward differences 
  double *coeffs; coefficients of integration formula 
  int k; differences used go up to order k-1 
*/
{
double s;
int i;

s = 0.0;
coeffs += k;
diffn += k;
for( i=0; i<k; i++ )
	{
	s += (*--coeffs) * (*--diffn);
	}
return( s );
}

/******************************************************************************/

void vcopy ( register double *p, register double *q, register int n )

/******************************************************************************/
/*
  Purpose:

    VCOPY copies N elements from array P to array Q.
*/
{
  do
  {
    *q++ = *p++;
  } while( --n );

  return;
}


/* Adams initialization program.
 */

/* Addresses within the work array */
static double *dv;
static double *dvp;
static double *vp;
static double *yp;
static double *vn;
static double *delta;
static double *sdelta;
static double *y0;

static double ccor;
static double hstep; /* step size (constant) */
static int order; /* Order of the prediction formula */
static int ordp1;
static int asiz;
static int dsiz;
static int jstep; /* counts steps taken */

/* Initialize pointers in work array, compute derivatives at
 * initial position, and start the difference tables.
 * neq >= nequat in adstep() below.  If neq > nequat, unused space
 * is left for extra variables that are not actually integrated.
 */

/******************************************************************************/

void adstart ( double h, double yn[], double work[], int neq, int ord, double t )

/******************************************************************************/
{
  double *p;
  int j;

  hstep = h;
  ccor = hstep * precof[ord];
  jstep = 0;
  dsiz = ord + 2;
  asiz = neq * dsiz;
  order = ord;
  ordp1 = ord + 1;
  p = work;
  dv = p;
  p += asiz;
  dvp = p;
  p += dsiz;
  vp = p;
  p += neq;
  yp = p;
  p += neq;
  vn = p;
  p += neq;
  delta = p;
  p += neq;
  sdelta = p;
  p += neq;
  y0 = p;


  func ( t, yn, vn );

  p = dv;
  for( j=0; j<neq; j++ )
  {
    dupdate ( p, 0, vn[j] );
    p += dsiz;
  }
  return;
}

/******************************************************************************/

double adstep ( double *t, double yn[], int nequat )

/******************************************************************************/
/*
  Purpose:

    ADSTEP takes an Adams-Bashforth-Moulton step.

  Modified:

    28 December 2007
*/
{
double e, e0, time;
double *pdv;
int i, j;
double intpol();

time = *t;
jstep += 1;

/* 
  Do Runge-Kutta for the first ord + 1 steps.
*/

if( jstep <= ordp1 )
	{
	rungek(  nequat, time, yn, hstep, yn, delta );
	func( time+hstep, yn, vn );
	pdv = dv;
	for( j=0; j<nequat; j++ )
		{
		dupdate( pdv, jstep, vn[j] );
		pdv += dsiz;
		}
	e = 0.0;
	goto done;
	}



/* Predict the next position
 * based on current y' difference table dv[].
 */
	pdv = dv;
	for( i=0; i<nequat; i++ )
		{
		yp[i] = yn[i] + hstep * intpol( pdv, precof, order );
		pdv += dsiz;
		}
/* Evaluate derivatives at the predicted position
 */
	func( time+hstep, yp, vp );

/* Correct the predicted position and velocity using the derivatives
 * evalutated at yp.
 */
	pdv = dv;
	for( i=0; i<nequat; i++ )
		{
		vcopy( pdv, dvp, dsiz ); /* y' difference table */
		dupdate( dvp, ordp1, vp[i] );
/* Note, the following line is equivalent to:
 *		yn[i] = yn[i] + hstep * intpol( dap, corcof, ordp1 );
 * where e is the corrected value of the next vn.
 */
		yn[i] = yp[i] + ccor * dvp[order];
		pdv += dsiz;
		}

/* Evaluate derivative at the final position
 */
	func( time+hstep, yn, vn );

	e = 0.0;
	pdv = dv;
	for( i=0; i<nequat; i++ )
		{
		dupdate( pdv, ordp1, vn[i] );
/* Note, the following line is equivalent to:
 *		yn[i] = yn[i] + hstep * intpol( dap, corcof, ordp1 );
 * where e is the corrected value of the next vn.
 */
		yn[i] = yp[i] + ccor * pdv[order];
/* Estimate the error
 */
		e0 = hstep * pdv[order] * corcof[order];
		if( e0 < 0.0 )
			e0 = -e0;
		if( e0 > e )
			e = e0;
		pdv += dsiz;
		}
done:
time += hstep;
*t = time;
return( e );
}