#include "sopnamsp.h"
#include "simplex.h"
#include "ntuple.h"
#include <math.h>

#include "timing.h"

//---------------------------------------------------------------
//-------------------  Classe   MinZFunction  -------------------
//---------------------------------------------------------------
// Interface de classe de function multivariable pour le SimplexMinmizer
/*!
  \class SOPHYA::MinZFunction
  \ingroup NTools
  Interface definition for a function object f(x[]) for which MinZSimplex can 
  search the minimum.
  The pure virtual method Value() should be implemented by the derived classes.
*/

MinZFunction::MinZFunction(unsigned int nvar)
  : mNVar(nvar)
{
}

MinZFunction::~MinZFunction()
{
}

//---------------------------------------------------------------
//-------------------  Classe   MinZFuncXi2  --------------------
//---------------------------------------------------------------
/*!
  \class SOPHYA::MinZXi2
  \ingroup NTools
  Implements the MinZFunction interface using a xi2 calculator 
  \sa GeneralXi2 GeneralFitData
*/
MinZFuncXi2::MinZFuncXi2(GeneralXi2* gxi2, GeneralFitData* gd)
  : mGXi2(gxi2) , mGData(gd), MinZFunction(gxi2->NPar())
{
}

MinZFuncXi2::~MinZFuncXi2()
{
}

double MinZFuncXi2::Value(double const xp[])
{
  int ndataused;
  return mGXi2->Value(*mGData, const_cast<double *>(xp), ndataused);
}

//---------------------------------------------------------------
//-------------------  Classe  MinZTestFunc   -------------------
//---------------------------------------------------------------
class MinZTestFunc : public  MinZFunction {
public:
  MinZTestFunc(int sel);
  virtual double Value(double const xp[]);
  string  ToString();
  Vector  OptParms();
protected:
  static int ISelToNvar(int isel);
  int mSel;
};

int MinZTestFunc::ISelToNvar(int isel)
{
  if (isel == 0) return 1;
  if (isel == 1) return 1;
  else if (isel == 2) return 1;
  else if (isel == 3) return 2;
  else if (isel == 4) return 3;
  else return 1;
}

MinZTestFunc::MinZTestFunc(int sel)
  : MinZFunction(ISelToNvar(sel)) 
{
  if ((sel < 0) || (sel > 4)) sel = 0;
  mSel = sel;
}

string MinZTestFunc::ToString()
{
  string rs;
  if (mSel == 0) {
    rs = "-x+(x-2)^2"; 
  }
  else if (mSel == 1) {
    rs = "0.1*x^2-3exp(-(x-2)^2)-5*exp(-0.5*(x+3)^2)";
  }
  else if (mSel == 2) {
    rs = "0.1*x^2-3exp(-(x-2)^2)+5*exp(-0.5*(x+3)^2)";
  }
  else if (mSel == 3) {
    rs = "1.3*(x-50.35)^2+25*(y+3.14)^2";
  }
  else if (mSel == 4) {
    rs = "(x-2.2)^2+2.*(y+3.6)^2+3.*(z-1.1)^2";
  }
  else rs = "????";
  return  rs;
}

Vector MinZTestFunc::OptParms()
{
  Vector xx;
  if (mSel == 0) {
    Vector rv(1);
    rv = 2.5;
    return rv;
  }
  else if (mSel == 1) {
    Vector rv(1);
    rv = -2.883;
    return rv;
  }
  else if (mSel == 2) {
    Vector rv(1);
    rv = 1.812;
    return rv;
  }
  else if (mSel == 3) {
    Vector rv(2);
    rv(0) = 50.35;
    rv(1) = -3.14;
    return rv;
  }
  else if (mSel == 4) {
    Vector rv(3);
    rv(0) = 2.2;
    rv(1) = -3.6;
    rv(2) = 1.1;
    return rv;
  }
  else xx = 0.;
  return xx ;
}


