/*
 Fit d'une gaussienne par une methode de chi2
*/
#include <machdefs.h>
#include <stdlib.h>
#include <stdio.h>
#include <iostream>
#include <math.h>
#include <string.h>

#include "sopnamsp.h"
#include "srandgen.h"
#include "minuitadapt.h"

// Ne pas changer NPAR
#define NPAR 7

const int DIMX = 25;
const int DIMY = 25;
const double HAUT=10000.;
const double X0=0., Y0=0.;
const double SX=5., SY=5., RHO=0.05;
const double FOND=100.;
//                         vol    x0    y0    sx    sy   rho  fond
const bool FIX[NPAR]={false,false,false,false,false,false,false};
const double ERR=5.;
const double ERRMIN=ERR*sqrt(FOND);
const double nSX=7., nSY=5.;
const bool USERGRAD=false;
const bool DOIMPROVE=true;
const bool DOMINOS=true;
const bool DOCONT=true;
const bool DOSCAN=true;
#define VARAND (drandpm1())
//#define VARAND 1.
#define MAXCALL 99999

void fcn(int_4 *,double *,double *,double *,int_4 *,double futils(double *));
double futils(double *x) {return 0.;}
double Gauss2D(double x,double y,double *param);
double dGauss2D(double x,double y,double *param,double *dparam);

double X[DIMX], Y[DIMY], Z[DIMX][DIMY], EZ[DIMX][DIMY];
int_4 IFLAG[10]={0,0,0,0,0,0,0,0,0,0};

