/**
*
*	@file:		DownhillSimplexOptimizer.cpp: implementation of the downhill Simplex Optimier
*
*	@author:	Matthias Wacker, Esther Platzer
*
*/

#include "optimization/DownhillSimplexOptimizer.h"
#include "optimization/Opt_Namespace.h"
#include "mateigen.h" // for SVD

#include <iostream>

using namespace optimization;

DownhillSimplexOptimizer::DownhillSimplexOptimizer(OptLogBase *loger): SuperClass(loger)
{
	m_abort			= false;
	m_simplexInitialized	= false;
	m_alpha			= 1.0;
	m_beta			= 0.5;
	m_gamma			= 1.0;
	m_rankdeficiencythresh= 0.01;
	m_rankcheckenabled= false;
}

DownhillSimplexOptimizer::DownhillSimplexOptimizer(const DownhillSimplexOptimizer &opt) : SuperClass(opt)
{
	m_abort					= opt.m_abort;
	m_simplexInitialized	= opt.m_simplexInitialized;
	m_vertices				= opt.m_vertices;
	m_y						= opt.m_y;
	m_alpha					= opt.m_alpha;
	m_beta					= opt.m_beta;
	m_gamma					= opt.m_gamma;
	m_rankdeficiencythresh= 0.01;
	m_rankcheckenabled= false;
}

DownhillSimplexOptimizer::~DownhillSimplexOptimizer()
{
}

bool DownhillSimplexOptimizer::setWholeSimplex(const matrix_type &simplex)
{
	if(simplex.rows() == static_cast<int>(m_numberOfParameters) && simplex.cols() == static_cast<int>(m_numberOfParameters + 1))
	{
		/*
		*	Check if simplex has rank(simplex)=numberOfParameters
		*	otherwise return false because of bad posed simplex 
		*/
//#ifdef OPTIMIZATION_DOWNHILLSIMPLEX_ENABLE_RANK_CHECK
        if(m_rankcheckenabled)
        {
            int rank = getRank(simplex, m_rankdeficiencythresh);
            
            if(rank < static_cast<int>(m_numberOfParameters))
            {
                m_simplexInitialized = false;
                return false;
            }
        }
//#endif

		m_vertices = simplex;
		m_simplexInitialized = true;
		
        for (int k = 0; k < static_cast<int>(m_numberOfParameters +1); k++)
        {
            m_y[k][0] = evaluateCostFunction(m_vertices(0,k,m_numberOfParameters-1,k));
        }
        
		return true;
	}
	else
	{
		m_simplexInitialized = false;
		return false;
	}
}

void DownhillSimplexOptimizer::init()
{
	SuperClass::init();
	
	// allocate matrices
	m_vertices		= matrix_type(m_numberOfParameters, m_numberOfParameters + 1);
	m_y			= matrix_type(m_numberOfParameters + 1,1);


	m_abort=false;
	m_simplexInitialized == false;// force a random initialization
	
	/*
		compute the function values corresponding to the simplex
	*/
	/* do a random initialization */
	if (m_simplexInitialized == false)
	{
		bool abort = false;
		while(abort == false)
		{
			matrix_type simplex(m_numberOfParameters,m_numberOfParameters+1);
			
			//	random initialization of the simplex
			for(int i = 0; i < static_cast<int>(m_numberOfParameters); i++)
			{
				for(int j = 0; j < static_cast<int>(m_numberOfParameters+1); j++)
				{
					simplex[i][j] = m_parameters[i][0];
					
					if( j == i+1 )
					{
						double tmpRand = m_scales[i][0];// * ((double)rand()) / RAND_MAX;
						simplex[i][j] += tmpRand;		
					}
				}
			}
			if(this->setWholeSimplex(simplex) == true)
			{
				/* simplexInitializen is only to indicate manual initialization .. */
				m_simplexInitialized = false;
				abort =true;
			}
		}
	}

	for (int k = 0; k < static_cast<int>(m_numberOfParameters +1); k++)
	{
		m_y[k][0] = evaluateCostFunction(m_vertices(0,k,m_numberOfParameters-1,k));
	}
}


int DownhillSimplexOptimizer::optimize()
{
    //if(m_simplexInitialized == false)
    //{
       this->init();
    //}
  
   	if(m_loger)
		m_loger->logTrace("Starting Downhill Simplex Optimization\n");

	int tmp=amoeba();
	m_parameters = m_vertices(0,tmp,m_numberOfParameters-1,tmp);
	
	m_currentCostFunctionValue = evaluateCostFunction(m_parameters);
	return m_returnReason;
}


