#include <iostream>
#include <TFile.h>
#include <TTree.h>
#include <TTreeReader.h>
#include <TStopwatch.h>
#include <TH1.h>
#include <TH2.h>
#include <TGraph.h>
#include <TRandom3.h>
#include <TCanvas.h>

TCanvas* cGraphs;
TCanvas* cProb;


void computeNSD2(TGraph* ref, TGraph* gr,
         Double_t threshDT/*microsec*/,
         Double_t OverlapTMin/*sec*/,
         Double_t OverlapTMax/*sec*/,
         Int_t& npts,
         Double_t& NSD2)
{
  NSD2 = 0;
  npts = 0;
  for (Long64_t p=0; p<ref->GetN(); p++) {
    Double_t tref, dtref, dt;
    ref->GetPoint(p, tref, dtref);
    if (tref>=OverlapTMin && tref<=OverlapTMax && dtref>threshDT) {
      dt = gr->Eval(tref);
      //    cout << i << " (" << tref << "," << yref << "," << y << ")" << endl;
      NSD2 += pow(dt-dtref,2);
      npts++;
    }
  }
  if (NSD2>0) {
    NSD2 = sqrt(NSD2);
    NSD2 /= npts;
  }
  else
    NSD2 = 1.0;
  return;
}





Double_t findshift(TGraph* ref, TGraph* gr,
           Double_t OverlapTMin/*sec*/,
           Double_t OverlapTMax/*sec*/,
           Double_t threshDT/*microsec*/)
{
  Double_t shift = 0;
  Int_t iterations = 0;
  TGraph* grShifted = (TGraph*)gr->Clone("grShifted");
  TRandom3 rdm;
  Int_t npts = 0;
  Double_t NSD2 = 0;  // normalized sum of differences squared (NSD2)
  Double_t prevNSD2 = 0;
  Double_t NSD2min = 1;
  Double_t tmin, tmax, aux;
  Double_t tini;
  TGraph* gInvNSD2 = new TGraph();
  
  ref->GetPoint(0, tini, aux);
  
  // First get a coarse distribution of the 1/NSD2
  for (Int_t p=0; p<gr->GetN(); p++) {
    npts = 0;
    NSD2 = 0;
    gr->GetPoint(p, shift, aux);
    shift -= tini;
    
    // shift the graph time
    for (Long64_t ps=0; ps<gr->GetN(); ps++) {
      Double_t tsec, dt;
      gr->GetPoint(ps, tsec, dt);
      grShifted->SetPoint(ps, tsec-shift, dt);
    }
    
    // check that the min/max times are within the overlap limits
    grShifted->GetPoint(0, tmin, aux);
    grShifted->GetPoint(grShifted->GetN()-1, tmax, aux);
    if (tmin<OverlapTMin && tmax>OverlapTMax) {
      // compute NSD2 of grShifted with respect to ref
      computeNSD2(ref, grShifted, threshDT, OverlapTMin, OverlapTMax, npts, NSD2);
      // cout << p << " ";
      // cout.precision(10);
      // cout << shift;
      // cout << " " << npts << " " << NSD2 << endl;
      // Fill histogram with inverse NSD2 whose GetRandom() method
      // will serve guide the search for a precise time shift.
      gInvNSD2->SetPoint(gInvNSD2->GetN(), shift, 1/NSD2);
    }
    //    cout << shift << " " << tmin1 << " " << tmax1 << "\n" << npts << " " << NSD2 << endl;
  }

  // Retreive time shift with largest 1/NSD2
  Double_t bestshift = 0;  // return value
  Double_t maxInvNSD2 = 0;
  Double_t invNSD2 = 0;
  for (Int_t i=0; i<gInvNSD2->GetN(); i++) {
    gInvNSD2->GetPoint(i, shift, invNSD2);
    if (invNSD2>maxInvNSD2) {
      bestshift = shift;
      maxInvNSD2 = invNSD2;
    }
  }
  
  cProb = new TCanvas("cPorb","Prob");
  gInvNSD2->Draw("al*");
  
  return bestshift;
}
  


