//////////////////////////////////////////////////////////////////////
//
//	BrentLineSearcher.cpp: Implementation of the Brent LineSearcher class.
//
//	Written by Matthias Wacker
//
//	Big parts of code are from numerical recipes in C
//
//////////////////////////////////////////////////////////////////////

#include "optimization/BrentLineSearcher.h"
#include <iostream>

#define BrentSHFT(a,b,c,d)	(a)=(b);(b)=(c);(c)=(d);
#define BrentSIGN(a,b) ((b) >= 0.0 ? fabs(a) : -fabs(a))

using namespace OPTIMIZATION;

BrentLineSearcher::BrentLineSearcher() : m_CGOLD(0.38196601125010515179541316563436), m_ZEPS(1.0e-10)
{
	m_a = 0.0;
	m_b = 3.8196601125010515179541316563436;
	m_c = 10.0;
	m_eps = 0.1;
}

BrentLineSearcher::BrentLineSearcher(CostFunction *costFunc,OptLogBase* loger) : SuperClass(costFunc,loger),m_CGOLD(0.38196601125010515179541316563436), m_ZEPS(1.0e-10)
{
	m_a = 0.0;
	m_b = 3.8196601125010515179541316563436;
	m_c = 10.0;
	m_eps = 0.1;
}

BrentLineSearcher::BrentLineSearcher(const BrentLineSearcher &lin) : SuperClass(lin),m_CGOLD(0.38196601125010515179541316563436), m_ZEPS(1.0e-10)
{
	m_a = lin.m_a;
	m_b = lin.m_b;
	m_c = lin.m_c;
	m_eps = lin.m_eps;
}

BrentLineSearcher & BrentLineSearcher::operator =(const BrentLineSearcher &lin)
{
	/*
			avoid self-copy
	*/
	if(this != &lin)
	{
		/*
			=operator of SuperClass
		*/
		SuperClass::operator=(lin);

		m_a = lin.m_a;
		m_b = lin.m_b;
		m_c = lin.m_c;

		m_eps = lin.m_eps;
	
	}

	return *this;

}

BrentLineSearcher::~BrentLineSearcher()
{
}

bool BrentLineSearcher::setBounds(double a, double c)
{
	if (a >= c)
	{
		return false;
	}

	m_a = a;
	m_b = a + m_CGOLD * (c - a);
	m_c = c;

	return true;
}

bool BrentLineSearcher::setEps(double eps)
{
	if(eps < 0)
	{
		return false;
	}
	
	m_eps = eps;
	return true;
}

double BrentLineSearcher::optimize()
{
	int numIter=0;
	double a=0.0;
	double b=0.0;
	double d=0.0;
	double etemp=0.0;
	double fu=0.0;
	double fv=0.0;
	double fw=0.0;
	double fx=0.0;
	double p=0.0;
	double q=0.0;
	double r=0.0;
	double tol1=0.0;
	double tol2=0.0;
	double u=0.0;
	double v=0.0;
	double w=0.0;
	double x=0.0;
	double xm=0.0;
	double e = 0.0;
	double xmin=0.0;
	int maxNumIter = 100;

	x=w=v= m_b;
	fw=fv=fx= evaluateSub(x);

	a = m_a;
	b = m_c;

	/*
		main loop
	*/
	for (numIter = 1;numIter <= maxNumIter; numIter++)
	{
		xm = 0.5* (a+b);
		tol2 = 2.0 * (tol1=m_eps*fabs(x)+m_ZEPS);
		
		if(fabs(x-xm) <= (tol2-0.5*(b-a)))
		{
			xmin = x;
			return xmin;
		}

		/*
			construct trial parabolic fit
		*/
		if(fabs(e) > tol1)
		{
			r=(x-w)*(fx-fv);
			q=(x-v)*(fx-fw);  // FIXME WACKER CHECK IF NR IS WRONG HERE!?
			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=m_CGOLD * (e=(x >= xm ? a-x : b-x ));
			}
			else
			{
				d=p/q; // take the parabolic step
				u = x+d;
				if(u-a < tol2 || b-u < tol2)
				{
					d=BrentSIGN(tol1, xm - x);
				}
			}
		}
		else
		{
			d=m_CGOLD * (e=(x >= xm ? a-x : b-x ));
		}

		u = (fabs(d) >= tol1 ? x+d : x + BrentSIGN(tol1,d));
		
		/*
			this is the one function evaluation per step
		*/
		fu= evaluateSub(u);

		if (fu <= fx)
		{
			if(u >= x )
			{
				a=x;
			}
			else
			{
				b=x;
			}

			BrentSHFT(v,w,x,u);
			BrentSHFT(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;
			}

		}

		if(m_verbose)
		{
			std::cout //<< "a,b,d,etemp,fu,fv,fw,fx,p,q,r,u,v,w,x,xm;" << std::endl
				<< a << " "
				<< b << " "
				<< d << " "
				<< etemp << " "
				<< fu << " "
				<< fv << " "
				<< fw << " "
				<< fx << " "
				<< p << " "
				<< q << " "
				<< r << " "
				<< u << " "
				<< v << " "
				<< w << " "
				<< x << " "
				<< xm << " "
				<< std::endl;

		}

	} // main loop
	
	if(m_pLoger)
		m_pLoger->logWarning("Brent took too many iterations.. Tol threshold was to low for the number of iterations..\n");

	return x;
}