//
//  main3.cpp
//  
//
//  Created by Andrea Fodor on 2016-04-28.
//
//

// C++ libs
#include <iostream>
#include <math.h>
#include <stdlib.h>

// ROOT libs
#include <TH1F.h>
#include <TRandom3.h>
#include <TCanvas.h>
#include "Math/Minimizer.h"
#include "Math/Factory.h"
#include "Math/Functor.h"
#include "TRandom2.h"
#include "TError.h"
#include <iostream>
#include <TFile.h>


using namespace std;



double ChiSq(const double *xx)
{
    
    double E1fit = xx[0];
    double E2fit = xx[1];
    double E1m = xx[2];
    double E2m = xx[3];
    double sig1 = xx[4];
    double sig2 = xx[5];
    double thetam = xx[6];
    double lambda = xx[7];
    double theta = xx[8];
    double m0 = 0.134;
    double sigt = 0.014;
    double chi = pow((E1fit - E1m)/sig1, 2) + pow((E2fit - E2m)/sig2, 2) + pow((theta - thetam)/sigt,2)+2*lambda*(2*E1fit*E2fit*(1-cos(theta)) - m0*m0);
    
    return chi;
}



int main(int argc, const char * argv[]) {
    
    
    int nevents = 1000;
    
    
    TRandom3* rand = new TRandom3();
    TH1F* pen = new TH1F("pe", "E1 + E2 -E0", 50, 0, 0);
    TH1F* pmom = new TH1F("pm", "p0 - (p1 + p2)", 50, 0, 0);
    TH1F* pdif1 = new TH1F("di1", "E1 - p1", 50, 0, 0);
    TH1F* pdif2 = new TH1F("di2", "E2 - p2", 50, 0, 0);
    TH1F* mass = new TH1F("inv_mass", "invariant mass", 50, 0, 0);
    TH1F* opa = new TH1F("oa", "Opening angle", 50, 0, 0);
    TH1F* fitted = new TH1F("fited", "Fitted values", 50, 0, 0);
    TH1F* chih = new TH1F("chih", "Chi Squared", 50, 0, 0);


    
    double array[5];
    const char * minName = "Minuit2";
    const char *algoName = "" ;
    int randomSeed = -1;


    //pion variables
    double E0lab, m0, p0, beta, gamma;
    
    //photons in CM frame
    double k1x, k1y, k1z;
    double k2x, k2y, k2z;
    double E1, E2;
    
    //photons in lab frame
    double p1x, p1y, p1z;
    double p2x, p2y, p2z;
    double E1lab, E2lab, theta, thetap;
    
    //measured values
    double E1m, E2m, thetam;
    
    m0 = 0.134;
    
    for(int ievent=0; ievent<nevents; ievent++){

    
        p0 =rand-> Gaus(1, 0.01);
        E0lab = sqrt(m0*m0 + p0*p0);
        beta = sqrt(1/(1+(m0/p0)*(m0/p0)));
        gamma = 1/sqrt(1- beta*beta);
    
        E1 = m0/2;
        E2 = m0/2;
    
        rand->Sphere(k1x, k1y, k1z, E1);
        k2x = -k1x;
        k2y = -k1y;
        k2z = -k1z;
    
        
        //boost to lab frame
        E1lab = gamma*(E1 - beta*k1z);
        E2lab = gamma*(E2 - beta*k2z);
        p1x = k1x;
        p1y = k1y;
        p2x = k2x;
        p2y = k2y;
        p1z = -beta*gamma*E1 + gamma*k1z;
        p2z = -beta*gamma*E2 + gamma*k2z;
    
        pen->Fill(E0lab - E1lab - E2lab);
        pmom->Fill(p0 - sqrt(pow(p1x + p2x, 2) + pow(p1y + p2y, 2) + pow(p1z + p2z, 2)));
        
        //opening angle
        theta = acos((p1x*p2x + p1y*p2y + p1z*p2z)/(E1lab*E2lab));
        thetap = acos(1 - (m0*m0)/(2*E1lab*E2lab));
        opa->Fill(theta - thetap);

    
        pdif1 ->Fill(E1lab - sqrt(p1x*p1x + p1y*p1y + p1z*p1z));
        pdif2->Fill(E2lab - sqrt(p2x*p2x + p2y*p2y + p2z*p2z));

        
        //smearing the values with a Gaussian
        E1m=rand->Gaus(E1lab, E1lab*sqrt(pow(0.02/sqrt(E1lab), 2) + 1/(0.08*8*pow(10,6)*E1lab) +0.007*0.007));
        E2m=rand->Gaus(E2lab, E2lab*sqrt(pow(0.02/sqrt(E2lab), 2) + 1/(0.08*8*pow(10,6)*E2lab) +0.007*0.007));
        thetam= rand -> Gaus(theta, 0.01);

        
        mass -> Fill(sqrt(2*E1m*E2m*(1-cos(thetam))));
        
        array[0] = E1m;
        array[1] = E2m;
        array[2] = E1lab*sqrt(pow(0.02/sqrt(E1lab), 2) + 1/(0.08*8*pow(10,6)*E1lab) +0.007*0.007);
        array[3] = E2lab*sqrt(pow(0.02/sqrt(E2lab), 2) + 1/(0.08*8*pow(10,6)*E2lab) +0.007*0.007);
        array[4] = thetam;
        
        
        
        ROOT::Math::Minimizer* min = ROOT::Math::Factory::CreateMinimizer(minName, algoName);
        
        
        // set tolerance , etc...
        min->SetMaxFunctionCalls(1000000); // for Minuit/Minuit2
        min->SetMaxIterations(10000);  // for GSL
        min->SetTolerance(0.0001);
        min->SetPrintLevel(0);
        
        // create funciton wrapper for minmizer
        // a IMultiGenFunction type
        ROOT::Math::Functor f(&ChiSq,9);
        double step[4] = {0.01,0.01, 0.01, 0.001};
        // starting point
        
        double variable[4] = {array[0], array[1], 0.5, array[4]};

        
        min->SetFunction(f);
        double low1;
        double low2;
        
        if(variable[0] - 100 < 0){ low1= 0;} else{low1 = variable[0] - 100;}
        if(variable[1] - 100 < 0){ low2= 0;} else{low2 = variable[1] - 100;}

        
        
        // Set the free variables to be minimized!
        min->SetLimitedVariable(0,"E1fit",variable[0], step[0], low1, variable[0]+100);
        min->SetLimitedVariable(1,"E2fit",variable[1], step[1], low2, variable[1]+100);
        min->SetFixedVariable(2, "E1m", array[0]);
        min->SetFixedVariable(3, "E2m", array[1]);
        min->SetFixedVariable(4, "sig1", array[2]);
        min->SetFixedVariable(5, "sig2", array[3]);
        min->SetFixedVariable(6, "thetam", array[4]);
        min->SetLimitedVariable(7, "lambda",variable[2], step[2], 0.4, 0.6);
        min->SetLimitedVariable(8, "theta", variable[3], step[3], variable[3]-0.006, variable[3]+0.006);
        // do the minimization
        min->Minimize();
        
        
        double* fit = const_cast <double*> (min->X());
        
        double ty = min->Status();
        
        if (ty ==0 && fit[0]>=0 && fit[1]>=0){
            fitted -> Fill (sqrt(2*fit[0]*fit[1]*(1-cos(fit[8]))));
            chih -> Fill(min->MinValue());
        }

        
    
    
    
    }

    
    TCanvas* can = new TCanvas();
    pen->Draw();
    can->Print("Econservation.pdf", "pdf");
    pmom->Draw();
    can->Print("Pconservation.pdf", "pdf");
    pdif1->Draw();
    can->Print("dif1.pdf", "pdf");
    pdif2->Draw();
    can->Print("dif2.pdf", "pdf");
    opa->Draw();
    can->Print("Opening_angle.pdf", "pdf");
    mass -> Draw();
    can->Print("lab_frame.pdf", "pdf");
    fitted->Draw();
    can->Print("fitted.pdf", "pdf");
    chih-> Draw();
    can-> Print("chi.pdf", "pdf");
    
    
    TFile* f = new TFile("result.root", "new");
    f->cd();
    mass->Write("Inv_mass_lab");
    fitted->Write("Fitted_inv_mass");
    
    
    return 0;


}