/**********************************************************
*  Multidimensional minimization of a function fsimplex(X)*
*  where X is an ndim-dimensional vector, by the downhill *
*  simplex method of Nelder and Mead.                     *
* ------------------------------------------------------- *
* SAMPLE RUN: Find a minimum of function F(x,y):          *
*             F=Sin(R)/R, where R = Sqrt(x*x+y*y).        *
*                                                         *
*  Number of iterations: 22                               *
*                                                         *
*  Best ndim+1 points:                                    *
*   4.122686  1.786915                                    *
*   4.166477  1.682698                                    *
*   4.142454  1.741176                                    *
*                                                         *
*  Best ndim+1 mimimum values:                            *
*   -0.2172336265                                         *
*   -0.2172336281                                         *
*   -0.2172336271                                         *
*                                                         *
* ------------------------------------------------------- *
* REFERENCE: "Numerical Recipes, The Art of Scientific    *
*             Computing By W.H. Press, B.P. Flannery,     *
*             S.A. Teukolsky and W.T. Vetterling,         *
*             Cambridge University Press, 1986"           *
*             [BIBLI 08].                                 *
*                                                         *
*                      C++ Release By J-P Moreau, Paris.  *
*                            (www.jpmoreau.fr)            *
**********************************************************/
#include <TRandom2.h>
#include <TCanvas.h>
#include <TF2.h>


#define   mp  22
#define   np  21    //maximum value for ndim=20 

typedef   double mat[mp][np];

mat p;
double y[mp], pt[mp];
int    i,iter,j,ndim;
double ftol;

//2D function and initial points
double func(double x,double y) {
  //return -sin(x)*sin(y)/(x*y);
  double r;
  r=sqrt(x*x+y*y);
  return -sin(r)/r;
}

// user defined function to minimize
double fsimplex(double *p) {
  return func(p[1],p[2]);
}

