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

#include "PndTrkLegendreTransform2.h"

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

using namespace std;


// -----   Default constructor   -------------------------------------------
PndTrkLegendreTransform2::PndTrkLegendreTransform2() {
  fThetaMin    = 0;    
  fThetaMax    = 180;  
  fRMin        = -0.07; // -1
  fRMax        = 0.07;  // 1
  // initialize fhLegendre // CHECK
  fThetaStepping  = (fThetaMax - fThetaMin)/NOFTHETASTEPS;
  fRadStepping = (fRMax - fRMin)/NOFRADSTEPS;
  fThreshold = 8;
  fNofGoodTracks = 0;
}

PndTrkLegendreTransform2::PndTrkLegendreTransform2(double rmin, double rmax) {
  fThetaMin    = 0;    
  fThetaMax    = 180;  
  fRMin        = rmin;
  fRMax        = rmax;
  // initialize fhLegendre // CHECK
  fThetaStepping  = (fThetaMax - fThetaMin)/NOFTHETASTEPS;
  fRadStepping = (fRMax - fRMin)/NOFRADSTEPS;
  fThreshold = 8;
  fNofGoodTracks = 0;
}

// -------------------------------------------------------------------------

// -----   Destructor   ----------------------------------------------------
PndTrkLegendreTransform2::~PndTrkLegendreTransform2() { 

}
// -------------------------------------------------------------------------

void PndTrkLegendreTransform2::ResetLegendre(){

  for(int itheta = 0; itheta < NOFTHETASTEPS; itheta++) {
    for(int irad = 0; irad < NOFRADSTEPS; irad++) {
      fhTotVotes[itheta][irad] = 0;
      for(int idet = 0; idet < NOFDETIDSTEPS; idet++) {
	fhVotes[itheta][irad][idet] = 0;
	for(int ihit = 0; ihit < NOFHITIDSTEPS; ihit++) {
	  fhLegendre[itheta][irad][idet][ihit] = 0;
	}
      }
    }
  }

  for(int itrk = 0; itrk < NOFPOSSIBLETRACKS; itrk++) {
    fhGoodTracks[itrk][0] = 0;
    fhGoodTracks[itrk][1] = 0;
  }
  
  fNofGoodTracks = 0;



}
// ------ HISTOGRAM --------------------------------------------------------
void PndTrkLegendreTransform2::Fill(int hitid, int thetastep, int radstep, int detstep) {
  int hitstep = fhVotes[thetastep][radstep][detstep];
  for(int ihit = 0; ihit < hitstep; ihit++) {
    int comparehit = fhLegendre[thetastep][radstep][detstep][ihit];
    if(hitid == comparehit) return;
  }

  // cout << "DET " << detstep << " HIT " << hitstep << " theta/rad " << thetastep << " " << radstep << endl; 
  fhLegendre[thetastep][radstep][detstep][hitstep] = hitid;
  fhVotes[thetastep][radstep][detstep]++;
  fhTotVotes[thetastep][radstep]++;

  if(fhTotVotes[thetastep][radstep] == fThreshold) {
    
    fhGoodTracks[fNofGoodTracks][0] = thetastep;
    fhGoodTracks[fNofGoodTracks][1] = radstep;
    fNofGoodTracks++;
  }

}
void PndTrkLegendreTransform2::FillLegendreHisto(int detid, int hitid, double x, double y, double radius) {

  // compute detstep CHECK
  // 0 mvd pix
  // 1 mvd str
  // 2 stt
  // 3 fts
  int detstep = detid; // CHECK only for now as test
  // compute r varying theta
  Double_t  thetaDeg    = fThetaMin;
 
  while(thetaDeg < fThetaMax) {
    Double_t  thetaRad = thetaDeg * TMath::DegToRad();
    int thetastep = (thetaDeg - fThetaMin)/fThetaStepping;

    // 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 = (r1 - fRMin)/fRadStepping;
      // compute hitstep

      if(radstep > 0) Fill(hitid, thetastep, radstep, detstep);

      radstep = (r2 - fRMin)/fRadStepping;
      // compute hitstep CHECK
      if(radstep > 0) Fill(hitid, thetastep, radstep, detstep);

    }
    else { // MVD, SCITIL, GEM
      double r = x * TMath::Cos(thetaRad) + y * TMath::Sin(thetaRad);
      int radstep = (r - fRMin)/fRadStepping;
      if(radstep > 0) Fill(hitid, thetastep, radstep, detstep);
      
    } 
    
    thetaDeg += fThetaStepping;
    
  }
}

