// @(#)root/mathcore:$Id: BrentMethods.cxx 36905 2010-11-24 15:44:34Z moneta $
// Authors: David Gonzalez Maline    01/2008 (MinimStep & MinimBrent)
//          Amnon Harel              04/2012 (BracketStep, GridBracketStep & BrentMethod)

/**********************************************************************
 *                                                                    *
 * Copyright (c) 2006 , LCG ROOT MathLib Team                         *
 *                                                                    *
 *                                                                    *
 **********************************************************************/

#include "ModBrentMethods.h"
#include "Math/IFunction.h"

#include <cmath>
#include <algorithm>

#ifndef ROOT_Math_Error
#include "Math/Error.h"
#endif

#include <iostream>

using std::cout;
using std::cerr;
using std::endl;

namespace ModBrentMethods { 

   double GridBracketStep(const ROOT::Math::IGenFunction* function, double &xmin, double &xmax, double fy, int npx,bool logStep)
{
   //   Grid search implementation, used to bracket the solution f(x)=fy 
   //   and later use Brent's method with the bracketed interval
   //   The step of the search is set to (xmax-xmin)/npx
   //   If no sign changes (of g(x) = f(x)-fy) are found, optimistically returns 
   //   double-step sized bracket around the point of closest approach (a-la MinimStep).

   //   Caller must ensure xmax > xmin

   cout<<"GBstep [ "<<xmin<<" - "<<xmax<<" ]"<<endl;

   if (npx < 2) return 0.5*(xmax+xmin); // no bracketing - return just mid-point

   double gmin = (*function)(xmin)-fy;
   double gmax = (*function)(xmax)-fy;
   if( gmin * gmax < 0 ) return 0.5*(xmax+xmin); // no need for a grid search - simply return the mid-point

   double zmin = logStep ? std::log(xmin) : xmin;
   double zmax = logStep ? std::log(xmax) : xmax;

   double dz = (zmax-zmin)/(npx-1);
   double agmin = std::fabs( gmin );
   double z_closest = zmin;
   double z_opp = zmin - std::fabs(zmin) - 1.;
   double prev_g = gmin;
   for (int i=1; i<npx; i++) {
      double z = zmin + i*dz;
      double x = logStep ? std::exp(z) : z; 
      double g = (*function)(x)-fy;
      if( g * prev_g < 0 ) {
	z_opp = z;
	break;
      }
      double ag = std::fabs(g);
      if( ag < agmin ) {
	z_closest = z;
	agmin = ag;
      }
      prev_g = g;
   }

   double zmid;
   if (z_opp >= zmin) {
     zmin = z_opp - dz;
     zmid = z_opp - 0.5*dz;
     zmax = z_opp;
   } else {
     zmin = std::max(zmin,z_closest-1.021*dz); // a bit different than dz so we'll get more new grid points
     zmid = z_closest;
     zmax = std::min(zmax,z_closest+1.031*dz); // a bit different than dz so we'll get more new grid points
   }
   
   xmin = logStep ? std::exp(zmin) : zmin; 
   xmax = logStep ? std::exp(zmax) : zmax; 
   double xmid = logStep ? std::exp(zmid) : zmid; 

   cout<<"GBstep gave "<<xmid<<" in [ "<<xmin<<" - "<<xmax<<" ] "<<endl;
   return xmid;
}

   int BracketStep(const ROOT::Math::IGenFunction* function, double &xmin, double &xmax, double fy, int &niter,
		   int npx, double absTol, double relTol, int itermax, bool useLog)
{
   /// recursive grid search. Returns status code: 0=OK, found a bracket ; -1=no sign changes found ; -2=itermax exceeded
   double width, mid_point, tolerance;
   niter = 0; 
   while (true) {
      double gmin = (*function)(xmin) - fy;
      double gmax = (*function)(xmax) - fy;
      if (gmin * gmax <= 0) break; // found bracket

      width = xmax - xmin;
      mid_point = 0.5 * ( xmin + xmax );
      tolerance = absTol + 4 * relTol * std::fabs(mid_point);
      if (width <= tolerance) {
	 return -1;
      }

      ModBrentMethods::GridBracketStep (function, xmin, xmax, fy, npx, useLog);

      niter += npx;
      if (niter > itermax) {
	 return -2;
      }
   }
   return 0;
}

