CRackTrajectoryBuilder.cc

Go to the documentation of this file.

//
// Package:         RecoTracker/SingleTrackPattern
// Class:           CRackTrajectoryBuilder
// Original Author:  Michele Pioppi-INFN perugia
//
// Package:         RecoTracker/SingleTrackPattern
// Class:           CRackTrajectoryBuilder
// Original Author:  Michele Pioppi-INFN perugia
 
#include <vector>
#include <iostream>
#include <cmath>
 
#include "RecoTracker/SingleTrackPattern/interface/CRackTrajectoryBuilder.h"
#include "DataFormats/Common/interface/OwnVector.h"
#include "TrackingTools/TransientTrackingRecHit/interface/TransientTrackingRecHit.h"
#include "TrackingTools/TrajectoryState/interface/BasicSingleTrajectoryState.h"
#include "MagneticField/Records/interface/IdealMagneticFieldRecord.h"
#include "FWCore/ParameterSet/interface/ParameterSet.h"
#include "DataFormats/GeometryVector/interface/GlobalPoint.h"
#include "FWCore/MessageLogger/interface/MessageLogger.h"
#include "TrackingTools/Records/interface/TrackingComponentsRecord.h"
#include "TrackingTools/Records/interface/TransientRecHitRecord.h"
#include "TrackingTools/TrackFitters/interface/TrajectoryStateWithArbitraryError.h"
 
//#include "RecoTracker/CkfPattern/interface/TransientInitialStateEstimator.h"
 
using namespace std;
 
CRackTrajectoryBuilder::CRackTrajectoryBuilder(const edm::ParameterSet& conf) {
  //minimum number of hits per tracks
 
  theMinHits = conf.getParameter<int>("MinHits");
  //cut on chi2
  chi2cut = conf.getParameter<double>("Chi2Cut");
  edm::LogInfo("CosmicTrackFinder") << "Minimum number of hits " << theMinHits << " Cut on Chi2= " << chi2cut;
 
  debug_info = conf.getUntrackedParameter<bool>("debug", false);
  fastPropagation = conf.getUntrackedParameter<bool>("fastPropagation", false);
  useMatchedHits = conf.getUntrackedParameter<bool>("useMatchedHits", true);
 
  geometry = conf.getUntrackedParameter<std::string>("GeometricStructure", "STANDARD");
  theBuilderName = conf.getParameter<std::string>("TTRHBuilder");
}
 
CRackTrajectoryBuilder::~CRackTrajectoryBuilder() {
  //  delete theInitialState;
}
 
void CRackTrajectoryBuilder::init(const edm::EventSetup& es, bool seedplus) {
  //  edm::ParameterSet tise_params = conf_.getParameter<edm::ParameterSet>("TransientInitialStateEstimatorParameters") ;
  // theInitialState          = new TransientInitialStateEstimator( es,tise_params);
 
  //services
  es.get<IdealMagneticFieldRecord>().get(magfield);
  es.get<TrackerDigiGeometryRecord>().get(tracker);
 
  if (seedplus) {
    seed_plus = true;
    thePropagator = new PropagatorWithMaterial(alongMomentum, 0.1057, &(*magfield));
    thePropagatorOp = new PropagatorWithMaterial(oppositeToMomentum, 0.1057, &(*magfield));
  } else {
    seed_plus = false;
    thePropagator = new PropagatorWithMaterial(oppositeToMomentum, 0.1057, &(*magfield));
    thePropagatorOp = new PropagatorWithMaterial(alongMomentum, 0.1057, &(*magfield));
  }
 
  theUpdator = new KFUpdator();
  //  theUpdator=       new KFStripUpdator();
  theEstimator = new Chi2MeasurementEstimator(chi2cut);
 
  edm::ESHandle<TransientTrackingRecHitBuilder> theBuilder;
  es.get<TransientRecHitRecord>().get(theBuilderName, theBuilder);
 
  RHBuilder = theBuilder.product();
 
  theFitter = new KFTrajectoryFitter(*thePropagator, *theUpdator, *theEstimator);
 
  theSmoother = new KFTrajectorySmoother(*thePropagatorOp, *theUpdator, *theEstimator);
}
 
