PndTrkLegendreTask.cxx

Go to the documentation of this file.

//
// PndTrkLegendreTask
// 
// Class for secondary track pattern recognition
//
// authors: Lia Lavezzi - INFN Pavia (2012)
//                                   

#include "PndTrkLegendreTask.h"

// stt
#include "PndSttHit.h"
#include "PndSttTube.h"
#include "PndSttMapCreator.h"
// sds
#include "PndSdsHit.h"
// track(cand)
#include "PndTrackCand.h"
#include "PndTrackCandHit.h"
#include "PndTrack.h"

#include "PndTrkConformalHit.h"
#include "PndTrkConformalHitList.h"
#include "PndTrkConformalTransform.h"
#include "PndTrkTools.h"
#include "PndTrkSkewHit.h"
#include "PndTrkFitter.h"
// fairroot
#include "FairRootManager.h"
#include "FairRunAna.h"
#include "FairRuntimeDb.h"
// ROOT
#include "TClonesArray.h"
#include "TVector3.h"
#include "TArc.h"
#include "TLine.h"
#include "TMarker.h"
#include "TSpectrum2.h" 
#include "TSpectrum.h" 
#include "TStopwatch.h" 
// tracking 
#include "PndTrkClusterList.h"
#include "PndTrkGlpkFits.h"
#include "PndTrkClean.h"

#include <iostream>
    
using namespace std;


// -----   Default constructor   -------------------------------------------
PndTrkLegendreTask::PndTrkLegendreTask() : FairTask("secondary track finder", 0), fDisplayOn(kFALSE), fPersistence(kTRUE), fUseMVDPix(kTRUE), fUseMVDStr(kTRUE), fUseSTT(kTRUE), fSecondary(kFALSE), fMvdPix_RealDistLimit(1000), fMvdStr_RealDistLimit(1000), fStt_RealDistLimit(1000), fMvdPix_ConfDistLimit(1000), fMvdStr_ConfDistLimit(1000), fStt_ConfDistLimit(1000), fInitDone(kFALSE) {
  sprintf(fSttBranch,"STTHit");
  sprintf(fMvdPixelBranch,"MVDHitsPixel");
  sprintf(fMvdStripBranch,"MVDHitsStrip");
  PndGeoHandling::Instance();
}

PndTrkLegendreTask::PndTrkLegendreTask(int verbose) : FairTask("secondary track finder", verbose), fDisplayOn(kFALSE), fPersistence(kTRUE), fUseMVDPix(kTRUE), fUseMVDStr(kTRUE), fUseSTT(kTRUE), fSecondary(kFALSE), fMvdPix_RealDistLimit(1000), fMvdStr_RealDistLimit(1000), fStt_RealDistLimit(1000), fMvdPix_ConfDistLimit(1000), fMvdStr_ConfDistLimit(1000), fStt_ConfDistLimit(1000), fInitDone(kFALSE) {
  sprintf(fSttBranch,"STTHit");
  sprintf(fMvdPixelBranch,"MVDHitsPixel");
  sprintf(fMvdStripBranch,"MVDHitsStrip");
  PndGeoHandling::Instance();
}

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

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

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



// -----   Public method Init   --------------------------------------------
InitStatus PndTrkLegendreTask::Init() {
  
  fEventCounter = 0;

  fMvdPix_RealDistLimit = 0.5; // CHECK limits
  fMvdStr_RealDistLimit = 0.5; // CHECK limits
  fStt_RealDistLimit =  1.5 * 0.5; // CHECK limits
  fMvdPix_ConfDistLimit = 0.003; // CHECK limits
  fMvdStr_ConfDistLimit = 0.003; // CHECK limits
  fStt_ConfDistLimit =  0.001; // CHECK limits
  if(fSecondary) {
     fMvdPix_ConfDistLimit = 0.007; // CHECK limits
     fMvdStr_ConfDistLimit = 0.007; // CHECK limits
   }
   

  // Get RootManager
  FairRootManager* ioman = FairRootManager::Instance();
  if ( ! ioman ) {
    cout << "-E- PndTrkLegendreTask::Init: "
         << "RootManager not instantiated, return!" << endl;
    return kFATAL;
  }

  // -- HITS -------------------------------------------------
  //
  // STT
  fSttHitArray = (TClonesArray*) ioman->GetObject(fSttBranch);
  if ( ! fSttHitArray ) {
    cout << "-W- PndTrkLegendreTask::Init: "
         << "No STTHit array, return!" << endl;
    return kERROR;
  }
  //
  // MVD PIXEL
  fMvdPixelHitArray = (TClonesArray*) ioman->GetObject(fMvdPixelBranch);
  if ( !fMvdPixelHitArray){
    std::cout << "-W- PndTrkLegendreTask::Init: " << "No MVD Pixel hitArray, return!" << std::endl;
    return kERROR;
  }
  //
  // MVD STRIP
  fMvdStripHitArray = (TClonesArray*) ioman->GetObject(fMvdStripBranch);
  if ( !fMvdStripHitArray){
    std::cout << "-W- PndTrkLegendreTask::Init: " << "No MVD Strip hitArray, return!" << std::endl;
    return kERROR;
  }

  fTrackArray = new TClonesArray("PndTrack");
  fTrackCandArray = new TClonesArray("PndTrackCand");
  ioman->Register("Track", "pr", fTrackArray, fPersistence); // CHECK
  ioman->Register("TrackCand",  "pr", fTrackCandArray, fPersistence); // CHECK

  
  // ----------------------------------------   maps of STT tubes
  fMapper = new PndSttMapCreator(fSttParameters);
  fTubeArray = fMapper->FillTubeArray();
  // ----------------------------------------------------  end map
 
  if(fDisplayOn) {
    display = new TCanvas("display", "display", 0, 0, 800, 800); // CHECK
    display->Divide(2, 2);
  }

  legendre = new PndTrkLegendreTransform();
  if(fSecondary) legendre->SetUpLegendreHisto(180, 0, 180, 1000, -1., 1.);
  else legendre->SetUpLegendreHisto();
  legendre->SetUpZoomHisto();

  conform = new PndTrkConformalTransform();

  tools = new PndTrkTools();
  fFitter = new PndTrkFitter(fVerbose);

  return kSUCCESS;

}

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

void PndTrkLegendreTask::SetParContainers() {
  FairRuntimeDb* rtdb = FairRunAna::Instance()->GetRuntimeDb();
  fSttParameters = (PndGeoSttPar*) rtdb->getContainer("PndGeoSttPar");
}
 
// -------------------------------------------------------------------------
 

void PndTrkLegendreTask::Initialize() {

  stthitlist = new PndTrkSttHitList(fTubeArray);
  mvdpixhitlist = new PndTrkSdsHitList(MVDPIXEL);
  mvdstrhitlist = new PndTrkSdsHitList(MVDSTRIP);

  if(fUseSTT) { 
    stthitlist->AddTCA(FairRootManager::Instance()->GetBranchId(fSttBranch), fSttHitArray);
    stthitlist->Instanciate();
  }

  if(fUseMVDPix) {
    mvdpixhitlist->AddTCA(FairRootManager::Instance()->GetBranchId(fMvdPixelBranch), fMvdPixelHitArray);
    mvdpixhitlist->InstanciatePixel();
  }

  if(fUseMVDStr) {
    mvdstrhitlist->AddTCA(FairRootManager::Instance()->GetBranchId(fMvdStripBranch), fMvdStripHitArray);
    mvdstrhitlist->InstanciateStrip();
  }

  conformalhitlist = new PndTrkConformalHitList();
  fFoundPeaks.clear();

  fInitDone = kTRUE;

}

