/*Powell - spectrum minimization routine -  logic closely based on routine in Press et al,
 * but totally recoded to be redistributable */
#include "cddefines.h"
#include "sign.h"
#include "func.h"
#include "varypar.h"
#include "powell.h"
#ifdef ITMAX
#undef ITMAX
#endif
#define	ITMAX	200
/*  */
#ifdef NMAX
#undef NMAX
#endif
#define	NMAX	50
/*  */
#ifdef CGOLD
#undef CGOLD
#endif
#define	CGOLD	.3819660
/*  */
#ifdef GLIMIT
#undef GLIMIT
#endif
#define	GLIMIT	100.
/*  */
#ifdef GOLD
#undef GOLD
#endif
#define	GOLD	1.618034
/*  */
#ifdef TINY
#undef TINY
#endif
#define	TINY	1.e-20

/* helper routine */
double Find1Dimin(double x);
double BrentPowell(double,double,double,double(*)(double),double,float*);
void MnBrakPowell(float*,float*,float*,float*,float*,float*,double(*)(double));

/* these are variables with file scope, used by various helper routines */
float pcom[NMAX], 
  xicom[NMAX];
long int ncom;

void LinearMinimum(float p[], 
	  float xi[], 
	  long int n, 
	  float *fret,
	  double toler);

void Powell(float p[], 
  float *xi, 
  long int *n, 
  long int np, 
  double toler, 
  long int *iter, 
  float *fret)
{
#define XI(I_,J_)	(*(xi+(I_)*(np)+(J_)))
	long int i, 
	  ibig, 
	  j;

	float del, 
	  dist,
	  fp, 
	  fptt, 
	  pt[NMAX], 
	  ptt[NMAX], 
	  t, 
	  xit[NMAX];

#	ifdef DEBUG_FUN
	fputs( "<+>Powell()\n", debug_fp );
#	endif

	*fret = (float)func(p);
	for( j=0; j < *n; j++ )
	{
		pt[j] = p[j];
	}

	*iter = 0;
L_1:
	*iter += 1;
	fp = *fret;
	ibig = 0;
	del = 0.;

	for( i=0; i < *n; i++ )
	{
		for( j=0; j < *n; j++ )
		{
			xit[j] = XI(i,j);
		}

		fptt = *fret;
		LinearMinimum(p,xit,*n,fret,toler);

		if( fabs(fptt-*fret) > del )
		{
			del = (float)fabs(fptt-*fret);
			ibig = i;
		}
	}

	/* old if(iter.eq.itmax) pause 'Powell exceeding maximum iterations'*/
	/* stop if number of function evaluations exceeds maximum */
	if( VaryPar.nOptimiz >= VaryPar.nIterOptim )
	{
		fprintf( ioQQQ, " Powell exceeding maximum iterations.\n This can be reset with the OPTIMIZE ITERATIONS command.\n" );
		
#		ifdef DEBUG_FUN
		fputs( " <->Powell()\n", debug_fp );
#		endif
		return;
	}

	dist = 0.f;
	for( j=0; j < *n; j++ )
	{
		ptt[j] = (float)(2.*p[j] - pt[j]);
		xit[j] = p[j] - pt[j];
		dist += POW2(xit[j]);
		pt[j] = p[j];
	}

	/* stop if distance between new and old minimum <= toler */
	if( sqrt(dist) <= toler )
	{ 
#		ifdef DEBUG_FUN
		fputs( " <->Powell()\n", debug_fp );
#		endif
		return;
	}

	fptt = (float)func(ptt);

	if( fptt >= fp )
		goto L_1;

	t = (float)(2.*(fp - 2.**fret + fptt)*POW2(fp - *fret - del) - 
		del*POW2(fp - fptt));

	if( t >= 0. )
		goto L_1;

	LinearMinimum(p,xit,*n,fret,toler);

	for( j=0; j < *n; j++ )
	{
		XI(ibig,j) = XI(*n-1,j);
		XI(*n-1,j) = xit[j];
	}
	goto L_1;
#	undef	XI
}