void CRackTrajectoryBuilder::run(const TrajectorySeedCollection& collseed,
                                 const SiStripRecHit2DCollection& collstereo,
                                 const SiStripRecHit2DCollection& collrphi,
                                 const SiStripMatchedRecHit2DCollection& collmatched,
                                 const SiPixelRecHitCollection& collpixel,
                                 const edm::EventSetup& es,
                                 edm::Event& e,
                                 vector<Trajectory>& trajoutput) {
  std::vector<Trajectory> trajSmooth;
  std::vector<Trajectory>::iterator trajIter;
 
  TrajectorySeedCollection::const_iterator iseed;
  unsigned int IS = 0;
  for (iseed = collseed.begin(); iseed != collseed.end(); iseed++) {
    bool seedplus = ((*iseed).direction() == alongMomentum);
    init(es, seedplus);
    hits.clear();
    trajFit.clear();
    trajSmooth.clear();
 
    Trajectory startingTraj = createStartingTrajectory(*iseed);
    Trajectory trajTmp;  // used for the opposite direction
    Trajectory traj = startingTraj;
 
    //first propagate track in opposite direction ...
    seed_plus = !seed_plus;
    vector<const TrackingRecHit*> allHitsOppsite = SortHits(collstereo, collrphi, collmatched, collpixel, *iseed, true);
    seed_plus = !seed_plus;
    if (!allHitsOppsite.empty()) {
      //there are hits which are above the seed,
      //cout << "Number of hits higher than seed " <<allHitsOppsite.size() << endl;
 
      AddHit(traj, allHitsOppsite, thePropagatorOp);
 
      if (debug_info) {
        cout << "Hits in opposite direction..." << endl;
        TransientTrackingRecHit::RecHitContainer::const_iterator iHit;
        for (iHit = hits.begin(); iHit != hits.end(); iHit++) {
          cout << (**iHit).globalPosition() << endl;
        }
      }
 
      // now add hist opposite to seed
      //now crate new starting trajectory
      reverse(hits.begin(), hits.end());
      if (thePropagatorOp
              ->propagate(traj.firstMeasurement().updatedState(),
                          tracker->idToDet((hits.front())->geographicalId())->surface())
              .isValid()) {
        TSOS startingStateNew =  //TrajectoryStateWithArbitraryError()//
            (thePropagatorOp->propagate(traj.firstMeasurement().updatedState(),
                                        tracker->idToDet((hits.front())->geographicalId())->surface()));
 
        if (debug_info) {
          cout << "Hits in opposite direction reversed..." << endl;
          TransientTrackingRecHit::RecHitContainer::const_iterator iHit;
          for (iHit = hits.begin(); iHit != hits.end(); iHit++) {
            cout << (**iHit).globalPosition() << endl;
          }
        }
 
        const TrajectorySeed& tmpseed = traj.seed();
        trajTmp = theFitter->fit(tmpseed, hits, startingStateNew).front();
 
        if (debug_info) {
          cout << "Debugging show fitted hits" << endl;
          auto hitsFit = trajTmp.recHits();
          for (auto hit = hitsFit.begin(); hit != hitsFit.end(); hit++) {
            cout << RHBuilder->build(&(*(*hit)->hit()))->globalPosition() << endl;
          }
        }
      }
    } else {
      if (debug_info)
        cout << "There are no hits in opposite direction ..." << endl;
    }
 
    vector<const TrackingRecHit*> allHits;
    if (trajTmp.foundHits()) {
      traj = trajTmp;
      allHits = SortHits(collstereo, collrphi, collmatched, collpixel, *iseed, false);
    } else {
      traj = startingTraj;
      hits.clear();
      allHits = SortHits(collstereo, collrphi, collmatched, collpixel, *iseed, true);
    }
 
    AddHit(traj, allHits, thePropagator);
 
    if (debug_info) {
      cout << "Debugging show All fitted hits" << endl;
      auto hits = traj.recHits();
      for (auto hit = hits.begin(); hit != hits.end(); hit++) {
        cout << (*hit)->globalPosition() << endl;
      }
 
      cout << qualityFilter(traj) << " <- quality filter good?" << endl;
    }
 
    if (debug_info)
      cout << "now do quality checks" << endl;
    if (qualityFilter(traj)) {
      const TrajectorySeed& tmpseed = traj.seed();
      std::pair<TrajectoryStateOnSurface, const GeomDet*> initState =
          innerState(traj);  //theInitialState->innerState(traj);
      if (initState.first.isValid())
        trajFit = theFitter->fit(tmpseed, hits, initState.first);
    }
 
    for (trajIter = trajFit.begin(); trajIter != trajFit.end(); trajIter++) {
      trajSmooth = theSmoother->trajectories((*trajIter));
    }
    for (trajIter = trajSmooth.begin(); trajIter != trajSmooth.end(); trajIter++) {
      if ((*trajIter).isValid()) {
        if (debug_info)
          cout << "adding track ... " << endl;
        trajoutput.push_back((*trajIter));
      }
    }
    delete theUpdator;
    delete theEstimator;
    delete thePropagator;
    delete thePropagatorOp;
    delete theFitter;
    delete theSmoother;
    //Only the first 30 seeds are considered
    if (IS > 30)
      return;
    IS++;
  }
}
 
