/*************************************************************
* Minimization of a Function FUNC of N Variables By Powell's *
*    Method Discarding the Direction of Largest Decrease     *
* ---------------------------------------------------------- *
* SAMPLE RUN: Find a minimum of function F(x,y):             *
*             F=Sin(R)/R, where R = Sqrt(x*x+y*y).           *
*                                                            *
* Number of iterations: 2                                    *
*                                                            *
* Minimum Value: -0.217234                                   *
*                                                            *
* at point: 3.177320  3.177320                               *
*                                                            *
* ---------------------------------------------------------- *
* 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 <stdio.h>
#include <math.h>

#define NMAX    20
#define ITMAX  200

typedef double MAT[NMAX][NMAX];

double P[NMAX];
MAT    XI;

double PCOM[NMAX], XICOM[NMAX];  //PCOM,XICOM,NCOM are common variables
int    ITER,N,NCOM;              //for LINMIN and F1DIM only
double FRET,FTOL;


// user defined function to minimize
double FUNC(double *P) {
  double R;
  R=sqrt(P[1]*P[1]+P[2]*P[2]);
  if (fabs(R) < 1e-12)
    return 1.0;
  else
    return (sin(R)/R);
}


double MAX(double a,double b) {
  if (a>=b) return a; else return b;
}

double MIN(double a,double b) {
  if (a<=b) return a; else return b;
}

double Sign(double a, double b) {
  if (b>=0) return fabs(a);
  else return -fabs(a);
}

double Sqr(double a) {
  return a*a;
}


void   LINMIN(double *, double *, int, double *);
void   MNBRAK(double *, double *, double *, double *, double *, double *);
double BRENT (double *, double *, double *, double, double *);


void POWELL(double *P, MAT XI, int N, double FTOL,int *ITER, double *FRET) {
/*--------------------------------------------------------------------------
  Minimization of a function  FUNC of N variables  (FUNC is not an argument, 
  it is a fixed function name). Input consists of an initial starting point 
  P, that is a vector of length N; an initial matrix XI  whose  logical 
  dimensions are N by N, physical dimensions NMAX by NMAX, and whose columns
  contain the initial set of directions (usually the N unit vectors); and 
  FTOL, the fractional tolerance in the function value such that failure to 
  decrease by more than this amount on one iteration signals doneness. On 
  output, P is set to the best point found, XI is the then-current direction 
  set,  FRET is the returned function value at P, and ITER is the number of 
  iterations taken. The routine LINMIN is used.
---------------------------------------------------------------------------*/
// Label: e1
  double PT[NMAX], PTT[NMAX], XIT[NMAX];
  double DEL,FP,FPTT,T;
  int I,IBIG,J;
  *FRET=FUNC(P);
  for (J=1; J<=N; J++)
    PT[J]=P[J];            //Save initial point
  *ITER=0;
e1:*ITER=*ITER+1;
  FP=*FRET;
  IBIG=0;
  DEL=0.0;                 //Will be the biggest function decrease
  for (I=1; I<=N; I++) {   //In each iteration, loop over all directions in the set.
    for (J=1; J<=N; J++)   //Copy the direction.
      XIT[J]=XI[J][I];
    FPTT=*FRET;
    LINMIN(P,XIT,N,FRET);  //Minimize along it
    if (fabs(FPTT-*FRET) > DEL) {
      DEL=fabs(FPTT-*FRET);
      IBIG=I;
    }
  }
  if (2.0*fabs(FP-*FRET) <= FTOL*(fabs(FP)+fabs(*FRET))) return; //Termination criterion
  if (*ITER == ITMAX) {
    printf("\n Powell exceeding maximum iterations.\n\n");
    return;
  }
  for (J=1; J<=N; J++) {
    PTT[J]=2.0*P[J]-PT[J]; //Construct the extrapolated point and the average
    XIT[J]=P[J]-PT[J];     //direction moved. Save the old starting point
    PT[J]=P[J];
  }
  FPTT=FUNC(PTT);              //Function value at extrapolated point
  if  (FPTT >= FP) goto e1;    //One reason not to use new direction
  T=2.0*(FP-2.0*(*FRET)+FPTT)*Sqr(FP-(*FRET)-DEL)-DEL*Sqr(FP-FPTT);
  if (T >= 0.0) goto e1;       //Other reason not to use new direction
  LINMIN(P,XIT,N,FRET);        //Move to the minimum of the new direction
  for (J=1; J<=N; J++)         //and save the new direction.
    XI[J][IBIG]=XIT[J];
  goto e1;
} //Powell()

