/* Minimizers.c */
/* 
 David Weenink, 20011016 

 djmw 20011016 removed some causes for compiler warnings
 djmw 20030205 Latest modification
*/

#include "NUM2.h"
#include "Graphics.h"
#include "Minimizers.h"

#define SIGN(x,s) ((s) < 0 ? -fabs (x) : fabs(x))
#define GOLD  1.618034
#define CGOLD 0.3819660
#define ITMAX 100.0
#define TINY 1.0e-20
#define ZEPS 1.0e-10
#define EPS 1.0e-10
#define TOL 2e-4
#define SHFT(a, b, c, d)	(a) = (b); (b) = (c); (c) = (d);
#define MOV3(a, b, c, d, e, f)	(a) = (d); (b) = (e); (c) = (f);
#define FUNC1(fx, x) for (i=1; i <= my nParameters; i++) my ptry[i] = p[i] + (x) * direction[i]; \
	(fx) = my func (my object, my ptry);
#define DFUNC1(df, x) for (i=1; i <= my nParameters; i++) my ptry[i] = p[i] + (x) * direction[i]; \
	my dfunc (my object, my ptry, my dp); for (df=0, i=1; i <= my nParameters; i++) df += my dp[i] * direction[i];

static int classMinimizer_after (I, Any aclosure)
{
    iam (Minimizer);
	(void) aclosure;
	
    if (my success || ! my gmonitor) return 1;
	
    if (my start == 1)
    {
    	char s[35];
    	Minimizer_drawHistory (me, my gmonitor, 0, my maxNumOfIterations, 0, 
			1.1 * my history[1], 1);
    	sprintf (s, "Dimension of search space: %6ld", my nParameters); 
    	Graphics_textTop (my gmonitor, 0, s);
    } 
    Graphics_setInner (my gmonitor);
    Graphics_line (my gmonitor, my iteration, my history[my iteration],
    	my iteration, my history[my iteration]);
    Graphics_unsetInner (my gmonitor);
	if (! Melder_monitor ((double) (my iteration) / my maxNumOfIterations,
		"Iterations: %ld, Function calls: %ld, Cost: %f", my iteration,
		my funcCalls, my minimum))
	{
		Melder_casual ("Minimization interrupted after %ld iterations.", 
			my iteration);
		return 0;
	}
    return 1;
}

static void classMinimizer_info (I) {(void) void_me;}

static void classMinimizer_destroy (I)
{
    iam (Minimizer);
    NUMdvector_free (my p, 1);
    NUMdvector_free (my history, 1);
    inherited (Minimizer) destroy (me);
}

static void classMinimizer_reset (I) {(void) void_me;}

static int classMinimizer_minimize (I) { (void) void_me; return 0; }

static void classMinimizer_setParameters (I, Any parameters)
{
	(void) void_me; (void) parameters;
}

class_methods (Minimizer, Thing)
   class_method_local (Minimizer, destroy)
   class_method_local (Minimizer, info)
   class_method_local (Minimizer, minimize)
   class_method_local (Minimizer, reset)
   class_method_local (Minimizer, setParameters)
class_methods_end

int Minimizer_init (I, long nParameters, Any object)
{
    iam (Minimizer);
	
    my nParameters = nParameters;
    if ((my p = NUMdvector (1, nParameters)) == NULL) return 0;
    my object = object;
    my minimum = 1.0e30;
    my after = classMinimizer_after;
    Minimizer_reset (me, NULL); /* added 27/11/97 */
    return 1;
}

void Minimizer_setParameters (I, Any parameters)
{
	iam (Minimizer);
	if (our setParameters) our setParameters (me, parameters);
}

int Minimizer_minimize (I, long maxNumOfIterations, double tolerance, 
	int monitor)
{
    iam (Minimizer); 
	int status;
	
    my tolerance = tolerance;
    if (maxNumOfIterations <= 0) return 1;
	
    if (my iteration + maxNumOfIterations > my maxNumOfIterations)
    {
		double *history;
		my maxNumOfIterations += maxNumOfIterations;
		if (my history) my history++; /* arrays start at 1 !! */
		history = (double *) Melder_realloc (my history, my maxNumOfIterations *
	    	sizeof(double));
		if (history == NULL) return 0;
		my history = --history; /* arrays start at 1 !! */
    }
    if (monitor) my gmonitor = Melder_monitor (0.0, "Starting...");
    my start = 1; /* for my after() */
    status = our minimize (me);
    if (monitor)
    {
    	(void) Melder_monitor (1.1, NULL);
    	if (my gmonitor)
    	{
    		Graphics_clearWs (my gmonitor); /* DON'T forget (my gmonitor) */
    		my gmonitor = NULL;
    	}
    }
    if (my success) Melder_casual("Minimizer_minimize: minimum %f reached \n"
		"after %ld iterations and %ld function calls.", my minimum, 
		my iteration, my funcCalls);
    return status;
}

int Minimizer_minimizeManyTimes (I, long numberOfTimes, long 
	maxIterationsPerTime, double tolerance)
{
	iam (Minimizer); 
	long i; 
	double *popt, fopt = my minimum;
	int monitorSingle = numberOfTimes == 1;
	
	popt = NUMdvector_copy (my p, 1, my nParameters);
	if (popt == NULL) return 0;
	
	if (! monitorSingle) Melder_progress (0.0, "Minimize many times");
	/* on first iteration start with current parameters 27/11/97 */
	for (i = 1; i <= numberOfTimes; i++)
	{
		if (! Minimizer_minimize (me, maxIterationsPerTime, tolerance, 
			monitorSingle)) break;
		Melder_casual("Current %ld: minimum = %.17g", i, my minimum);
		if (my minimum < fopt)
		{
			NUMdvector_copyElements (my p, popt, 1, my nParameters);
			fopt = my minimum;
		}
		Minimizer_reset (me, NULL); 
		if (! monitorSingle && ! Melder_progress ((double)i / numberOfTimes,
			" %ld from %ld", i, numberOfTimes)) break;
	}
	if (! monitorSingle) Melder_progress (1.0, NULL);
	Minimizer_reset (me, popt);
	NUMdvector_free (popt, 1);
	return 1;
}

void Minimizer_setAfterEachIteration (I, int (*after) (I, Any aclosure), 
	Any aclosure)
{
    iam(Minimizer);
    my after = after;
    my aclosure = aclosure;
}

void Minimizer_reset (I, const double guess[])
{
    iam (Minimizer); 
    long i;
	
    if (guess)
    {
    	for (i = 1; i <= my nParameters; i++)
		{
			my p[i] = guess[i];
		}
    }
    else
    {
    	for (i = 1; i <= my nParameters; i++)
		{
			my p[i] = NUMrandomUniform (-1, 1);
		}
    }
	/*
		Don't use NUMdvector_free: realloc in Minimizer_minimize
	*/
    if (my history != NULL)
    {
    	my history++; 
		Melder_free (my history);
    }
    my maxNumOfIterations = my success = my funcCalls = my iteration = 0;
    my minimum = 1.0e38;
    our reset (me);
}

