/*!\file:  BrentSearch.cpp
 * \brief optimization algorithm
 */ 

#ifdef HAVE_CONFIG_H
	#include <config.h>
#else
#error "Cannot compile with HAVE_CONFIG_H symbol! run configure first!"
#endif

#include <float.h>
#include <iomanip>

#include "../Exceptions/exceptions.h"
#include "../io/io.h"
#include "../MemOps/MemOps.h"
#include "./Verbosity.h"
#include "./OptPars.h"
#include "./OptArgs.h"
#include "./types.h"
#include "./isnan.h"

void BrentSearch(IssmDouble* psearch_scalar,IssmDouble* pJ,OptPars* optpars,IssmDouble (*f)(IssmDouble,OptArgs*), OptArgs* optargs){

	/* This routine is optimizing a given function using Brent's method
	 * (Golden or parabolic procedure)*/

	/*Intermediary*/
	IssmDouble si,gold,intervalgold,oldintervalgold;
	IssmDouble parab_num,parab_den;
	IssmDouble distance,cm_jump;
	IssmDouble fxmax,fxmin,fxbest;
	IssmDouble fx,fx1,fx2;
	IssmDouble xmax,xmin,xbest;
	IssmDouble x,x1,x2,xm;
	IssmDouble tol1,tol2,seps;
	IssmDouble tolerance = 1.e-4;
	int        maxiter ,iter;
	bool       loop= true,goldenflag;

	/*Recover parameters:*/
	xmin=optpars->xmin;
	xmax=optpars->xmax;
	maxiter=optpars->maxiter;
	cm_jump=optpars->cm_jump;

	/*initialize counter and get response at the boundaries*/
	iter=0;
	fxmin = (*f)(xmin,optargs);
	if (xIsNan<IssmDouble>(fxmin)) _error_("Function evaluation returned NaN");
	cout<<setprecision(5);
	if(VerboseControl()) _printf0_("\n");
	if(VerboseControl()) _printf0_("       Iteration         x           f(x)       Tolerance         Procedure\n");
	if(VerboseControl()) _printf0_("\n");
	if(VerboseControl()) _printf0_("           N/A    "<<setw(12)<<xmin<<"  "<<setw(12)<<fxmin<<"           N/A         boundary\n");
	fxmax = (*f)(xmax,optargs);
	if (xIsNan<IssmDouble>(fxmax)) _error_("Function evaluation returned NaN");
	if(VerboseControl()) _printf0_("           N/A    "<<setw(12)<<xmax<<"  "<<setw(12)<<fxmax<<"           N/A         boundary\n");

	/*test if jump option activated and xmin==0*/
	if (!xIsNan<IssmDouble>(cm_jump) && (xmin==0) && (fxmax/fxmin)<cm_jump){
		*psearch_scalar=xmax;
		*pJ=fxmax;
		return;
	}

	/*initialize optimization variables*/
	seps=sqrt(DBL_EPSILON);    //precision of a IssmDouble
	distance=0.0;              //new_x=old_x + distance
	gold=0.5*(3.0-sqrt(5.0));  //gold = 1 - golden ratio
	intervalgold=0.0;          //distance used by Golden procedure

	/*1: initialize the values of the 4 x needed (x1,x2,x,xbest)*/
	x1=xmin+gold*(xmax-xmin);
	x2=x1;
	xbest=x1;
	x=xbest;

	/*2: call the function to be evaluated*/
	fxbest = (*f)(x,optargs);
	if(xIsNan<IssmDouble>(fxbest)) _error_("Function evaluation returned NaN");
	iter=iter+1;

	/*3: update the other variables*/
	fx1=fxbest;
	fx2=fxbest;
	/*xm is always in the middle of a and b*/
	xm=0.5*(xmin+xmax);                           
	/*update tolerances*/
	tol1=seps*sqrt(pow(xbest,2))+tolerance/3.0;
	tol2=2.0*tol1;

	/*4: print result*/
	if(VerboseControl())
	 _printf0_("         "<<setw(5)<<iter<<"    "<<setw(12)<<xbest<<"  "<<setw(12)<<fxbest<<"  "<<setw(12)<<pow(pow(xbest-xm,2),0.5)<<"         initial\n");
	if (!xIsNan<IssmDouble>(cm_jump) && (xmin==0) && ((fxbest/fxmin)<cm_jump)){
		if(VerboseControl()) _printf0_("      optimization terminated: current x satisfies criteria 'cm_jump'=" << cm_jump << "\n");
		loop=false;
	}

	while(loop){

		goldenflag=true;

		// Is a parabolic fit possible ?
		if (sqrt(pow(intervalgold,2))>tol1){

			// Yes, so fit parabola
			goldenflag=false;
			parab_num=(xbest-x1)*(xbest-x1)*(fxbest-fx2)-(xbest-x2)*(xbest-x2)*(fxbest-fx1);;
			parab_den=2.0*(xbest-x1)*(fxbest-fx2)-2.0*(xbest-x2)*(fxbest-fx1);

			//reverse p if necessary
			if(parab_den>0.0){ 
				parab_num=-parab_num;
			}
			parab_den=sqrt(pow(parab_den,2));
			oldintervalgold=intervalgold;
			intervalgold=distance;

			// Is the parabola acceptable (we use seps here because in some configuration parab_num==parab_den*(xmax-xbest)
			// and the result is not repeatable anymore
			if (( sqrt(pow(parab_num,2)) < sqrt(pow(0.5*parab_den*oldintervalgold,2))) &&
						(parab_num>parab_den*(xmin-xbest)+seps) && 
						(parab_num<parab_den*(xmax-xbest)-seps)){

				// Yes, parabolic interpolation step
				distance=parab_num/parab_den;
				x=xbest+distance;

				// f must not be evaluated too close to min_x or max_x
				if (((x-xmin)<tol2) || ((xmax-x)<tol2)){
					if ((xm-xbest)<0.0) si=-1;
					else                si=1;
					//compute new distance
					distance=tol1*si;
				}
			}
			else{
				// Not acceptable, must do a golden section step
				goldenflag=true;
			}
		}

		//Golden procedure
		if(goldenflag){
			// compute the new distance d
			if(xbest>=xm){
				intervalgold=xmin-xbest;    
			}
			else{ 
				intervalgold=xmax-xbest;  
			}
			distance=gold*intervalgold;
		}

		// The function must not be evaluated too close to xbest
		if(distance<0) si=-1;
		else           si=1;
		if(sqrt(pow(distance,2))>tol1) x=xbest+si*sqrt(pow(distance,2));
		else                           x=xbest+si*tol1;

		//evaluate function on x
		fx = (*f)(x,optargs);
		if(xIsNan<IssmDouble>(fx)) _error_("Function evaluation returned NaN");
		iter=iter+1;

		// Update a, b, xm, x1, x2, tol1, tol2
		if (fx<=fxbest){
			if (x>=xbest) xmin=xbest;
			else          xmax=xbest;
			x1=x2;    fx1=fx2;
			x2=xbest; fx2=fxbest;
			xbest=x;  fxbest=fx;
		}
		else{ // fx > fxbest
			if (x<xbest) xmin=x;
			else         xmax=x;
			if ((fx<=fx2) || (x2==xbest)){
				x1=x2; fx1=fx2;
				x2=x;  fx2=fx;
			}
			else if ( (fx <= fx1) || (x1 == xbest) || (x1 == x2) ){
				x1=x;  fx1=fx;
			}
		}
		xm = 0.5*(xmin+xmax);
		tol1=seps*pow(pow(xbest,2),0.5)+tolerance/3.0;
		tol2=2.0*tol1;
		if(VerboseControl())
		 _printf0_("         "<<setw(5)<<iter<<"    "<<setw(12)<<x<<"  "<<setw(12)<<fx<<"  "<<setw(12)<<pow(pow(xbest-xm,2),0.5)<<
					 "         "<<(goldenflag?"golden":"parabolic")<<"\n");

		/*Stop the optimization?*/
		if (sqrt(pow(xbest-xm,2)) < (tol2-0.5*(xmax-xmin))){
			if(VerboseControl()) _printf0_("      optimization terminated: current x satisfies criteria 'tolx'=" << tolerance << "\n");
			loop=false;
		}
		else if (iter>=maxiter){
			if(VerboseControl()) _printf0_("      exiting: Maximum number of iterations has been exceeded  ('maxiter'=" << maxiter << ")\n");
			loop=false;
		}
		else if (!xIsNan<IssmDouble>(cm_jump) && (xmin==0) && ((fxbest/fxmin)<cm_jump)){
			if(VerboseControl()) _printf0_("      optimization terminated: current x satisfies criteria 'cm_jump'=" << cm_jump << "\n");
			loop=false;
		}
		else{
			//continue
			loop=true;
		}
	}//end while

	//Now, check that the value on the boundaries are not better than current fxbest
	if (fxbest>fxmin){
		xbest=optpars->xmin; fxbest=fxmin;
	}
	if (fxbest>fxmax){
		xbest=optpars->xmax; fxbest=fxmax;
	}

	/*Assign output pointers: */
	*psearch_scalar=xbest;
	*pJ=fxbest;
}
