/* EGAD: powell.cpp
   
Navin Pokala and Tracy Handel
Dept. of Molecular and Cell Biology
University of California, Berkeley
Copyright (C) 2003 Regents of the University of California
GNU Public License
Aug 12 2003

Absolutely no warranties are made or are implied with the use of this program or its parts.

This file contains functions for powell minimization of objective functions with POWELL argument.

These have all been adapted from Numerical Recipies in C.
   
*/


#include "powell.h"

#define GOLD 1.618034
#define CGOLD 0.3819660
#define GLIMIT 100.0
#define ITMAX 200

#define NR_END 1
#define FREE_ARG char*
#define SQR(a) ((arg=(a)) == 0.0 ? 0.0 : arg*arg)
#define SIGN(a, b) ((b) >= 0.0 ? fabs(a): -fabs(a))
#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 delX	1.0e-3
#define delX_2	2.0e-3

static double arg;
static int ncom;
static double *pcom = NULL;
static double *xicom = NULL;
static double *xt = NULL;
static double (*nrfunc)(double [], POWELL *);


/* Numerical Recipes standard error handler;  from Numerical Recipes in C */
void nrerror(char error_text[])
{
	fprintf(stderr,"Numerical Recipes run-time error...\n");
	fprintf(stderr,"%s\n",error_text);
	fprintf(stderr,"...now exiting to system...\n");
	exit(1);
}

/* allocate a double vector with subscript range v[nl..nh] */
double *Vector(long nl, long nh)
{
	double *v;

	v=(double *)malloc((size_t) ((nh-nl+1+NR_END)*sizeof(double)));
	if (!v) nrerror("allocation failure in Vector()");
	return v-nl+NR_END;
}

/* free a double vector allocated with Vector() */
void free_Vector(double *v, long nl, long nh)
{
	free((FREE_ARG) (v+nl-NR_END));
}

void mnbrak(double *ax, double *bx, double *cx, double *fa, double *fb, double *fc, double (*func)(double, POWELL *), POWELL *powell)
{
	double ulim,u,r,q,fu,dum;
	int counter;

	*fa=(*func)(*ax, powell);
	*fb=(*func)(*bx, powell);
	if (*fb > *fa) 
	{
		SHFT(dum,*ax,*bx,dum)
		SHFT(dum,*fb,*fa,dum)
	}
	*cx=(*bx)+GOLD*(*bx-*ax);
	*fc=(*func)(*cx, powell);

	counter = 0;
	while (*fb > *fc) 
	{
		r=(*bx-*ax)*(*fb-*fc);
		q=(*bx-*cx)*(*fb-*fa);
		u=(*bx)-((*bx-*cx)*q-(*bx-*ax)*r)/(2.0*SIGN(max(fabs(q-r),TINY),q-r));
		ulim=(*bx)+GLIMIT*(*cx-*bx);
		if ((*bx-u)*(u-*cx) > 0.0) 
		{
			fu=(*func)(u,powell);
			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);
			fu=(*func)(u, powell);
		} 
		else if ((*cx-u)*(u-ulim) > 0.0) 
		{
			fu=(*func)(u, powell);
			if (fu < *fc) 
			{
				SHFT(*bx,*cx,u,*cx+GOLD*(*cx-*bx))
				SHFT(*fb,*fc,fu,(*func)(u, powell))
			}
		} 
		else if ((u-ulim)*(ulim-*cx) >= 0.0) 
		{
			u=ulim;
			fu=(*func)(u, powell);
		} 
		else 
		{
			u=(*cx)+GOLD*(*cx-*bx);
			fu=(*func)(u, powell);
		}
		SHFT(*ax,*bx,*cx,u)
		SHFT(*fa,*fb,*fc,fu)
		
		++counter;

		if(counter>5)	/* flat surface; return */
			return;
	}
}