void Minimizer_drawHistory (I, Any graphics, long iFrom, long iTo, double hmin, 
    double hmax, int garnish)
{
    iam (Minimizer); 
	float *history;
    long i, itmin, itmax;
	
    if (my history == NULL ||
		(history = NUMfvector (1, my iteration)) == NULL) return;
    for (i = 1; i <= my iteration; i++)
	{
		history[i] = my history[i];
	}
    if (iTo <= iFrom)
	{
		iFrom = 1; iTo = my iteration;
	}
    itmin = iFrom; itmax = iTo;
    if (itmin < 1) itmin = 1; 
    if (itmax > my iteration) itmax = my iteration;
    if (hmax <= hmin)
	{
		NUMfvector_extrema (history, itmin, itmax, & hmin, & hmax);
	}
    if (hmax <= hmin)
	{
		hmin -= 0.5 * fabs (hmin);
		hmax += 0.5 * fabs (hmax);
	}
    Graphics_setInner (graphics);
    Graphics_setWindow (graphics, iFrom, iTo, hmin, hmax);
    Graphics_function (graphics, history, itmin, itmax, itmin, itmax);
    NUMfvector_free (history, 1);
    Graphics_unsetInner (graphics);
    if (garnish)
    {
    	Graphics_drawInnerBox (graphics);    
   		Graphics_textBottom (graphics, 1, "Number of iterations");
		Graphics_marksBottom (graphics, 2, 1, 1, 0);
    	Graphics_marksLeft (graphics, 2, 1, 1, 0);
    }
}

/************ class LineMinimizer *******************************/
	
static void NUMmnbrakm (I, double *ax, double *bx, double *cx, double *fa,
	double *fb, double *fc, double p[], double direction[])
{
	iam (LineMinimizer); 
	double ulim, u, r, q, fu, dum; 
	long i;

	FUNC1 (*fa, *ax)
	FUNC1 (*fb, *bx)
	if (*fb > *fa) /* switch roles of a and b */
	{
		dum = *ax; *ax = *bx; *bx = dum;
		dum = *fb; *fb = *fa; *fa = dum;
	}
	*cx = *bx + GOLD * (*bx - *ax);
	FUNC1 (*fc, *cx)
	while (*fb > *fc)
	{
		r = (*bx - *ax) * (*fb - *fc);
		q = (*bx - *cx) * (*fb - *fa);
		u = (*bx) - ((*bx - *cx) * q - (*bx - *ax) * r) /
			(2.0 * SIGN ((dum = fabs(q - r)) > TINY ? dum : TINY, q - r));
		ulim = *bx + my maxLineStep * (*cx - *bx);
		if ((*bx - u) * (u - *cx) > 0.0)
		{
			FUNC1 (fu, u)
			if (fu < *fc)
			{
				*ax = *bx; *bx = u; *fa = *fb; *fb = fu; return;
			}
			else if (fu > *fb)
			{
				*cx = u; *fc = fu; return;
			}
			u = *cx + GOLD * (*cx - *bx);
			FUNC1 (fu, u)
		}
		else if ((*cx - u) * (u - ulim) > 0.0)
		{
			FUNC1 (fu, u)
			if (fu < *fc)
			{
				SHFT (*bx, *cx, u, *cx + GOLD * (*cx - *bx))
				FUNC1 (dum, u)
				SHFT (*fb, *fc, fu, dum)
			}
		}
		else if ((u - ulim) * (ulim - *cx) >= 0.0)
		{
			u = ulim;
			FUNC1 (fu, u)
		}
		else
		{
			u = *cx + GOLD * (*cx - *bx);
			FUNC1 (fu, u)
		}
		SHFT (*ax, *bx, *cx, u)
		SHFT (*fa, *fb, *fc, fu)
	}
}

static double NUMbrentm (I, double ax, double bx, double cx, double tol,
	double *xmin, double p[], double direction[])
{
	iam (LineMinimizer); 
	long iter, i;
	double a, b, d = 0, etemp, fu, fv, fw, fx, pp, q, r, tol1, tol2;
	double u, v, w, x, xm, e = 0.0;
	
	a = (ax < cx ? ax : cx);
	b = (ax > cx ? ax : cx);
	x = w = v = bx;
	FUNC1 (fx, x)
	fw = fv = fx;
	for (iter = 1; iter <= ITMAX; iter++)
	{
		xm = 0.5 * (a + b);
		tol2 = 2.0 * (tol1 = tol * fabs (x) + ZEPS);
		if (fabs(x - xm) <= (tol2 - 0.5 * (b - a)))
		{
			*xmin = x; return fx;
		}
		if (fabs(e) > tol1)
		{
			r = (x - w) * (fx - fv);
			q = (x - v) * (fx - fw);
			pp = (x - v) * q - (x - w) * r;
			q = 2.0 * (q - r);
			if (q > 0.0) pp = -pp;
			q = fabs (q);
			etemp = e;
			e = d;
			if (fabs (pp) >= fabs (0.5 * q * etemp) || pp <= q * (a - x) ||
				pp >= q * (b - x))
			{
				e = x >= xm ? a - x : b - x;
				d = CGOLD * e;
			}
			else
			{
				d = pp / q;
				u = x + d;
				if (u - a < tol2 || b - u < tol2) d = SIGN (tol1, xm - x);
			}
		}
		else
		{
			e = x >= xm ? a - x : b - x;
			d = CGOLD * e;
		}
		u = (fabs (d) >= tol1 ? x + d : x + SIGN (tol1, d));
		FUNC1 (fu, u)
		if (fu <= fx)
		{
			if (u >= x) a = x;
			else b = x;
			SHFT (v, w, x, u)
			SHFT (fv, fw, fx, fu)
		}
		else
		{
			if (u < x) a = u; 
			else b = u;
			if (fu <= fw || w == x)
			{
				v = w; 
				w = u; 
				fv = fw; 
				fw = fu;
			}
			else if (fu <= fv || v == x || v == w) 
			{
				v = u;
				fv = fu;
			}
		}
	}
	(void) Melder_error ("NUMbrentm: too many iterations.");
	*xmin = x;
	return fx;
}

static void classLineMinimizer_linmin (I, double p[], double fp, 
	double direction[], double *fret)
{
	iam (LineMinimizer); 
	long i; 
	double xx = 1, xmin, fx, fb, fa, bx, ax = 0;
	(void) fp;
	
	NUMmnbrakm (me, &ax, &xx, &bx, &fa, &fx, &fb, p, direction);
	*fret = NUMbrentm (me, ax, xx, bx, TOL, &xmin, p, direction);
	for (i = 1; i <= my nParameters; i++)
	{
		direction[i] *= xmin;
		p[i] += direction[i];
	}
}

static void classLineMinimizer_destroy (I)
{
    iam (LineMinimizer);
    NUMdvector_free (my ptry, 1);
    NUMdvector_free (my direction, 1);
    inherited (LineMinimizer) destroy (me);
}

class_methods (LineMinimizer, Minimizer)
   class_method_local (LineMinimizer, destroy)
   class_method_local (LineMinimizer, linmin)
class_methods_end

int LineMinimizer_init (I, long nParameters, Any object, 
	double (*func)(Any, const double []))
{
    iam (LineMinimizer);
	
    if (! Minimizer_init (me, nParameters, object) ||
    	((my direction = NUMdvector (1, nParameters)) == NULL) ||
    	((my ptry = NUMdvector (1, nParameters)) == NULL)) return 0;
    my func = func;
    my maxLineStep = 100;
    return 1;
}

/**************** class PowellMinimizer *********************************/

