#if !defined (__CINT__) || defined (__MAKECINT__)

#include <vector>
#include <algorithm>
#include <iostream>
#include "TF1.h"

#endif 

#include "TSiPed.hh"
#include "Stntuple/loop/TStnAna.hh"

TSiPed::TSiPed(const char* name, const char* title, const char* filename) 
  : TStnModule(name,title),
    fPed(NULL), fSi(NULL), fDigiCode(-1), fNevt(0), fFile(NULL), fPedDist(NULL), fProf(NULL),
    fNChan(256), fDPS(false), fL00Ped(false), fHlist(NULL)
{
  fFilename=filename;
}

TSiPed::~TSiPed() {
  if (fPed) delete fPed;
  if (fPedDist) {
    delete fPedDist;
  } 
  if (fHlist) delete fHlist;
}

int TSiPed::BeginJob() {
  fEvtPed.resize(fNChan);
  fPedIgor.resize(fNChan);
  fNoiseIgor.resize(fNChan);
  for (int i=0; i<fNChan; i++) {
    fEvtPed[i].reserve(10000);
  }

  fSi=(TSiStripBlock *)fAna->RegisterDataBlock("SiStripsBlock","TSiStripBlock");
  if (!fSi) {
    printf(" >>> branch *** %s *** doesn't exist \n","SiStripsBlock");
    fEnabled = 0;
  }

  BookHistograms();
  
  return 0;
}

int TSiPed::BeginRun() {
  return 0;
}

void TSiPed::BookHistograms() {
  char filename[80];
  sprintf(filename,"%s%d%s","peds",fDigiCode,".root");
  const char *fname;
  if (fFilename.c_str()!="") {
    fname=fFilename.c_str();
  }
  else {
    fname=filename;
  }
  fFile=new TFile(fname,"RECREATE","Pedestals");
  char treename[80];
  sprintf(treename,"%s%d","peds",fDigiCode);
  fPed=new TTree(treename,"Pedestals");
  fPed->Branch("p",&fp,"stp/I:ped/F:pedH:pedI:noiseH0:noiseH1:noiseI");

  char arrname[80];
  sprintf(arrname,"%s%d","pedDist",fDigiCode);
  fHlist = new TObjArray(0);
  fHlist->SetName(arrname);
  fPedDist=new TH1F[fNChan];
  for (int i=0; i<fNChan; i++){
    char histname[80];
    sprintf(histname,"%s%d_%d","pedDist",fDigiCode,i);
    fPedDist[i].SetBins(281,-25.5,255.5);
    fPedDist[i].SetName(histname);
    fPedDist[i].SetTitle(histname);
    fHlist->Add(&fPedDist[i]);
  }

  char profname[80];
  sprintf(profname,"%s%d","prof",fDigiCode);
  fProf=new TProfile(profname,profname,fNChan,0.0,float(fNChan-1));
  
}

Float_t TSiPed::median(TH1F *h) {
  if (!h) return -1;

  int n=h->GetNbinsX();
  Float_t a=h->GetSum();
  int i0,i;
  Float_t a2=0;

  for (i=1; i<=n && a2<a/2.0; i++) {
    a2+=h->GetBinContent(i);
  }
  i--;
  i0=i-1;

  Float_t x0=h->GetBinCenter(i0)+h->GetBinWidth(i0)/2.0;
  Float_t x1=h->GetBinCenter(i)+h->GetBinWidth(i)/2.0;
  Float_t y0=h->Integral(1,i0)/a;
  Float_t y1=h->Integral(1,i)/a;

  return (0.5 - y0)*(x1 - x0)/(y1 - y0) + x0;
}

Float_t TSiPed::DPS(std::vector<float> &h) {
  std::sort(h.begin(),h.end());
  return ( int( (h[33]+1.65) + 0.5 ) );
}

