/*
Optimizer.cpp
Written by Matthew Fisher

Generic non-linear optimizer.  Uses conjugate gradient descent (or other inferior/sub-component algorithms like steepest descent.)
For a full description of the Optimizer class, se Optimizer.h.
*/

const double SmallDelta = 1e-4;			//small delta used to calculate the derivative
const double LineSearchEpsilon = 1e-4;	//starting epsilon for the line search
const int LineSearchIterations = 20;	//maximum number of line search iterations

void Optimizer::ComputeGradient()
{
	double BaseError = Error();			//store the starting error
	double VStart,NewError;

	for(UINT i=0;i<Variables.Length();i++)
	{
		VStart = *(Variables[i].Target);		//store the starting weight
		*(Variables[i].Target) += SmallDelta;	//displace a small amount
		NewError = Error();						//get the new error
		(*Variables[i].Target) = VStart;		//restore the old weight
		Variables[i].Partial = (NewError - BaseError) / SmallDelta;	//approximate the partial derivative
	}
}

void Optimizer::AdaptiveLearning(double &LearningRate, double Alpha, double Beta)
{
	double BaseError = Error();		//store the old error
	ComputeGradient();
	for(UINT i=0;i<Variables.Length();i++)
	{
		*(Variables[i].Target) -= Variables[i].Partial * LearningRate;	//gradient descent
	}
	double CurError = Error();
	if(CurError < BaseError)		//if the step was good,
	{
		LearningRate *= Alpha;		//increase the learning rate
	}
	else
	{
		for(UINT i=0;i<Variables.Length();i++)
		{
			*(Variables[i].Target) += Variables[i].Partial * LearningRate;	//otherwise reset,
		}
		LearningRate *= Beta;		//and decresase the learning rate
	}
}

void Optimizer::MomentumGradientDescent(double LearningRate, double MomentumRate)
{
	ComputeGradient();
	for(UINT i=0;i<Variables.Length();i++)
	{
		*(Variables[i].Target) += -Variables[i].Partial * LearningRate + MomentumRate * Variables[i].OldPartial;	//proceed according to the previous weight change and the gradient,
		Variables[i].OldPartial = -Variables[i].Partial * LearningRate + MomentumRate * Variables[i].OldPartial;	//and store this weight change as the previous weight change
	}
}

void Optimizer::GradientDescent(double LearningRate)
{
	ComputeGradient();
	for(UINT i=0;i<Variables.Length();i++)
	{
		*(Variables[i].Target) -= Variables[i].Partial * LearningRate;	//descent along the gradient
	}
}

void Optimizer::SteepestDescent()
{
	ComputeGradient();
	for(UINT i=0;i<Variables.Length();i++)
	{
		Variables[i].LineSearchDirection = -Variables[i].Partial;		//set up the line search direction as the gradient
	}
	LineSearch();	//search in this direction
}

void Optimizer::LineSearch()
{
	bool Exit = false;
	double PrevError = Error(), CurError,	//errors
		DistPrevW, DistCurW, DistNextW, DistM1, DistM2, //five points for line search
		ErrorM, ErrorM1, ErrorM2;	//current error at the three middle points

	for(UINT i=0;i<Variables.Length();i++)
	{
		Variables[i].LineSearchStart = *(Variables[i].Target);	//store where we started
	}

	UINT LSIters = 0;
	while(!Exit)
	{
		DistPrevW = LineSearchEpsilon * pow(2,LSIters-2);
		DistCurW = LineSearchEpsilon * pow(2,LSIters-1);
		DistNextW = LineSearchEpsilon * pow(2,LSIters);	//calculate the three distances
		for(UINT i=0;i<Variables.Length();i++)
		{
			*(Variables[i].Target) = Variables[i].LineSearchStart + DistNextW * Variables[i].LineSearchDirection;	//go the appropriate distance along this direction,
		}
		CurError = Error();	//get the error
		if(CurError > PrevError) Exit = true;	//if we gained error, exit the search
		else PrevError = CurError;				//otherwise keep going
		LSIters++;
		if(LSIters > 25)						//if we're stuck in an infinite loop,
		{
			cout << "Iterations failed." << endl;
			for(UINT i=0;i<Variables.Length();i++)
			{
				*(Variables[i].Target) = Variables[i].LineSearchStart;	//reset
			}
			return;														//and abort
		}
	}

	for(UINT Iterations=0;Iterations<LineSearchIterations;Iterations++)		//loop a bunch of times
	{
		DistM1 = (DistPrevW + DistCurW) * 0.5f;
		DistM2 = (DistCurW + DistNextW) * 0.5f;	//get the midpoints

		for(i=0;i<Variables.Length();i++)
			*(Variables[i].Target) = Variables[i].LineSearchStart + DistM1 * Variables[i].LineSearchDirection;
		ErrorM1 = Error();						//get the error at the left midpoint

		for(i=0;i<Variables.Length();i++)
			*(Variables[i].Target) = Variables[i].LineSearchStart + DistM2 * Variables[i].LineSearchDirection;
		ErrorM2 = Error();						//get the error at the right midpoint

		for(i=0;i<Variables.Length();i++)
			*(Variables[i].Target) = Variables[i].LineSearchStart + DistCurW * Variables[i].LineSearchDirection;
		ErrorM = Error();						//get the error at the central point

		if(ErrorM1 < ErrorM2 && ErrorM1 < ErrorM) {	//depending on which is smaller,
			DistPrevW = DistPrevW;
			DistCurW = DistM1;
			DistNextW = DistCurW;				//load left three points
		} else if(ErrorM < ErrorM2) {
			DistPrevW = DistM1;
			DistCurW = DistCurW;
			DistNextW = DistM2;					//load middle three points
		} else {
			DistPrevW = DistCurW;
			DistCurW = DistM2;
			DistNextW = DistNextW;				//load right three points
		}
	}
	for(i=0;i<Variables.Length();i++)			//move into the midpoint
		*(Variables[i].Target) = Variables[i].LineSearchStart + DistCurW * Variables[i].LineSearchDirection;
}

void Optimizer::ConjugateReset(int _RepeatRate)
{
	for(UINT i=0;i<Variables.Length();i++)
	{
		Variables[i].OldPartial = 0.0;			//reset beta
	}
	RepeatRate = _RepeatRate;					//load the repeat rate
	RepeatsLeft = RepeatRate;
}

void Optimizer::ConjugateGradient()
{
	double Beta, Numerator = 0.0, Denominator = 0.0;

	ComputeGradient();
	
	for(UINT i=0;i<Variables.Length();i++)
	{
		Numerator += Variables[i].Partial * (Variables[i].Partial - Variables[i].OldPartial);
		Denominator += Variables[i].OldPartial * Variables[i].OldPartial;						//load the numerator and denominator of beta
	}
	if(Denominator == 0.0) Beta = 0.0;
	else Beta = Numerator / Denominator;	//load beta

	RepeatsLeft--;
	if(RepeatsLeft == 0)
	{
		RepeatsLeft = RepeatRate;			//reset counter has come
		Beta = 0.0;
	}

	for(i=0;i<Variables.Length();i++)
	{
		Variables[i].LineSearchDirection = -Variables[i].Partial + Beta * Variables[i].LineSearchDirection;	//load the line search direction based upon conj. gradient
	}
	LineSearch();		//search

	for(UINT i=0;i<Variables.Length();i++)
	{
		Variables[i].OldPartial = Variables[i].Partial;	//save the current partials
	}
}