static int classPowellMinimizer_minimize (I)
{
	iam (PowellMinimizer); 
	long i, ibig, j; 
	int status = 0;
	double del, fp, fptt, t, *pt = NULL, *ptt = NULL;
	
	if (! (pt  = NUMdvector (1, my nParameters)) ||
		! (ptt = NUMdvector (1, my nParameters))) goto end;
	my minimum = my func (my object, my p);
	for (j = 1; j <= my nParameters; j++) 
	{
		pt[j] = my p[j];
	}
	while (my iteration < my maxNumOfIterations)
	{
		fp = my minimum; 
		ibig = 0;
		del = 0; /* Will be the biggest function decrease. */
		 
		/*
			In each iteration, loop over all directions in the set.
		*/
		for (i = 1; i <= my nParameters; i++)
		{
			/* Copy the direction, */
			for (j = 1; j <= my nParameters; j++)
			{
				my direction[j] = my directions[j][i];
			}
			fptt = my minimum;
			our linmin (me, my p, fp, my direction, & my minimum); 
			/* 
				Minimize along it, and record if it is the largest decrease 
				so far. 
			*/
			if (fabs(fptt - my minimum) > del)
			{
				del = fabs(fptt - my minimum); 
				ibig = i;
			}
		}
		my history[++my iteration] = my minimum;
		
		/*
			Termination criterion.
		*/
		
		my success = 2.0 * fabs(fp - my minimum) <= my tolerance * 
			(fabs (fp) + fabs (my minimum));
		if ((my after && ! my after (me, my aclosure)) || my success) break;
		
		/*
			Construct the extrapolated point and the average direction moved.
			Save the old starting point
		*/
		
		for (j = 1; j <= my nParameters; j++)
		{
			ptt[j] = 2 * my p[j] - pt[j];
			my direction[j] = my p[j] - pt[j];
			pt[j] = my p[j];
		}
		
		/*
			Function value at extrapolated point
		*/
		
		fptt = my func (my object, ptt);
		if (fptt < fp)
		{
			double t1 = fp - my minimum - del, t2 = fp - fptt;
			t = 2 * (fp - 2 * my minimum + fptt) * t1 * t1 - del * t2 * t2;
			if (t < 0)
			{
				/*
					Move to the minimum of the new direction,
					and save the new direction.
				*/
				
				our linmin (me, my p, fp, my direction, &my minimum);
				for (j = 1; j <= my nParameters; j++)
				{
					my directions[j][ibig] = my directions[j][my nParameters] 
						= my direction[j];
				}
			}
		}
	}
	status = 1;
end:
	NUMdvector_free (ptt, 1);
	NUMdvector_free (pt , 1);
	return status; 
}

static void classPowellMinimizer_destroy (I)
{
    iam (PowellMinimizer);
    NUMdmatrix_free (my directions, 1, 1);
    inherited (PowellMinimizer) destroy (me);
}

static void classPowellMinimizer_reset (I)
{
    iam (PowellMinimizer); long i;
	for (i=1; i <= my nParameters; i++) my directions[i][i] = 1;
    inherited (LineMinimizer) reset (me);
}

class_methods (PowellMinimizer, LineMinimizer)
   class_method_local (PowellMinimizer, destroy)
   class_method_local (PowellMinimizer, reset)
   class_method_local (PowellMinimizer, minimize)
class_methods_end

Any PowellMinimizer_create (long nParameters, Any object, 
	double (*func)(Any, const double []))
{
	PowellMinimizer me = new (PowellMinimizer); 
	long i;
	
	if (me == NULL) return NULL;
	
	if (LineMinimizer_init (me, nParameters, object, func))
	{
		my directions = NUMdmatrix (1, my nParameters, 1, nParameters);
		if (my directions != NULL)
		{
			for (i=1; i <= my nParameters; i++)
			{
				my directions[i][i] = 1;
			}
			return me;
		}
	
	}
	forget (me);
	return NULL;
}

/****************  class PolakRibiereMinimizer *************************/

static double NUMdbrentm (I, double ax, double bx, double cx, double tol,
	double *xmin, double p[], double direction[])
{
	iam (PolakRibiereMinimizer); 
	long i, iter, ok1, ok2;
	double a, b, d, d1, d2, du, dv, dw, dx, e = 0.0;
	double fu, fv, fw, fx, olde, tol1, tol2, u, u1, u2, v, w, x, xm;

	a = (ax < cx ? ax : cx);
	b = (ax > cx ? ax : cx);
	x = w = v = bx;
	FUNC1 (fx, x)
	fw = fv = fx;
	DFUNC1 (dx, x)
	dw = dv = dx;
	for (iter = 1; iter <= ITMAX; iter++)
	{
		xm = 0.5 * (a + b);
		tol1 = tol * fabs (x) + ZEPS;
		tol2 = 2.0 * tol1;
		if (fabs(x - xm) <= (tol2 - 0.5 * (b - a)))
		{
			*xmin = x; return fx;
		}
		if (fabs (e) > tol1)
		{
			d1 = 2.0 * (b - a); d2 = d1;
			if (dw != dx) d1 = (w - x) * dx / (dx - dw);
			if (dv != dx) d2=(v-x)*dx/(dx-dv);
			u1 = x + d1; u2 = x + d2;
			ok1 = (a - u1) * (u1 - b) > 0.0 && dx * d1 <= 0.0;
			ok2 = (a - u2) * (u2 - b) > 0.0 && dx * d2 <= 0.0;
			olde = e; e = d;
			if (ok1 || ok2)
			{
				if (ok1 && ok2) d = (fabs (d1) < fabs (d2) ? d1 : d2);
				else if (ok1) d = d1;
				else d = d2;
				if (fabs (d) <= fabs (0.5 * olde))
				{
					u = x + d;
					if (u - a < tol2 || b - u < tol2) d = SIGN (tol1, xm - x);
				}
				else { d = 0.5 * (e = (dx >= 0.0 ? a - x : b - x)); }
			}
			else { d = 0.5 * (e = (dx >= 0.0 ? a - x : b - x)); }
		}
		else { d = 0.5 * (e = (dx >= 0.0 ? a - x : b - x)); }
		if (fabs (d) >= tol1)
		{
			u = x + d; FUNC1 (fu, u)
		}
		else
		{
			u = x + SIGN (tol1, d); FUNC1 (fu, u)
			if (fu > fx) { *xmin = x; return fx; }
		}
		DFUNC1 (du, u)
		if (fu <= fx)
		{
			if (u >= x) a = x; else b = x;
			MOV3 (v, fv, dv, w, fw, dw)
			MOV3 (w, fw, dw, x, fx, dx)
			MOV3 (x, fx, dx, u, fu, du)
		}
		else
		{
			if (u < x) a = u; else b = u;
			if (fu <= fw || w == x)
			{
				MOV3 (v, fv, dv, w, fw, dw)
				MOV3 (w, fw, dw, u, fu, du)
			}
			else if (fu < fv || v == x || v == w)
			{
				MOV3 (v, fv, dv, u, fu, du)
			}
		}
	}
	(void) Melder_error ("NUMdbrentm: too many iterations.");
	return 1e30; /* never get here */
}

static void classPolakRibiereMinimizer_linmin (I, double p[], double fp, 
	double direction[], double *fret)
{
	iam (PolakRibiereMinimizer); 
	double xx = 1, xmin, fx, fb, fa, bx, ax = 0;
	long i;
	(void) fp;
	
	NUMmnbrakm (me, &ax, &xx, &bx, &fa, &fx, &fb, p, direction);
	*fret = NUMdbrentm (me, ax, xx, bx, TOL, & xmin, p, direction);
	
	for (i = 1; i <= my nParameters; i++)
	{
		direction[i] *= xmin;
		p[i] += direction[i];
	}
}

static void classPolakRibiereMinimizer_destroy (I)
{
    iam (PolakRibiereMinimizer);
    NUMdvector_free (my dp, 1);
    inherited (PolakRibiereMinimizer) destroy (me);
}

