//////////////////////////////////////////////////////////////////////
//
//	GradientDescentOptimizer.cpp: Implementation of the GradienDescentOptimizer
//								  Optimizer class.
//
//	Written by Matthias Wacker
//
//////////////////////////////////////////////////////////////////////

#include "optimization/GradientDescentOptimizer.h"

using namespace optimization;

//#include <iostream>


GradientDescentOptimizer::GradientDescentOptimizer(OptLogBase *loger)
	: SuperClass(loger)
{
	m_stepLength = -1;
}


GradientDescentOptimizer::GradientDescentOptimizer( const GradientDescentOptimizer &opt) : SuperClass(opt)
{
	m_stepSize = opt.m_stepSize;
	m_stepLength = opt.m_stepLength;
}

GradientDescentOptimizer::~GradientDescentOptimizer()
{
}

void GradientDescentOptimizer::setStepSize(const matrix_type & stepSize)
{
	m_stepSize = stepSize;
	m_stepLength = -m_stepSize.MaxVal();
}

void GradientDescentOptimizer::init()
{
	SuperClass::init();

	if (m_stepSize.rows() != static_cast<int>(m_numberOfParameters))
	{
		m_stepSize = m_scales; 
	}
	else
	{
		// check for nonzero stepsize
		bool tmp=false;
		for(int i = 0; i < static_cast<int>(m_numberOfParameters); ++i)
		{
			if(m_stepSize[i][0] > 0 )
			{
				tmp=true;
			}
		}
		if(tmp==false)
		{
			std::cerr <<  "GradientDescentOptimizer::init  all stepsizes zero - check your code!"<< endl;
			exit(1);
		}
	}

	
	m_numIter = 0;

	m_analyticalGradients = m_costFunction->hasAnalyticGradient();
	
	/*
		FIXME: WACKER: compute first value and first gradient!
	*/
	m_currentCostFunctionValue = evaluateCostFunction(m_parameters);

	m_gradient = (m_analyticalGradients == true && 
				(m_costFunction->hasAnalyticGradient() == true) ) ?
							getAnalyticalGradient(m_parameters) : 
							getNumericalGradient(m_parameters, m_stepSize);


}