void PndTrkLegendreTask::Exec(Option_t*) {
  fTrackArray->Delete();
  fTrackCandArray->Delete();

  if(fVerbose > 0) cout << "*********************** " << fEventCounter << " ***********************" << endl;
   // CHECK delete this ---
  //     if(fEventCounter == 126 || fEventCounter == 526) {
  //        fEventCounter++;
  //        return;
  //      }
   // ---
  fEventCounter++;

  // CHECK : it may happen that the hit of a track generate more than one track; e.g.
  // some of them are associated together to form a track, some are left out. If later the left outs
  // result in a new peak, this new peak will have also the old, already used hits, associated to it.
  // This happens since the used hits do not enter the new legendre transform, but can be associated
  // to the found cluster with the distance criterion. In this way the final cluster, which then is 
  // fitted again with the mvd in the legendre plane, will give an existing peak.
  // This results in an infinite loop ---> FIX IT! (how: timeout? better hit-to-cluster association? 
  // forbidden peak positions in legendre transform?)

  // CHECK THIS!
  if(fSttHitArray->GetEntriesFast() < 3) {
    Reset();  
    return;
  }
  
  Initialize();
  if(fVerbose > 1) {
  cout << "number of stt    hits " << fSttHitArray->GetEntriesFast() << endl;
  cout << "number of mvdpix hits " << fMvdPixelHitArray->GetEntriesFast() << endl;
  cout << "number of mvdstr hits " << fMvdStripHitArray->GetEntriesFast() << endl;
  }

  


  if(fDisplayOn)  {
    Refresh();
    char goOnChar;
    // cout << "Start?";
    //  cin >> goOnChar;
    display->Update();
    display->Modified();
    cout << " STARTING" << endl;
  }

  // translation and rotation - CHECK no rotation now
  Double_t delta = 0, trasl[2] = {0., 0.};
  if(!fSecondary)  {
    conform->SetOrigin(trasl[0], trasl[1], delta);
    Int_t nchits = FillConformalHitList();
    //  if(nchits == 0) return; // CHECK 
    // cout << nchits << " " << trasl[0] << " " <<  trasl[1] << " " << delta << endl;
  }

  int maxpeak = 1000;
  int ipeak = 0;
  PndTrkClusterList *clusterlist = new PndTrkClusterList();
  std::vector< std::pair<double, double> > foundpeaks;


  // LOOP OVER PEAKS UNTILL ITS HEIGHT IS <= 3
  fTimer = new TStopwatch();
  fTimer->Start();
  fTime = 0;
  while(maxpeak > 3) {
 
    // TIME
    if(fDisplayOn == kFALSE) {
      fTimer->Stop();
      fTime += fTimer->RealTime();
      if(fTime > 1.5) {
         if(fVerbose > 0) cerr << fTime << endl;
        Reset();  
        return;
      }
      fTimer->Start();
    }
    //

    if(fSecondary) {

      // translation and rotation - CHECK
      fRefHit = FindReferenceHit();
        if(fVerbose > 1) cout << "refhit " << fRefHit << endl;
      if(fRefHit == NULL)  {
        Reset();  
        return;
      }
      ComputeTraAndRot(fRefHit, delta, trasl);
      conform->SetOrigin(trasl[0], trasl[1], delta);
      Int_t nchits = FillConformalHitList();
      //  if(nchits == 0) return; // CHECK 
      if(fVerbose > 1)   cout << nchits << " " << trasl[0] << " " <<  trasl[1] << " " << delta << endl;
    }
 
    PndTrkCluster cluster;

    if(fDisplayOn)  {
      Refresh();
      char goOnChar;
      cout << endl;
      cout << "Next peak? ";
      cin >> goOnChar;
      display->Update();
      display->Modified();
    }

    if(fVerbose > 1)   cout << "@@@@ PEAK No. " << ipeak << " @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@" << endl;
    ipeak++;

    // APPLY LEGENDRE TO STT ALONE
    // -------------------------------------------------------
    
    double theta_max, r_max;
    maxpeak = ApplyLegendre(theta_max, r_max);
    if(maxpeak <= 3) break;
   
    double fitm, fitq;
    legendre->ExtractLegendreSingleLineParameters(fitm, fitq);
  
    // ADD HITS TO CLUSTER: PIXEL, STRIP AND STT PARALLEL
    // -------------------------------------------------------
    // method of hit-to-track association:
    // 0 conformal: mvd and stt in conformal plane
    // 1 real: mvd and stt in real plane
    // 2 mixed: mvd in real/stt in conformal plane
    int method = 0; 
//     cout << "cluster " << cluster.GetNofHits() << endl;
    cluster = CreateClusterByDistance(method, fitm, fitq);
//     cout << "CREATED " << endl;
//    for(int ihit = 0; ihit < cluster.GetNofHits(); ihit++) {
//      PndTrkHit *comparehit = (PndTrkHit*) cluster.GetHit(ihit);
//      cout << "hit " << comparehit->GetHitID() << " " << comparehit->GetDetectorID() << endl;
//     }

    // APPLY LEGENDRE TO CLUSTER + MVD
    // -------------------------------------------------------
  
    maxpeak = ApplyLegendre(&cluster, theta_max, r_max);
 

    // CLEANUP
    // -------------------------------------------------------
    PndTrkCluster cleancluster = Cleanup(cluster);
    cluster = cleancluster;
 

    // FIT WITH LEAST SQUARE STRAIGHT LINE
    // -------------------------------------------------------
  

    double fitm2, fitq2;
    fFitter->Reset();

    for(int ihit = 0; ihit < cluster.GetNofHits(); ihit++) 
      {
        PndTrkHit *hit = cluster.GetHit(ihit);
        double conformal[3];

        PndTrkConformalHit *chit = NULL;
        if(hit->GetDetectorID() == FairRootManager::Instance()->GetBranchId(fSttBranch)) chit = conform->GetConformalSttHit(hit);
        else chit = conform->GetConformalHit(hit);

        double xi1 = chit->GetU() + fitm * chit->GetIsochrone()/ TMath::Sqrt(fitm * fitm + 1);
        double yi1 = chit->GetV() - chit->GetIsochrone() / TMath::Sqrt(fitm * fitm + 1);

        double xi2 = chit->GetU() - fitm * chit->GetIsochrone() / TMath::Sqrt(fitm * fitm + 1);
        double yi2 = chit->GetV() + chit->GetIsochrone()/ TMath::Sqrt(fitm * fitm + 1);

        double xi = 0, yi = 0;

        fabs(yi1 - (fitm * xi1 + fitq)) < fabs(yi2 - (fitm * xi2 + fitq)) ? (yi = yi1, xi = xi1) : (yi = yi2, xi = xi2);

        double sigma =  chit->GetIsochrone();
        fFitter->SetPointToFit(xi, yi, sigma);

        if(fDisplayOn) {
          display->cd(2);
          TMarker *mrk = new TMarker(xi, yi, 6);
          mrk->Draw("SAME");
        } 
      }
 
    fFitter->StraightLineFit(fitm2, fitq2);
     

    if(fDisplayOn) {
      char goOnChar;
      display->cd(2);
      TLine *line = new TLine(-0.07, fitq + fitm * (-0.07), 0.07, fitq + fitm * (0.07));
      line->Draw("SAME");
      TLine *line2 = new TLine(-0.07, fitq2 + fitm2 * (-0.07), 0.07, fitq2 + fitm2 * (0.07));
      line2->SetLineColor(2);
      line2->Draw("SAME");
    
      display->Update();
      display->Modified();
      //      cin >> goOnChar;  
    }

    fitm = fitm2;
    fitq = fitq2;


  

    // -------------------------------------------------------
    if(cluster.GetNofHits() > 3) {
      if(fVerbose > 1)   cout << "ADDING CLUSTER WITH " << cluster.GetNofHits() << " hits " << endl;
      clusterlist->AddCluster(&cluster);
    }
    else {
      maxpeak = -1;  
      if(fVerbose > 1) cout << "MAXPEAK " << maxpeak << endl;
      continue;
    }
    if(fDisplayOn) {
      char goOnChar;
      display->cd(1);
      LightCluster(&cluster);
      cout << "waiting 2 " << endl;
      cin >> goOnChar;
    }
  
    // center and radius
    Double_t xc, yc, R;
    FromConformalToRealTrack(fitm, fitq, xc, yc, R);
   if(fVerbose > 1)  cout << "XR, YC, R: " << xc << " " << yc << " " << R << endl;

    PndTrkTrack *track = new PndTrkTrack(&cluster, xc, yc, R);

    if(fDisplayOn) {
      display->cd(1);
      track->Draw(kRed);
      display->Update();
      display->Modified();
      char goOnChar;
      cout << "waiting 3 " << endl;
      cin >> goOnChar;
    }


    // SKEWED ASSOCIATION ********* CHECK *********
    // -------------------------------------------------------
   if(fVerbose > 1)  cout << "%%%%%%%%%%%%%%%%%%%% ZFINDER %%%%%%%%%%%%%%%%%%%%%%%%%%" << endl;

    if(fDisplayOn) {
      RefreshZ();
      DrawZGeometry(2);
    }
    PndTrkCluster skewhitlist = CreateSkewHitList(track);
    skewhitlist = CleanUpSkewHitList(&skewhitlist);

    int iz = -1, jz = -1; 
    //   cout  << "##################### CHOOSE Z" << endl; 

    // 7. select which intersection is the most likely and add it to z fitting
    fFitter->Reset();
    for(int ihit = 0; ihit < skewhitlist.GetNofHits() - 1; ihit++) {
      PndTrkSkewHit *skewhit1 = (PndTrkSkewHit*) skewhitlist.GetHit(ihit);
      if(!skewhit1) continue;
      TVector3 fin_intersection1[2] = {skewhit1->GetIntersection1(), skewhit1->GetIntersection2()};
      double phi1[2] = {skewhit1->GetPhi1(), skewhit1->GetPhi2()};
      
      int jhit = ihit + 1;
      PndTrkSkewHit *skewhit2 = (PndTrkSkewHit*) skewhitlist.GetHit(jhit);
      if(!skewhit2) continue;
      TVector3 fin_intersection2[2] = {skewhit2->GetIntersection1(), skewhit2->GetIntersection2()};
      double phi2[2] = {skewhit2->GetPhi1(), skewhit2->GetPhi2()};
      
      //     cout << "start from " << ihit << " " << jhit << ": " << iz << " " << jz << endl;
      if(jz != -1) {
        iz = jz;
        double distance = fabs(fin_intersection1[iz].Z() - fin_intersection2[0].Z());
        jz = 0;
        // 11 22
        fabs(fin_intersection1[iz].Z() - fin_intersection2[1].Z()) < distance ? (distance = fabs(fin_intersection1[iz].Z() - fin_intersection2[1].Z()), jz = 1) : distance;
      }
      else {
        // 11 21      
        double distance = fabs(fin_intersection1[0].Z() - fin_intersection2[0].Z());
        iz = 0;
        jz = 0;
        // 11 22
        fabs(fin_intersection1[0].Z() - fin_intersection2[1].Z()) < distance ? (distance = fabs(fin_intersection1[0].Z() - fin_intersection2[1].Z()), iz = 0, jz = 1 ): distance;
        // 12 21
        fabs(fin_intersection1[1].Z() - fin_intersection2[0].Z()) < distance ? (distance = fabs(fin_intersection1[1].Z() - fin_intersection2[0].Z()), iz = 1, jz = 0) : distance;
        // 12 22
        fabs(fin_intersection1[1].Z() - fin_intersection2[1].Z()) < distance ? (distance = fabs(fin_intersection1[1].Z() - fin_intersection2[1].Z()), iz = 1, jz = 1): distance;
      }
      //      cout << "end with " << ihit << " " << jhit << ": " << iz << " " << jz << endl;

        fFitter->SetPointToFit(phi1[iz], fin_intersection1[iz].Z(), 1.);// (fin_intersection1[0].Z() + fin_intersection1[1].Z())/2.); // CHECK sigma?

        // CHECK
        if(iz == 0) skewhit1->SetPosition(skewhit1->GetIntersection1());
        else skewhit1->SetPosition(skewhit1->GetIntersection2());

        // CHECK last point?
        if(ihit == skewhitlist.GetNofHits() - 2) {
          fFitter->SetPointToFit(phi2[jz], fin_intersection2[jz].Z(), 1.);// (fin_intersection2[0].Z() + fin_intersection2[1].Z())/2.); // CHECK sigma?

        // CHECK
        if(jz == 0) skewhit2->SetPosition(skewhit2->GetIntersection1());
        else skewhit2->SetPosition(skewhit2->GetIntersection2());
        }
        
        
        if(fDisplayOn) {
          char goOnChar;
          display->cd(4);
          TMarker *mrkfoundzphi1 = new TMarker(phi1[iz], fin_intersection1[iz].Z(), 20);
          mrkfoundzphi1->SetMarkerColor(kYellow);
          mrkfoundzphi1->Draw("SAME");

          TMarker *mrkfoundzphi2 = new TMarker(phi2[jz], fin_intersection2[jz].Z(), 20);
          mrkfoundzphi2->SetMarkerColor(kYellow);
          mrkfoundzphi2->Draw("SAME");

          display->Update();
          display->Modified();  
          //    cin >> goOnChar;   
        }
    }


    // ADD MVD POINTS ==================== RECHECK
    PndTrkCluster *mvdcluster = track->GetCluster();
    for(int ihit = 0; ihit < mvdcluster->GetNofHits(); ihit++) {
      //      cout << ihit << " TROVATO " << endl;
      PndTrkHit *hit = mvdcluster->GetHit(ihit);
      if(hit->GetDetectorID() != FairRootManager::Instance()->GetBranchId(fMvdPixelBranch) && hit->GetDetectorID() != FairRootManager::Instance()->GetBranchId(fMvdStripBranch)) continue;
      TVector3 position = hit->GetPosition();
      double phi = track->ComputePhi(position);
      hit->SetPhi(phi);
      //      cout << ihit << " TROVATO " << phi << " " << position.Z() << endl;

        if(fDisplayOn) {
          char goOnChar;
          display->cd(4);
          TMarker *mrkfoundzphi = new TMarker(phi, position.Z(), 21);
          mrkfoundzphi->SetMarkerColor(kOrange);
          mrkfoundzphi->Draw("SAME");
          display->Update();
          display->Modified();  
          //    cin >> goOnChar;   
        }
     fFitter->SetPointToFit(phi, position.Z(), 0.1); // CHECK ERROR 
    }
    // ===================================


    Double_t ml, pl;
    Bool_t straightlinefit = fFitter->StraightLineFit(ml, pl);


    // CHECK the following fit: if it fails, put the hits into play again?
    if(straightlinefit = kTRUE) {

      if(fDisplayOn) {
        char goOnChar;
        display->cd(4);
        TLine *l222 = new TLine(-1000, -1000 * ml + pl, 1000, 1000 * ml + pl);
        l222->Draw("SAME");
        display->cd(3);
        l222->Draw("SAME");
        display->Update();
        display->Modified();  
        //      cin >> goOnChar;   
      }

      PndTrkCluster *trkcluster = track->GetCluster();
      // 8. last association z --> HIT: fill position and add to cluster ???? CHECK
      for(int ihit = 0; ihit < skewhitlist.GetNofHits(); ihit++) {
        PndTrkSkewHit *skewhit = (PndTrkSkewHit*) skewhitlist.GetHit(ihit);
        bool doescontain = trkcluster->DoesContain(skewhit);
        //      cout << "SKEW HIT " << skewhit->GetHitID() << " " << doescontain << endl;
        if(doescontain == kTRUE) {
          trkcluster->Replace(skewhit);
        }
        else trkcluster->AddHit(skewhit);
        //      trkcluster->AddHit(skewhit);

        TVector3 intersection1 = skewhit->GetIntersection1();
        double phi1 = track->ComputePhi(intersection1);
        double calcz1 = ml * phi1 + pl;
        TVector3 intersection2 = skewhit->GetIntersection2();
        double phi2 = track->ComputePhi(intersection2);
        double calcz2 = ml * phi2 + pl;
      
        fabs(intersection1.Z() - calcz1) <  fabs(intersection2.Z() - calcz2) ? skewhit->SetPosition(intersection1) : skewhit->SetPosition(intersection2);

      }

  
      // 9. add skewedhitlist to cluster of the track
      // fill the last two (missing) parameters
      for(int ihit = 0; ihit < trkcluster->GetNofHits(); ihit++) {
        PndTrkHit *hit = trkcluster->GetHit(ihit);      
        double phi = track->ComputePhi(hit->GetPosition()); // CHECK is it neede to recalculate it?
        hit->SetSortVariable(phi);
        hit->SetPhi(phi);

      }
      trkcluster->Sort();
      track->ComputeCharge();

      // 10. z-phi refit
      Bool_t zfit = ZPhiFit(0, trkcluster, ml, pl);
      //      cout << "zfit1 on " << trkcluster->GetNofHits() << " hits/ " << zfit << " " << ml << " " << pl << " tanl " << - track->GetCharge() * ml * (180./TMath::Pi())/R << endl;

      PndTrkCluster *tmpcluster = CleanupZPhiFit(trkcluster, ml, pl);
      trkcluster = tmpcluster;

      if(fDisplayOn) {
        char goOnChar;
        display->cd(4);
        DrawZGeometry();
        hzphi->SetXTitle("#phi");
        hzphi->SetYTitle("#z");
        hzphi->Draw();
        display->Update();
        display->Modified();  
      }

      zfit = ZPhiFit(1, trkcluster, ml, pl);
    
      // 11. fill the last two (missing) parameters

      //  tanl = -q ml (180/pi) /R: See PndTrkTrack.h for an explanation
     
  if(zfit) {
        track->SetTanL(- track->GetCharge() * ml * (180./TMath::Pi())/R);
        track->SetZ0(pl);
      }
      else { // CHECK put these in default
        track->SetTanL(-999);
        track->SetZ0(-999);
      } 
      //   cout << "GET TANL " << ml << " " << R << " " << track->GetTanL() << endl;

      // test -------------------------------------------------------
      PndTrkCluster *trkcluster2 = track->GetCluster();
      // trkcluster2->Print();

      if(fDisplayOn) {
        char goOnChar;
        display->cd(1);
        Refresh();
        for(int ihit = 0; ihit < trkcluster2->GetNofHits(); ihit++) {
          PndTrkHit *hit = trkcluster2->GetHit(ihit);
          hit->Draw(kMagenta);
        
          display->Update();
          display->Modified();  
          //    cin >> goOnChar;   
        }
      }
    }
    else { // CHECK put these in default
      track->SetTanL(-999);
      track->SetZ0(-999);
    }
    // ------------------------------------------------------- end test

    // TRANSFORM TO PNDTRACK AND PNDTRACKCAND
    // -------------------------------------------------------
    RegisterTrack(track);
   if(fVerbose > 1)  cout << "MAXPEAK " << maxpeak << endl;
  }
  if(fDisplayOn) {
    char goOnChar;
    display->Update();
    display->Modified();
    cout << "Finish? ";
    cin >> goOnChar;
    cout << "FINISH" << endl;
  }

  Reset();  
  delete clusterlist;

  
}