int PndTrkLegendreTransform2::GetNofMaxima() {
  return fNofGoodTracks;
}

std::vector< std::pair< int, int> > PndTrkLegendreTransform2::ExtractLegendreMaximum(int imax, double &theta_max, double &r_max) {
  int thetastep = fhGoodTracks[imax][0];
  int radstep = fhGoodTracks[imax][1];
  
  theta_max = fThetaMin + thetastep * fThetaStepping;
  r_max = fRMin + radstep * fRadStepping;

  std::vector< std::pair < int, int > > listofpairs;
  int votesfrommvdpix = fhVotes[thetastep][radstep][0];
  for(int ihit = 0; ihit < votesfrommvdpix; ihit++) {
    std::pair<int, int > thispair = std::make_pair(0, fhLegendre[thetastep][radstep][0][ihit]);
    listofpairs.push_back(thispair);
  }
  int votesfrommvdstr = fhVotes[thetastep][radstep][1];
  for(int ihit = 0; ihit < votesfrommvdstr; ihit++) {
    std::pair<int, int > thispair = std::make_pair(1, fhLegendre[thetastep][radstep][1][ihit]);
    listofpairs.push_back(thispair);
  }

  int votesfromstt = fhVotes[thetastep][radstep][2];
  for(int ihit = 0; ihit < votesfromstt; ihit++) {
    std::pair<int, int > thispair = std::make_pair(2, fhLegendre[thetastep][radstep][2][ihit]);
    listofpairs.push_back(thispair);
  }

  int votesfromfts = fhVotes[thetastep][radstep][3];
  for(int ihit = 0; ihit < votesfromfts; ihit++) {
    std::pair<int, int > thispair = std::make_pair(3, fhLegendre[thetastep][radstep][3][ihit]);
    listofpairs.push_back(thispair);
  }

  return listofpairs;
}

void PndTrkLegendreTransform2::ExtractLegendreSingleLineParameters(int imax, double &slope, double &intercept) {
   double theta, r;
   std::vector< std::pair< int, int> > thislist = ExtractLegendreMaximum(imax, theta, r);
   ExtractLineParameters(theta, r, slope, intercept);
}


void PndTrkLegendreTransform2::ExtractLineParameters(double theta, double r, double &slope, double &intercept) {
  // r = x cost + y sint --> y = r/sint - x/tant

  if(theta == 0.) theta = 1.e-6; // CHECK

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

}





void PndTrkLegendreTransform2::Draw() {
  TH2F *fhLegendreHisto = new TH2F("fhLegendreHisto", "", NOFTHETASTEPS, fThetaMin, fThetaMax, NOFRADSTEPS, fRMin, fRMax);
  
  for(int itheta = 0; itheta < NOFTHETASTEPS; itheta++) {
    for(int irad = 0; irad < NOFRADSTEPS; irad++) {
      double theta = fThetaMin + itheta * fThetaStepping;
      double r = fRMin + irad * fRadStepping;
      fhLegendreHisto->Fill(theta, r, fhTotVotes[itheta][irad]);
    }
  }

  fhLegendreHisto->Draw("colz");

  for(int imax = 0; imax < fNofGoodTracks; imax++) {
    int thetamaxstep = fhGoodTracks[imax][0];
    int rmaxstep = fhGoodTracks[imax][1];
    double theta_max = fThetaMin + thetamaxstep * fThetaStepping;
    double r_max = fRMin + rmaxstep * fRadStepping;
    TMarker *mrk = new TMarker(theta_max, r_max, 21);

    cout << "DRAW MAX " << imax << " "  << thetamaxstep << " " << rmaxstep << " " << fhTotVotes[thetamaxstep][rmaxstep] << endl;
    mrk->Draw("SAME");
  }
}




ClassImp(PndTrkLegendreTransform2)