/*==========================================================================*/
int main(int nargv, char *argv[])
{
 /* initialisation */
 if(DIMX*DIMY<=NPAR) exit(-1);

 int nran=0;
 if(nargv>1) nran=atoi(argv[1]);
 for(int i=0;i<nran;i++) drand01();

 /* compute volume */
 double vol=HAUT*(2*M_PI*SX*SY)/sqrt(1.-RHO*RHO);
 cout<<"haut="<<HAUT<<" vol="<<vol<<endl;
 cout<<"Fond="<<FOND<<endl;

 /* remplissage de la densite de probabilite a fiter */
 double par[NPAR],epar[NPAR];
 printf("Donnees a fiter %d :\n",DIMX*DIMY);
   par[0]=vol; par[1]=X0; par[2]=Y0;
   par[3]=SX; par[4]=SY; par[5]=RHO;
   par[6]=FOND;
 for(int i=0;i<DIMX;i++) {
   X[i] = X0 +nSX*SX*(2.*(i+0.5)/DIMX-1.);
   for(int j=0;j<DIMY;j++) {
     Y[j] = Y0 +nSY*SY*(2.*(j+0.5)/DIMY-1.);
     double f = Gauss2D(X[i],Y[j],par);
     Z[i][j] = f;
     double ef = (fabs(f)>1.) ? ERR*sqrt(fabs(f)): ERR;
     EZ[i][j] = (ef>ERRMIN)? ef: ERRMIN;
     Z[i][j] += EZ[i][j]*NorRand();
   }
 }

/*********************** minuit minimisation ***********************/
 printf("\n\n");
 MinuitAdapt MMM(fcn,futils);

 /* initialise */
 MMM.SetTitle("Minuit fit Gaussienne 2D+Fond");
 MMM.Clear();
 MMM.SetRandom(1000000);

 /* set print and warning level, precision etc... (-1,0,1,2,3) */
 MMM.PrintLevel(1);
 MMM.SetWidthPage(120);
 MMM.SetWarnings(true);
 MMM.SetErrorDef(1.);
 MMM.SetEpsMachine(1.e-13);
 MMM.SetStrategy(1);
 if(USERGRAD) MMM.SetGradient(1); else MMM.SetGradient(-1);

 /* set parameters */
 MMM.DefineParameter(1,"Vol",vol,fabs(vol)/50.);
 MMM.DefineParameter(2,"X0",X0,SX/5.);
 MMM.DefineParameter(3,"Y0",Y0,SY/5.);
 MMM.DefineParameter(4,"Sx",SX,SX/5.,0.01*SX,10.*SX);
 MMM.DefineParameter(5,"Sy",SY,SY/5.,0.01*SY,10.*SY);
 MMM.DefineParameter(6,"Rho",RHO,0.0001,-1.,1.);
 double x=(FOND!=0.)? fabs(FOND)/10.: 0.01;
 MMM.DefineParameter(7,"Fond",FOND,x);

 /* set parameters */
   x = (FIX[0])? vol: vol+VARAND*vol/5.;
 MMM.SetParameter(1,x);
   x = (FIX[1])? X0: X0+VARAND*SX;
 MMM.SetParameter(2,x);
   x=(FIX[2])? Y0: Y0+VARAND*SY;
 MMM.SetParameter(3,x);
   x=(FIX[3])? SX: SX+VARAND*SX/2.;
 MMM.SetParameter(4,x);
   x=(FIX[4])? SY: SY+VARAND*SY/2.;
 MMM.SetParameter(5,x);
   x=(FIX[5])? RHO: 0.;
 MMM.SetParameter(6,x);
   x=(FIX[6])? FOND: FOND+VARAND*FOND/3.;
 MMM.SetParameter(7,x);

 /* fix parameters */
 for(int i=0;i<NPAR;i++) if(FIX[i]) MMM.SetFix(i+1);
 MMM.ShowParameter();

 /* minimize */
 MMM.Migrad(MAXCALL,0.01);
 //MMM.Minimize(MAXCALL,0.01);
 //MMM.Simplex(MAXCALL,0.01);
 //MMM.Seek(MAXCALL,5.);
 MMM.ShowFcnValue();

 if(DOIMPROVE) MMM.Improve(MAXCALL);
 MMM.ShowFcnValue();

 if(DOMINOS) MMM.Minos(MAXCALL);
 MMM.ShowFcnValue();

 /* get parameters and errors */
 cout<<endl;
 for(int i=0;i<NPAR;i++) {
   string dum; int_4 ivarbl;
   double b1,b2,eparab,eplus,eminus,globcc;
   MMM.GetParameter(i+1,dum,par[i],epar[i],b1,b2,ivarbl);
   MMM.GetErrors(i+1,eplus,eminus,eparab,globcc);
   printf("> parameter %d \"%s\" = %g %g (%g,%g) int var=%d\n"
         ,i+1,dum.c_str(),par[i],epar[i],b1,b2,ivarbl);
   printf("            e+=%g e-=%g eparab=%g globcc=%g\n"
         ,eplus,eminus,eparab,globcc);
 }
 fflush(stdout);
 cout<<"haut(sig)="<<par[0]*sqrt(1-RHO*RHO)/(2*M_PI*SX*SY)
     <<"  haut(sig_fit)="<<par[0]*sqrt(1-par[5]*par[5])/(2*M_PI*par[3]*par[4])<<endl;

 TMatrix<r_8> emat = MMM.GetErrorsMatrix();
 cout<<"GetErrorsMatrix:"<<emat<<endl;

 /* get covariance matrix */
 MMM.ShowCovariance();
 MMM.ShowCorrelations();
 MMM.ShowEigenValues();

 /* contour plot */
 if(DOCONT) {
   for(int i=1;i<NPAR;i++) {
     if(FIX[i]) continue;
     for(int j=i+1;j<=NPAR;j++) {
       if(FIX[j]) continue;
       //MMM.DrawContour(i,j,20);
       TVector<r_8> xcont,ycont;
       int_4 ncontok = MMM.GetContour(i,j,20,xcont,ycont);
       cout<<"Contour "<<i<<" "<<j<<" ncontok="<<ncontok<<endl;
       if(ncontok<1) continue;
       for(int k=0;k<ncontok;k++) cout<<" ("<<xcont[k]<<","<<ycont[k]<<")";
       cout<<endl;
     }
   }
 }

 /* scan parameters */
 if(DOSCAN) {
   MMM.ShowFcnValue();
   for(int i=0;i<NPAR;i++) {
     if(FIX[i]) continue;
     MMM.Scan(i+1,par[i]-2.*epar[i],par[i]+2.*epar[i],20);
   }
   MMM.ShowFcnValue();
 }

 /* end */
 MMM.ShowRandom();
 MMM.Return();

 printf("\n\n");
/*******************************************************************/

 /* fin de minimisation, print ! */
 double xi2=0.;
 for(int i=0;i<DIMX;i++) for(int j=0;j<DIMY;j++) {
   double f = Z[i][j]-Gauss2D(X[i],Y[j],par);
   xi2 += f*f/(EZ[i][j]*EZ[i][j]);
 }
 cout<<"1: "<<vol<<"\tfit="<<par[0]<<"\tefit="<<epar[0]<<"\td="<<par[0]-vol;
   if(epar[0]>0.) cout<<"\td/e="<<(par[0]-vol)/epar[0]; cout<<endl;
 cout<<"2: "<<X0<<"\tfit="<<par[1]<<"\tefit="<<epar[1]<<"\td="<<par[1]-X0;
   if(epar[1]>0.) cout<<"\td/e="<<(par[1]-X0)/epar[1]; cout<<endl;
 cout<<"3: "<<Y0<<"\tfit="<<par[2]<<"\tefit="<<epar[2]<<"\td="<<par[2]-Y0;
   if(epar[2]>0.) cout<<"\td/e="<<(par[2]-Y0)/epar[2]; cout<<endl;
 cout<<"4: "<<SX<<"\tfit="<<par[3]<<"\tefit="<<epar[3]<<"\td="<<par[3]-SX;
   if(epar[3]>0.) cout<<"\td/e="<<(par[3]-SX)/epar[3]; cout<<endl;
 cout<<"5: "<<SY<<"\tfit="<<par[4]<<"\tefit="<<epar[4]<<"\td="<<par[4]-SY;
   if(epar[4]>0.) cout<<"\td/e="<<(par[4]-SY)/epar[4]; cout<<endl;
 cout<<"6: "<<RHO<<"\tfit="<<par[5]<<"\tefit="<<epar[5]<<"\td="<<par[5]-RHO;
   if(epar[5]>0.) cout<<"\td/e="<<(par[5]-RHO)/epar[5]; cout<<endl;
 cout<<"7: "<<FOND<<"\tfit="<<par[6]<<"\tefit="<<epar[6]<<"\td="<<par[6]-FOND;
   if(epar[6]>0.) cout<<"\td/e="<<(par[6]-FOND)/epar[6]; cout<<endl;
 cout<<"Xi2="<<xi2<<"\txi2/n="<<xi2/(DIMX*DIMY-NPAR)<<endl;

 exit(0);
}

