/*
 * GradJSearch.c:
 */

#include "../../../config.h"

#if defined(_PARALLEL_) && defined(_HAVE_PETSC_)

#include "../include/cielo.h"
#include "../modules.h"
#include "./parallel.h"

#undef __FUNCT__ 
#define __FUNCT__ "GradJSearch"
#undef CLEANUP
#define CLEANUP GradJSearchLocalCleanup();

void GradJSearchLocalCleanup(void);


int GradJSearch(double* search_vector,FemModel* femmodel,int step){
	
	/*Error management: */
	int noerr=1;
	int i,n;
	int dummy;
	ParameterInputs* inputs=NULL;
	int status;

	//status=GoldenSearch(search_vector,femmodel->workspaceparams->J+step,-1,1,femmodel->workspaceparams->tolx,(int)femmodel->workspaceparams->maxiter[step],
	//		femmodel->workspaceparams->fit[step],femmodel->workspaceparams->optscal[step],&objectivefunctionC,femmodel);  //do only one dimension search for now.

	status=BrentSearch(search_vector,femmodel->workspaceparams->J+step,-1,1,femmodel->workspaceparams->tolx,(int)femmodel->workspaceparams->maxiter[step],
			femmodel->workspaceparams->fit[step],femmodel->workspaceparams->optscal[step],&objectivefunctionC,femmodel);  //do only one dimension search for now.

	TESTEXIT(noerr);
	
	return status;
}

void GradJSearchLocalCleanup(void){
	return;
}

int GoldenSearch(double* psearch_scalar,double* pJ,double xa, double xb, double tolerance, int maxiter, double fit,double optscal,double (*f)(double*,double,double,FemModel*,ParameterInputs*),FemModel* femmodel){
	
	double xc, xd, fc, fd;
	double oneminustau = 1 - (sqrt(5) - 1) / 2;
	int iter = 0;
	ParameterInputs* inputs=NULL;
	int status;

	inputs=NewParameterInputs();

	xc = xa + oneminustau * (xb - xa);
	fc = (*f)(&xc,fit,optscal,femmodel,inputs);
	xd = xb - oneminustau * (xb - xa);
	fd = (*f)(&xd,fit,optscal,femmodel,inputs);
	do {
		iter++;
		if (fc < fd) {
			xb = xd;
			xd = xc;
			xc = xa + oneminustau * (xb - xa);
			fd = fc;
			fc = (*f)(&xc,fit,optscal,femmodel,inputs);
		}
		else {
			xa = xc;
			xc = xd;
			xd = xb - oneminustau * (xb - xa);
			fc = fd;
			fd = (*f)(&xd,fit,optscal,femmodel,inputs);
		}
		_printf_("         iter# %i f(x) %g x %g  toler %g/%g iter %i/%i\n",iter,(fc+fd)/2,(xa+xb)/2,fabs(xb-xa),tolerance,iter,maxiter);
	} 
	while (fabs(xb - xa) > tolerance && iter < maxiter);

	if (fabs(xb-xa)<tolerance)status=0;
	else status=1;

	/*Assign output pointers: */
	*psearch_scalar=(xa+xb)/2;
	*pJ=(fc+fd)/2;
	
	return status;
}