   double BrentMethod(const ROOT::Math::IGenFunction* function, double &xmin, double &xmax, double fy,  
		      bool &ok, int &niter, double epsabs, double epsrel, int itermax)
{
   //Finds x in bracket [xmin, xmax] such that function(x)=fy and x.
   //The input xmin and xmax must define a valid bracket, at one func>=fy and at the other func<=fy.

   //x is found using Brent's method, as a root of g(x)=function(x)-fy.
   //Brent's method uses either bisection, linear interpolation, or inverse quadratic
   //interpolation in each iteration.
   //Details about convergence and properties of this algorithm can be
   //found in the book by R.P.Brent "Algorithms for Minimization Without Derivatives"
   //or in the "Numerical Recipes", chapter 10.2
   //
   //The output root position should be accurate to 4*epsrel*abs(x)+epsabs
   //Brent had esprel as the relative machine precision defined as the smallest 
   //representable number such that  1.+macheps .gt. 1.
   //
   //if ok=true the method has converged
  
   // Implementation based on Brent's Fortran code (http://www.netlib.org/go/zeroin.f)
   // Hence this is more or less assembly code with comments
   // Here f(x) = function(x) - fy

   double a=xmin, b=xmax;
   double fa = (*function)(xmin)-fy;
   double fb = (*function)(xmax)-fy;
   if( fa * fb > 0 ) { ok = false; niter = -1; return xmin - std::fabs( xmin ) - 1.; }; // mark illegal inputs

   double c = a;
   double fc = fa;
   double step = b-a; // proposed step size
   double old_step = step; // An old step size to be used in comparisons.
                           // After a bisection, this is the previous step.
                           // After an interpolation, the next-to-previous step.
   double tol; // the tolerance for the current b
   double xm; // bracket half width, signed
   double s,p,r,q; // temporaries. p and q may be meaningful as commented below.
   
   niter = 0;
   while( true ) { // at this point the bracket edges are b and c

      ++niter;
     
      // ensure b has smaller |f| than c. If not so originally, also set a=c (thus forgeting 
      // about the previous guess, probably because in this case b isn't between a and c)
      if (std::fabs(fc) < std::fabs(fb)) {
	 a = b;
	 b = c;
	 c = a;
	 fa = fb;
	 fb = fc;
	 fc = fa;
      }
      // at this point b is the current best guess. a contains the previous estimate, which may be equal to c.

      tol = 2*epsrel*(std::fabs(b))+0.5*epsabs;
      xm = 0.5*(c - b);

      if (std::fabs(xm) <= tol || fb==0) { // convergence and succesful exit
         ok = true;
	 return b;
      }
      if( niter > itermax ) { // didn't converge
	 ok = false;
	 xmin = std::min( b, c );
	 xmax = std::max( b, c );
	 return b;
      }

      // Choose Step size
      if( std::fabs(old_step) < tol || // the old step was suspicously small
	  std::fabs(fa) <= std::fabs(fb) // haven't improved the guess in the previous step
	  ) { // use bisection.

	old_step = step = xm; 

      } else { // try to use an interpolation

	s = fb/fa; // no division by zero since |fa|=(?)=|fc|>|fb|>0

	// the interpolations calculate a denominator, q, and an enumerator, p, for d
	if (a == c) { // use linear interpolation, since only two points are in play
	  
	  p = 2.*xm*s;
	  q = 1.-s;

	} else  { // use inverse quadratic interpolation

	  q = fa/fc; // here used as a temp variable, like s and r
	  r = fb/fc;
	  p = s*(2.*xm*q*(q-r) - (b-a)*(r-1.));
	  q = (q-1.)*(r-1.)*(s-1.);
	  
	}
	if( p > 0 ) { // ensure positive p and flip the sign of p/q
	  q *= -1;
	} else {
	  p *= -1;
	}

	// Note: slight simplification of zeroin.f, as "s" isn't really used
	if ( ( 2.*p < 3.*xm*q-std::fabs(tol*q)) && // ensures the |b-new_b| interval is reduced (from |b-c|) by atleast 25% + tolerance
	     ( p < std::fabs(0.5*old_step*q)) // proposed interpolation step smaller than half of "the old step"
	    ) {
	  // going ahead with the interpolation
	  old_step = step;
	  step = p/q;
	} else {
	  // giving up on the interpolation and using bisection instead
	  old_step = step = xm;
	}
      }

      // Prepare the new interval:
      // 1) first update b ( and store the old guess in a )
      a = b;
      fa = fb;
      if (std::fabs(step) > tol) {
	b = b + step;
      } else {
	if (xm > 0.) {
	  b += tol;
	} else {
	  b -= tol;
	}
      }
      fb = (*function)(b)-fy;

      // 2) If needed, update c, to ensure b and c have opposite signs in the next iteration
      if (fb * fc > 0.) {
	 c = a; 
	 fc = fa;
	 old_step = step = b-a;
      }
   }
}


