//////////////////////////////////////////////////////////////////////
//
//	AdaptiveDirectionRandomSearchOptimizer.cpp: Implementation of the 
//		ADRS Optimizer
//
//
//////////////////////////////////////////////////////////////////////

#include "AdaptiveDirectionRandomSearchOptimizer.h"

#include <stdlib.h>
#include <time.h>
#include <iostream>

#include "numbase.h" // ice random
#include "optimization/AdditionalIceUtils.h"
#include "optimization/Opt_Namespace.h"

using namespace optimization;

AdaptiveDirectionRandomSearchOptimizer::AdaptiveDirectionRandomSearchOptimizer(
	unsigned int numOfPoints,OptLogBase *loger)	: SuperClass(loger)
{
	m_numberOfParallelPoints = numOfPoints;

	
	m_b0 = 1.0;
	m_bfac = 0.5;
	m_bThresTimesNotDecreased = 0;
	
	m_bk = new double[m_numberOfParallelPoints];
	m_pointsAccepted = new bool[m_numberOfParallelPoints];
	for(int j = 0; j < static_cast<int>(m_numberOfParallelPoints); j++)
	{
		m_bk[j] = m_b0;
		m_pointsAccepted[j]= false;
	}
	m_bBreak = 0.1;
	m_backupActive = false;

	
	m_c0f = 0.75;
	m_c0s = 0.75;
	m_c1f = -0.75;
	m_c1s = 1.25;
	m_D = 3;

	m_initFuncThreshActive = false;
	m_initFuncTresh = 0.0;
	
	// advanced initialization is turned off per default
	m_advancedInit= false;
}


AdaptiveDirectionRandomSearchOptimizer::AdaptiveDirectionRandomSearchOptimizer( const AdaptiveDirectionRandomSearchOptimizer &opt) : SuperClass(opt)
{

	m_numberOfParallelPoints = opt.m_numberOfParallelPoints;
	
	m_Parameters = opt.m_Parameters;
	m_b0		= opt.m_b0;
	m_bfac		= opt.m_bfac;

	m_bk = new double[m_numberOfParallelPoints];
	for(int j = 0; j < static_cast<int>(m_numberOfParallelPoints); j++)
	{
		m_bk[j] = opt.m_bk[j];
	}

	m_pointsAccepted = new bool[m_numberOfParallelPoints];
	for(int j = 0; j < static_cast<int>(m_numberOfParallelPoints); j++)
	{
		m_pointsAccepted[j] = opt.m_pointsAccepted[j];
	}
	
	m_bBreak = opt.m_bBreak;
	m_backupActive	= opt.m_backupActive;
	m_backupPoint	= opt.m_backupPoint;
	m_backupPointValue = opt.m_backupPointValue;

	m_bThresTimesNotDecreased = opt.m_bThresTimesNotDecreased;
	m_c0f		= opt.m_c0f;
	m_c0s		= opt.m_c0s;
	m_c1f		= opt.m_c1f;
	m_c1s		= opt.m_c1s;
	m_D			= opt.m_D;
	m_initFuncThreshActive = opt.m_initFuncThreshActive;
	m_initFuncTresh = opt.m_initFuncTresh;
	
	// advanced init setup
	m_advancedInit= opt.m_advancedInit;
	m_advancedinitLowerBounds= opt.m_advancedinitLowerBounds;
	m_advancedinitUpperBounds= opt.m_advancedinitUpperBounds;
}