void fcn(int_4 *npar,double *grad,double *fval,double *xval
        ,int_4 *iflag,double futils(double *))
{
  //cout<<"iflag="<<*iflag<<endl;
  IFLAG[0]++;
  if(*iflag>0 && *iflag<10) IFLAG[*iflag]++;

  // Read input,init,... data values
  //  if(*iflag==1) {...}

  // Instruct Minuit to redefine the problem
  // and forget about previously best fitted values.
  //  if(*iflag==5) {...}

  // Always compute Chi2 or Likelyhood (here iflag==4)
 *fval=0.;
 for(int i=0;i<DIMX;i++) for(int j=0;j<DIMY;j++) {
   double f = Z[i][j]-Gauss2D(X[i],Y[j],xval);
   *fval += f*f/(EZ[i][j]*EZ[i][j]);
 }

 // Compute (optionnal) the first derivative of Chi2 / parameters
 if(*iflag==2) {
   // Return gradient of chi2 (if SET GRA called)
   // C'est DChi2/DPi = -2*sum{(Yi-F(Xi))/EYi^2 * dF/dPi(Xi)}
   double dpar[NPAR];
   for(int j=0;j<NPAR;j++) grad[j]=0.;
   for(int i=0;i<DIMX;i++) for(int j=0;j<DIMY;j++) {
     double f=-2.*(Z[i][j]-Gauss2D(X[i],Y[j],xval))/(EZ[i][j]*EZ[i][j]);
     dGauss2D(X[i],Y[j],xval,dpar);
     for(int k=0;k<NPAR;k++) grad[k]+= f*dpar[k];
   }
 }

 // Called at the end of the fit (on the Minuit RETURN)
 if(*iflag==3) {
   cout<<"Call fcn iflag="<<*iflag<<" npar="<<*npar<<endl;
   for(int k=0;k<NPAR;k++) cout<<" P"<<k+1<<"="<<xval[k];
   cout<<endl;
   cout<<"Number of fcn calls="<<IFLAG[0]<<endl;
   for(int k=1;k<10;k++)
     cout<<"  iflag="<<k<<" number of calls="<<IFLAG[k]<<endl;
 }
}

double Gauss2D(double x,double y,double *param)
// xc = (x-p1)/p3 ; yc = (y-p2)/p4
// f(x) = p0*(1-rho^2)/(2*Pi*p3*p4) * exp(-0.5*(xc^2+yc^2-2*p5*xc*yc))
{
 double N = sqrt(1.-param[5]*param[5])/(2*M_PI*param[3]*param[4]);
 double X = (x-param[1])/param[3];
 double Y = (y-param[2])/param[4];
 double z2 = (X*X + Y*Y - 2.*param[5]*X*Y)/2.;
 if(z2<100.) z2=exp(-z2); else z2=0.;
 return param[0]*N*z2 + param[6];
}

double dGauss2D(double x,double y,double *param,double *dparam)
{
 double unmr2 = 1.-param[5]*param[5];
 double N = sqrt(unmr2)/(2*M_PI*param[3]*param[4]);
 double X = (x-param[1])/param[3];
 double Y = (y-param[2])/param[4];

 double XmrY = X-param[5]*Y;
 double YmrX = Y-param[5]*X;
 double z2 = (X*(XmrY-param[5]*Y)+Y*Y)/2.;

 /* g(x,y) */
 double PSF = 0.;
 if(z2<100.) PSF = N * exp(-z2);
 /* dg(x,y)/d(Vol) */
 dparam[0] = PSF;
 /* dg(x,y)/d(x0) */
 dparam[1] =  param[0]* PSF* XmrY/param[3];
 /* dg(x,y)/d(y0) */
 dparam[2] =  param[0]* PSF* YmrX/param[4];
 /* dg(x,y)/d(sx)*/
 dparam[3] =  param[0]* PSF* (X*XmrY-1.)/param[3];
 /* dg(x,y)/d(sy) */
 dparam[4] =  param[0]* PSF* (Y*YmrX-1.)/param[4];
 /* dg(x,y)/d(rho) */
 dparam[5] =  param[0]* PSF* (X*Y-2.*param[5]/unmr2);
 /* dg(x,y)/d(Fond) */
 dparam[6] = 1.;

 return param[0] * PSF + param[6];
}