void   LINMIN(double *P, double *XI, int N, double *FRET) {
/*---------------------------------------------------------
  Given an N dimensional point P and a N dimensional direc-
  tion XI, moves and resets P to where the function FUNC(P)
  takes on a minimum along the direction XI from P, and
  replaces XI by the actual vector displacement that P was
  moved. Also returns as FRET the value of FUNC at the
  returned location P. This is actually all accomplished by
  calling the routines MNBRAK and BRENT.
---------------------------------------------------------*/
  double AX,BX,FA,FB,FX,TOL,XMIN,XX;
  int J;
  TOL=1e-4;
  NCOM=N;
  for (J=1; J<=N; J++) {
    PCOM[J]=P[J];
    XICOM[J]=XI[J];
  }
  AX=0.0;
  XX=1.0;
  BX=2.0;
  MNBRAK(&AX,&XX,&BX,&FA,&FX,&FB);
  *FRET=BRENT(&AX,&XX,&BX,TOL,&XMIN);
  for (J=1; J<=N; J++) {
    XI[J]=XMIN*XI[J];
    P[J] += XI[J];
  }
}


double F1DIM(double X) {
  double XT[NMAX]; int J;
  for (J=1; J<=NCOM; J++)
    XT[J]=PCOM[J] + X*XICOM[J];
  return FUNC(XT);
}


void   MNBRAK(double *AX,double *BX,double *CX,
			  double *FA, double *FB, double *FC)  {
/*---------------------------------------------------------------------
 Given a Function F1DIM(X), and given distinct initial points AX and
 BX, this routine searches in the downhill direction (defined by the
 Function as evaluated at the initial points) and returns new points
 AX, BX, CX which bracket a minimum of the Function. Also returned
 are the Function values at the three points, FA, FB and FC.
---------------------------------------------------------------------*/
  //Label:  e1
  const double GOLD=1.618034, GLIMIT=100.0, TINY=1e-20;
/*The first parameter is the default ratio by which successive intervals
  are magnified; the second is the maximum magnification allowed for
  a parabolic-fit step. */

  double DUM,FU,Q,R,U,ULIM;
  *FA=F1DIM(*AX);
  *FB=F1DIM(*BX);
  if (*FB > *FA) {
    DUM=*AX;
    *AX=*BX;
    *BX=DUM;
    DUM=*FB;
    *FB=*FA;
    *FA=DUM;
  }
  *CX=*BX+GOLD*(*BX-*AX);
  *FC=F1DIM(*CX);
e1:if (*FB >= *FC) {
    R=(BX-AX)*(FB-FC);
    Q=(BX-CX)*(FB-FA);
    U=*BX-((*BX-*CX)*Q-(*BX-*AX)*R)/(2.0*Sign(MAX(fabs(Q-R),TINY),Q-R));
    ULIM=*BX+GLIMIT*(*CX-*BX);
    if ((*BX-U)*(U-*CX) > 0.0) {
      FU=F1DIM(U);
      if (FU < *FC) {
        *AX=*BX;
        *FA=*FB;
        *BX=U;
        *FB=FU;
        goto e1;
      }
      else if (FU > *FB) {
        *CX=U;
        *FC=FU;
        goto e1;
      }
      U=*CX+GOLD*(*CX-*BX);
      FU=F1DIM(U);
    }
    else if ((*CX-U)*(U-ULIM) > 0.0) {
      FU=F1DIM(U);
      if (FU < *FC) {
        *BX=*CX;
        *CX=U;
        U=*CX+GOLD*(*CX-*BX);
        *FB=*FC;
        *FC=FU;
        FU=F1DIM(U);
      }
    }
    else if ((U-ULIM)*(ULIM-*CX) >= 0.0) {
      U=ULIM;
      FU=F1DIM(U);
    }
    else {
      U=*CX+GOLD*(*CX-*BX);
      FU=F1DIM(U);
    }
    *AX=*BX;
    *BX=*CX;
    *CX=U;
    *FA=*FB;
    *FB=*FC;
    *FC=FU;
    goto e1;
  }  
}