double MinZTestFunc::Value(double const xp[])
{
  double retval = 0;
  if (mSel == 0) {
    double x = xp[0];
    retval = -x+(x-2.)*(x-2.);
  }
  else if ((mSel == 1) || (mSel == 2)) {
    double x = xp[0];
    retval = 0.1*x*x;
    x = xp[0]-2.;
    x = x*x;
    retval -= 3*exp(-x);
    x = xp[0]+3.;
    x = 0.5*x*x;
    if (mSel == 1) retval -= 5*exp(-x);
    else retval += 5*exp(-x);
  }
  else if (mSel == 3) {
    double x = xp[0]-50.35;
    double y = xp[1]+3.14;
    retval = 1.3*x*x+25.*y*y;
  }
  else if (mSel == 4) {
    double x = xp[0]-2.2;
    double y = xp[1]+3.6;
    double z = xp[2]-1.1;
    retval = x*x+2.*y*y+3.*z*z;
  }
  else retval = 0.;
  return retval;
}

//---------------------------------------------------------------
//-------------------  Classe   MinZSimplex  --------------------
//---------------------------------------------------------------
string __Vec2Str4MinZ_AutoTest(Vector& xx) 
{
  string rs;
  char buff[32];
  for(int i=0; i<xx.Size(); i++) {
    sprintf(buff," %g " , xx(i));
    rs += buff;
  }
  return rs;
}

/*!
  \class SOPHYA::MinZSimplex
  \ingroup NTools
  This class implements non linear minimization (optimization)
  in a multidimensional space following the \b Simplex method.
  A \b Simplex is a geometrical figure made of N+1 points in a
  N-dimensional space. (triangle in a plane, tetrahedron in 3-d space).
  The minimization method implemented in this class is based on the 
  algorithm described in "Numerical Recipes, Chapter X". 

  The algorithm has been slightly enhanced :
  - More complex convergence / stop test 
  - A new transformation of the simplex has been included (ExpandHigh)

  For each step, on of the following geometrical transform is performed
  on the Simplex figure:
  - Reflection : reflection away from the high point (expansion by factor Alpha)
  - ReflecExpand : reflection way from the high point and expansion by factor Beta2
  - ContractHigh : Contraction along the high point (factor Beta)
  - ContractLow : Contraction toward the low point (factor Beta2)
  - ExpandHigh : Expansion along the high point

  \sa GeneralFit

  The following sample code shows a usage example:
  \code
  include "simplex.h"
  ...
  // Define our function to be minimized:
  class MySFunc : public MinZFunction {
  public:
    MySFunc() : MinZFunction(2) {}
    virtual double Value(double const xp[]) 
    { return (xp[0]*xp[0]+2*xp[1]*xp[1]); }
  };

  ... 

  MySFunc mysf;
  MinZSimplex simplex(&mysf);
  // Guess the center and step for constructing the initial simplex
  Vector x0(2); x0 = 1.;
  Vector step(2); step = 2.;
  simplex.SetInitialPoint(x0);
  simplex.SetInitialStep(step);
  Vector oparm(2);
  int rc = simplex.Minimize(oparm);
  if (rc != 0) {
    string srt; 
    int sr = simplex.StopReason(srt);
    cout << " Convergence Pb, StopReason= " << sr << " : " << srt << endl;
    }
  else {
    cout << " Converged: NStep= " << simplex.NbIter() 
         << " OParm= " << oparm << endl;
    }
  \endcode
*/

