#ifndef LINEOPTIMIZERGOLD_H
#define LINEOPTIMIZERGOLD_H

#include <cassert>
#include <iostream>
using namespace std;

typedef unsigned int const uintc;

#include <typeop.h>
#include <print.h>

/*!
  \brief Minimize fn on x0+di*t

  A linear line minimization algorithm.  The golden ratio
  is used to choose the next best interval point.

  The function has been templated as FN.
  The function state has been templated as XI.
  The initial position and direction templated with X type.

  While XI and X are both N dimensional vectors with elements of
  the same type they are different types to support complex functions fn.
  For most applications this generality will not be needed.

\par Usage
  Construct the optimizer.
  Realize the initial position x0 and direction in memory.

  Call reset(...) before executing the minimizer.  This 
  initializes the interval.

  You can reuse the optimizer by writing to the direction and 
  initial position and calling reset.

\par Example
\verbatim
  double d1[] = {0.0,1.0,1.0};
  double x0[] = {0.0,0.0,0.0};
  parab2 fn;
  lineoptimizergold<parab2&,double*,double*,double> 
    opt(3,fn,fn.xi,x0,d1);

  opt.reset(0.0,5.0);
  for (uint i=0; i<10; ++i)
  {
    ++opt;
    opt.printstate();
  }
\endverbatim
*/
template< typename FN, typename XI, typename X, typename T >
class lineoptimizergold
{
public:

  /** Dimension. */
  uintc dim;

  /** The function being minimized. */
  typename typeop<FN>::Tref fn;
  /** The function state. */
  XI xi;
  /** Line constant. */
  X x0;
  /** Direction. */
  X di;

  /** The t values on the line correspond to index positions. */
  T ti[4];

  /** The value of the function at the index position. */
  T fti[4];

  /** Indexes to vectors. */
  uint ai[4];

  /** Interval length*goldratio */
  T lengthgold;

  /** The client could assign the golden ratio themselves if needed. */
  static T goldratio;

  /** Evaluate the function on the line at t. */
  void fneval(T& fval, T const t)
  {
    for (uint i=0; i<dim; ++i)
      xi[i] = x0[i]+di[i]*t;
//cout << "***";
//cout << printvecfunc(xi,dim) << endl;
    fn(fval);
  }

  /* Initialize memory, does no computations. */
  lineoptimizergold
  (
    uintc dim_, 
    typename typeop<FN>::Tref fn_, 
    XI xi_, 
    X x0_, 
    X di_
  );

  /** Printing the objects state. */
  ostream & print(ostream & os) const;

  /** Print out the four consecutive positions and their values. */
  void printstate() const
  {
    cout << "ti:  ";
    for (uint i=0; i<4; ++i)
      cout << ti[ai[i]] << " ";
    cout << endl;
    cout << "fti: ";
    for (uint i=0; i<4; ++i)
      cout << fti[ai[i]] << " ";
    cout << endl;
  }

  /** x0+di*[t0,t1] search interval initialized. */
  void reset( T const t0, T const t1 );

  /** One iteration of the line minimization algorithm. */
  void operator ++ ();

  /** Make sure the ti[ai[]] are consecutive (monotonic) */
  bool const verify() const;

  /** Realize the minimum state by setting xi to the lowest state. */
  T const minimumrealize();

};

//-----------------------------------------------
//  Implementation


template< typename FN, typename XI, typename X, typename T >
void lineoptimizergold<FN,XI,X,T>::reset( T const t0, T const t1 )
{
  assert(t1>t0);

  ti[0] = t0;
  fneval(fti[0],ti[0]);
//cout << SHOW(fti[0]) << endl;
  ti[3] = t1;
  fneval(fti[3],ti[3]);
//cout << SHOW(fti[3]) << endl;

  lengthgold = goldratio*(t1-t0);
  ti[1] = ti[3]-lengthgold;
  fneval(fti[1],ti[1]);
  ti[2] = ti[0]+lengthgold; 
  fneval(fti[2],ti[2]);

  for (uint i=0; i<4; ++i)
    ai[i] = i;

  lengthgold *= goldratio;
}



