////////////////////////////////////////////////////////////
//
// PndTrkLegendreTransform3
// 
// Class to perform the Legendre Transformation
//
// authors: Lia Lavezzi - INFN Pavia (2012)
//                                   
////////////////////////////////////////////////////////////

#include "PndTrkLegendreTransform3.h"

#include <iostream>
#include "TH2F.h"
#include "TMarker.h"
#include "TMath.h"

using namespace std;


// -----   Default constructor   -------------------------------------------
PndTrkLegendreTransform3::PndTrkLegendreTransform3() {
  fThetaMin    = 0;    
  fThetaMax    = 180;  
  fRMin        = -0.07; // -145
  fRMax        = 0.07;  // 1

  fThetaStepping  = (fThetaMax - fThetaMin)/NOFTHETASTEPS;
  fRadStepping = (fRMax - fRMin)/NOFRADSTEPS;
  fThreshold = 15;
  fLegendrePlane.clear();
  fListOfClusters = PndTrkClusterList();
}

// -----   Destructor   ----------------------------------------------------
PndTrkLegendreTransform3::~PndTrkLegendreTransform3() { 
  // delete fListOfClusters;
}

void PndTrkLegendreTransform3::ResetLegendre(){
  fLegendrePlane.clear();
  fMaxima.clear();
  fListOfClusters.Reset();
}

// ----------------------------------------------------------------------
// theta always in deg!
int PndTrkLegendreTransform3::FromThetaToThetaStep(double theta) {
  return (int) ((theta - fThetaMin)/fThetaStepping);
}

double PndTrkLegendreTransform3::FromThetaStepToTheta(int thetastep) {
return thetastep * fThetaStepping + fThetaMin;
}

int PndTrkLegendreTransform3::FromRToRStep(double r) {
  return (int) ((r - fRMin)/fRadStepping);
}

double PndTrkLegendreTransform3::FromRStepToR(int radstep) {
  return radstep * fRadStepping + fRMin;
}

int PndTrkLegendreTransform3::FromThetaRStepToGlobalStep(int thetastep, int radstep) {
  return (int) (NOFTHETASTEPS * radstep + thetastep);
}

void PndTrkLegendreTransform3::FromGlobalStepToThetaRStep(int globalstep, int &thetastep, int &radstep) {
  radstep =  globalstep / NOFTHETASTEPS;
  thetastep = globalstep - radstep * NOFTHETASTEPS;
}

int PndTrkLegendreTransform3::FromThetaRToGlobalStep(double theta, double r) {
  int thetastep = FromThetaToThetaStep(theta);
  int radstep = FromRToRStep(r);
  return (int) (NOFTHETASTEPS * radstep + thetastep);
}

void PndTrkLegendreTransform3::FromGlobalStepToThetaR(int globalstep, double &theta, double &r) {
  int radstep, thetastep;
  FromGlobalStepToThetaRStep(globalstep, thetastep, radstep);
  
  theta = FromThetaStepToTheta(thetastep);
  r =  FromRStepToR(radstep);
}
// ----------------------------------------------------------------------

void PndTrkLegendreTransform3::Fill(PndTrkHit *hit, int globalstep) {


  std::map< int, PndTrkLegendreCluster* >::iterator it;
  it = fLegendrePlane.find(globalstep);

  // if found, just add
  if(it != fLegendrePlane.end())
    {
      PndTrkLegendreCluster *associatedhits = (*it).second;

      if(associatedhits->DoesContain(hit)) return;

      associatedhits->AddHit(hit);
      if(associatedhits->GetNofHits() == fThreshold) {
	fMaxima.push_back(globalstep);
      }
      fLegendrePlane[globalstep] = associatedhits;
  }
  else { // new one
    PndTrkLegendreCluster *associatedhit = new PndTrkLegendreCluster();
    associatedhit->AddHit(hit);

    double theta, r;
    FromGlobalStepToThetaR(globalstep, theta, r);
    associatedhit->SetTheta(theta);
    associatedhit->SetR(r);
    std::pair<int, PndTrkLegendreCluster* > startclus = make_pair(globalstep, associatedhit);
    fLegendrePlane.insert(startclus);
  } 

}

void PndTrkLegendreTransform3::FillLegendreHisto(PndTrkHit *hit, double x, double y, double radius) {
  // compute r varying theta
  Double_t  thetaDeg    = fThetaMin;
  while(thetaDeg < fThetaMax) {
    Double_t  thetaRad = thetaDeg * TMath::DegToRad();
    int thetastep = FromThetaToThetaStep(thetaDeg);
    // CHECK define once for all r
    // and radstep to have it faster

    // STT
    if(radius > 0.) {
      double r1 = x * TMath::Cos(thetaRad) + y * TMath::Sin(thetaRad) + radius;
      double r2 = x * TMath::Cos(thetaRad) + y * TMath::Sin(thetaRad) - radius;

      int radstep = FromRToRStep(r1);
      if(radstep > 0) Fill(hit, FromThetaRStepToGlobalStep(thetastep, radstep));

      radstep = FromRToRStep(r2);
      if(radstep > 0)  Fill(hit, FromThetaRStepToGlobalStep(thetastep, radstep));

    }
    else { // MVD, SCITIL, GEM
      double r = x * TMath::Cos(thetaRad) + y * TMath::Sin(thetaRad);
      int radstep = FromRToRStep(r);
      if(radstep > 0)  Fill(hit, FromThetaRStepToGlobalStep(thetastep, radstep));

    } 
    
    thetaDeg += fThetaStepping;
    
  }
}