static int classPolakRibiereMinimizer_minimize (I)
{
	iam (PolakRibiereMinimizer); 
	int status = 1;
	long j; 
	double gg, gam, fp, dgg, *g = NULL, *h = NULL, *xi = NULL;

	if (! (g  = NUMdvector (1, my nParameters)) ||
		! (h  = NUMdvector (1, my nParameters)) ||
		! (xi = NUMdvector (1, my nParameters))) { status = 0; goto end; }
	fp = my func (my object, my p);
	my dfunc (my object, my p, xi);
	for (j=1; j <= my nParameters; j++)
	{	/* Initial minimization direction is -gradient (down-hill) */
		g[j] = -xi[j]; xi[j] = h[j] = g[j];
	}
	while (my iteration < my maxNumOfIterations)
	{
		our linmin (me, my p, fp, xi, &my minimum);
		my success = 2.0 * fabs(my minimum - fp) <= my tolerance * 
			(fabs (my minimum) + fabs (fp) + EPS);
		my history[++my iteration] = my minimum;
		if ((my after && ! my after (me, my aclosure)) || my success) goto end;
		fp = my func (my object, my p);
		my dfunc (my object, my p, xi);
		dgg = gg = 0.0;
		for (j = 1; j <= my nParameters; j++)
		{
			gg += g[j] * g[j];
			dgg += (xi[j] + g[j]) * xi[j];
		}
		if (gg == 0.0)
		{
			my success = 1;
			goto end;
		}
		gam = dgg / gg;
		for (j = 1; j <= my nParameters; j++)
		{
			g[j] = -xi[j];
			xi[j] = h[j] = g[j] + gam * h[j];
		}
	}
end:
	NUMdvector_free (xi, 1);
	NUMdvector_free ( h, 1);
	NUMdvector_free ( g, 1);
	return status;
}

class_methods (PolakRibiereMinimizer, LineMinimizer)
   class_method_local (PolakRibiereMinimizer, minimize)
   class_method_local (PolakRibiereMinimizer, linmin)
   class_method_local (PolakRibiereMinimizer, destroy)
class_methods_end

Any PolakRibiereMinimizer_create (long nParameters, Any object, 
	double (*func) (Any object, const double p[]),
	void (*dfunc) (Any object, const double p[], double dp[]))
{
	PolakRibiereMinimizer me = new (PolakRibiereMinimizer);
	if (! me || ! LineMinimizer_init (me, nParameters, object, func) ||
		! (my dp = NUMdvector (1, nParameters))) { forget (me); return me; }
	my dfunc = dfunc;
	return me;
}

/**************  class SteepestDescentMinimizer **********************/

static int classSteepestDescentMinimizer_minimize (I)
{
	iam (SteepestDescentMinimizer);
	long i; 
	int status = 1;
	double fret, *dp = NULL, *dpp = NULL;
	
	if (((dp  = NUMdvector (1, my nParameters)) == NULL) ||
		((dpp = NUMdvector (1, my nParameters)) == NULL))
	{
		status = 0; 
		goto end;
	}
	
	fret = my func (my object, my p);
	while (my iteration < my maxNumOfIterations)
	{
		my dfunc (my object, my p, dp);
		for (i = 1; i <= my nParameters; i++)
		{
			dpp[i] = - my eta * dp[i] + my momentum * dpp[i];
			my p[i] += dpp[i];
		}
		my history[++my iteration] = my minimum = my func (my object, my p);
		my success = 2.0 * fabs (fret - my minimum) < my tolerance * 
			(fabs (fret) + fabs (my minimum));
		if ((my after && ! my after (me, my aclosure)) || my success)  break;
		fret = my minimum;
	}
end:
	NUMdvector_free (dpp, 1);
	NUMdvector_free ( dp, 1);
	return status;
}

static void classSteepestDescentMinimizer_setParameters (I, Any p)
{
	iam (SteepestDescentMinimizer);
	if (p)
	{
		structSteepestDescentMinimizer_parameters thee = p;
		my eta = thy eta;
		my momentum = thy momentum;
	}
	else
	{
		my eta = 0.1; my momentum = 0.9;
	}
}

class_methods (SteepestDescentMinimizer, Minimizer)
   class_method_local (SteepestDescentMinimizer, minimize)
   class_method_local (SteepestDescentMinimizer, setParameters)
class_methods_end

Any SteepestDescentMinimizer_create (long nParameters, Any object, 
	double (*func) (Any object, const double p[]),
	 void (*dfunc) (Any object, const double p[], double dp[]))
{
	SteepestDescentMinimizer me = new (SteepestDescentMinimizer);
	if (me == NULL) return NULL;
	if (Minimizer_init (me, nParameters, object))
	{
		my func = func;
		my dfunc = dfunc;
		return me;
	}
	forget (me); 
	return me;
}	

/****************  class LevenbergMarquardtMinimizer *******************/

static int classLevenbergMarquardtMinimizer_minimize (I)
{
    iam ( LevenbergMarquardtMinimizer); 
	int status = 0; 
	long i, j, ntimes = 0;
    double *da = NULL, *dp = NULL, *ptry = NULL, **alpha = NULL, **oneda = NULL;
    double **ddp = NULL, minCurrent, minPrevious, lambda = 0.001;
      
	if (! (da    = NUMdvector (1, my nParameters)) ||
		! (dp    = NUMdvector (1, my nParameters)) ||
		! (ptry  = NUMdvector (1, my nParameters)) ||
		! (alpha = NUMdmatrix (1, my nParameters, 1, my nParameters)) ||
		! (ddp   = NUMdmatrix (1, my nParameters, 1, my nParameters)) ||
		! (oneda = NUMdmatrix (1, my nParameters, 1, 1))) goto end;
	minCurrent = my hfunc (my object, my p, dp, alpha);
	minPrevious = minCurrent;
	for (i=1; i <= my nParameters; i++) ptry[i] = my p[i];
	while (my iteration < my maxNumOfIterations)
	{
		for (i = 1; i <= my nParameters; i++)
		{
			for (j = 1; j <= my nParameters; j++) ddp[i][j] = alpha[i][j];
			ddp[i][i] *= 1.0 + lambda;
			oneda[i][1] = dp[i];
		}
		if (! NUMgaussj_d (ddp, my nParameters, oneda, 1)) goto end;
		for (i = 1; i <= my nParameters; i++) 
		{
			da[i] = oneda[i][1]; ptry[i] = my p[i] + da[i];
		}
		minCurrent = my hfunc (my object, ptry, da, ddp);
		if (minCurrent < minPrevious)
		{
			lambda *= 0.1;
			if (fabs (minCurrent - minPrevious) < my tolerance) ntimes++;
			minPrevious = minCurrent;
			for (i=1; i <= my nParameters; i++)
			{
				for (j=1; j <= my nParameters; j++)  alpha[i][j] = ddp[i][j];
				dp[i] = da[i];
				my p[i] = ptry[i];
			}
		}
		else
		{
			lambda *= 10.0; ntimes = 0;
			minCurrent = minPrevious;
		}
		my history[++my iteration] = my minimum = minCurrent;
		my success = ntimes > 2;
		if ((my after && ! my after (me, my aclosure)) || my success)  break;
	}
	status = 1;
end:
	NUMdmatrix_free (oneda, 1, 1);
	NUMdmatrix_free (ddp, 1, 1);
	NUMdmatrix_free (alpha, 1, 1);
	NUMdvector_free (ptry, 1);
	NUMdvector_free (dp, 1);
	NUMdvector_free (da, 1);
	return status;
}

class_methods (LevenbergMarquardtMinimizer, Minimizer)
   class_method_local (LevenbergMarquardtMinimizer, minimize)
class_methods_end

