#include <TCanvas.h>
#include <TChain.h>
#include <TFile.h>
#include <TGraph.h>
#include <TH1.h>
#include <TSpectrum.h>
#include <TFitter.h>
#include <TStopwatch.h>
#include <iostream>
#include <TSystem.h>
#include <TPolyMarker.h>
#include <fstream>

using namespace std;


//===========================================================================================================
//                              Init mini
//===========================================================================================================

int iteration_count = 0;
const int nb_parameter = 1;
double* parameter;
int m_section = 9;
const int Bin = 1500;
//===========================================================================================================


double DistCalc(const double *parameter);
long double NumericalMinimization(const char* minName, const char* algoName);

void ToffHighRes(int section =8, double initToff = 0.0){

    m_section = section;
    //===========================================================================================================

    double buffer[nb_parameter] = {initToff};
    parameter = new double[nb_parameter];
    parameter = buffer;
    //===========================================================================================================

    DistCalc(parameter);
    long double ToffSection = NumericalMinimization("Minuit","Migrad");

    //===========================================================================================================
    TChain *chain = new TChain("PhysicsTree");
    chain->Add("../../root/analysis/Run247NoAlex.root");
 
    TH1F * hOriginal= new TH1F ("hOriginal",Form("Section_%d Original",m_section),Bin,2.5,4.5);
    TH1F *hCorrected = new TH1F("hCorrected" , Form("Section_%d Corrected",m_section),Bin,2.5,4.5) ;
    TString to_draw = Form("FF_Brho/3.10761* (FF_T13+%f)/FF_D13*29.9792*sqrt(1-pow(FF_D13,2)"
            "/(pow((FF_T13+%f)*29.9792,2)))>>hOriginal"
            ,parameter[0],parameter[0]) ;

    TString to_draw2 = Form("FF_Brho/3.10761* (FF_T13+%Lf)/FF_D13*29.9792*sqrt(1-pow(FF_D13,2)"
            "/(pow((FF_T13+%Lf)*29.9792,2)))>>hCorrected"
            ,ToffSection,ToffSection) ;

    TString cond = Form("FPMW_Section==%d",m_section);
    //===========================================================================================================

    TCanvas* c1 = new TCanvas("c2","c2",800,800);
    c1->Divide(2);
    c1->cd(1);
    chain->Draw(to_draw,cond,"");
    gPad->SetGrid();
    c1->cd(2);
    chain->Draw(to_draw2,cond,"");
    gPad->SetGrid();

    //===========================================================================================================
    //                                      Saving
    //===========================================================================================================

    string filename = "Output/ToffAoQ_Sec_" +to_string(m_section)+".cal";
    ofstream outfile(Form("Output/ToffAoQ_Sec_%d.cal",m_section)) ;
    string target_prefix = "ToffAoQ_Sec_" + to_string(m_section);
       
    if(outfile.is_open()){
        outfile << target_prefix << " " << ToffSection;
        outfile.close();
    }
}

long double NumericalMinimization(const char* minName, const char* algoName){

    ROOT::Math::Minimizer* min = ROOT::Math::Factory::CreateMinimizer(minName, algoName);

    min->SetMaxFunctionCalls(1000000);
    min->SetMaxIterations(10000);
    min->SetTolerance(0.01);
    min->SetPrecision(0.01);
    min->SetPrintLevel(1);

    ROOT::Math::Functor f(&DistCalc,nb_parameter);

    min->SetFunction(f);

    double step = 0.1 , lowerBound = -1.2 , higherBound = 1.2;
    min->SetLimitedVariable(0,Form("ToffSection_%d",m_section),parameter[0],step,lowerBound,higherBound);
    min->Minimize();

    const double* xs = min->X();

    DistCalc(xs);
    return xs[0];

}


