#include <stdcxx.hxx>
#include <assert.h>
#include <numerics.h>
#include <math.h>
#ifdef __SunOS5
# include <sunmath.h>
#endif

extern "C" int finite(double);


#define D	1	/* Debugging on (1), or off (0) */

/* UNTESTED!!!!! */

C double  GOLD = 0.38197;	/* 1-golden ratio */


LOCAL mmze_out parab(double x1, double x2, double x3,
							 double f1, double f2, double f3);
LOCAL mmze_out
min2(double (*f)(double x, C void *p), double xl, double xm, double xh,
			C void *p, double fl, double fm, double fh,
			double epsx, double epsf);

LOCAL mmze_out
min1(double (*f)(double x, C void *p), double xl, double xm, double xh,
			C void *p, double fl, double fm, double fh,
			double epsx, double epsf)
{
 assert(xm>xl && xm<xh);
 assert(fm<=fl && fm<=fh);

 if(xh-xl<epsx || (fabs(fl-fm)<epsf && fabs(fh-fm)<epsf))
 	return parab(xl, xm, xh, fl, fm, fh);

 C double x1 = xh-xm>xm-xl ? GOLD*(xh-xm)+xm : GOLD*(xl-xm)+xm;
 C double f1 = (*f)(x1, p);

 if(D)
	fprintf(stderr, "DBG: min1(golden section) (%g,%g,%g) -> %g@%g\n",
			xl, xm, xh, f1, x1);

 if(f1<fm && x1<xm)
	return min2(f, xl, x1, xm, p, fl, f1, fm, epsx, epsf);
 else if(f1 < fm)
	return min2(f, xm, x1, xh, p, fm, f1, fh, epsx, epsf);
 else if(x1 < xm)
	return min2(f, x1, xm, xh, p, f1, fm, fh, epsx, epsf);
 return min2(f, xl, xm, x1, p, fl, fm, f1, epsx, epsf);
}


C double FRAC2 = 0.05;

LOCAL mmze_out
min2(double (*f)(double x, C void *p), double xl, double xm, double xh,
			C void *p, double fl, double fm, double fh,
			double epsx, double epsf)
{
 assert(xm>xl && xm<xh);
 assert(fm<=fl && fm<=fh);

 if(D)
	fprintf(stderr, "DBG: min2: (%g@%g, %g@%g, %g@%g)\n",
				fl, xl, fm, xm, fh, xh);

 C mmze_out tmp = parab(xl, xm, xh, fl, fm, fh);
 if(xh-xl<epsx || (fabs(fl-fm)<epsf && fabs(fh-fm)<epsf))
 	return tmp;

 if(D)
	fprintf(stderr, "DBG: min2(parab): (%g,%g,%g) -> %g\n", xl, xm, xh, tmp.x);

 double x1 = tmp.x;
 if(x1 == xl || x1 == xh)
	return min1(f, xl, xm, xh, p, fl, fm, fh, epsx, epsf);
 /* No point in evaluating too close to the ends -- there's already
		been an evaluation there. */
 else if(x1<xm && x1-xl<FRAC2*(xm-xl))
	x1 += FRAC2*(xm-xl);
 else if(x1>xm && xh-x1<FRAC2*(xh-xm))
	x1 -= FRAC2*(xh-xm);
 								/* Same with the middle. */
 else if(x1<xm && xm-x1<FRAC2*(xm-xl))
	x1 -= FRAC2*(xm-xl);
 else if(x1>xm && x1-xm<FRAC2*(xh-xm))
	x1 += FRAC2*(xh-xm);

 C double f1 = (*f)(x1, p);

 if(D)
	fprintf(stderr, "DBG: min2: adjust x1 to %g.  F=%g\n", x1, f);

 if(f1<fm && x1<xm)
	return min1(f, xl, x1, xm, p, fl, f1, fm, epsx, epsf);
 else if(f1 < fm)
	return min1(f, xm, x1, xh, p, fm, f1, fh, epsx, epsf);
 else if(x1 < xm)
	return min1(f, x1, xm, xh, p, f1, fm, fh, epsx, epsf);
 return min1(f, xl, xm, x1, p, fl, fm, f1, epsx, epsf);
}