Any LevenbergMarquardtMinimizer_create (long nParameters, Any object, 
	double (*hfunc) (Any object, const double p[], double dp[], double **ddp))
{
	LevenbergMarquardtMinimizer me = new (LevenbergMarquardtMinimizer);
	if (! me || ! Minimizer_init (me, nParameters, object)) forget (me);
	else my hfunc = hfunc;
	return me;
}

/****************  class SimplexMinimizer ******************************/

static void classSimplexMinimizer_destroy (I)
{
    iam (SimplexMinimizer);
    NUMdmatrix_free (my simplex, 1, 1);
    NUMdvector_free (my y, 1);
    inherited (SimplexMinimizer) destroy (me);
}

static void classSimplexMinimizer_reset (I)
{
    iam (SimplexMinimizer);
    inherited (SimplexMinimizer) reset (me);
    SimplexMinimizer_initSimplex (me);
}

static double amotry (I, double psum[], double ptry[], long ihi, double fac)
{
    iam (SimplexMinimizer);
    double fac1, fac2, ytry; 
	long j;
    
    fac1 = ( 1.0 - fac) / my nParameters;
    fac2 = fac1 - fac;
    for (j = 1; j <= my nParameters; j++)
	{
		ptry[j] = psum[j] * fac1 - my simplex[ihi][j] * fac2;
	}
    ytry = my func (my object, ptry);
    if (ytry <= my y[ihi])
    {
    	my y[ihi] = ytry;
		for (j = 1; j <= my nParameters; j++) 
		{
			psum[j] += ptry[j] - my simplex[ihi][j];
			my simplex[ihi][j] = ptry[j];
		}
    }
    return ytry;
}

static int classSimplexMinimizer_minimize (I)
{
    iam (SimplexMinimizer); 
	int status = 1;
    double sum, ysave, ytry, *psum = NULL, *ptry = NULL;
    long i, ihi, ilo, inhi, j, mpts = my nParameters + 1;
    
    if (! (psum = NUMdvector (1, my nParameters)) ||
    	! (ptry = NUMdvector (1, my nParameters)))
	{
		status = 0; goto end;
	}
    for (j = 1; j <= my nParameters; j++)
    {
		for (sum = 0, i = 1; i <= mpts; i++) sum += my simplex[i][j];
		psum[j] = sum;
    }
	while (my iteration < my maxNumOfIterations)
	{
    	ilo = 1;
    	ihi = my y[1] > my y[2] ? ( inhi = 2, 1) : ( inhi = 1, 2);
    	for (i = 1; i <= mpts; i++)
    	{
			if (my y[i] <= my y[ilo]) ilo = i;
			if (my y[i] > my y[ihi]) { inhi = ihi; ihi = i; }
			else if (my y[i] > my y[inhi] && i != ihi) inhi = i;
    	}
    	my history[++my iteration] = my minimum = my y[ilo];
    	my success = 2.0 * fabs (my y[ihi] - my y[ilo]) /
    		 (fabs (my y[ihi]) + fabs (my y[ilo])) < my tolerance;
    	if ((my after && ! my after (me, my aclosure)) || my success)
    	{
			double swap = my y[1]; 
			my minimum = my y[1] = my y[ilo]; 
			my y[ilo] = swap;
			
			for (i = 1; i <= my nParameters; i++)
			{
				swap = my simplex[1][i];
				my p[i] = my simplex[1][i] = my simplex[ilo][i];
				my simplex[ilo][i] = swap; 
			}
			goto end;
    	}
		
    	/*
			Reflect the simplex from the high point
		*/
		
    	ytry = amotry (me, psum, ptry, ihi, -1.0);
    	if (ytry <= my y[ilo])
		{
			/*
				Gives a better result, so try additional extrapolation
			*/
			
			ytry = amotry (me, psum, ptry, ihi, 2.0);
		}
    	else if (ytry >= my y[inhi])
    	{
			/*
				Reflected point is worst than second-highest:
				Do one-dimensional contraction
			*/
			
			ysave = my y[ihi];
			ytry = amotry (me, psum, ptry, ihi, 0.5);
			if (ytry >= ysave)
			{
	    		/*
					Didn't help. Contract around the best point
				*/
				
	    		for (i=1; i <= mpts; i++)
	    		{
					if (i != ilo)
					{
		    			for (j=1; j <= my nParameters; j++)
		    			{
							my simplex[i][j] = psum[j] = 0.5 * 
								( my simplex[i][j] + my simplex[ilo][j]);
		    			}
		    			my y[i] = my func (my object, psum);
					}
	    		}
	    		for (j = 1; j <= my nParameters; j++)
    			{
					for (sum = 0, i = 1; i <= mpts; i++)
					{
						sum += my simplex[i][j];
					}
					psum[j] = sum;
   				}
			}
    	}
    }
end:
    NUMdvector_free (ptry, 1);
    NUMdvector_free (psum, 1);
    return status;
}

class_methods (SimplexMinimizer, Minimizer)
   class_method_local (SimplexMinimizer, destroy)
   class_method_local (SimplexMinimizer, reset)
   class_method_local (SimplexMinimizer, minimize)
class_methods_end

int SimplexMinimizer_init (I, long nParameters, Any object, 
	double (*func) (Any object, const double p[]), double scale)
{
	iam (SimplexMinimizer);
	if(	! Minimizer_init (me, nParameters, object) ||
		! (my y = NUMdvector (1, nParameters+1)) ||
		! (my simplex = NUMdmatrix (1, nParameters+1, 1, nParameters)))
			return 0;
	my scale = scale; my func = func;
	return 1;
}

Any SimplexMinimizer_create (long nParameters, Any object, 
	double (*func) (Any object, const double p[]), double scale)
{
	SimplexMinimizer me = new (SimplexMinimizer);
	if (me == NULL || 
		! SimplexMinimizer_init (me, nParameters, object, func, scale)) 
			forget (me);
	return me;
}

void SimplexMinimizer_initSimplex (I)
{
    iam (SimplexMinimizer); 
	long i, j;
	
	for (i = 1; i <= my nParameters+1; i++)
	{
		for (j=1; j <= my nParameters; j++) my simplex[i][j] = 0;
	}
	for (i = 2; i <= my nParameters+1; i++) my simplex[i][i-1] = my scale;
	for (i = 1; i <= my nParameters+1; i++)
	{
		/*
			P[i] = P[0] + my scale * e[i]
		*/
		for (j = 1; j <= my nParameters; j++) my simplex[i][j] += my p[j];
		my y[i] = my func (my object, my simplex[i]);
	}
}


/******************  class SimulatedAnnealingMinimizer ******************/

/*
	Extrapolates by a factor 'fac' through the face of the simplex across
	from the high point, tries it, and replaces the high point if the new 
	point is better.                                                             */

static double amotsa (I, double psum[], double pb[], double ptry[],
	double *yb, long ihi, double *yhi, double fac, double tt)
{
    iam (SimulatedAnnealingMinimizer);
    double fac1, fac2, yflu, ytry; 
	long j;
    
    fac1 = ( 1.0 - fac) / my nParameters;
    fac2 = fac1 - fac;
    for (j = 1; j <= my nParameters; j++)
	{
		ptry[j] = psum[j] * fac1 - my simplex[ihi][j] * fac2;
	}
    ytry = my func (my object, ptry);
    if (ytry <= *yb)
    {
		/*
			Save the best-ever.
		*/
		
		for (j=1; j <= my nParameters; j++)
		{
			pb[j] = ptry[j];
		}
		*yb = ytry;
    }
	
    /*
		Subtract thermal fluctuation.
	*/
	
    yflu = ytry - tt * log (NUMrandomUniform (0, 1));
    if (yflu < *yhi)
    {
		my y[ihi] = ytry; *yhi = yflu;
		for (j = 1; j <= my nParameters; j++)
		{
	    	psum[j] += ptry[j] - my simplex[ihi][j];
	    	my simplex[ihi][j] = ptry[j];
		}
    }
    return yflu;
}

