#include "TH1F.h"
#include "TRandom.h"
#include "TRandom3.h"
#include "TH1F.h"
#include "TF1.h"
#include "TGraph.h"
#include "TCanvas.h"
#include "TFile.h"
#include "TTree.h"
#include "TStyle.h"
#include "TSystem.h"
#include "TVectorT.h"
#include "TGenPhaseSpace.h"
#include "TStopwatch.h"
#include "TMath.h"
#include <TMinuit.h>
#include <vector>
#include <iostream>
#include <iomanip>
#include <sstream>
#include <fstream>
#include <string>
#include <cstdlib>
#include <ctime>
using namespace std;
using namespace TMath;

#include "/ustcfs/BES3User/2021/ypei/BESIII/Jpsi2XiPair/ConstraintFitTest/SingleFit/v3.0/Headfile/amplitudesquare.hh"
#include "/ustcfs/BES3User/2021/ypei/BESIII/Jpsi2XiPair/ConstraintFitTest/SingleFit/v3.0/Headfile/Constant.h"
#include "/ustcfs/BES3User/2021/ypei/software/myMinuit/blindMinuit.h"
R__LOAD_LIBRARY(/ustcfs/BES3User/2021/ypei/software/myMinuit/libblindMinuit.so);

//Input file key word
TString datafile = "./data.dat";
TString outname = "result.txt";
int FitTimes = 1;

vector<double> data_XiThe;
vector<double> data_LamThe; 
vector<double> data_LamPhi; 
vector<double> data_LambarThe; 
vector<double> data_LambarPhi; 
vector<double> data_pThe; 
vector<double> data_pPhi; 
vector<double> data_pbarThe; 
vector<double> data_pbarPhi; 

//Decay Parameter
const int Npar = 8+1;
int N_data = 0;


void fcnMLLG(Int_t &npar, Double_t *gin, Double_t &f, Double_t *pp, Int_t iflag);
int ReadDat(TString File ,vector<double>& XiThe, vector<double>& LamThe, vector<double>& LamPhi, vector<double>& LambarThe, vector<double>& LambarPhi, 
    vector<double>& pThe, vector<double>& pPhi, vector<double>& pbarThe, vector<double>& pbarPhi, bool IsPrint);

int main()
{
	cout << "Time Counting Start!" << endl;
  clock_t StartTime = clock();
	data_XiThe.clear();
  data_LamThe.clear();
  data_LamPhi.clear();
  data_LambarThe.clear();
  data_LambarPhi.clear();
  data_pThe.clear();
  data_pPhi.clear();
  data_pbarThe.clear();
  data_pbarPhi.clear();

	cout << "##Loading data ..........." << endl;
  N_data = ReadDat(datafile, data_XiThe, data_LamThe, data_LamPhi, data_LambarThe, data_LambarPhi, data_pThe, data_pPhi, data_pbarThe, data_pbarPhi, false);
  cout << "Data Events = " << N_data << endl;

  
  //////////////////////////////////////////////////////Start Fit//////////////////////////////////////////////////
  //TMinuit *minuit=new TMinuit(Npar);
  blindMinuit *minuit = new blindMinuit(Npar);
  minuit->setRandomSeed(42);
  minuit->blindParameter(false);
  Int_t ierflag=0; 
  Double_t arglist[Npar];
  minuit->SetFCN(fcnMLLG);
 
  double Vstart[Npar] = 
  {
    In_alpha_jpsi,
    In_DeltaPhi,
    In_alpha_Xim,
    In_alpha_Xip,
    In_LYphi_Xim,
    In_LYphi_Xip,
    In_alpha_Lam,
    In_alpha_Lambar,
    0.0
  };
  static double Vstep[Npar] = {0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.1};

double RangeLam = 100;

  cout << "Fit Times = " << FitTimes << endl;
  do {
    arglist[0]= 0;
    minuit->mnexcm("SET PRINT", arglist, 1, ierflag);
    arglist[0]= 0.5;
    minuit->mnexcm("SET ERR", arglist, 1, ierflag);
    
    minuit->mnparm(0, "alpha_jpsi"     ,Vstart[0], Vstep[0], -1.0, 1.0, ierflag);
    minuit->mnparm(1, "dphi_jpsi"      ,Vstart[1], Vstep[1], -TMath::Pi(), TMath::Pi(),ierflag);
    minuit->mnparm(2, "alpha_Xim"   ,   Vstart[2], Vstep[2], -1.0, 0.0, ierflag);
    minuit->mnparm(3, "alpha_Xip",      Vstart[3], Vstep[3],  0.0, 1.0, ierflag);
    minuit->mnparm(4, "LYphi_Xim"   ,   Vstart[4], Vstep[4], -1.0, 1.0, ierflag);
    minuit->mnparm(5, "LYphi_Xip",      Vstart[5], Vstep[5], -1.0, 1.0, ierflag);
    minuit->mnparm(6, "alpha_Lam"   ,   Vstart[6], Vstep[6],  0.0, 1.0, ierflag);
    minuit->mnparm(7, "alpha_Lambar",   Vstart[7], Vstep[7], -1.0, 0.0, ierflag);
    minuit->mnparm(8, "lambda",         0, 0.01, -1000, 1000, ierflag);

    minuit->mnexcm("MINI",arglist,0,ierflag);
    minuit->mnexcm("MINOS",arglist,0,ierflag);
    minuit->mnmatu(1);
   
    double outErr[Npar];
    for (int i = 0; i < Npar; i++)
    {
      minuit->GetParameter(i, Vstart[i], outErr[i]);
    }
    
    FitTimes--;
    cout << "Fit Times = " << FitTimes << endl;
  } while (FitTimes > 0);

  //Print Result
  double outPar[Npar], outErr[Npar];
  for (int i = 0; i < Npar; i++)
  {
    minuit->GetParameter(i, outPar[i], outErr[i]);
  }

  double covRaw[Npar * Npar];
  minuit->mnemat(covRaw, Npar);

  ofstream OutFile(outname);
  if(!OutFile)
  { 
    cout << "Wrong open OutFile of " << outname << endl;
    cerr << "Wrong open OutFile of " << outname << endl;
    return 0;
  }

  //Saved
  for (int i = 0; i < Npar; i++)
  {
    OutFile << outPar[i] << " " << outErr[i] << " ";
  }
  OutFile << endl;

  for ( int i = 0; i < Npar; i++ )
  {
    for ( int j = 0; j < Npar; j++ )
    {
      OutFile << covRaw[i * Npar + j] << " ";
    }
    OutFile << endl;
  }
  OutFile << endl;

  OutFile.close();
  cout << "... !!!Finished!!! ..." << endl;
	clock_t EndTime = clock();
	cout << "Run time = " << (double)(EndTime - StartTime) / CLOCKS_PER_SEC << endl;  
  
  return 1;
}


