// @(#)root/mathcore:$Id$
// Author: L. Moneta Tue Nov 28 10:52:47 2006

/**********************************************************************
 *                                                                    *
 * Copyright (c) 2006  LCG ROOT Math Team, CERN/PH-SFT                *
 *                                                                    *
 *                                                                    *
 **********************************************************************/

// Implementation file for class FitUtil

#include "Fit/FitUtil.h"

#include "Fit/BinData.h"
#include "Fit/UnBinData.h"
//#include "Fit/BinPoint.h"

#include "Math/IParamFunction.h"
#include "Math/Integrator.h"
#include "Math/IntegratorMultiDim.h"
#include "Math/WrappedFunction.h"
#include "Math/OneDimFunctionAdapter.h"
#include "Math/RichardsonDerivator.h"

#include "Math/Error.h"
#include "Math/Util.h"  // for safe log(x)

#include <limits>
#include <cmath>
#include <cassert> 
//#include <memory>

//#define DEBUG
#ifdef DEBUG
#define NSAMPLE 10
#include <iostream> 
#endif

//todo: 

//  need to implement integral option

namespace ROOT { 

   namespace Fit { 

      namespace FitUtil { 

         // internal class to evaluate the function or the integral 
         // and cached internal integration details
         // if useIntegral is false no allocation is done
         // and this is a dummy class
         // class is templated on any parametric functor implementing operator()(x,p) and NDim()
         // contains a constant pointer to the function
         template <class ParamFunc = ROOT::Math::IParamMultiFunction> 
         class IntegralEvaluator { 

         public: 

            IntegralEvaluator(const ParamFunc & func, const double * p, bool useIntegral = true) :
               fDim(0),
               fParams(0),
               fFunc(0),
               fIg1Dim(0), 
               fIgNDim(0),
               fFunc1Dim(0),
               fFuncNDim(0)
            { 
               if (useIntegral) { 
                  SetFunction(func, p); 
               }
            }

            void SetFunction(const ParamFunc & func, const double * p = 0) { 
               // set the integrand function and create required wrapper 
               // to perform integral in (x) of a generic  f(x,p)
               fParams = p;
               fDim = func.NDim(); 
               // copy the function object to be able to modify the parameters 
               //fFunc = dynamic_cast<ROOT::Math::IParamMultiFunction *>( func.Clone() ); 
               fFunc = &func;
               assert(fFunc != 0); 
               // set parameters in function
               //fFunc->SetParameters(p); 
               if (fDim == 1) { 
                  fFunc1Dim = new ROOT::Math::WrappedMemFunction< IntegralEvaluator, double (IntegralEvaluator::*)(double ) const > (*this, &IntegralEvaluator::F1);
                  fIg1Dim = new ROOT::Math::IntegratorOneDim(); 
                  //fIg1Dim->SetFunction( static_cast<const ROOT::Math::IMultiGenFunction & >(*fFunc),false);
                  fIg1Dim->SetFunction( static_cast<const ROOT::Math::IGenFunction &>(*fFunc1Dim) );
               } 
               else if (fDim > 1) {
                  fFuncNDim = new ROOT::Math::WrappedMemMultiFunction< IntegralEvaluator, double (IntegralEvaluator::*)(const double *) const >  (*this, &IntegralEvaluator::FN, fDim);
                  fIgNDim = new ROOT::Math::IntegratorMultiDim();
                  fIgNDim->SetFunction(*fFuncNDim);
               }
               else
                  assert(fDim > 0);                
            }
            
            void SetParameters(const double *p) { 
               // copy just the pointer
               fParams = p; 
            }

            ~IntegralEvaluator() { 
               if (fIg1Dim) delete fIg1Dim; 
               if (fIgNDim) delete fIgNDim; 
               if (fFunc1Dim) delete fFunc1Dim; 
               if (fFuncNDim) delete fFuncNDim; 
               //if (fFunc) delete fFunc; 
            }
            
            // evaluation of integrand function (one-dim)
            double F1 (double x) const { 
               double xx[1]; xx[0] = x;
               return (*fFunc)( xx, fParams);
            }
            // evaluation of integrand function (multi-dim)
            double FN(const double * x) const { 
               return (*fFunc)( x, fParams);
            }

            double Integral(const double *x1, const double * x2) { 
               // return unormalized integral
               return (fIg1Dim) ? fIg1Dim->Integral( *x1, *x2) : fIgNDim->Integral( x1, x2); 
            }
            double operator()(const double *x1, const double * x2) { 
               // return normalized integral, divided by bin volume (dx1*dx...*dxn) 
               if (fIg1Dim) { 
                  double dV = *x2 - *x1;
                  return fIg1Dim->Integral( *x1, *x2)/dV; 
               }
               else if (fIgNDim) { 
                  double dV = 1; 
                  for (unsigned int i = 0; i < fDim; ++i) 
                     dV *= ( x2[i] - x1[i] );                   
                  return fIgNDim->Integral( x1, x2)/dV; 
//                   std::cout << " do integral btw x " << x1[0] << "  " << x2[0] << " y " << x1[1] << "  " << x2[1] << " dV = " << dV << " result = " << result << std::endl;
//                   return result; 
               }
               else 
                  assert(1.); // should never be here
               return 0;
            }

         private: 

            unsigned int fDim; 
            const double * fParams;
            //ROOT::Math::IParamMultiFunction * fFunc;  // copy of function in order to be able to change parameters    
            const ParamFunc * fFunc;       //  reference to a generic parametric function  
            ROOT::Math::IntegratorOneDim * fIg1Dim; 
            ROOT::Math::IntegratorMultiDim * fIgNDim; 
            ROOT::Math::IGenFunction * fFunc1Dim; 
            ROOT::Math::IMultiGenFunction * fFuncNDim; 
         }; 


         // derivative with respect of the parameter to be integrated
         template<class GradFunc = IGradModelFunction>
         struct ParamDerivFunc { 
            ParamDerivFunc(const GradFunc & f) : fFunc(f), fIpar(0) {}
            void SetDerivComponent(unsigned int ipar) { fIpar = ipar; }
            double operator() (const double *x, const double *p) const { 
               return fFunc.ParameterDerivative( x, p, fIpar ); 
            } 
            unsigned int NDim() const { return fFunc.NDim(); }
            const GradFunc & fFunc; 
            unsigned int fIpar; 
         };