/*
*	Numerical Recipies in C
*/
int DownhillSimplexOptimizer::amoeba()
{
	
	double tol =m_funcTol;

    const int ndim=m_numberOfParameters;     	// dimension
    //int max_not_improve=10*ndim;				// maximum number of not 
                                                // successful steps 
    //int count_not_improve=0;
    //double last_best_value=1e99;	
 
	const int spts=ndim+1;						// number of vertices
    int i,j, max_val;
    
    int ilo; 							    	// index of lowest vertex-point
    int ihi;									// index of hightest (best)
                                                // vertex-point
    int inhi; 									// index of next hightest 
                                                // vertex-point
    double rtol,ytry;
    
	double ysave;								// save function value 
    matrix_type psum(1,ndim);

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

    
    for (j=0;j<ndim;j++)
	{ 											// get sum of vertex-coordinates
		double sum=0.0;
		
		for (i=0;i<spts;i++)
			sum += m_vertices[j][i];
		psum[0][j]=sum;
    }
    
    for (;;)
	{											// loop until terminating
		if(m_verbose)
		{
			for(int u = 0; u < ndim+1; u++)
			{
				for(int v = 0; v < ndim ; v++)
				{
					std::cout << m_vertices[v][u] << " ";
				}
				std::cout<< " " << m_y[u][0] << std::endl;
			}
			std::cout << std::endl;
		}


		ilo = 0;    								
		// first we must determine 
		// which point is highest, 
		// next-highest
		// and lowest
		// by looping over the simplex

		/*
			this works only, if m_numberOfParameters=ndim >= 2
		*/
		if(ndim >= 2)
		{
			ihi = m_y[1][0]>m_y[2][0] ? (inhi=1,2) : (inhi=2,1); 	 
			for (i=0;i<spts;i++)
			{					 	               		
				if (m_y[i][0] <= m_y[ilo][0]) ilo=i;
				if (m_y[i][0] > m_y[ihi][0]) {
					inhi=ihi;
					ihi=i;
				}
				else
					if (m_y[i][0] > m_y[inhi][0])
						if (i != ihi) 
							inhi=i;
			}
		}
		else
		{
			if(m_y[0][0]>m_y[1][0])
			{
				ilo  = 1;
				inhi = 1;
				ihi  = 0;
			}
			else
			{
				ilo  = 0;
				inhi = 0;
				ihi  = 1;
			}
		}
		
		// log parameters if parameter logger is given (does nothing for other loggers!)
        if(m_loger)
        {
          // extract optimal parameters from simplex
          matrix_type optparams(m_vertices.rows(),1,0);

          for(int i= 0; i< m_vertices.rows(); ++i)
          {
              optparams[i][0]= m_vertices[i][ilo];
          }
          matrix_type fullparams= m_costFunction->getFullParamsFromSubParams(optparams);
          m_loger->writeParamsToFile(fullparams);
        }

//#define DEBUGESTHER
//#ifdef DEBUGESTHER
//           cout<<"Iteration: "<<m_numIter<<" ";
//            for(unsigned int paramnum= 0; paramnum< m_numberOfParameters; paramnum++)
//            {
//                cout<<m_vertices[paramnum][ilo]<<" ";
//            }
//            cout<<endl;
//			//std::cout<<"---------- ihi inhi ilo ---------- " <<ihi << " "<<inhi << " "<<ilo << " " << std::endl;
//#endif
		
		/*
			Check m_abort .. outofbounds may have occurred in amotry() last iteration
		*/
		if(m_abort == true)
		{
			break;
		}

		/*
			Check time criterion
		*/
		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 end optimization */
				m_returnReason = SUCCESS_TIMELIMIT;
				break;
			}
		}

		
		/*
			check functol criterion
		*/
		if(m_funcTolActive == true)
		{
			rtol=2.0*fabs(m_y[ihi][0]-m_y[ilo][0])/(fabs(m_y[ihi][0])+fabs(m_y[ilo][0])+1e-10);
												// compute the fractional 
			// range from highest to lowest
			
			#ifdef OPT_DEBUG        
				std::cout<<"rtol"<<"  "<<rtol<< std::endl;
			#endif    
			
			if (rtol<tol)  
			{  										// if satisfactory (terminating
				//  criterion)
				max_val=(int)m_y[ilo][0];				// save lowest value 
				m_returnReason = SUCCESS_FUNCTOL;
				break;          							// return
			}
		}
		
		/*
			check param tol criterion
		*/
		if (m_paramTolActive == true)
		{
			// get norm of
			//matrix_type tmp = m_vertices(0,ihi,m_numberOfParameters-1,ihi) - m_vertices(0,ilo,m_numberOfParameters,ilo);
			//double norm)

			if (   (m_vertices(0,ihi,m_numberOfParameters-1,ihi) - 
				m_vertices(0,ilo,m_numberOfParameters-1,ilo)).Norm(0) < m_paramTol)
			{
				/*
					set according return reason and end optimization
				*/
				m_returnReason = SUCCESS_PARAMTOL;
				break;
			}
		}
		
		
		
		m_numIter++;

		
		/*
			check max num iter criterion
		*/
		if(m_maxNumIterActive == true)
		{
			if (m_numIter >= m_maxNumIter)
			{										
				//max_val=(int)m_y[ilo][0];
				m_returnReason = SUCCESS_MAXITER;
				break;
			}
		}
        
		
													// Begin a new iteration. 
													// First extrapolate by the 
													// factor alpha through the 
													// face of the simplex across 
													// from the high point, i.e. 
													// reflect the simplex from the
													// high point:
		ytry=amotry(psum,ihi,-m_alpha);
		if (ytry < m_y[ilo][0])
		{
			
							 						// result is better than best
													// point, try additional 
													// extrapolation
			ytry=amotry(psum,ihi,m_gamma);
			
			#ifdef OPT_DEBUG        
				std::cout<<"Case one .. reflected highest through simplex" << std::endl;
			#endif   

		}
		else
		{
		
			if (ytry >= m_y[inhi][0]) 
			{ 			
				
														// The reflected point is worse
														// than the second
				ysave=m_y[ihi][0];        					// highest, so look for 
														// intermediate lower point.
				
				ytry=amotry(psum,ihi,m_beta);
				#ifdef OPT_DEBUG        
					std::cout<<"Case two .. looking for intermediate point" << std::endl;
				#endif   
				if (ytry >= ysave)
				{					 					// Can't seem to get rid 
														// of that high point. Better
					#ifdef OPT_DEBUG        
						std::cout<<"Case three .. contract around lowest point" << std::endl;
					#endif 


					for (i=0;i<spts;i++) 
					{						 			//  contract around lowest 
						// (best) point 
						if (i!=ilo) 
						{
							for (j=0;j<ndim;j++) 
							{
								psum[0][j]=0.5*(m_vertices[j][i]+m_vertices[j][ilo]); 
								#ifdef OPT_DEBUG                    
								printf("psum(%d)=%f\n",j,psum[0][j]);
								#endif                  
								m_vertices[j][i]=psum[0][j];
							}
							if (checkParameters(!psum)) 
							{
								m_y[i][0]=	evaluateCostFunction(!psum); 
								//eval_count++; 				// count function evaluations
							}
							else
							{    
								m_returnReason = ERROR_XOUTOFBOUNDS; // out of domain !!!
								break;
							}
						}
						for (j=0;j<ndim;j++)
						{							 				// get sum of vertex-coordinates
							double sum=0.0;
							for (int ii=0;ii<spts;ii++)
								sum += m_vertices[j][ii];
							psum[0][j]=sum;
						}//for
					}//for								
				}//if (ytry >= ysave)
			}//if (ytry >= m_y(inhi))
		}//else
						
    }											// next iteration
    
    return ilo; 								// return index of best point
}


