#include "CbmTrdModuleRecT.h"
#include "CbmTrdDigi.h"
#include "CbmTrdCluster.h"
#include "CbmTrdHit.h"
#include "CbmTrdParModDigi.h"

#include <FairLogger.h>

#include <TGeoPhysicalNode.h>

#include <TClonesArray.h>
#include <TGraphErrors.h>
#include <TF1.h>

#include <iostream>
#include <vector>

using std::cout;
using std::endl;

Double_t CbmTrdModuleRecT::fgDT[] = {4.181e-6, 1586, 24};
TGraphErrors* CbmTrdModuleRecT::fgEdep = NULL;
TGraphErrors* CbmTrdModuleRecT::fgT = NULL;
TF1* CbmTrdModuleRecT::fgPRF = NULL;
//_______________________________________________________________________________
CbmTrdModuleRecT::CbmTrdModuleRecT()
  : CbmTrdModuleRec()
  ,fConfigMap(0)
  ,fT0(0)
  ,fBuffer()
{
    
}

//_______________________________________________________________________________
CbmTrdModuleRecT::CbmTrdModuleRecT(Int_t mod, Int_t ly, Int_t rot)
  : CbmTrdModuleRec(mod, ly, rot)
  ,fConfigMap(0)
  ,fT0(0)
  ,fBuffer()
  {
//  printf("AddModuleT @ %d\n", mod);
  SetNameTitle(Form("TrdRecT%d", mod), "Reconstructor for triangular read-out.");
}

//_______________________________________________________________________________
CbmTrdModuleRecT::~CbmTrdModuleRecT()
{
}

//_______________________________________________________________________________
Bool_t CbmTrdModuleRecT::AddDigi(CbmTrdDigi *d, Int_t id)
{
  if(CWRITE()) cout<<"add @"<<id<<" "<<d->ToString();

  Int_t ch  = d->GetAddressChannel(), col,
        row = GetPadRowCol(ch, col),      
        tm  = d->GetTimeDAQ()-fT0;
  if(CWRITE()) printf("row[%2d] col[%2d] tm[%2d]\n", row, col, tm);
  CbmTrdCluster *cl(NULL);
  
  // get the link to saved clusters
  std::map<Int_t,std::vector<CbmTrdCluster*>>::iterator it=fBuffer.find(row);
  // check for saved 
  if(it!=fBuffer.end()){
    Bool_t kINSERT(kFALSE);
    std::vector<CbmTrdCluster*>::iterator itc=fBuffer[row].begin();
    for(; itc!=fBuffer[row].end(); itc++){
      UShort_t tc = (*itc)->GetStartTime();
      if(CWRITE()) cout<<(*itc)->ToString();;
      if(tm<tc-5) continue;
      else if(tm<tc+5) {
        if(CWRITE()) printf("match time \n");
        if((*itc)->IsChannelInRange(ch)==0){ 
          (*itc)->AddDigi(id, ch);
          kINSERT=kTRUE;
          if(CWRITE()) cout<<"          => Ad "<<(*itc)->ToString();        
          break;
        } 
      } else {
        if(CWRITE()) printf("break for time \n");
        break;
      }
    }
    
    if(!kINSERT){
      if(itc != fBuffer[row].end() && itc != fBuffer[row].begin()){
        itc--;
        fBuffer[row].insert(itc, cl = new CbmTrdCluster(fModAddress, id, ch, row, tm));
        if(CWRITE()) cout<<"          => Cl (I) "<<cl->ToString();        
      } else {
        fBuffer[row].push_back(cl = new CbmTrdCluster(fModAddress, id, ch, row, tm));
        if(CWRITE()) cout<<"          => Cl (Pb) "<<cl->ToString();                
      }
      cl->SetTrianglePads();
    }
  } else {
    fBuffer[row].push_back(cl = new CbmTrdCluster(fModAddress, id, ch, row, tm));
    cl->SetTrianglePads();
    if(CWRITE()) cout<<"          => Cl (Nw) "<<cl->ToString();                
  }
  return kTRUE;
}

//_______________________________________________________________________________
Int_t CbmTrdModuleRecT::GetOverThreshold() const
{
  Int_t nch(0);
  for(std::map<Int_t, std::vector<CbmTrdCluster*>>::const_iterator ir=fBuffer.cbegin(); ir!=fBuffer.cend(); ir++){
    for(std::vector<CbmTrdCluster*>::const_iterator itc=(*ir).second.cbegin(); itc!=(*ir).second.cend(); itc++) nch+=(*itc)->GetNCols();
  }
  return nch;
}