double brent(double ax, double bx, double cx, double (*f)(double, POWELL *), double tol, double *xmin, POWELL *powell)
{
	int iter;
	double a,b,d,etemp,fu,fv,fw,fx,p,q,r,tol1,tol2,u,v,w,x,xm;
	double e=0.0;

	d=0;
	a=(ax < cx ? ax : cx);
	b=(ax > cx ? ax : cx);
	x=w=v=bx;
	fw=fv=fx=(*f)(x,powell);
	for (iter=1;iter<=ITMAX;iter++) 
	{
		xm=0.5*(a+b);
		tol2=2.0*(tol1=tol*fabs(x)+TINY);
		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);
			p=(x-v)*q-(x-w)*r;
			q=2.0*(q-r);
			if (q > 0.0) p = -p;
			q=fabs(q);
			etemp=e;
			e=d;
			if (fabs(p) >= fabs(0.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 < tol2 || b-u < tol2)
					d=SIGN(tol1,xm-x);
			}
		} 
		else 
		{
			d=CGOLD*(e=(x >= xm ? a-x : b-x));
		}
		u=(fabs(d) >= tol1 ? x+d : x+SIGN(tol1,d));
		fu=(*f)(u, powell);
		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;
			}
		}
	}
/*	nrerror("Too many iterations in brent");  */
	
	
	

	*xmin=x;
	return fx;
}

double f1dim(double x, POWELL *powell)
{
	int j;
	double f;

	for (j=1;j<=ncom;j++) xt[j]=pcom[j]+x*xicom[j];
	f=(*nrfunc)(xt,powell);

	return f;
}


void linmin(double p[], double xi[], int n, double *fret, double (*func)(double [],POWELL *), POWELL *powell)
{
	int j;
	double xx,xmin,fx,fb,fa,bx,ax;

	for (j=1;j<=n;j++) 
	{
		pcom[j]=p[j];
		xicom[j]=xi[j];
	}
	ax=0.0;
	xx=1.0;
	mnbrak(&ax,&xx,&bx,&fa,&fx,&fb,f1dim,powell);
	*fret=brent(ax,xx,bx,f1dim,TOL,&xmin,powell);
	for (j=1;j<=n;j++) 
	{
		xi[j] *= xmin;
		p[j] += xi[j];
	}

}

/*
	powell minimization with objective function (func *) that takes a POWELL arguement.
	Adapted from Numerical Recipies in C. 
	p = solution array of size n.
	fret = objective function score of p.
	(func *) = objective function (with arguments double [] and POWELL *).
	Completes at least one full iteration.
		returns prematurely if NUM_FUNCTION_CALLS > powell->protein->parameters.max_function_calls or 
		if elapsed time > MAX_OPTIMIZATION_TIME
*/