/*LinearMinimum helper routine for simplx */
void LinearMinimum(float p[], 
	  float xi[], 
	  long int n, 
	  float *fret,
	  double toler)
{
	long int j;

	float ax, 
	  bx, 
	  fa, 
	  fb, 
	  fx, 
	  xmin, 
	  xx;

#	ifdef DEBUG_FUN
	fputs( "<+>LinearMinimum()\n", debug_fp );
#	endif
	/*routine needes BrentPowell,Find1Dimin,MnBrakPowell */

	ncom = n;
	for( j=0; j < n; j++ )
	{
		pcom[j] = p[j];
		xicom[j] = xi[j];
	}

	ax = 0.;
	xx = 1.;

	MnBrakPowell(&ax,&xx,&bx,&fa,&fx,&fb,Find1Dimin);

	*fret = (float)BrentPowell(ax,xx,bx,Find1Dimin,toler,&xmin);

	for( j=0; j < n; j++ )
	{
		xi[j] *= xmin;
		p[j] += xi[j];
	}
	
#	ifdef DEBUG_FUN
	fputs( " <->LinearMinimum()\n", debug_fp );
#	endif
	return;
}

/*Find1Dimin helper routine for simplex  */

double Find1Dimin(double x)
{
	long int j;
	double f1dim_v;
	float xt[NMAX];

#	ifdef DEBUG_FUN
	fputs( "<+>Find1Dimin()\n", debug_fp );
#	endif
	/*U    USES func */

	for( j=0; j < ncom; j++ )
	{
		xt[j] = (float)(pcom[j] + x*xicom[j]);
	}

	f1dim_v = func(xt);

#	ifdef DEBUG_FUN
	fputs( " <->Find1Dimin()\n", debug_fp );
#	endif
	return( f1dim_v );
}

/*BrentPowell helper for Powell optimization routine */
double BrentPowell(double ax, 
  double bx, 
  double cx, 
  double (*f)(double), 
  double tol, 
  float *xmin)
{
/* do not take steps smaller than TOL */
#define TOL 1.e-6

	long int iter;
	double a, 
	  b, 
	  brent_v, 
	  d=0., 
	  e, 
	  etemp, 
	  fu, 
	  fv, 
	  fw, 
	  fx, 
	  p, 
	  q, 
	  r, 
	  u, 
	  v, 
	  w, 
	  x, 
	  xm;

#	ifdef DEBUG_FUN
	fputs( "<+>BrentPowell()\n", debug_fp );
#	endif

	a = MIN2(ax,cx);
	b = MAX2(ax,cx);
	v = bx;
	w = v;
	x = v;
	e = 0.;
	fx = (*f)(x);
	fv = fx;
	fw = fx;

	for( iter=0; iter < ITMAX; iter++ )
	{
		xm = 0.5*(a + b);
		/* stop if the size of the search area is smaller than tol */
		if( fabs(0.5*(b-a)) <= tol )
		{
			*xmin = (float)x;
			brent_v = fx;
#			ifdef DEBUG_FUN
			fputs( " <->BrentPowell()\n", debug_fp );
#			endif
			return( brent_v );
		}

		if( fabs(e) > tol )
		{
			r = (x - w)*(fx - fv);
			q = (x - v)*(fx - fw);
			p = (x - v)*q - (x - w)*r;
			q = 2.*(q - r);

			if( q > 0. )
				p = -p;

			q = fabs(q);
			etemp = e;
			/* ms c compiler flagged d as being used before being defined.  fixed by
			 * setting to 0 at start */
			e = d;
			if( fabs(p) >= fabs(.5*q*etemp) || p <= q*(a - x) || p >= q*(b - x) )
				d = CGOLD*(e = (x >= xm ? a-x : b-x));
			else
			{
				d = p/q;
				u = x + d;
				if( u - a < 2.*TOL || b - u < 2.*TOL )
					d = sign(TOL,xm-x);
			}
		}
		else
		{
			d = CGOLD*(e = (x >= xm ? a-x : b-x));
		}

		u = (fabs(d) >= TOL ? x + d : x + sign(TOL,d));
		fu = (*f)(u);

		if( fu <= fx )
		{
			if( u >= x )
			{
				a = x;
			}
			else
			{
				b = x;
			}
			v = w;
			fv = fw;
			w = x;
			fw = fx;
			x = u;
			fx = fu;
		}
		else
		{
			if( u < x )
			{
				a = u;
			}
			else
			{
				b = u;
			}
			if( fu <= fw || w == x )
			{
				v = w;
				fv = fw;
				w = u;
				fw = fu;
			}
			else if( (fu <= fv || v == x) || v == w )
			{
				v = u;
				fv = fu;
			}
		}
	}
	puts( "BrentPowell exceed maximum iterations" );
	puts( "[Stop in BrentPowell]" );
	exit(1);
#	undef TOL
}