Trajectory CRackTrajectoryBuilder::createStartingTrajectory(const TrajectorySeed& seed) const {
  Trajectory result(seed, seed.direction());
  std::vector<TM>&& seedMeas = seedMeasurements(seed);
  for (auto i : seedMeas)
    result.push(std::move(i));
  return result;
}
 
std::vector<TrajectoryMeasurement> CRackTrajectoryBuilder::seedMeasurements(const TrajectorySeed& seed) const {
  std::vector<TrajectoryMeasurement> result;
  auto const& hitRange = seed.recHits();
  for (auto ihit = hitRange.begin(); ihit != hitRange.end(); ihit++) {
    //RC TransientTrackingRecHit* recHit = RHBuilder->build(&(*ihit));
    TransientTrackingRecHit::RecHitPointer recHit = RHBuilder->build(&(*ihit));
    const GeomDet* hitGeomDet = (&(*tracker))->idToDet(ihit->geographicalId());
    TSOS invalidState(new BasicSingleTrajectoryState(hitGeomDet->surface()));
 
    if (ihit == hitRange.end() - 1) {
      TSOS updatedState = startingTSOS(seed);
      result.emplace_back(invalidState, updatedState, recHit);
 
    } else {
      result.emplace_back(invalidState, recHit);
    }
  }
 
  return result;
}
 