int PndTrkLegendreTransform3::GetNofMaxima() {
  return fMaxima.size();
}

PndTrkLegendreCluster *PndTrkLegendreTransform3::ExtractHighestMaximum() {
  int tmpcounter = 0;
  PndTrkLegendreCluster *cluster = NULL;
  //  cout << "fMaxima " << fMaxima.size() << endl;
  for(int imax = 0; imax < fMaxima.size(); imax++) {
    int iglobalstep = fMaxima.at(imax);
    std::map< int, PndTrkLegendreCluster* >::iterator it;
    it = fLegendrePlane.find(iglobalstep);
    int counter = ((*it).second)->GetNofHits();
  
    if(counter > tmpcounter) {
      cluster = (*it).second;
      tmpcounter = counter;
    }
  }
  return cluster;

}

PndTrkLegendreCluster PndTrkLegendreTransform3::ExtractLegendreMaximum(int imax) {
  if(fMaxima.size() == 0) return PndTrkLegendreCluster();

  if(imax > fMaxima.size()) return PndTrkLegendreCluster();

  int globalstep = fMaxima.at(imax);
  std::map< int, PndTrkLegendreCluster* >::iterator it;
  it = fLegendrePlane.find(globalstep);
  return  *((*it).second);
}

PndTrkLegendreCluster PndTrkLegendreTransform3::ExtractHighestMaximumWithLineParameters(double &slope, double &intercept) {
  double theta, r;
  PndTrkLegendreCluster *thislist = ExtractHighestMaximum();
  ExtractLineParameters(thislist->GetTheta(), thislist->GetR(), slope, intercept);
  return *thislist;
}

PndTrkLegendreCluster PndTrkLegendreTransform3::ExtractLegendreMaximumWithLineParameters(int imax, double &slope, double &intercept) {
  double theta, r;
  PndTrkLegendreCluster thislist = ExtractLegendreMaximum(imax);
  ExtractLineParameters(thislist.GetTheta(), thislist.GetR(), slope, intercept);
   return thislist;
}

void PndTrkLegendreTransform3::ExtractLineParameters(PndTrkLegendreCluster *cluster, double &slope, double &intercept) {
  ExtractLineParameters(cluster->GetTheta(), cluster->GetR(), slope, intercept);
}

void PndTrkLegendreTransform3::ExtractLineParameters(double theta, double r, double &slope, double &intercept) {
  // r = x cost + y sint --> y = r/sint - x/tant
  //  cout << "theta " << theta << " r " << r << endl;
  if(theta == 0.) theta = 1.e-6; // CHECK

  slope = - 1./TMath::Tan(TMath::DegToRad() * theta);
  intercept = r/TMath::Sin(TMath::DegToRad() * theta);

}

// CHECK to be changed
PndTrkClusterList PndTrkLegendreTransform3::GetClusterList() {
  for(int imax = 0; imax < fMaxima.size(); imax++) {
    int iglobalstep = fMaxima.at(imax);
    std::map< int, PndTrkLegendreCluster* >::iterator it;
    it = fLegendrePlane.find(iglobalstep);
    PndTrkLegendreCluster *cluster = ((*it).second);
    double theta, r;
    FromGlobalStepToThetaR(iglobalstep, theta, r);
    cluster->SetTheta(theta);
    cluster->SetR(r);
    
    fListOfClusters.AddCluster(cluster);
  }
  return fListOfClusters;
}





void PndTrkLegendreTransform3::Draw() {
  TH2F *fhLegendreHisto = new TH2F("fhLegendreHisto", "", NOFTHETASTEPS, fThetaMin, fThetaMax, NOFRADSTEPS, fRMin, fRMax);
  
  std::map < int, PndTrkLegendreCluster *>::iterator it =  fLegendrePlane.begin();

  while(it != fLegendrePlane.end()) {
    PndTrkLegendreCluster *cluster = (*it).second;
    
    fhLegendreHisto->Fill(cluster->GetTheta(), cluster->GetR(), cluster->GetNofHits());
    it++;
  }

  fhLegendreHisto->Draw("colz");
}
void PndTrkLegendreTransform3::DrawMaximum(int imax) {
  
  double theta, r;
  PndTrkLegendreCluster thislist = ExtractLegendreMaximum(imax);
  
  TMarker *mrk = new TMarker(thislist.GetTheta(), thislist.GetR(), 21);
  
  cout << "DRAW MAX " << imax << " "  << thislist.GetTheta() << " " << thislist.GetR() << " " << thislist.GetNofHits() << endl;
  mrk->Draw("SAME");
}

void PndTrkLegendreTransform3::DrawMaximumHits(int imax) {

  PndTrkLegendreCluster thislist = ExtractLegendreMaximum(imax);
  thislist.LightUp();
}




ClassImp(PndTrkLegendreTransform3)