#ifndef LINEOPTIMIZERGOLD2_H
#define LINEOPTIMIZERGOLD2_H

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

typedef unsigned int const uintc;

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

/*!
  \brief Minimize fn on a 1D path in N dimensional space.

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

\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;
  typedef linepathd1<parab2&,double*,double*,double> lpth;
  lineoptimizergold2<lpth,double> 
    opt( lpth(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 LNPATH, typename T >
class lineoptimizergold2
{
public:

  /** This is a path in 1D though N dimensional space. */
  LNPATH linepath;

  /** 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;

  /* Initialize memory, does no computations. */
  lineoptimizergold2( LNPATH linepath_) :
    linepath(linepath_) {}

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

  /** Print out the four consecutive positions and their values. */
  void printstate() const;

  /** Search interval initialized. */
  void reset( T const t0, T const t1 );

  /** Do not re-evaluate the end points. Generally for derived classes. */
  void resetInnerTwoPoints();

  /** 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 LNPATH, typename T >
void lineoptimizergold2<LNPATH,T>::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;
}

template< typename LNPATH, typename T >
void lineoptimizergold2<LNPATH,T>::resetInnerTwoPoints()
{
  lengthgold = goldratio*(ti[ai[3]]-ti[ai[0]]);
  ti[ai[1]] = ti[ai[3]]-lengthgold;
  linepath.eval(fti[ai[1]],ti[ai[1]]);
  ti[ai[2]] = ti[ai[0]]+lengthgold; 
  linepath.eval(fti[ai[2]],ti[ai[2]]);

  lengthgold *= goldratio;
}


template< typename LNPATH, typename T >
void lineoptimizergold2<LNPATH,T>::reset( T const t0, T const t1 )
{
  assert(t1>t0);

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

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

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

  lengthgold *= goldratio;
}



template< typename LNPATH, typename T >
T const lineoptimizergold2<LNPATH,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];
    }
  }

  linepath.eval(t);

  return f;
}

template< typename LNPATH, typename T >
bool const lineoptimizergold2<LNPATH,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 LNPATH, typename T >
void lineoptimizergold2<LNPATH,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;
    assert(verify());
    linepath.fn(fti[ai[2]],ti[ai[2]]);

/*
    ti[ai[1]] = ti[ai[3]]-lengthgold;
    linepath.fneval(fti[ai[1]],ti[ai[1]]);
*/
  }
  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());
    linepath.fn(fti[ai[1]],ti[ai[1]]);
  }

}

template< typename LNPATH, typename T >
T lineoptimizergold2<LNPATH,T>::goldratio = 0.61803398875;

template< typename LNPATH, typename T >
ostream & lineoptimizergold2<LNPATH,T>::print(ostream & os) const
{
  //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