vector<const TrackingRecHit*> CRackTrajectoryBuilder::SortHits(const SiStripRecHit2DCollection& collstereo,
                                                               const SiStripRecHit2DCollection& collrphi,
                                                               const SiStripMatchedRecHit2DCollection& collmatched,
                                                               const SiPixelRecHitCollection& collpixel,
                                                               const TrajectorySeed& seed,
                                                               const bool bAddSeedHits) {
  //The Hits with global y more than the seed are discarded
  //The Hits correspondign to the seed are discarded
  //At the end all the hits are sorted in y
  vector<const TrackingRecHit*> allHits;
 
  SiStripRecHit2DCollection::DataContainer::const_iterator istrip;
  float yref = 0.;
 
  if (debug_info)
    cout << "SEED " << startingTSOS(seed) << endl;
  if (debug_info)
    cout << "seed hits size " << seed.nHits() << endl;
 
  //seed debugging:
  // GeomDet *seedDet =  tracker->idToDet(seed.startingState().detId());
 
  //  edm::LogInfo("CRackTrajectoryBuilder::SortHits") << "SEED " << seed.startingState();
 
  edm::LogInfo("CRackTrajectoryBuilder::SortHits") << "SEED " << startingTSOS(seed);
  //  edm::LogInfo("CRackTrajectoryBuilder::SortHits" << "seed hits size " << seed.nHits();
 
  //  if (seed.nHits()<2)
  //  return allHits;
 
  float_t yMin = 0.;
  float_t yMax = 0.;
 
  int seedHitSize = seed.nHits();
 
  vector<int> detIDSeedMatched(seedHitSize);
  vector<int> detIDSeedRphi(seedHitSize);
  vector<int> detIDSeedStereo(seedHitSize);
 
  auto const& hRange = seed.recHits();
  for (auto ihit = hRange.begin(); ihit != hRange.end(); ihit++) {
    // need to find track with lowest (seed_plus)/ highest y (seed_minus)
    // split matched hits ...
    const SiStripMatchedRecHit2D* matchedhit = dynamic_cast<const SiStripMatchedRecHit2D*>(&(*ihit));
 
    yref = RHBuilder->build(&(*ihit))->globalPosition().y();
    if (ihit == hRange.begin()) {
      yMin = yref;
      yMax = yref;
    }
 
    if (matchedhit) {
      auto m = matchedhit->monoHit();
      auto s = matchedhit->stereoHit();
      float_t yGlobRPhi = RHBuilder->build(&m)->globalPosition().y();
      float_t yGlobStereo = RHBuilder->build(&s)->globalPosition().y();
 
      if (debug_info)
        cout << "Rphi ..." << yGlobRPhi << endl;
      if (debug_info)
        cout << "Stereo ..." << yGlobStereo << endl;
 
      if (yGlobStereo < yMin)
        yMin = yGlobStereo;
      if (yGlobRPhi < yMin)
        yMin = yGlobRPhi;
 
      if (yGlobStereo > yMax)
        yMax = yGlobStereo;
      if (yGlobRPhi > yMax)
        yMax = yGlobRPhi;
 
      detIDSeedMatched.push_back(matchedhit->geographicalId().rawId());
      detIDSeedRphi.push_back(m.geographicalId().rawId());
      detIDSeedStereo.push_back(s.geographicalId().rawId());
 
      if (bAddSeedHits) {
        if (useMatchedHits) {
          hits.push_back((RHBuilder->build(&(*ihit))));
        } else {
          if (((yGlobRPhi > yGlobStereo) && seed_plus) || ((yGlobRPhi < yGlobStereo) && !seed_plus)) {
            hits.push_back((RHBuilder->build(&m)));
            hits.push_back((RHBuilder->build(&s)));
          } else {
            hits.push_back((RHBuilder->build(&s)));
            hits.push_back((RHBuilder->build(&m)));
          }
        }
      }
    } else if (bAddSeedHits) {
      hits.push_back((RHBuilder->build(&(*ihit))));
      detIDSeedRphi.push_back(ihit->geographicalId().rawId());
      detIDSeedMatched.push_back(-1);
      detIDSeedStereo.push_back(-1);
    }
 
    if (yref < yMin)
      yMin = yref;
    if (yref > yMax)
      yMax = yref;
 
    //    if (bAddSeedHits)
    //      hits.push_back((RHBuilder->build(&(*ihit))));
 
    LogDebug("CosmicTrackFinder") << "SEED HITS" << RHBuilder->build(&(*ihit))->globalPosition();
    if (debug_info)
      cout << "SEED HITS" << RHBuilder->build(&(*ihit))->globalPosition() << endl;
    //    if (debug_info) cout <<"SEED HITS"<< seed.startingState().parameters() << endl;
  }
 
  yref = (seed_plus) ? yMin : yMax;
 
  SiPixelRecHitCollection::DataContainer::const_iterator ipix;
  for (ipix = collpixel.data().begin(); ipix != collpixel.data().end(); ipix++) {
    float ych = RHBuilder->build(&(*ipix))->globalPosition().y();
    if ((seed_plus && (ych < yref)) || (!(seed_plus) && (ych > yref)))
      allHits.push_back(&(*ipix));
  }
 
  if (useMatchedHits)  // use matched
  {
    //add the matched hits ...
    SiStripMatchedRecHit2DCollection::DataContainer::const_iterator istripm;
 
    for (istripm = collmatched.data().begin(); istripm != collmatched.data().end(); istripm++) {
      float ych = RHBuilder->build(&(*istripm))->globalPosition().y();
 
      int cDetId = istripm->geographicalId().rawId();
      bool noSeedDet = (detIDSeedMatched.end() == find(detIDSeedMatched.begin(), detIDSeedMatched.end(), cDetId));
 
      if (noSeedDet)
        if ((seed_plus && (ych < yref)) || (!(seed_plus) && (ych > yref)))
          allHits.push_back(&(*istripm));
    }
 
    //add the rpi hits, but only accept hits that are not matched hits
    for (istrip = collrphi.data().begin(); istrip != collrphi.data().end(); istrip++) {
      float ych = RHBuilder->build(&(*istrip))->globalPosition().y();
      StripSubdetector monoDetId(istrip->geographicalId());
      if (monoDetId.partnerDetId()) {
        edm::LogInfo("CRackTrajectoryBuilder::SortHits") << "this det belongs to a glued det " << ych << endl;
        continue;
      }
      int cDetId = istrip->geographicalId().rawId();
      bool noSeedDet = (detIDSeedRphi.end() == find(detIDSeedRphi.begin(), detIDSeedRphi.end(), cDetId));
      if (noSeedDet)
        if ((seed_plus && (ych < yref)) || (!(seed_plus) && (ych > yref))) {
          bool hitIsUnique = true;
          //now
          for (istripm = collmatched.data().begin(); istripm != collmatched.data().end(); istripm++) {
            //       if ( isDifferentStripReHit2D ( *istrip, (istripm->stereoHit() ) ) == false)
            if (isDifferentStripReHit2D(*istrip, (istripm->monoHit())) == false) {
              hitIsUnique = false;
              edm::LogInfo("CRackTrajectoryBuilder::SortHits") << "rphi hit is in matched hits; y: " << ych << endl;
              break;
            }
          }  //end loop over all matched
          if (hitIsUnique) {
            //      if (debug_info) cout << "adding rphi hit " << &(*istrip) << endl;
            allHits.push_back(&(*istrip));
          }
        }
    }
 
  } else  // dont use matched ...
  {
    for (istrip = collrphi.data().begin(); istrip != collrphi.data().end(); istrip++) {
      float ych = RHBuilder->build(&(*istrip))->globalPosition().y();
      if ((seed_plus && (ych < yref)) || (!(seed_plus) && (ych > yref)))
        allHits.push_back(&(*istrip));
    }
 
    for (istrip = collstereo.data().begin(); istrip != collstereo.data().end(); istrip++) {
      float ych = RHBuilder->build(&(*istrip))->globalPosition().y();
      if ((seed_plus && (ych < yref)) || (!(seed_plus) && (ych > yref)))
        allHits.push_back(&(*istrip));
    }
  }
 
  if (seed_plus)
    stable_sort(allHits.begin(), allHits.end(), CompareHitY_plus(*tracker));
  else
    stable_sort(allHits.begin(), allHits.end(), CompareHitY(*tracker));
 
  if (debug_info) {
    if (debug_info)
      cout << "all hits" << endl;
 
    //starting trajectory
    Trajectory startingTraj = createStartingTrajectory(seed);
 
    if (debug_info)
      cout << "START " << startingTraj.lastMeasurement().updatedState() << endl;
    if (debug_info)
      cout << "START  Err" << startingTraj.lastMeasurement().updatedState().localError().matrix() << endl;
 
    vector<const TrackingRecHit*>::iterator iHit;
    for (iHit = allHits.begin(); iHit < allHits.end(); iHit++) {
      GlobalPoint gphit = RHBuilder->build(*iHit)->globalPosition();
      if (debug_info)
        cout << "GH " << gphit << endl;
 
      //      tracker->idToDet((*iHit)->geographicalId())->surface();
 
      TSOS prSt = thePropagator->propagate(startingTraj.lastMeasurement().updatedState(),
                                           tracker->idToDet((*iHit)->geographicalId())->surface());
 
      if (prSt.isValid()) {
        if (debug_info)
          cout << "PR " << prSt.globalPosition() << endl;
        //if (debug_info) cout << "PR  Err" << prSt.localError().matrix() << endl;
 
      } else {
        if (debug_info)
          cout << "not valid" << endl;
      }
    }
    if (debug_info)
      cout << "all hits end" << endl;
  }
 
  return allHits;
}
 