int BrentSearch(double* psearch_scalar,double* pJ,double a, double b, double tolerance, int maxiter, double fit,double optscal,double (*f)(double*,double,double,FemModel*,ParameterInputs*),FemModel* femmodel){

	/* This routine is optimizing a given function using Brent's method
	 * (Golden or parabolic procedure)*/
 
	/*optimization variable: */
	double si;
	double gold;
	double intervalgold;
	double oldintervalgold;
	double parab_num,parab_den;
	double distance;

	/*function values: */
	double fxmax,fxmin,fxbest,fval;
	double fx,fx1,fx2;

	/*x : */
	double xmax,xmin,xbest;
	double x,x1,x2,xm,xval;

	/*tolerances: */
	double tol1,tol2,seps;

	/*counters: */
	int iter,goldenflag,loop;

	/*inputs: */
	int status;
	ParameterInputs* inputs=NULL;
	
	/*Recover inputs: */
	inputs=NewParameterInputs();

	//initialize counter and boundaries
	iter=0;

	//get the value of the function at the first boundary
	fxmin = (*f)(&a,fit,optscal,femmodel,inputs);

	//display result
	_printf_("\n        Iteration       x           f(x)       Tolerance         Procedure\n\n");
	_printf_("        %s   %12.6g  %12.6g  %s","   N/A",a,fxmin,"         N/A         boundary\n");

	//get the value of the function at the first boundary b and display result
	fxmax = (*f)(&b,fit,optscal,femmodel,inputs);
	_printf_("        %s   %12.6g  %12.6g  %s","   N/A",b,fxmax,"         N/A         boundary\n");

	//initialize the other variables
	seps=sqrt(DBL_EPSILON); //precision of a double
	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

	//Compute initial point
	
	//1: initialize the value of the 4 x needed (x1,x2,x,xbest)
	x1=a+gold*(b-a);
	x2=x1;
	xbest=x1;
	x=xbest;

	//2: call the function to be evaluated
	fxbest = (*f)(&x,fit,optscal,femmodel,inputs);
	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*(a+b);                           
	//update tolerances
	tol1=seps*sqrt(pow(xbest,2))+tolerance/3.0;
	tol2=2.0*tol1;

	//4: print result
	_printf_("         %5i    %12.6g  %12.6g  %12.6g  %s\n",iter,xbest,fxbest,pow(pow(xbest-xm,2),0.5),"       initial");

	//Main Loop
	loop=1;
	while(loop){

		goldenflag=1;

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

			// Yes, so fit parabola
			goldenflag=0;
			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
			if (( sqrt(pow(parab_num,2)) < sqrt(pow(0.5*parab_den*oldintervalgold,2))) &&
						(parab_num>parab_den*(a-xbest)) &&
						(parab_num<parab_den*(b-xbest))){

				// 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-a)<tol2) || ((b-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=1;
			}
		}

		//Golden procedure
		if(goldenflag){

			// compute the new distance d
			if(xbest>=xm){
				intervalgold=a-xbest;    
			}
			else{ 
				intervalgold=b-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,fit,optscal,femmodel,inputs);
		iter=iter+1;

		// Update a, b, xm, x1, x2, tol1, tol2
		if (fx<=fxbest){
			if (x>=xbest){
				a=xbest;
			}
			else{
				b=xbest;
			}
			x1=x2;    fx1=fx2;
			x2=xbest; fx2=fxbest;
			xbest=x;  fxbest=fx;
		}

		else{ // fx > fxbest
			if (x < xbest){
				a=x;
			}
			else{
				b=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*(a+b);
		tol1=seps*pow(pow(xbest,2),0.5)+tolerance/3.0;
		tol2=2.0*tol1;

		//print result
		if (goldenflag){
			_printf_("         %5i    %12.6g  %12.6g  %12.6g  %s\n",iter,x,fx,pow(pow(xbest-xm,2),0.5),"       golden");
		}
		else{
			_printf_("         %5i    %12.6g  %12.6g  %12.6g  %s\n",iter,x,fx,pow(pow(xbest-xm,2),0.5),"       parabolic");
		}

		//Stop the optimization?
		if (sqrt(pow(xbest-xm,2)) < (tol2-0.5*(b-a))){
			_printf_("\nOptimization terminated:\nthe current x satisfies the termination criteria using 'tolx' of %g \n", tolerance);
			loop=0;
			status=0;
		}
		else if (iter>=maxiter){
			_printf_("\nExiting: Maximum number of iterations has been exceeded  - increase 'maxiter'\n");
			loop=0;
			status=1;
		}
		else{
			//continue
			loop=1;
		}
	}//end while

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

	/*Assign output pointers: */
	*psearch_scalar=xval;
	*pJ=fval;
	
	return status;
}
#endif //#if defined(_PARALLEL_) && defined(_HAVE_PETSC_)