mmze_out
minimize_search(double (*f)(double x, C void *p),
		double xl, double xm, double xh,
		double fl, double fm, double fh,
		C void *p, double epsx, double epsf, int depth)
{
 assert(xl<xm && xm<xh);

 if(D)
	fprintf(stderr, "DBG: ms: (%g@%g, %g@%g, %g@%g)\n",
			fl, xl, fm, xm, fh, xh);

 if(depth > 5)
	return mmze_out(quiet_nan(0), quiet_nan(0));

 if(fm<=fl && fm<=fh)
	return min1(f, xl, xm, xh, p, fl, fm, fh, epsx, epsf);

 C double xm1 = fh<fm ? GOLD*(xm-xh)+xh : GOLD*(xh-xm)+xm;
 C double fm1 = (*f)(xm1, p);
 assert(xm1>xm && xm1<xh);
 C double xm2 = fm<fl ? GOLD*(xl-xm)+xm : GOLD*(xm-xl)+xl;
 C double fm2 = (*f)(xm2, p);
 assert(xm2>xl && xm2<xm);

 if(fm1 < fm2)	{
	mmze_out m1 = minimize_search(f, xm, xm1, xh, fm, fm1, fh,
				p, epsx, epsf, depth+1);

	if(finite(m1.x) && finite(m1.y))
		return m1;

	return minimize_search(f, xl, xm2, xm, fl, fm2, fm,
				p, epsx, epsf, depth+1);
	}
 else	{
	mmze_out m2 = minimize_search(f, xl, xm2, xm, fl, fm2, fm,
				p, epsx, epsf, depth+1);

	if(finite(m2.x) && finite(m2.y))
		return m2;

	return minimize_search(f, xm, xm1, xh, fm, fm1, fh,
				p, epsx, epsf, depth+1);
	}
}



mmze_out
minimize(double (*f)(double x, C void *p), double xl, double xh, C void *p,
			double epsx, double epsf)
{
 if(xl > xh)	{
	double tmp = xl;
	xl = xh;
	xh = tmp;
	}
 C double fl = (*f)(xl, p);
 C double fh = (*f)(xh, p);
 C double xm = fh<fl ? GOLD*(xl-xh)+xh : GOLD*(xh-xl)+xl;
 C double fm = (*f)(xm, p);

 if(D)
	fprintf(stderr, "DBG: minimize: (%g@%g, %g@%g, %g@%g)\n",
			fl, xl, fm, xm, fh, xh);

 return minimize_search(f, xl, xm, xh, fl, fm, fh, p, epsx, epsf, 0);
}






LOCAL mmze_out
parab(double a, double b, double c, double fa, double fb, double fc)
{
 C double xmin = b + 0.5 * (xx(b-a)*(fb-fc) - xx(b-c)*(fb-fa))
							/ ((b-a)*(fb-fc) - (b-c)*(fb-fa));
 C double curv = ((fb-fa)*(c-a) - (fc-fa)*(b-a))
				/	(xx(b-a)*(c-a) + xx(c-a)*(b-a));

 if(!finite(xmin) || !(curv > 0))	{
	if(!isnan(fa) && !(fa>fb) && !(fa>fc))
		return mmze_out(a, fa);
	else if(!isnan(fb) && !(fb>fa) && !(fb>fc))
		return mmze_out(b, fb);
	else if(!isnan(fc) && !(fc>fa) && !(fc>fb))
		return mmze_out(b, fb);
	else
		return mmze_out(quiet_nan(0), quiet_nan(0));
	}

 if(xmin < a)
	return mmze_out(a, fa);
 if(xmin > c)
	return mmze_out(c, fc);
 
 C double slope = ((fb-fa)*xx(c-a) - (fc-fa)*xx(b-a))
				/ ((b-a)*xx(c-a) - (c-a)*xx(b-a));

 return mmze_out(xmin,	curv*xx(xmin-a) + slope*(xmin-a) + fa);
}