void PndTrkLegendreTask::Reset()
{
  if(fDisplayOn) {
    char goOnChar;
    display->Update();
    display->Modified();
    cout << "Finish? ";
    cin >> goOnChar;
    cout << "FINISH" << endl;
  }

  if(fInitDone) {
    if(stthitlist) delete stthitlist;
    if(mvdpixhitlist)  delete mvdpixhitlist; 
    if(mvdstrhitlist)  delete mvdstrhitlist;
    if(fTimer) {
      fTimer->Stop();
      fTime += fTimer->RealTime();
    
      if(fVerbose > 0) cerr << fEventCounter << " Real time " << fTime << " s" << endl;
    }
  }

  fInitDone = kFALSE;
}
// ============================================================================================
Int_t PndTrkLegendreTask::FillConformalHitList() {
  conformalhitlist->Reset();
  conformalhitlist->SetConformalTransform(conform);
    
  // FILL ONCE FOR ALL THE CONFORMAL HIT LIST 
  for(int jhit = 0; jhit < mvdpixhitlist->GetNofHits(); jhit++) {
    PndTrkHit *hit = mvdpixhitlist->GetHit(jhit);
    PndTrkConformalHit * chit = conform->GetConformalHit(hit);
    conformalhitlist->AddHit(chit);    
  }
  for(int jhit = 0; jhit < mvdstrhitlist->GetNofHits(); jhit++) {
    PndTrkHit *hit = mvdstrhitlist->GetHit(jhit);
    PndTrkConformalHit * chit = conform->GetConformalHit(hit);
    conformalhitlist->AddHit(chit);    
  }
  for(int jhit = 0; jhit < stthitlist->GetNofHits(); jhit++) {
    PndTrkHit *hit = stthitlist->GetHit(jhit);
    //   if(hit->IsSttSkew()) continue;
    //     PndTrkConformalHit * chit = conform->GetConformalSttHit(hit);
    //     conformalhitlist->AddHit(chit);    
   if(hit->IsSttSkew()) {
     // continue;
     PndTrkConformalHit * chit = conform->GetConformalHit(hit);
     conformalhitlist->AddHit(chit);    
   }
   else {
     PndTrkConformalHit * chit = conform->GetConformalSttHit(hit);
     conformalhitlist->AddHit(chit);    
   } 
  }
  
  return conformalhitlist->GetNofHits();
}

void PndTrkLegendreTask::FillLegendreHisto(Int_t mode)
{
  // mode 0 STT alone
  //      1 STT + MVD
  for(int ihit = 0; ihit < conformalhitlist->GetNofHits(); ihit++) {
    PndTrkConformalHit *chit = conformalhitlist->GetHit(ihit);
    if(mode == 0 && chit->GetDetectorID() != FairRootManager::Instance()->GetBranchId(fSttBranch)) continue;   
    PndTrkHit *hit = chit->GetHit();
    if(hit->IsUsed()) continue;
    legendre->FillLegendreHisto(chit->GetU(), chit->GetV(), chit->GetIsochrone());
    if(fDisplayOn) {
      DrawConfHit(chit->GetU(), chit->GetV(), chit->GetIsochrone());
    }
  }
}
 
void PndTrkLegendreTask::FillLegendreHisto(PndTrkCluster *cluster)
{
  // ---------------------------------------------------------------
  for(int ihit = 0; ihit < cluster->GetNofHits(); ihit++) {
    PndTrkHit *hit = cluster->GetHit(ihit);
    double conformal[3];
    PndTrkConformalHit * chit = conform->GetConformalSttHit(hit);
    legendre->FillLegendreHisto(chit->GetU(), chit->GetV(), chit->GetIsochrone());
    //      conformalhitlist->AddHit(chit);    
  }
    
  // add mvd hits to stt cluster
  for(int ihit = 0; ihit < conformalhitlist->GetNofHits(); ihit++) {
    PndTrkConformalHit *chit = conformalhitlist->GetHit(ihit);
    if(chit->GetDetectorID() == FairRootManager::Instance()->GetBranchId(fSttBranch)) continue;   
    PndTrkHit *hit = chit->GetHit();
    if(hit->IsUsed()) continue;
    legendre->FillLegendreHisto(chit->GetU(), chit->GetV(), chit->GetIsochrone());
    if(fDisplayOn) {
      DrawConfHit(chit->GetU(), chit->GetV(), chit->GetIsochrone());
    }
  }
}
 

 
PndTrkCluster PndTrkLegendreTask::CreateClusterByConfDistance(double fitm, double fitq) {
  
  PndTrkCluster cluster;
  // ADD HIT IN CONFORMAL PLANE WITH DISTANCE CRITERION
  // cost x + sint y - r = 0 -> |cost x0 + sint y0 - r|
  //    cout << "CONFORMALHITLIST " << conformalhitlist->GetNofHits() << endl;
  for(int ihit = 0; ihit < conformalhitlist->GetNofHits(); ihit++) {
    PndTrkConformalHit *chit = conformalhitlist->GetHit(ihit);
    double dist = chit->GetDistanceFromTrack(fitm, fitq);
    int hitID = chit->GetHitID();
    int detID = chit->GetDetectorID();
    
    // CHECK limits
    double distlimit = 0;
    if(detID == FairRootManager::Instance()->GetBranchId(fMvdPixelBranch)) distlimit = 0.003;
    else if(detID == FairRootManager::Instance()->GetBranchId(fMvdStripBranch)) distlimit = 0.003;
    else if(detID == FairRootManager::Instance()->GetBranchId(fSttBranch)) {
      if(chit->GetIsochrone() > 1.e-4) distlimit = 6 * chit->GetIsochrone();
      else distlimit = 0.001;
    }
    // ---------------------------
    Bool_t accept = kFALSE;
  

    if(dist < distlimit) {
      if(fDisplayOn) {
        if(chit->GetIsochrone() > 0) {
          TArc *arc = new TArc(chit->GetU(), chit->GetV(), chit->GetIsochrone());
          arc->SetFillStyle(0);
          arc->SetLineColor(5);
          display->cd(2);
          arc->Draw("SAME");
        }
        else {
          TMarker *mrk;
          if(detID == FairRootManager::Instance()->GetBranchId(fMvdPixelBranch)) mrk = new TMarker(chit->GetU(), chit->GetV(), 21);
          else if(detID == FairRootManager::Instance()->GetBranchId(fMvdStripBranch)) mrk = new TMarker(chit->GetU(), chit->GetV(), 25);
          mrk->SetMarkerColor(5);
          display->cd(2);
          mrk->Draw("SAME");
        }
      }

      PndTrkHit *hit;
      if(detID == FairRootManager::Instance()->GetBranchId(fMvdPixelBranch)) {
        hit = mvdpixhitlist->GetHit(hitID);
      }
      else if(detID == FairRootManager::Instance()->GetBranchId(fMvdStripBranch)) {
        hit = mvdstrhitlist->GetHit(hitID);
      }
      else if(detID == FairRootManager::Instance()->GetBranchId(fSttBranch)) {
        hit = stthitlist->GetHit(hitID);
      }
    
      // ********** CHECK ***********
      // if you put "is used" the efficiency beecomes much lower!
      // DO NOT:        if(!hit->IsUsed())
      cluster.AddHit(hit);
       accept = kTRUE;

    }
    
    //      else cout << "DISCARDED " << hitID << " " << detID << " " << dist << " " << distlimit << endl;
  }

  return cluster;
}

PndTrkCluster PndTrkLegendreTask::CreateClusterByRealDistance(double xc0, double yc0, double R0) {
  // ADD HIT IN REAL PLANE WITH DISTANCE CRITERION
  // sqrt((xc - x)**2 + (yc - y)**2) < distlim
  PndTrkCluster cluster;

  // .........................................................
  if(fDisplayOn) {
    TArc *arc = new TArc(xc0, yc0, R0);
    arc->SetFillStyle(0);
    arc->SetLineColor(5);
    display->cd(1);
    arc->Draw("SAME");
  }
  for(int ihit = 0; ihit < stthitlist->GetNofHits(); ihit++) {
    PndTrkHit *hit = stthitlist->GetHit(ihit);
    if(hit->IsSttSkew()) continue;
    double dist = hit->GetXYDistanceFromTrack(xc0, yc0, R0);
    //      cout << ihit << " dist " << dist << endl;
    // CHECK limits
    double distlimit = 1.5 * 0.5;
    if(dist < distlimit){ 
      //  cout << "TRACK  "  << xc0  << " " << yc0 << " " << R0 << endl;
      //  cout << "ADD HIT " << dist << " " << distlimit << " " << hit->GetHitID() << endl;
      cluster.AddHit(hit);
      if(fDisplayOn) {
        TArc *arc = new TArc(hit->GetPosition().X(), hit->GetPosition().Y(), hit->GetIsochrone());
        arc->SetFillStyle(0);
        arc->SetLineColor(5);
        display->cd(1);
        arc->Draw("SAME");
      }
    }
  }
  for(int ihit = 0; ihit < mvdpixhitlist->GetNofHits(); ihit++) {
    PndTrkHit *hit = mvdpixhitlist->GetHit(ihit);
    double dist = hit->GetXYDistanceFromTrack(xc0, yc0, R0);
    // CHECK limits
    double distlimit = 0.5;
    if(dist < distlimit)  {
      cluster.AddHit(hit);
      if(fDisplayOn) {
        TMarker *mrk = new TMarker(hit->GetPosition().X(), hit->GetPosition().Y(), 21);
        mrk->SetMarkerColor(5);
        display->cd(1);
        mrk->Draw("SAME");
      }
    }
  }
  for(int ihit = 0; ihit < mvdstrhitlist->GetNofHits(); ihit++) {
    PndTrkHit *hit = mvdstrhitlist->GetHit(ihit);
    double dist = hit->GetXYDistanceFromTrack(xc0, yc0, R0);
    // CHECK limits
    double distlimit = 0.5;
    if(dist < distlimit){ 
      cluster.AddHit(hit);
      if(fDisplayOn) {
        TMarker *mrk = new TMarker(hit->GetPosition().X(), hit->GetPosition().Y(), 25);
        mrk->SetMarkerColor(5);
        display->cd(1);
        mrk->Draw("SAME");
      }
    }
  }
  return cluster;

}