double DistCalc(const  double* parameter){

    long double distance = 0;
    iteration_count++;  // Increment iteration count

    //===========================================================================================================
    //                                      LoadTree
    //===========================================================================================================
    //===========================================================================================================

    TChain *chain = new TChain("PhysicsTree");
    chain->Add("../../root/analysis/Run247NoAlex.root");
    //===========================================================================================================
    //                              Draw hist
    //===========================================================================================================

    TH1F *hist = (new TH1F("hist" , "hist" , Bin , 2.5 , 4.5));

    TString to_draw = Form("FF_Brho/3.10761* (FF_T13+%f)/FF_D13*29.9792*sqrt(1-pow(FF_D13,2)/(pow((FF_T13+%f)*29.9792,2)))>>hist"
            ,parameter[0],parameter[0]) ;
    TString cond = Form("FPMW_Section==%d",m_section);
    chain->Draw(to_draw,cond,"");

    //===========================================================================================================
    TSpectrum PeakFinder(30);
    Double_t source[Bin], dest[Bin];
    // Load hist in a array
    for (int i=0 ; i< Bin ; i++){
        source[i] = hist->GetBinContent(i+1);
    }
    //===========================================================================================================
    // find peaks
    int nPeaks = PeakFinder.SearchHighRes(source,dest,Bin,3,30,kTRUE,20,kTRUE,10);
    Double_t *xPeaks = PeakFinder.GetPositionX(); //peak position in bin number
    vector<Double_t> fPositionX(nPeaks), fPositionY(nPeaks); // Vector to hold peak position in X and Y with size number of peak
    Double_t fPositionXarr[nPeaks], fPositionYarr[nPeaks]; // Vector to hold peak position in X and Y with size number of peak
    vector<double> peakClose3, peakClose4;


    for (int i = 0; i < nPeaks; i++) {
        Double_t a = xPeaks[i];
        int bin = 1 + Int_t(a + 0.5);
        fPositionX[i] = hist->GetBinCenter(bin);
        fPositionXarr[i] = hist->GetBinCenter(bin);
        fPositionY[i] = hist->GetBinContent(bin);
        fPositionYarr[i] = hist->GetBinContent(bin);

        // find if closest to 3 or 4
        (round(fPositionX[i]) == 3 ? peakClose3 : peakClose4).push_back(fPositionX[i]) ;
    }


    //===========================================================================================================
    // find peak nearest to 3 if there is at least 2 peak near 3
    long double bestPeak3 = 1e6;
    long double bestPeak4 = 1e6;

    if (peakClose3.size()>2){
        for (int j=0; j<peakClose3.size(); j++){
            //cout << "Curr dist " << fabs(xPeaks[j] - 3.0) << endl;
            //cout << "Curr position " << peakClose3[j]  << endl;
            //cout << "Best dist" <<fabs(bestPeak - 3.0) << endl;
            if (fabs(peakClose3[j] - 3.0 ) < fabs(bestPeak3 -3.0)){
                bestPeak3 = peakClose3[j];
            } // end cond nearer
        }// end loop peaks
    }

    if (peakClose4.size()>2){
        for (int j=0; j<peakClose4.size(); j++){
            //cout << "Curr dist " << fabs(xPeaks[j] - 3.0) << endl;
            //cout << "Curr position " << xPeaks[j]  << endl;
            //cout << "Best dist" <<fabs(bestPeak - 3.0) << endl;
            if (fabs(peakClose4[j] - 4.0 ) < fabs(bestPeak4 -4.0)){
                bestPeak4 = peakClose4[j];
            } // end cond nearer
        }// end loop peaks
    }


    //===========================================================================================================

    cout << " ************************ " << iteration_count  << " *************************** "  << endl;
    cout << "Toff " << parameter[0] << endl;
    if (peakClose3.size()>2){
        cout << "Best peak3 " << bestPeak3 << endl;
    }
    if (peakClose4.size()>2){
        cout << "Best peak4 " << bestPeak4 << endl;
    }
    //===========================================================================================================
    distance += fabs( (peakClose3.size() > 2 ?  bestPeak3 : 3.0) - 3.0) 
        +fabs( (peakClose4.size() > 2 ?  bestPeak4 : 4.0) - 4.0); 
    /*
    //if result check is needed
    TPolyMarker *pm = (TPolyMarker *)hist->GetListOfFunctions()->FindObject("TPolyMarker");
    if (pm) {
    hist->GetListOfFunctions()->Remove(pm);
    delete pm;
    }
    pm = new TPolyMarker(nPeaks, fPositionXarr, fPositionYarr);
    hist->GetListOfFunctions()->Add(pm);
    pm->SetMarkerStyle(23);
    pm->SetMarkerColor(kRed);
    pm->SetMarkerSize(1.3);

    TH1F *d = new TH1F("d", "", Bin, 2.5, 4.5);
    for (int i = 0; i < Bin; i++){
    d->SetBinContent(i + 1, dest[i]);
    }
    d->SetLineColor(kRed);

    hist->Draw();
    d->Draw("SAME");
    //d->Delete();
    */

    hist->Delete();

    // Garbage collector
    chain->Reset();
    delete chain;

    return distance;

}