#include <math.h>
#include "gpk_std.h"
#include "numerics.h"
#include "gpkieee754.h"
#include "assert.h"

/* #define DEBUG */

#ifdef DEBUG
# include <stdio.h>
#endif
#ifdef __SunOS5
# include <sunmath.h>
#endif

inline double min(double a, double b)	{return a<b ? a : b;}

LOCAL double
root1(double (*f)(double x, C void *p), double xl, double xh,
			C void *p, double fl, double fh, double epsx, double epsf)
{
#ifdef DEBUG
 fprintf(stderr, "root1: x: %g %g; f: %g %g\n", xl, xh, fl, fh);
#endif

 assert(xh >= xl);

 if(fl == 0)
	return xl;
 if(fh == 0)
	return xh;
 if(xh-xl<epsx || fabs(fl)<epsf || fabs(fh)<epsf)
 	return (xl*fh-xh*fl)/(fh-fl);

 assert((fh>0) != (fl>0));

 C double xsec = (xl*fh-xh*fl)/(fh-fl);
 C double fsec = (*f)(xsec, p);
 C double afs = fabs(fsec);
 C double minfabs = min(fabs(fl), fabs(fh));
 C double sb = afs<minfabs ? sqrt(0.5*afs/(afs+minfabs)) : 0.5;
 C double xbi = (fsec>0)==(fl>0) ? (xsec*(1-sb)+xh*sb) : (xl*sb+xsec*(1-sb));
 C double fbi = (*f)(xbi, p);
 assert(xsec>=xl && xsec<=xh);
 assert(xbi>=xl && xbi<=xh);

 double xx[4], ff[4];
 xx[0] = xl;	xx[3] = xh;
 ff[0] = fl;	ff[3] = fh;
 if(xbi < xsec)	{
 	xx[1] = xbi;	ff[1] = fbi;
	xx[2] = xsec;	ff[2] = fsec;
	}
 else	{
 	xx[2] = xbi;	ff[2] = fbi;
	xx[1] = xsec;	ff[1] = fsec;
	}

 int i;
 for(i=1; i<4; i++)
	if((ff[i]>0.0) != (ff[i-1]>0.0))
		return root1(f, xx[i-1], xx[i], p, ff[i-1], ff[i], epsx, epsf);
 assert(0);
 return 0.0;
}



class ddb	{
	private:
	C static double qn;
	public:
	double low, high;
	double flow, fhigh;
	ddb(): low(qn), high(qn), flow(qn), fhigh(qn) {}
	ddb(double l, double h, double fl, double fh)
		:low(l), high(h), flow(fl), fhigh(fh) {}
	BOOL in() C;
	ddb operator|(C ddb& x) C;
	operator BOOL () C	{return in() && low<=high;}
	};

C double ddb::qn = quiet_nan(0);

BOOL ddb::in() C
{
 return (flow<0.0 && fhigh>=0.0) || (flow>=0.0 && fhigh<0.0);
}

ddb ddb::operator|(C ddb& x) C
{
 return *((!this->in() && x.in()) ? &x : this);
}

LOCAL ddb
root2(double (*f)(double x, C void *p), double xl, double xh,
			C void *p, double fl, double fh, unsigned n)
{
#ifdef DEBUG
 fprintf(stderr, "root2: x: %g %g; f: %g %g\n", xl, xh, fl, fh);
#endif
 if(n == 0)
 	return ddb();
 C double xc = 0.5*(xl+xh);
 C double fc = (*f)(xc, p);
#ifdef DEBUG
 fprintf(stderr, "\troot2: xc: %g; fc: %g\n", xc, fc);
#endif
 if((fc>0.0) == (fl<=0.0))
 	return ddb(xl, xc, fl, fc);
 else if((fc>0.0) == (fh<=0.0))
 	return ddb(xc, xh, fc, fh);
 return root2(f, xl, xc, p, fl, fc, n-1)
 			| root2(f, xc, xh, p, fc, fh, n-1);
}

double
rootO(root_function f, double xl, double xh,
			C void *p, double epsx, double epsf, int depth)
{
 if(xh < xl)	return quiet_nan(0);

 double xtl, xth, lxtl, lxth, lftl, lfth, ftl, fth;
 xtl = xth = 0.5*(xl+xh);
 ftl = fth = (*f)(xtl, p);
 if(ftl == 0)
	return xtl;

 /* Explore the edges, looking for a change of sign. */
 int i;
 for(i=0;i<depth+1;i++)	{
	lxtl = xtl; lxth = xth;	lftl = ftl;	lfth = fth;
	xtl = 0.5*(xl + lxtl);
 	ftl = (*f)(xtl, p);
 	if(ftl == 0)	return xtl;
	if(((ftl>0)&&(lftl<=0)) || ((ftl<=0)&&(lftl>0)))
		return root1(f, xtl, lxtl, p, ftl, lftl, epsx, epsf);

	xth = 0.5*(xh + lxth);
 	fth = (*f)(xth, p);
 	if(fth == 0)	return xth;
	if(((fth>0)&&(lfth<=0)) || ((fth<=0)&&(lfth>0)))
		return root1(f, lxth, xth, p, lfth, fth, epsx, epsf);
	}

/* Try splitting the middle differently, looking for a change of sign. */
 C ddb region = root2(f, xtl, xth, p, ftl, fth, depth);
 if(region)
	return root1(f, region.low, region.high, p, region.flow,
					region.fhigh, epsx, epsf);
 return quiet_nan(0);		/* outside region. */
}

double
root(root_function f, double xl, double xh,
			C void *p, double epsx, double epsf, int depth)
{
 if(xh < xl)	return quiet_nan(0);
 C double fl = (*f)(xl, p);
 if(fl == 0.0)	return xl;
 C double fh = (*f)(xh, p);
 if(fh == 0.0)	return xh;
 if(xh == xl)	return quiet_nan(0);

 if(epsx > 0.0)	{	/* prevent ridiculously large depths */
	C int td = 1 + (int)(log((xh-xl)/epsx)/log(2.0));
	if(td < depth)	depth = td;
	}

 if((fl>0.0) == (fh>0.0))	{
	C ddb region = root2(f, xl, xh, p, fl, fh, depth);
#ifdef DEBUG
	fprintf(stderr, "root: region from root2 x: %g to %g\n", region.low, region.high);
#endif
	if(region)
		return root1(f, region.low, region.high, p, region.flow,
						region.fhigh, epsx, epsf);
	return quiet_nan(0);	/* outide region */
	}
 return root1(f, xl, xh, p, fl, fh, epsx, epsf);
}

#ifdef HAS_TEMPLATES

Block<double>
roots(double (*f)(double x, C void *p), double xl, double xh, C void * p,
		double epsx, double epsf, int depth)
{
 int n = 0;
 BOOL again = FALSE;
 double rt, xl0 = xl;
 Block<double> r;
 do	{
	rt = root(f, xl, xh, p, epsx, epsf, depth);
	if(again=(rt>=xl && rt<=xh))	{
#		ifdef DEBUG
		cerr << "ROOTS:GOOD:" << rt << endl;
#		endif
		again = r.reserve(n+1);
		r[n++] = rt;
		}
	xl = rt + (epsx>0.0 ? epsx : 0.001*(xh-xl0));
	} while(xl<xh && again);
 r.size(n);
 return r;
}

#endif