int GradientDescentOptimizer::optimize()
{
 	/*
		call init
	*/
	init();


	if(m_loger)
		m_loger->logTrace("GradientDescentOptimizer: starting optimization ... \n");



	/*
		start time criteria
	*/
	m_startTime = clock();


	bool abort = false;
//	double stepLength = -1.0;
	double cosAngle = 0.0;
	double downScaleFactor = 0.5;

	matrix_type stepSize = m_stepSize;
	double stepLength = m_stepLength;
	

	/*
		compute start value and first gradient!
	*/
	m_currentCostFunctionValue = evaluateCostFunction(m_parameters);

	m_gradient = (m_analyticalGradients == true && 
				(m_costFunction->hasAnalyticGradient() == true) ) ?
							getAnalyticalGradient(m_parameters) : 
							getNumericalGradient(m_parameters, m_stepSize);



	/*
		check abort criteria for gradient
	*/	
	if(m_gradientTolActive)
	{
		if(m_gradient.Norm(0) < m_gradientTol){
			m_returnReason = SUCCESS_GRADIENTTOL;
			abort = true;

			if(m_verbose == true)
			{
				std::cout << "# Gradient Descenct :: aborting because of GradientTol" << std::endl;
			}
		}
	}	


	/* do the optimization */
	while(abort == false)
	{
		/*
			increase iteration counter
		*/
		m_numIter++;
		
		if(m_verbose == true)
		{
			std::cout << "# Gradient Descenct :: Iteration: " << m_numIter << " : current parameters :\n ";
			for(int r = 0; r < static_cast<int>(m_numberOfParameters); r++)
			{
				std::cout<< m_parameters[r][0] << " ";
			}
			std::cout << m_currentCostFunctionValue << std::endl;

			std::cout << " current gradient :\n ";
			for(int r = 0; r < static_cast<int>(m_numberOfParameters); r++)
            {
                std::cout<< m_gradient[r][0] << " ";
            }
            std::cout << std::endl;

		}

		/*
			Check iteration limit
		*/
		if(m_maxNumIterActive)
		{
			if(m_numIter >= m_maxNumIter)
			{
				if(m_verbose == true)
				{
					std::cout << "# Gradient Descenct :: aborting because of numIter" << std::endl;
				}


				/* set according return status and return */
				m_returnReason = SUCCESS_MAXITER;
				abort = true;
				continue;
			}
		}


		/*
			store last results for comparison
		*/
		matrix_type oldParams = m_parameters;
		matrix_type oldGradient = m_gradient;
		double oldFuncValue = m_currentCostFunctionValue;
		

		/*
			get gradient
		*/
		//m_gradient = (m_analyticalGradients == true) ? getAnalyticalGradient(m_parameters) : getNumericalGradient(m_parameters, stepSize);
		m_gradient = (m_analyticalGradients == true && 
				(m_costFunction->hasAnalyticGradient() == true) ) ?
							getAnalyticalGradient(m_parameters) : 
							getNumericalGradient(m_parameters, stepSize);

		/*
			weight with scalings
		*/
		//m_gradient = m_gradient.ElMul(m_scales);
		for(int k=0; k < static_cast<int>(m_numberOfParameters); ++k)
		{
			m_gradient[k][0] *= m_scales[k][0];
		}

		/*
			check gradient tol
		*/
		if(m_gradientTolActive)
		{
			if(m_gradient.Norm(0) < m_gradientTol)
			{
				if(m_verbose == true)
				{
					std::cout << "# Gradient Descenct :: aborting because of GradientTol" << std::endl;
				}

				m_returnReason = SUCCESS_GRADIENTTOL;
				abort = true;
				continue;
			}
		}

		/*
			set gradient length to 1, if length of gradient is too small .. 
			return ERROR_COMPUTATION_UNSTABLE
			(this can happen if gradienTol is not active..)
			FIXME: WACKER think about a "usefull" limit
		*/
		if (m_gradient.Norm(0) > 1.0e-50)
		{
			for(int k=0; k < static_cast<int>(m_numberOfParameters); ++k)
			{
				m_gradient[k][0] /= m_gradient.Norm(0);
			}
		}
		else
		{
			m_returnReason = ERROR_COMPUTATION_UNSTABLE;
			if(m_verbose == true)
			{
				std::cout << "# Gradient Descenct :: aborting because of ERROR_COMPUTATION_UNSTABLE " << std::endl;
			}

			abort =true;
			continue;
		}
		
		/*
			check direction change to scale down for convergence!
		*/
		if(( !m_gradient * oldGradient)[0][0] < cosAngle)
		{
			/*
				direction change is more than acos(cosAngle) deg.
				scaledown by downScaleFactor
			*/

			for(int k=0; k < static_cast<int>(m_numberOfParameters); ++k)
				stepSize[k][0] *= downScaleFactor;

			stepLength *= downScaleFactor;
			/*FIXME: WACKER: only as long
			as there is no steplength computation!*/
		}
		
				
		/*
			set next iteration step
		*/
		m_parameters = m_parameters + m_gradient * stepLength ; 

		/*
			Check if it is in bounds, paramTol, funcTol, NumIter, gradienttol, maxSeconds
		*/
		if(m_lowerParameterBoundActive || m_upperParameterBoundActive)
		{
			for(int i=0; i <static_cast<int>(m_numberOfParameters); i++)
			{
				if( m_upperParameterBoundActive)
				{
					if(m_parameters[i][0] >= m_upperParameterBound[i][0])
					{
						if(m_verbose == true)
						{
							std::cout << "# Gradient Descenct :: aborting because of parameter Bounds" << std::endl;
						}

						/* set according return status and the last parameters and return */
						m_returnReason = ERROR_XOUTOFBOUNDS;
						m_parameters = oldParams;
						abort = true;
						continue;
					}
				}
				if( m_lowerParameterBoundActive)
				{

					if(m_parameters[i][0] <= m_lowerParameterBound[i][0])
					{
						if(m_verbose == true)
						{
							std::cout << "# Gradient Descenct :: aborting because of parameter Bounds" << std::endl;
						}

						/* set according return status and return the last parameter*/
						m_returnReason = ERROR_XOUTOFBOUNDS;
						m_parameters = oldParams;
						abort = true;
						continue;
					}
				}
			}
		}

		/*
			Get new Costfunction Value
		*/
		m_currentCostFunctionValue = evaluateCostFunction(m_parameters);

		/*
			Check ParamTol
		*/
		if(m_paramTolActive)
		{
			if( (m_parameters - oldParams).Norm(0) < m_paramTol)
			{
				if(m_verbose == true)
				{
					std::cout << "Gradient Descenct :: aborting because of param Tol" << std::endl;
				}

				/* set according return status and the last parameters and return */
				m_returnReason = SUCCESS_PARAMTOL;
				/*m_parameters = oldParams;*/
				abort = true;
				continue;
			}

		}
	
		if(m_funcTolActive)
		{
			if( fabs((m_currentCostFunctionValue - oldFuncValue)) < m_funcTol)
			{
				if(m_verbose == true)
				{
					std::cout << "# Gradient Descenct :: aborting because of Func Tol" << std::endl;
				}

				/* set according return status and the last parameters and return */
				m_returnReason = SUCCESS_FUNCTOL;
				/* m_parameters = oldParams; */
				abort = true;
				continue;
			}
		}

		/*
			Check Optimization Timilimit, if active
		*/
		if(m_maxSecondsActive)
		{
			m_currentTime = clock();
		
			//std::cout << (_startTime - _currentTime) << std::endl;
			//std::cout << CLOCKS_PER_SEC << std::endl;
			/* time limit exceeded ? */
			if(((float)(m_currentTime - m_startTime )/CLOCKS_PER_SEC) >= m_maxSeconds )
			{
				/*
					
				*/
				if(m_verbose == true)
				{
					std::cout << "# Gradient Descenct :: aborting because of Time Limit" << std::endl;
				}

				/* set according return status and the last parameters and return */
				m_returnReason = SUCCESS_TIMELIMIT;
				m_parameters = oldParams;
				abort = true;
				continue;
			}
		}

	}

	if(m_verbose)
	{
		std::cout << "# Gradient Descenct :: RESULT: ";
		for(int r = 0; r < static_cast<int>(m_numberOfParameters); r++)
		{
			std::cout<< m_parameters[r][0] << " ";
		}
		std::cout << "    value:   " << m_currentCostFunctionValue  << std::endl;
	}

	return m_returnReason;

}