double DownhillSimplexOptimizer::amotry(matrix_type & psum, int ihi, double fac)
{
    // extrapolates by a factor fac through the face of the simplex across
    // from the low point, tries it, and replaces the low point if
    // the new point is better
    
    const double maxreal= (m_maximize == true)?	-1.0e+300 : 1.0e+300;
    double fac1,fac2,ytry;
    int ndim=m_numberOfParameters;
    matrix_type ptry(1,ndim);
    fac1=(1.0-fac)/ndim;
    fac2=fac1-fac;
    
    for (int j=0;j<ndim;j++) 
    ptry[0][j]=psum[0][j]*fac1-m_vertices[j][ihi]*fac2;
    
    if (checkParameters(!ptry)) 
	{ 
		ytry=evaluateCostFunction(!ptry);	// evaluate function at the 
														// trial point
//		eval_count++;	    							// count function evaluations 
		if (ytry<m_y[ihi][0])  {	    					// if trial point is better
														// than lowest point,
			m_y[ihi][0]=ytry;	    						// then replace the lowest
			for (int j=0; j<ndim;j++) {
				psum[0][j] = psum[0][j] + ptry[0][j]-m_vertices[j][ihi]; 	// update psum
				m_vertices[j][ihi]=ptry[0][j];		  	// replace lowest point
			}
		}
    }
    else 
	{
		ytry=maxreal;  								// out of domain
		m_abort = true;
		m_returnReason = ERROR_XOUTOFBOUNDS;
    
	}
    return ytry;
}


void DownhillSimplexOptimizer::setDownhillParams(double alpha, double beta, double gamma)
{

	m_alpha = alpha;
	m_beta = beta;
	m_gamma = gamma;
}


void DownhillSimplexOptimizer::setRankDeficiencyThresh(float rankdeficiencythresh)
{
    m_rankdeficiencythresh= rankdeficiencythresh;
}


void DownhillSimplexOptimizer::setRankCheckStatus(bool status)
{
    m_rankcheckenabled= status;
}


bool DownhillSimplexOptimizer::getRankCheckStatus()
{
    return m_rankcheckenabled;
}


unsigned int getRank(const matrix_type &A,double numZero)
{
	unsigned int tmpCount = 0;
	matrix_type U,s,Vt;

	//std::cout << "A.rows " << A.rows() << "A.cols() " << A.cols() << std::endl;
	if(A.rows() < A.cols())
	{
		SingularValueDcmp(!A, U, s, Vt);			// call of the singular value decomp.
	}
	else
	{
		SingularValueDcmp(A, U, s, Vt);			// call of the singular value decomp.
	}
	
	for(int i= 0; i < s.rows();i++)		// count singular values > numZero
	{
		if( s[i][i] > numZero )
		{
			tmpCount++;
		}
	}
	
	return tmpCount;
}