// test fit of fumili using a gauss function 
// with and without providing derivatives

#include "Math/IParamFunction.h"
#include "Fit/Fitter.h"
#include "Fit/BinData.h"
#include "TF2.h"
#include "TH2D.h"
#include "TVirtualFitter.h"
#include "HFitInterface.h"
#include "Math/MinimizerOptions.h"
#include "TMinuit.h"
#include "Math/Derivator.h"
#include "TGraph.h"
#include "TCanvas.h"
	
class GaussModelFunction : public ROOT::Math::IParamMultiGradFunction { 

public: 

	GaussModelFunction(double amp = 1, double meanx = 0, double sigmax = 1, double meany = 0, double sigmay = 1)  { 
		fPar[0] = amp;
		fPar[1] = meanx;
		fPar[2] = sigmax; 
		fPar[3] = meany;
		fPar[4] = sigmay; 
	}
	GaussModelFunction(const double *p) { 
		fPar[0] = p[0];
		fPar[1] = p[1];
		fPar[2] = p[2]; 
		fPar[3] = p[3];
		fPar[4] = p[4]; 
	}

	
	unsigned int NPar() const { return 5; }
	unsigned int NDim() const { return 2; }

	void SetParameters(const double * p) {
		std::copy(p,p+5,fPar);
	}

	void SetParameters(double amp, double meanx, double sigmax, double meany, double sigmay)  { 
		fPar[0] = amp;
		fPar[1] = meanx;
		fPar[2] = sigmax; 
		fPar[3] = meany;
		fPar[4] = sigmay; 
	}
	
	const double * Parameters() const { return fPar; }

		
	std::string ParameterName(unsigned int i) const { 
		if (i == 0) return "Amp";
		if (i == 1) return "MeanX";
		if (i == 2) return "SigmaX";
		if (i == 3) return "MeanY";
		if (i == 4) return "SigmaY";
		return "Invalid";
	}
	
	ROOT::Math::IBaseFunctionMultiDim * Clone() const { 
		return new GaussModelFunction(fPar[0],fPar[1],fPar[2],fPar[3],fPar[4]);
	} 

	void ParameterGradient(const double * x , const double * p, double * grad ) const { 
//cerr << "ParameterGradient, params: " << p[0] << " " << p[1] << " " << p[2] << endl;
		if (p == 0) { 
			ParameterGradient(x,fPar,grad); 
			return; 
		}
		double amp =	p[0]; 
		double meanx =  p[1];
		double sigmax = p[2];
		double meany =  p[3];
		double sigmay = p[4];
		double z = (x[0]-meanx)/sigmax;
		double z1 = (x[1]-meany)/sigmay;
//		double y1 = (x[0]-(mean+0.02))/sigma;
//		double y2 = (x[0]-(mean-0.02))/sigma;
		grad[0] =  exp(-0.5*z*z)*exp(-0.5*z1*z1);
		grad[1] = amp*grad[0]*z/sigmax;
//		grad[1] = (amp * std::exp( -0.5 * y1 * y1)-amp * std::exp( -0.5 * y2 * y2))/0.04;
		grad[2] = grad[1] * z; 
		grad[3] = amp*grad[0]*z1/sigmay;
		grad[4] = grad[3] * z1; 
	}

	double fPar[5];

private:

	double DoEvalPar(const double * x, const double *p) const { 
		if (p == 0) { 
			return DoEvalPar(x,fPar); 
		}
		double amp =	p[0]; 
		double meanx =  p[1];
		double sigmax = p[2];
		double meany =  p[3];
		double sigmay = p[4];
		double z =  (x[0]- meanx)/ sigmax;
		double z1 =  (x[1]- meany)/ sigmay;  
		return amp * std::exp( -0.5 * z * z)*std::exp( -0.5 * z1 * z1);
	}

	double DoParameterDerivative(const double *x, const double * p, unsigned int ipar) const { 
cerr << "DoParameterDerivative" << endl;
		double g[5];
		ParameterGradient(x,p,g);
		return g[ipar];
	}

};
	
	

void exampleGradFit2D(std::string fitterName="Minuit",std::string algoname="Simplex")  { 

	TH2D * h1 = new TH2D("h2","h2",200,-5,5, 200, -5, 5); 

	TF2 *f2 = new TF2("gaus2", "exp(-0.5*x*x)*exp(-0.5*y*y)", -5, 5, -5, 5);

	h1->FillRandom("gaus2"); 

	h1->Draw("colz");

	new TCanvas();

	GaussModelFunction gaus;

	ROOT::Math::MinimizerOptions::SetDefaultMinimizer(fitterName.c_str(),algoname.c_str()); 
	ROOT::Math::MinimizerOptions::SetDefaultTolerance(0.01); 

	ROOT::Fit::BinData d; 
	ROOT::Fit::FillData(d,h1,0); 
	ROOT::Fit::Fitter fitter; 

	gaus.SetParameters(100,0,1, 0, 1);



	std::cout <<"\n\n**********************************************************\n";
	std::cout <<"Chi2 Fit using analytical gradient of Gaussian function \n\n";

	// set first the funciton (needed if parameter limits must be set)
	fitter.SetFunction(gaus);  
	// set limit on sigma 
	fitter.Config().ParSettings(2).SetLimits(0,100);
	fitter.Config().ParSettings(0).SetLimits(0, 200);
	fitter.Config().ParSettings(1).SetLimits(-1, 1);
	//fitter.Config().MinimizerOptions().SetPrintLevel(3);
	fitter.Fit(d);
	fitter.Result().Print(std::cout);

	TF2 *f;

	// to  draw need to create a TF1 from pointer to fit function
	// ponter needs to be alive as TF1 so needs to clone
	if (fitter.Result().FittedFunction() ) {
		ROOT::Math::IParamMultiFunction * ff = dynamic_cast<ROOT::Math::IParamMultiFunction *>
			(fitter.Result().FittedFunction()->Clone() );
		f = new TF2("fitFunc",ff,-5,5,-5,5,0);
		h1->Draw();
		f->Draw("same cont");
	}
/*
	TGraph *g = new TGraph();
	int i=0;

//	double *p[] = {gaus.fPar[0], gaus.fPar[1], gaus.fPar[2]};
	double p[] = {98.15, 0.0096, 0.995};

	cerr << p[0] << endl;

	for(double x=-5; x<=5; x+=0.1)
	{
		Double_t xm[]={x, 0};
//		double ev=0;
		double ev = ROOT::Math::Derivator::Eval((ROOT::Math::IParamMultiFunction&)gaus, xm, p, 2);
		g->SetPoint(i, x, ev);
		cout << x << " " << ev << endl;
		i++;
	}
//	cerr << "deriv " << ROOT::Math::Derivator::Eval((ROOT::Math::IParamFunction&)gaus, xm, 0);

	new TCanvas();
	g->Draw("A*");
*/

}