  double MinimStep(const ROOT::Math::IGenFunction* function, int type, double &xmin, double &xmax, double fy, int npx,bool logStep)
{
   //   Grid search implementation, used to bracket the minimum and later
   //   use Brent's method with the bracketed interval
   //   The step of the search is set to (xmax-xmin)/npx
   //   type: 0-returns MinimumX
   //         1-returns Minimum
   //         2-returns MaximumX
   //         3-returns Maximum
   //         4-returns X corresponding to fy

   if (logStep) { 
      xmin = std::log(xmin);
      xmax = std::log(xmax);
   }

   //cout<<"AH MinimStep type: "<<type<<", [ "<<xmin<<" - "<<xmax<<" ]";

   if (npx < 2) return 0.5*(xmax-xmin); // no bracketing - return just mid-point
   double dx = (xmax-xmin)/(npx-1);
   double xxmin = (logStep) ? std::exp(xmin) : xmin; 
   double yymin;
   if (type < 2)
      yymin = (*function)(xxmin);
   else if (type < 4)
      yymin = -(*function)(xxmin);
   else
      yymin = std::fabs((*function)(xxmin)-fy);

   for (int i=1; i<=npx-1; i++) {
      double x = xmin + i*dx;
      if (logStep) x = std::exp(x); 
      double y = 0;
      if (type < 2)
         y = (*function)(x);
      else if (type < 4)
         y = -(*function)(x);
      else
         y = std::fabs((*function)(x)-fy);
      if (y < yymin) {
         xxmin = x; 
         yymin = y;
      }
   }

   if (logStep) {
      xmin = std::exp(xmin); 
      xmax = std::exp(xmax); 
   }


   xmin = std::max(xmin,xxmin-dx);
   xmax = std::min(xmax,xxmin+dx);

   //cout<<" -> [ "<<xmin<<" - "<<xmax<<" ] "<<endl;
   return std::min(xxmin, xmax);
}

   double MinimBrent(const ROOT::Math::IGenFunction* function, int type, double &xmin, double &xmax, double xmiddle, double fy,  bool &ok, int &niter, double epsabs, double epsrel, int itermax)
{
   //Finds a minimum of a function, if the function is unimodal  between xmin and xmax
   //This method uses a combination of golden section search and parabolic interpolation
   //Details about convergence and properties of this algorithm can be
   //found in the book by R.P.Brent "Algorithms for Minimization Without Derivatives"
   //or in the "Numerical Recipes", chapter 10.2
   // convergence is reached using  tolerance = 2 *( epsrel * abs(x) + epsabs)
   //
   //type: 0-returns MinimumX
   //      1-returns Minimum
   //      2-returns MaximumX
   //      3-returns Maximum
   //      4-returns X corresponding to fy
   //if ok=true the method has converged

   const double c = 3.81966011250105097e-01; //  (3.-std::sqrt(5.))/2.  (comes from golden section)
   double u, v, w, x, fv, fu, fw, fx, e, p, q, r, t2, d=0, m, tol;
   v = w = x = xmiddle;
   e=0;

   double a=xmin;
   double b=xmax;
   if (type < 2)
      fv = fw = fx = (*function)(x);
   else if (type < 4)
      fv = fw = fx = -(*function)(x);
   else
      fv = fw = fx = std::fabs((*function)(x)-fy);

   for (int i=0; i<itermax; i++){
      m=0.5*(a + b);
      tol = epsrel*(std::fabs(x))+epsabs;
      t2 = 2*tol;

      if (std::fabs(x-m) <= (t2-0.5*(b-a))) {
         //converged, return x
         ok=true;
         niter = i-1; 
         if (type==1)
            return fx;
         else if (type==3)
            return -fx;
         else
            return x;
      }

      if (std::fabs(e)>tol){
         //fit parabola
         r = (x-w)*(fx-fv);
         q = (x-v)*(fx-fw);
         p = (x-v)*q - (x-w)*r;
         q = 2*(q-r);
         if (q>0) p=-p;
         else q=-q;
         r=e;   // Deltax before last
         e=d;   // last delta x
         // current Deltax = p/q
         // take a parabolic  step only if: 
         // Deltax < 0.5* (DeltaX before last) && Deltax > a && Deltax < b
         // (a BUG in testing this condition is fixed 11/3/2010 (with revision  32544)
         if (std::fabs(p) >= std::fabs(0.5*q*r) || p <= q*(a-x) || p >= q*(b-x)) {
            //  condition fails - do not take parabolic step 
            e=(x>=m ? a-x : b-x);
            d = c*e;
         } else { 
            // take a parabolic interpolation step
            d = p/q;
            u = x+d;
            if (u-a < t2 || b-u < t2)
               //d=TMath::Sign(tol, m-x);
               d=(m-x >= 0) ? std::fabs(tol) : -std::fabs(tol);
         } 
      } else {
         e=(x>=m ? a-x : b-x);
         d = c*e;
      }
      u = (std::fabs(d)>=tol ? x+d : x+ ((d >= 0) ? std::fabs(tol) : -std::fabs(tol)) );
      if (type < 2)
         fu = (*function)(u);
      else if (type < 4)
         fu = -(*function)(u);
      else
         fu = std::fabs((*function)(u)-fy);
      //update a, b, v, w and x
      if (fu<=fx){
         if (u<x) b=x;
         else a=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;
         }
      }
   }
   //didn't converge
   ok = false;
   xmin = a;
   xmax = b;
   niter = itermax;
   return x;

}
} // namespace ModBrentMethods