#ifndef LINEOPTIMIZERPARABOLA_H
#define LINEOPTIMIZERPARABOLA_H

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

typedef unsigned int const uintc;

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

#include <lineoptimizergold2.h>



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


\par Usage


\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;
  lineoptimizerparabola<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 lineoptimizerparabola : public lineoptimizergold2<LNPATH,T>
{
public:

  typedef lineoptimizergold2<LNPATH,T> lopg;

  /** A small positive number used for zero tests. */ 
  static T zero;

  /** Initialize memory, does no computations. */
  lineoptimizerparabola( LNPATH linepath_) :
    lineoptimizergold2<LNPATH,T>(linepath_) {}

  /** From consecutive indexed points fit a parabola for tnew. */
  bool const parabolamin
  (
    T & tnew,
    uintc a0,
    uintc a1, 
    uintc a2 
  ) const;

  //void convergencetest();

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

};

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

/*
KEEP THIS CODE, even though it failed to deliver a gain.
template< typename LNPATH, typename T >
void lineoptimizerparabola<LNPATH,T>::convergencetest()
{
  T df0 = lopg::fti[lopg::ai[1]] - lopg::fti[lopg::ai[0]];
  df0 *= df0;

  T df3 = lopg::fti[lopg::ai[3]] - lopg::fti[lopg::ai[2]];
  df3 *= df3;

  T factor=40.0;

  // Test for convergence from the left hand side.
  if ( lopg::fti[lopg::ai[2]] < lopg::fti[lopg::ai[1]] )
  {
    if (df0*factor<df3)
    {
      T t = lopg::ti[lopg::ai[2]] + lopg::ti[lopg::ai[1]] - lopg::ti[lopg::ai[0]];
      T ft;
      lopg::linepath.fneval(ft,t);
      if (lopg::fti[lopg::ai[2]] < ft)
      {
        lopg::ti[lopg::ai[3]] = t;
        lopg::fti[lopg::ai[3]] = ft;
cout << "modified" << endl;
      }
    
      return;
    }
  }
}
*/


template< typename LNPATH, typename T >
void lineoptimizerparabola<LNPATH,T>::operator ++ ()
{
  lopg::lengthgold *= lopg::goldratio;
//cout << SHOW(lengthgold) << endl;

  // Minimization
  uint rej;

  T w;

  //printstate();

  // Test - reject x0 if successful.
  if (lopg::fti[lopg::ai[2]]<lopg::fti[lopg::ai[1]])
  {

    // If parabola minimization fails, do two gold section minimizations
    if (parabolamin(w,lopg::ai[1],lopg::ai[2],lopg::ai[3])==false)
    {
cout << "parabolamin failed in x0 rejection." << endl;

      rej = lopg::ai[0];
//cout << "parab min failed, rejecting x0" << endl;
lopg::printstate();
      lopg::ai[0] = lopg::ai[1];
      lopg::ai[1] = rej;
      lopg::resetInnerTwoPoints();

      return;
    }

//cout << "Reject x0" << endl;
    rej = lopg::ai[0];
    lopg::ti[rej] = w;
    lopg::linepath.fneval(lopg::fti[rej],lopg::ti[rej]);
    lopg::ai[0] = lopg::ai[1];

    if (w<lopg::ti[lopg::ai[2]])
      lopg::ai[1] = rej;
    else
    {
      lopg::ai[1] = lopg::ai[2];
      lopg::ai[2] = rej;
    }

    //convergencetest();

    //printstate();
    assert(lopg::verify());
    return;
  }


//cout << "Reject x3" << endl;
  rej = lopg::ai[3];

  // Test - reject x3 if successful.
  if (lopg::fti[lopg::ai[1]]<lopg::fti[lopg::ai[2]])
  {

    // If parabola minimization fails, do two gold section minimizations
    if (parabolamin(w,lopg::ai[0],lopg::ai[1],lopg::ai[2])==false)
    {
cout << "parabolamin failed in x3 rejection." << endl;
//cout << "parab min failed, rejecting x3" << endl;
      rej = lopg::ai[3];

      lopg::ai[3] = lopg::ai[2];
      lopg::ai[2] = rej;
      lopg::resetInnerTwoPoints();

      return;
    }

//cout << "Reject x0" << endl;
    rej = lopg::ai[3];
    lopg::ti[rej] = w;
    lopg::linepath.fneval(lopg::fti[rej],lopg::ti[rej]);
    lopg::ai[3] = lopg::ai[2];

    if (w<lopg::ti[lopg::ai[1]])
    {
      lopg::ai[2] = lopg::ai[1];
      lopg::ai[1] = rej;
    }
    else
    {
      lopg::ai[2] = rej;
    }

    //printstate();
    assert(lopg::verify());
    return;
  }

  // No interval can be rejected because both points are the same height.
  // This is a hack. If the value between 1 and 2 is the same then the method
  // will fail.  
  lopg::ti[lopg::ai[1]] += lopg::ti[ lopg::ai[2] ];
  lopg::ti[lopg::ai[1]] *= (T)0.5;
  lopg::linepath.fneval(lopg::fti[lopg::ai[1]],lopg::ti[lopg::ai[1]]);

}


template< typename LNPATH, typename T >
bool const lineoptimizerparabola<LNPATH,T>::parabolamin
(
  T & tnew, 
  uintc a0,
  uintc a1, 
  uintc a2 
) const
{
// Scale and minimize
// max/min at -b/2a = (-q^2*f2+q^2*f0+f1-f0)/(2*(f1-f0+q(f0-f2))
// where q=(t1-t0)/(t2-t0)
  assert( lopg::ti[a2] != lopg::ti[a0] );

  T f5 = lopg::fti[a1]-lopg::fti[a0];
  T q = (lopg::ti[a1]-lopg::ti[a0])/(lopg::ti[a2]-lopg::ti[a0]);
  T den = (f5+q*(lopg::fti[a0]-lopg::fti[a2]))*2;
  if ( den*den < zero )
    return false;
  T num = q*q*(lopg::fti[a0]-lopg::fti[a2])+f5;
  T w = num/den;
  if (w<=(T)0.0)
    return false;
  if (w>=(T)1.0)
    return false;

  tnew = lopg::ti[a0] + (lopg::ti[a2]-lopg::ti[a0])*w;

  return true;
}


template< typename LNPATH, typename T >
T lineoptimizerparabola<LNPATH,T>::zero = 1E-20;

#endif