PndTrkCluster PndTrkLegendreTask::CreateClusterByMixedDistance(double fitm, double fitq) {
  
  PndTrkCluster cluster;
  double delta = conformalhitlist->GetConformalTransform()->GetRotation();
  // CHECK 
  // ADD HIT IN MIXED MODE REAL P. FOR MVD & CONFORMAL P. FOR STT,
  // WITH DISTANCE CRITERION
  // real: fabs(R - sqrt((xc - x)**2 + (yc - y)**2)) < distlim
  // cofn: cost x + sint y - r = 0 -> |cost x0 + sint y0 - r|

  // CHECK if this needs to be kept --> change xc0 to xc etc
  // center and radius
  Double_t xc0, yc0, R0;
  FromConformalToRealTrack(fitm, fitq, xc0, yc0, R0);
  // .........................................................
  if(fDisplayOn) {
    TArc *arc = new TArc(xc0, yc0, R0);
    arc->SetFillStyle(0);
    arc->SetLineColor(5);
    display->cd(1);
    arc->Draw("SAME");
  }
  for(int ihit = 0; ihit < mvdpixhitlist->GetNofHits(); ihit++) {
    PndTrkHit *hit = mvdpixhitlist->GetHit(ihit);
    double dist = hit->GetXYDistanceFromTrack(xc0, yc0, R0);
    // CHECK limits
    double distlimit = 0.5;
    if(dist < distlimit)  {
      cluster.AddHit(hit);
      if(fDisplayOn) {
        TMarker *mrk = new TMarker(hit->GetPosition().X(), hit->GetPosition().Y(), 21);
        mrk->SetMarkerColor(5);
        display->cd(1);
        mrk->Draw("SAME");
      }
    }
  }
  for(int ihit = 0; ihit < mvdstrhitlist->GetNofHits(); ihit++) {
    PndTrkHit *hit = mvdstrhitlist->GetHit(ihit);
    double dist = hit->GetXYDistanceFromTrack(xc0, yc0, R0);
    // CHECK limits
    double distlimit = 0.5;
    if(dist < distlimit){ 
      cluster.AddHit(hit);
      if(fDisplayOn) {
        TMarker *mrk = new TMarker(hit->GetPosition().X(), hit->GetPosition().Y(), 25);
        mrk->SetMarkerColor(5);
        display->cd(1);
        mrk->Draw("SAME");
      }
    }
  }

  for(int ihit = 0; ihit < conformalhitlist->GetNofHits(); ihit++) {
    PndTrkConformalHit *chit = conformalhitlist->GetHit(ihit);
    double dist = chit->GetDistanceFromTrack(fitm, fitq);
    
    int hitID = chit->GetHitID();
    int detID = chit->GetDetectorID();
    
    // CHECK limits
    if(detID != FairRootManager::Instance()->GetBranchId(fSttBranch)) continue;
    double distlimit = 0;
    if(chit->GetIsochrone() > 1.e-4) distlimit = 6 * chit->GetIsochrone();
    else distlimit = 0.001;
    // ---------------------------
      
    if(dist < distlimit) {
        
      //        cout << "ADDED " << hitID << " " << detID << " " << dist << " " << distlimit << endl;
        
      if(fDisplayOn) {
        TArc *arc = new TArc(chit->GetU(), chit->GetV(), chit->GetIsochrone());
        arc->SetFillStyle(0);
        arc->SetLineColor(5);
        display->cd(2);
        arc->Draw("SAME");
      }
        
      PndTrkHit *hit = stthitlist->GetHit(hitID);

      // ********** CHECK ***********
      // if you put "is used" the efficiency beecomes much lower!
      // DO NOT:        if(!hit->IsUsed())
      cluster.AddHit(hit);
    }
  }
  return cluster;
}


Bool_t PndTrkLegendreTask::DoesRealHitBelong(PndTrkHit *hit, double x0, double y0, double R) {
  double   dist = hit->GetXYDistanceFromTrack(x0, y0, R);
  int detID = hit->GetDetectorID();
  if(detID == FairRootManager::Instance()->GetBranchId(fMvdPixelBranch)) {
    if(dist < fMvdPix_RealDistLimit) return kTRUE;
  }
  else if(detID == FairRootManager::Instance()->GetBranchId(fMvdStripBranch)) {
    if(dist < fMvdStr_RealDistLimit) return kTRUE;
  }
  else if(detID == FairRootManager::Instance()->GetBranchId(fSttBranch)) {
    if(dist < fStt_RealDistLimit) return kTRUE;
  }
  return kFALSE;
}
  

Bool_t PndTrkLegendreTask::DoesConfHitBelong(PndTrkConformalHit *chit, double fitm, double fitp) {
  double dist = chit->GetDistanceFromTrack(fitm, fitp);
  int detID = chit->GetDetectorID();
  if(detID == FairRootManager::Instance()->GetBranchId(fMvdPixelBranch)) {
    if(dist < fMvdPix_ConfDistLimit) return kTRUE;
  }
  else if(detID == FairRootManager::Instance()->GetBranchId(fMvdStripBranch)) {
    if(dist < fMvdStr_ConfDistLimit) return kTRUE;
  }
  else if(detID == FairRootManager::Instance()->GetBranchId(fSttBranch)) {
    if(chit->GetIsochrone() > 1.e-4) {
      if(dist < (6 * chit->GetIsochrone())) return kTRUE; 
    } 
    else if(dist < fStt_ConfDistLimit) return kTRUE;
  }
  
 return kFALSE;
}

PndTrkCluster PndTrkLegendreTask::CreateClusterByDistance(Int_t mode, double fitm, double fitp) {
  PndTrkCluster cluster;

  // mode: -------------------------------------------
  // 0 all conformal: pix conf - str conf - stt conf
  // 1 all real:      pix real - str real - stt real
  // 2 mix:           pix real - str real - stt conf
  // --------------------------------------------------
  Double_t x0, y0, R;
  FromConformalToRealTrack(fitm, fitp, x0, y0, R);
 
  for(int ihit = 0; ihit < conformalhitlist->GetNofHits(); ihit++) {
    PndTrkConformalHit *chit = conformalhitlist->GetHit(ihit);
    PndTrkHit *hit = chit->GetHit();
    Bool_t accept = kFALSE;

    if(hit == fRefHit) accept = DoesRealHitBelong(hit, x0, y0, R);
    else {
      if(mode == 0) accept = DoesConfHitBelong(chit, fitm, fitp); 
      else if(mode == 1)  accept = DoesRealHitBelong(hit, x0, y0, R);
      else if(mode == 2) {
        if(hit->GetDetectorID() == FairRootManager::Instance()->GetBranchId(fSttBranch)) accept = DoesConfHitBelong(chit, fitm, fitp);
        else  accept = DoesRealHitBelong(hit, x0, y0, R);
      }
    }

    if(accept == kTRUE && fDisplayOn) {
      display->cd(1);
      if(hit->GetDetectorID() == FairRootManager::Instance()->GetBranchId(fSttBranch)) {
        TArc *arc = new TArc(hit->GetPosition().X(), hit->GetPosition().Y(), hit->GetIsochrone());
        arc->SetFillStyle(0);
        arc->SetLineColor(5);
        arc->Draw("SAME");
      }
      else {
        TMarker *mrk;
        if(hit->GetDetectorID() == FairRootManager::Instance()->GetBranchId(fMvdPixelBranch)) mrk = new TMarker(hit->GetPosition().X(), hit->GetPosition().Y(), 21);
        else if(hit->GetDetectorID() == FairRootManager::Instance()->GetBranchId(fMvdStripBranch)) mrk = new TMarker(hit->GetPosition().X(), hit->GetPosition().Y(), 25);
        mrk->SetMarkerColor(5);
        mrk->Draw("SAME");
      }
    }
    
      // ********** CHECK ***********
      // if you put "is used" the efficiency beecomes much lower!
      // DO NOT:        if(!hit->IsUsed())
      if(accept == kTRUE) {
        //    
        //      cout << "ADDING TO CLUSTER " << hit->GetHitID() << " " << hit->GetDetectorID() << endl;
        cluster.AddHit(hit);
      }
  }
  return cluster;
}

void PndTrkLegendreTask::FromConformalToRealTrack(double fitm, double fitp, double &x0, double &y0, double &R) {
 // CHECK if this needs to be kept --> change xc0 to xc etc
  // center and radius
  Double_t xcrot0, ycrot0;
  ycrot0 = 1 / (2 * fitp);
  xcrot0 = - fitm * ycrot0;
  R =  sqrt(xcrot0 * xcrot0 + ycrot0 * ycrot0);
  // re-rotation and re-traslation of xc and yc
  // rotation    
  x0 = TMath::Cos(conformalhitlist->GetConformalTransform()->GetRotation())*xcrot0 - TMath::Sin(conformalhitlist->GetConformalTransform()->GetRotation())*ycrot0;
  y0 = TMath::Sin(conformalhitlist->GetConformalTransform()->GetRotation())*xcrot0 + TMath::Cos(conformalhitlist->GetConformalTransform()->GetRotation())*ycrot0;
  // traslation 
  x0 += conformalhitlist->GetConformalTransform()->GetTranslation().X();
  y0 += conformalhitlist->GetConformalTransform()->GetTranslation().Y();
}

void PndTrkLegendreTask::DrawGeometry() {
  if(hxy == NULL)  hxy = new TH2F("hxy", "xy plane", 100, -43, 43, 100, -43, 43);
  else hxy->Reset();
  display->cd(1);
  hxy->Draw();
  display->Update();
  display->Modified();  
 
}

void PndTrkLegendreTask::DrawZGeometry(int whichone, double phimin, double phimax, double zmin, double zmax)
{
  if(hxz == NULL) hxz = new TH2F("hxz", "xz plane", 100, -43, 43, 100, -43, 113);
  else hxz->Reset();
  display->cd(3);
  hxz->Draw();
  if(whichone == 2) {
    if(hzphi == NULL) hzphi = new TH2F("hzphi", "z - phi plane", phimax - phimin, phimin, phimax, zmax - zmin, zmin, zmax);
    else {
      hzphi->Reset();
      hzphi->GetXaxis()->SetLimits(phimin, phimax);
      hzphi->GetYaxis()->SetLimits(zmin, zmax);
    }
    display->cd(4);
    hzphi->Draw();
  }
  display->Update();
  display->Modified();  

}

// ============================================================================================
// DISPLAY  ***********************************************************************************
// ============================================================================================

void PndTrkLegendreTask::Refresh(){
  // CHECK
  char goOnChar;
  //  cout << "Refresh?" << endl;
  //  cin >> goOnChar;
  //  cout << "REFRESHING" << endl;
  DrawGeometry();
  if(fVerbose)  cout << "Refresh stt" << endl;
  DrawHits(stthitlist);
  if(fVerbose)  cout << "Refresh pixel" << endl;
  DrawHits(mvdpixhitlist);
  if(fVerbose)  cout << "Refresh strip" << endl;
  DrawHits(mvdstrhitlist);
  if(fVerbose)  cout << "Refresh stop" << endl;

}

void PndTrkLegendreTask::DrawHits(PndTrkHitList *hitlist) {
  display->cd(1);
  hitlist->Draw();
  display->Update();
  display->Modified();
}
void PndTrkLegendreTask::DrawGeometryConf(double x1, double y1, double x2, double y2) {
  // CHECK
  char goOnChar;
  //  cout << "DRAWING GEOMETRY CONF" << endl;
  // cin >> goOnChar;
  
  // CHECK previous calculations, now not used;
  if(huv == NULL)  huv = new TH2F("huv", "uv plane", 100, x1, y1, 100, x2, y2);
  else {
    huv->Reset(); 
    huv->GetXaxis()->SetLimits(x1, y1);
    huv->GetYaxis()->SetLimits(x2, y2);
   }
  display->cd(2);
  huv->Draw();
  display->Update();
  display->Modified();  
 
}