void fcnMLLG(Int_t &npar, Double_t *gin, Double_t &f, Double_t *pp, Int_t iflag)
{
  double alpha_jpsi = pp[0];
  double DeltaPhi = pp[1];
  double alpha_Xim = pp[2];
  double alpha_Xip = pp[3];
  double LYphi_Xim = pp[4];
  double LYphi_Xip = pp[5];
  double alpha_Lam = pp[6];
  double alpha_Lambar = pp[7];
  double lambda = pp[8];
  double LogLike = 0.0;
  
  AngDissemi *angdis = new AngDissemi(); 

  //data Log Likelihood
  for (int i = 0; i < N_data; i++)
  {
    double eval = angdis->Amp( data_XiThe[i], data_LamThe[i], data_LamPhi[i], data_LambarThe[i], data_LambarPhi[i], data_pThe[i], data_pPhi[i], data_pbarThe[i], data_pbarPhi[i],
        alpha_jpsi, DeltaPhi, alpha_Xim, LYphi_Xim, alpha_Xip, LYphi_Xip, alpha_Lam, alpha_Lambar);
    eval = eval/2.0/(1+alpha_jpsi/3);
    if (eval <= 0)
    {
      f = 0;
      return ;
    }
    LogLike-=TMath::Log(eval);
  }

  double ConsFunc = (1-alpha_Xim*alpha_Xim)*cos(LYphi_Xim)*cos(LYphi_Xim) - (1-alpha_Xip*alpha_Xip)*cos(LYphi_Xip)*cos(LYphi_Xip);
  LogLike -= lambda*pow( ConsFunc, 2 );

  f = LogLike;

  for (int i = 0; i < Npar; i++)
  {
    cout << pp[i] << " ";
  }
  cout << " ConsFunc = " << ConsFunc << endl;
  cout << "Loglike = " << LogLike << endl << endl;

  delete angdis;
}


int ReadDat(TString File ,vector<double>& XiThe, vector<double>& LamThe, vector<double>& LamPhi, vector<double>& LambarThe, vector<double>& LambarPhi, 
    vector<double>& pThe, vector<double>& pPhi, vector<double>& pbarThe, vector<double>& pbarPhi, bool IsPrint)
{
  fstream ffile(File,std::ios::in);

  if ( !ffile.is_open() )
  {
    cout << "Wrong! file " << File << " is not exist!!" << endl;
    cerr << "Wrong! file " << File << " is not exist!!" << endl;
    exit(0);
  }

  int index;
  double xithe, lamthe, lamphi, lambarthe, lambarphi, pthe, pphi, pbarthe, pbarphi;

  int Nn = 0;
	XiThe.clear(); LamThe.clear(); LamPhi.clear(); LambarThe.clear(); LambarPhi.clear();
	pThe.clear(); pPhi.clear(); pbarThe.clear(); pbarPhi.clear();
  while(!ffile.eof())
  {
    ffile >> index >> xithe >> lamthe >> lamphi >> lambarthe >> lambarphi >> pthe >> pphi >> pbarthe >> pbarphi; 
    if (IsPrint) cout << "i = " << index << ", xithe = " << xithe << ", lamthe = " << lamthe << ", lamphi = " <<  lamphi << ", lambarthe = " << lambarthe << 
      ", lambarphi = " << lambarphi << ", pthe = " << pthe << ", pphi = " << pphi << ", pbarthe = " << pbarthe << ", pbarphi = " << pbarphi << endl;
    Nn++;
    XiThe.emplace_back(xithe);
    LamThe.emplace_back(lamthe);
    LamPhi.emplace_back(lamphi);
    LambarThe.emplace_back(lambarthe);
    LambarPhi.emplace_back(lambarphi);
    pThe.emplace_back(pthe);
    pPhi.emplace_back(pphi);
    pbarThe.emplace_back(pbarthe);
    pbarPhi.emplace_back(pbarphi);
  }
	ffile.close();
  return Nn - 1;
}