static int classSimulatedAnnealingMinimizer_minimize (I)
{
    iam (SimulatedAnnealingMinimizer); 
	int status = 1;
    double sum, yhi, ylo, ynhi, ysave, yt, ytry, yb = 1e30; 
    double *psum = NULL, *pb = NULL, *ptry = NULL;
    long i, ihi, ilo, j, m, n, mpts = my nParameters + 1;
    
    if (! (psum = NUMdvector (1, my nParameters)) ||
    	! (pb = NUMdvector (1, my nParameters)) ||
    	! (ptry = NUMdvector (1, my nParameters)))
	{
		status = 0; goto end;
	}
    for (n = 1; n <= my nParameters; n++)
    {
		for (sum = 0, m = 1; m <= mpts; m++) sum += my simplex[m][n];
		psum[n] = sum;
    }
	while (my iteration < my maxNumOfIterations)
	{
		double tt =  -my temperature (me, my tclosure);
		    
    	/*
			Determine which point is highest (worst), next-highest and 
			lowest (best).
    		Whenever we look at a vertex it gets a random thermal fluctuation.
		*/
		
    	ilo = 1; 
		ihi = 2;
    	ynhi = ylo = my y[1] + tt * log (NUMrandomUniform (0, 1));
    	yhi = my y[2] + tt * log (NUMrandomUniform (0, 1));
    	if (ylo > yhi)
    	{
			ihi = 1; ilo = 2; ynhi = yhi;
			yhi = ylo; ylo = ynhi;
    	}
    	/* Loop over the points in the simplex */
    	for (i = 3; i <= mpts; i++)
    	{
			yt = my y[i] + tt * log (NUMrandomUniform (0, 1));
			if (yt <= ylo)
			{
				ilo = i; ylo = yt;
			}
			if (yt > yhi)
			{
				ynhi = yhi; ihi = i; yhi = yt;
			}
			else if (yt > ynhi)
			{ 
				ynhi = yt;
			}
    	}
		
    	/* 
			Compute the fractional range and return if satisfactory
    		If returning put best point and value in slot 1.
		*/
		
    	my history[++my iteration] = my minimum = ylo;
    	my success = (2.0 * fabs (yhi - ylo) / ( fabs (yhi) + fabs (ylo))) 
			< my tolerance;
    	if ((my after && ! my after (me, my aclosure)) || my success)
    	{
			double swap = my y[1]; 
			my minimum = my y[1] = my y[ilo]; 
			my y[ilo] = swap;
			
			for (n = 1; n <= my nParameters; n++)
			{
				swap = my simplex[1][n];
				my p[n] = my simplex[1][n] = my simplex[ilo][n];
				my simplex[ilo][n] = swap; 
			}
			goto end;
    	}
		
    	/*
			Reflect the simplex from the high point
		*/
		
    	ytry = amotsa (me, psum, pb, ptry, &yb, ihi, & yhi, -1.0, tt);
    	if (ytry <= ylo)
		{
			/*
				Gives a better result, so try additional extrapolation
			*/
			
			ytry = amotsa (me, psum, pb, ptry, &yb, ihi, & yhi, 2.0, tt);
		}
    	else if (ytry >= ynhi)
    	{
			/* 
				Reflected point is worst than second-highest:
				Do one-dimensional contraction
			*/
			
			ysave = yhi;
			ytry = amotsa (me, psum, pb, ptry, &yb, ihi, & yhi, 0.5, tt);
			if (ytry >= ysave)
			{
	    		/*
					Didn't help. Contract around the best point
				*/
				
	    		for (i = 1; i <= mpts; i++)
	    		{
					if (i != ilo)
					{
		    			for (j = 1; j <= my nParameters; j++)
		    			{
							my simplex[i][j] = psum[j] = 0.5 * 
								(my simplex[i][j] + my simplex[ilo][j]);
		    			}
		    			my y[i] = my func (my object, psum);
					}
	    		}
				for (n = 1; n <= my nParameters; n++)
    			{
					for (sum = 0, m = 1; m <= mpts; m++)
					{
						sum += my simplex[m][n];
					}
					psum[n] = sum;
    			}
			}
    	}
    }
end:
    NUMdvector_free (ptry, 1);
    NUMdvector_free (pb, 1);
    NUMdvector_free (psum, 1);
    return status;
}

double _SimulatedAnnealingMinimizer_temperatureStep (I, Any tclosure)
{
	iam (SimulatedAnnealingMinimizer); 
	structSimulatedAnnealingMinimizer_step t = tclosure;
	if (my iteration == 1) my tc = my t0 = t->t0;
	else if (my iteration % t->step == 1) my tc *= t->fraction;
	return my tc;
}

class_methods (SimulatedAnnealingMinimizer, SimplexMinimizer)
   class_method_local (SimulatedAnnealingMinimizer, minimize)
class_methods_end

Any SimulatedAnnealingMinimizer_create (long nParameters, Any object, 
	double (*func) (Any object, const double p[]), double scale, 
	double (*temperature) (I, Any tclosure), Any tclosure)
{
	SimulatedAnnealingMinimizer me;
	
	Melder_assert (nParameters >= 2);
	
	me = new (SimulatedAnnealingMinimizer);
	if (me == NULL) return NULL;
	
	if (SimplexMinimizer_init (me, nParameters, object, func, scale))
	{
		my temperature = temperature;
		my tclosure = tclosure;
		return me;
	}
	forget (me);
	return me;
}

void SimulatedAnnealingMinimizer_setTemperatureProc (I, 
	double (*temperature) (I, Any tclosure), Any tclosure)
{
    iam (SimulatedAnnealingMinimizer);
    my temperature = temperature;
    my tclosure = tclosure;
}

/**************  class FletcherPowellMinimizer ***********************/

#define ALF 1.0e-4
#define TOLX 1.0e-7

/* lnsrch: best point found is in my ptry (==pnew) */
static void classFletcherPowellMinimizer_linmin (I, double p[], double fold, 
	double direction[], double *fret)
{
	iam (FletcherPowellMinimizer); 
	long i, n = my nParameters;
	double a, alam, alam2, alamin, b, disc, f2, fold2;
	double rhs1, rhs2, slope, sum, temp, test, tmplam;

	for (sum = 0.0, i = 1; i <= n; i++) sum += direction[i] * direction[i];
	sum = sqrt (sum);
	if (sum > my maxLineStep)
	{
		for (i = 1; i <= n; i++) direction[i] *= my maxLineStep / sum;
	}
	for (slope = 0.0, i = 1; i <= n; i++) slope += my dp[i] * direction[i];
	test = 0.0;
	for (i = 1; i <= n; i++)
	{ 
		temp = fabs (p[i]) / (fabs (p[i]) > 1 ? fabs (p[i]) : 1);
		if (temp > test) test = temp;
	}
	alamin = TOLX / test;
	alam = 1.0;
	for(;;)
	{
		for (i = 1; i <= n; i++) my ptry[i] = p[i] + alam * direction[i];
		*fret = my func (my object, my ptry);
		if (alam < alamin)
		{
			for (i = 1; i <= n; i++) my ptry[i] = p[i];
			return;
		}
		else if (*fret <= fold + ALF * alam * slope)
		{
			break;
		}
		else
		{
			if (alam == 1.0)
			{
				tmplam = - slope / (2.0 * (*fret - fold - slope));
			}
			else
			{
				rhs1 = *fret - fold - alam * slope;
				rhs2 = f2 - fold2 - alam2 * slope;
				a = (rhs1 / (alam * alam) - rhs2 / (alam2 * alam2)) 
					/ (alam - alam2);
				b = (-alam2 * rhs1 / (alam * alam) + alam * rhs2 
					/ (alam2 * alam2)) / (alam - alam2);
				if (a == 0.0)
				{
					tmplam = - slope / (2.0 * b);
				}
				else
				{
					disc = b * b - 3.0 * a * slope;
					if (disc < 0) Melder_casual
						("FletcherPowellMinimizer::linmin roundoff problem.");
					else tmplam = (-b + sqrt (disc)) / (3.0 * a);
				}
				if (tmplam > 0.5 * alam) tmplam = 0.5 * alam;
			}
		}
		alam2 = alam;
		f2 = *fret;
		fold2 = fold; 
		alam = tmplam > 0.1 * alam ? tmplam : 0.1 * alam;
	}
	/*
		Update direction and point
	*/
	for (i = 1; i <= n; i++)
	{
		direction[i] = my ptry[i] - p[i];
		p[i] = my ptry[i];
	}
}