//_______________________________________________________________________________
Int_t CbmTrdModuleRecT::FindClusters()
{
  CbmTrdCluster *cl(NULL);
  
  // get the link to saved clusters
  Int_t ncl(0);
  std::vector<CbmTrdCluster*>::iterator itc0, itc1;
  for(std::map<Int_t, std::vector<CbmTrdCluster*>>::iterator ir=fBuffer.begin(); ir!=fBuffer.end(); ir++){
    Bool_t kMERGE(kTRUE);
    while(kMERGE && (*ir).second.size()>1){ // try merge clusters
      itc0=(*ir).second.begin(), itc1=itc0; itc1++;
      kMERGE=kFALSE;
      for(; itc1!=(*ir).second.end(); itc1++){
        if((*itc1)->Merge((*itc0))){
          kMERGE = kTRUE;
          delete (*itc0); 
          itc1=(*ir).second.erase(itc0);
        }
        itc0 = itc1;
      }        
    }
    for(itc0=(*ir).second.begin(); itc0!=(*ir).second.end(); itc0++){ 
      cl = new((*fClusters)[ncl++]) CbmTrdCluster(*(*itc0));
      cl->SetTrianglePads();
      if(cl->GetNCols() != cl->GetNofDigis()){
        LOG(warn) << GetName() << "::FindClusters : digi to pads failed for:";
        cout<<cl->ToString();
        cout<<cl->CbmCluster::ToString();
      }
      delete (*itc0); 
    }
  }
  fBuffer.clear();
  
  //printf("fClusters[%p] nCl[%d]\n", (void*)fClusters, fClusters->GetEntriesFast());
  LOG(info) << GetName() << "::FindClusters : " << ncl;
  return ncl;
}

//_______________________________________________________________________________
Bool_t CbmTrdModuleRecT::MakeHits()
{
  return kTRUE;  
}