void PndTrkLegendreTask::DrawLegendreHisto() {
  display->cd(3);
  legendre->Draw();
  display->Update();
  display->Modified();
}

void PndTrkLegendreTask::RefreshConf() { // CHECK delete
  // CHECK
  char goOnChar;
  //  cout << "RefreshConf?" << endl;
  //  cin >> goOnChar;
  //  cout << "REFRESHING CONF" << endl;
}

void PndTrkLegendreTask::RefreshZ() { // CHECK delete
  // CHECK
  char goOnChar;
  // cout << "RefreshZ?" << endl;
  //  cin >> goOnChar;
  // cout << "REFRESHING Z" << endl;
}


void PndTrkLegendreTask::DrawConfHit(double u, double v, double r, int marker) {
  display->cd(2);
  if(r >= 0) {
    TArc *arc = new TArc(u, v, r);
    arc->SetFillStyle(0);
    arc->Draw("SAME");
  }
  else {
    TMarker *mrk = new TMarker(u, v, marker);
    mrk->Draw("SAME");
  }
  //  display->Update();
  //   display->Modified();
}

PndTrkCluster PndTrkLegendreTask::CreateSttCluster(PndTrkHit *firsthit) {


  PndTrkCluster cluster;
  cluster.AddHit(firsthit);
  cluster.SetIRegion(firsthit->GetIRegion());

  // 2) LOOP  on all the hits
  for(int ihit = 0; ihit < stthitlist->GetNofHits(); ihit++) {
    PndTrkHit *hit = stthitlist->GetHit(ihit);  
    
    if(hit->IsUsed()) { 
      //  cout << "already used IV" << endl;
      continue; 
    }
    if(!hit->IsRegion(firsthit->GetIRegion())) continue;
    //    cout << ihit << " " << cluster.GetNofHits() << endl;
    // 3) LOOP (int hit3) on all the hits IN THE CLUSTER
    for(int jhit = 0; jhit < cluster.GetNofHits(); jhit++) {
      PndTrkHit *clushit = cluster.GetHit(jhit);        


      if(*hit == *clushit) {
        continue;
      }
      
      Bool_t success = IsSttAssociate(hit, clushit);
      //       cout << "success " << success << endl;
      if(!success) continue;

      cluster.AddHit(hit);
      break;
    }
  }

  if(cluster.GetNofHits() < 2) for(int ihit = 0; ihit < cluster.GetNofHits(); ihit++) cluster.DeleteAllHits();
  return cluster;
}

Bool_t PndTrkLegendreTask::IsSttAssociate(PndTrkHit *hit1, PndTrkHit *hit2) {

  double distance = hit1->GetXYDistance(hit2); // ComputeDistance(hit1, hit2);

  if(distance < STTPARALDISTANCE) {
    //  cout << "distance " << distance <<  endl;
    return kTRUE;
  }
  return kFALSE;

}

void PndTrkLegendreTask::LightCluster(PndTrkCluster *cluster) {
  
  //  cout << "LIGHT UP" << endl;
  Refresh();
  cluster->LightUp();
  display->Update();
  display->Modified();
}

// ================================== ZFINDER
PndTrkCluster PndTrkLegendreTask::CreateSkewHitList(PndTrkTrack *track) {

  double xc = track->GetCenter().X();
  double yc = track->GetCenter().Y();
  double R = track->GetRadius();
  PndTrkCluster skewhitlist;

    double phimin = 400, phimax = -1, zmin = 1000, zmax = -1;
    for(int ihit = 0; ihit < stthitlist->GetNofHits(); ihit++) {
      PndTrkHit *hit = stthitlist->GetHit(ihit);        
      //  if(hit->IsUsed()) { 
      //  cout << "already used IV" << endl;
      //  continue; 
      //  }
      if(hit->IsSttSkew() == kFALSE) continue;
   
      int tubeID = hit->GetTubeID();
      PndSttTube *tube = (PndSttTube*) fTubeArray->At(tubeID);
   
      TVector3 wireDirection = tube->GetWireDirection();
      Double_t halflength = tube->GetHalfLength();
      
      TVector3 first  = tube->GetPosition() + wireDirection * halflength; // CHECK
      TVector3 second = tube->GetPosition() - wireDirection * halflength; // CHECK
      //    if(fDisplayOn) {
      //        char goOnChar;
      //        display->cd(1);
      //        TLine *l = new TLine(first.X(), first.Y(), second.X(), second.Y());
      //        l->SetLineColor(kBlue);
      //        l->Draw("SAME");
      //        display->Update();
      //        display->Modified();
      //        cin >> goOnChar;   
      //       }
first.Print();
second.Print();
      double m1 = (first - second).Y()/(first - second).X();
      double q1 = first.Y() - m1 * first.X();
      
      // 1. compute intersection between the track circle and the wire
      TVector2 intersection1, intersection2;
      Int_t nofintersections = tools->ComputeSegmentCircleIntersection(TVector2(first.X(), first.Y()), TVector2(second.X(), second.Y()), xc, yc, R, intersection1, intersection2);
      
      if(nofintersections == 0) continue;
      if(nofintersections >= 2) {
        cout << "ERROR: MORE THAN 1 INTERSECTION!!" << endl;
        continue; // CHECK
      }
    
      if(fDisplayOn) {
        char goOnChar;
        display->cd(1);
        TLine *l = new TLine(first.X(), first.Y(), second.X(), second.Y());
        l->SetLineColor(kBlue);
        l->Draw("SAME");

        TMarker *mrk = new TMarker(intersection1.X(), intersection1.Y(), 20);
        mrk->SetMarkerColor(kBlue);
        mrk->Draw("SAME");

        display->Update();
        display->Modified();


        //      cin >> goOnChar;   
      }
  

      // 2. find the tangent to the track in the intersection point
      // tangent approximation
      TVector2 tangent = tools->ComputeTangentInPoint(xc, yc, intersection1);
       
      // 3. rotate clockwise the tangent/point/(wire, not explicitely)
      // in order to have the wire parallel to the x axis;
      // then translate everything to have the wire ON the x axis
      double beta = wireDirection.Phi();
      if(beta < 0) beta += TMath::Pi();
      // ... rotate the tangent
      double rtx = TMath::Cos(beta) * tangent.X() + TMath::Sin(beta) * tangent.Y();
      double rty = TMath::Cos(beta) * tangent.Y() - TMath::Sin(beta) * tangent.X();
      TVector2 rottangent(rtx, rty);
      rottangent = rottangent.Unit();
      // ... rotate the point
      double rx = TMath::Cos(beta) * intersection1.X() + TMath::Sin(beta) * intersection1.Y();
      double ry = TMath::Cos(beta) * intersection1.Y() - TMath::Sin(beta) * intersection1.X();
       
      // translation
      Double_t deltay = ry;
      rty -= deltay;
      ry -= deltay;

      // rotm, rotp
      Double_t rotm = rottangent.Y()/rottangent.X();
      Double_t rotp = ry - rotm * rx;

      // ellipsis
      double a = hit->GetIsochrone() * TMath::Cos(SKEW_ANGLE); // CHECK skew angle hard coded
      double b = hit->GetIsochrone();

      // center of ellipsis
      Double_t x0a, x0b, y0;
      y0 = 0.;
      x0a = (-rotp + TMath::Sqrt(b * b + a * a * rotm * rotm)) / rotm;
      x0b = (-rotp - TMath::Sqrt(b * b + a * a * rotm * rotm)) / rotm;
 
      // intersection point
      double intxa = (x0a * b * b - rotm * rotp * a * a) / (b * b + rotm * rotm * a * a);
      double intya = rotm * intxa + rotp;
      double intxb = (x0b * b * b - rotm * rotp * a * a) / (b * b + rotm * rotm * a * a);
      double intyb = rotm * intxb + rotp;
   
      // 4. retraslate/rerotate all back to the original plane
      // retranslate
      y0 += deltay; 
      intya  += deltay; 
      intyb  += deltay; 
   
      // rerotate
      double x0anew = TMath::Cos(beta) * x0a - TMath::Sin(beta) * y0;
      double y0anew = TMath::Cos(beta) * y0 + TMath::Sin(beta) * x0a;
      double x0bnew = TMath::Cos(beta) * x0b - TMath::Sin(beta) * y0;
      double y0bnew = TMath::Cos(beta) * y0 + TMath::Sin(beta) * x0b;
   
      double intxanew = TMath::Cos(beta) * intxa - TMath::Sin(beta) * intya;
      double intyanew = TMath::Cos(beta) * intya + TMath::Sin(beta) * intxa;
      double intxbnew = TMath::Cos(beta) * intxb - TMath::Sin(beta) * intyb;
      double intybnew = TMath::Cos(beta) * intyb + TMath::Sin(beta) * intxb;
   
      intxa = intxanew;
      intya = intyanew;
      intxb = intxbnew;
      intyb = intybnew;

      // now we have x0a, y0a, center of the 1st ellipse
      // and x0b, y0b, center of the 2nd ellipse
      x0a = x0anew;
      double y0a = y0anew;
      x0b = x0bnew;
      double y0b = y0bnew;

      if(fDisplayOn) {
        char goOnChar;
        display->cd(1);

        TEllipse *ell1 = new TEllipse(x0a, y0a, a, b, 0, 360, -beta);
        ell1->SetFillStyle(0);
        ell1->SetLineColor(4);
        ell1->Draw("SAME");
        TEllipse *ell2 = new TEllipse(x0b, y0b, a, b, 0, 360, -beta);
        ell2->SetFillStyle(0);
        ell2->SetLineColor(6);
        ell2->Draw("SAME");

        TMarker *mrkinta = new TMarker(intxa, intya, 20);
        mrkinta->SetMarkerColor(4);
        mrkinta->Draw("SAME");
        TMarker *mrkintb = new TMarker(intxb, intyb, 20);
        mrkintb->SetMarkerColor(6);
        mrkintb->Draw("SAME");
        //       cin >> goOnChar;   
      }

      // 5. calculate z coordinate for each intersection
       
      // calculate z0a, z0b of the center of the ellipse
      Double_t t = ((x0a + y0a) - (first.X() + first.Y())) /  ((second.X() - first.X()) + (second.Y() - first.Y()));
      Double_t z0a = first.Z() + (second.Z() - first.Z()) * t;
      //    cout << "0 : calculate t, z0a " << t << " " << z0a << endl;
 
      t = ((x0b + y0b) - (first.X() + first.Y())) /  ((second.X() - first.X()) + (second.Y() - first.Y()));
      Double_t z0b = first.Z() + (second.Z() - first.Z()) * t;
 
      TVector3 center1(x0a, y0a, z0a);
      TVector3 center2(x0b, y0b, z0b);
      if(fDisplayOn) {
        char goOnChar;
        display->cd(3);
        //      cout << "COMPUTE Z COORDINATE" << endl;
        RefreshZ();
        DrawZGeometry();
        TLine *linezx = new TLine(first.X(), first.Z(), second.X(), second.Z());
        linezx->Draw("SAME");
        TMarker *mrkza = new TMarker(x0a, z0a, 20);
        mrkza->SetMarkerColor(4);
        mrkza->Draw("SAME");
        TMarker *mrkzb = new TMarker(x0b, z0b, 20);
        mrkzb->SetMarkerColor(6);
        mrkzb->Draw("SAME");
        //       cin >> goOnChar;   
      }

      // calculate the z of the intersection ON the ellipse (CHECK this step calculations!)
      double dx = intxa - x0a;
      double dy = intya - y0a;
      TVector3 dxdy(dx, dy, 0.0);

      TVector3 tfirst = first + dxdy;
      TVector3 tsecond = second + dxdy;

      t = ((intxa + intya) - (tfirst.X() + tfirst.Y())) /  ((tsecond.X() - tfirst.X()) + (tsecond.Y() - tfirst.Y()));
      double intza = tfirst.Z() + (tsecond.Z() - tfirst.Z()) * t;
      if(fDisplayOn) {
        char goOnChar;
        display->cd(3);
        TLine *linezx1 = new TLine(tfirst.X(), tfirst.Z(), tsecond.X(), tsecond.Z());
        linezx1->SetLineStyle(1);
        linezx1->Draw("SAME");
        TMarker *mrkza1 = new TMarker(intxa, intza, 20);
        mrkza1->SetMarkerColor(kBlue - 9);
        mrkza1->Draw("SAME");
        // cin >> goOnChar;   
      }

      tfirst = first - dxdy;
      tsecond = second - dxdy;

      t = ((intxb + intyb) - (tfirst.X() + tfirst.Y())) /  ((tsecond.X() - tfirst.X()) + (tsecond.Y() - tfirst.Y()));
      double intzb = tfirst.Z() + (tsecond.Z() - tfirst.Z()) * t;
 
      TVector3 fin_intersection1(intxa, intya, intza);
      TVector3 fin_intersection2(intxb, intyb, intzb);

      if(fDisplayOn) {
        char goOnChar;
        display->cd(3);
        TLine *linezx2 = new TLine(tfirst.X(), tfirst.Z(), tsecond.X(), tsecond.Z());
        linezx2->SetLineStyle(1);
        linezx2->Draw("SAME");
        TMarker *mrkzb1 = new TMarker(intxb, intzb, 20);
        mrkzb1->SetMarkerColor(kMagenta - 7);
        mrkzb1->Draw("SAME");
        //       cin >> goOnChar;   
      }
      //        int1.SetXYZ(intxa, intya, intza);
      //        int2.SetXYZ(intxb, intyb, intzb);
      //        errz = fabs(intza - intzb)/2.   ;
   
      // CHECK to be changed
      int trackID = 1;
      double phi1 = track->ComputePhi(fin_intersection1);
      double phi2 = track->ComputePhi(fin_intersection2);

      PndTrkSkewHit *skewhit = new PndTrkSkewHit(*hit, trackID, center1, fin_intersection1, phi1, center2, fin_intersection2, phi2, a, b, -1, beta);
      //      skewhit->Print();
      skewhitlist.AddHit(skewhit);

   
    }
    //     cout << "PHI: " << phimin << " " << phimax << endl;
    //     cout << "Z  : " << zmin << " " << zmax << endl;


    return skewhitlist;
}
 