template< typename FN, typename XI, typename X, typename T >
T const lineoptimizergold<FN,XI,X,T>::minimumrealize()
{
  T t = ti[0];
  T f = fti[0]; 
  for (uint i=1; i<4; ++i)
  {
    if (fti[i]<f)
    {
      t = ti[i];
      f = fti[i];
    }
  }

  for (uint i=0; i<dim; ++i)
    xi[i] = x0[i]+di[i]*t;

  return f;
}

template< typename FN, typename XI, typename X, typename T >
bool const lineoptimizergold<FN,XI,X,T>::verify() const
{
  bool valid=true;

  if ( ti[ai[1]] < ti[ai[0]] )
    valid=false;
  if ( ti[ai[2]] < ti[ai[0]] )
    valid=false;
  if ( ti[ai[3]] < ti[ai[0]] )
    valid=false;

  if ( ti[ai[2]] < ti[ai[1]] )
    valid=false;
  if ( ti[ai[3]] < ti[ai[1]] )
    valid=false;

  if ( ti[ai[3]] < ti[ai[2]] )
    valid=false;

  if (valid==false)
  {
    cout << "error: ti invalid" << endl;
    for (uint i=0; i<4; ++i)
      cout << ti[ai[i]] << " ";
    cout << endl;
  }

  return valid;
}



template< typename FN, typename XI, typename X, typename T >
void lineoptimizergold<FN,XI,X,T>::operator ++ ()
{
  lengthgold *= goldratio;
//cout << SHOW(lengthgold) << endl;

  // Minimization
  uint rej;

  //printstate();

  // Test - reject x0 if successful.
  if (fti[ai[2]]<fti[ai[1]])
  {
//cout << "Reject x0" << endl;
    rej = ai[0];
    ai[0] = ai[1];
    ai[1] = ai[2];
    ai[2] = rej;
    ti[ai[2]] = ti[ai[3]]-lengthgold;
    //printstate();
    assert(verify());
    fneval(fti[ai[2]],ti[ai[2]]);
  }
  else
  {
//cout << "Reject x3" << endl;
    rej = ai[3];
    ai[3] = ai[2];
    ai[2] = ai[1];
    ai[1] = rej;
    ti[ai[1]] = ti[ai[0]]+lengthgold;
    //printstate();
    assert(verify());
    fneval(fti[ai[1]],ti[ai[1]]);
  }

}

template< typename FN, typename XI, typename X, typename T >
lineoptimizergold<FN,XI,X,T>::lineoptimizergold
(
  uintc dim_, 
  typename typeop<FN>::Tref fn_, 
  XI xi_, 
  X x0_, 
  X di_ 
)
  : dim(dim_), fn(fn_), xi(xi_), x0(x0_), di(di_)
{
}

template< typename FN, typename XI, typename X, typename T >
T lineoptimizergold<FN,XI,X,T>::goldratio = 0.61803398875;

template< typename FN, typename XI, typename X, typename T >
ostream & lineoptimizergold<FN,XI,X,T>::print(ostream & os) const
{
  os << "dim=" << dim << endl;
  //os << "xi[]=" << printvecfunc(xi,dim) << endl;
  os << "xi[]=" << print(xi,xi+dim) << endl;
  //os << "x0[]=" << printvecfunc(x0,dim) << endl;
  os << "x0[]=" << print(x0,x0+dim) << endl;
  //os << "di[]=" << printvecfunc(di,dim) << endl;
  os << "di[]=" << print(di,di+dim) << endl;
  //os << "ti[]=" << printvecfunc(ti,4) << endl; 
  os << "ti[]=" << print(ti,ti+4) << endl; 
  //os << "fti[]=" << printvecfunc(fti,4) << endl; 
  os << "fti[]=" << print(fti,fti+4) << endl; 
  //os << "ai[]=" << printvecfunc(ai,4) << endl; 
  os << "ai[]=" << print(ai,ai+4) << endl; 
  os << SHOW(goldratio) << endl;

  return os;
}

#endif