/*!
  \brief Auto test function
  \param tsel : select autotest (0,1,2,3,4)  , tsel<0 -> all
  \param prtlev : printlevel 
*/
int MinZSimplex::AutoTest(int tsel, int prtlev)
{
  int rc = 0;
  cout << " --- MinZSimplex::AutoTest() --- TSel= " << tsel << " PrtLev=" << prtlev << endl;
  for(int i=0; i<5; i++) {
    if ((tsel >= 0) && (tsel != i))  continue; 
    cout << " ======= Test avec ISel= " << i;
    Vector xx;
    MinZTestFunc mzf(i);
    cout << " - Func= " << mzf.ToString() << endl;
    Vector rv = mzf.OptParms();
    xx = rv;
    for(int j=0; j<2; j++) {
      double vi = 50.*(j-0.5);
      for(int k=0; k<2; k++) {
	double vs = (k == 0) ? 1. : 10. ;
	cout << "--[" << j << "," << k 
	     << "] Initialisation avec IniPoint= " << vi << " IniStep= " << vs << endl;
	MinZSimplex simplex(&mzf);
	xx = vi;
	simplex.SetInitialPoint(xx);
	xx = vs;
	simplex.SetInitialStep(xx);
	simplex.SetPrtLevel(prtlev);
	int rcs = simplex.Minimize(xx);
	Vector diff = rv-xx;
	double d2 = diff.Norm2();
	cout << " Rc(simplex.Minimize() = " << rc << " NIter= " 
	     << simplex.NbIter() << " ===> Distance^2= " << d2
	     << "\nConverged to " <<  __Vec2Str4MinZ_AutoTest(xx) 
	     << "  Best Value= " << __Vec2Str4MinZ_AutoTest(rv) 
	     << "  Diff = " << __Vec2Str4MinZ_AutoTest(diff) << endl;
	if ((rcs > 5) || (d2 > 0.5))  rc ++;
      }
    }
  }
  cout << " --- MinZSimplex::AutoTest() --- Rc=" << rc << " -- END ----- " << endl;
  return rc;
}

//! Constructor from pointer to MinZFunction object 
MinZSimplex::MinZSimplex(MinZFunction *mzf)
  : mZF(mzf) , mPoint0(mZF->NVar()) , mStep0(mZF->NVar())
{
  SetMaxIter();
  SetControls();
  Vector xx(NDim());
  xx = 0.;
  SetInitialPoint(xx);
  xx = 1.0;
  SetInitialStep(xx);
  SetStopTolerance();
  mIter = -1;
  mStop = -1;
  SetPrtLevel();
}

MinZSimplex::~MinZSimplex()
{
}