/*helper routine for simplex */

void MnBrakPowell(float *ax, 
	  float *bx, 
	  float *cx, 
	  float *fa, 
	  float *fb, 
	  float *fc, 
	  double (*powelfunc)(double))
{
	double dum, 
	  fu, 
	  q, 
	  r, 
	  ulim;
	double u;

#	ifdef DEBUG_FUN
	fputs( "<+>MnBrakPowell()\n", debug_fp );
#	endif
	*fa = (float)(*powelfunc)(*ax);
	*fb = (float)(*powelfunc)(*bx);
	if( *fb > *fa )
	{
		dum = *ax;
		*ax = *bx;
		*bx = (float)dum;
		dum = *fb;
		*fb = *fa;
		*fa = (float)dum;
	}
	*cx = *bx + (float)(GOLD*(*bx - *ax));
	*fc = (float)(*powelfunc)(*cx);
L_1:
	if( *fb >= *fc )
	{
		r = (*bx - *ax)*(*fb - *fc);
		q = (*bx - *cx)*(*fb - *fa);
		u = fabs(q-r);
		u = *bx - ((*bx - *cx)*q - (*bx - *ax)*r)/(2.*sign(MAX2(u,TINY),q-r));
		ulim = *bx + GLIMIT*(*cx - *bx);
		if( (*bx - u)*(u - *cx) > 0. )
		{
			fu = (*powelfunc)(u);
			if( fu < *fc )
			{
				*ax = *bx;
				*fa = *fb;
				*bx = (float)u;
				*fb = (float)fu;
				
#				ifdef DEBUG_FUN
				fputs( " <->MnBrakPowell()\n", debug_fp );
#				endif
				return;
			}
			else if( fu > *fb )
			{
				*cx = (float)u;
				*fc = (float)fu;
				
#				ifdef DEBUG_FUN
				fputs( " <->MnBrakPowell()\n", debug_fp );
#				endif
				return;
			}
			u = *cx + GOLD*(*cx - *bx);
			fu = (*powelfunc)(u);
		}
		else if( (*cx - u)*(u - ulim) > 0. )
		{
			fu = (*powelfunc)(u);
			if( fu < *fc )
			{
				*bx = *cx;
				*cx = (float)u;
				u = *cx + GOLD*(*cx - *bx);
				*fb = *fc;
				*fc = (float)fu;
				fu = (*powelfunc)(u);
			}
		}
		else if( (u - ulim)*(ulim - *cx) >= 0. )
		{
			u = ulim;
			fu = (*powelfunc)(u);
		}
		else
		{
			u = *cx + GOLD*(*cx - *bx);
			fu = (*powelfunc)(u);
		}
		*ax = *bx;
		*bx = *cx;
		*cx = (float)u;
		*fa = *fb;
		*fb = *fc;
		*fc = (float)fu;
		goto L_1;
	}
	
#	ifdef DEBUG_FUN
	fputs( " <->MnBrakPowell()\n", debug_fp );
#	endif
	return;
}