TrajectoryStateOnSurface CRackTrajectoryBuilder::startingTSOS(const TrajectorySeed& seed) const {
  PTrajectoryStateOnDet pState(seed.startingState());
  const GeomDet* gdet = (&(*tracker))->idToDet(DetId(pState.detId()));
  TSOS State = trajectoryStateTransform::transientState(pState, &(gdet->surface()), &(*magfield));
  return State;
}
 
void CRackTrajectoryBuilder::AddHit(Trajectory& traj,
                                    const vector<const TrackingRecHit*>& _Hits,
                                    Propagator* currPropagator) {
  vector<const TrackingRecHit*> Hits = _Hits;
  if (Hits.empty())
    return;
 
  if (debug_info)
    cout << "CRackTrajectoryBuilder::AddHit" << endl;
  if (debug_info)
    cout << "START " << traj.lastMeasurement().updatedState() << endl;
 
  vector<TrackingRecHitRange> hitRangeByDet;
  TrackingRecHitIterator prevDet;
 
  prevDet = Hits.begin();
  for (TrackingRecHitIterator iHit = Hits.begin(); iHit != Hits.end(); iHit++) {
    if ((*prevDet)->geographicalId() == (*iHit)->geographicalId())
      continue;
 
    hitRangeByDet.push_back(make_pair(prevDet, iHit));
    prevDet = iHit;
  }
  hitRangeByDet.push_back(make_pair(prevDet, Hits.end()));
 
 
  if (fastPropagation) {
    for (TrackingRecHitRangeIterator iHitRange = hitRangeByDet.begin(); iHitRange != hitRangeByDet.end(); iHitRange++) {
      const TrackingRecHit* currHit = *(iHitRange->first);
      DetId currDet = currHit->geographicalId();
 
      TSOS prSt =
          currPropagator->propagate(traj.lastMeasurement().updatedState(), tracker->idToDet(currDet)->surface());
 
      if (!prSt.isValid()) {
        if (debug_info)
          cout << "Not Valid: PRST" << prSt.globalPosition();
        //        if (debug_info) cout << "Not Valid: HIT" << *currHit;
 
        continue;
      }
 
      TransientTrackingRecHit::RecHitPointer bestHit = RHBuilder->build(currHit);
      double chi2min = theEstimator->estimate(prSt, *bestHit).second;
 
      if (debug_info)
        cout << "Size " << iHitRange->first - (*iHitRange).second << endl;
      for (TrackingRecHitIterator iHit = (*iHitRange).first + 1; iHit != iHitRange->second; iHit++) {
        if (debug_info)
          cout << "loop3 "
               << " " << Hits.end() - iHit << endl;
 
        TransientTrackingRecHit::RecHitPointer tmpHit = RHBuilder->build(*iHit);
        double currChi2 = theEstimator->estimate(prSt, *tmpHit).second;
        if (currChi2 < chi2min) {
          chi2min = currChi2;
          bestHit = tmpHit;
        }
      }
      //now we have check if the track can be added to the trajectory
      if (debug_info)
        cout << chi2min << endl;
      if (chi2min < chi2cut) {
        if (debug_info)
          cout << "chi2 fine : " << chi2min << endl;
        TSOS UpdatedState = theUpdator->update(prSt, *bestHit);
        if (UpdatedState.isValid()) {
          hits.push_back(bestHit);
          traj.push(TM(prSt, UpdatedState, bestHit, chi2min));
          if (debug_info)
            edm::LogInfo("CosmicTrackFinder") << "STATE UPDATED WITH HIT AT POSITION "
 
                                              << bestHit->globalPosition() << UpdatedState << " " << traj.chiSquared();
          if (debug_info)
            cout << "STATE UPDATED WITH HIT AT POSITION "
 
                 << bestHit->globalPosition() << UpdatedState << " " << traj.chiSquared();
          if (debug_info)
            cout << "State is valid ..." << endl;
          break;  // now we need to
        } else {
          edm::LogWarning("CosmicTrackFinder") << " State can not be updated with hit at position " << endl;
          TSOS UpdatedState = theUpdator->update(prSt, *bestHit);
          if (UpdatedState.isValid()) {
            cout << "NOT! UPDATED WITH HIT AT POSITION "
 
                 << bestHit->globalPosition() << UpdatedState << " " << traj.chiSquared();
          }
        }
      }
    }
  }  //simple version end
  else {
    //first sort the dets in the order they are traversed by the trajectory
    // we need three loops:
    // 1: loop as long as there can be an new hit added to the trajectory
    // 2: loop over all dets that might be hit
    // 3: loop over all hits on a certain det
 
    std::vector<std::pair<TrackingRecHitRangeIterator, TSOS> > trackHitCandidates;
    std::vector<std::pair<TrackingRecHitRangeIterator, TSOS> >::iterator iHitRange;
    std::vector<uint32_t> processedDets;
    do {
      //create vector of possibly hit detectors...
      trackHitCandidates.clear();
      DetId currDet;
      for (TrackingRecHitRangeIterator iHit = hitRangeByDet.begin(); iHit != hitRangeByDet.end(); iHit++) {
        const TrackingRecHit* currHit = *(iHit->first);
        currDet = currHit->geographicalId();
 
        if (find(processedDets.begin(), processedDets.end(), currDet.rawId()) != processedDets.end())
          continue;
 
        TSOS prSt =
            currPropagator->propagate(traj.lastMeasurement().updatedState(), tracker->idToDet(currDet)->surface());
        if ((!prSt.isValid()) ||
            (theEstimator->Chi2MeasurementEstimatorBase::estimate(prSt, tracker->idToDet(currDet)->surface()) == false))
          //          if ( ( !prSt.isValid() ) ||  (theEstimator->estimate(prSt,tracker->idToDet(currDet)->surface() ) == false) )
          continue;
 
        trackHitCandidates.push_back(make_pair(iHit, prSt));
      }
 
      if (trackHitCandidates.empty())
        break;
 
      if (debug_info)
        cout << Hits.size() << " (int) trackHitCandidates.begin() " << trackHitCandidates.size() << endl;
      if (debug_info)
        cout << "Before sorting ... " << endl;
 
      if (debug_info)
        for (iHitRange = trackHitCandidates.begin(); iHitRange != trackHitCandidates.end(); iHitRange++) {
          if (debug_info)
            cout << (tracker->idToDet((*(iHitRange->first->first))->geographicalId()))->position();
        }
      if (debug_info)
        cout << endl;
 
      stable_sort(trackHitCandidates.begin(),
                  trackHitCandidates.end(),
                  CompareDetByTraj(traj.lastMeasurement().updatedState()));
 
      if (debug_info)
        cout << "After sorting ... " << endl;
      if (debug_info) {
        for (iHitRange = trackHitCandidates.begin(); iHitRange != trackHitCandidates.end(); iHitRange++) {
          if (debug_info)
            cout << (tracker->idToDet((*(iHitRange->first->first))->geographicalId()))->position();
        }
        cout << endl;
      }
 
      for (iHitRange = trackHitCandidates.begin(); iHitRange != trackHitCandidates.end(); iHitRange++)  //loop over dets
      {
        //now find the best hit of the detector
        if (debug_info)
          cout << "loop2 " << trackHitCandidates.size() << " " << trackHitCandidates.end() - iHitRange << endl;
        const TrackingRecHit* currHit = *(iHitRange->first->first);
 
        TransientTrackingRecHit::RecHitPointer bestHit = RHBuilder->build(currHit);
        TSOS currPrSt = (*iHitRange).second;
 
        if (debug_info)
          cout << "curr position" << bestHit->globalPosition();
        for (TrackingRecHitIterator iHit = (*iHitRange).first->first + 1; iHit != iHitRange->first->second; iHit++) {
          TransientTrackingRecHit::RecHitPointer tmpHit = RHBuilder->build(*iHit);
          if (debug_info)
            cout << "curr position" << tmpHit->globalPosition();
        }
      }
      if (debug_info)
        cout << "Cross check end ..." << endl;
 
      //just a simple test if the same hit can be added twice ...
      //      for( iHitRange = trackHitCandidates.begin(); iHitRange != trackHitCandidates.end(); iHitRange++ )      //loop over all hits
 
      //      break;
 
      for (iHitRange = trackHitCandidates.begin(); iHitRange != trackHitCandidates.end();
           iHitRange++)  //loop over detsall hits
      {
        //now find the best hit of the detector
        if (debug_info)
          cout << "loop2 " << trackHitCandidates.size() << " " << trackHitCandidates.end() - iHitRange << endl;
 
        const TrackingRecHit* currHit = *(iHitRange->first->first);
 
        processedDets.push_back(currHit->geographicalId().rawId());
 
        TransientTrackingRecHit::RecHitPointer bestHit = RHBuilder->build(currHit);
 
        if (debug_info)
          cout << "curr position A" << bestHit->globalPosition() << endl;
        TSOS currPrSt = (*iHitRange).second;
        double chi2min = theEstimator->estimate(currPrSt, *bestHit).second;
 
        if (debug_info)
          cout << "Size " << iHitRange->first->second - (*iHitRange).first->first << endl;
        for (TrackingRecHitIterator iHit = (*iHitRange).first->first + 1; iHit != iHitRange->first->second; iHit++) {
          if (debug_info)
            cout << "loop3 "
                 << " " << Hits.end() - iHit << endl;
 
          TransientTrackingRecHit::RecHitPointer tmpHit = RHBuilder->build(*iHit);
          if (debug_info)
            cout << "curr position B" << tmpHit->globalPosition() << endl;
          double currChi2 = theEstimator->estimate(currPrSt, *tmpHit).second;
          if (currChi2 < chi2min) {
            if (debug_info)
              cout << "Is best hit" << endl;
            chi2min = currChi2;
            bestHit = tmpHit;
          }
        }
        //now we have checked the det and can remove the entry from the vector...
 
        //if (debug_info) cout << "before erase ..." << endl;
        //this is to slow ...
        // hitRangeByDet.erase( (*iHitRange).first,(*iHitRange).first+1 );
        //if (debug_info) cout << "after erase ..." << endl;
 
        if (debug_info)
          cout << chi2min << endl;
        //if the track can be added to the trajectory
        if (chi2min < chi2cut) {
          if (debug_info)
            cout << "chi2 fine : " << chi2min << endl;
 
          //  if (debug_info) cout << "previaous state " << traj.lastMeasurement().updatedState() <<endl;
          TSOS UpdatedState = theUpdator->update(currPrSt, *bestHit);
          if (UpdatedState.isValid()) {
            hits.push_back(bestHit);
            traj.push(TM(currPrSt, UpdatedState, bestHit, chi2min));
            if (debug_info)
              edm::LogInfo("CosmicTrackFinder") << "STATE UPDATED WITH HIT AT POSITION "
                                                //   <<tmphitbestdet->globalPosition()
                                                << UpdatedState << " " << traj.chiSquared();
            if (debug_info)
              cout << "Added Hit" << bestHit->globalPosition() << endl;
            if (debug_info)
              cout << "State is valid ..." << UpdatedState << endl;
            //cout << "updated state " << traj.lastMeasurement().updatedState() <<endl;
 
            //        return; //break;
            //
            //        TSOS prSt= currPropagator->propagate(traj.lastMeasurement().updatedState(),
            //                        tracker->idToDet( bestHit->geographicalId() )->surface());
            //
            //        if ( prSt.isValid())
            //      cout << "the same hit can be added twice ..." << endl;
            //
 
            break;
          } else {
            if (debug_info)
              edm::LogWarning("CosmicTrackFinder")
                  << " State can not be updated with hit at position " << bestHit->globalPosition();
            //      cout << "State can not be updated with hit at " << bestHit->globalPosition() << endl;
          }
          continue;
        } else {
          //      cout << "chi2 to big : " << chi2min << endl;
        }
        if (debug_info)
          cout << " continue 1 " << endl;
      }
      //now we remove all already processed dets from the list ...
      // hitRangeByDet.erase( (*trackHitCandidates.begin()).first,(*iHitRange).first+1 );
 
      if (debug_info)
        cout << " continue 2 " << endl;
    }
    //if this was the last exit
    while (iHitRange != trackHitCandidates.end());
  }
}
 