////////////////////////////////////////////////////////////////////////////////
//  _______ _                     _     _  __ _                               //
// |__   __(_)                   | |   (_)/ _| |                              //
//    | |   _ _ __ ___   ___  ___| |__  _| |_| |_                             //
//    | |  | | '_ ` _ \ / _ \/ __| '_ \| |  _| __|                            //
//    | |  | | | | | | |  __/\__ \ | | | | | | |_                             //
//    |_|  |_|_| |_| |_|\___||___/_| |_|_|_|  \__|                            //
//                                                                            //
// Main function of this code.
////////////////////////////////////////////////////////////////////////////////

int timeshift(
	UShort_t Board=0, UShort_t Chan=5,
         UShort_t minE=400, UShort_t maxE=600,
//          UShort_t minE= 1800, UShort_t maxE=2300,
      //   UShort_t Board=1, UShort_t Chan=4,
       //  UShort_t minE= 1700, UShort_t maxE=2200,
        //  UShort_t Board=2, UShort_t Chan=4,
        //  UShort_t minE= 1500, UShort_t maxE=1900,
          UShort_t refBoard=3, UShort_t refChan=5,
          UShort_t refMinE= 400, UShort_t refMaxE=600,
          Double_t OverlapTMin= 0.3/*sec*/,
          Double_t OverlapTMax= 10,/*sec*///
          Double_t threshDT= 100/*microsec*/, //80
          Int_t MaxEntry=5000000, Int_t FindShift=1)
{
    

    TString infile = "/home/bavarians/exp/test0/DAQ/run/RAW/DataR_run.root";

  TFile* myFile = new TFile(infile);
  TTree* tree = (TTree*)myFile->Get("Data_R");
  tree->Print();
  
  // Simple progress monitor
  TStopwatch StpWatch;
  Long64_t numEntries = tree->GetEntries();;
  long double Frac[6];
  int fIndex = 0;
  Frac[0] = 0.01;
  Frac[1] = 0.25;
  Frac[2] = 0.5;
  Frac[3] = 0.75;
  Frac[4] = 0.9;
  Frac[5] = 1.0;

   
  TTreeReader theReader("Data_R", myFile);
  TTreeReaderValue<ULong64_t> rvTimestamp(theReader, "Timestamp");
  TTreeReaderValue<UShort_t> rvBoard = {theReader, "Board"};
  TTreeReaderValue<UShort_t> rvChannel = {theReader, "Channel"};
  TTreeReaderValue<UShort_t> rvEnergy = {theReader, "Energy"};
  TTreeReaderValue<UInt_t> rvFlags = {theReader, "Flags"};


  Long64_t nentries = theReader.GetEntries();

  ULong64_t* ts0 = new ULong64_t[nentries]; // reference channel
  ULong64_t* ts1 = new ULong64_t[nentries];

  cout << "Extracting data from " << infile << " ..." << endl;
  
  Long64_t entry = 0;
  Long64_t e0 = 0;
  Long64_t e1 = 0;


  // Save the timestamps in arrays for selected channels using beam related events (energy cut)
  while(theReader.Next() && entry<MaxEntry) {
    if (*rvTimestamp/1e12<2*OverlapTMax) {
      if (*rvBoard==refBoard && *rvChannel==refChan && *rvEnergy>refMinE && *rvEnergy<refMaxE) {
    ts0[e0] = *rvTimestamp;
    e0++;
      }
      else if (*rvBoard==Board && *rvChannel==Chan && *rvEnergy>minE && *rvEnergy<maxE) {
    ts1[e1] = *rvTimestamp;
    e1++;
      }
    }
  }
  
  cout << "Within the first " << 2*OverlapTMax << " seconds found:\n"
       << e0 << " entries of Board " << refBoard << " Chan " << refChan << "\n"
       << e1 << " entries of Board " << Board << " Chan " << Chan << "\n" << endl;

  Double_t tshift = 0.0;// 7.2303 - 6.52073 + 0.00534 + 0.08806 + 0.00012; // run_17

  // Make TGraphs for each channel taking only events with deltaT
  // greater than a threshold value (in microseconds). This optimizes
  // the timeshift search, focusing on the large deltaT peaks which
  // are much less frequent and thus easy to spot.
  TGraph* Delta0 = new TGraph();
  Delta0->SetLineColor(kRed);
  TGraph* Delta1 = new TGraph();
  Delta1->SetLineColor(kBlue);

  
  Double_t MaxDeltaT = 0;
  for (Long64_t i=0; i<e0-1; i++) {
    Double_t DTus = (ts0[i+1]-ts0[i])/1e6; // microseconds, hence 1/1e6
    if (DTus>threshDT) {
      Delta0->SetPoint(Delta0->GetN(), ts0[i]/1e12/*sec*/, DTus);
      if (DTus>MaxDeltaT)
    MaxDeltaT = DTus;
    }
  }

  for (Long64_t i=0; i<e1-1; i++) {
    Double_t DTus = (ts1[i+1]-ts1[i])/1e6; // microseconds, hence 1/1e6
    if (DTus>threshDT) {
      Delta1->SetPoint(Delta1->GetN(), ts1[i]/1e12-tshift, DTus);
      if (DTus>MaxDeltaT)
    MaxDeltaT = DTus;
    }
  }
  
  
  Double_t tref, yref, y;
  Double_t NSD2 = 0;  // normalized sum of differences squared (NSD2)
  Double_t NSD2min = 1;
  Double_t tmin0, tmax0, tmin1, tmax1, tmin2, tmax2;
  tmin0 = ts0[0]/1e12;
  tmax0 = ts0[e0-1]/1e12;
  tmin1 = ts1[0]/1e12;
  tmax1 = ts1[e1-1]/1e12;
    
  if (tmin0<=OverlapTMin && tmax0>=OverlapTMax && tmin1<=OverlapTMin && tmax1>=OverlapTMax) {
    if (FindShift) {
      cout << "executing findshift() ..." << endl;
      tshift = findshift(Delta0, Delta1,
             OverlapTMin, OverlapTMax, threshDT);
      cout.precision(10);
      cout << "best timeshift: " << tshift << " sec" << endl;
    }
    else {
      // Just compute the
      Int_t npts = 0;
      computeNSD2(Delta0, Delta1, threshDT, OverlapTMin, OverlapTMax, npts, NSD2);
      cout << tshift << " " << npts << " " << NSD2 << endl;
    }
  }
  else
    cout << "Need more entries for channel 0 or 2" << endl;
  
  
  // Draw the graphs
  cGraphs = new TCanvas("cGraphs","graphs");
  TH2F* hbk = new TH2F("hbk","",10000,OverlapTMin, OverlapTMax, 20,0,1.2*MaxDeltaT);
  hbk->GetXaxis()->SetTitle("Time [s]");
  hbk->GetXaxis()->CenterTitle();
  hbk->GetYaxis()->SetTitle("#DeltaT [#mus]");
  hbk->GetYaxis()->CenterTitle();
  TH2F* hba;
  if (FindShift) {
    cGraphs->Divide(1,2);
    hbk->SetTitle("Before");
    hba = new TH2F("hba","After",10000,OverlapTMin, OverlapTMax, 20,0,1.2*MaxDeltaT);
    hba->GetXaxis()->SetTitle("Time [s]");
    hba->GetXaxis()->CenterTitle();
    hba->GetYaxis()->SetTitle("#DeltaT [#mus]");
    hba->GetYaxis()->CenterTitle();
    cGraphs->cd(1);
    hbk->Draw();
    Delta0->Draw("lp same");
    TGraph* Delta1clone = (TGraph*)Delta1->Clone();Delta1clone->Draw("lp same");

    cGraphs->cd(2);
    hba->Draw();
    Delta0->Draw("lp same");
    for (Int_t i=0; i<Delta1->GetN(); i++) {
      Double_t tsec, DTus;
      Delta1->GetPoint(i, tsec, DTus);
      Delta1->SetPoint(i, tsec-tshift, DTus);
    }
    Delta1->Draw("lp same");
  }
  else {
    hbk->Draw();
    Delta0->Draw("lp same");
    Delta1->Draw("lp same");
  }

  
  return 0;
}