#include "Math/Minimizer.h"
#include "Math/Factory.h"
#include "Math/Functor.h"
#include "TRandom2.h"
#include "TError.h"
#include <iostream>
 

double jteb = 136.945;
double cost = 0.156321;
double vtop = 35696.2;
double jte1 = 266.617;
double jte2 = 67.4984;
double ptx= 49.822;
double pty=6.5479;
double ptz=-363.926;

double MW=80.379;
double Mtop=173.3, gammatop = 1.35;
double sig1=15., sig2=15., gammaW=2.085;

double myFCN(const double * par) {
    double chi2 = 0;  
    double k1 = par[0];
    double k2 = par[1];
    double M12 = sqrt(2.*k1*k2*(1.-cost));
    double kb = 2*(k1+k2)*jteb;
    double mtjj = sqrt(M12+kb-vtop); //top mass from Wjets + bjet
    double etoph = k1+k2+jteb ; // Energy of hadronic Top Reco
    double mtoph = sqrt(etoph*etoph - ptx*ptx - pty*pty - ptz*ptz) ;

    chi2 = (jte1-k1)*(jte1-k1)/sig1/sig1 + (jte2-k2)*(jte2-k2)/sig2/sig2   
            + (M12-MW)*(M12-MW)/gammaW/gammaW + (mtoph-Mtop)*(mtoph-Mtop)/gammatop/gammatop ;
    return chi2; 
}

int minimisho(){
//    ROOT::Math::Functor f(&RosenBrock,2);
    ROOT::Math::Minimizer* minimum = ROOT::Math::Factory::CreateMinimizer("Minuit2", "Migrad");
    minimum->SetTolerance(0.01);
    minimum->SetPrintLevel(3);
    ROOT::Math::Functor f(&myFCN,2);
    minimum->SetFunction(f);

    double step[2] = {0.01,0.01};
    double variable[2] = { jte1,jte2 }; // starting point

    minimum->SetLowerLimitedVariable(0,"k1",variable[0], step[0],0.); //Lower value is energy=0
    minimum->SetLowerLimitedVariable(1,"k2",variable[1], step[1],0.);
    minimum->Minimize(); // do the minimization

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

   return 0;
}