bool CRackTrajectoryBuilder::qualityFilter(const Trajectory& traj) {
  int ngoodhits = 0;
  if (geometry == "MTCC") {
    auto hits = traj.recHits();
    for (auto hit = hits.begin(); hit != hits.end(); hit++) {
      unsigned int iid = (*hit)->hit()->geographicalId().rawId();
      //CHECK FOR 3 hits r-phi
      if (((iid >> 0) & 0x3) != 1)
        ngoodhits++;
    }
  } else
    ngoodhits = traj.foundHits();
 
  if (ngoodhits >= theMinHits) {
    return true;
  } else {
    return false;
  }
}
 
//----------------------------------------------------------------------------------------------------------
// little helper function that returns false if both hits conatin the same information
 
bool CRackTrajectoryBuilder::isDifferentStripReHit2D(const SiStripRecHit2D& hitA, const SiStripRecHit2D& hitB) {
  if (hitA.geographicalId() != hitB.geographicalId())
    return true;
  if (hitA.localPosition().x() != hitB.localPosition().x())
    return true;
  if (hitA.localPosition().y() != hitB.localPosition().y())
    return true;
  if (hitA.localPosition().z() != hitB.localPosition().z())
    return true;
 
  //  if (debug_info) cout << hitA.localPosition() << endl;
  //  if (debug_info) cout << hitB << endl;
 
  return false;
}
 
