///////////////////////////////////////////////////////////////// // // Track Finding based on the Conformal Mapping + Hough // ///////////////////////////////////////////////////////////////// #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include

#include #include /// Global parameters to be used in this class
namespace {
/// The way using namespacing parameters might be not good,
/// however they are required for non class member functions...
  const Double_t fPolarity = -1.;
  const Int_t    fMaxFiltering = 10;
  const Double_t fIniDeltaRFilter[fMaxFiltering]
    = {5.0, 4.0, 3.0, 2.5, 2.0, 1.75, 1.0, 0.75, 0.5, 0.5};
  const Double_t fPhiMin = -M_PI/90.;
}

//======================================================================
// fCheckDeltaRFunction
//======================================================================
Double_t fCheckDeltaRFunction(Double_t* x, Double_t* par)
{
  /// This is a function to check the distance between a point on linear function
  /// and the radius of a circle.

  // x      : angle
  // par[0] : slope
  // par[1] : offset
  // par[2] : radius
  // par[3] : drift distance

  if ( (sin(x[0])-par[0]*cos(x[0]))==0 ) return 1e10;

  Double_t rwire  = par[1] / (sin(x[0])-par[0]*cos(x[0]));
  Double_t deltaR = (rwire-par[2]>0)? (rwire-par[2]-par[3]) : (rwire-par[2]+par[3]);
  return fabs(deltaR);
}

//======================================================================
// fCheckHelixDeltaRFunction
//======================================================================
Double_t fCheckHelixDeltaRFunction(Double_t* x, Double_t* par)
{
  /// This is a function to check the distance between a point on 3D linear function
  /// and the curveture of a helix

  // x       : angle
  // par[0]  : wire end0 X after (0,0) translation
  // par[1]  : wire end0 Y after (0,0) translation
  // par[2]  : wire end0 Z after (0,0) translation
  // par[3]  : wire end1 X after (0,0) translation
  // par[4]  : wire end1 Y after (0,0) translation
  // par[5]  : wire end1 Z after (0,0) translation
  // par[6]  : drift distance
  // par[7]  : radius
  // par[8]  : startPhi
  // par[9]  : startZ
  // par[10] : dZ/dPhi

  Double_t xpos = par[7] * cos(fPolarity*x[0] + par[8]);
  Double_t ypos = par[7] * sin(fPolarity*x[0] + par[8]);
  Double_t zpos = par[10] * x[0] + par[9]; /// Absolute Z position

  /// Compatible to GetWirePositionAtZ() method
  TVector3 end0  = TVector3(par[0], par[1], par[2]);
  TVector3 end1  = TVector3(par[3], par[4], par[5]);
  TVector3 dir   = end1-end0;
  Double_t ratio = (zpos-end0.Z())/(end1.Z()-end0.Z());

  if (ratio<0 || ratio>1) return 1e10;

  TVector3 wPos  = (end0 + ratio * dir);

  if (fabs(zpos-wPos.Z())>0.5) return 1e10;

  Double_t dist = sqrt((wPos.X()-xpos)*(wPos.X()-xpos)+
                       (wPos.Y()-ypos)*(wPos.Y()-ypos)+
                       (wPos.Z()-zpos)*(wPos.Z()-zpos));

  return fabs(dist-par[6]);
}

//======================================================================
// fHelixTrackZRFunction
//======================================================================
Double_t fHelixTrackZRFunction(Double_t* x, Double_t* par)
{
  /// This is a function to check the distance between a point on 3D linear function
  /// and the curveture of a helix

  // x       : angle
  // par[0]  : circle center x
  // par[1]  : circle center y
  // par[2]  : radius
  // par[3]  : startPhi
  // par[4]  : startZ
  // par[5]  : dZ/dPhi

  Double_t phi  = (x[0] - par[4]) / par[5];
  Double_t xpos = par[2] * cos(fPolarity*phi + par[3]) + par[0];
  Double_t ypos = par[2] * sin(fPolarity*phi + par[3]) + par[1];

  return hypot(xpos, ypos);
}

//======================================================================
// Contructor
//======================================================================
IHoughTrackFinder::IHoughTrackFinder(const char* name, const char* title)
:IVTrackFinder(name, title)
,fWeightedPS(true)
,fRadiusThreForCircleFit(0.5)
,fRadiusThreForHelixFit(0.5)
,fRadiusResoForFit(0.1)
,fNBinTheta(180)
,fNBinR(150)
,fRangeR(0.03)
,fNMaxPeak(1000)
,fSigmaHough(2.0)
,fPeakThreHough(12.5)
,fMakeCombiHits(true)
,fMaxDeltaXYcls(3.)
,fMaxDeltaZcls(1e9)
,fMaxAbsZcls(70.)
,fMinimumRadius(20.)
,fMaximumRadius(45.)
,fRemoveIsolatedHits(true)
,fNeighborR(2.0)
,fNeighborPhi(0.03)
,fMinHitsPerCircle1st(15)
,fMinHitsPerCircle2nd(20)
,fNumFilter(0)
,fOutputHists(true)
,fNumOfEvents(0)
,fIsMC(false)
,fFunctorCircleCombi(this,&IHoughTrackFinder::CircleFitCombiFunction,3) /// Need to register function as a "ROOT::Math::Functor" ?
,fFunctorCircle(this,&IHoughTrackFinder::CircleFitFunction,3)           /// Need to register function as a "ROOT::Math::Functor" ?
,fFunctorHelix(this,&IHoughTrackFinder::HelixFitFunction,6)             /// Need to register function as a "ROOT::Math::Functor" ?
{
  //COMETNamedDebug("IHoughTrackFinder","Constructor");
  fMinDeltaRFilter.resize(fMaxFiltering);
  fMinDeltaRFilter.assign(fIniDeltaRFilter, fIniDeltaRFilter + fMaxFiltering);
  fHitSelection    = new COMET::IHitSelection("HoughHitSelection", "hit selection used for IHoughTrackFinder");
  fFuncDeltaR      = new TF1("FuncDeltaR", fCheckDeltaRFunction, -M_PI_2, M_PI_2, 4);
  fFuncHelixDeltaR = new TF1("FuncHelixDeltaR", fCheckHelixDeltaRFunction, fPhiMin, 3.*M_PI, 11);
  fHough           = new TF1("FuncHough", "[0]*cos(x)+[1]*sin(x)", -M_PI/2., M_PI/2.); 
}

//======================================================================
// Destructor
//======================================================================
IHoughTrackFinder::~IHoughTrackFinder() {
  delete fHitSelection;
  delete fFuncDeltaR;
  delete fFuncHelixDeltaR;
  delete fHough;
}