void powell_minimization(double p[], int n, int max_iterations, double *fret, double (*func)(double [], POWELL *), POWELL *powell, time_t start_time)
{
	int i,ibig,j;
	double del,fp,fptt,t;
    	double ftol = TOL;
	int iter;
        time_t now;
	static int n_max=0;
	static double **xi,*pt,*ptt,*xit;
	static char *filename=NULL, *escape_hatch_filename=NULL;
	FILE *file_ptr;
	extern double MAX_OPTIMIZATION_TIME;
	extern int LOGFILE_FLAG, NUM_FUNCTION_CALLS, GET_PID;
 	extern char *CURRENT_WORKING_DIRECTORY;


	if(escape_hatch_filename==NULL)
	{
		escape_hatch_filename = (char *)calloc(MAXLINE,sizeof(char));
		sprintf(escape_hatch_filename,"%s/escape.POWELL.%d",CURRENT_WORKING_DIRECTORY,GET_PID);
	}

	ncom=n;
	if(n > n_max)
	{
		if(n_max!=0)
		{
			free_Vector(xit,1,n_max);
			free_Vector(ptt,1,n_max);
			free_Vector(pt,1,n_max);
		
			free_Vector(xt,1,n_max);  xt=NULL;
			free_Vector(xicom,1,n_max);  xicom=NULL;
			free_Vector(pcom,1,n_max);	 pcom=NULL;
			
			for(i=1;i<=n_max;++i)
				free_memory(xi[i]);
			free_memory(xi);
		}
	
		n_max=n;

		xi = (double **)calloc(n+2,sizeof(double *));
		for(i=1;i<=n;++i)
			xi[i] = (double *)calloc(n+2,sizeof(double));
			
		pcom=Vector(1,n);
		xicom=Vector(1,n);
		xt=Vector(1,ncom);
		
		pt=Vector(1,n);
		ptt=Vector(1,n);
		xit=Vector(1,n);
	}
	
	for(i=1;i<=n;++i)
	{
		for(j=1;j<=n;++j)
				xi[i][j] = 0.0;
		xi[i][i] = 1.0;
	}

	nrfunc=func;
	*fret=(*func)(p,powell);


	now = time(NULL);
	if(LOGFILE_FLAG != 0)
		{
			if(filename==NULL)
				filename = (char *)calloc(MAXLINE,sizeof(char));
			sprintf(filename, "%s.log", powell->protein->parameters.output_prefix);
	    		file_ptr = fopen_file(filename, "a");
	    		fprintf(file_ptr, "powell start n = %d\t\t", n);
			fprintf(file_ptr, "%f\t%s",  (*fret), ctime(&now) );
			fprintf(file_ptr,"To exit powell minimization and move to the next step:");
			fprintf(file_ptr,"\ttouch %s\n",escape_hatch_filename);
	    		fclose(file_ptr);
		}



	for (j=1;j<=n;j++)
		pt[j]=p[j];

	for (iter=1;;++(iter)) 
	{
		fp=(*fret);
		ibig=0;
		del=0.0;
		for (i=1;i<=n;i++) 
		{
			
			for (j=1;j<=n;j++) 
				xit[j]=xi[j][i];
			 
			fptt=(*fret);
			linmin(p,xit,n,fret,func,powell);	
			if (fabs(fptt-(*fret)) > del) 
			{
				del=fabs(fptt-(*fret));
				ibig=i;
			}
		}
	
		now = time(NULL);

		if(LOGFILE_FLAG != 0)
		{
			sprintf(filename, "%s.log", powell->protein->parameters.output_prefix);
	    		file_ptr = fopen_file(filename, "a");
	    		fprintf(file_ptr, "powell iteration %d\t%d\t%f\t%f\t%f\t%s", iter, NUM_FUNCTION_CALLS, fp, (*fret),
											2.0*fabs(fp-(*fret))/(fabs(fp)+fabs(*fret)),  ctime(&now) );
	    		fclose(file_ptr);

		/*
			if(DEBUG==1)
				printf("powell iteration %d\t%d\t%f\t%f\t%s", iter, NUM_FUNCTION_CALLS, (*fret),
											2.0*fabs(fp-(*fret))/(fabs(fp)+fabs(*fret)),  ctime(&now) );
		*/

		}
			
		if(NUM_FUNCTION_CALLS >= powell->protein->parameters.max_function_calls || 
				difftime(now,start_time) >= MAX_OPTIMIZATION_TIME ||
				does_this_file_exist(escape_hatch_filename)==1)
			{
			iter = max_iterations + 2;

			if(does_this_file_exist(escape_hatch_filename)==1)
			{
				NUM_FUNCTION_CALLS = powell->protein->parameters.max_function_calls+10;
				rm_file(escape_hatch_filename);
			}
			}

		if (2.0*fabs(fp-(*fret)) <= 2*ftol*(fabs(fp)+fabs(*fret)) || iter >= max_iterations)
			return;
		
		for (j=1;j<=n;j++) 
		{
			ptt[j]=2.0*p[j]-pt[j];
			xit[j]=p[j]-pt[j];
			pt[j]=p[j];
		}
		fptt=(*func)(ptt,powell);
		if (fptt < fp) 
		{
			t=2.0*(fp-2.0*(*fret)+fptt)*SQR(fp-(*fret)-del)-del*SQR(fp-fptt);
			if (t < 0.0) 
			{
				linmin(p,xit,n,fret,func,powell);
				for (j=1;j<=n;j++) 
				{
					xi[j][ibig]=xi[j][n];
					xi[j][n]=xit[j];
				}
			}
		}
	}
}