PndTrkCluster PndTrkLegendreTask::CleanUpSkewHitList(PndTrkCluster *skewhitlist) {


  // 6. FIRST CLEANING of the track skewed associations
  // find most probable z - CHECK what happens if a track is very fwd peaked?
  //                        TRY WITH DELTA z = COST
  //    cout  << "##################### FIND MOST PROBABLE Z" << endl; 
  double phimin = 400, phimax = -1, zmin = 1000, zmax = -1;

  for(int ihit = 0; ihit < skewhitlist->GetNofHits(); ihit++) {
    PndTrkSkewHit *skewhit = (PndTrkSkewHit*) skewhitlist->GetHit(ihit);
    if(!skewhit) continue;
    TVector3 fin_intersection1 = skewhit->GetIntersection1();
    TVector3 fin_intersection2 = skewhit->GetIntersection2();

    double phi1 = skewhit->GetPhi1();
    double phi2 = skewhit->GetPhi2();

    if(phi1 < phimin) phimin = phi1;
    if(phi2 < phimin) phimin = phi2;
    if(fin_intersection1.Z() < zmin) zmin = fin_intersection1.Z();
    if(fin_intersection2.Z() < zmin) zmin = fin_intersection2.Z();
      
    if(phi1 > phimax) phimax = phi1;
    if(phi2 > phimax) phimax = phi2;
    if(fin_intersection1.Z() > zmax) zmax = fin_intersection1.Z();
    if(fin_intersection2.Z() > zmax) zmax = fin_intersection2.Z();
    // ~~~~~~~~~~~~~~~~~~~~~~~~~~~ CHECK
  }
  //     cout << "PHI: " << phimin << " " << phimax << endl;
  //     cout << "Z  : " << zmin << " " << zmax << endl;

  // DISPLAY ----------------------------
  if(fDisplayOn) {
    char goOnChar;
    display->cd(4);
    phimin -= 30;
    phimax += 30;
    zmin -= 13;
    zmax += 13;
    DrawZGeometry(2, phimin, phimax, zmin, zmax);
    hzphi->SetXTitle("#phi");
    hzphi->SetYTitle("#z");
    hzphi->Draw();
    display->Update();
    display->Modified();  
   
     
    for(int ihit = 0; ihit < skewhitlist->GetNofHits(); ihit++) {
      PndTrkSkewHit *skewhit = (PndTrkSkewHit*) skewhitlist->GetHit(ihit);
      if(!skewhit) continue;

      TVector3 fin_intersection1 = skewhit->GetIntersection1();
      TVector3 fin_intersection2 = skewhit->GetIntersection2();

      double phi1 = skewhit->GetPhi1();
      double phi2 = skewhit->GetPhi2();

      TLine *linezphi = new TLine(phi1, fin_intersection1.Z(), phi2, fin_intersection2.Z());
      // TLine *linezphi = new TLine(fin_intersection1.Z(), phi1, fin_intersection2.Z(), phi2);
      linezphi->SetLineStyle(1);
      linezphi->Draw("SAME");

      TMarker *mrkzphi1 = new TMarker(phi1, fin_intersection1.Z(), 20);
      // TMarker *mrkzphi1 = new TMarker(fin_intersection1.Z(), phi1, 20);

      mrkzphi1->SetMarkerColor(kBlue - 9);
      mrkzphi1->Draw("SAME");
        
      TMarker *mrkzphi2 = new TMarker(phi2, fin_intersection2.Z(), 20);
      //         TMarker *mrkzphi2 = new TMarker(fin_intersection2.Z(), phi2, 20);
      mrkzphi2->SetMarkerColor(kMagenta - 7);
      mrkzphi2->Draw("SAME");
    }
    display->Update();
    display->Modified();  
    //      cin >> goOnChar;   
  }
  // DISPLAY ----------------------------

  TH1F hz("hz", "z",  (zmax - zmin)/10., zmin, zmax); // CHECK
  for(int ihit = 0; ihit < skewhitlist->GetNofHits(); ihit++) {
    PndTrkSkewHit *skewhit = (PndTrkSkewHit*) skewhitlist->GetHit(ihit);
    if(!skewhit) continue;
    TVector3 fin_intersection1 = skewhit->GetIntersection1();
    TVector3 fin_intersection2 = skewhit->GetIntersection2();
    hz.Fill(fin_intersection1.Z());
    hz.Fill(fin_intersection2.Z());
  }
  int maxbinz = hz.GetMaximumBin();
  double mostprobZ = hz.GetBinCenter(maxbinz);
  //    cout << mostprobZ << endl;
    
  // delete hits too far away ..........   
  //    cout  << "##################### DELETE HITS" << endl; 
  PndTrkCluster tmpskewhitlist;
  for(int ihit = 0; ihit < skewhitlist->GetNofHits(); ihit++) {
    PndTrkSkewHit *skewhit = (PndTrkSkewHit*) skewhitlist->GetHit(ihit);
    if(!skewhit) continue;
    TVector3 fin_intersection1 = skewhit->GetIntersection1();
    TVector3 fin_intersection2 = skewhit->GetIntersection2();
    double phi1 = skewhit->GetPhi1();
    double phi2 = skewhit->GetPhi2();
      
    if(fabs(fin_intersection1.Z() - mostprobZ) > 30. && fabs(fin_intersection2.Z() - mostprobZ) > 30.) {
      //        cout << "THROW AWAY " << ihit << " " << fabs(fin_intersection1.Z() - mostprobZ) << "  " << fabs(fin_intersection2.Z() - mostprobZ) << endl;
      continue;
    }
    skewhit->SetSortVariable((phi1 + phi2)/2.);
    tmpskewhitlist.AddHit(skewhit);
  }
  tmpskewhitlist.Sort();
  return tmpskewhitlist;
}



void PndTrkLegendreTask::RegisterTrack(PndTrkTrack *track) {
  PndTrack *finaltrack = track->ConvertToPndTrack();

  TClonesArray& clref1 = *fTrackArray;
  Int_t size = clref1.GetEntriesFast();
  PndTrack *outputtrack = new(clref1[size]) PndTrack(finaltrack->GetParamFirst(), finaltrack->GetParamLast(), finaltrack->GetTrackCand());

  TClonesArray& clref2 = *fTrackCandArray;
  size = clref2.GetEntriesFast();
  PndTrackCand *outputtrackcand = new(clref2[size]) PndTrackCand(finaltrack->GetTrackCand());

  if(fVerbose > 1) {
    cout << "MOM FIRST: TOT, PT, PL " << outputtrack->GetParamFirst().GetMomentum().Mag() << " " << outputtrack->GetParamFirst().GetMomentum().Perp() << " " << outputtrack->GetParamFirst().GetMomentum().Z() << endl;
    cout << "MOM LAST: TOT, PT, PL " << outputtrack->GetParamLast().GetMomentum().Mag() << " " << outputtrack->GetParamLast().GetMomentum().Perp() << " " << outputtrack->GetParamLast().GetMomentum().Z() << endl;
  }
    if(fDisplayOn) {
    char goOnChar;
    display->cd(1);
    Refresh();
 
    track->Draw(kRed);
    display->Update();
    display->Modified();

    cout << "X, Y, R " << track->GetCenter().X() << " " << track->GetCenter().Y() << " " << track->GetRadius() << endl;
    cout << "Z0, TANL " << track->GetZ0() << " " << track->GetTanL() << endl;
    cout << "CHARGE " <<  track->GetCharge() << endl;
    //      cin >> goOnChar;
  }

}


// ----------------------- FOR SECONDARIES
PndTrkHit *PndTrkLegendreTask::FindSttReferenceHit()
{

  if(stthitlist->GetNofHits() == 0) return NULL;
  // loop on stt hits
  int tmphitid = -1;
  Double_t tmpiso = 1.;
  for(int jhit = 0; jhit < stthitlist->GetNofHits(); jhit++) {
    PndTrkHit *hit = stthitlist->GetHit(jhit);
    if(hit->IsUsed()) { 
      if(fVerbose > 1) cout << "STT hit " << jhit << "already used " << endl; 
      continue; }
    if(hit->IsSttSkew()) continue;
    if(hit->GetIsochrone() < tmpiso) {
      tmphitid = jhit;
      tmpiso = hit->GetIsochrone();
    }
  }   
  if(tmphitid == -1)  return NULL;

  PndTrkHit *refhit = stthitlist->GetHit(tmphitid);
  if(fVerbose > 1) cout << "STT REFERENCE HIT " <<  tmphitid << " " << refhit->GetIsochrone() << endl;
  return refhit;
}

PndTrkHit *PndTrkLegendreTask::FindMvdPixelReferenceHit()
{
  if(mvdpixhitlist->GetNofHits() == 0) return NULL;
  // loop on mvd pix hits
  int tmphitid = -1;
  PndTrkHit *refhit = NULL;
  for(int jhit = 0; jhit < mvdpixhitlist->GetNofHits(); jhit++) {
    PndTrkHit *hit = mvdpixhitlist->GetHit(jhit);
    if(hit->IsUsed()) { 
      if(fVerbose > 1) cout << "already used V" << endl; 
      continue; 
    }
    tmphitid = jhit;
    break;
  }   
  if(tmphitid == -1)  return NULL;
  refhit = mvdpixhitlist->GetHit(tmphitid);
   if(fVerbose > 1) cout << "MVD PIXEL REFERENCE HIT " << refhit->GetHitID() << endl;
  return refhit;
}

PndTrkHit *PndTrkLegendreTask::FindMvdStripReferenceHit()
{
  if(mvdstrhitlist->GetNofHits() == 0) return NULL;
  // loop on mvd str hits
  int tmphitid = -1;
  PndTrkHit *refhit = NULL;
  for(int jhit = 0; jhit < mvdstrhitlist->GetNofHits(); jhit++) {
    PndTrkHit *hit = mvdstrhitlist->GetHit(jhit);
    if(hit->IsUsed()) { 
       if(fVerbose > 1) cout << "already used V" << endl; 
      continue; 
    }
    tmphitid = jhit;
    break;
  }   
  if(tmphitid == -1)  return NULL;
  refhit = mvdstrhitlist->GetHit(tmphitid);
   if(fVerbose > 1) cout << "MVD STRIP REFERENCE HIT " << refhit->GetHitID() << endl;
  return refhit;
}