double BRENT(double *AX,double *BX,double *CX, double TOL, double *XMIN) {
/*------------------------------------------------------------------
 Given a function F1DIM, and a bracketing triplet of abscissas AX,
 BX,CX (such that BX is between AX and CX, and F(BX) is less than
 both F(AX) and F(CX)), this routine isolates the minimum to a
 fractional precision of about TOL using Brent's method. The abscissa
 of the minimum is returned in XMIN, and the minimum function value 
 is returned as BRENT, the returned function value.
-------------------------------------------------------------------*/
//Labels: e1,e2,e3
  const double CGOLD=0.3819660, ZEPS=1e-10;
/*Maximum allowed number of iterations; golden ratio; and a small
  number which protects against trying to achieve fractional accuracy
  for a minimum that happens to be exactly zero */
  double A,B,D,E,ETEMP,FX,FU,FV,FW,P,Q,R,TOL1,TOL2,U,V,W,X,XM;
  int ITER;
  A=MIN(*AX,*CX);
  B=MAX(*AX,*CX);
  V=*BX;
  W=V;
  X=V;
  E=0.0;
  FX=F1DIM(X);
  FV=FX;
  FW=FX;
  for (ITER=1; ITER<=ITMAX; ITER++) {                    //main loop
    XM=0.5*(A+B);
    TOL1=TOL*fabs(X)+ZEPS;
    TOL2=2.0*TOL1;
    if (fabs(X-XM) <= (TOL2-0.5*(B-A))) goto e3;     //Test for done here
    if (fabs(E) > TOL1) {               //Construct a trial parabolic fit
      R=(X-W)*(FX-FV);
      Q=(X-V)*(FX-FW);
      P=(X-V)*Q-(X-W)*R;
      Q=0.2*(Q-R);
      if (Q > 0.0) P=-P;
      Q=fabs(Q);
      ETEMP=E;
      E=D;
      if (fabs(P) >= fabs(0.5*Q*ETEMP) || P <= Q*(A-X) || P >= Q*(B-X)) goto e1;
//  The above conditions determine the acceptability of the
//  parabolic fit. Here it is o.k.
      D=P/Q;
      U=X+D;
      if (U-A < TOL2 || B-U < TOL2)  D=Sign(TOL1,XM-X);
      goto e2;
    }
e1: if (X >= XM)
      E=A-X;
    else
      E=B-X;
    D=CGOLD*E;
e2: if (fabs(D) >= TOL1)
      U=X+D;
    else
      U=X+Sign(TOL1,D);
    FU=F1DIM(U);   //This the one function evaluation per iteration
    if (FU <= FX) {
      if (U >= X)
        A=X;
      else
        B=X;
      V=W;
      FV=FW;
      W=X;
      FW=FX;
      X=U;
      FX=FU;
    }
    else {
      if (U < X)
        A=U;
      else
        B=U;
      if (FU <= FW || W == X) {
        V=W;
        FV=FW;
        W=U;
        FW=FU;
      }
      else if (FU <= FV || V == X || V == W) {
        V=U;
        FV=FU;
      }
    }
  }
  printf("\n Brent exceed maximum iterations.\n\n");
e3:*XMIN=X;   //exit section
  return FX;
}


void main()  {

  N=2;
  P[1]=2.0; P[2]=2.0;
  XI[1][1]=1.0; XI[1][2]=1.0;
  XI[2][1]=1.0; XI[2][2]=1.0;
  FTOL=1e-8;

  POWELL(P,XI,N,FTOL,&ITER,&FRET);

  printf("\n Number of iterations: %d\n\n", ITER);
  printf(" Minimum value: %f\n\n", FRET);
  printf(" at point: %f  %f\n\n", P[1], P[2]);

}

// end of file tpowell.cpp