#include <TCanvas.h>
#include <TH1.h>
#define NANODE 9
//_______________________________________________________________________________
CbmTrdHit* CbmTrdModuleRecT::MakeHit(Int_t ic, const CbmTrdCluster *c, std::vector<const CbmTrdDigi*> *digis)
{
  if(!fgEdep){ // first use initialization of PRF helppers
    fgEdep = new TGraphErrors;
    fgEdep->SetLineColor(kBlue);fgEdep->SetLineWidth(2);
    fgT = new TGraphErrors;
    fgT->SetLineColor(kBlue);fgT->SetLineWidth(2);
    fgPRF=new TF1("prf", "[0]*exp(-0.5*((x-[1])/[2])**2)", -10, 10);
    fgPRF->SetLineColor(kRed);
    fgPRF->SetParNames("E", "x", "prf");
  }
  TH1 *hf(NULL); TCanvas *cvs(NULL);
  if(CDRAW()){
    cvs = new TCanvas("c", "TRD Anode Hypothesis",10,600,1000,500); cvs->Divide(2,1,1.e-5,1.e-5);
    TVirtualPad *vp = cvs->cd(1);
    vp->SetLeftMargin(0.06904908);
    vp->SetRightMargin(0.009969325);
    vp->SetTopMargin(0.01402806);
    vp = cvs->cd(2);
    vp->SetLeftMargin(0.06904908);
    vp->SetRightMargin(0.009969325);
    vp->SetTopMargin(0.01402806);
    hf = new TH1I("hf", ";x [pw];s [ADC chs]", 100,-3, 3);
    hf->GetYaxis()->SetRangeUser(-50, 4500);
    hf->Draw("p");
  }
  if(CWRITE()) cout<<GetName()<<" c["<<ic<<"] "<<c->ToString();
  Double_t  r, re,      // rect signal
            t, te,      // tilt signal
            max(0),     // max signal
            xlo, xhi,   // space range
            xc(-0.5);   // pad position 
  Int_t dt, j(0),
        //ovf(0),
        col, row = GetPadRowCol(c->GetStartCh(), col);
  vector<Double_t> vs,  // cluster charge profile 
                   vse, // cluster charge errors 
                   vx,  // cluster space distribution
                   vxe; // cluster space error parametrization
  vector<Char_t>   vt;  // cluster time profile
  ULong64_t t0=0; 
  Char_t  ddt,          // signal time offset wrt prompt  
          dt0(0),       // cluster time offset wrt arbitrary t0
          //typ,          // cluster max signal type
          pad(0);          // cluster max relative pad index
  for(vector<const CbmTrdDigi*>::iterator i=digis->begin(); i!= digis->end(); i++, j++){   
    if(CWRITE()) cout<<(*i)->ToString();
    if(t0==0) t0 = (*i)->GetTimeDAQ(); // set arbitrary t0 to avoid double digis loop
    r = (*i)->GetCharge(t, dt);
    //if(t>4094 || r>4094) ovf = 1;
    if(t>0) t-=1000;
    if(r>0) r-=1000;
    
    // process tilt signal
    ddt=(*i)->GetTimeDAQ()-t0; if(ddt < dt0) dt0 = ddt;
    te = 100.;  //fgErrParam[0] + fgErrParam[1]*t + fgErrParam[2]*t*t ;
    vt.push_back(ddt);
    vs.push_back(t); 
    vse.push_back(te);
    vx.push_back(xc); 
    vxe.push_back(0.035); 
    if(t>max){
      max=t; /*typ='T';*/pad=j;
    }
    xc+=0.5;
    
    // process rect signal
    ddt+=dt; if(ddt < dt0) dt0 = ddt;
    re = 100.;  //fgErrParam[0] + fgErrParam[1]*r + fgErrParam[2]*r*r ;
    vt.push_back(ddt);
    vs.push_back(r);
    vse.push_back(re);
    vx.push_back(xc); 
    vxe.push_back(0.); 
    if(r>max){
      max=r; /*typ='R';*/pad=j;
    }
    xc+=0.5;
  }
  // reset t0, time and space profile
  t0+=dt0; col+=pad;
  for(UInt_t idx(0); idx<vx.size(); idx++){vt[idx]-=dt0;vx[idx]-=pad;}

  // add front and back anchor points if needed
  if(TMath::Abs(vs[0])>1.e-3) {
    xc = vx[0]; ddt=vt[0]; 
    vs.insert(vs.begin(), 0);
    vse.insert(vse.begin(), 10);
    vt.insert(vt.begin(), ddt);
    vx.insert(vx.begin(), xc-0.5);
    vxe.insert(vxe.begin(), 0);
  }
  Int_t n(vs.size()-1);
  if(TMath::Abs(vs[n])>1.e-3) {
    xc = vx[n]; ddt=vt[n];
    vs.push_back(0);
    vse.push_back(10);
    vt.push_back(ddt);
    vx.push_back(xc+0.5);
    vxe.push_back(0.035);
    n++;
  }
  xlo = vx[0]-0.5; xhi = vx[n]+0.5;
  if(n<fgEdep->GetN()) for(Int_t ip(fgEdep->GetN()-1); ip>=n; ip--) fgEdep->RemovePoint(ip);
    
  if(CWRITE()) {
    printf("t0[%llu] dt0[%d] col[%2d]\n", t0, dt0, col);
    for(UInt_t idx(0); idx<vs.size(); idx++){    
      printf("%2d %2d %6.1f+-%6.1f %5.2f+-%5.2f\n", idx, vt[idx], vs[idx], vse[idx], vx[idx], vxe[idx]);
    }
  }
  // process space profile; hypothesis testing
  Int_t anode(-1);
  Double_t  dy(1./Double_t(NANODE)), xcl[NANODE+1], ecl[NANODE+1],
            chiArray[NANODE+1]={0.}, chi2(1e5), par[3];
  for(Int_t ia(0); ia<=NANODE; ia++){
    for(UInt_t idx(0); idx<vs.size(); idx++){
      if(vxe[idx]>0){
        fgEdep->SetPoint(idx, vx[idx]+0.5-ia*dy, vs[idx]);
        fgEdep->SetPointError(idx, vxe[idx], vse[idx]);
      } else {
        fgEdep->SetPoint(idx, vx[idx], vs[idx]);
        fgEdep->SetPointError(idx, vxe[idx], vse[idx]);
      }
    }
    fgPRF->SetParameter(0, max);
    fgPRF->SetParameter(1, 0.); fgPRF->SetParLimits(1, -1.2, 1.2);
    //fgPRF->FixParameter(2, 0.65); //SGM); 
    fgPRF->SetParameter(2, 0.65); fgPRF->SetParLimits(2, 0.45, 10.5);
    //fgPRF->ReleaseParameter(2);    fgPRF->SetParLimits(2, 0.4, 0.8);
    fgEdep->Fit(fgPRF, "QB", "goff", xlo, xhi);
    if(!fgPRF->GetNDF()) return NULL;
    chiArray[ia]= fgPRF->GetChisquare()/fgPRF->GetNDF();
    xcl[ia]     = fgPRF->GetParameter(1);
    ecl[ia]     = fgPRF->Integral(xlo, xhi);
    if(chiArray[ia]<chi2){chi2 = chiArray[ia]; anode=ia; fgPRF->GetParameters(par);}
    
    if(CWRITE()) printf("  REF%d :: M[%6.3f] S[%5.3f] chi2[%f] Eth[%f]\n", ia, col+fgPRF->GetParameter(1), fgPRF->GetParameter(2), chiArray[ia], fgPRF->Integral(xlo, xhi));
  }
//   const Int_t ncols = fDigiPar->GetNofColumns();
//   const Int_t nrows = fDigiPar->GetNofRows();
  TVector3 local_pad, local_pad_err;
  Int_t srow, sector= fDigiPar->GetSectorRow(row, srow);
  fDigiPar->GetPadPosition(sector, col, srow, local_pad, local_pad_err);
  Double_t 
    local[3] = {
      local_pad[0]/* + xcl[anode]*/,
      local_pad[1]/* + (4.5-anode)*fDigiPar->GetAnodeWireSpacing()*/,
      local_pad[2]+0.2}, 
      global[3], globalErr[3] = {xcl[anode]*fDigiPar->GetPadSizeX(0), (4.5-anode)*fDigiPar->GetAnodeWireSpacing()/*0.02, 0.08*/, 0.08};
  LocalToMaster(local, global);
  
  // process time profile
  n-=2;
  if(n<fgT->GetN()) for(Int_t ip(fgT->GetN()-1); ip>=n; ip--) fgT->RemovePoint(ip);
  for(UInt_t idx(1); idx<vt.size()-1; idx++){
    Double_t dtFEE = fgDT[0]*(vs[idx]-fgDT[1])*(vs[idx]-fgDT[1])/CbmTrdDigi::Clk(CbmTrdDigi::kFASP);
    if(vxe[idx]>0) vx[idx] += 0.5-anode*dy;
    fgT->SetPoint(idx-1, vx[idx], vt[idx] - dtFEE);
  }
  if(n>1) fgT->Fit("pol1", "Q", "goff");
  if(CDRAW()){
    cvs->cd(1);
    for(UInt_t idx(0); idx<vs.size(); idx++){
      fgEdep->SetPoint(idx, vx[idx], vs[idx]);
      fgEdep->SetPointError(idx, vxe[idx], vse[idx]);
    }

    hf->Draw("p");     hf->GetXaxis()->SetRangeUser(xlo-0.25, xhi+0.25);
    hf->SetTitle(Form("%d[%d] row[%d] col[%2d] an[%+d] m[%+4.2f] s[%4.2f] E[%7.2f] chi2[%7.2f]", ic, Int_t(vs.size()), row, col, anode, fgPRF->GetParameter(1), fgPRF->GetParameter(2), fgPRF->Integral(xlo, xhi), fgPRF->GetChisquare()/fgPRF->GetNDF()));
    hf->GetXaxis()->SetRangeUser(xlo-0.25, xhi+0.25);
    hf->GetYaxis()->SetRangeUser(-100., 4500);
    fgEdep->Draw("pl"); 
    fgEdep->GetFunction("prf")->SetParameters(par);

    cvs->cd(2);
    hf = (TH1*)hf->DrawClone("p");     
    hf->GetXaxis()->SetRangeUser(xlo+0.25, xhi-0.25);
    hf->GetYaxis()->SetRangeUser(-10, 20);
    hf->SetTitle(Form("%d row[%d] col[%2d] an[%+d] m[%+4.2f] s[%4.2f] E[%7.2f] chi2[%7.2f]", ic, row, col, anode, fgPRF->GetParameter(1), fgPRF->GetParameter(2), fgPRF->Integral(xlo, xhi), fgPRF->GetChisquare()/fgPRF->GetNDF()));
    //       hf->GetXaxis()->SetRangeUser(xlo-0.25, xhi+0.25);
    //       //hf->GetYaxis()->SetRangeUser(0., 4500);
    fgT->Draw("pl");
    cvs->Modified(); cvs->Update();
    cvs->SaveAs(Form("cl_%02d_A%d.gif", ic, anode));
  }
  TF1 *f = fgT->GetFunction("pol1");
  Double_t time = f->GetParameter(0)-fgDT[2],
            dtime = TMath::Abs(f->GetParameter(1)*(xhi-xlo-2)),
            tdrift  = 100; // should depend on Ua
  
  Int_t nofHits = fHits->GetEntriesFast();
  CbmTrdHit *hit = new ((*fHits)[nofHits]) CbmTrdHit(fModAddress, 
                                      global, globalErr, chi2, 
                                      ic, 
                                      0, 0, ecl[anode],
                                      CbmTrdDigi::Clk(CbmTrdDigi::kFASP)*(t0+time)- tdrift, dtime);
//   if(typ == 'R') hit->SetClassType();
//   else hit->SetClassType(kFALSE);
//   if(ovf) hit->SetOverFlow();
  if(CWRITE()) cout<<hit->ToString();
  return hit;
}

ClassImp(CbmTrdModuleRecT)