#define EPSDFP 2.2e-16
#define TOLXDFP (4*EPSDFP)
#define STPMX 100.0

static int classFletcherPowellMinimizer_minimize (I)
{
	iam (FletcherPowellMinimizer); 
	int status = 1;
	long n = my nParameters, i, j;
	double den, fac, fad, fae, fp, sum = 0.0, sumdg, sumxi, temp, test;
	double *dg = NULL, *hdg = NULL, **hessin = NULL;

	if (! (dg = NUMdvector (1 , n)) ||
		! (hdg = NUMdvector (1, n)) ||
		! (hessin = NUMdmatrix (1, n, 1, n)))
	{
		status = 0; goto end;
	}
	fp = my func (my object, my p);
	my dfunc (my object, my p, my dp);
	for (i = 1; i <= n; i++)
	{
		for (j = 1; j <= n; j++) hessin[i][j] = 0.0;
		hessin[i][i] = 1.0;
		my direction[i] = - my dp[i];
		sum += my p[i] * my p[i];
	} 
	my maxLineStep = STPMX * (sqrt (sum) > n ? sqrt (sum) : n);
	while (my iteration < my maxNumOfIterations)
	{
		our linmin (me, my p, fp, my direction, & my minimum);
		
		/* 
			Save minimum for next line search
		*/
		
		fp = my history[++my iteration] = my minimum;
		
		/*
			Test for convergence on delta x
		*/
		
		test = 0.0;
		for (i = 1; i <= n; i++)
		{ 
			temp = fabs (my direction[i]) 
				/ (fabs (my p[i]) > 1 ? fabs (my p[i]) : 1);
			if (temp > test) test = temp;
		}
		if (! (my success = (test < TOLXDFP)))
		{
			for (i = 1; i <= n; i++) dg[i] = my dp[i];
			my dfunc (my object, my p, my dp);
			test = 0.0;
			den = my minimum > 1 ? my minimum : 1;
			for (i = 1; i <= n; i++)
			{
				temp = fabs (my dp[i]) * 
					(fabs (my p[i]) > 1 ? fabs (my p[i]) : 1) / den;
				if (temp > test) test = temp;
			}
			my success = test < my tolerance;
		}
		if ((my after && ! my after (me, my aclosure)) || my success) goto end;
		for (i = 1; i <= n; i++) dg[i] = my dp[i] - dg[i];
		for (i = 1; i <= n; i++)
		{
			hdg[i] = 0.0;
			for (j = 1; j <= n; j++) hdg[i] += hessin[i][j] * dg[j];
		}
		fac = fae = sumdg = sumxi = 0.0;
		for (i = 1; i <= n; i++)
		{
			fac += dg[i] * my direction[i];
			fae += dg[i] * hdg[i];
			sumdg += dg[i] * dg[i];
			sumxi += my direction[i] * my direction[i];
		}
		if (fac * fac > EPSDFP * sumdg * sumxi)
		{
			fac = 1.0 / fac;
			fad = 1.0 / fae;
			for (i = 1; i <= n; i++)
			{
				dg[i] = fac * my direction[i] - fad * hdg[i];
			}
			for (i = 1; i <= n; i++)
			{
				for (j = 1; j <= n; j++)
				{
					hessin[i][j] += fac * my direction[i] * my direction[j]
						- fad * hdg[i] * hdg[j] + fae * dg[i] * dg[j];
				}
			}
		}
		for (i = 1; i <= n; i++)
		{
			my direction[i] = 0.0;
			for (j = 1; j <= n; j++)
			{
				my direction[i] -= hessin[i][j] * my dp[j];
			}
		}
	}
end:
	NUMdmatrix_free (hessin, 1, 1);
	NUMdvector_free (hdg,1);
	NUMdvector_free (dg, 1);
	return status;
}

static void classFletcherPowellMinimizer_destroy (I)
{
    iam (FletcherPowellMinimizer);
    NUMdvector_free (my dp, 1);
    inherited (FletcherPowellMinimizer) destroy (me);
}

class_methods (FletcherPowellMinimizer, LineMinimizer)
   class_method_local (FletcherPowellMinimizer, minimize)
   class_method_local (FletcherPowellMinimizer, linmin)
   class_method_local (FletcherPowellMinimizer, destroy)
class_methods_end

Any FletcherPowellMinimizer_create (long nParameters, Any object, 
	double (*func) (Any object, const double p[]),
	void (*dfunc) (Any object, const double p[], double dp[]))
{
	FletcherPowellMinimizer me = new (FletcherPowellMinimizer);
	
	if (me == NULL) return NULL;
	
	if (LineMinimizer_init (me, nParameters, object, func))
	{
		my dp = NUMdvector (1, nParameters);
		if (my dp != NULL)
		{
			my dfunc = dfunc;
			return me;
		}
	}
	forget (me); 
	return me;
}

/*****************  class VDSmagtMinimizer ******************************/