PndTrkHit *PndTrkLegendreTask::FindMvdReferenceHit()
{
  PndTrkHit *refhit = NULL;
  refhit = FindMvdStripReferenceHit();
  // refhit = FindMvdPixelReferenceHit();
  if(refhit != NULL) return refhit;
  //  refhit = FindMvdStripReferenceHit();
  FindMvdPixelReferenceHit();
  return refhit;
}

PndTrkHit *PndTrkLegendreTask::FindReferenceHit()
{
  PndTrkHit *refhit = NULL;
  // refhit = FindMvdReferenceHit();
  refhit = FindSttReferenceHit();
  if(refhit != NULL) return refhit;
  // refhit = FindSttReferenceHit();
  FindMvdReferenceHit();

 return refhit;
}

void PndTrkLegendreTask::ComputeTraAndRot(PndTrkHit *hit, Double_t &delta, Double_t trasl[2]) {
  
  trasl[0] = hit->GetPosition().X();
  trasl[1] = hit->GetPosition().Y();

  delta = 0.; // TMath::ATan2(hit->GetPosition().Y() - 0., hit->GetPosition().X() - 0.); // CHECK 
  
}


void PndTrkLegendreTask::RePrepareLegendre(PndTrkCluster *cluster) {

  //    cout << "RESETTING LEGENDRE HISTO" << endl;
  legendre->ResetLegendreHisto();
 
  if(fDisplayOn) {
    RefreshConf();
    if(fSecondary) DrawGeometryConf(-1., 1., -1., 1.);
    else   DrawGeometryConf(-0.07, 0.07, -0.07, 0.07);
  }
  // cout << "%%%%%%%%%%%%%%%%%%%% XY FINDE %%%%%%%%%%%%%%%%%%%%%%%%%%" << endl;
  FillLegendreHisto(cluster);
}

void PndTrkLegendreTask::PrepareLegendre() {

  //    cout << "RESETTING LEGENDRE HISTO" << endl;
  legendre->ResetLegendreHisto();
 
  if(fDisplayOn) {
    RefreshConf();
    if(fSecondary) DrawGeometryConf(-1., 1., -1., 1.);
    else   DrawGeometryConf(-0.07, 0.07, -0.07, 0.07);
  }
  if(fVerbose > 1) cout << "%%%%%%%%%%%%%%%%%%%% XY FINDER %%%%%%%%%%%%%%%%%%%%%%%%%%" << endl;
  FillLegendreHisto(0);
}

Int_t PndTrkLegendreTask::ApplyLegendre(double &theta_max, double &r_max) {
  PrepareLegendre();
  return ExtractLegendre(0, theta_max, r_max);
}

Int_t PndTrkLegendreTask::ApplyLegendre(PndTrkCluster *cluster, double &theta_max, double &r_max) {
  RePrepareLegendre(cluster);
  return ExtractLegendre(1, theta_max, r_max);
}



Int_t  PndTrkLegendreTask::ExtractLegendre(Int_t mode, double &theta_max, double &r_max) {
  if(fDisplayOn) {
    char goOnChar;
    //      cin >> goOnChar;
    DrawLegendreHisto();
    //    cout << "LEGENDRE (nof conf hits = " <<  conformalhitlist->GetNofHits() << ")" << endl;
    display->cd();
    //      cin >> goOnChar;
    display->Update();
    display->Modified();
  }
  
  // FIND MAXIMUM IN LEGENDRE HISTO

  //  legendre->ApplyThresholdLegendreHisto(0.3);
  int  maxpeak = legendre->ExtractLegendreMaximum(theta_max, r_max);

  bool alreadythere = false;
  if(mode == 0) {

    if(maxpeak <= 3) {
      if(fVerbose > 1) cout << "MAXPEAK " << maxpeak <<  ", BREAK NOW! "  << endl;
      return maxpeak;
    }
  
    for(int ialready = 0; ialready < fFoundPeaks.size(); ialready++) {
      std::pair<double, double> foundthetar = fFoundPeaks.at(ialready);
      double foundtheta = foundthetar.first;
      double foundr = foundthetar.second;
      // IF THIS PEAK WAS ALREADY FOUND, DELETE THE PEAK AND GO ON (TO AVOID INFINITE LOOPS)
      if(theta_max == foundtheta && r_max == foundr) {
        legendre->DeleteZoneAroundXYLegendre(theta_max, r_max);
        maxpeak = legendre->ExtractLegendreMaximum(theta_max, r_max);
        alreadythere = true;
        if(fVerbose > 0) cout << "OH NO! THIS PEAK IS ALREADY THERE" << endl;
        return -1;
      }
    }
    
    if(alreadythere == false) {
      std::pair<double, double> tr(theta_max, r_max);
      fFoundPeaks.push_back(tr);
    }
  }
  // ZOOM LEGENDRE HISTO
  legendre->SetUpZoomHisto(theta_max, r_max, 3, 0.005);
  //   cout << "THETA/R " << theta_max << " " << r_max << " maxpeak " << maxpeak << endl;

  for(int ihit = 0; ihit < conformalhitlist->GetNofHits(); ihit++) {
    PndTrkConformalHit *chit = conformalhitlist->GetHit(ihit);
    legendre->FillZoomHisto(chit->GetU(), chit->GetV(), chit->GetIsochrone());
  }

  if(mode == 0 && alreadythere == true) {
     cout << "THIS PEAK IS ALREADY THERE" << endl;
    legendre->DeleteZoneAroundXYZoom(theta_max, r_max);
  }

 int maxpeakzoom = legendre->ExtractZoomMaximum(theta_max, r_max);
  //  cout << "THETA/R ZOOM " << theta_max << " " << r_max <<  " maxpeakzoom " << maxpeakzoom << endl;

  if(fDisplayOn) {
    char goOnChar;
    display->cd(3);
    TMarker *mrk = new TMarker(theta_max, r_max, 29);
    mrk->Draw("SAME");
    display->cd(4);
    legendre->DrawZoom();
    mrk->Draw("SAME");
    display->Update();
    display->Modified();
    //      cin >> goOnChar;
  }
  
  return maxpeak;
}


PndTrkCluster PndTrkLegendreTask::Cleanup(PndTrkCluster cluster) {
  
  // CLEANUP
  // -------------------------------------------------------
  //    CleanCluster(cluster);
  // cluster->SortMvdPixelByLayerID();
  //    cluster->SortByLayerID();
  PndTrkClean clean;
  int leftvotes = 0;
  int rightvotes = 0;

  TVector2 trasl = conform->GetTranslation();
  Double_t angle = conform->GetRotation();

  // SET SORT VARIABLE 
  // MVD PIX PART OF CLUSTER
  PndTrkCluster mvdpixcluster = cluster.GetMvdPixelHitList();
  for(int ihit = 0; ihit < mvdpixcluster.GetNofHits(); ihit++) {
    PndTrkHit *hit = mvdpixcluster.GetHit(ihit);
    if(fSecondary) hit->SetSortVariable(hit->GetDistance(TVector3(trasl.X(), trasl.Y(), angle)));
    else hit->SetSortVariable(hit->GetDistance(TVector3(0., 0., 0)));
    int sensorID = hit->GetSensorID();
    int layerID = clean.FindMvdLayer(sensorID);
    if(layerID%2 == 0) rightvotes++;
    else leftvotes++;
  }
  // MVD STR PART OF CLUSTER
  PndTrkCluster mvdstrcluster = cluster.GetMvdStripHitList();
  for(int ihit = 0; ihit < mvdstrcluster.GetNofHits(); ihit++) {
    PndTrkHit *hit = mvdstrcluster.GetHit(ihit);
    if(fSecondary) hit->SetSortVariable(hit->GetDistance(TVector3(trasl.X(), trasl.Y(), angle)));
    else hit->SetSortVariable(hit->GetDistance(TVector3(0., 0., 0)));
    int sensorID = hit->GetSensorID();
    int layerID = clean.FindMvdLayer(sensorID);
    if(layerID%2 == 0) rightvotes++;
    else leftvotes++;
  }
  // STT PARALLEL PART OF CLUSTER
  PndTrkCluster sttcluster = cluster.GetSttParallelHitList();
  for(int ihit = 0; ihit < sttcluster.GetNofHits(); ihit++) {
    PndTrkHit *hit = sttcluster.GetHit(ihit);
    int layerID = -999, tubeID = hit->GetTubeID(), sectorID = -999;
    PndSttTube *tube = (PndSttTube*) fTubeArray->At(tubeID);
    layerID = tube->GetLayerID();
    sectorID = tube->GetSectorID();
    if(fSecondary) hit->SetSortVariable(hit->GetDistance(TVector3(trasl.X(), trasl.Y(), angle)));
    else hit->SetSortVariable(1000+layerID);
    if(sectorID < 3) rightvotes++;
    else leftvotes++;
  }

  // cout << "VOTES: LEFT " << leftvotes << " RIGHT " << rightvotes << endl;
  for(int ihit = 0; ihit < cluster.GetNofHits(); ihit++) {
    PndTrkHit *hit = cluster.GetHit(ihit);
    //  cout << "for sorting " << hit->GetDetectorID () << " " << hit->GetHitID() << " " << hit->GetSortVariable() << endl;
  }

  cluster.Sort();
  for(int ihit = 0; ihit < cluster.GetNofHits(); ihit++) {
    PndTrkHit *hit = cluster.GetHit(ihit);
    // cout << "sorted " << hit->GetDetectorID () << " " << hit->GetHitID() << " " << hit->GetSortVariable() << endl;
  }

  //    cluster.Print();

  // DELETE HITS
  PndTrkCluster deletioncluster;

  // LEFT/RIGHT CRITERION ------------------------------------------ NOW OFF
  // PIXEL
  mvdpixcluster = cluster.GetMvdPixelHitList();
  for(int ihit = 0; ihit < mvdpixcluster.GetNofHits(); ihit++) {
    PndTrkHit *hit = mvdpixcluster.GetHit(ihit);
    int sensorID = hit->GetSensorID();
    int layerID = clean.FindMvdLayer(sensorID);
    //  cout << "pixel sorted " <<  hit->GetHitID() << " " << hit->GetSortVariable() << " " << sensorID << " " << layerID << endl;


    if((leftvotes > rightvotes) && (layerID%2 == 0)) {
      // deletioncluster.AddHit(hit);
      // cout << "pixel DELETED " <<  hit->GetHitID() << " " << ihit << " " << mvdpixcluster.GetNofHits() << endl;
    }
    else  if((leftvotes < rightvotes) && (layerID%2 != 0)) {
      // deletioncluster.AddHit(hit);
      // cout << "pixel DELETED " << hit->GetHitID() << " " << ihit << " " << mvdpixcluster.GetNofHits() << endl;
    }

    if(fDisplayOn) {
      char goOnChar;
      display->cd(1);
      hit->Draw(kGreen);
      display->Update();
      display->Modified();
      cout << "WAITING" << endl;
      cin >> goOnChar;  
    }
  }
  // STRIP
  mvdstrcluster = cluster.GetMvdStripHitList();
  for(int ihit = 0; ihit < mvdstrcluster.GetNofHits(); ihit++) {
    PndTrkHit *hit = mvdstrcluster.GetHit(ihit);
    int sensorID = hit->GetSensorID();
    //  cout << "strip sorted " <<  hit->GetHitID() << " " << hit->GetSortVariable() << " " << sensorID << " " << clean.FindMvdLayer(sensorID) << endl;
    int layerID = clean.FindMvdLayer(sensorID);
    if((leftvotes > rightvotes) && (layerID%2 == 0)) {
      //        deletioncluster.AddHit(hit);
      //        cout << "strip DELETED " <<  hit->GetHitID() << endl;
    }
    else  if((leftvotes < rightvotes) && (layerID%2 != 0)) {
      //        deletioncluster.AddHit(hit);
      //        cout << "strip DELETED " <<  hit->GetHitID() << endl;
    }
    if(fDisplayOn) {
      char goOnChar;
      display->cd(1);
      hit->Draw(kBlue);
      display->Update();
      display->Modified();
      //        cin >> goOnChar;        
    } 
  }

  // STT PARALLEL
  sttcluster = cluster.GetSttParallelHitList();
  for(int ihit = 0; ihit < sttcluster.GetNofHits(); ihit++) {
    PndTrkHit *hit = sttcluster.GetHit(ihit);
    int layerID = -999, tubeID = hit->GetTubeID();
    PndSttTube *tube = (PndSttTube*) fTubeArray->At(tubeID);
    layerID = tube->GetLayerID();
    //      cout << "stt sorted " <<  hit->GetHitID() << " " << hit->GetSortVariable() << " " << tubeID << " " << layerID << endl;

    int  sectorID = tube->GetSectorID();

    if((leftvotes > rightvotes) &&  (sectorID < 3)) {
      //                deletioncluster.AddHit(hit);
      //        cout << "stt DELETED " <<  hit->GetHitID() << " " << sectorID << endl;
    }
    else if((leftvotes < rightvotes) && (sectorID >= 3)) {
      //                deletioncluster.AddHit(hit);
      //        cout << "stt DELETED " <<  hit->GetHitID() << " " << sectorID << endl;
    }
    // ~~~~~~~~~~~~~~~~~~ check *******************
    else { // NEIGHBORING CRITERION ------------------------------------------------ 
      TArrayI neighboring = tube->GetNeighborings();
      //        cout << "NEIGHBORING TO TUBE " << tubeID << endl;
      //        for(int nhit = 0; nhit < neighboring.GetSize(); nhit++) {
      //          cout << " " << neighboring.At(nhit);
      //        }
      //        cout << endl;

      int neighboringcounter = 0;
      for(int ohit = 0; ohit < sttcluster.GetNofHits(); ohit++) {
        if(ihit == ohit) continue;       
        PndTrkHit *otherhit = sttcluster.GetHit(ohit);
        int otherhitID = otherhit->GetTubeID();
          
        for(int nhit = 0; nhit < neighboring.GetSize(); nhit++) {
          if(otherhitID == neighboring.At(nhit)) neighboringcounter++;
        }

        //      if(isthere == false) 
        //        {
        //          deletioncluster.AddHit(hit);
        //          cout << "stt DELETEDbis " <<  hit->GetHitID() << " " << sectorID << endl;
        //        }
      }
      //        cout << "hit " << ihit << " has " << neighboringcounter << " NEIGHBORINS" << endl;
        
      if(layerID != 0 && layerID != 7 && layerID != 8 && ihit != sttcluster.GetNofHits() - 1 && neighboringcounter == 1) {
        //        cout << "I SHOULD DELETE THIS" << endl;
        //  deletioncluster.AddHit(hit);
        // OFF FOR NOW: delete if there is just 1 neighboring
        //        cout << "stt DELETED " <<  hit->GetHitID() << " " << sectorID << endl;         
      }
        
      // ON, delete if there is no neighboring (isolated hit)
      if(neighboringcounter == 0) {
        //        cout << "I WILL DELETE THIS" << endl;
        deletioncluster.AddHit(hit);
        //        cout << "stt DELETED " <<  hit->GetHitID() << " " << sectorID << endl;
      }
        
      if(fDisplayOn) {
        char goOnChar;
        display->cd(1);
        hit->Draw(kCyan);
        display->Update();
        display->Modified();
        //        cin >> goOnChar;      
      } 
    }
  }

  if(fDisplayOn) {
    char goOnChar;
    //      cin >> goOnChar;    
  } 
  // ~~~~~~~~~~~~~~~~~~



  // ACTUAL DELETION OF HITS

  PndTrkCluster finalcluster;
  for(int ihit = 0; ihit < cluster.GetNofHits(); ihit++) {
    PndTrkHit *hit = cluster.GetHit(ihit);
    bool stop = false;
    for(int jhit = 0; jhit < deletioncluster.GetNofHits(); jhit++) {
      PndTrkHit *dhit = deletioncluster.GetHit(jhit);
      if(hit == dhit) {
        // ********** CHECK ***********
        //        cout << "trhrow in again" << endl;
        //        dhit->SetUsedFlag(0);
        stop = true;
        break;
      }
    }
    if(!stop) finalcluster.AddHit(hit);
  }

  return finalcluster;
}
   