/*
	operator=
*/
AdaptiveDirectionRandomSearchOptimizer & AdaptiveDirectionRandomSearchOptimizer::operator=(const AdaptiveDirectionRandomSearchOptimizer &opt)
{
		
	/*
			avoid self-copy
	*/
	if(this != &opt)
	{
		delete[] m_bk;
		delete[] m_pointsAccepted;
				
		/*
			=operator of SuperClass
		*/
		SuperClass::operator=(opt);

			
		/*
			own values:
		*/
		m_numberOfParallelPoints = opt.m_numberOfParallelPoints;
		m_Parameters = opt.m_Parameters;
		m_b0		= opt.m_b0;
		m_bfac		= opt.m_bfac;

		m_bBreak = opt.m_bBreak;

		m_backupActive	= opt.m_backupActive;
		m_backupPoint	= opt.m_backupPoint;
		m_backupPointValue = opt.m_backupPointValue;

		m_bk = new double[m_numberOfParallelPoints];
		for(int j = 0; j < static_cast<int>(m_numberOfParallelPoints); j++)
		{
			m_bk[j] = opt.m_bk[j];
		}

		m_pointsAccepted = new bool[m_numberOfParallelPoints];
		for(int j = 0; j < static_cast<int>(m_numberOfParallelPoints); j++)
		{
			m_pointsAccepted[j] = opt.m_pointsAccepted[j];
		}
		

		m_bThresTimesNotDecreased = opt.m_bThresTimesNotDecreased;
		m_c0f		= opt.m_c0f;
		m_c0s		= opt.m_c0s;
		m_c1f		= opt.m_c1f;
		m_c1s		= opt.m_c1s;
		m_D			= opt.m_D;
		m_initFuncThreshActive = opt.m_initFuncThreshActive;
		m_initFuncTresh = opt.m_initFuncTresh;
		
		// advanced init setup
		m_advancedInit= opt.m_advancedInit;
		m_advancedinitLowerBounds= opt.m_advancedinitLowerBounds;
		m_advancedinitUpperBounds= opt.m_advancedinitUpperBounds;
		
	}

  	return *this;
}


AdaptiveDirectionRandomSearchOptimizer::~AdaptiveDirectionRandomSearchOptimizer()
{
	delete[] m_bk;
	delete[] m_pointsAccepted;
}


void AdaptiveDirectionRandomSearchOptimizer::init()
{
	
	m_Parameters = matrix_type(m_numberOfParameters,m_numberOfParallelPoints);
	
	// if not set before, set default value
	if(m_bThresTimesNotDecreased == 0)
		m_bThresTimesNotDecreased = static_cast<unsigned int>(m_numberOfParameters * m_numberOfParameters*  5.0);
	
	SuperClass::init();

	// "reset"
	for(int j = 0; j < static_cast<int>(m_numberOfParallelPoints); j++)
	{
		m_bk[j] = m_b0;
		m_pointsAccepted[j]= false;
	}
	m_backupActive = false;

	/*
		seed rand
	*/
	ice::Randomize();

	/*
		check if bounds are active! bounds are needed
		to generate usefull random start points
	*/
	if(!(m_upperParameterBoundActive == true && m_lowerParameterBoundActive == true))
	{
		if(m_loger)
		{
			m_loger->logError("parameter bounds are not active! Please set	proper parameter bounds for the random start point generation. This event is has no further exception handling");
		}
		m_lowerParameterBound = m_parameters;
		m_upperParameterBound = m_parameters;
		m_lowerParameterBoundActive = true;
		m_upperParameterBoundActive = true;

	}

	/*
	generate random start points
	*/
	if(m_advancedInit == false)
	{
		for(int k = 0; k < static_cast<int>(m_numberOfParameters);k++)
		{
			for(int l = 0; l < static_cast<int>(m_numberOfParallelPoints);l++)
			{
				m_Parameters[k][l] = m_parameters[k][0] + m_scales[k][0] *2.0* (ice::RandomD() - 0.5);
				
			}
		}
	}
	else
	{
		// dimension check
		assert(m_advancedinitLowerBounds.rows() == (int)m_numberOfParameters && m_advancedinitUpperBounds.rows() == (int)m_numberOfParameters);
		
		for(int k = 0; k < static_cast<int>(m_numberOfParameters);k++)
		{
			for(int l = 0; l < static_cast<int>(m_numberOfParallelPoints);l++)
			{
				m_Parameters[k][l] = m_advancedinitLowerBounds[k][0] +ice::RandomD() * (m_advancedinitUpperBounds[k][0] - m_advancedinitLowerBounds[k][0] ) ;
			}
		}
	}


	/*
		evaluate SET !!
	*/
	m_CurrentCostFunctionValues = evaluateSetCostFunction(m_Parameters);
	/*
		If the threshold was set, check if all points are below the threshold
	*/
	if(m_initFuncThreshActive)
	{
		
		bool pointOk=false;
		for(int u = 0; u < static_cast<int>(m_numberOfParallelPoints); u++)
		{
			/*
				if the are not ok ... generate new points for those, who arent..
			*/
			if(m_CurrentCostFunctionValues[u][0] < m_initFuncTresh)
			{
				pointOk = true;
			}
			else
			{
				pointOk = false;
			}
			
			while(pointOk == false)
			{
				for(int k = 0; k < static_cast<int>(m_numberOfParameters);k++)
				{
					m_Parameters[k][u] = m_parameters[k][0] + m_scales[k][0] * 2.0*(ice::RandomD() - 0.5);
						
				}
				
				/*
					reevaluate the value and check against threshold
				*/
				//double tmpNewValue = evaluateCostFunction(m_Parameters.Sub(m_numberOfParameters,1,0,u));
				double tmpNewValue = evaluateCostFunction(m_Parameters(0,u,m_numberOfParameters-1,u));
			
				/*
					if point is ok now go to next point
				*/
				if(tmpNewValue < m_initFuncTresh)
				{
					m_CurrentCostFunctionValues[u][0] = tmpNewValue;
					pointOk = true;				
				}

			} // while point not ok
		} // for all points
	} // if threshold active

	/*if(m_verbose)
	{
		std::cout << "AdaptiveDirectionRandomSearch :: Initial parameterSet: ";
		for(int l=0;l<m_numberOfParallelPoints;l++)
		{
			for(int r = 0; r < static_cast<int>(m_numberOfParameters); r++)
			{
				std::cout<< m_Parameters[r][l] << " ";
			}
			std::cout << m_CurrentCostFunctionValues[l][0]  << std::endl;
		}
		std::cout << std::endl;


		std::cout << "Number of Evaluations needed for a proper initilization: " << m_costFunction->getNumberOfEvaluations() << std::endl;
	
		
	}*/



	/*
		(re)set m_bk
	*/
	for(int j = 0; j < static_cast<int>(m_numberOfParallelPoints); j++)
	{
		m_bk[j] = m_b0;
	}

}


