#include <assert.h>
#include <stdlib.h>
#include <math.h>
#include <stdio.h>
#include "gpk_std.h"
#include "gpklib/bulsto.h"
#include "gpklib.h"

#define FRAC    0.66    /* used to make steps align better with tvals[] */
#define STEPALIGN       3e-2 /* max expansion to make a step align. */
#define FAILSHRINK      0.3
#define OK_SHRINK	0.9
#define OK_EXPAND	1.12
#define ACCURACY        1e-15   /* machine accuracy */
#define SILLY           1e-6    /* used to limit stepsize. */

#define DEBUG
#define PRINTSTUFF	1	/* how often to print diagnostics */
#undef DEBUG
#undef PRINTSTUFF
#undef DEBUG2


C static int NUSE = 5;

#ifdef DEBUG2
# define L	fprintf(stderr, "%d\n", __LINE__)
#else
# define L	/**/
#endif
struct bsresult	{int nsteps; double erratend;};
/*
Returns 0 for step failed, or the index of the number of substeps if
the step succeeded.  This index should be used to adjust the stepsize so that
the returned value is equal to NUSE.
If the step is successful, it also calls (*derivs)(), to determine whether
the step led to a legal region. If so, dydx0 is returned updated.
If the step failed, both y and dydx0 are returned unchanged.
*/

struct bsresult bsstep(double *y, double *dydx0, int nv, double h,
			double eps, C double *yscal,
			BOOL (*derivs)(double *dy, C double *y, int ny,
					void *c, int nc),
			void *c, int nc)
{
 double **d; 
 C LOCAL int nseq[] = {1,2,3,4,6,9,13,17,25,33,49};
 double errmax=1.0;
 int i, j;
 int ms = R_NOERR;
 double x[sizeof(nseq)/sizeof(*nseq)];
 struct bsresult rv;
#ifdef PRINTSTUFF
 LOCAL unsigned foo = 0;     /* just to space printing. */
#endif

 L;
 double *ysav = double1alloc(nv);
 double *yerr = double1alloc(nv);
 double *dysav = double1alloc(nv);
 double *yseq = double1alloc(nv);
 d = double2alloc(nv, NUSE);	L;

 assert((double*)ysav && (double*)yerr && (double*)dysav && (double*)yseq && d);
 if(eps < ACCURACY)    
	eps = ACCURACY;
 for(i=0; i<nv; i++)      {
	ysav[i] = y[i];
	dysav[i] = dydx0[i];
	}
	L;
 for(i=0; i<(int)(sizeof(nseq)/sizeof(*nseq)); i++)   {
#      ifdef DEBUG
	fprintf(stderr, "bsstep i=%1d, %2d substeps, h=%7.3g ", i, nseq[i], h);
#      endif
	L;
	ms = euler(ysav, dysav, nv, h, nseq[i], yseq, derivs, c, nc);
	if(ms != R_NOERR)     {
		fprintf(stderr, "euler fails: err type %d\n", ms);
		break;
		}

	x[i] = 1.0/nseq[i];	L;
	if(!rzextr(i, x, yseq, y, yerr, nv, NUSE, d))   {
		fprintf(stderr, "rzextr fails\n");
		break;
		}
	for(errmax=0.0,j=0; j<nv; j++)    {
		double ertmp;
		double tol = fabs(ysav[j]) + fabs(y[j]) + yscal[j];

		ertmp = fabs(yerr[j]) / tol;
		if(ertmp > errmax)     
			errmax = ertmp;
		}
#      ifdef DEBUG
	fprintf(stderr, "err:%7.3g", errmax);
#      endif
	if(i>0 && errmax<=eps && (*derivs)(dydx0, y, nv, c, nc)) {
#	       ifdef DEBUG
		fprintf(stderr, "OK");
#	       endif
#              ifdef PRINTSTUFF
		if(foo++%PRINTSTUFF == 1)
		    fprintf(stderr, "Bul-Sto-Euler err=%g at i=%d; step=%g ok\n", errmax,
					 i, h);
#              endif
#	       ifdef DEBUG
		fprintf(stderr, "!!");
#	       endif
		double2free(d);
		double1free(ysav);
		double1free(yerr);
		double1free(dysav);
		double1free(yseq);
		rv.nsteps = i;
		rv.erratend = errmax;
		return  rv;      /* Success!! */
		}
#      ifdef DEBUG
	else    fprintf(stderr, "nogo\n");
#      endif
	}
 fprintf(stderr, "bsstep:Step too big: i=%d, err=%g, ms=%d, step=%g\n", i,
 					errmax, ms, h);
 L;
 for(i=0;i<nv;i++)      {
	y[i] = ysav[i];
	dydx0[i] = dysav[i];
	}
 double2free(d);
 double1free(ysav);
 double1free(yerr);
 double1free(dysav);
 double1free(yseq);
 rv.nsteps = -ms;	rv.erratend = errmax;
 return rv;              /* Failure !! */
}