Float_t TSiPed::noise(TH1F *h, int flag) {
  if (!h || flag<0 || flag>1) return -1;

  float lo,hi,factor;
  if (flag==0) {
    lo=0.1587;
    hi=0.8413;
    factor=0.5;
  }
  else {
    lo=0.3085;
    hi=0.69146;
    factor=1.0;
  }
  
  int n=h->GetNbinsX();
  Float_t a=h->GetSum();
  int i0,i;
  Float_t a2=0;

  for (i=1; i<=n && a2<a*lo; i++) {
    a2+=h->GetBinContent(i);
  }
  i--;
  i0=i-1;

  Float_t x0=h->GetBinCenter(i0)+h->GetBinWidth(i0)/2.0;
  Float_t x1=h->GetBinCenter(i)+h->GetBinWidth(i)/2.0;
  Float_t y0=h->Integral(1,i0)/a;
  Float_t y1=h->Integral(1,i)/a;

  float xlo = (lo - y0)*(x1 - x0)/(y1 - y0) + x0;

  a2=0;
  for (i=1; i<=n && a2<a*hi; i++) {
    a2+=h->GetBinContent(i);
  }
  i--;
  i0=i-1;

  x0=h->GetBinCenter(i0)+h->GetBinWidth(i0)/2.0;
  x1=h->GetBinCenter(i)+h->GetBinWidth(i)/2.0;
  y0=h->Integral(1,i0)/a;
  y1=h->Integral(1,i)/a;

  float xhi = (hi - y0)*(x1 - x0)/(y1 - y0) + x0;


  return (xhi-xlo)*factor;
}

int TSiPed::Event(int ientry) {
  //  fSi->fSiStripList->Delete();
  fSi->Clear();

  if (fSi->GetEntry(ientry)) fNevt++;
  
  fSi->InitEvent();

  std::vector<float> h(128,0.0); // Chip's ADC values
  std::vector<float> dps(7,0.0); // Chip's DPS value
  std::vector<float> l00ped(fNChan,0.0); // Ladder's L00-like fitted pedestal
  std::vector<float> p(128,0.0); // Chip's L00-like fitted pedestal
  
  for (int i=fSi->fIndexFirstHit[fDigiCode]; i<=fSi->fIndexLastHit[fDigiCode]; i++) {
    h[fSi->SiStrip(i)->fStrip % 128] = fSi->SiStrip(i)->fADC + fSi->SiStrip(i)->fPed;
    if (fSi->SiStrip(i)->fStrip % 128 == 127) { // Last strip in chip
      dps[fSi->SiStrip(i)->fStrip / 128] = DPS(h);
      if (fL00Ped) {
	//	L00Fit(h,p);
	double prob=0;
	fSiPed.fitL00Ped(0, h, p, prob);
	if (prob<0.05) {
	  float l00_fallback = dps[fSi->SiStrip(i)->fStrip / 128];
	  for (int istr=0; istr<128; istr++) p[istr]=l00_fallback;
	}
	if (fDebugLevel>1) {
	  for (int j=0; j<128; j++) {
	    std::cout << "(" << fSi->SiStrip(i)->fStrip << ") " << "i " << j << "= " << h[j] << ", " << p[j] << std::endl;
	  }
	}
	l00ped.insert( l00ped.begin()+(fSi->SiStrip(i)->fStrip / 128)*128, 
		       p.begin(), p.end() );
	if (fDebugLevel>1) {
	  for (int j=0; j<fNChan; j++) {
	    std::cout << "j " << j << "= " << l00ped[j] << std::endl;
	  }
	}
      }
      h.clear();
      h.resize(128,0.0);
    }
  }
  std::cout << std::endl;
  

  for (int i=fSi->fIndexFirstHit[fDigiCode]; i<=fSi->fIndexLastHit[fDigiCode]; i++) {
    float adc=0;
    if (fDPS) {
      adc = (fSi->SiStrip(i)->fADC + fSi->SiStrip(i)->fPed) - dps[fSi->SiStrip(i)->fStrip / 128];
    }
    else if (fL00Ped) {
      adc = (fSi->SiStrip(i)->fADC + fSi->SiStrip(i)->fPed) - l00ped[fSi->SiStrip(i)->fStrip];
    }
    else {
      adc = fSi->SiStrip(i)->fADC + fSi->SiStrip(i)->fPed;
    }
    fEvtPed[fSi->SiStrip(i)->fStrip].push_back(adc);
    fPedDist[fSi->SiStrip(i)->fStrip].Fill(adc);
    fProf->Fill(fSi->SiStrip(i)->fStrip,adc);
    fPedIgor[fSi->SiStrip(i)->fStrip]=fSi->SiStrip(i)->fPed;
    fNoiseIgor[fSi->SiStrip(i)->fStrip]=fSi->SiStrip(i)->fNoise;
  }
  
  if (fSi->fDtL1A[fDigiCode] != 255) {
    std::cout << fSi->fBEState[fDigiCode] << " " << fSi->fDtL1A[fDigiCode] << std::endl;
  }
  return 0;
}  

