// @(#)root/minuit2:$Id$
// Authors: M. Winkler, F. James, L. Moneta, A. Zsenei   2003-2005

/**********************************************************************
 *                                                                    *
 * Copyright (c) 2005 LCG ROOT Math team,  CERN/PH-SFT                *
 *                                                                    *
 **********************************************************************/

#include "Minuit2/FumiliBuilder.h"
#include "Minuit2/FumiliStandardMaximumLikelihoodFCN.h"
#include "Minuit2/GradientCalculator.h"
//#include "Minuit2/Numerical2PGradientCalculator.h"
#include "Minuit2/MinimumState.h"
#include "Minuit2/MinimumError.h"
#include "Minuit2/FunctionGradient.h"
#include "Minuit2/FunctionMinimum.h"
#include "Minuit2/MnLineSearch.h"
#include "Minuit2/MinimumSeed.h"
#include "Minuit2/MnFcn.h"
#include "Minuit2/MnMachinePrecision.h"
#include "Minuit2/MnPosDef.h"
#include "Minuit2/MnParabolaPoint.h"
#include "Minuit2/LaSum.h"
#include "Minuit2/LaProd.h"
#include "Minuit2/MnStrategy.h"
#include "Minuit2/MnHesse.h"



// //#define DEBUG 1
// #if defined(DEBUG) || defined(WARNINGMSG)
#include "Minuit2/MnPrint.h"
//#endif



namespace ROOT {