//----backfitting
std::pair<TrajectoryStateOnSurface, const GeomDet*> CRackTrajectoryBuilder::innerState(const Trajectory& traj) const {
  int lastFitted = 999;
  int nhits = traj.foundHits();
  if (nhits < lastFitted + 1)
    lastFitted = nhits - 1;
 
  std::vector<TrajectoryMeasurement> measvec = traj.measurements();
  TransientTrackingRecHit::ConstRecHitContainer firstHits;
 
  bool foundLast = false;
  int actualLast = -99;
  for (int i = lastFitted; i >= 0; i--) {
    if (measvec[i].recHit()->isValid()) {
      if (!foundLast) {
        actualLast = i;
        foundLast = true;
      }
      firstHits.push_back(measvec[i].recHit());
    }
  }
  TSOS unscaledState = measvec[actualLast].updatedState();
  AlgebraicSymMatrix55 C = ROOT::Math::SMatrixIdentity();
  // C *= 100.;
 
  TSOS startingState(unscaledState.localParameters(),
                     LocalTrajectoryError(C),
                     unscaledState.surface(),
                     thePropagator->magneticField());
 
  // cout << endl << "FitTester starts with state " << startingState << endl;
 
  KFTrajectoryFitter backFitter(*thePropagator, KFUpdator(), Chi2MeasurementEstimator(100., 3));
 
  PropagationDirection backFitDirection = traj.direction() == alongMomentum ? oppositeToMomentum : alongMomentum;
 
  // only direction matters in this contest
  TrajectorySeed fakeSeed = TrajectorySeed(PTrajectoryStateOnDet(), edm::OwnVector<TrackingRecHit>(), backFitDirection);
 
  vector<Trajectory> fitres = backFitter.fit(fakeSeed, firstHits, startingState);
 
  if (fitres.size() != 1) {
    // cout << "FitTester: first hits fit failed!" << endl;
    return std::pair<TrajectoryStateOnSurface, const GeomDet*>();
  }
 
  TrajectoryMeasurement firstMeas = fitres[0].lastMeasurement();
  const TSOS& firstState = firstMeas.updatedState();
 
  //  cout << "FitTester: Fitted first state " << firstState << endl;
  //cout << "FitTester: chi2 = " << fitres[0].chiSquared() << endl;
 
  TSOS initialState(
      firstState.localParameters(), LocalTrajectoryError(C), firstState.surface(), thePropagator->magneticField());
 
  return std::pair<TrajectoryStateOnSurface, const GeomDet*>(initialState, firstMeas.recHit()->det());
}