         // simple gradient calculator using the 2 points rule

         class SimpleGradientCalculator { 

         public: 
            // construct from function and gradient dimension gdim
            // gdim = npar for parameter gradient
            // gdim = ndim for coordinate gradients
            // construct (the param values will be passed later)
            // one can choose between 2 points rule (1 extra evaluation) istrat=1
            // or two point rule (2 extra evaluation)
            // (found 2 points rule does not work correctly - minuit2FitBench fails) 
            SimpleGradientCalculator(int gdim, const IModelFunction & func,double eps = 2.E-8, int istrat = 1) : 
               fEps(eps),
               fPrecision(1.E-8 ), // sqrt(epsilon)
               fStrategy(istrat), 
               fN(gdim ),
               fFunc(func),
               fVec(std::vector<double>(gdim) ) // this can be probably optimized 
            {}

            // internal method to calculate single partial derivative
            // assume cached vector fVec is already set
            double DoParameterDerivative(const double *x, const double *p, double f0, int k) const {
               double p0 = p[k];
               double h = std::max( fEps* std::abs(p0), 8.0*fPrecision*(std::abs(p0) + fPrecision) );
               fVec[k] += h;
               double deriv = 0; 
               // t.b.d : treat case of infinities 
               //if (fval > - std::numeric_limits<double>::max() && fval < std::numeric_limits<double>::max() ) 
               double f1 = fFunc(x, &fVec.front() );
               if (fStrategy > 1) { 
                  fVec[k] = p0 - h; 
                  double f2 = fFunc(x, &fVec.front() );
                  deriv = 0.5 * ( f2 - f1 )/h;
               }
               else 
                  deriv = ( f1 - f0 )/h;
               
               fVec[k] = p[k]; // restore original p value
               return deriv; 
            }
            // number of dimension in x (needed when calculating the integrals) 
            unsigned int NDim() const { 
               return fFunc.NDim(); 
            }
            // number of parameters (needed for grad ccalculation)
            unsigned int NPar() const { 
               return fFunc.NPar(); 
            }

            double ParameterDerivative(const double *x, const double *p, int ipar) const {
               // fVec are the cached parameter values
               std::copy(p, p+fN, fVec.begin()); 
               double f0 = fFunc(x, p);
               return DoParameterDerivative(x,p,f0,ipar); 
            }

            // calculate all gradient at point (x,p) knnowing already value f0 (we gain a function eval.)
            void ParameterGradient(const double * x, const double * p, double f0, double * g) { 
               // fVec are the cached parameter values
               std::copy(p, p+fN, fVec.begin()); 
               for (unsigned int k = 0; k < fN; ++k) {
                  g[k] = DoParameterDerivative(x,p,f0,k);
               }
            }

            // calculate gradient w.r coordinate values
            void Gradient(const double * x, const double * p, double f0, double * g) { 
               // fVec are the cached coordinate values
               std::copy(x, x+fN, fVec.begin()); 
               for (unsigned int k = 0; k < fN; ++k) {
                  double x0 = x[k]; 
                  double h = std::max( fEps* std::abs(x0), 8.0*fPrecision*(std::abs(x0) + fPrecision) );
                  fVec[k] += h;
                  // t.b.d : treat case of infinities 
                  //if (fval > - std::numeric_limits<double>::max() && fval < std::numeric_limits<double>::max() ) 
                  double f1 = fFunc( &fVec.front(), p );
                  if (fStrategy > 1) { 
                     fVec[k] = x0 - h; 
                     double f2 = fFunc( &fVec.front(), p  );
                     g[k] = 0.5 * ( f2 - f1 )/h;
                  }
                  else 
                     g[k] = ( f1 - f0 )/h;

                  fVec[k] = x[k]; // restore original x value
               }
            }

         private:

            double fEps; 
            double fPrecision;
            int fStrategy; // strategy in calculation ( =1 use 2 point rule( 1 extra func) , = 2 use r point rule) 
            unsigned int fN; // gradient dimension
            const IModelFunction & fFunc; 
            mutable std::vector<double> fVec; // cached coordinates (or parameter values in case of gradientpar)
         };


         // function to avoid infinities or nan
         double CorrectValue(double rval) { 
            // avoid infinities or nan in  rval
            if (rval > - std::numeric_limits<double>::max() && rval < std::numeric_limits<double>::max() ) 
               return rval; 
            else if (rval < 0)
               // case -inf
               return -std::numeric_limits<double>::max(); 
            else 
               // case + inf or nan
               return  + std::numeric_limits<double>::max(); 
         }
         bool CheckValue(double & rval) { 
            if (rval > - std::numeric_limits<double>::max() && rval < std::numeric_limits<double>::max() ) 
               return true; 
            else if (rval < 0) { 
               // case -inf
               rval =  -std::numeric_limits<double>::max(); 
               return false; 
            }
            else { 
               // case + inf or nan
               rval =  + std::numeric_limits<double>::max(); 
               return false; 
            }
         }


