//////////////////////////////////////////////////////////////////////
//
//	PowellBrentOptimizer.cpp: Implementation of the 
//												ADRS Optimizer
//
//	Written by Matthias Wacker
//
// 
//
//////////////////////////////////////////////////////////////////////

#include "optimization/PowellBrentOptimizer.h"
#include "core/optimization/blackbox/Definitions_core_opt.h"
//#include <iostream>

using namespace OPTIMIZATION;

PowellBrentOptimizer::PowellBrentOptimizer(OptLogBase *loger) : SuperClass(loger)
{
  //xi = matrix_type(m_numberOfParameters, m_numberOfParameters);
  //m_lin = BrentLineSearcher(m_costFunction,loger);
  m_lin_Lower		= -5.0;
  m_lin_Upper		= 5.0;
  m_lin_Eps		= 1.0e-3;//1.0e-2;
}


PowellBrentOptimizer::PowellBrentOptimizer( const PowellBrentOptimizer &opt) : SuperClass(opt)
{
  m_lin= opt.m_lin;
  xi= opt.xi;
  m_lin_Lower= opt.m_lin_Lower;
  m_lin_Upper= opt.m_lin_Upper;
  m_lin_Eps= opt.m_lin_Eps;
  
}

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

      
    /*
      own values:
    */
    m_lin= opt.m_lin;
    xi= opt.xi;
    m_lin_Lower= opt.m_lin_Lower;
    m_lin_Upper= opt.m_lin_Upper;
    m_lin_Eps= opt.m_lin_Eps;
    
  }
    return *this;
}


PowellBrentOptimizer::~PowellBrentOptimizer()
{
}


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

  //xi =0.0;
  xi = matrix_type(m_numberOfParameters, m_numberOfParameters);
  m_lin = BrentLineSearcher(m_costFunction,m_loger);


  for(uint i = 0; i < m_numberOfParameters;i++)
  {
    for(uint j = 0; j < m_numberOfParameters; j++)
    {
      if(i == j)
      {
        //xi(i,j) = 1.0;
        xi(i,j) = 1.0;
      }
    }
  }

  if(m_funcTolActive)
    m_lin.setEps(m_funcTol);

  if(m_maximize == true)
    m_lin.setMaximize(true);

  //if(m_verbose)
  //	m_lin.setVerbose(true);
}


void PowellBrentOptimizer::setLineSearchOptions(double lower, double upper, double eps)
{
  m_lin_Lower= lower;
  m_lin_Upper= upper;
  m_lin_Eps= eps;
  m_lin.setBounds(lower, upper);
  m_lin.setEps(eps);
}


int PowellBrentOptimizer::optimize()
{
  this->init();
  /*
    start time criteria
  */
  m_startTime = clock();

  int n = m_numberOfParameters;
  
  double ftol = m_funcTol;
  double TINY = 1.0e-25;
  
  bool abort = false;
  
  int i,ibig, j;
  double del, fp, fptt, t;
  
  matrix_type pt(m_numberOfParameters,1);
  matrix_type ptt(m_numberOfParameters,1);
  matrix_type xit(m_numberOfParameters,1);
  
  /*
    eval at parameters
  */
  double fret = evaluateCostFunction(m_parameters);
  
  /*
    save the initial point
  */
  pt = m_parameters;

  if(m_verbose)
  {
    for(uint r = 0; r < m_numberOfParameters; r++)
    {
      std::cout<< m_parameters(r,0) << " ";
    }
    
    std::cout<< fret << std::endl;
  }
  
  while(abort == false)
  {
    /*
      Check iteration limit
    */
    if(m_maxNumIterActive)
    {
      if(m_numIter >= m_maxNumIter)
      {
        /* set according return status and return */
        m_returnReason = SUCCESS_MAXITER;
        abort = true;
        continue;
      }
    }
    /*
      increase iteration counter
    */
    ++m_numIter;
    
    fp = fret;
    ibig = 0;
    del = 0.0;
    
    for(i = 0; i < n;i++)
    {
      for(j = 0; j<n;j++)
      {
        //xit(j,0) = xi(j,i) * m_scales(j,0);
        xit(j,0) = xi(j,i) * m_scales(j,0);
      }
      fptt=fret;
      
      /* do a one dimensional line search */
      m_lin.setBounds(m_lin_Lower ,m_lin_Upper);
      m_lin.setEps(m_lin_Eps);
      m_lin.setX0(m_parameters);
      m_lin.setH0(xit);
      double lambda = m_lin.optimize();
      
      fret = m_lin.evaluateSub(lambda);
      xit = xit * lambda;
      
      // if value improved, accept new point
      if(fret < fptt )
      {
        m_parameters= m_parameters + xit;
      }
      
      if(m_verbose)
      {
        for(uint r = 0; r < m_numberOfParameters; r++)
        {
          std::cout<< m_parameters(r,0) << " ";
        }
        
        std::cout <<fret<< std::endl;
      }
      
      
      if(fptt - fret > del)
      {
        del = fptt - fret;
        ibig = i;
      }
    }
  
    // check for improvement -> if there is none, abort iteration!
    if(fret >= fp)
    {
      m_returnReason=SUCCESS_NO_BETTER_POINT; 
      abort=true;
      continue;
    }



    if(m_funcTolActive)
    {
      /*
        a recommended abort criteria
      */
      if(2.0 * (fp - fret ) <= ftol *(fabs(fp) + fabs(fret) ) + TINY)
      {
        m_returnReason = SUCCESS_FUNCTOL;
        abort = true;
        continue;
      }
    }


    /*
      construct extrapolated point and the average direction moved
      save old starting point
    */
    for (j=0; j<n;j++)
    {
      //ptt(j,0)= 2.0*m_parameters(j,0)-pt(j,0);
      //xit(j,0)= m_parameters(j,0)-pt(j,0);
      //pt(j,0) = m_parameters(j,0);
      ptt(j,0)= 2.0*m_parameters(j,0)-pt(j,0);
      xit(j,0)= m_parameters(j,0)-pt(j,0);
      pt(j,0) = m_parameters(j,0);
    }

    fptt = evaluateCostFunction(ptt);

    if(fptt < fp)
    {
      t = 2.0*(fp-2.0*fret+fptt)*sqrt(fp-fret-del) - del *sqrt(fp-fptt);
      if(t < 0.0)
      {
        /* 
          move to minimum of new 
          direction and save the new direction
        */
        m_lin.setBounds(m_lin_Lower,m_lin_Upper);
        m_lin.setX0(m_parameters);
        m_lin.setH0(xit);

        double lambda = m_lin.optimize();
      
        fret = m_lin.evaluateSub(lambda);
        xit= xit*lambda;
        m_parameters= m_parameters+ xit;
        
        // normalize direction
        xit = xit * (1.0 / xit.frobeniusNorm());  
        for(j=0; j < n ;j++)
        {
          xi(j,ibig) = xi(j,n-1);
          xi(j,n-1)= xit(j,0);
        }
      }
    }


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

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

    /*
      Check Bounds if Active
    */
    if(!checkParameters(m_parameters))
    {
      if(m_verbose == true)
      {
        std::cout << "PowellBrent :: aborting because of parameter Bounds" << std::endl;
      }

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

    /*if(m_verbose)
    {
      for(int r = 0; r < m_numberOfParameters; r++)
      {
        std::cout<< m_parameters(r,0) << " ";
      }
      
      std::cout << std::endl;
    }*/
    
  }
  
  
  return m_returnReason;
  
}