#include "Math/Minimizer.h"
#include "Math/Factory.h"
#include "Math/Functor.h"
#include "TRandom3.h"
#include "TCanvas.h"
#include "TFrame.h"
#include "TGraphErrors.h"
#include "TF1.h"

#include <iostream>
#include <functional>   // std::mem_fn
#include <cmath>

// based from https://root.cern/root/htmldoc/guides/minuit2/Minuit2.html#a-complete-example
// this class represents a line in 2D
class LineFunction 
{
public:
    LineFunction(double m, double q): theSlope(m), theIntercept(q) {}
    ~LineFunction() {}

    double m() const {return theSlope;}
    double q() const {return theIntercept;}

    double operator()(double x) const 
    {
        return x*m() + q();
    }

private:
    double theSlope;
    double theIntercept;
};

// this class contains the function to minimize
class LineFcn
{
public:
    LineFcn(const std::vector<double>& y, const std::vector<double>& x,
            const std::vector<double>& err): 
            theMeasurements(y), thePositions(x),
            theMVariances(err)
            {}

    ~LineFcn() {}

    double operator()(const double* par) const
    {
        LineFunction aLine(par[0],par[1]);
        double chi2 = 0.;
        for(unsigned int n = 0; n < theMeasurements.size(); n++) 
        {
            chi2 += ((aLine(thePositions[n]) - theMeasurements[n]) *
             (aLine(thePositions[n]) - theMeasurements[n]) /
             theMVariances[n]);
        }
        return chi2;
    }

    double Eval(const double* par)
    {
        return this->operator()(par);
    }

private:
    std::vector<double> theMeasurements;
    std::vector<double> thePositions;
    std::vector<double> theMVariances;
};

// this class generate a given number of random points
// along a straight line of given parameters
class LineGenerator
{
public:
    LineGenerator(unsigned int N, double m, const double q)
    {
        TRandom3 gen(1234); // fix the seed
        LineFunction ff = LineFunction(m,q); // make a line from given parameters
        for( unsigned int i=0; i<N; ++i ) // loop over the given number of points
        {
            double x = gen.Uniform(fXmin,fXmax); // get the abscissa values
            double y = ff(x) + gen.Uniform(-1.,1.); // calculate the line value
                                                    // and add a random offset
           // std::cout << x << "," << y << std::endl;
           // fill the verctors
            fX.push_back( x ); 
            fY.push_back( y );
            fErr.push_back(1./3.);
        }
    }

    std::vector<double> measurements() const {return fY;}
    std::vector<double> positions() const {return fX;}
    std::vector<double> variances() const {return fErr;}

private:
    std::vector<double> fX;
    std::vector<double> fY;
    std::vector<double> fErr;

    double fXmin = -1.;
    double fXmax = 1.;
};

// this is used only to plot a TF1
double LinePlot(double* x, double* p)
{
    LineFunction lf(p[0],p[1]);
    return lf(x[0]);
}


// based on https://root-forum.cern.ch/t/how-to-use-minuit2minimizer/36300/3
int LineNumericalMinimization(  const char *minName = "Minuit2",
                                const char *algoName = "" )
{
    // create minimizer giving a name and a name (optionally) for the specific
    // algorithm
    // possible choices are:
    //   minName                     algoName
    //  Minuit /Minuit2             Migrad, Simplex,Combined,Scan  (default is Migrad)
    //  Minuit2                     Fumili2
    //  Fumili
    //  GSLMultiMin                 ConjugateFR, ConjugatePR, BFGS, BFGS2, SteepestDescent
    //  GSLMultiFit
    //  GSLSimAn
    //  Genetic
    ROOT::Math::Minimizer* minimum = ROOT::Math::Factory::CreateMinimizer(minName, algoName);

    // set tolerance , etc...
    minimum->SetMaxFunctionCalls(1000000); // for Minuit/Minuit2
    minimum->SetMaxIterations(10000);  // for GSL
    minimum->SetTolerance(0.001);
    minimum->SetPrintLevel(1);

    // these are the parameters that we want to find
    double true_m = 3.;
    double true_q = 1.;
    // small number of measurements
    unsigned int number_of_points = 100;
    // create a random vector of measuremets
    LineGenerator true_line(number_of_points,true_m,true_q);

    // create the object to minimize
    // initialize it with the fake measurements
    LineFcn line(true_line.measurements(),
                true_line.positions(),
                true_line.variances());

    // create function wrapper for minimizer
    // a IMultiGenFunction type

    // this works for ROOT 6.30: 
    // from https://root.cern.ch/doc/master/Functor_8h_source.html
    // /// Construct from a pointer to member function (multi-dim type).
    // template <class PtrObj, typename MemFn>
    // Functor(const PtrObj& p, MemFn memFn, unsigned int dim )
    //    : fDim{dim}, fFunc{std::bind(memFn, p, std::placeholders::_1)}
    // {}
    // where
    // https://cplusplus.com/reference/functional/mem_fn/
    // template <class Ret, class T>  /* unspecified */ mem_fn (Ret T::* pm);
    // Returns a function object whose functional call invokes the member function pointed by pm.
    ROOT::Math::Functor f(&line, std::mem_fn(&LineFcn::Eval), 2);
    // for ROOT 6.28 use instead:
    // /**
    //    construct from a pointer to member function (multi-dim type)
    // */
    //    template <class PtrObj, typename MemFn>
    //    Functor(const PtrObj& p, MemFn memFn, unsigned int dim )
    //       : fImpl(new MemFunHandler<Functor, PtrObj, MemFn>(dim, p, memFn))
    //    {}
    // ROOT::Math::Functor f(&line, &LineFcn::Eval, 2);


    // assign the functor to the minimizer
    minimum->SetFunction(f);

    // standard setup
    double step[2] = {0.01,0.01};
    double init_par[2] = {0.,0.};

    // Set the free variables to be minimized !
    minimum->SetVariable(0,"m",init_par[0], step[0]);
    minimum->SetVariable(1,"q",init_par[1], step[1]);

    // do the minimization
    minimum->Minimize();

    // check the results
    const double *xs = minimum->X();
    std::cout << "Minimum: f(" << xs[0] << "," << xs[1] << "): "
                << minimum->MinValue() << " = " << line(xs) << std::endl;

    // plot the results
    TCanvas* c1 = new TCanvas("c1","Line Numerical Minimization",200,10,700,500);
    c1->SetGrid();
    TGraphErrors* gr = new TGraphErrors(number_of_points,
                                        true_line.positions().data(),
                                        true_line.measurements().data(),0,
                                        true_line.variances().data());
    gr->SetTitle("Data");
    gr->SetMarkerColor(4);
    gr->SetMarkerStyle(21);
    gr->Draw("AP");
    TF1* f1 = new TF1("Fit",LinePlot,-2.,2.,2);
    f1->SetParameters(xs[0],xs[1]);
    f1->SetParNames("slope","intercept");
    f1->Draw("SAME");
    gPad->BuildLegend();

    return 0;
}