         // calculation of the integral of the gradient functions
         // for a function providing derivative w.r.t parameters
         // x1 and x2 defines the integration interval , p the parameters
         template <class GFunc> 
         void CalculateGradientIntegral(const GFunc & gfunc, 
                                        const double *x1, const double * x2, const double * p, double *g) { 

            // needs to calculate the integral for each partial derivative
            ParamDerivFunc<GFunc> pfunc( gfunc); 
            IntegralEvaluator<ParamDerivFunc<GFunc> > igDerEval( pfunc, p, true);
            // loop on the parameters         
            unsigned int npar = gfunc.NPar();  
            for (unsigned int k = 0; k < npar; ++k ) { 
               pfunc.SetDerivComponent(k); 
               g[k] = igDerEval( x1, x2 ); 
            }
         }



      } // end namespace  FitUtil      



//___________________________________________________________________________________________________________________________
// for chi2 functions
//___________________________________________________________________________________________________________________________

double FitUtil::EvaluateChi2(const IModelFunction & func, const BinData & data, const double * p, unsigned int & nPoints) {  
   // evaluate the chi2 given a  function reference  , the data and returns the value and also in nPoints 
   // the actual number of used points
   // normal chi2 using only error on values (from fitting histogram)
   // optionally the integral of function in the bin is used 
   
   unsigned int n = data.Size();

   double chi2 = 0;
   nPoints = 0; // count the effective non-zero points
   
   // do not cache parameter values (it is not thread safe)
   //func.SetParameters(p); 

   
   // get fit option and check case if using integral of bins
   const DataOptions & fitOpt = data.Opt();
   bool useBinIntegral = fitOpt.fIntegral && data.HasBinEdges(); 
   bool useBinVolume = (fitOpt.fBinVolume && data.HasBinEdges());

#ifdef DEBUG
   std::cout << "\n\nFit data size = " << n << std::endl;
   std::cout << "evaluate chi2 using function " << &func << "  " << p << std::endl; 
   std::cout << "use empty bins  " << fitOpt.fUseEmpty << std::endl;
   std::cout << "use integral    " << fitOpt.fIntegral << std::endl;
   std::cout << "use all error=1 " << fitOpt.fErrors1 << std::endl;
#endif


   IntegralEvaluator<> igEval( func, p, useBinIntegral); 

   double maxResValue = std::numeric_limits<double>::max() /n;
   double wrefVolume = 1.0; 
   std::vector<double> xc; 
   if (useBinVolume) { 
      wrefVolume /= data.RefVolume();
      xc.resize(data.NDim() );
   }

   for (unsigned int i = 0; i < n; ++ i) { 


      double y, invError; 
      // in case of no error in y invError=1 is returned
      const double * x1 = data.GetPoint(i,y, invError);

      double fval = 0;

      double binVolume = 1.0; 
      if (useBinVolume) { 
         unsigned int ndim = data.NDim(); 
         const double * x2 = data.BinUpEdge(i);  
         for (unsigned int j = 0; j < ndim; ++j) {
            binVolume *= std::abs( x2[j]-x1[j] );
            xc[j] = 0.5*(x2[j]+ x1[j]);
         }
         // normalize the bin volume using a reference value
         binVolume *= wrefVolume;
      }

      const double * x = (useBinVolume) ? &xc.front() : x1;

      if (!useBinIntegral) {
         fval = func ( x, p );
      }
      else {
         // calculate integral normalized by bin volume
         // need to set function and parameters here in case loop is parallelized
         fval = igEval( x1, data.BinUpEdge(i)) ; 
      }
      // normalize result if requested according to bin volume
      if (useBinVolume) fval *= binVolume;

//#define DEBUG
#ifdef DEBUG      
      std::cout << x[0] << "  " << y << "  " << 1./invError << " params : "; 
      for (unsigned int ipar = 0; ipar < func.NPar(); ++ipar) 
         std::cout << p[ipar] << "\t";
      std::cout << "\tfval = " << fval << " bin volume " << binVolume << " ref " << wrefVolume << std::endl; 
#endif
//#undef DEBUG

      if (invError > 0) { 
         nPoints++;

         double tmp = ( y -fval )* invError;  	  
         double resval = tmp * tmp;
         

         // avoid inifinity or nan in chi2 values due to wrong function values 
         if ( resval < maxResValue )  
            chi2 += resval; 
         else {  
            //nRejected++; 
            chi2 += maxResValue;
         }
      }

      
   }
   nPoints=n;

#ifdef DEBUG
   std::cout << "chi2 = " << chi2 << " n = " << nPoints  /*<< " rejected = " << nRejected */ << std::endl;
#endif


   return chi2;
}


//___________________________________________________________________________________________________________________________

double FitUtil::EvaluateChi2Effective(const IModelFunction & func, const BinData & data, const double * p, unsigned int & nPoints) {  
   // evaluate the chi2 given a  function reference  , the data and returns the value and also in nPoints 
   // the actual number of used points
   // method using the error in the coordinates
   // integral of bin does not make sense in this case
   
   unsigned int n = data.Size();

#ifdef DEBUG
   std::cout << "\n\nFit data size = " << n << std::endl;
   std::cout << "evaluate effective chi2 using function " << &func << "  " << p << std::endl; 
#endif

   assert(data.HaveCoordErrors() ); 

   double chi2 = 0;
   //int nRejected = 0; 
   

   //func.SetParameters(p); 

   unsigned int ndim = func.NDim();

   // use Richardson derivator
   ROOT::Math::RichardsonDerivator derivator;

   double maxResValue = std::numeric_limits<double>::max() /n;



   for (unsigned int i = 0; i < n; ++ i) { 


      double y = 0;
      const double * x = data.GetPoint(i,y);

      double fval = func( x, p );

      double delta_y_func = y - fval; 


      double ey = 0;
      const double * ex = 0; 
      if (!data.HaveAsymErrors() )
         ex = data.GetPointError(i, ey); 
      else { 
         double eylow, eyhigh = 0; 
         ex = data.GetPointError(i, eylow, eyhigh); 
         if ( delta_y_func < 0) 
            ey = eyhigh; // function is higher than points 
         else
            ey = eylow; 
      }
      double e2 = ey * ey; 
      // before calculating the gradient check that all error in x are not zero
      unsigned int j = 0; 
      while ( j < ndim && ex[j] == 0.)  { j++; } 
      // if j is less ndim some elements are not zero
      if (j < ndim) { 
         // need an adapter from a multi-dim function to a one-dimensional
         ROOT::Math::OneDimMultiFunctionAdapter<const IModelFunction &> f1D(func,x,0,p);
         // select optimal step size  (use 10--2 by default as was done in TF1:
         double kEps = 0.01;
         double kPrecision = 1.E-8;
         for (unsigned int icoord = 0; icoord < ndim; ++icoord) { 
            // calculate derivative for each coordinate 
            if (ex[icoord] > 0) {               
               //gradCalc.Gradient(x, p, fval, &grad[0]);
               f1D.SetCoord(icoord);
               // optimal spep size (take ex[] as scale for the points and 1% of it 
               double x0= x[icoord];
               double h = std::max( kEps* std::abs(ex[icoord]), 8.0*kPrecision*(std::abs(x0) + kPrecision) );
               double deriv = derivator.Derivative1(f1D, x[icoord], h);  
               double edx = ex[icoord] * deriv; 
               e2 += edx * edx;  
#ifdef DEBUG
               std::cout << "error for coord " << icoord << " = " << ex[icoord] << " deriv " << deriv << std::endl;
#endif
            }
         } 
      }
      double w2 = (e2 > 0) ? 1.0/e2 : 0;  
      double resval = w2 * ( y - fval ) *  ( y - fval); 

#ifdef DEBUG      
      std::cout << x[0] << "  " << y << " ex " << ex[0] << " ey  " << ey << " params : "; 
      for (unsigned int ipar = 0; ipar < func.NPar(); ++ipar) 
         std::cout << p[ipar] << "\t";
      std::cout << "\tfval = " << fval << "\tresval = " << resval << std::endl; 
#endif
      
      // avoid (infinity and nan ) in the chi2 sum 
      // eventually add possibility of excluding some points (like singularity) 
      if ( resval < maxResValue )  
         chi2 += resval; 
      else 
         chi2 += maxResValue;
      //nRejected++; 
      
   }
   
   // reset the number of fitting data points
   nPoints = n;  // no points are rejected 
   //if (nRejected != 0)  nPoints = n - nRejected;   

#ifdef DEBUG
   std::cout << "chi2 = " << chi2 << " n = " << nPoints  << std::endl;
#endif

   return chi2;

}


//___________________________________________________________________________________________________________________________
double FitUtil::EvaluateChi2Residual(const IModelFunction & func, const BinData & data, const double * p, unsigned int i, double * g) {  
   // evaluate the chi2 contribution (residual term) only for data with no coord-errors
   // This function is used in the specialized least square algorithms like FUMILI or L.M.
   // if we have error on the coordinates the method is not yet implemented 
   //  integral option is also not yet implemented
   //  one can use in that case normal chi2 method
  
   if (data.GetErrorType() == BinData::kCoordError && data.Opt().fCoordErrors ) { 
      MATH_ERROR_MSG("FitUtil::EvaluateChi2Residual","Error on the coordinates are not used in calculating Chi2 residual");
      return 0; // it will assert otherwise later in GetPoint
   }


   //func.SetParameters(p);

   double y, invError = 0; 

   const DataOptions & fitOpt = data.Opt();
   bool useBinIntegral = fitOpt.fIntegral && data.HasBinEdges(); 
   bool useBinVolume = (fitOpt.fBinVolume && data.HasBinEdges());

   const double * x1 = data.GetPoint(i,y, invError);

   IntegralEvaluator<> igEval( func, p, useBinIntegral); 
   double fval = 0; 
   unsigned int ndim = data.NDim(); 
   double binVolume = 1.0; 
   const double * x2 = 0; 
   if (useBinVolume || useBinIntegral) x2 = data.BinUpEdge(i);  

   double * xc = 0;
 
   if (useBinVolume) { 
      xc = new double[ndim];
      for (unsigned int j = 0; j < ndim; ++j) {
         binVolume *= std::abs( x2[j]-x1[j] );
         xc[j] = 0.5*(x2[j]+ x1[j]);         
      }
      // normalize the bin volume using a reference value
      binVolume /= data.RefVolume();
   }

   const double * x = (useBinVolume) ? xc : x1;

   if (!useBinIntegral) {
      fval = func ( x, p );
   }
   else {
      // calculate integral (normalized by bin volume) 
      // need to set function and parameters here in case loop is parallelized
      fval = igEval( x1, x2) ; 
   }
   // normalize result if requested according to bin volume
   if (useBinVolume) fval *= binVolume;

   double resval =   ( y -fval )* invError;  	   

   // avoid infinities or nan in  resval
   resval = CorrectValue(resval);
   
   // estimate gradient 
   if (g != 0) {  

      unsigned int npar = func.NPar(); 
      const IGradModelFunction * gfunc = dynamic_cast<const IGradModelFunction *>( &func); 

      if (gfunc != 0) { 
         //case function provides gradient
         if (!useBinIntegral ) {
            gfunc->ParameterGradient(  x , p, g );  
         }
         else { 
            // needs to calculate the integral for each partial derivative
            CalculateGradientIntegral( *gfunc, x1, x2, p, g); 
         }
      }
      else { 
         SimpleGradientCalculator  gc( npar, func); 
         if (!useBinIntegral ) 
            gc.ParameterGradient(x, p, fval, g); 
         else { 
            // needs to calculate the integral for each partial derivative
            CalculateGradientIntegral( gc, x1, x2, p, g); 
         }
      }
      // mutiply by - 1 * weight
      for (unsigned int k = 0; k < npar; ++k) {
         g[k] *= - invError;
         if (useBinVolume) g[k] *= binVolume;      
      }       
   }

   if (useBinVolume) delete [] xc;

   return resval; 

}

void FitUtil::EvaluateChi2Gradient(const IModelFunction & f, const BinData & data, const double * p, double * grad, unsigned int & nPoints) { 
   // evaluate the gradient of the chi2 function
   // this function is used when the model function knows how to calculate the derivative and we can  
   // avoid that the minimizer re-computes them 
   //
   // case of chi2 effective (errors on coordinate) is not supported

   if ( data.HaveCoordErrors() ) {
      MATH_ERROR_MSG("FitUtil::EvaluateChi2Residual","Error on the coordinates are not used in calculating Chi2 gradient");            return; // it will assert otherwise later in GetPoint
   }

   unsigned int nRejected = 0; 

   const IGradModelFunction * fg = dynamic_cast<const IGradModelFunction *>( &f); 
   assert (fg != 0); // must be called by a gradient function

   const IGradModelFunction & func = *fg; 
   unsigned int n = data.Size();


#ifdef DEBUG
   std::cout << "\n\nFit data size = " << n << std::endl;
   std::cout << "evaluate chi2 using function gradient " << &func << "  " << p << std::endl; 
#endif

   const DataOptions & fitOpt = data.Opt();
   bool useBinIntegral = fitOpt.fIntegral && data.HasBinEdges(); 
   bool useBinVolume = (fitOpt.fBinVolume && data.HasBinEdges());

   double wrefVolume = 1.0; 
   std::vector<double> xc; 
   if (useBinVolume) { 
      wrefVolume /= data.RefVolume();
      xc.resize(data.NDim() );
   }

   IntegralEvaluator<> igEval( func, p, useBinIntegral); 

   //int nRejected = 0; 
   // set values of parameters 

   unsigned int npar = func.NPar(); 
   //   assert (npar == NDim() );  // npar MUST be  Chi2 dimension
   std::vector<double> gradFunc( npar ); 
   // set all vector values to zero
   std::vector<double> g( npar); 

   for (unsigned int i = 0; i < n; ++ i) { 


      double y, invError = 0; 
      const double * x1 = data.GetPoint(i,y, invError);

      double fval = 0; 
      const double * x2 = 0; 

      double binVolume = 1; 
      if (useBinVolume) { 
         unsigned int ndim = data.NDim(); 
         x2 = data.BinUpEdge(i);  
         for (unsigned int j = 0; j < ndim; ++j) {
            binVolume *= std::abs( x2[j]-x1[j] );
            xc[j] = 0.5*(x2[j]+ x1[j]);
         }
         // normalize the bin volume using a reference value
         binVolume *= wrefVolume;
      }

      const double * x = (useBinVolume) ? &xc.front() : x1;

      if (!useBinIntegral ) {
         fval = func ( x, p ); 
         func.ParameterGradient(  x , p, &gradFunc[0] ); 
      }
      else { 
         x2 = data.BinUpEdge(i); 
         // calculate normalized integral and gradient (divided by bin volume)
         // need to set function and parameters here in case loop is parallelized 
         fval = igEval( x1, x2 ) ; 
         CalculateGradientIntegral( func, x1, x2, p, &gradFunc[0]); 
      }
      if (useBinVolume) fval *= binVolume;

#ifdef DEBUG      
      std::cout << x[0] << "  " << y << "  " << 1./invError << " params : "; 
      for (unsigned int ipar = 0; ipar < npar; ++ipar) 
         std::cout << p[ipar] << "\t";
      std::cout << "\tfval = " << fval << std::endl; 
#endif
      if ( !CheckValue(fval) ) { 
         nRejected++; 
         continue;
      } 

      // loop on the parameters
      unsigned int ipar = 0; 
      for ( ; ipar < npar ; ++ipar) { 

         // correct gradient for bin volumes
         if (useBinVolume) gradFunc[ipar] *= binVolume;

         // avoid singularity in the function (infinity and nan ) in the chi2 sum 
         // eventually add possibility of excluding some points (like singularity) 
         double dfval = gradFunc[ipar];
         if ( !CheckValue(dfval) ) { 
               break; // exit loop on parameters
         } 
 
         // calculate derivative point contribution
         double tmp = - 2.0 * ( y -fval )* invError * invError * gradFunc[ipar];  	  
         g[ipar] += tmp;
      
      }

      if ( ipar < npar ) { 
          // case loop was broken for an overflow in the gradient calculation  
         nRejected++; 
         continue;
      } 


   } 

   // correct the number of points
   nPoints = n; 
   if (nRejected != 0)  {
      assert(nRejected <= n);
      nPoints = n - nRejected;
      if (nPoints < npar)  MATH_ERROR_MSG("FitUtil::EvaluateChi2Gradient","Error - too many points rejected for overflow in gradient calculation");
   } 

   // copy result 
   std::copy(g.begin(), g.end(), grad);

}

//______________________________________________________________________________________________________
//
//  Log Likelihood functions       
//_______________________________________________________________________________________________________

// utility function used by the likelihoods 

// for LogLikelihood functions

double FitUtil::EvaluatePdf(const IModelFunction & func, const UnBinData & data, const double * p, unsigned int i, double * g) {  
   // evaluate the pdf contribution to the generic logl function in case of bin data
   // return actually the log of the pdf and its derivatives 

   //func.SetParameters(p);


   const double * x = data.Coords(i);
   double fval = func ( x, p ); 
   double logPdf = ROOT::Math::Util::EvalLog(fval);
   //return 
   if (g == 0) return logPdf;

   const IGradModelFunction * gfunc = dynamic_cast<const IGradModelFunction *>( &func); 

   // gradient  calculation
   if (gfunc != 0) { 
      //case function provides gradient
      gfunc->ParameterGradient(  x , p, g );  
   }
   else { 
      // estimate gradieant numerically with simple 2 point rule 
      // should probably calculate gradient of log(pdf) is more stable numerically
      SimpleGradientCalculator gc(func.NPar(), func); 
      gc.ParameterGradient(x, p, fval, g ); 
   }       
   // divide gradient by function value since returning the logs
   for (unsigned int ipar = 0; ipar < func.NPar(); ++ipar) {
      g[ipar] /= fval; // this should be checked against infinities
   }

#ifdef DEBUG
   std::cout << x[i] << "\t"; 
   std::cout << "\tpar = [ " << func.NPar() << " ] =  "; 
   for (unsigned int ipar = 0; ipar < func.NPar(); ++ipar) 
      std::cout << p[ipar] << "\t";
   std::cout << "\tfval = " << fval;
   std::cout << "\tgrad = [ "; 
   for (unsigned int ipar = 0; ipar < func.NPar(); ++ipar) 
      std::cout << g[ipar] << "\t";
   std::cout << " ] "   << std::endl; 
#endif


   return logPdf;
}

double FitUtil::EvaluateLogL(const IModelFunction & func, const UnBinData & data, const double * p,
                                   int iWeight,  bool extended, unsigned int &nPoints) {  
   // evaluate the LogLikelihood 

   unsigned int n = data.Size();

#ifdef DEBUG
   std::cout << "\n\nFit data size = " << n << std::endl;
   std::cout << "func pointer is " << typeid(func).name() << std::endl;
#endif

   double logl = 0;
   //unsigned int nRejected = 0; 

   // this is needed if function must be normalized 
   bool normalizeFunc = false; 
   double norm = 1.0;
   if (normalizeFunc) { 
      // compute integral of the function 
      std::vector<double> xmin(data.NDim());
      std::vector<double> xmax(data.NDim());
      IntegralEvaluator<> igEval( func, p, true); 
      data.Range().GetRange(&xmin[0],&xmax[0]);
      norm = igEval.Integral(&xmin[0],&xmax[0]);
   }

   // needed to compue effective global weight in case of extended likelihood 
   double sumW = 0;
   double sumW2 = 0;

   for (unsigned int i = 0; i < n; ++ i) { 
      const double * x = data.Coords(i);
      double fval = func ( x, p ); 
      if (normalizeFunc) fval = fval / norm;

#ifdef DEBUG      
      std::cout << "x [ " << data.NDim() << " ] = "; 
      for (unsigned int j = 0; j < data.NDim(); ++j)
         std::cout << x[j] << "\t"; 
      std::cout << "\tpar = [ " << func.NPar() << " ] =  "; 
      for (unsigned int ipar = 0; ipar < func.NPar(); ++ipar) 
         std::cout << p[ipar] << "\t";
      std::cout << "\tfval = " << fval << std::endl; 
#endif
      // function EvalLog protects against negative or too small values of fval
      double logval =  ROOT::Math::Util::EvalLog( fval);       
      if (iWeight > 0) { 
         double weight = data.Weight(i); 
         logval *= weight; 
         if (iWeight ==2) { 
            logval *= weight; // use square of weights in likelihood
            if (extended) { 
               // needed sum of weights and sum of weight square if likelkihood is extended
               sumW += weight; 
               sumW2 += weight*weight; 
            }
         }
      }
      logl += logval;
   }

   if (extended) { 
      // add Poisson extended term
      double extendedTerm = 0; // extended term in likelihood  
      double nuTot = 0;
      // nuTot is integral of function in the range
      // if function has been normalized integral has been already computed 
      if (!normalizeFunc) {
         IntegralEvaluator<> igEval( func, p, true); 
         std::vector<double> xmin(data.NDim());
         std::vector<double> xmax(data.NDim());
         data.Range().GetRange(&xmin[0],&xmax[0]);
         nuTot = igEval.Integral( &xmin[0], &xmax[0]);
         // force to be last parameter value
         //nutot = p[func.NDim()-1];
         if (iWeight != 2)  
            extendedTerm = - nuTot;  // no need to add in this case n log(nu) since is already computed before
         else {            
            // case use weight square in likelihood : compute total effective weight = sw2/sw
            // ignore for the moment case when sumW is zero 
            extendedTerm = - (sumW2 / sumW) * nuTot;
         }
            
      }
      else {
         nuTot = norm;
         extendedTerm = - nuTot + double(n) *  ROOT::Math::Util::EvalLog( nuTot); 
         // in case of weights need to use here sum of weights (to be done)
      }
      logl += extendedTerm;

#ifdef DEBUG
      // for (unsigned int ipar = 0; ipar < func.NPar(); ++ipar) 
      //    std::cout << p[ipar] << "\t";
      // std::cout << std::endl;
      std::cout << "fit is extended n = " << n << " nutot " << nuTot << " extended LL term = " <<  extendedTerm << " logl = " << logl
                << std::endl;
#endif
   }
   
   // reset the number of fitting data points
   nPoints = n; 
//    if (nRejected != 0)  { 
//       assert(nRejected <= n);
//       nPoints = n - nRejected;
//       if ( nPoints < func.NPar() ) 
//          MATH_ERROR_MSG("FitUtil::EvaluateLogL","Error too many points rejected because of bad pdf values");

//    }
#ifdef DEBUG
   std::cout << "Logl = " << logl << " np = " << nPoints << std::endl;
#endif
   
   return -logl;
}

void FitUtil::EvaluateLogLGradient(const IModelFunction & f, const UnBinData & data, const double * p, double * grad, unsigned int & ) { 
   // evaluate the gradient of the log likelihood function

   const IGradModelFunction * fg = dynamic_cast<const IGradModelFunction *>( &f); 
   assert (fg != 0); // must be called by a grad function
   const IGradModelFunction & func = *fg; 

   unsigned int n = data.Size();
   //int nRejected = 0; 

   unsigned int npar = func.NPar(); 
   std::vector<double> gradFunc( npar ); 
   std::vector<double> g( npar); 

   for (unsigned int i = 0; i < n; ++ i) { 
      const double * x = data.Coords(i);
      double fval = func ( x , p); 
      func.ParameterGradient( x, p, &gradFunc[0] );
      for (unsigned int kpar = 0; kpar < npar; ++ kpar) { 
         if (fval > 0)  
            g[kpar] -= 1./fval * gradFunc[ kpar ]; 
         else if (gradFunc [ kpar] != 0) { 
            const double kdmax1 = std::sqrt( std::numeric_limits<double>::max() );
            const double kdmax2 = std::numeric_limits<double>::max() / (4*n);
            double gg = kdmax1 * gradFunc[ kpar ];  
            if ( gg > 0) gg = std::min( gg, kdmax2);
            else gg = std::max(gg, - kdmax2);
            g[kpar] -= gg;
         }
         // if func derivative is zero term is also zero so do not add in g[kpar]
      }
            
    // copy result 
   std::copy(g.begin(), g.end(), grad);
   }
}
//_________________________________________________________________________________________________
// for binned log likelihood functions      
//------------------------------------------------------------------------------------------------
double FitUtil::EvaluatePoissonBinPdf(const IModelFunction & func, const BinData & data, const double * p, unsigned int i, double * g ) {  
   // evaluate the pdf (Poisson) contribution to the logl (return actually log of pdf)
   // and its gradient

   double y = 0; 
   const double * x1 = data.GetPoint(i,y);

   const DataOptions & fitOpt = data.Opt();
   bool useBinIntegral = fitOpt.fIntegral && data.HasBinEdges(); 
   bool useBinVolume = (fitOpt.fBinVolume && data.HasBinEdges());

   IntegralEvaluator<> igEval( func, p, useBinIntegral); 
   double fval = 0; 
   const double * x2 = 0; 
   // calculate the bin volume
   double binVolume = 1; 
   std::vector<double> xc; 
   if (useBinVolume) { 
      unsigned int ndim = data.NDim(); 
      xc.resize(ndim);
      x2 = data.BinUpEdge(i);  
      for (unsigned int j = 0; j < ndim; ++j) {
         binVolume *= std::abs( x2[j]-x1[j] );
         xc[j] = 0.5*(x2[j]+ x1[j]);
      }
      // normalize the bin volume using a reference value
      binVolume /= data.RefVolume();
   }

   const double * x = (useBinVolume) ? &xc.front() : x1;

   if (!useBinIntegral ) {
      fval = func ( x, p ); 
   }
   else { 
      // calculate integral normalized (divided by bin volume) 
      x2 =  data.BinUpEdge(i);
      fval = igEval( x1, x2 ) ; 
   }
   if (useBinVolume) fval *= binVolume;

   // logPdf for Poisson: ignore constant term depending on N
   fval = std::max(fval, 0.0);  // avoid negative or too small values 
   double logPdf =  - fval; 
   if (y > 0.0) {
      // include also constants due to saturate model (see Baker-Cousins paper)
      logPdf += y * ROOT::Math::Util::EvalLog( fval / y) + y;  
   }
   // need to return the pdf contribution (not the log)

   //double pdfval =  std::exp(logPdf);

  //if (g == 0) return pdfval; 
   if (g == 0) return logPdf; 

   unsigned int npar = func.NPar(); 
   const IGradModelFunction * gfunc = dynamic_cast<const IGradModelFunction *>( &func); 

   // gradient  calculation
   if (gfunc != 0) { 
      //case function provides gradient
      if (!useBinIntegral ) 
         gfunc->ParameterGradient(  x , p, g );  
      else { 
         // needs to calculate the integral for each partial derivative
         CalculateGradientIntegral( *gfunc, x1, x2, p, g); 
      }

   }
   else {       
      SimpleGradientCalculator  gc(func.NPar(), func); 
      if (!useBinIntegral ) 
         gc.ParameterGradient(x, p, fval, g); 
      else { 
        // needs to calculate the integral for each partial derivative
         CalculateGradientIntegral( gc, x1, x2, p, g);  
      } 
   }
   // correct g[] do be derivative of poisson term 
   for (unsigned int k = 0; k < npar; ++k) {
      // apply bin volume correction
      if (useBinVolume) g[k] *= binVolume;  

      // correct for Poisson term
      if ( fval > 0) 
         g[k] *= ( y/fval - 1.) ;//* pdfval; 
      else if (y > 0) { 
         const double kdmax1 = std::sqrt( std::numeric_limits<double>::max() );
         g[k] *= kdmax1; 
      }
      else   // y == 0 cannot have  negative y
         g[k] *= -1;         
   }       
     

#ifdef DEBUG
   std::cout << "x = " << x[0] << " logPdf = " << logPdf << " grad"; 
   for (unsigned int ipar = 0; ipar < npar; ++ipar) 
      std::cout << g[ipar] << "\t";
   std::cout << std::endl;
#endif 

//   return pdfval;
   return logPdf;
}

double FitUtil::EvaluatePoissonLogL(const IModelFunction & func, const BinData & data, 
                                    const double * p, int iWeight, bool extended,  unsigned int &   nPoints ) {  
   // evaluate the Poisson Log Likelihood
   // for binned likelihood fits
   // this is Sum ( f(x_i)  -  y_i * log( f (x_i) ) )
   // add as well constant term for saturated model to make it like a Chi2/2
   // by default is etended. If extended is false the fit is not extended and 
   // the global poisson term is removed (i.e is a binomial fit)
   // (remember that in this case one needs to have a function with a fixed normalization
   // like in a non extended binned fit)
   //
   // if use Weight use a weighted dataset 
   // iWeight = 1 ==> logL = Sum( w f(x_i) )
   // case of iWeight==1 is actually identical to weight==0
   // iWeight = 2 ==> logL = Sum( w*w * f(x_i) )
   //
   // nPoints returns the points where bin content is not zero
         

   unsigned int n = data.Size();
#ifdef DEBUG
   std::cout << "Evaluate PoissonLogL for params = [ "; 
   for (unsigned int j=0; j < func.NPar(); ++j) std::cout << p[j] << " , ";
   std::cout << "]  - data size = " << n << std::endl;
#endif
   
   double nloglike = 0;  // negative loglikelihood 
   nPoints = 0;  // npoints


   // get fit option and check case of using integral of bins
   const DataOptions & fitOpt = data.Opt();
   bool useBinIntegral = fitOpt.fIntegral && data.HasBinEdges(); 
   bool useBinVolume = (fitOpt.fBinVolume && data.HasBinEdges());
   bool useW2 = (iWeight == 2);

   double wrefVolume = 1.0; 
   std::vector<double> xc; 
   if (useBinVolume) { 
      wrefVolume /= data.RefVolume();
      xc.resize(data.NDim() );
   }

   IntegralEvaluator<> igEval( func, p, fitOpt.fIntegral); 

   // double nuTot = 0; // total number of expected events (needed for non-extended fits) 
   // double wTot = 0; // sum of all weights  
   // double w2Tot = 0; // sum of weight squared  (these are needed for useW2)


   for (unsigned int i = 0; i < n; ++ i) { 
      const double * x1 = data.Coords(i);
      double y = data.Value(i);
      
      double fval = 0;   
      double binVolume = 1.0; 

      if (useBinVolume) { 
         unsigned int ndim = data.NDim(); 
         const double * x2 = data.BinUpEdge(i);  
         for (unsigned int j = 0; j < ndim; ++j) {
            binVolume *= std::abs( x2[j]-x1[j] );
            xc[j] = 0.5*(x2[j]+ x1[j]);
         }
         // normalize the bin volume using a reefrence value
         binVolume *= wrefVolume;
      }

      const double * x = (useBinVolume) ? &xc.front() : x1;

      if (!useBinIntegral) {
         fval = func ( x, p );
      }
      else {
         // calculate integral (normalized by bin volume) 
         // need to set function and parameters here in case loop is parallelized
         fval = igEval( x1, data.BinUpEdge(i)) ; 
      }
      if (useBinVolume) fval *= binVolume;



#ifdef DEBUG
      int NSAMPLE = 100;
      if (i%NSAMPLE == 0) { 
         std::cout << "evt " << i << " x1 = [ "; 
         for (unsigned int j=0; j < func.NDim(); ++j) std::cout << x[j] << " , ";
         std::cout << "]  ";
         if (fitOpt.fIntegral) { 
            std::cout << "x2 = [ "; 
            for (unsigned int j=0; j < func.NDim(); ++j) std::cout << data.BinUpEdge(i)[j] << " , ";
            std::cout << "] ";
         }
         std::cout << "  y = " << y << " fval = " << fval << std::endl;
      }
#endif


      // EvalLog protects against 0 values of fval but don't want to add in the -log sum 
      // negative values of fval 
      fval = std::max(fval, 0.0);


      double tmp = 0; 
      if (useW2) { 
         // apply weight correction . Effective weight is error^2/ y
         // and expected events in bins is fval/weight
         // can apply correction only when y is not zero otherwise weight is undefined
         // (in case of weighted likelihood I don't care about the constant term due to 
         // the saturated model)
         if (y != 0) { 
            double error = data.Error(i);
            double weight = (error*error)/y;  // this is the bin effective weight
            if (extended) { 
               tmp = fval * weight;
               // wTot  += weight; 
               // w2Tot += weight*weight; 
            }
            tmp -= weight * y * ROOT::Math::Util::EvalLog( fval);
         }
         
         //  need to compute total weight and weight-square
         // if (extended ) { 
         //    nuTot += fval;
         // }

      }
      else {
         // standard case no weights or iWeight=1 
         // this is needed for Poisson likelihood (which are extened and not for multinomial) 
         // the formula below  include constant term due to likelihood of saturated model (f(x) = y)
         // (same formula as in Baker-Cousins paper, page 439 except a factor of 2
         if (extended) tmp = fval -y ;
         if (y >  0) { 
            tmp +=  y *  (ROOT::Math::Util::EvalLog( y) - ROOT::Math::Util::EvalLog(fval));  
            nPoints++;
         }
      }


      nloglike +=  tmp;  
   }
   
   // if (notExtended) { 
   //    // not extended : remove from the Likelihood the global Poisson term
   //    if (!useW2)  
   //       nloglike -= nuTot - yTot * ROOT::Math::Util::EvalLog( nuTot);
   //    else {
   //       // this needs to be checked
   //       nloglike -= wTot* nuTot - wTot* yTot * ROOT::Math::Util::EvalLog( nuTot);
   //    }
         
   // }

   // if (extended && useW2) { 
   //    // effective total weight is total sum of weight square / sum of weights
   //    //nloglike += (w2Tot/wTot) * nuTot; 
   // }

   
#ifdef DEBUG
   std::cout << "Loglikelihood  = " << nloglike << std::endl;
#endif
   
   return nloglike;  
}

void FitUtil::EvaluatePoissonLogLGradient(const IModelFunction & f, const BinData & data, const double * p, double * grad ) { 
   // evaluate the gradient of the Poisson log likelihood function

   const IGradModelFunction * fg = dynamic_cast<const IGradModelFunction *>( &f); 
   assert (fg != 0); // must be called by a grad function
   const IGradModelFunction & func = *fg; 

   unsigned int n = data.Size();

   const DataOptions & fitOpt = data.Opt();
   bool useBinIntegral = fitOpt.fIntegral && data.HasBinEdges(); 
   bool useBinVolume = (fitOpt.fBinVolume && data.HasBinEdges());

   double wrefVolume = 1.0;
   std::vector<double> xc;  
   if (useBinVolume) {
      wrefVolume /= data.RefVolume();
      xc.resize(data.NDim() );
   }

   IntegralEvaluator<> igEval( func, p, useBinIntegral); 

   unsigned int npar = func.NPar(); 
   std::vector<double> gradFunc( npar ); 
   std::vector<double> g( npar); 

   for (unsigned int i = 0; i < n; ++ i) { 
      const double * x1 = data.Coords(i);
      double y = data.Value(i);
      double fval = 0; 
      const double * x2 = 0; 

      double binVolume = 1.0; 
      if (useBinVolume) { 
         x2 = data.BinUpEdge(i);  
         unsigned int ndim = data.NDim(); 
         for (unsigned int j = 0; j < ndim; ++j) { 
            binVolume *= std::abs( x2[j]-x1[j] );
            xc[j] = 0.5*(x2[j]+ x1[j]);
         }
         // normalize the bin volume using a reference value
         binVolume *= wrefVolume;
      }

      const double * x = (useBinVolume) ? &xc.front() : x1;

      if (!useBinIntegral) {
         fval = func ( x, p );
         func.ParameterGradient(  x , p, &gradFunc[0] ); 
      }
      else {
         // calculate integral (normalized by bin volume) 
         // need to set function and parameters here in case loop is parallelized
         x2 = data.BinUpEdge(i);
         fval = igEval( x1, x2) ; 
         CalculateGradientIntegral( func, x1, x2, p, &gradFunc[0]); 
      }
      if (useBinVolume) fval *= binVolume;
      
      // correct the gradient
      for (unsigned int kpar = 0; kpar < npar; ++ kpar) { 

         // correct gradient for bin volumes
         if (useBinVolume) gradFunc[kpar] *= binVolume; 

         // df/dp * (1.  - y/f )
         if (fval > 0)  
            g[kpar] += gradFunc[ kpar ] * ( 1. - y/fval ); 
         else if (gradFunc [ kpar] != 0) { 
            const double kdmax1 = std::sqrt( std::numeric_limits<double>::max() );
            const double kdmax2 = std::numeric_limits<double>::max() / (4*n);
            double gg = kdmax1 * gradFunc[ kpar ];  
            if ( gg > 0) gg = std::min( gg, kdmax2);
            else gg = std::max(gg, - kdmax2);
            g[kpar] -= gg;
         }
      }            

      // copy result 
      std::copy(g.begin(), g.end(), grad);
   }
}
   
}

} // end namespace ROOT