void dsimplex(mat p, double *y, int ndim, double ftol, int *iter) {
/*-------------------------------------------------------------------
! Multidimensional minimization of the function fsimplex(X) where X is
! an ndim-dimensional vector, by the downhill simplex method of
! Nelder and Mead. Input is a matrix P whose ndim+1 rows are ndim-
! dimensional vectors which are the vertices of the starting simplex
! (Logical dimensions of P are P(ndim+1,ndim); physical dimensions
! are input as P(NP,NP)). Also input is the vector Y of length ndim
! +1, whose components must be pre-initialized to the values of fsimplex
! evaluated at the ndim+1 vertices (rows) of P; and FTOL the fractio-
! nal convergence tolerance to be achieved in the function value. On
! output, P and Y will have been reset to ndim+1 new points all within
! FTOL of a minimum function value, and iter gives the number of ite-
! rations taken.
!-------------------------------------------------------------------*/
// Label:  e1
const int nmax=20, itmax=500;
//Expected maximum number of dimensions, three parameters which define
// the expansions and contractions, and maximum allowed number of
//iterations.
  double pr[mp], prr[mp], pbar[mp];
  double alpha=1.0, beta=0.5, gamma=2.0;
  int i,ihi,ilo,inhi,j,mpts;
  double rtol,ypr,yprr;
  mpts=ndim+1;
  *iter=0;

 // Create some histograms.
 TCanvas *c1 = new TCanvas("c1","Downhill Simplex",200,10,600,600);
  c1->SetGrid();
  Double_t scale=5.;
  TF1* main   = new TF2("MyFunc","func(x,y)",-scale,scale,-scale,scale);
  main->Draw("Surf9");
  //  main->SetNpx(100000);

  Double_t xx[3] = {1.,-2.,4.};
  Double_t yy[3] = {2.,-3.,2.};
  Double_t zz[3];
  for (i=0;i<3;i++){
    zz[i]=func(xx[i],yy[i]);
  }
  Double_t pp[9];
  for (i=0;i<3;i++) {
    pp[3*i]=xx[i];
    pp[3*i+1]=yy[i];
    pp[3*i+2]=zz[i];
  }
  TPolyLine3D *pline = new TPolyLine3D(3,pp);

  pline->SetNextPoint(xx[0],yy[0],zz[0]);
  pline->SetLineColor(4);
  pline->SetLineWidth(2);
  pline->Draw();


e1:ilo=1;

  Double_t max_range=0.;
  for (i=0;i<3;i++) {
    pp[3*i]=p[i+1][1];
    pp[3*i+1]=p[i+1][2];
    pp[3*i+2]=y[i+1];
    if (fabs(p[i+1][1])>max_range) {
      max_range=fabs(p[i+1][1]);
    }
    if (fabs(p[i+1][2])>max_range) {
      max_range=fabs(p[i+1][2]);
    }
    //printf("range=%f \n",p[i+1][2]);
  }
  
  if (max_range < scale*0.1) {
    scale *= 0.1;
    printf("scale=%f \n",scale);
    main->SetRange(-scale,-scale,+scale,+scale);
    main->Draw("Surf9");
    TPolyLine3D *pline = new TPolyLine3D(3,pp);

    pline->SetNextPoint(pp[0],pp[1],pp[2]);
    pline->SetLineColor(4);
    pline->SetLineWidth(2);
    pline->Draw();
    c1->Update();
  } else {
  pline->SetPolyLine(3,pp);
  pline->SetNextPoint(pp[0],pp[1],pp[2]);
  pline->Draw();
  }
  c1->Update();
  gSystem->Sleep(200);
	
  //Find the highest and next highest point as well as the lowest (best) point
  if (y[1] > y[2]) {
    ihi=1;
    inhi=2;
  }
  else {
    ihi=2;
    inhi=1;
  }
  for (i=1; i<=mpts; i++) {
    if (y[i] < y[ilo])  ilo=i;
    if (y[i] > y[ihi]) {
      inhi=ihi;
      ihi=i;
    }
    else if (y[i] > y[inhi])
      if (i != ihi)  inhi=i;
  }
//Compute the fractional range from highest to lowest and return if
//satisfactory.
  rtol=2.0*fabs(y[ihi]-y[ilo])/(fabs(y[ihi])+fabs(y[ilo]));
  if (rtol < ftol)  return;  //normal exit
  if (*iter == itmax) {
    printf("Downhill simplex exceeding maximum iterations.\n");
    return;
  }

  //Now try to find the next solution in 3 scenaria (alpha, beta, gamma)
  *iter= (*iter) + 1;
  for (j=1; j<=ndim; j++) { 
    pbar[j]=0.0; 
  } 
  for (i=1; i<=mpts; i++) {
    if (i != ihi) {
      for (j=1; j<=ndim; j++) {
        pbar[j] += p[i][j]; } 
    }
  }
  //reflection from the high point
  for (j=1; j<=ndim; j++) {
    pbar[j] /= 1.0*ndim;
    pr[j]=(1.0+alpha)*pbar[j] - alpha*p[ihi][j];
  }
  ypr=fsimplex(pr);
  if (ypr <= y[ilo]) {
    //try extended reflection
    for (j=1; j<=ndim; j++) {
      prr[j]=gamma*pr[j] + (1.0-gamma)*pbar[j];
    }
    yprr=fsimplex(prr);
    if (yprr < y[ilo]) {
      for (j=1; j<=ndim; j++) {
	p[ihi][j]=prr[j];
      }
      y[ihi]=yprr;
    }
    else {
      for (j=1; j<=ndim; j++) {
	p[ihi][j]=pr[j];
      }
      y[ihi]=ypr;
    }
  }
  else if (ypr >= y[inhi]) {
    //reflected point is worse than second highest but better than highest
	if (ypr < y[ihi]) {
	  for (j=1; j<=ndim; j++)  {
	    p[ihi][j]=pr[j];
	  }
	  y[ihi]=ypr;
	}
	//do a one dim contraction
	for (j=1; j<=ndim; j++) {
	  prr[j]=beta*p[ihi][j] + (1.0-beta)*pbar[j];
	}
	yprr=fsimplex(prr);
	if (yprr < y[ihi]) {
	  for (j=1; j<=ndim; j++) {
	    p[ihi][j]=prr[j];
	  }
	  y[ihi]=yprr;
	} 
	else {
	  //nothing helps, do a full contraction
	  for (i=1; i<=mpts; i++) {
	    if (i != ilo) {
	      for (j=1; j<=ndim; j++) {
		pr[j]=0.5*(p[i][j] + p[ilo][j]);
	        p[i][j]=pr[j];
	      }
	      y[i]=fsimplex(pr);
	    }
	  }
	}
  }
  else {
    for (j=1; j<=ndim; j++)  {
      p[ihi][j]=pr[j];
    }
    y[ihi]=ypr;
  }
  goto e1;
}


void rsimplex() {
  ndim=2;      // 2 variables
  ftol=1e-8;   // user given tolerance

  //define ndim+1 initial vertices (one by row)
  p[1][1]= 1.0; p[1][2]=2.0;
  p[2][1]=-2.0; p[2][2]=-3.0;
  p[3][1]= 4.0; p[3][2]=2.0;

//initialize y to the values of fsimplex evaluated 
//at the ndim+1 vertices (rows] of p
  for (i=1; i<=ndim+1; i++) {
    pt[1]=p[i][1]; pt[2]=p[i][2];
    y[i]=fsimplex(pt);
  }

 
  //call main function
  dsimplex(p,y,ndim,ftol,&iter);

  //print results
  printf("\n Number of iterations: %d\n\n", iter);
  printf(" Best ndim+1 points:\n");
  for (i=1 ; i<=ndim+1; i++) {
    for (j=1; j<=ndim; j++)  printf("  %f", p[i][j]);
    printf("\n");
  }
  printf("\n Best ndim+1 mimimum values:\n");
  for (i=1; i<=ndim+1; i++) printf(" %14.10f\n", y[i]);
  printf("\n");

}