//! Perform the minimization 
/*!
  Return 0 if success
  \param fpoint : vector containing the optimal point 
  
  Convergence test :
  \verbatim
  On minimise f(x) f=mZF->Value() , 
              f_max = max(f) sur simplex , f_min = min(f) sur simplex 
              fm = (abs(f_max)+abs(f_min))
              [Delta f] = abs(f_max-f_min)
              [Delta f/f]simplex = 2.*Delta f / fm
              fm2 = (abs(f_max)+abs(f_max(iter-1)))
              [Delta f_max/f_max]iter = [f_max(iter-1)-f_max]/fm2 
  Test d'arret :  
   fm < mTol0                                                       OU 
   [Delta f/f]simplex              < mTol1   mRep1 fois de suite    OU 
   [Delta f_max/f_max]iter         < mTol2   mRep2 fois de suite  
*/
int MinZSimplex::Minimize(Vector& fpoint)
{
  // vector< TVector<r_8> > splx;
  Vector splx[100];
  Vector Y(NDim()+1);
  // On calcule le simplex initial
  // N = NDim, N+1 points (pp) ds l'espace a N dimensions
  // Point0, Point0 + Step0(i) e_i  
  Vector pp,ppc;
  pp = mPoint0;
  //ppc = pp;
  //splx.push_back(ppc);
  splx[0] = pp;
  int i,j,k;
  for(i=0; i<NDim(); i++) {
    Vector pps;
    pps = mPoint0;
    pps(i) += mStep0(i);
    //splx.push_back(pps);    
    splx[i+1] = pps;
  }
  int mpts = NDim()+1;
  // calcul des valeurs de la fonction sur les sommets
  for(i=0; i<mpts; i++) 
    Y(i) = Value(splx[i]); 

  int iter = 0;
  mIter = iter;
  mStop = 0;

  int nbugrtol2 = 0;
  bool stop = false, stop0=false;      
  int rc = 0;
  int ilo, ihi, inhi;
  int move = 0;
  char* smov[6] = { "None", "Reflection", "ReflecExpand", "ContractHigh", "ContractLow", "ExpandHigh" };
  int movcnt[6] = {0,0,0,0,0,0};

  int nrep1=0, nrep2=0;
  FindMinMax12(Y, ilo, ihi, inhi);
  double yhilast = Y(ihi);
  yhilast += fabs(yhilast); 

  while (!stop) {  // 
    FindMinMax12(Y, ilo, ihi, inhi);
    double ymean = (fabs(Y(ihi))+fabs(Y(ilo)));
    if (ymean < mTol0) { stop0 = true; ymean = mTol0; }
    double rtol1 = 2.*fabs(Y(ihi)-Y(ilo))/ymean;
    double ym2 = (fabs(yhilast)+fabs(Y(ihi)));
    if (ym2 < mTol0) ym2 = mTol0; 
    double rtol2 = 2.*(yhilast-Y(ihi))/ym2;
    yhilast = Y(ihi);
    if (rtol2 < 0.) {
      if (move != 40) { 
	cout << " !!!! MinZSimplex::Minimize() BUG RTol2< 0. --> Chs " << endl;
	nbugrtol2++;
      }
      else nrep2 = 0;
      rtol2 = -rtol2;
    }
    if (PrtLevel() > 1)  
      cout << "--MinZSimplex::Minimize() - Iter=" << iter
	   << " Move= " << move << " (" <<  smov[move/10] << ")" << endl;
    if (PrtLevel() > 2)  
      cout << "..ILO=" << ilo << " IHI=" << ihi << " INHI=" << inhi
	   << " Y(ILO)=" << Y(ilo) << " Y(IHI)=" << Y(ihi) << "\n"
	   << "...YMean_Abs=" << ymean <<  " RTOL1=" << rtol1 << " RTOL2=" << rtol2 <<  endl;
    if (PrtLevel() > 3) {
      for(i=0; i<mpts; i++) {
	cout << "....Simplex[" << i << "]= "; 
	for(j=0; j<NDim(); j++) cout << splx[i](j) << " , ";
	cout << " Y=Value= " << Y(i) << endl;
      }
    }
    if (rtol1 < mTol1) nrep1++;
    else nrep1 = 0;
    if (rtol2 < mTol2) nrep2++;
    else nrep2 = 0;
    
    if (stop0) { mStop = 1; rc = 0; stop = true; break; }
    if (nrep1 > mRep1) { mStop = 2; rc = 0; stop = true; break; }
    if (nrep2 > mRep2) { mStop = 3; rc = 0; stop = true; break; }
    if (iter > MaxIter() ) { mStop = 0, rc = iter;  break; }
    iter++;
    if (iter > 0)  movcnt[move/10]++;  

    // Next iteration, on modifie le simplex
    // Calcul du centre de gravite su simplex, hors le point le + haut
    Vector pbar(NDim());
    pbar = 0.;
    for(i=0; i<mpts; i++) {
      if (i == ihi)  continue;
      pbar += splx[i];
    }
    pbar /= (double)NDim();
    // On calcule le sommet oppose a point IHI (le + haut)
    Vector pr, prr;
    double YPR, YPRR;
    pr = (1.+Alpha())*pbar-Alpha()*splx[ihi];
    YPR = Value(pr);
    if (YPR < Y(ilo)) {   // Amelioaration par rapport au meilleur point,
      // on va plus loin d'un facteur gamma
      prr = Gamma()*pr+(1.-Gamma())*pbar;
      YPRR = Value(prr);
      if (YPRR < Y(ilo)) {  // On remplace le IHI par YPRR 
	splx[ihi] = prr;
	Y(ihi) = YPRR;
	move = 20;
      }
      else {  // sinon, on remplace par YPR
	splx[ihi] = pr;
	Y(ihi) = YPR;
	move = 10;
      }
    }
    else {  // Moins bon que le meilleur point .. 
      if (YPR > Y(inhi)) {  // Plus mauvais que le second plus haut (INHI)
	if (YPR < Y(ihi)) {   // Mais meilleur que le plus haut (IHI)
	  splx[ihi] = pr;     // On remplace donc le plus haut
	  Y(ihi) = YPR;		
	  move = 11;
	}
	else { // Plus mauvais que le plus mauvais IHI 
	  // on tente avec un point intermediaire 
	  prr = Beta()*splx[ihi]+(1.-Beta())*pbar;
	  YPRR = Value(prr);
	  if (YPRR < Y(ihi)) {   // Le point intermediaire ameliore les choses
	    splx[ihi] = prr;     // On remplace donc le point le + haut
	    Y(ihi) = YPRR;
	    move = 30;
	  }
	  else {  
	    // On tente aussi de rester du meme cote, mais aller plus loin 
	    prr = Gamma2()*splx[ihi]+(1.-Gamma2())*pbar;
	    YPRR = Value(prr);
	    if (YPRR < Y(ihi)) {   // Le point intermediaire ameliore les choses
	      splx[ihi] = prr;     // On remplace donc le point le + haut
	      Y(ihi) = YPRR;
	      move = 50;
	    }
	    else { 
	    // Rien n'y fait, on contracte autour du meilleur point 
	      for(i=0; i<mpts; i++) {
		if (i == ilo)  continue;
		splx[i] = Beta2()*splx[i]+(1.-Beta())*splx[ilo];
		Y(i) = Value(splx[i]);
		move = 40;
	      } 
	    }
	  }
	}
      }
      else {  // Meilleur que le IHI et le INHI
	splx[ihi] = pr;     // On remplace le plus haut
	Y(ihi) = YPR;	
	move = 12;
      }
    }			 
  }   // Fin de la boucle while principale

  fpoint = splx[ilo];
  mIter = iter;

  if (PrtLevel() > 0) {
    string sr;
    StopReason(sr);
    cout << "-----MinZSimplex::Minimize()/Ended - NIter=" << iter 
	 << " Moves[0..5]= " << movcnt[0] << "," << movcnt[1] << "," 
	 << movcnt[2] << "," << movcnt[3] << "," 
	 << movcnt[4] << "," << movcnt[5] 
	 << "\n..MinZSimplex Stop=" << StopReason() << " -> " << sr << endl;
    
    if (nbugrtol2 > 0)  cout << "MinZSimplex::Minimize()/Warning - nbugrtol2= " << nbugrtol2 << endl;
  }
  return rc;
}

//! Return the stop reason and fills the corresponding string description
int MinZSimplex::StopReason(string& s)
{
  char* sr[5] = { "NoConverg, MaxIterReached", "OK, fm<Tol0", "OK, Df/f<Tol1", 
		  "OK, [Df/f max]Iter<Tol2" "Error - Wrong StopReason" };
  int stop = mStop;
  if ((stop < 0) || (stop > 3)) stop = 4;
  s = sr[stop];
  return mStop;
}

int MinZSimplex::FindMinMax12(Vector& fval, int& ilo, int& ihi, int& inhi)
{
  ilo = 0;
  if (fval(0) > fval(1)) { ihi = 0;  inhi = 1; }
  else { ihi = 1;  inhi = 0; }
  
  for(int k=0; k<fval.Size(); k++) {
    if (fval(k) < fval(ilo))  ilo = k;
    if (fval(k) > fval(ihi)) {
      inhi = ihi;
      ihi = k;
    }
    else if (fval(k) > fval(inhi)) {
      if (k != ihi)  inhi = k;  // ce test n'est peut-etre pas necessaire ???
    }
  }
  return ilo;
}