   namespace Minuit2 {




double inner_product(const LAVector&, const LAVector&);

FunctionMinimum FumiliBuilder::Minimum(const MnFcn& fcn, const GradientCalculator& gc, const MinimumSeed& seed, const MnStrategy& strategy, unsigned int maxfcn, double edmval) const {
   // top level function to find minimum from a given initial seed
   // iterate on a minimum search in case of first attempt is not successful

   edmval *= 0.0001;
   //edmval *= 0.1; // use small factor for Fumili


#ifdef DEBUG
   std::cout<<"FumiliBuilder convergence when edm < "<<edmval<<std::endl;
#endif

   if(seed.Parameters().Vec().size() == 0) {
      return FunctionMinimum(seed, fcn.Up());
   }


   //   double edm = Estimator().Estimate(seed.Gradient(), seed.Error());
   double edm = seed.State().Edm();

   FunctionMinimum min(seed, fcn.Up() );

   if(edm < 0.) {
#ifdef WARNINGMSG
      MN_INFO_MSG("FumiliBuilder: initial matrix not pos.def.");
#endif
      return min;
   }

   std::vector<MinimumState> result;
   //   result.reserve(1);
   result.reserve(8);

   result.push_back( seed.State() );

   int printLevel = PrintLevel();
   if (printLevel >1) {
      std::cout << "Fumili: start iterating until Edm is < " << edmval << std::endl;
      MnPrint::PrintState(std::cout, seed.State(), "Fumili: Initial state  ");
   }
   if (TraceIter() ) TraceIteration(result.size()-1, result.back() );


   // do actual iterations


   // try first with a maxfxn = 50% of maxfcn
   // Fumili in principle needs much less iterations
   int maxfcn_eff = int(0.5*maxfcn);
   int ipass = 0;
   double edmprev = 1;

   do {


      min = Minimum(fcn, gc, seed, result, maxfcn_eff, edmval);


      // second time check for validity of function Minimum
      if (ipass > 0) {
         if(!min.IsValid()) {
#ifdef WARNINGMSG
            MN_INFO_MSG("FumiliBuilder: FunctionMinimum is invalid.");
#endif
            return min;
         }
      }

      // resulting edm of minimization
      edm = result.back().Edm();

#ifdef DEBUG
      std::cout << "approximate edm is  " << edm << std::endl;
      std::cout << "npass is " << ipass << std::endl;
#endif

      // call always Hesse (error matrix from Fumili is never accurate since is approximate)

#ifdef DEBUG
      std::cout << "FumiliBuilder will verify convergence and Error matrix. " << std::endl;
      std::cout << "dcov is =  "<<  min.Error().Dcovar() << std::endl;
#endif

//       // recalculate the gradient using the numerical gradient calculator
//       Numerical2PGradientCalculator ngc(fcn, min.Seed().Trafo(), strategy);
//       FunctionGradient ng = ngc( min.State().Parameters() );
//       MinimumState tmp( min.State().Parameters(), min.State().Error(), ng, min.State().Edm(), min.State().NFcn() );

      MinimumState st = MnHesse(strategy)(fcn, min.State(), min.Seed().Trafo(),maxfcn);
      result.push_back( st );

      if (printLevel > 1) {
         MnPrint::PrintState(std::cout, st, "Fumili: After Hessian  ");
         if (TraceIter() ) TraceIteration(result.size()-1, result.back() );
      }


      // check edm
      edm = st.Edm();
#ifdef DEBUG
      std::cout << "edm after Hesse calculation " << edm << std::endl;
      std::cout << "state after Hessian calculation  " << st << std::endl;
#endif

      // break the loop if edm is NOT getting smaller
      if (ipass > 0 && edm >= edmprev) {
#ifdef WARNINGMSG
         MN_INFO_MSG("FumiliBuilder: Exit iterations, no improvements after Hesse ");
         MN_INFO_VAL2("current edm is ", edm);
         MN_INFO_VAL2("previous value ",edmprev);
#endif
         break;
      }
      if (edm > edmval) {
#ifdef DEBUG
         std::cout << "FumiliBuilder: Tolerance is not sufficient - edm is " << edm << " requested " << edmval
                   << " continue the minimization" << std::endl;
#endif
      }
      else {
         // Case when edm < edmval after Heasse but min is flagged eith a  bad edm:
         // make then a new Function minimum since now edm is ok
         if (min.IsAboveMaxEdm() ) {
            min = FunctionMinimum( min.Seed(), min.States(), min.Up() );
            break;
         }

      }

      // end loop on iterations
      // ? need a maximum here (or max of function calls is enough ? )
      // continnue iteration (re-calculate function Minimum if edm IS NOT sufficient)
      // no need to check that hesse calculation is done (if isnot done edm is OK anyway)
      // count the pass to exit second time when function Minimum is invalid
      // increase by 20% maxfcn for doing some more tests
      if (ipass == 0) maxfcn_eff = maxfcn;
      ipass++;
      edmprev = edm;

   }  while (edm > edmval );



   // Add latest state (Hessian calculation)
   min.Add( result.back() );

   return min;

}

FunctionMinimum FumiliBuilder::Minimum(const MnFcn& fcn, const GradientCalculator& gc, const MinimumSeed& seed, std::vector<MinimumState>& result, unsigned int maxfcn, double edmval) const {

   // function performing the minimum searches using the FUMILI algorithm
   // after the modification when I iterate on this functions, so it can be called many times,
   //  the seed is used here only to get precision and construct the returned FunctionMinimum object


   /*
    Three options were possible:

    1) create two parallel and completely separate hierarchies, in which case
    the FumiliMinimizer would NOT inherit from ModularFunctionMinimizer,
    FumiliBuilder would not inherit from MinimumBuilder etc

    2) Use the inheritance (base classes of ModularFunctionMinimizer,
                            MinimumBuilder etc), but recreate the member functions Minimize() and
    Minimum() respectively (naming them for example minimize2() and
                            minimum2()) so that they can take FumiliFCNBase as Parameter instead FCNBase
    (otherwise one wouldn't be able to call the Fumili-specific methods).

    3) Cast in the daughter classes derived from ModularFunctionMinimizer,
    MinimumBuilder.

    The first two would mean to duplicate all the functionality already existent,
    which is a very bad practice and Error-prone. The third one is the most
    elegant and effective solution, where the only constraint is that the user
    must know that they have to pass a subclass of FumiliFCNBase to the FumiliMinimizer
    and not just a subclass of FCNBase.
    BTW, the first two solutions would have meant to recreate also a parallel
    structure for MnFcn...
    **/
   //  const FumiliFCNBase* tmpfcn =  dynamic_cast<const FumiliFCNBase*>(&(fcn.Fcn()));

   const MnMachinePrecision& prec = seed.Precision();

   const MinimumState & initialState = result.back();

   double edm = initialState.Edm();


#ifdef DEBUG
   std::cout << "\n\nDEBUG FUMILI Builder  \nInitial State: "
      << " Parameter " << initialState.Vec()
      << " Gradient " << initialState.Gradient().Vec()
      << " Inv Hessian " << initialState.Error().InvHessian()
      << " edm = " << initialState.Edm()
      << " maxfcn = " << maxfcn
      << " tolerance = " << edmval
      << std::endl;
#endif


   // iterate until edm is small enough or max # of iterations reached
   edm *= (1. + 3.*initialState.Error().Dcovar());
   MnAlgebraicVector step(initialState.Gradient().Vec().size());

   // initial lambda Value
   double lambda = 0.001;

   int printLevel = MnPrint::Level();
   do {

      //     const MinimumState& s0 = result.back();
      MinimumState s0 = result.back();

      step = -1.*s0.Error().InvHessian()*s0.Gradient().Vec();


#ifdef DEBUG
      std::cout << "\n\n---> Iteration - " << result.size()
      << "\nFval = " << s0.Fval() << " numOfCall = " << fcn.NumOfCalls()
      << "\nInternal Parameter values " << s0.Vec()
      << " Newton step " << step << std::endl;
#endif

      double gdel = inner_product(step, s0.Gradient().Grad());
      if(gdel > 0.) {
#ifdef WARNINGMSG
         MN_INFO_MSG("FumiliBuilder: matrix not pos.def, gdel > 0");
         MN_INFO_VAL(gdel);
#endif
         MnPosDef psdf;
         s0 = psdf(s0, prec);
         step = -1.*s0.Error().InvHessian()*s0.Gradient().Vec();
         gdel = inner_product(step, s0.Gradient().Grad());
#ifdef WARNINGMSG
         MN_INFO_VAL2("After correction ",gdel);
#endif
         if(gdel > 0.) {
            result.push_back(s0);
            return FunctionMinimum(seed, result, fcn.Up());
         }
      }


      //     MnParabolaPoint pp = lsearch(fcn, s0.Parameters(), step, gdel, prec);

      //     if(fabs(pp.Y() - s0.Fval()) < prec.Eps()) {
      //       std::cout<<"FumiliBuilder: no improvement"<<std::endl;
      //       break; //no improvement
      //     }


      //     MinimumParameters p(s0.Vec() + pp.X()*step, pp.Y());

      // if taking a full step

      // take a full step

      MinimumParameters p(s0.Vec() + step,  fcn( s0.Vec() + step ) );

      // check that taking the full step does not deteriorate minimum
      // in that case do a line search
      if ( p.Fval() >= s0.Fval()  ) {
         MnLineSearch lsearch;
         MnParabolaPoint pp = lsearch(fcn, s0.Parameters(), step, gdel, prec);

         if(fabs(pp.Y() - s0.Fval()) < prec.Eps()) {
            //std::cout<<"FumiliBuilder: no improvement"<<std::endl;
            break; //no improvement
         }
         p =  MinimumParameters(s0.Vec() + pp.X()*step, pp.Y() );
      }

#ifdef DEBUG
      std::cout << "Before Gradient " << fcn.NumOfCalls() << std::endl;
#endif

      FunctionGradient g = gc(p, s0.Gradient());

#ifdef DEBUG
      std::cout << "After Gradient " << fcn.NumOfCalls() << std::endl;
#endif

      //FunctionGradient g = gc(s0.Parameters(), s0.Gradient());


      // move Error updator after Gradient since the Value is cached inside

      MinimumError e = ErrorUpdator().Update(s0, p, gc, lambda);


      edm = Estimator().Estimate(g, s0.Error());


#ifdef DEBUG
      std::cout << "Updated new point: \n "
         << " FVAL      " << p.Fval()
         << " Parameter " << p.Vec()
         << " Gradient " << g.Vec()
         << " InvHessian " << e.Matrix()
         << " Hessian " << e.Hessian()
         << " edm = " << edm << std::endl << std::endl;
#endif

      if(edm < 0.) {
#ifdef WARNINGMSG
         MN_INFO_MSG("FumiliBuilder: matrix not pos.def., edm < 0");
#endif
         MnPosDef psdf;
         s0 = psdf(s0, prec);
         edm = Estimator().Estimate(g, s0.Error());
         if(edm < 0.) {
            result.push_back(s0);
            if (TraceIter() ) TraceIteration(result.size()-1, result.back() );
            return FunctionMinimum(seed, result, fcn.Up());
         }
      }

      // check lambda according to step
      if ( p.Fval() < s0.Fval()  )
         // fcn is decreasing along the step
         lambda *= 0.1;
      else {
         lambda *= 10;
         // if close to minimum stop to avoid oscillations around minimum value
         if ( edm < 0.1 ) break;
      }

#ifdef DEBUG
      std::cout <<  " finish iteration- " << result.size() << " lambda =  "  << lambda << " f1 = " << p.Fval() << " f0 = " << s0.Fval() << " num of calls = " << fcn.NumOfCalls() << " edm " << edm << std::endl;
#endif

      result.push_back(MinimumState(p, e, g, edm, fcn.NumOfCalls()));
      if (TraceIter() ) TraceIteration(result.size()-1, result.back() );

      if (printLevel > 1) {
         MnPrint::PrintState(std::cout, result.back(), "Fumili: Iteration # ",result.size());
      }


      edm *= (1. + 3.*e.Dcovar());


   } while(edm > edmval && fcn.NumOfCalls() < maxfcn);



   if(fcn.NumOfCalls() >= maxfcn) {
#ifdef WARNINGMSG
      MN_INFO_MSG("FumiliBuilder: call limit exceeded.");
#endif
      return FunctionMinimum(seed, result, fcn.Up(), FunctionMinimum::MnReachedCallLimit());
   }

   if(edm > edmval) {
      if(edm < fabs(prec.Eps2()*result.back().Fval())) {
#ifdef WARNINGMSG
         MN_INFO_MSG("FumiliBuilder: machine accuracy limits further improvement.");
#endif
         return FunctionMinimum(seed, result, fcn.Up());
      } else if(edm < 10.*edmval) {
         return FunctionMinimum(seed, result, fcn.Up());
      } else {
#ifdef WARNINGMSG
         MN_INFO_MSG("FumiliBuilder: finishes without convergence.");
         MN_INFO_VAL2("FumiliBuilder: ",edm);
         MN_INFO_VAL2("    requested: ",edmval);
#endif
         return FunctionMinimum(seed, result, fcn.Up(), FunctionMinimum::MnAboveMaxEdm());
      }
   }
   //   std::cout<<"result.back().Error().Dcovar()= "<<result.back().Error().Dcovar()<<std::endl;

#ifdef DEBUG
   std::cout << "Exiting successfully FumiliBuilder \n"
      << "NFCalls = " << fcn.NumOfCalls()
      << "\nFval = " <<  result.back().Fval()
      << "\nedm = " << edm << " requested = " << edmval << std::endl;
#endif

   return FunctionMinimum(seed, result, fcn.Up());
}

   }  // namespace Minuit2

}  // namespace ROOT
