//////////////////////////////////////////////////////////////////////
//
//	ArmijoLineSearcher.cpp: Implementation of the Brent LineSearcher class.
//
//	Written by Matthias Wacker
//
//	Big parts of code are from numerical recipes in C
//
//////////////////////////////////////////////////////////////////////

#include "optimization/ArmijoLineSearcher.h"
#include <iostream>

using namespace OPTIMIZATION;

ArmijoLineSearcher::ArmijoLineSearcher() : m_dGamma(0.7), m_dL(1.0),m_dSigma(0.33)
{
}

ArmijoLineSearcher::ArmijoLineSearcher(CostFunction *costFunc,OptLogBase* loger) : SuperClass(costFunc,loger),m_dGamma(0.5), m_dL(1.0),m_dSigma(0.33)
{
}

ArmijoLineSearcher::ArmijoLineSearcher(const ArmijoLineSearcher &lin) : SuperClass(lin)
{
	m_dGamma = lin.m_dGamma;
	m_dL = lin.m_dL;
	m_dSigma = lin.m_dSigma;
}

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

		m_dGamma = lin.m_dGamma;
		m_dL = lin.m_dL;
		m_dSigma = lin.m_dSigma;
	
	}

	return *this;

}

ArmijoLineSearcher::~ArmijoLineSearcher()
{
}

void ArmijoLineSearcher::setGammaLSigma(double gamma, double L, double sigma)
{
	m_dGamma=gamma;
	m_dL=L;
	m_dSigma=sigma;
}

void ArmijoLineSearcher::setXkGkDk(const matrix_type xk, const matrix_type gk, const matrix_type dk)
{
	this->setX0(xk);
	this->setH0(dk);

	m_gk=gk;
	m_dk=dk;
}

double ArmijoLineSearcher::optimize()
{
	double x=0.0;
  double ndk2=m_dk.frobeniusNorm();ndk2*=ndk2;

  double fk= evaluateSub(0.0);
double tmp= (!m_gk * m_dk)(0,0);
  double sk= - tmp / (m_dL * ndk2 );
  x=sk;
  int maxIter=30;
  int iter=0;
  // own rule 
  double fk_old=fk;
  double x_old=0.0;
  double fk_new;
  bool improved = false;
  
  while(  iter < maxIter)
  {
    fk_new=evaluateSub(x);

    if ( fk_new > fk_old)
    {
      x=x_old;
      //cout << "loop 1: end " << endl;
      break;
    }
    
    //cout << "loop 1: iter "<< iter << " " << x << " " << fk_new<<endl;
    fk_old = fk_new;
    x_old = x;
    x/=m_dGamma;
    ++iter;
  }
  if( iter <= 1 )
  {
    iter=0;
    fk_old=fk;
    x_old=0.0;
    x=sk;
    while( iter < maxIter)
    {
      fk_new=evaluateSub(x);
  
      // fk_new is worse than old, and we are lower than fk
      if( fk_new > fk_old && improved==true)
      {
        // abort with fk_old which was generated by x_old
        x = x_old;
        break;
      }
    
          
      //cout << "loop 2: iter "<< iter << " " << x << " " << fk_new<<endl;
      fk_old=fk_new;
      x_old=x;
      x*=m_dGamma;
      ++iter;
      if( fk_old < fk)
      {
        improved = true;
      }
    }
  
  }
  // own rule till here

  // original implementation 
  //iter=0.0;
  //while( fk - evaluateSub(x) < - m_dSigma * x *tmp  && iter < maxIter)
  //{
  //	x*=m_dGamma;
  //	iter++;
  //}


//	if(m_pLoger)
//		m_pLoger->logWarning("Brent took too many iterations.. Tol threshold was to low for the number of iterations..\n");

  return x;
}