struct xdot_s	{
	void *c;
	BOOL (*xdot_func)(double *xdot, C double *x, int nx, void *c, int nc);
	};


LOCAL BOOL xdot_neg(double *xdot, C double *x, int nx, void *c, int nc)
{
 BOOL tmp;
 C struct xdot_s *cc = (C struct xdot_s *)c;

 if((tmp = (*cc->xdot_func)(xdot, x, nx, cc->c, nc)) != 0)   {  /* assignment */
 	int i;
	for(i=0;i<nx;i++)
		xdot[i] = -xdot[i];
	}
	L;
 return tmp;
}


LOCAL BOOL copy(double *to, const double *from,
				int ndoub, void *c, int nc, int nout)
{
 double *C lto = to+nout;

 assert(ndoub >= nout);
 for(;to<lto;)
	*to++ = *from++;
 L;
 return TRUE;
}

/* this handles integration in reverse time by negating the array of tv */
/* and negating xdot. */


struct runge_s bulsto(double *x, void *c, int nx, int nc,
			BOOL (*xdot)(double *xdot, const double *x, int nx,
					void *c, int nc),
			double eps, C double *tv, int ntv,
			BOOL (*out)(double *xo, const double *x, int nx,
					void *c, int nc, int nout),
			int nout, C double *lim)
{
 struct runge_s rv;
 double newstep, last_interval;
 double *limit, *tvl;
 C double *tvals;
 int l;
 struct xdot_s negative;
 void *c_xdot;

 assert(x && nx>0 && tv && ntv>1);
 assert(out || (nout<=nx));
 rv.isgood = FALSE;
 rv.stepcount = 0;
 rv.errtype = R_NOMEM;

 if(nout > 0)
	rv.xx = (double *)malloc((unsigned)ntv*sizeof(*rv.xx)*nout);
 else  
	rv.xx = x;


 limit = double1alloc(nx);
 assert(limit);
 if(lim)        {
	int j;
	for(j=0;j<nx;j++)
		limit[j] = fabs(lim[j]);
	}
 else {
	int j;
	for(j=0;j<nx;j++)
		limit[j] = 0.0;
	}

 if(tv[1] > tv[0])  {
	tvals = tv;
	tvl = 0;
	c_xdot = c;
	}
 else   {
	int i;
	tvl = double1alloc(ntv);
	assert(tvl);
	for(i=0;i<ntv;i++)
		tvl[i] = -tv[i];
	tvals = tvl;

	negative.c = c;
	negative.xdot_func = xdot;
	c_xdot = (void *)&negative;
	xdot = xdot_neg;
	L;
	}

 {int j; for(j=1;j<ntv;j++)     assert(tvals[j]>tvals[j-1]);}
 rv.t = tvals[0];	L;


 double *dxdt0 = double1alloc(nx);
 assert(dxdt0);

 L;

 if(!rv.xx || !dxdt0 || !tvals)     {
	if(rv.xx && rv.xx!=x)
		free((void *)(rv.xx));
	double1free(limit);
	if(tvals == tvl)     
		double1free(tvl);
	fprintf(stderr, "Cannot allocate output array\n");
	double1free(dxdt0);
	rv.xx = (double *)0;
 	L;
	return rv;
	}
 L;
 if(!out && nout>0)
	out = copy;
 L;
 if(out && !(*out)(&rv.xx[0], x, nx, c, nc, nout))   {
	fprintf(stderr, "Output routine fails on first attempt\n");
	rv.errtype = R_OUTFAIL;
	rv.xx = (double *)0;
	if(rv.xx && rv.xx!=x)
		free((void *)(rv.xx));
	double1free(limit);
	if(tvals == tvl)
		double1free(tvl);
	double1free(dxdt0);
	L;
	return rv;
	}
 L;

 if(!(*xdot)(dxdt0, x, nx, c_xdot, nc)) {
	L;
	rv.errtype = R_BADSTART;
	double1free(limit);
	if(tvals == tvl)     
		double1free(tvl);
	double1free(dxdt0);
	L;
	return rv;
	}
 L;

 newstep = tvals[1] - tvals[0];
 last_interval = tvals[1] - tvals[0];
 L;
 for(rv.stepcount=1,l=nout;rv.stepcount<ntv;rv.stepcount++,l+=nout)  {
	BOOL ends_at_tval;
	BOOL step_succeeded;
	C double interval = tvals[rv.stepcount] - tvals[rv.stepcount-1];
	L;

	/* If the step size was limited by the interval between reporting points,
		we may be able to increase the step size for the next interval. */
	if( interval > last_interval  && newstep*(1+OK_EXPAND)>last_interval)
		newstep *= sqrt(interval / last_interval);
	L;

	do {
 		struct bsresult ic;
		assert(rv.t < tvals[rv.stepcount]);	L;
				/* Adjust step length to match required reporting points: */
		if(rv.t+newstep*(1+STEPALIGN) > tvals[rv.stepcount])	{
			newstep = tvals[rv.stepcount] - rv.t;
			ends_at_tval = TRUE;
			}
		else if(rv.t+2*newstep > tvals[rv.stepcount])	{
			newstep = (tvals[rv.stepcount]-rv.t)*(1+STEPALIGN)/2.0;
			ends_at_tval = FALSE;
			}
		else	ends_at_tval = FALSE;
		L;

		assert(newstep > 0.0);	L;
		ic = bsstep(x, dxdt0, nx, newstep, eps*sqrt(newstep/interval), limit, xdot, c_xdot, nc);

		step_succeeded = (ic.nsteps > 0);
		if(step_succeeded)	{
			rv.t += newstep;
			newstep *= ic.nsteps>NUSE ? OK_SHRINK
						: (ic.nsteps<NUSE ? OK_EXPAND : 1.0);
			}
		else newstep *= FAILSHRINK;
		L;

		if(newstep<=rv.t*ACCURACY || newstep<=interval*SILLY)  {
			fprintf(stderr, "R_SMALLSTEP: %g %g %g\n", newstep,
													rv.t, interval);
			rv.errtype = ic.nsteps<0 ? -ic.nsteps
						 : R_SMALLSTEP;
			double1free(limit);
			if(tvals == tvl)
				double1free(tvl);
			double1free(dxdt0);
			L;
			return rv;
			}
		L;
		}while(!(ends_at_tval && step_succeeded));
	L;
	if(out && !(*out)(&rv.xx[l], x, nx, c, nc, nout))    {
		rv.errtype = R_OUTFAIL;
		double1free(limit);
		if(tvals == tvl)
			double1free(tvl);
		double1free(dxdt0);
		L;
		return rv;
		}
	L;
	}
 L;
 rv.isgood = TRUE;
 rv.errtype = R_NOERR;
 double1free(limit);
 if(tvals == tvl)
	double1free(tvl);
 double1free(dxdt0);
 L;
 return rv;
}