bool AdaptiveDirectionRandomSearchOptimizer::setControlSeqParams(double b0, double bfac,
	unsigned int bThresTimesNotDecreased,double bBreak)
{
	if(b0 <= 0 || bfac <= 0 || bfac > 1 || bThresTimesNotDecreased == 0 || bBreak <= 0)
	{
		return false;
	}
	
	m_b0 = b0;
	m_bfac = bfac;
	m_bThresTimesNotDecreased = bThresTimesNotDecreased;
	m_bBreak = bBreak;
	
	return true;
}


void AdaptiveDirectionRandomSearchOptimizer::activateAdvancedInit(bool enable, matrix_type& lowerBounds, matrix_type& upperBounds)
{
	m_advancedInit= enable;
	m_advancedinitLowerBounds= lowerBounds;
	m_advancedinitUpperBounds= upperBounds;
}


matrix_type AdaptiveDirectionRandomSearchOptimizer::generatePoint()
{
	matrix_type newPoint(m_numberOfParameters,1);

	for(int i = 0; i < static_cast<int>(m_numberOfParameters); i++)
	{
		if(m_scales[i][0] > 0.0 )
		{
			newPoint[i][0] = m_scales[i][0] * 2.0*(ice::RandomD() - 0.5);
		}
	}
	//double div=newPoint.Norm(0);
	double div=newPoint.Norm(0);
	
	if (div > 1.0e-50)
	{
		newPoint = newPoint * (1.0/div);
	}
	else
	{
		newPoint=this->generatePoint();
	}

	return newPoint;
}	


matrix_type AdaptiveDirectionRandomSearchOptimizer::generatePoints()
{
	matrix_type newPoints(m_numberOfParameters,m_numberOfParallelPoints);
	matrix_type newPoint(m_numberOfParameters,1);

	for(int j = 0; j < static_cast<int>(m_numberOfParallelPoints);j++)
	{
		newPoint = this->generatePoint();
	
		for(int i = 0; i < static_cast<int>(m_numberOfParameters); i++)
		{
			newPoints[i][j] = newPoint[i][0];
		}
	}

	return newPoints;
}	