//======================================================================
// CircleFitFunctionUseCombiHits
//======================================================================
Double_t IHoughTrackFinder::CircleFitCombiFunction(const Double_t *par) const
{
  // par[0] : X center of circle
  // par[1] : Y center of circle
  // par[2] : radius of circle

  Double_t chi2  = 0; /// to be minimize
  Int_t nGoodHit = 0; /// Number of good hits

  Double_t deltaR;
  Int_t nHits = fClusters->GetN();
  Double_t *x = fClusters->GetX();
  Double_t *y = fClusters->GetY();
  for (Int_t ihit = 0; ihit < nHits; ihit++) {
    deltaR  = hypot(x[ihit]-par[0], y[ihit]-par[1]) - par[2];
    if (fabs(deltaR)>4.*fNeighborR) continue;
    chi2   += pow(deltaR/fNeighborR,2);
    if (fabs(deltaR)<2.*fNeighborR) nGoodHit++;
  }
  return chi2/((Double_t)nGoodHit);
}

//======================================================================
// CircleFitFunction
//======================================================================
Double_t IHoughTrackFinder::CircleFitFunction(const Double_t *par) const
{
  // par[0] : X center of circle
  // par[1] : Y center of circle
  // par[2] : radius of circle

  Double_t chi2     = 0; /// Chi-square for minimization
  Int_t    nGoodHit = 0; /// Number of good hits
  //Int_t    nParam   = 3; /// Number of paramters

  Double_t xcent  = par[0];
  Double_t ycent  = par[1];
  Double_t radius = par[2];

  COMET::IGeometryId geomId;
  TVector3 wireEnd0, wireEnd1; /// Both wire ends in DS coordinate
  Double_t dDist;     /// Drift distance
  Int_t    wire;      /// Wire ID
  Double_t deltaR;    /// R_{hit} - R_{circle}
  Double_t xy0[2] = {0,0};
  Double_t xy1[2] = {0,0};

  Double_t slope  = 0;
  Double_t offset = 0;

  Double_t phi0, dphi;
  Double_t phiRange[2] = {-M_PI, M_PI};

  fFuncDeltaR->FixParameter(2, radius);

  /// Chi-square calculation
  for (COMET::IHitSelection::iterator hitIt = fHitSelection->begin(); hitIt != fHitSelection->end(); ++hitIt) {
    if (!((*hitIt)->GetChannelId().IsCDCChannel())) continue;

    geomId   = (*hitIt)->GetGeomId();
    dDist    = (*hitIt)->GetDriftDistance();
    wire     = COMET::IGeomInfo::Get().CDC().GeomIdToWire(geomId);

    COMET::IGeomInfo::Get().DetectorSolenoid().GetDetPositionInDSCoordinate(COMET::IGeomInfo::Get().CDC().GetWireEnd0(wire), wireEnd0);
    COMET::IGeomInfo::Get().DetectorSolenoid().GetDetPositionInDSCoordinate(COMET::IGeomInfo::Get().CDC().GetWireEnd1(wire), wireEnd1);

    /// Move the origin of coordinate to circle center
    xy0[0] = wireEnd0.X()-xcent; xy0[1] = wireEnd0.Y()-ycent;
    xy1[0] = wireEnd1.X()-xcent; xy1[1] = wireEnd1.Y()-ycent;

    phi0   = atan2(xy0[1], xy0[0]);
    dphi   = atan2(xy1[1]-xy0[1], xy1[0]-xy0[0]);

    if (dphi>0) {phiRange[0] = phi0; phiRange[1] = phi0+dphi;}
    else {phiRange[1] = phi0; phiRange[0] = phi0+dphi;}

    slope  = (xy1[1]-xy0[1])/(xy1[0]-xy0[0]);
    offset = xy0[1]-slope*xy0[0];

    /// Check both for left-right
    fFuncDeltaR->FixParameter(0, slope);
    fFuncDeltaR->FixParameter(1, offset);
    fFuncDeltaR->FixParameter(3, dDist);

    deltaR = fFuncDeltaR->GetMinimum(phiRange[0], phiRange[1]);

    if (fabs(deltaR) FixParameter(7,  radius);   /// Radius
  fFuncHelixDeltaR->FixParameter(8,  startPhi); /// Start Phi
  fFuncHelixDeltaR->FixParameter(9,  startZ);   /// Start Z
  fFuncHelixDeltaR->FixParameter(10, dZdPhi);   /// dZ/dPhi

  /// Chi-square calculation
  for (COMET::IHitSelection::iterator hitIt = fHitSelection->begin(); hitIt != fHitSelection->end(); ++hitIt) {
    if (!((*hitIt)->GetChannelId().IsCDCChannel())) continue;

    geomId   = (*hitIt)->GetGeomId();
    dDist    = (*hitIt)->GetDriftDistance();
    wire     = COMET::IGeomInfo::Get().CDC().GeomIdToWire(geomId);

    COMET::IGeomInfo::Get().DetectorSolenoid().GetDetPositionInDSCoordinate(COMET::IGeomInfo::Get().CDC().GetWireEnd0(wire), wireEnd0);
    COMET::IGeomInfo::Get().DetectorSolenoid().GetDetPositionInDSCoordinate(COMET::IGeomInfo::Get().CDC().GetWireEnd1(wire), wireEnd1);

    xy0[0] = wireEnd0.X()-xcent; xy0[1] = wireEnd0.Y()-ycent;
    xy1[0] = wireEnd1.X()-xcent; xy1[1] = wireEnd1.Y()-ycent;

    phi0   = atan2(xy0[1], xy0[0]) - startPhi;
    dphi   = atan2(xy1[1]-xy0[1], xy1[0]-xy0[0]);

    if (dphi>0) {phiRange[0] = phi0-startPhi; phiRange[1] = phi0+dphi-startPhi;}
    else {phiRange[1] = phi0-startPhi; phiRange[0] = phi0+dphi-startPhi;}

    if (phiRange[0] FixParameter(0,  xy0[0]);
    fFuncHelixDeltaR->FixParameter(1,  xy0[1]);
    fFuncHelixDeltaR->FixParameter(2,  wireEnd0.Z());
    fFuncHelixDeltaR->FixParameter(3,  xy1[0]);
    fFuncHelixDeltaR->FixParameter(4,  xy1[1]);
    fFuncHelixDeltaR->FixParameter(5,  wireEnd1.Z());
    fFuncHelixDeltaR->FixParameter(6,  dDist);

    deltaR = fFuncHelixDeltaR->GetMinimum(phiRange[0], phiRange[1]);

    if (fabs(deltaR) > &track)
{
  return;
}

//======================================================================
// Process (main method to be called)
//======================================================================
COMET::IHandle IHoughTrackFinder::Process(const COMET::IAlgorithmResult& input)
{
  COMETNamedDebug("IHoughTrackFinder","Process");

  if (fOutputHists && fNumOfEvents<100) PrepareOutputs();

  fHelixTrackParamArray.clear();

  // Output algorithm result
  COMET::IHandle result(new COMET::IAlgorithmResult(fName.Data(), "Hough track finding result"));

  COMET::IHandle hits = input.GetHitSelection();
  if (!hits || hits->empty()) {
    COMETNamedError("IHoughTrackFinder"," No hits are found !!");
    return result;
  }

  std::cout << hits->GetName() << std::endl;
  if (strcmp((hits->GetName()),"mchits")==0) {
    fIsMC = true;
    Int_t npoints = 0;
    TVector3 posMC, local;
    COMETNamedDebug("IHoughTrackFinder", "$$$ Number of MC hits : " << hits->size());
    for (UInt_t ihit = 0; ihit < hits->size(); ihit++) {
      COMET::IHandle mchit = hits->at(ihit);
      posMC  = mchit->GetPositionMC();
      COMET::IGeomInfo::Get().DetectorSolenoid().GetDetPositionInDSCoordinate(posMC, local);
      npoints = fMCHits->GetN();
      fMCHits->SetPoint(npoints, local.X(), local.Y());
      fMCHitsZR->SetPoint(npoints, local.Z(), hypot(local.X(), local.Y()));
    }
  } else fIsMC = false;

  COMETNamedDebug("IHoughTrackFinder", "Found Hough hits");

  fHistPhiR = new TH2F(Form("HoughHist_Ev%d",fNumOfEvents), "2D Hough Histogram", fNBinTheta, -M_PI/2., M_PI/2., fNBinR, -fRangeR, fRangeR);

  //--------------------------------
  // Make ReconTrackCand
  //--------------------------------
  // Container
  COMET::IReconObjectContainer* trackContainer = 
    new COMET::IReconObjectContainer(Form("trackcandidates_%s",fName.Data()), Form("Container of track candidates found by %s",fName.Data()));

  /// Main function to find track candidates
  std::vector tracks = MakeTrackCandidatesFromHits(hits);

  //COMETNamedDebug("IHoughTrackFinder","Fill ReconTrackCand");
  if (tracks.size()) {
    for (UInt_t itr = 0; itr < tracks.size(); itr++) {
      trackContainer->push_back(COMET::IHandle (tracks.at(itr)));
    }
  }

  // Add to the result
  if(trackContainer->size() > 0) {
    COMETNamedDebug("IHoughTrackFinder", "Size of tracks = " << trackContainer->size());
    result->AddResultsContainer(trackContainer);
  }

  //COMETNamedDebug("IHoughTrackFinder", "Before WriteOutputs");
  if (fOutputHists && fNumOfEvents<100) WriteOutputs();

  delete fHistPhiR;

  COMETNamedDebug("IHoughTrackFinder", "End Process");
  return result;
}

//======================================================================
// MakeATrackCandidateFromHits
//======================================================================
std::vector IHoughTrackFinder::MakeTrackCandidatesFromHits(const COMET::IHandle & hits)
{
  COMETNamedDebug("IHoughTrackFinder","MakeTrackCandidatesFromHits");
  std::vector tracks;

  TVector3 local;
  Int_t npoints = -1;
  Int_t wire    = -1;
  COMET::IGeometryId geomId;
  if (fOutputHists && fNumOfEvents<100) {
    for (UInt_t ihit = 0; ihit < hits->size(); ihit++) {
      geomId   = hits->at(ihit)->GetGeomId();
      wire     = COMET::IGeomInfo::Get().CDC().GeomIdToWire(geomId);
      COMET::IGeomInfo::Get().DetectorSolenoid().GetDetPositionInDSCoordinate(COMET::IGeomInfo::Get().CDC().GetWirePosition(wire), local);
      //COMETNamedDebug("IHoughTrackFinder","WirePosition Global " << COMET::IGeomInfo::Get().CDC().GetWirePosition(wire).X() << " "
      //                                                           << COMET::IGeomInfo::Get().CDC().GetWirePosition(wire).Y() << " "
      //                                                           << COMET::IGeomInfo::Get().CDC().GetWirePosition(wire).Z());
      //COMETNamedDebug("IHoughTrackFinder","WirePosition Local  " << local.X() << " " << local.Y() << " " << local.Z());
      npoints = fAllHitWires->GetN();
      fAllHitWires->SetPoint(npoints, local.X(), local.Y());
    }
  }

  COMET::IHitSelection* newhits;
  if (fRemoveIsolatedHits) newhits = RemoveIsolatedHits(hits);
  else newhits = &(*hits);

  std::vector combiHits;
  FillHistogramPhiR(newhits, combiHits);

  std::vector < std::vector > circles; /// Vector of circle parameters (x0, y0, radius)

  Int_t npeaks = PeakSearch2D(circles);
  COMETNamedDebug("IHoughTrackFinder","Number of found peaks from Hough: " << npeaks);

  if (fMakeCombiHits) {
    CircleCombiFitting(circles);
    CircleCombiFitting(circles);
    CircleCombiFitting(circles);
  }

  TLorentzVector position;
  TVector3 direction;
  Double_t momentum;

  TVector3 posMean, posRMS;
  for (Int_t ic = 0; ic < npeaks; ic++) {
    fNumFilter = 0;
    std::vector circle = circles.at(ic);
    std::vector cerror;

    /// Exclude the hits that are far from the circle line
    COMET::IHitSelection* selectedHits = HitFiltering(circle, newhits, posMean, posRMS);
    fNumFilter++;

    if (selectedHits->size() size(); ihit++) {
        geomId   = selectedHits->at(ihit)->GetGeomId();
        wire     = COMET::IGeomInfo::Get().CDC().GeomIdToWire(geomId);
        COMET::IGeomInfo::Get().DetectorSolenoid().GetDetPositionInDSCoordinate(COMET::IGeomInfo::Get().CDC().GetWirePosition(wire), local);
        npoints = fFilteredWires1st->GetN();
        fFilteredWires1st->SetPoint(npoints, local.X(), local.Y());
      }
    }

    /// Re-fine the circle parameters only using selected hits
    COMETNamedDebug("IHoughTrackFinder", "Initial circle: " << circle.at(0) << " " << circle.at(1) << " " << circle.at(2));
    cerror.clear();
    cerror.push_back(5);
    cerror.push_back(5);
    cerror.push_back(5);
    if (!RefineCircle(circle, cerror, selectedHits)) {
      delete selectedHits;
      continue;
    }

    if (!RefineCircle(circle, cerror, selectedHits)) {
      delete selectedHits;
      continue;
    }

    /// More strict filtering using drift distance...
    delete selectedHits;
    selectedHits = HitFiltering(circle, newhits, posMean, posRMS);
    fNumFilter++;

    if (selectedHits->size() size(); ihit++) {
        geomId   = selectedHits->at(ihit)->GetGeomId();
        wire     = COMET::IGeomInfo::Get().CDC().GeomIdToWire(geomId);
        COMET::IGeomInfo::Get().DetectorSolenoid().GetDetPositionInDSCoordinate(COMET::IGeomInfo::Get().CDC().GetWirePosition(wire), local);
        npoints = fFilteredWires2nd->GetN();
        fFilteredWires2nd->SetPoint(npoints, local.X(), local.Y());
      }
    }

    Double_t meanPhi = (meanPhi>0)? posMean.Phi() : posMean.Phi()+M_PI_2;
    Double_t meanZ   = posMean.Z();
    if (meanZ>+40.) meanZ = +40.;
    if (meanZ<-40.) meanZ = -40.;
    Double_t rmsZ    = (posRMS.Z()<10.)? posRMS.Z() : 10.;
    circle.push_back(meanPhi); /// Initla value for startPhi
    circle.push_back(meanZ); /// Initla value for startZ
    circle.push_back(0); /// Initla value for dZ/dPhi
    cerror.push_back(M_PI/60.); /// Initla error for startPhi
    cerror.push_back(3.*rmsZ); /// Initla error for startZ
    cerror.push_back(10./M_PI); /// Initla error for dZ/dPhi
    /// Re-fine the circle parameters only using selected hits
    if (!RefineCircle(circle, cerror, selectedHits, true)) {
    //if (!RefineCircle(circle, cerror, selectedHits)) {
      delete selectedHits;
      continue;
    }

    delete selectedHits;
    selectedHits = HitFiltering(circle, newhits, posMean, posRMS);
    fNumFilter++;

    if (selectedHits->size() size(); ihit++) {
        geomId   = selectedHits->at(ihit)->GetGeomId();
        wire     = COMET::IGeomInfo::Get().CDC().GeomIdToWire(geomId);
        COMET::IGeomInfo::Get().DetectorSolenoid().GetDetPositionInDSCoordinate(COMET::IGeomInfo::Get().CDC().GetWirePosition(wire), local);
        npoints = fFilteredWires3rd->GetN();
        fFilteredWires3rd->SetPoint(npoints, local.X(), local.Y());
      }
    }

    meanPhi = (meanPhi>0)? posMean.Phi() : posMean.Phi()+M_PI_2;
    meanZ   = posMean.Z();
    if (meanZ>+40.) meanZ = +40.;
    if (meanZ<-40.) meanZ = -40.;
    rmsZ    = (posRMS.Z()<10.)? posRMS.Z() : 10.;
    circle.at(3) = meanPhi;  // Initla value for startPhi
    circle.at(4) = meanZ;    // Initla value for startZ
    cerror.at(3) = M_PI/45.; // Initla error for startPhi
    cerror.at(4) = 3.*rmsZ;  // Initla error for startZ
    /// Re-fine the circle parameters only using selected hits
    if (!RefineCircle(circle, cerror, selectedHits, true)) {
      delete selectedHits;
      continue;
    }
    if (!RefineCircle(circle, cerror, selectedHits, true)) {
      delete selectedHits;
      continue;
    }

    HelixTrackParam helixParam;
    for (UInt_t ip = 0; ip < circle.size(); ip++) {
      helixParam.at(ip) = circle.at(ip);
    }
    fHelixTrackParamArray.push_back(helixParam);

    COMETNamedDebug("IHoughTrackFinder", "Re-fined circle: " << circle.at(0) << " " << circle.at(1) << " " << circle.at(2));
    if (fOutputHists && fNumOfEvents<100) {
      TArc aCircle = TArc(circle.at(0), circle.at(1), circle.at(2));
      fCircles3rd.push_back(aCircle);
    }

    position  = TLorentzVector(helixParam.at(0)+helixParam.at(2)*cos(helixParam.at(3)), // start X
                               helixParam.at(1)+helixParam.at(2)*sin(helixParam.at(3)), // start Y
                               helixParam.at(4), // start Z
                               0); // start timing is temporary set to zero

    direction = CalcDirection(helixParam);

    /// Assuming 1T uniform field... (p ~ 3 [MeV/c/cm/T] * B * R)
    momentum  = 3.*helixParam.at(2);

    COMETNamedDebug("IHoughTrackFinder", "Save Results");
    COMET::IReconTrackCand* aTrack = new COMET::IReconTrackCand();
    aTrack->SetAlgorithmName(Form("IHoughTrackFinderResult_%d", (Int_t)tracks.size()));
    aTrack->SetName(Form("IHoughTrackFinderResult_%d", (Int_t)tracks.size()));
    aTrack->SetTitle(Form("Hough track candidate: %04d", (Int_t)tracks.size()));
    aTrack->AddHits(selectedHits);

    /// Set track state (the way written in oaEvent/src/IReconBase.hxx
    COMET::IHandle trackState = aTrack->GetState();
    trackState->SetMomentum(momentum);   /// In [MeV/c]  (would like to know the unit convention... though I think MeV/c is reasonable to us)
    trackState->SetDirection(direction); /// Unit vector
    trackState->SetPosition(position);   /// In [cm]     (would like to know the unit convention... though I think cm is reasonable to us)

    tracks.push_back(aTrack);
  }

  return tracks;
}

//======================================================================
// Finalize
//======================================================================
Int_t IHoughTrackFinder::Finish()
{
  if (fOutputHists) {
    fOutFile->Close();
    delete fOutFile;
  }
  return 1;
}

//======================================================================
// Fill histogram
//======================================================================
Int_t IHoughTrackFinder::FillHistogramPhiR(const COMET::IHitSelection* hits, std::vector & combiHitsLoc)
{
  COMETNamedDebug("IHoughTrackFinder","FillHistogramPhiR");
  combiHitsLoc.clear();

  TVector3 local0, local1;
  Double_t hitX, hitY;
  Double_t hitU, hitV;

  std::vector combiHits;

  if (fMakeCombiHits) MakeCombiHits(hits, combiHits);

  UInt_t nHits = (fMakeCombiHits)? combiHits.size() : hits->size();

  COMETNamedDebug("IHoughTrackFinder", "Number of Hits = " << nHits);

  Double_t weight = 0;

  if (nHits) {

    Double_t binWidth = M_PI/((Double_t)fNBinTheta);
    Double_t minVal   = -M_PI/2.;
    Double_t binVal   = 0;

    COMET::IGeometryId geomId;
    Int_t wire = -1;

    for (UInt_t ihit = 0; ihit < nHits; ihit++) {

      weight = 1.;

      if (!fMakeCombiHits) {
        geomId = hits->at(ihit)->GetGeomId();
        wire = COMET::IGeomInfo::Get().CDC().GeomIdToWire(geomId);
        COMET::IGeomInfo::Get().DetectorSolenoid().GetDetPositionInDSCoordinate(COMET::IGeomInfo::Get().CDC().GetWirePosition(wire), local0);
      } else {
        COMET::IGeomInfo::Get().DetectorSolenoid().GetDetPositionInDSCoordinate(combiHits.at(ihit), local0);
        if (fOutputHists && fNumOfEvents<100)
          {Int_t npoints = fClusters->GetN();fClusters->SetPoint(npoints, local0.X(), local0.Y());}
      }

      if (fWeightedPS) {
        for (UInt_t jhit = 0; jhit < nHits; jhit++) {
          if (ihit==jhit) continue;

          if (!fMakeCombiHits) {
            geomId = hits->at(jhit)->GetGeomId();
            wire = COMET::IGeomInfo::Get().CDC().GeomIdToWire(geomId);
            COMET::IGeomInfo::Get().DetectorSolenoid().GetDetPositionInDSCoordinate(COMET::IGeomInfo::Get().CDC().GetWirePosition(wire), local1);
          } else {
            COMET::IGeomInfo::Get().DetectorSolenoid().GetDetPositionInDSCoordinate(combiHits.at(ihit), local1);
          }

          if ( fabs(local1.Perp()-local0.Perp()) SetParameters(hitU, hitV);

      for (Int_t ibin = 0; ibin < fNBinTheta; ibin++) {
         binVal = minVal + binWidth * (0.5 + ((Double_t)ibin));
         fHistPhiR->Fill(binVal, fHough->Eval(binVal), weight);
      }
    }
  }
  return 0;
}

//======================================================================
// 2D peak search in given histogram
//======================================================================
Int_t IHoughTrackFinder::PeakSearch2D(std::vector< std::vector >& circles)
{
  COMETNamedDebug("IHoughTrackFinder","PeakSearch2D");

  TSpectrum2* s2d   = new TSpectrum2(fNMaxPeak);
  Float_t** source  = new Float_t*[fNBinTheta];
  Float_t** dest    = new Float_t*[fNBinTheta];

  for (Int_t ix = 0; ix < fNBinTheta; ix++) {
    source[ix] = new Float_t[fNBinR];
    dest[ix]   = new Float_t[fNBinR];
    for (Int_t iy = 0; iy < fNBinR; iy++) {
      source[ix][iy] = fHistPhiR->GetBinContent(ix+1, iy+1);
    }
  }

  Int_t nfound = s2d->SearchHighRes(source, dest, fNBinTheta, fNBinR, fSigmaHough, fPeakThreHough, false, 3, false, 1);

  Float_t *peakX = s2d->GetPositionX();
  Float_t *peakY = s2d->GetPositionY();

  Double_t rho    = 0;
  Double_t theta  = 0;
  Double_t weight = 0;

  // Use the weighted average peak
  // If multi peaks are found very nearby, only the largest peak will be selected
  const  Int_t deltaBin  = 2;
  Bool_t haveNearPeak    = false;
  for (Int_t i = 0; i < nfound; i++) {
    haveNearPeak  = false;

    for (Int_t j = 0; j < nfound; j++) {

      if (i==j) continue;

      if ( fabs(peakX[i]-peakX[j]) < deltaBin && fabs(peakY[i]-peakY[j]) < deltaBin ) {
        if (fHistPhiR->GetBinContent(((Int_t)(peakX[i]+0.5)), ((Int_t)(peakY[i]+0.5))) < 
            fHistPhiR->GetBinContent(((Int_t)(peakX[j]+0.5)), ((Int_t)(peakY[j]+0.5)))) {
          haveNearPeak = true;
          break;
        }
      }
    }

    if (haveNearPeak) continue;

    theta  = 0;
    rho    = 0;
    weight = 0;

    for (Int_t ix = -1; ix <= 1; ix++) {
      for (Int_t iy = -1; iy <= 1; iy++) {
        theta  += fHistPhiR->GetXaxis()->GetBinCenter( ((Int_t)(peakX[i]+0.5))+ix ) *
                  fHistPhiR->GetBinContent(((Int_t)(peakX[i]+0.5))+ix, ((Int_t)(peakY[i]+0.5))+iy);
        rho    += fHistPhiR->GetYaxis()->GetBinCenter( ((Int_t)(peakY[i]+0.5))+iy ) *
                  fHistPhiR->GetBinContent(((Int_t)(peakX[i]+0.5))+ix, ((Int_t)(peakY[i]+0.5))+iy);
        weight += fHistPhiR->GetBinContent(((Int_t)(peakX[i]+0.5))+ix, ((Int_t)(peakY[i]+0.5))+iy);
      }
    }

    theta /= weight;
    rho   /= weight;

    std::vector cparam; // x0, y0, r
    HoughToCircleParam(theta, rho, cparam);

    /// Circle with too small or too large radius is excluded
    if ( fabs(cparam.at(2)) fMaximumRadius ) continue;

    circles.push_back(cparam);
  }

  Int_t npeaks = circles.size();

  for (Int_t ix = 0; ix < fNBinTheta; ix++) {
    delete [] source[ix];
    delete [] dest[ix];
  }
  delete [] source;
  delete [] dest;

  delete s2d;
  return npeaks;
}

//======================================================================
// Remove hits if they don't have neighboring hits
//======================================================================
COMET::IHitSelection* IHoughTrackFinder::RemoveIsolatedHits(const COMET::IHandle & hits)
{
  COMETNamedDebug("IHoughTrackFinder","RemoveIsolatedHits");
  Bool_t findNeighbor = false;

  COMET::IHitSelection* newhits = new COMET::IHitSelection("houghhits", "Hits found by IHoughTrackFinder");

  if (hits) {


    TVector3 pos0, pos1; /// Global position
    TVector3 loc0, loc1; /// Position in DS coordinate
    Int_t wire    = -1;
    COMET::IGeometryId geomId;

    for (COMET::IHitSelection::const_iterator hitIt0 = hits->begin(); hitIt0 != hits->end(); ++hitIt0) {

      geomId = (*hitIt0)->GetGeomId();
      wire   = COMET::IGeomInfo::Get().CDC().GeomIdToWire(geomId);

      pos0 = COMET::IGeomInfo::Get().CDC().GetWirePosition(wire);
      COMET::IGeomInfo::Get().DetectorSolenoid().GetDetPositionInDSCoordinate(pos0, loc0);

      for (COMET::IHitSelection::const_iterator hitIt1 = hits->begin(); hitIt1 != hits->end(); ++hitIt1) {

        if ((*hitIt0)==(*hitIt1)) continue;

        geomId = (*hitIt0)->GetGeomId();
        wire   = COMET::IGeomInfo::Get().CDC().GeomIdToWire(geomId);

        pos1 = COMET::IGeomInfo::Get().CDC().GetWirePosition(wire);
        COMET::IGeomInfo::Get().DetectorSolenoid().GetDetPositionInDSCoordinate(pos1, loc1);

        if ( fabs(loc0.Perp()-loc1.Perp()) < fNeighborR && atan2((loc0.Y()-loc1.Y()),(loc0.X()-loc1.X())) < fNeighborPhi ) {
          findNeighbor = true;
          break;
        }
      }

      if (findNeighbor) {
        newhits->AddHit((*hitIt0));
      }
    }
  }
  return newhits;
}

//======================================================================
// 2nd Remove hits which are far from a circle candidate
//======================================================================
COMET::IHitSelection* IHoughTrackFinder::HitFiltering(const std::vector & circle, const COMET::IHitSelection* hits,
                                                      TVector3& posMean, TVector3& posRMS)
{
  COMETNamedDebug("IHoughTrackFinder", "HitFiltering");

  COMET::IHitSelection* newhits = new COMET::IHitSelection("HoughHitSelection2ndFiltering", "hits after 2nd filtering");

  posMean = TVector3(0,0,0), posRMS = TVector3(0,0,0);

  Int_t    wire  = -1;
  Double_t dDist = -1.;
  TVector3 local; /// Position in DS coordinate
  TVector3 wireEnd0, wireEnd1; /// Both wire ends in DS coordinate
  COMET::IGeometryId geomId;
  TVector3 wirePos;
  Bool_t   goodHit = false;
  Double_t phiTmp  = 0;
  for (COMET::IHitSelection::const_iterator hitIt = hits->begin(); hitIt != hits->end(); ++hitIt) {
    goodHit = false;
    /// CDC wire
    if (!((*hitIt)->GetChannelId().IsCDCChannel())) continue;

    dDist  = (*hitIt)->GetDriftDistance();
    geomId = (*hitIt)->GetGeomId();
    wire   = COMET::IGeomInfo::Get().CDC().GeomIdToWire(geomId);

    COMET::IGeomInfo::Get().DetectorSolenoid().GetDetPositionInDSCoordinate(COMET::IGeomInfo::Get().CDC().GetWireEnd0(wire), wireEnd0);
    COMET::IGeomInfo::Get().DetectorSolenoid().GetDetPositionInDSCoordinate(COMET::IGeomInfo::Get().CDC().GetWireEnd1(wire), wireEnd1);

    if (circle.size()==3) { /// Circle
      goodHit = CircleFiltering(circle, wireEnd0, wireEnd1, dDist, wirePos);
    } else { /// Helix
      goodHit = HelixFiltering(circle, wireEnd0, wireEnd1, dDist, wirePos);
    }
    if (goodHit) {
      newhits->AddHit((*hitIt));

      posMean += wirePos;
      posRMS  += TVector3(wirePos.X() * wirePos.X(),
                          wirePos.Y() * wirePos.Y(),
                          wirePos.Z() * wirePos.Z());
    }
  }
  if (newhits->size()==0) return newhits;

  posMean *= 1./(Double_t)newhits->size();
  posRMS  *= 1./(Double_t)newhits->size();
  posRMS   = TVector3(sqrt( (posRMS.X()-posMean.X()*posMean.X()) ),
                      sqrt( (posRMS.Y()-posMean.Y()*posMean.Y()) ),
                      sqrt( (posRMS.Z()-posMean.Z()*posMean.Z()) ));

  return newhits;
}

//======================================================================
// Filtering with circle function
//======================================================================
Bool_t IHoughTrackFinder::CircleFiltering(const std::vector & circle, const TVector3& end0, const TVector3& end1, const Double_t& dDist, TVector3& wirePos)
{
  Double_t deltaRFilter = fMinDeltaRFilter.at(fNumFilter);

  Double_t radius = circle.at(2);
  /// Move the origin of coordinate to circle center
  Double_t xy0[2] = {end0.X()-circle.at(0), end0.Y()-circle.at(1)};
  Double_t xy1[2] = {end1.X()-circle.at(0), end1.Y()-circle.at(1)};

  Double_t phi0   = atan2(xy0[1], xy0[0]);
  Double_t dphi   = atan2(xy1[1]-xy0[1], xy1[0]-xy0[0]);

  Double_t phiRange[2] = {-M_PI, M_PI};

  if (dphi>0) {phiRange[0] = phi0; phiRange[1] = phi0+dphi;}
  else {phiRange[1] = phi0; phiRange[0] = phi0+dphi;}

  Double_t slope  = (xy1[1]-xy0[1])/(xy1[0]-xy0[0]);
  Double_t offset = xy0[1]-slope*xy0[0];

  /// Check both for left-right
  fFuncDeltaR->FixParameter(0, slope);
  fFuncDeltaR->FixParameter(1, offset);
  fFuncDeltaR->FixParameter(2, radius);
  fFuncDeltaR->FixParameter(3, dDist);

  Double_t deltaR = fFuncDeltaR->GetMinimum(phiRange[0], phiRange[1]);
  Double_t minPhi = fFuncDeltaR->GetMinimumX(phiRange[0], phiRange[1]);

  Double_t rpos = offset/(sin(minPhi)-slope*cos(minPhi));
  Double_t xpos = rpos * cos(minPhi);
  Double_t ypos = rpos * sin(minPhi);
  Double_t zpos = end0.Z() + (end1.Z()-end0.Z()) * (xpos-xy0[0]) / (xy1[0]-xy0[0]);

//  if (fabs(zpos)>fabs(end0.Z()) || fabs(zpos)>fabs(end1.Z())) return false;
  if (fabs(zpos)>90) return false;

  wirePos = TVector3(xpos+circle.at(0), ypos+circle.at(1), zpos);

  if (deltaR & circle, const TVector3& end0, const TVector3& end1, const Double_t& dDist, TVector3& wirePos)
{
  Double_t deltaRFilter = fMinDeltaRFilter.at(fNumFilter);

  Double_t xy0[2] = {end0.X()-circle.at(0), end0.Y()-circle.at(1)};
  Double_t xy1[2] = {end1.X()-circle.at(0), end1.Y()-circle.at(1)};

  Double_t phi0   = atan2(xy0[1], xy0[0]) - circle.at(3);
  Double_t dphi   = atan2(xy1[1]-xy0[1], xy1[0]-xy0[0]);

  Double_t phiRange[2] = {0, M_PI_2};

  if (dphi>0) {phiRange[0] = phi0; phiRange[1] = phi0+dphi;}
  else {phiRange[1] = phi0; phiRange[0] = phi0+dphi;}

  /// make sure phi is positive
  if (phiRange[0] FixParameter(0,  xy0[0]);
  fFuncHelixDeltaR->FixParameter(1,  xy0[1]);
  fFuncHelixDeltaR->FixParameter(2,  end0.Z());
  fFuncHelixDeltaR->FixParameter(3,  xy1[0]);
  fFuncHelixDeltaR->FixParameter(4,  xy1[1]);
  fFuncHelixDeltaR->FixParameter(5,  end1.Z());
  fFuncHelixDeltaR->FixParameter(6,  dDist);
  fFuncHelixDeltaR->FixParameter(7,  circle.at(2)); /// Radius
  fFuncHelixDeltaR->FixParameter(8,  circle.at(3)); /// Start Phi
  fFuncHelixDeltaR->FixParameter(9,  circle.at(4)); /// Start Z
  fFuncHelixDeltaR->FixParameter(10, circle.at(5)); /// dZ/dPhi

  Double_t deltaR = fFuncHelixDeltaR->GetMinimum(phiRange[0], phiRange[1]);
  Double_t minPhi = fFuncHelixDeltaR->GetMinimumX(phiRange[0], phiRange[1]);

  Double_t xpos = circle.at(2) * cos(fPolarity*minPhi+circle.at(3));
  Double_t ypos = circle.at(2) * sin(fPolarity*minPhi+circle.at(3));
  Double_t zpos = end0.Z() + (end1.Z()-end0.Z()) * (xpos-xy0[0]) / (xy1[0]-xy0[0]);

  //if (fabs(zpos)>fabs(end0.Z()) || fabs(zpos)>fabs(end1.Z())) return false;
  if (fabs(zpos)>90) return false;

  wirePos = TVector3(xpos+circle.at(0), ypos+circle.at(1), zpos);

  if (deltaR & circle, std::vector & cerror,
                                     const COMET::IHitSelection* hits, Bool_t helixMode)
{
  COMETNamedDebug("IHoughTrackFinder", "RefineCircle");
  Bool_t success = false;

  if (!helixMode) success = CircleFitting(circle, cerror, hits);
  else success = HelixFitting(circle, cerror, hits);

  return success;
}

//======================================================================
// MakeCombiHits
//======================================================================
Bool_t IHoughTrackFinder::MakeCombiHits(const COMET::IHitSelection* hits, std::vector & combiHits)
{
  COMETNamedDebug("IHoughTrackFinder","MakeCombiHits");
  combiHits.clear();
  std::vector tmpHits;
  if (hits) {

    UInt_t nHits = hits->size();
    std::vector< std::vector< Bool_t > > used(nHits);
    for (UInt_t i = 0; i < nHits; i++) used.at(i).resize(nHits,false);

    TVector3 crossPoint, local;
    Int_t wire0  = -1;
    Int_t wire1  = -1;
    Int_t layer0 = -1;
    Int_t layer1 = -1;
    Double_t dist;
    COMET::IGeometryId geomId;

    Int_t i0 = 0;
    Int_t i1 = 0;
    for (COMET::IHitSelection::const_iterator hitIt0 = hits->begin(); hitIt0 != hits->end(); ++hitIt0) {
      i1 = 0;

      /// CDC wire
      if (!((*hitIt0)->GetChannelId().IsCDCChannel())) { i0++; continue; }

      geomId = (*hitIt0)->GetGeomId();
      wire0  = COMET::IGeomInfo::Get().CDC().GeomIdToWire(geomId);
      layer0 = COMET::IGeomInfo::Get().CDC().GeomIdToLayer(geomId);

      for (COMET::IHitSelection::const_iterator hitIt1 = hits->begin(); hitIt1 != hits->end(); ++hitIt1) {

        if (i0==i1) { i1++; continue; }

        if (used.at(i0).at(i1) || used.at(i1).at(i0)) { i1++; continue; }

        /// CDC wire
        if (!((*hitIt1)->GetChannelId().IsCDCChannel())) { i1++; continue; }

        geomId = (*hitIt1)->GetGeomId();
        wire1  = COMET::IGeomInfo::Get().CDC().GeomIdToWire(geomId);
        layer1 = COMET::IGeomInfo::Get().CDC().GeomIdToLayer(geomId);

        if (abs(layer0-layer1)!=1) { i1++; continue; }

        dist = DistanceBetweenWires(COMET::IGeomInfo::Get().CDC().GetWireEnd0(wire0),
                                    COMET::IGeomInfo::Get().CDC().GetWireEnd1(wire0),
                                    COMET::IGeomInfo::Get().CDC().GetWireEnd0(wire1),
                                    COMET::IGeomInfo::Get().CDC().GetWireEnd1(wire1),
                                    crossPoint);

        COMET::IGeomInfo::Get().DetectorSolenoid().GetDetPositionInDSCoordinate(crossPoint, local);
        /// Make a combi hit
        if (dist ::iterator hitIt0 = tmpHits.begin(); hitIt0 != tmpHits.end(); ++hitIt0) {
    haveNeighbor = false;
    for (std::vector ::iterator hitIt1 = tmpHits.begin(); hitIt1 != tmpHits.end(); ++hitIt1) {
      if ((*hitIt0)==(*hitIt1)) continue;

      if ( hypot(((*hitIt0).X()-(*hitIt1).X()), (*hitIt0).Y()-(*hitIt1).Y()) < sqrt(2.)*fNeighborR ) {
        haveNeighbor = true;
        break;
      }
    }
    if (haveNeighbor) combiHits.push_back((*hitIt0));
  }

  return true;
}

//======================================================================
// Calculate the distance between two wires
//======================================================================
Double_t IHoughTrackFinder::DistanceBetweenWires(const TVector3& wireAEnd0, const TVector3& wireAEnd1,
                                                 const TVector3& wireBEnd0, const TVector3& wireBEnd1,
                                                 TVector3& crossPoint)
{
  Double_t length = std::min( (wireAEnd1-wireAEnd0).Mag(), (wireBEnd1-wireBEnd0).Mag() );

  TVector3 normA, normB;
  normA = (wireAEnd1-wireAEnd0).Unit();
  normB = (wireBEnd1-wireBEnd0).Unit();

  Double_t scaleA, scaleB;

  if (fabs(normA.Dot(normB))>=1.) return 1e9;

  TVector3 vectAB = wireBEnd0-wireAEnd0;

  scaleA = (vectAB.Dot(normA) - vectAB.Dot(normB) * normA.Dot(normB)) /
           (1. - pow(normA.Dot(normB),2));
  scaleB = (vectAB.Dot(normA) * normA.Dot(normB) - vectAB.Dot(normB)) /
           (1. - pow(normA.Dot(normB),2));

  if (fabs(scaleA)>length || fabs(scaleB)>length) return 1e9;

  TVector3 vectA = wireAEnd0 + scaleA * normA;
  TVector3 vectB = wireBEnd0 + scaleB * normB;

  crossPoint = (vectA+vectB);
  crossPoint *= 0.5;

  //COMETNamedDebug("IHoughTrackFinder", " scaleA =   " << scaleA << ", scaleB =   " << scaleB << "\n"
  //        << " wireAEnd0  " << wireAEnd0.X() << " " << wireAEnd0.Y() << " " << wireAEnd0.Z() << "\n"
  //        << " wireAEnd1  " << wireAEnd1.X() << " " << wireAEnd1.Y() << " " << wireAEnd1.Z() << "\n"
  //        << " wireBEnd0  " << wireBEnd0.X() << " " << wireBEnd0.Y() << " " << wireBEnd0.Z() << "\n"
  //        << " wireBEnd1  " << wireBEnd1.X() << " " << wireBEnd1.Y() << " " << wireBEnd1.Z() << "\n"
  //        << " vectA      " << vectA.X() << " " << vectA.Y() << " " << vectA.Z() << "\n"
  //        << " vectB      " << vectB.X() << " " << vectB.Y() << " " << vectB.Z() << "\n"
  //        << " crossPoint " << crossPoint.X() << " " << crossPoint.Y() << " " << crossPoint.Z() << "\n"
  //        << " distance between wires " << (vectA-vectB).Mag());

  return (vectA-vectB).Mag();
}

//======================================================================
// CircleCombiFitting
//======================================================================
void IHoughTrackFinder::CircleCombiFitting(std::vector< std::vector >& circles)
{
  COMETNamedDebug("IHoughTrackFinder", "CircleCombiFitting");

  const Int_t nParams = 3;
  const std::string parNames[nParams] = {"x0", "y0", "radius"};

  for (UInt_t ic = 0; ic < circles.size(); ic++) {
    ROOT::Math::Minimizer* minimizer = ROOT::Math::Factory::CreateMinimizer("Minuit2", "Migrad");
    /// Set tolerance, etc...
    minimizer->SetMaxFunctionCalls(100000); /// For Minuit/Minuit2
    minimizer->SetTolerance(0.01);
    minimizer->SetPrintLevel(0);

    /// Create function wrapper for minimizer
    /// a IMultiGenFunction type
    ROOT::Math::Functor func(fFunctorCircleCombi, nParams);
    minimizer->SetFunction(func);

    /// Set the free variables to be minimized
    for (Int_t ip = 0; ip < nParams; ip++)
      minimizer->SetLimitedVariable(ip, parNames[ip], circles.at(ic).at(ip), 0.2,
                                    circles.at(ic).at(ip)-2.0, circles.at(ic).at(ip)+2.0);

    /// Do the minimization
    minimizer->Minimize();

    const Double_t *resTmp = minimizer->X();

    minimizer->Clear();

    for (Int_t ip = 0; ip < nParams; ip++) {
      circles.at(ic).at(ip) = resTmp[ip];
      minimizer->SetLimitedVariable(ip, parNames[ip], circles.at(ic).at(ip), 0.2,
                                    circles.at(ic).at(ip)-2.0, circles.at(ic).at(ip)+2.0);
    }

    /// 2nd iteration
    minimizer->Minimize();
    minimizer->PrintResults();

    const Double_t *result = minimizer->X();
    for (Int_t ip = 0; ip < nParams; ip++)
      circles.at(ic).at(ip) = result[ip];

    minimizer->Clear();
  }
  return;
}

//======================================================================
// Do fitting using 2-D circle function
//======================================================================
Bool_t IHoughTrackFinder::CircleFitting(std::vector & circle, std::vector & cerror, const COMET::IHitSelection* hits)
{
  COMETNamedDebug("IHoughTrackFinder", "CircleFitting");
  Bool_t success = false;

  for (COMET::IHitSelection::const_iterator hitIt=hits->begin();hitIt!=hits->end();++hitIt)
    fHitSelection->AddHit((*hitIt));

  const Int_t nParams = 3;
  const std::string parNames[nParams] = {"x0", "y0", "radius"};

  ROOT::Math::Minimizer* minimizer = ROOT::Math::Factory::CreateMinimizer("Minuit2", "Migrad");

  /// Set tolerance, etc...
  minimizer->SetMaxFunctionCalls(1000); /// For Minuit/Minuit2
  minimizer->SetTolerance(0.01);
  minimizer->SetPrintLevel(0);

  /// Create function wrapper for minimizer
  /// a IMultiGenFunction type
  ROOT::Math::Functor func(fFunctorCircle, nParams);
  minimizer->SetFunction(func);

  /// Set the free variables to be minimized
  for (Int_t ip = 0; ip < nParams; ip++)
    minimizer->SetLimitedVariable(ip, parNames[ip], circle.at(ip), 0.02*cerror.at(ip),
                                  circle.at(ip)-0.5, circle.at(ip)+0.5);

  /// Do the minimization
  minimizer->Minimize();
  minimizer->PrintResults();

  /// Get results
  const Double_t *resTmp = minimizer->X();

  for (Int_t ip = 0; ip < nParams; ip++) {
    circle.at(ip) = resTmp[ip];
    minimizer->SetLimitedVariable(ip, parNames[ip], circle.at(ip), 0.02*cerror.at(ip),
                                  circle.at(ip)-0.5, circle.at(ip)+0.5);
  }

  /// 2nd iteration
  minimizer->Minimize();
  minimizer->PrintResults();

  /// Get results
  const Double_t *result = minimizer->X();
  for (Int_t ip = 0; ip < nParams; ip++) {
    COMETNamedDebug("IHoughTrackFinder", parNames[ip] << ": " << result[ip]);
    circle.at(ip) = result[ip];
  }
  success = true;

  minimizer->Clear();
  fHitSelection->clear();
  return success;
}

//======================================================================
// Do fitting using 3-D hexix function
//======================================================================
Bool_t IHoughTrackFinder::HelixFitting(std::vector & circle, std::vector & cerror, const COMET::IHitSelection* hits)
{
  COMETNamedDebug("IHoughTrackFinder", "HelixFitting");
  Bool_t success = false;

  for (COMET::IHitSelection::const_iterator hitIt=hits->begin();hitIt!=hits->end();++hitIt)
    fHitSelection->AddHit((*hitIt));

  if (circle.at(4)<-50.) circle.at(4) = -50.;
  if (circle.at(4)>+50.) circle.at(4) = +50.;

  const Int_t nParams = 6;
  const std::string parNames[nParams] = {"x0", "y0", "radius", "startPhi", "startZ", "dZ/dPhi"};
  std::vector < std::pair > ranges;
  std::pair range0(circle.at(0)-1.0, circle.at(0)+1.0);
  std::pair range1(circle.at(1)-1.0, circle.at(1)+1.0);
  std::pair range2(circle.at(2)-1.0, circle.at(2)+1.0);
  std::pair range3(circle.at(3)-cerror.at(3), circle.at(3)+cerror.at(3));
  std::pair range4(circle.at(4)-cerror.at(4), circle.at(4)+cerror.at(4));
  std::pair