PndTrkCluster* PndTrkLegendreTask::CleanupZPhiFit(PndTrkCluster *cluster, double fitm, double fitp) {

  PndTrkCluster *cleancluster = new PndTrkCluster();
  for(int ihit = 0; ihit < cluster->GetNofHits(); ihit++) {
    PndTrkHit *hit = cluster->GetHit(ihit);     
    if(hit->IsSttParallel()) {
      cleancluster->AddHit(hit);
      continue; // CHECK IsSttParrallel will be changed
    }

    TVector3 position = hit->GetPosition();
    double phi = hit->GetPhi();

    // d = | m x - y + p | / sqrt(m**2 + 1)
    double distance = fabs(fitm * phi -  position.Z()  + fitp )/sqrt(fitm * fitm + 1);
    //    cout << "distance " << hit->GetHitID() << " " << distance << endl;

    if(hit->GetDetectorID() == FairRootManager::Instance()->GetBranchId(fMvdPixelBranch) || hit->GetDetectorID() == FairRootManager::Instance()->GetBranchId(fMvdStripBranch)) {
      if(distance < 1) cleancluster->AddHit(hit);
    }
    else if(hit->GetDetectorID() == FairRootManager::Instance()->GetBranchId(fSttBranch)) {
      if(distance < 5) cleancluster->AddHit(hit);
    }
  }
  //  cout << "cleancluster " << cleancluster->GetNofHits () << endl;
  return cleancluster;
}


Bool_t PndTrkLegendreTask::ZPhiFit(int iter, PndTrkCluster *cluster, double &fitm, double &fitp)
{  

  if(iter != 0) {
    if(fDisplayOn) {
      char goOnChar;
      display->cd(3);
      DrawZGeometry();
      hzphi->SetXTitle("#phi");
      hzphi->SetYTitle("#z");
      hzphi->Draw();
      display->Update();
      display->Modified();  
    }
  }
    
  fFitter->Reset();
  PndTrkHit *hit = NULL;
  PndTrkHit *refhit = NULL;
  double refiso = 1000;
  for(int ihit = 0; ihit < cluster->GetNofHits(); ihit++) {
    hit = cluster->GetHit(ihit);        
    if(hit->IsSttParallel()) continue; // CHECK IsSttParrallel will be changed
        
        
    TVector3 position = hit->GetPosition();
    double phi = hit->GetPhi(); 
    //    fFitter->SetPointToFit(phi, position.Z(), 0.1);
   
    // CHECK refhit stuff
    if(hit->GetDetectorID() == FairRootManager::Instance()->GetBranchId(fMvdPixelBranch) || hit->GetDetectorID() == FairRootManager::Instance()->GetBranchId(fMvdStripBranch)) {
     if(iter != 0) refhit = hit;
      fFitter->SetPointToFit(phi, position.Z(), 0.001);
    }
    else if(hit->GetDetectorID() == FairRootManager::Instance()->GetBranchId(fSttBranch)) {
      if(iter != 0 && refhit == NULL && hit->GetIsochrone() < refiso) refhit = hit; 
      fFitter->SetPointToFit(phi, position.Z(), 0.1);
    }
    
    if(fDisplayOn) {
      char goOnChar;
      if(iter == 0) display->cd(4);
      TMarker *mrkfoundzphi = NULL;
      if(hit->GetDetectorID() == FairRootManager::Instance()->GetBranchId(fMvdPixelBranch) || hit->GetDetectorID() == FairRootManager::Instance()->GetBranchId(fMvdStripBranch)) {
        mrkfoundzphi = new TMarker(phi, position.Z(), 21);
        if(iter == 0)   mrkfoundzphi->SetMarkerColor(kOrange);
        else mrkfoundzphi->SetMarkerColor(4);
      }
      else if(hit->GetDetectorID() == FairRootManager::Instance()->GetBranchId(fSttBranch)) {
        mrkfoundzphi  = new TMarker(phi, position.Z(), 20);
        if(iter == 0)   mrkfoundzphi->SetMarkerColor(kGreen);
        else mrkfoundzphi->SetMarkerColor(4);
   }
      mrkfoundzphi->Draw("SAME");
      //      display->cd(3);
      //      mrkfoundzphi->Draw("SAME");

    }
  }
  // ===================================
  bool fit = kFALSE;
   if(iter == 0) fit = fFitter->StraightLineFit(fitm, fitp);
   else {
     if(refhit != NULL) fit = fFitter->ConstrainedStraightLineFit(refhit->GetPhi(), refhit->GetPosition().Z(), fitm, fitp);
     else fit = fFitter->StraightLineFit(fitm, fitp);
   }

  if(fit) {
    if(fDisplayOn) {
      char goOnChar;
      if(iter != 0)  display->cd(4);
      TLine *l22 = new  TLine(-1000, -1000 * fitm + fitp, 1000, 1000 * fitm  + fitp);
      if(iter == 0)     l22->SetLineColor(3);
      else  if(iter == 1)  l22->SetLineColor(4);
      l22->Draw("SAME");
      display->cd(3);
      l22->Draw("SAME");
      display->Update();
      display->Modified();  
           cin >> goOnChar;   
    }
  }
  // ++++++++++++++++++++++++++++++++++

  return fit;
}

double PndTrkLegendreTask::ComputeZRediduals(PndTrkCluster *cluster, double fitm, double fitp) {

  TH1F hresiduals("hresiduals", "residuals in z", 20, -5, 5);

  for(int ihit = 0; ihit < cluster->GetNofHits(); ihit++) {
    PndTrkHit *hit = cluster->GetHit(ihit);     
    
    if(hit->GetDetectorID() != FairRootManager::Instance()->GetBranchId(fSttBranch)) continue;
    if(hit->IsSttParallel()) continue;
    
    TVector3 position = hit->GetPosition();
    double phi = hit->GetPhi();
    
    
    double distance = fitm * phi  + fitp - position.Z();

    hresiduals.Fill(distance);
  }

  if(fDisplayOn)  {
    display->cd(4);
    char goOnChar;
    cout << "residuals";
    cin >> goOnChar;
    hresiduals.Draw();
    display->Update();
    display->Modified();
     cin >> goOnChar;
 }
  return hresiduals.GetMean();
}

double PndTrkLegendreTask::CorrectZ(PndTrkCluster *cluster, double deltaz, double fitm, double fitp) {

  for(int ihit = 0; ihit < cluster->GetNofHits(); ihit++) {
    PndTrkHit *hit = cluster->GetHit(ihit);     
    
    if(hit->GetDetectorID() != FairRootManager::Instance()->GetBranchId(fSttBranch)) continue;
    if(hit->IsSttParallel()) continue;
    
    TVector3 position = hit->GetPosition();
    double newz = position.Z() - deltaz;
    
    
    int tubeID = hit->GetTubeID();
    PndSttTube *tube = (PndSttTube*) fTubeArray->At(tubeID);
   
    TVector3 wireDirection = tube->GetWireDirection();
    Double_t halflength = tube->GetHalfLength();
      
    TVector3 first  = tube->GetPosition() + wireDirection * halflength; // CHECK
    TVector3 second = tube->GetPosition() - wireDirection * halflength; // CHECK
    
    double m, p;
    double newx = position.X();
    double newy = position.Y();
    if((second.X() - first.X()) != 0) {
      m = (second.Z() - first.Z())/(second.X() - first.X());
      p = position.Z() - m * position.X();
      newx = (newz - p)/m;
    }

    if((second.Y() - first.Y()) != 0) {
      
      m = (second.Z() - first.Z())/(second.Y() - first.Y());
      p = position.Z() - m * position.Y();
      newy = (newz - p)/m;
    }

    hit->SetPosition(TVector3(newx, newy, newz));

    if(fDisplayOn)  {
      display->cd(1);
      char goOnChar;
      cout << endl;
      cout << "SEE IT" ;
      TMarker *mrk = new TMarker(newx, newy, 6);
      mrk->SetMarkerColor(kOrange);
      mrk->Draw("SAME");
      display->Update();
      display->Modified();
      
      cout << "old " << position.X() << " " << position.Y() << " " << position.Z() << endl;
      cout << "new " << newx << " " << newy << " " << newz << endl;

      cin >> goOnChar;
    }
    
  }
}
  ClassImp(PndTrkLegendreTask)