bool AdaptiveDirectionRandomSearchOptimizer::setRecallParams(
	double c0s,
	double c1s,
	double c0f,
	double c1f,
	double D)
{
	if (c0s < 0			||
		c0s >=1			||
		c1s <0			||
		c0s+c1s <= 1	||
		c0f <= 0		||
		c0f >= 1		||
		c1f >= 0		||
		c0f + c1f < -1.0||
		c0f + c1f > 1.0 || 
		D <= 0.0)
	{
		return false;
	}

	m_c0s = c0s;
	m_c1s = c1s;
	m_c0f = c0f;
	m_c1f = c1f;
	m_D	  = D;
	
	return true;
}

void AdaptiveDirectionRandomSearchOptimizer::acceptPoints(matrix_type oldValues, matrix_type newValues)
{
	for(int i = 0;i< static_cast<int>(m_numberOfParallelPoints);i++)
	{
		if(newValues[i][0] < oldValues[i][0])
		{
			m_pointsAccepted[i]=true;
		}
		else
		{
			m_pointsAccepted[i]=false;
		}
	}
}


int AdaptiveDirectionRandomSearchOptimizer::optimize()
{


	init();


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

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


	bool abort = false;

	/*
		declare and initialize algorithm specific local variables
	*/
	matrix_type		newPoints;
	matrix_type		newValues;
	//matrix_type		oldValues;

	matrix_type		deltaX(m_numberOfParameters,m_numberOfParallelPoints);
	matrix_type		deltaXold(m_numberOfParameters,m_numberOfParallelPoints);
	
	matrix_type		dk(m_numberOfParameters,m_numberOfParallelPoints);
	matrix_type		dkold(m_numberOfParameters,m_numberOfParallelPoints);
	


	int i = 0;
	int j = 0;

	/*
		begin with the first step outside the loop
	*/
	m_numIter++;
	unsigned int *timesNotDecreased = new unsigned int[m_numberOfParallelPoints];
	
	for(int k = 0; k< static_cast<int>(m_numberOfParallelPoints);k++)
	{
		timesNotDecreased[k] = 0; 
	}


	/*
		generate a new delta X (potential step)
	*/
	matrix_type tmp = generatePoints();
	for(j = 0; j< static_cast<int>(m_numberOfParallelPoints);j++)
	{
		for(i = 0;i < static_cast<int>(m_numberOfParameters);i++)
		{
			deltaX[i][j] = tmp[i][j] * m_bk[j] ;
		}
	}

	/*
		check costfunction at potential new point
	*/
	newPoints = m_Parameters + deltaX;
	newValues = evaluateSetCostFunction(newPoints);

	/*
		are the new points better?
	*/
	acceptPoints(m_CurrentCostFunctionValues,newValues);
	
	for(j = 0; j < static_cast<int>(m_numberOfParallelPoints);j++)
	{
		if(m_pointsAccepted[j] == true)
		{
			for(i =0;i < static_cast<int>(m_numberOfParameters);i++)
			{
				/*
					set the new point
				*/
				m_Parameters[i][j] = newPoints[i][j];

				/*
					update the recall factor
				*/
				dk[i][j] = dkold[i][j] * m_c0s + deltaXold[i][j] * m_c1s;
			
			}
			m_CurrentCostFunctionValues[j][0] = newValues[j][0];

			/*
				reset the counter for failed attempts
			*/
			timesNotDecreased[j] = 0;
		}
		else
		{
			for(i =0; i < static_cast<int>(m_numberOfParameters);i++)
			{
				/*
					update the recall factor
				*/
				dk[i][j] = dkold[i][j] * m_c0f + deltaXold[i][j] * m_c1f;
			
			}
			
			/*
				raise the counter for failed attempts
			*/
			timesNotDecreased[j] ++;
		}
	}

	/* 
		do the optimization in the main loop
	*/
	while(abort == false)
	{
		/*
			increase iteration counter
		*/
		m_numIter++;
		
		/*
			Check iteration limit
		*/
		if(m_maxNumIterActive)
		{
			if(m_numIter >= m_maxNumIter)
			{
				/* set according return status and return */
				m_returnReason = SUCCESS_MAXITER;
				abort = true;
				continue;
			}
		}
		
		/*
			save the old deltaX
		*/
		deltaXold = deltaX;

		/*
			generate a new delta X (potential step)
		*/
		matrix_type tmp = generatePoints();
		for(j = 0; j< static_cast<int>(m_numberOfParallelPoints);j++)
		{
			for(i = 0; i < static_cast<int>(m_numberOfParameters);i++)
			{
				deltaX[i][j] = dk[i][j] + tmp[i][j] * m_bk[j] ;
			}
		}
		
		/*
			check costfunction at potential new point
		*/
		newPoints = m_Parameters + deltaX;
		newValues = evaluateSetCostFunction(newPoints);

		/*
			save the old dk
		*/
		dkold = dk;


		/*
			are the new points better?
		*/
		acceptPoints(m_CurrentCostFunctionValues,newValues);
		
		for(j = 0; j < static_cast<int>(m_numberOfParallelPoints);j++)
		{
			if(m_pointsAccepted[j] == true)
			{
				for(i =0; i < static_cast<int>(m_numberOfParameters);i++)
				{
					/*
						set the new point
					*/
					m_Parameters[i][j] = newPoints[i][j];
					
					/*
						update the recall factor
					*/
					dk[i][j] = dkold[i][j] * m_c0s + deltaXold[i][j] * m_c1s;
					
				}
				m_CurrentCostFunctionValues[j][0] = newValues[j][0];
				
				/*
					reset the counter for failed attempts
				*/
				timesNotDecreased[j] = 0;
			}
			else
			{
				for(i =0;i < static_cast<int>(m_numberOfParameters);i++)
				{
					/*
						update the recall factor
					*/
					dk[i][j] = dkold[i][j] * m_c0f + deltaXold[i][j] * m_c1f;
					
				}
				
				/*
				raise the counter for failed attempts
				*/
				timesNotDecreased[j] ++;
			}

			/*
				scaledown m_bk if there was no improvement for a certain time
			*/
			if(timesNotDecreased[j] >= m_bThresTimesNotDecreased)
			{
				m_bk[j] = m_bk[j] * m_bfac;
				timesNotDecreased[j] = 0;
			}

			/*
				if m_bk < m_bBreak .. 
			*/
			if( m_bk[j]  < m_bBreak )
			{
				/*  */
				if(m_backupActive)
				{
					if(m_CurrentCostFunctionValues[j][0] < m_backupPointValue)
					{
						//m_backupPoint = m_Parameters.Sub(m_numberOfParameters,1,0,j);
						m_backupPoint = m_Parameters(0,j,m_numberOfParameters-1,j);
						m_backupPointValue = m_CurrentCostFunctionValues[j][0];
					}
				}
				else
				{
					//m_backupPoint = m_Parameters.Sub(m_numberOfParameters,1,0,j);
					m_backupPoint = m_Parameters(0,j,m_numberOfParameters-1,j);
					m_backupPointValue = m_CurrentCostFunctionValues[j][0];
					m_backupActive = true;
				}
				
				/*
					reset counters
				*/
				m_bk[j] = m_b0;
				timesNotDecreased[j] = 0;

				matrix_type resProb = m_CurrentCostFunctionValues;
				double maxVal=m_CurrentCostFunctionValues.MaxVal();
				for(int i=0; i < (int)m_numberOfParallelPoints; ++i)
				{
					resProb[i][0] -= maxVal;
				}
				
				double denom = MatrixSum(resProb);
				/*
				ensure numerical stability
				*/
				if( fabs(denom) < 1.0e-50)
				{
					denom = denom < 0.0 ? -1.0e-50 : 1.0e-50;
				}

				resProb = resProb * (1.0/denom);
					
				
				double sum = 0.0;
				for(int u = 0; u < static_cast<int>(m_numberOfParallelPoints); u++)
				{
					sum += resProb[u][0];
					resProb[u][0] = sum;
				}

				/*
					generate random number [0,1]
				*/
				double choice = ice::RandomD();
				int u_chosen = 0;

				for(int u = 0; u < static_cast<int>(m_numberOfParallelPoints); u++)
				{
					u_chosen = u;
					
					if( choice < resProb[u][0] )
					{
						break;
					}
				}

				/*
					set m_parameters 
				*/
				for(int v = 0; v < static_cast<int>(m_numberOfParameters); v++)
				{
					m_Parameters[v][j]=m_Parameters[v][u_chosen];

				}
				
				m_CurrentCostFunctionValues[j][0] = m_CurrentCostFunctionValues[u_chosen][0];
								
			}


			/*
				dk has to be <= D * m_bk
			*/
			//double norm= dk(0,j,m_numberOfParameters-1,j).Norm(0);
			double norm= dk(0,j,m_numberOfParameters-1,j).Norm(0);
			if( norm >= m_D * m_bk[j])
			{
				if(norm < 1.0e-50)
				{
					//m_loger->logWarning("ADRS Optimizer: Warning Computation gets unstable");
					norm = 1.0e-50;
				}
				
				for(i =0;i < static_cast<int>(m_numberOfParameters);i++)
				{
					/*
						update the recall factor
					*/
					dk[i][j] = dk[i][j] * 1.0/norm;
					
				}
			}
		}
		
		if(m_verbose)
		{
			std::cout << "# AdaptiveDirectionRandomSearch :: parameterSet: ";
			for(int l=0;l < static_cast<int>(m_numberOfParallelPoints);l++)
			{
				for(int r = 0; r < static_cast<int>(m_numberOfParameters); r++)
				{
					std::cout<< m_Parameters[r][l] << " ";
				}
				std::cout << m_bk[l] << " "<< 
					m_CurrentCostFunctionValues[l][0]  << std::endl;
			}
			std::cout <<"# "<< std::endl;
		}
		
		
		//	fixme wacker for plotting
		/*
		for(int l=0;l<m_numberOfParallelPoints;l++)
		{
				for(int r = 0; r < 2; r++)
				{
					std::cout<< m_Parameters[r][l] << " ";
				}
				std::cout << m_CurrentCostFunctionValues[l][0]  << std::endl;
		}

*/
		
		/*
			Check if it is in bounds, maxSeconds
		*/
		/*
		if(!checkParameters(m_parameters))
		{
			// set according return status and the last parameters and return 
			m_returnReason = ERROR_XOUTOFBOUNDS;
			abort = true;
		}*/

		/*
			check kind of paramtol
		*/
		if(m_paramTolActive)
		{
			if(m_numberOfParallelPoints > 1 )
			{
				/*
					check if distance from one point to all others is below a the threshold
				*/
				matrix_type paramSet = m_Parameters;
				bool abortNow = true;
				
				for(int e = 0; e < static_cast<int>(m_numberOfParallelPoints);e++)
				{
					if(	(paramSet(0,e,m_numberOfParameters-1,e) 	- paramSet(0,0,m_numberOfParameters-1,0)).Norm(0) > m_paramTol)
					{
						abortNow = false;
					}
				}
				if(abortNow)
				{
					abort = true;
					m_returnReason = SUCCESS_PARAMTOL;
				}
			}

		}


		/*
			Check Optimization Timelimit, if active
		*/
		if(m_maxSecondsActive)
		{
			m_currentTime = clock();

			/* time limit exceeded ? */
			if(((float)(m_currentTime - m_startTime )/CLOCKS_PER_SEC) >= m_maxSeconds )
			{
				/* set according return status and the last parameters and return */
				m_returnReason = SUCCESS_TIMELIMIT;
				abort = true;
				continue;
			}
		}

	}

	
	/*
		find the best value..
	*/
	unsigned int u_best = 0;
	for(int u = 0; u < static_cast<int>(m_numberOfParallelPoints); u++)
	{
		if( m_CurrentCostFunctionValues[u][0] < m_CurrentCostFunctionValues[u_best][0] )
		{
			u_best = u;
		}
		
	}


	/*
		regular points include the best one 
	*/
	//m_parameters = m_Parameters.Sub(m_numberOfParameters,1,0,u_best);
	m_parameters = m_Parameters(0,u_best,m_numberOfParameters-1,u_best);
	m_currentCostFunctionValue = m_CurrentCostFunctionValues[u_best][0];
	
	if (m_backupActive)
	{
		/*
			compare with backup point
		*/
		if( m_backupPointValue < m_CurrentCostFunctionValues[u_best][0] )
		{
			/*
				backup point is best
			*/
			m_parameters = m_backupPoint;
			m_currentCostFunctionValue = m_backupPointValue;
		}
	
	}
	
	delete[] timesNotDecreased;

	return m_returnReason;

}