static int classVDSmagtMinimizer_minimize (I)
{
    iam (VDSmagtMinimizer); 
	long i;
    int decrease_direction_found = 1, iteration = 1;
    double rtemp, rtemp2;
	
    /*
    	df is estimate of function reduction obtainable during line search
     	restart = 2 => line search in direction of steepest descent
     	restart = 1 => line search with Powell-restart.
     	flag = 1 => no decrease in function value during previous line search;
     	flag = 2 => line search did not decrease gradient
     	    OK; must restart
    */
	
	if (my restart_flag)
	{     
    	my minimum = my func (my object, my p);
    	my dfunc (my object, my p, my dp);
 		my df = my minimum;
    	my restart = 2;
		my one_up = my flag = 0;
		my gcg0 = my gopt_sq = 0.0;
	}
	my restart_flag = 1;
    while (++my iteration <= my maxNumOfIterations)
    {     
		if (my flag & 1)
		{
	    	if (my one_up)
			{
				decrease_direction_found = 0; 
				my iteration--; 
				break;
			}
	    	else
			{ 
				my one_up = 1;
			}
		}
		else
		{
			my one_up = 0;
		}
		if (my flag & 2)
		{
			my restart = 2; /* my flag & 1 ??? */
		}
		else if (fabs ((double) my gcg0) > 0.2 * my gopt_sq)
		{
			my restart = 1;
		}
		if (my restart == 0)
		{
	    	for (rtemp = rtemp2 = 0.0, i = 1; i <= my nParameters; i++) 
	    	{ 
				rtemp += my gc[i] * my grst[i]; 
				rtemp2 += my gc[i] * my srst[i]; 
	    	}
	    	my gamma = rtemp / my gamma_in;
	    	if (fabs (my beta * my gropt - my gamma * rtemp2) > 0.2*my gopt_sq)
			{
				my restart = 1;
			}
	    	else
			{
				for (i=1; i <= my nParameters; i++)
				{ 
	    			my s[i] = my beta * my s[i] + my gamma * my srst[i] 
						- my gc[i];
				}
			}
		}
		if (my restart == 2) 
		{
	    	for (i = 1; i <= my nParameters; i++) my s[i] = - my dp[i];
	    	my restart = 1;
		} 
		else if (my restart == 1) 
		{
	    	my gamma_in = my gropt - my gr0;
	    	for (i=1; i <= my nParameters; i++) 
	    	{ 
				my srst[i] = my s[i]; 
				my s[i] = my beta * my s[i] - my gc[i];
				my grst[i] = my gc[i] - my g0[i]; 
	    	}
	    	my restart = 0;
		}
		
		/*
			Begin line search
			lineSearch_iteration = #iterations during current line search
		*/
		
		my flag = 0;
		my lineSearch_iteration = 0;
		for (rtemp = 0.0, i = 1; i <= my nParameters; i++) 
		{ 
	    	rtemp += my dp[i] * my s[i]; 
	    	my g0[i] = my dp[i]; 
		}
		my gr0 = my gropt = rtemp;
		if (iteration == 1) my alphamin = fabs (my df / my gropt);
		if (my gr0 > 0) 
		{
	    	my flag = 1;
	    	my restart = 2;
			continue;
		}
		my f0 = my minimum;
		
		/*
			alpha = length of step along line;
		 	dalpha = change in alpha
		 	alphamin = position of min along line
		*/
		
		my alplim = -1;
		my again = -1;
		rtemp = fabs (my df / my gropt);
		my dalpha = my alphamin < rtemp ? my alphamin : rtemp;
		my alphamin = 0;
		do 
		{
	    	do 
			{
				if (my lineSearch_iteration) 
				{
		    		if (! (my fch == 0))
					{
						my gr2s += (my temp + my temp) / my dalpha;
					}
					
		    		if (my alplim < -0.5)
					{
						my dalpha = 9.0 * my alphamin;
					}
		    		else
					{
						my dalpha = 0.5 * (my alplim - my alphamin);
					}
					
		    		my grs = my gropt + my dalpha * my gr2s;
		    		if (my gropt * my grs < 0)
					{
						my dalpha *= my  gropt / (my gropt - my grs);
					}
				}
				my alpha = my alphamin + my dalpha;
				for (i = 1; i <= my nParameters; i++)
				{
					my pc[i] = my p[i] + my dalpha * my s[i];
				}
    			my fc = my func (my object, my pc);
    			my dfunc (my object, my pc, my gc);
				iteration++;
				my lineSearch_iteration++;
				for (my gsq = my grc = 0.0, i = 1; i <= my nParameters; i++) 
				{ 
		    		my gsq += my gc[i] * my gc[i];
		    		my grc += my gc[i] * my s[i];
				} 
				my fch = my fc - my minimum;
				my gr2s = (my grc - my gropt) / my dalpha;
				my temp = (my fch + my fch) / my dalpha - my grc - my gropt;
				if ((my fc < my minimum) ||
					((my fc == my minimum) && (my grc / my gropt > -1))) 
				{
		    		double *tmp;
		    		my gopt_sq = my gsq;
		    		my history[my iteration] = my minimum = my fc;
		    		tmp = my p; my p = my pc; my pc = tmp;
		    		tmp = my dp; my dp = my gc; my gc = tmp;
		    		if (my grc * my gropt <= 0) my alplim = my alphamin;
		    		my alphamin = my alpha;
		    		my gropt = my grc;
		    		my dalpha = -my dalpha;
		    		my success = my gsq < my tolerance;
					if ((my after && ! my after (me, my aclosure)) || 
						my success) return 1;
		    		if (fabs (my gropt / my gr0) < my lineSearchGradient) break;
				}
				else
				{
					my alplim = my alpha;
				}
	    	} while (my lineSearch_iteration 
				<= my lineSearchMaxNumOfIterations);
		
	    	my fc = my history[my iteration] = my minimum;
	    	for (rtemp = 0.0, i = 1; i <= my nParameters; i++)
	    	{
				my pc[i] = my p[i]; 
				my gc[i] = my dp[i]; 
				rtemp += my gc[i] * my g0[i]; 
	    	}  
	    	my gcg0 = rtemp;
	    	if (fabs(my gropt - my gr0) > my tolerance)
	    	{
				my beta = (my gopt_sq - my gcg0) / (my gropt - my gr0);
				if (fabs (my beta * my gropt) < 0.2 * my gopt_sq) break;
	    	}
	    	my again++;
	    	if (my again > 0) my flag += 2;
		} while (my flag < 1);
		
		if (my f0 <= my minimum) my flag += 1;
		my df = my gr0 * my alphamin;
	}
	if (my iteration > my maxNumOfIterations)
	{
		my iteration = my maxNumOfIterations;
	}
    if (decrease_direction_found) my restart_flag = 0;
    return 1;
}

static void classVDSmagtMinimizer_destroy (I)
{
    iam (VDSmagtMinimizer);
    NUMdvector_free (my dp, 1);
    NUMdvector_free (my pc, 1);
    NUMdvector_free (my gc, 1);
    NUMdvector_free (my g0, 1);
    NUMdvector_free (my s, 1);
    NUMdvector_free (my srst, 1);
    NUMdvector_free (my grst, 1);
    inherited (VDSmagtMinimizer) destroy (me);
}

static void classVDSmagtMinimizer_reset (I)
{
    iam (VDSmagtMinimizer);
    my restart_flag = 1;
}

static void classVDSmagtMinimizer_setParameters (I, Any parameters)
{
	iam (VDSmagtMinimizer);
	if (parameters)
	{
		structVDSmagtMinimizer_parameters p = parameters;
    	my lineSearchGradient = p->lineSearchGradient;
    	my lineSearchMaxNumOfIterations = p->lineSearchMaxNumOfIterations;
    }
}

class_methods (VDSmagtMinimizer, Minimizer)
   class_method_local (VDSmagtMinimizer, minimize)
   class_method_local (VDSmagtMinimizer, destroy)
   class_method_local (VDSmagtMinimizer, reset)
   class_method_local (VDSmagtMinimizer, setParameters)
class_methods_end

Any VDSmagtMinimizer_create (long nParameters, void *object, 
	double (*func) (void *object, const double x[]),
    void (*dfunc) (void *object, const double x[], double dx[]))
{
    VDSmagtMinimizer me = new (VDSmagtMinimizer);
	
	if (me == NULL) return NULL;
	if (Minimizer_init (me, nParameters, object) &&
		((my dp   = NUMdvector (1, nParameters)) != NULL)  &&
		((my pc   = NUMdvector (1, nParameters)) != NULL) &&
		((my gc   = NUMdvector (1, nParameters)) != NULL) &&
		((my g0   = NUMdvector (1, nParameters)) != NULL) &&
		((my s    = NUMdvector (1, nParameters)) != NULL) &&
		((my srst = NUMdvector (1, nParameters)) != NULL) &&
		((my grst = NUMdvector (1, nParameters)) != NULL))
	{
    	my func = func; 
		my dfunc = dfunc;
    	my lineSearchGradient = 0.9; 
    	my lineSearchMaxNumOfIterations = 5;
		return me;
	}
	forget (me);
	return NULL;
}

/* End of file Minimizers.c */