int TSiPed::EndJob() {
  for (int i=0; i<fNChan; i++) {
    std::sort(fEvtPed[i].begin(), fEvtPed[i].end());
    fp.stp=i;
    if (fNevt%2==0) {
      fp.ped=(fEvtPed[i][fNevt/2]+fEvtPed[i][fNevt/2+1])/2.0;
    }
    else {
      fp.ped=fEvtPed[i][fNevt/2+1];
    }

    fp.pedH=median(&fPedDist[i]);
    
    fp.pedI=fPedIgor[i];

    fp.noiseH0=noise(&fPedDist[i],0);
    fp.noiseH1=noise(&fPedDist[i],1);
    fp.noiseI=fNoiseIgor[i];

    fPed->Fill();
    fEvtPed[i].clear();
  }
  fEvtPed.clear();
  fFile->cd();
  fHlist->Write();
  fFile->Write();
  fFile->Close();
  return 0;
}

void TSiPed::L00Fit(const std::vector<float> &adc, std::vector<float> &ped) {

  // Make input histo
  TH1F *h_adc_chip=new TH1F("h_adc_chip","h_adc_chip",128,-0.5,127.5);
  for (int i=0; i<128; i++) h_adc_chip->SetBinContent(i+1,adc[i]);

  // Fit parameters
  double par[7] = {7*0.};
  double par_mask[7] = {7*0.};

  //Set ranges on functions
  int exp1_lo = 0;
  int exp1_hi = 16; 
  int gauss_lo = 17; 
  int gauss_hi = 110; 
  int exp2_lo = 111; 
  int exp2_hi = 127;
 
  TF1 *f1 = new TF1("f1","expo",exp1_lo,exp1_hi);
  TF1 *f2 = new TF1("f2","pol2",gauss_lo,gauss_hi);
  TF1 *f3 = new TF1("f3","expo",exp2_lo,exp2_hi);
  TF1 *f4 = new TF1("f4","expo(0)+pol2(2)+expo(5)",0,127);

  //Format fit functions
  f1->SetLineColor(3);
  f1->SetLineWidth(3);
  f2->SetLineColor(3);
  f2->SetLineWidth(3);
  f3->SetLineColor(3);
  f3->SetLineWidth(3);
  f2->SetLineColor(3);
  f2->SetLineWidth(3);
  f4->SetLineColor(4);
  f4->SetLineWidth(5);
  
  // Initial fits
  //  TH1F *h_adc_chip=adc;
  h_adc_chip->Fit("f1","Q0R");
  h_adc_chip->Fit("f2","Q0R+");
  h_adc_chip->Fit("f3","Q0R+");
  
  // Final inital fit
  f1->GetParameters(&par[0]);
  f2->GetParameters(&par[2]);
  f3->GetParameters(&par[5]);
  f4->SetParameters(par);
  h_adc_chip->Fit("f4","Q0R+");      
  //  h_adc_chip->Draw();

  //Subtract fitted pededtal
  TH1F *h_adc_chip_mask= new TH1F("adc_chip_mask","Masked ADC vs. Chan",128,-0.5,127.5);
  TF1 *ped_fit = h_adc_chip->GetFunction("f4");
  for (int bin=1; bin<=128; bin++) {
    float x = h_adc_chip->GetBinCenter(bin);
    double ped_val = ped_fit->Eval(x);
    double diff = h_adc_chip->GetBinContent(bin)-ped_val;

    //Mask off signal + neighbor channels
    if (diff > 20) {
      if ((bin >= 3)&&(bin <= 126)) {
	h_adc_chip_mask->SetBinContent(bin-2,ped_val);	    
	h_adc_chip_mask->SetBinContent(bin-1,ped_val);
	h_adc_chip_mask->SetBinContent(bin,ped_val);	    
	h_adc_chip_mask->SetBinContent(bin+1,ped_val);
	h_adc_chip_mask->SetBinContent(bin+2,ped_val);
      }
      else if (bin == 2) {
	h_adc_chip_mask->SetBinContent(bin-1,ped_val);
	h_adc_chip_mask->SetBinContent(bin,ped_val);	    
	h_adc_chip_mask->SetBinContent(bin+1,ped_val);
	h_adc_chip_mask->SetBinContent(bin+2,ped_val);
      }
      else if (bin == 1) {
	h_adc_chip_mask->SetBinContent(bin,ped_val);	    
	h_adc_chip_mask->SetBinContent(bin+1,ped_val);
	h_adc_chip_mask->SetBinContent(bin+2,ped_val);
      }
      else if (bin == 127) {
	h_adc_chip_mask->SetBinContent(bin-2,ped_val);	    
	h_adc_chip_mask->SetBinContent(bin-1,ped_val);
	h_adc_chip_mask->SetBinContent(bin,ped_val);	    
	h_adc_chip_mask->SetBinContent(bin+1,ped_val);
      }
      else if (bin == 128) {
	h_adc_chip_mask->SetBinContent(bin-2,ped_val);	    
	h_adc_chip_mask->SetBinContent(bin-1,ped_val);
	h_adc_chip_mask->SetBinContent(bin,ped_val);	    
      }
    }
    else {
      h_adc_chip_mask->SetBinContent(bin,h_adc_chip->GetBinContent(bin));
    }
  }

  //Refit with signal chan masked off
  TF1 *f5 = new TF1("f5","expo",exp1_lo,exp1_hi);
  TF1 *f6 = new TF1("f6","pol2",gauss_lo,gauss_hi);
  TF1 *f7 = new TF1("f7","expo",exp2_lo,exp2_hi);
  TF1 *f8 = new TF1("f8","expo(0)+pol2(2)+expo(5)",0,127);
  f8->SetLineColor(2);
  f8->SetLineWidth(4);
  h_adc_chip_mask->Fit("f5","Q0R");
  h_adc_chip_mask->Fit("f6","Q0R+");
  h_adc_chip_mask->Fit("f7","Q0R+");
  f5->GetParameters(&par_mask[0]);
  f6->GetParameters(&par_mask[2]);
  f7->GetParameters(&par_mask[5]);
  f8->SetParameters(par_mask);
  h_adc_chip_mask->Fit("f8","Q0R+");

  //Re-subtract re-fitted pededtal
//   TH1F *h_adc_chip_fix = new TH1F("adc_chip_fix","Corrected ADC vs. Chan",128,0,127);
//   TH1F *h_adc_fix = new TH1F("adc_ped_fix","ADC - Fitted Pedestal",101,-50,50);
  TF1 *ped_fit_mask = h_adc_chip_mask->GetFunction("f8");
  //  h_adc_chip_mask->Draw();
  //  ped_fit_mask->Draw("same");
  for (int bin=1; bin<=128; bin++) {
    float x_mask = h_adc_chip_mask->GetBinCenter(bin);
    double ped_val_mask = ped_fit_mask->Eval(x_mask);
    ped[bin-1]=ped_val_mask;
    //    double diff_mask = h_adc_chip->GetBinContent(bin)-ped_val_mask;
    //    h_adc_chip_fix->Fill(x_mask,diff_mask);
    //    h_adc_fix->Fill(diff_mask);
  }

  //Clean up functions
  f1->Delete();
  f2->Delete();
  f3->Delete();
  f4->Delete();
  f5->Delete();
  f6->Delete();
  f7->Delete();
  // Return pedestal function.  USER MUST DELETE THIS!!!!
//   ped=ped_fit_mask;
//   ped->Print();
//  ped=(TF1*)ped_fit_mask->Clone();
//  ped_fit_mask->Copy(*ped);
  //  ped->SetName("L00ChipPed");
  f8->Delete();
  //  ped->Print();
  h_adc_chip_mask->Delete();
  h_adc_chip->Delete();
}