FullLDCTrackingAlg.cpp

        dz =     sqrt(trkhit.getCovMatrix()[5]);
      }
      
      // now that the hit type has been established carry on and create a
      
      TrackerHitExtended * hitExt = new TrackerHitExtended( trkhit );
      
      // SJA:FIXME: just use planar res for now
      hitExt->setResolutionRPhi(drphi);
      hitExt->setResolutionZ(dz);
      
      // set type is now only used in one place where it is set to 0 to reject hits from a fit, set to INT_MAX to try and catch any missuse
      hitExt->setType(int(INT_MAX));
      // det is no longer used set to INT_MAX to try and catch any missuse
      hitExt->setDet(int(INT_MAX));
      
      _allSETHits.push_back( hitExt );
      mapTrackerHits[trkhit] = hitExt;
      
      
      double pos[3];
      
      for (int i=0; i<3; ++i) {
        pos[i] = trkhit.getPosition()[i];
      }
      
      debug() << " SET Hit " <<  trkhit.id() << " added : @ " << pos[0] << " " << pos[1] << " " << pos[2] << " drphi " << hitExt->getResolutionRPhi() << " dz " << hitExt->getResolutionZ() << "  layer = " << layer << endmsg;
    }
  }
    
  // Reading VTX Hits
  const edm4hep::TrackerHitCollection* hitVTXCol = nullptr;
  try {
    hitVTXCol = _VTXTrackerHitColHdl.get();
  }
  catch ( GaudiException &e ) {
    debug() << "Collection " << _VTXTrackerHitColHdl.fullKey() << " is unavailable in event " << _nEvt << endmsg;
  }

  if(hitVTXCol){
    int nelem = hitVTXCol->size();
    debug() << "Number of VTX hits = " << nelem << endmsg;
    for(edm4hep::ConstTrackerHit trkhit : *hitVTXCol){
      // FIXME tracker hit plane type of the hit
      TrackerHitExtended* hitExt = new TrackerHitExtended(trkhit);
      
      // SJA:FIXME: just use planar res for now
      // FIXME should use the correct resolution
      // hitExt->setResolutionRPhi(trkhit->getdU());
      // hitExt->setResolutionZ(trkhit->getdV());
      hitExt->setResolutionRPhi(trkhit.getCovMatrix()[2]);
      hitExt->setResolutionZ(trkhit.getCovMatrix()[5]);

      // type and det are no longer used, set to INT_MAX to try and catch any missuse
      hitExt->setType(int(INT_MAX));      
      hitExt->setDet(int(INT_MAX));
      _allVTXHits.push_back( hitExt );
      mapTrackerHits[trkhit] = hitExt;
      
      double pos[3];
      
      for (int i=0; i<3; ++i) {
        pos[i] = trkhit.getPosition()[i];
      }
      
      int layer = getLayerID(trkhit);
      
      debug() << " VXD Hit " <<  trkhit.id() << " added : @ " << pos[0] << " " << pos[1] << " " << pos[2] << " drphi " << hitExt->getResolutionRPhi() << " dz " << hitExt->getResolutionZ() << "  layer = " << layer << endmsg;
    }
  }
    
  // Reading TPC Tracks
  const edm4hep::TrackCollection* tpcTrackCol = nullptr;
  try {
    tpcTrackCol = _TPCTrackColHdl.get();
  }
  catch ( GaudiException &e ) {
    debug() << "Collection " << _TPCTrackColHdl.fullKey() << " is unavailable in event " << _nEvt << endmsg;
  }
  if(tpcTrackCol){
    int nelem = tpcTrackCol->size();
    debug() << "Number of TPC Tracks = " << nelem << endmsg;
    debug() << " Trk    ID        p          D0         Z0       Px       Py       Pz   ntpc ndf  Chi2/ndf nlinkedTracks" << endmsg;
    int iTrk = -1;
    for(edm4hep::ConstTrack tpcTrack : *tpcTrackCol){
      iTrk++;
      ConstTrackerHitVec hitVec(tpcTrack.trackerHits_begin(), tpcTrack.trackerHits_end());
      int nHits = int(hitVec.size());
      
      debug() << toString( iTrk, tpcTrack ,  _bField ) << endmsg;
      
      TrackExtended * trackExt = new TrackExtended( tpcTrack );

      trackExt->setOmega(getOmega(tpcTrack));
      trackExt->setTanLambda(getTanLambda(tpcTrack));
      trackExt->setPhi(getPhi(tpcTrack));
      trackExt->setD0(getD0(tpcTrack));
      trackExt->setZ0(getZ0(tpcTrack));
      float cov[15];
      float param[5];
      //      float reso[4];
      param[0] = getOmega(tpcTrack);
      param[1] = getTanLambda(tpcTrack);
      param[2] = getPhi(tpcTrack);
      param[3] = getD0(tpcTrack);
      param[4] = getZ0(tpcTrack);
      
      std::array<float, 15> Cov = getCovMatrix(tpcTrack);
      int NC = int(Cov.size());
      for (int ic=0;ic<NC;ic++) {
        cov[ic] =  Cov[ic];
      }
      
      trackExt->setCovMatrix(cov);
      trackExt->setNDF(tpcTrack.getNdf());
      trackExt->setChi2(tpcTrack.getChi2());            
      for (int iHit=0;iHit<nHits;++iHit) {
	edm4hep::ConstTrackerHit hit = hitVec[iHit];
        TrackerHitExtended * hitExt = mapTrackerHits[hit];
        hitExt->setTrackExtended( trackExt );
        trackExt->addTrackerHitExtended( hitExt );      
      }      
      _allTPCTracks.push_back( trackExt );                
    }      
  }

  // Reading Si Tracks
  const edm4hep::TrackCollection* siTrackCol = nullptr;
  try {
    siTrackCol = _SiTrackColHdl.get();
  }
  catch ( GaudiException &e ) {
    debug() << "Collection " << _SiTrackColHdl.fullKey() << " is unavailable in event " << _nEvt << endmsg;
  }
  if(siTrackCol){
    int nelem = siTrackCol->size();
    debug() << "Number of Si Tracks = " << nelem << endmsg;
    debug() << " Trk    ID        p          D0         Z0       Px       Py       Pz   ntpc ndf  Chi2/ndf nlinkedTracks" << endmsg;
    int iTrk = -1;
    for(edm4hep::ConstTrack siTrack : *siTrackCol){
      iTrk++;
      double prob = ( siTrack.getNdf() > 0 ? gsl_cdf_chisq_Q(  siTrack.getChi2() ,  (double) siTrack.getNdf() )  : 0. ) ;
      if( prob < _minChi2ProbForSiliconTracks ) {
        debug() << "Si Tracks " << siTrack << " id : " << siTrack.id() << " rejected with prob " << prob << " < " << _minChi2ProbForSiliconTracks << endmsg;
        continue;
      }
      
      TrackExtended * trackExt = new TrackExtended( siTrack );
      ConstTrackerHitVec hitVec(siTrack.trackerHits_begin(), siTrack.trackerHits_end());
      int nHits = int(hitVec.size());
      trackExt->setOmega(getOmega(siTrack));
      trackExt->setTanLambda(getTanLambda(siTrack));
      trackExt->setPhi(getPhi(siTrack));
      trackExt->setD0(getD0(siTrack));
      trackExt->setZ0(getZ0(siTrack));
      float cov[15];
      float param[5];
      
      param[0] = getOmega(siTrack);
      param[1] = getTanLambda(siTrack);
      param[2] = getPhi(siTrack);
      param[3] = getD0(siTrack);
      param[4] = getZ0(siTrack);      
            
      std::array<float, 15> Cov = getCovMatrix(siTrack);
      int NC = int(Cov.size());
      for (int ic=0;ic<NC;ic++) {
        cov[ic] =  Cov[ic];
      } 
      trackExt->setCovMatrix(cov);
      trackExt->setNDF(siTrack.getNdf());
      trackExt->setChi2(siTrack.getChi2());      
      char strg[200];
      HelixClass helixSi;
      for (int iHit=0;iHit<nHits;++iHit) {
	edm4hep::ConstTrackerHit hit = hitVec[iHit];
        TrackerHitExtended * hitExt = mapTrackerHits[hit];
        hitExt->setTrackExtended( trackExt );
        
        trackExt->addTrackerHitExtended( hitExt );      
      }
      
      float d0Si = trackExt->getD0();
      float z0Si = trackExt->getZ0();
      float omegaSi = trackExt->getOmega();
      float phi0Si = trackExt->getPhi();
      float tanLSi = trackExt->getTanLambda();
      helixSi.Initialize_Canonical(phi0Si,d0Si,z0Si,omegaSi,tanLSi,_bField);
      float pxSi = helixSi.getMomentum()[0];
      float pySi = helixSi.getMomentum()[1];
      float pzSi = helixSi.getMomentum()[2];
      const float pTot = sqrt(pxSi*pxSi+pySi*pySi+pzSi*pzSi);
      const int ndfSi = trackExt->getNDF();
      float Chi2Si = trackExt->getChi2()/float(trackExt->getNDF());
      sprintf(strg,"%3i   %5i %9.3f  %9.3f  %9.3f  %7.2f  %7.2f  %7.2f %4i %4i %8.3f",iTrk, siTrack.id(), pTot, d0Si,z0Si,pxSi,pySi,pzSi,nHits, ndfSi, Chi2Si);
      debug() << strg << endmsg;
      
      if(nHits>0){
        _allSiTracks.push_back( trackExt );
      }else{
        delete trackExt;
      }
    }
  }
}


void FullLDCTrackingAlg::CleanUp(){
  
  int nNonCombTpc = int(_allNonCombinedTPCTracks.size());  
  for (int i=0;i<nNonCombTpc;++i) {
    TrackExtended * trkExt = _allNonCombinedTPCTracks[i];
    GroupTracks * group = trkExt->getGroupTracks();
    delete group;
  }
  _allNonCombinedTPCTracks.clear();
  
  int nNonCombSi = int(_allNonCombinedSiTracks.size());  
  for (int i=0;i<nNonCombSi;++i) {
    TrackExtended * trkExt = _allNonCombinedSiTracks[i];
    GroupTracks * group = trkExt->getGroupTracks();
    delete group;
  }
  _allNonCombinedSiTracks.clear();
  
  int nSITHits = int(_allSITHits.size());
  for (int i=0;i<nSITHits;++i) {
    TrackerHitExtended * hitExt = _allSITHits[i];
    delete hitExt;
  }
  _allSITHits.clear();
  
  int nSETHits = int(_allSETHits.size());
  for (int i=0;i<nSETHits;++i) {
    TrackerHitExtended * hitExt = _allSETHits[i];
    delete hitExt;
  }
  _allSETHits.clear();
  
  int nTPCHits = int(_allTPCHits.size());
  for (int i=0;i<nTPCHits;++i) {
    TrackerHitExtended * hitExt = _allTPCHits[i];
    delete hitExt;
  }
  _allTPCHits.clear();
  
  int nFTDHits = int(_allFTDHits.size());
  for (int i=0;i<nFTDHits;++i) {
    TrackerHitExtended * hitExt = _allFTDHits[i];
    delete hitExt;
  }
  _allFTDHits.clear();
  
  int nETDHits = int(_allETDHits.size());
  for (int i=0;i<nETDHits;++i) {
    TrackerHitExtended * hitExt = _allETDHits[i];
    delete hitExt;
  }
  _allETDHits.clear();
  
  int nVTXHits = int(_allVTXHits.size());
  for (int i=0;i<nVTXHits;++i) {
    TrackerHitExtended * hitExt = _allVTXHits[i];
    delete hitExt;
  }
  _allVTXHits.clear();
  
  int nSiTrk = int(_allSiTracks.size());
  for (int i=0;i<nSiTrk;++i) {
    TrackExtended * trkExt = _allSiTracks[i];
    delete trkExt;
  }
  _allSiTracks.clear();
  
  int nTPCTrk = int(_allTPCTracks.size());
  for (int i=0;i<nTPCTrk;++i) {
    TrackExtended * trkExt = _allTPCTracks[i];
    delete trkExt;
  }
  _allTPCTracks.clear();
  
  int nCombTrk = int(_allCombinedTracks.size());
  for (int i=0;i<nCombTrk;++i) {
    TrackExtended * trkExt = _allCombinedTracks[i];
    GroupTracks * group = trkExt->getGroupTracks();
    delete group;
    delete trkExt;    
  }
  _allCombinedTracks.clear();
  
  //   int nImplTrk = int(_trkImplVec.size());
  //   for (int i=0;i<nImplTrk;++i) {
  //     TrackExtended * trkImpl = _trkImplVec[i];
  //     delete trkImpl;
  //   }
  _trkImplVec.clear();
  
  //AS: Dont delete the individual entries, some of them are cleared elsewhere, I think
  _candidateCombinedTracks.clear();
}

/*
 
 Compare all TPC tracks with all Silicon tracks
 and compare using CompareTrkII which really only compares delta d0 and delta z0
 then calculate dOmega and the angle between the tracks, which are checked against cuts here.
 
 if CompareTrkII and the cuts on dOmega and angle succecede try and full fit of the hits using CombineTracks
 assuming that the merger was not vetoed VetoMerge
 i.e. if the momentum of either track is less than 2.5 GeV
 or if following a full fit the NDF+10 of the combined tracks is less than the NDF_first + NDF_second
 
 if this is successful the track is added to _allCombinedTracks and the TPC and Si Segements are added to _candidateCombinedTracks
 
 */

void FullLDCTrackingAlg::MergeTPCandSiTracks() {
  
  int nTPCTracks = int(_allTPCTracks.size());
  int nSiTracks  = int(_allSiTracks.size());
  
  debug() << " MergeTPCandSiTracks called nTPC tracks " << nTPCTracks << " - nSiTracks " << nSiTracks << endmsg;
  
  for (int iTPC=0;iTPC<nTPCTracks;++iTPC) {
    TrackExtended * tpcTrackExt = _allTPCTracks[iTPC];
    for (int iSi=0;iSi<nSiTracks;++iSi) {
      TrackExtended * siTrackExt = _allSiTracks[iSi];
      int iComp = 0;
      float angle = 0;

      debug() << " compare tpc trk " << toString( iTPC,  tpcTrackExt->getTrack(), _bField  ) << endmsg;
      debug() << "    to si trk    " << toString( iSi,   siTrackExt->getTrack(),  _bField  ) << endmsg;
      
      float dOmega = CompareTrkII(siTrackExt,tpcTrackExt,_d0CutForMerging,_z0CutForMerging,iComp,angle);
      
      if ( (dOmega<_dOmegaForMerging) && (angle<_angleForMerging) && !VetoMerge(tpcTrackExt,siTrackExt)) {
	
        debug() << " call CombineTracks for tpc trk " << tpcTrackExt << " si trk " << siTrackExt << endmsg;
	
        TrackExtended *combinedTrack = CombineTracks(tpcTrackExt,siTrackExt,_maxAllowedPercentageOfOutliersForTrackCombination, false);
	
        debug() << " combinedTrack returns " << combinedTrack << endmsg;
        
        if (combinedTrack != NULL) {


          _allCombinedTracks.push_back( combinedTrack );
          _candidateCombinedTracks.insert(tpcTrackExt);
          _candidateCombinedTracks.insert(siTrackExt);

	  //          streamlog_out(DEBUG3) << " combinedTrack successfully added to _allCombinedTracks : " << toString( 0,combinedTrack->getTrack(), _bField  )  << endmsg;
          debug() << " *** combinedTrack successfully added to _allCombinedTracks : tpc " << iTPC << " si " << iSi   << endmsg;
	  
          if (_debug >= 3 ) {
            int iopt = 1;
            // PrintOutMerging(tpcTrackExt,siTrackExt,iopt);
          }
        }
	else{
          if (_debug >= 3 ) {
            int iopt = 6;
            // PrintOutMerging(tpcTrackExt,siTrackExt,iopt);
          }
        }
      }
      else {
        if (_debug >= 3 ) {
          int iopt = 6;
          // PrintOutMerging(tpcTrackExt,siTrackExt,iopt);
        }
      }
    }
  }
  
  
}


/*
 
 All Si and TPC tracks which have been merged using MergeTPCandSiTracks are excluded from this search.
 
 The remaining TPC tracks and Silicon tracks will be tested using CompareTrkIII 

 CompareTrkIII does the following 
 
 i)   significance d0    < d0Cut
 ii)  significance z0    < z0Cut
 iii) pdot > 0.999
 
 if above three cuts are passed return:
 
 the significance dAngle (return by reference)
 and
 the significance dOmega

 if CompareTrkIII and the cuts on the significance of dOmega and angle succecede try and full fit of the hits using CombineTracks
 assuming that the merger was not vetoed VetoMerge 
 i.e. if the momentum of either track is less than 2.5 GeV
      and there is no overlapping of the segments
 
 if this is successful the track is added to _allCombinedTracks and the TPC and Si Segements are added to _candidateCombinedTracks
 
 */

void FullLDCTrackingAlg::MergeTPCandSiTracksII() {
  
  int nTPCTracks = int(_allTPCTracks.size());
  int nSiTracks  = int(_allSiTracks.size());
  
  debug() << " MergeTPCandSiTracksII called nTPC tracks " << nTPCTracks << " - nSiTracks " << nSiTracks << endmsg;

  for (int iTPC=0;iTPC<nTPCTracks;++iTPC) {
    
    // check if the tpc track has already been merged with CompareTrkII
    TrackExtended * tpcTrackExt = _allTPCTracks[iTPC];
    if(_candidateCombinedTracks.find(tpcTrackExt) != _candidateCombinedTracks.end() )continue;
    
    for (int iSi=0;iSi<nSiTracks;++iSi) {
      
      // check if the tpc track has already been merged with CompareTrkII
      TrackExtended * siTrackExt = _allSiTracks[iSi];
      if(_candidateCombinedTracks.find(siTrackExt)!= _candidateCombinedTracks.end() )continue;

      int iComp = 0;
      float angleSignificance = 0;

      float significance = CompareTrkIII(siTrackExt,tpcTrackExt,_d0CutForMerging,_z0CutForMerging,iComp,angleSignificance);

      debug() << " MergeTPCandSiTracksII - tpctrk " << iTPC << " - " << iSi <<  " - significance " << significance
	      << " angleSignificance " << angleSignificance << endmsg;
      
      if ( (significance<10) && (angleSignificance<5) && !VetoMerge(tpcTrackExt,siTrackExt) ) {

        TrackExtended * combinedTrack = CombineTracks(tpcTrackExt,siTrackExt,_maxAllowedPercentageOfOutliersForTrackCombination, false);
        
        debug() << " combinedTrack returns " << combinedTrack << endmsg;
        
	if (combinedTrack != NULL) {
          
          _allCombinedTracks.push_back( combinedTrack );
	  debug() << " *** combinedTrack successfully added to _allCombinedTracks : tpc " << iTPC << " si " << iSi   << endmsg;
	  
	  if (_debug >= 3 ) {
            int iopt = 1;
            // PrintOutMerging(tpcTrackExt,siTrackExt,iopt);
          }
        }
	else{
          if (_debug >= 3 ) {
            int iopt = 6;
            // PrintOutMerging(tpcTrackExt,siTrackExt,iopt);
          }
        }
      }
      else {
        if (_debug >= 3 ) {
          int iopt = 6;
          // PrintOutMerging(tpcTrackExt,siTrackExt,iopt);
        }
      }
    }
  }
}

// if testCombinationOnly is true then hits will not be assigned to the tracks 
TrackExtended * FullLDCTrackingAlg::CombineTracks(TrackExtended * tpcTrack, TrackExtended * siTrack, float maxAllowedOutliers, bool testCombinationOnly) {
  
  TrackExtended * OutputTrack = NULL;
  
  TrackerHitExtendedVec siHitVec = siTrack->getTrackerHitExtendedVec();
  TrackerHitExtendedVec tpcHitVec = tpcTrack->getTrackerHitExtendedVec();
  
  int nSiHits = int(siHitVec.size());
  int nTPCHits = int(tpcHitVec.size());
  int nHits = nTPCHits + nSiHits;
  
  //std::cout << "FullLDCTrackingAlg::CombineTracks nSiHits = " << nSiHits << endmsg;
  //std::cout << "FullLDCTrackingAlg::CombineTracks nTPCHits = " << nTPCHits << endmsg;
  
  ConstTrackerHitVec trkHits;
  trkHits.reserve(nHits);
  
  for (int ih=0;ih<nSiHits;++ih) {
    edm4hep::ConstTrackerHit trkHit = siHitVec[ih]->getTrackerHit();
    if(trkHit.isAvailable()) { 
      trkHits.push_back(trkHit);   
    }
    else{
      throw EVENT::Exception( std::string("FullLDCTrackingAlg::CombineTracks: TrackerHit pointer == NULL ")  ) ;
    }
  }  
  
  for (int ih=0;ih<nTPCHits;++ih) {
    edm4hep::ConstTrackerHit trkHit = tpcHitVec[ih]->getTrackerHit();
    if(trkHit.isAvailable()) { 
      trkHits.push_back(trkHit);   
    }
    else{
      throw EVENT::Exception( std::string("FullLDCTrackingAlg::CombineTracks: TrackerHit pointer == NULL ")  ) ;
    }
  }      
  
  double chi2_D;
  int ndf;
  
  if( trkHits.size() < 3 ) { 
    
    return 0 ;
    
  }
  
  debug() << "FullLDCTrackingAlg::CombineTracks: Sorting Hits " << trkHits.size() << endmsg;
  
  std::vector< std::pair<float, edm4hep::ConstTrackerHit> > r2_values;
  r2_values.reserve(trkHits.size());
  
  for (ConstTrackerHitVec::iterator it=trkHits.begin(); it!=trkHits.end(); ++it) {
    edm4hep::ConstTrackerHit h = *it;
    float r2 = h.getPosition()[0]*h.getPosition()[0]+h.getPosition()[1]*h.getPosition()[1];
    r2_values.push_back(std::make_pair(r2, *it));
  }
  
  sort(r2_values.begin(),r2_values.end());
  
  trkHits.clear();
  trkHits.reserve(r2_values.size());
  
  for (std::vector< std::pair<float, edm4hep::ConstTrackerHit> >::iterator it=r2_values.begin(); it!=r2_values.end(); ++it) {
    trkHits.push_back(it->second);
  }
  
  debug() << "FullLDCTrackingAlg::CombineTracks: Start Fitting: AddHits: number of hits to fit " << trkHits.size() << endmsg;
  
  std::auto_ptr<MarlinTrk::IMarlinTrack> marlin_trk_autop(_trksystem->createTrack());
  MarlinTrk::IMarlinTrack& marlin_trk = *marlin_trk_autop.get();
  
  edm4hep::TrackState pre_fit ;

  int errorCode = IMarlinTrack::success;

  try{
    pre_fit = getTrackStateAt(tpcTrack->getTrack(), 3/*lcio::TrackState::AtLastHit*/);
  }
  catch(std::runtime_error& e){
    error() << e.what() << " should be checked (TPC track)" << endmsg;
    try{
      pre_fit = getTrackStateAt(siTrack->getTrack(), 3/*lcio::TrackState::AtLastHit*/);
    }
    catch(std::runtime_error& e){
      error() << e.what() << " should be checked (Si track)" << endmsg;
    }
  }
  
  // setup initial dummy covariance matrix
  std::array<float,15> covMatrix;
  
  for (unsigned icov = 0; icov<covMatrix.size(); ++icov) {
    covMatrix[icov] = 0;
  }
  
  covMatrix[0]  = ( _initialTrackError_d0    ); //sigma_d0^2
  covMatrix[2]  = ( _initialTrackError_phi0  ); //sigma_phi0^2
  covMatrix[5]  = ( _initialTrackError_omega ); //sigma_omega^2
  covMatrix[9]  = ( _initialTrackError_z0    ); //sigma_z0^2
  covMatrix[14] = ( _initialTrackError_tanL  ); //sigma_tanl^2
  
  pre_fit.covMatrix = covMatrix;
  
  errorCode = MarlinTrk::createFit( trkHits, &marlin_trk, &pre_fit, _bField, IMarlinTrack::backward , _maxChi2PerHit );
  
  if ( errorCode != IMarlinTrack::success ) {
    debug() << "FullLDCTrackingAlg::CombineTracks: creation of fit fails with error " << errorCode << endmsg;
    return 0;
  }
  
  edm4hep::Vector3d point(0.,0.,0.); // nominal IP
  
  edm4hep::TrackState trkState ;
  errorCode = marlin_trk.propagate(point, trkState, chi2_D, ndf ) ;
  
  if ( errorCode != IMarlinTrack::success ) {
    debug() << "FullLDCTrackingAlg::CombineTracks: propagate to IP fails with error " << errorCode << endmsg;    
    return 0;
  }
  
  if ( ndf < 0  ) {
    debug() << "FullLDCTrackingAlg::CombineTracks: Fit failed NDF is less that zero  " << ndf << endmsg;
    return 0;
  }
  
  float chi2Fit = chi2_D/float(ndf);
  
  if ( chi2Fit > _chi2FitCut ) {
    debug() << "FullLDCTrackingAlg::CombineTracks: track fail Chi2 cut of " << _chi2FitCut << " chi2 of track = " <<  chi2Fit << endmsg;
    return 0;
  }
    
  debug() << "FullLDCTrackingAlg::CombineTracks: Check for outliers " << endmsg;
  
  std::vector<std::pair<edm4hep::ConstTrackerHit, double> > outliers ;
  marlin_trk.getOutliers(outliers);
  
  float outlier_pct = outliers.size()/float(trkHits.size()) ;
  
  if ( outlier_pct > maxAllowedOutliers) {
    debug() << "FullLDCTrackingAlg::CombineTracks: percentage of outliers " << outlier_pct << " is greater than cut maximum: " << maxAllowedOutliers << endmsg;
    return 0;
  }

  // sort the hits into outliers from TPC and Silicon as we will reject the combination if more that 2 Si hits get rejected ...
  
  std::vector<TrackerHitExtended*> siHitInFit;
  std::vector<TrackerHitExtended*> siOutliers;
  std::vector<TrackerHitExtended*> tpcHitInFit;
  std::vector<TrackerHitExtended*> tpcOutliers;
  
  for (int i=0;i<nSiHits;++i) {
    
    bool hit_is_outlier = false;
    
    // we need to make sure that in the case of a composite hit we reject this as well
    ConstTrackerHitVec hits;
    
    // all hits, both the 2D tracker hit, as well as any raw hits which belong to it
    ConstTrackerHit hit = siHitVec[i]->getTrackerHit();
    hits.push_back(hit);
    
    // add the raw hits ...
    int nRawHits = hit.rawHits_size();
    if ( nRawHits>0 ) {
      for (unsigned ihit=0; ihit < nRawHits; ++ihit) {
	try{
	  int type = hit.getType();
	  if(UTIL::BitSet32(type)[UTIL::ILDTrkHitTypeBit::COMPOSITE_SPACEPOINT]){
	    edm4hep::ConstTrackerHit rawHit = Navigation::Instance()->GetTrackerHit(hit.getRawHits(ihit));
	    hits.push_back(rawHit);
	  }
	  else debug() << "not space point, id=" << hit.id() << endmsg; 
	}
	catch(std::runtime_error& e){
	  debug() << e.what() << " raw hit id = " << hit.getRawHits(ihit) << " for TrackerHit id = " << hit.id() << endmsg;
	}
      }
    }
    
    // now double loop over the outliers and the hits assosiated with this TrackerHitExtended and compare
    for ( unsigned ohit = 0; ohit < outliers.size(); ++ohit) {
      for (unsigned ihit = 0; ihit < hits.size(); ++ihit) {
        
        // compare outlier pointer to TrackerHit pointer
        if( outliers[ohit].first == hits[ihit] ){
          // silicon outlier found so add the TrackerHitExtended to the list of outliers
          hit_is_outlier = true;
          siOutliers.push_back(siHitVec[i]);
          break;
        }
        if (hit_is_outlier) {
          break;
        }
      }
    }
    
    if( hit_is_outlier == false ){
      // add the TrackerHitExtended to the list of silicon hits used in the fit
      siHitInFit.push_back(siHitVec[i]);
    }
    
  }
  
  // more simple as TPC hits are never composite
  for (int i=0;i<nTPCHits;++i) {
    
    bool hit_is_outlier = false;
    
    for ( unsigned ihit = 0; ihit < outliers.size(); ++ihit) {

      // compare outlier pointer to TrackerHit pointer
      if( outliers[ihit].first == tpcHitVec[i]->getTrackerHit() ){
        // tpc outlier found so add the TrackerHitExtended to the list of outliers
        hit_is_outlier = true;
        tpcOutliers.push_back(tpcHitVec[i]);
        break;
      }
    }
    
    if( hit_is_outlier == false ){
      // add the TrackerHitExtended to the list of tpc hits used in the fit
      tpcHitInFit.push_back(tpcHitVec[i]);
    }
  }
  
  debug() << "FullLDCTrackingAlg::CombineTracks: Check for Silicon Hit rejections ... " << endmsg;
  
  if ( (int)siOutliers.size() > _maxAllowedSiHitRejectionsForTrackCombination ) {
    debug() << "FullLDCTrackingAlg::CombineTracks: Fit rejects " << siOutliers.size() << " silicon hits : max allowed rejections = " << _maxAllowedSiHitRejectionsForTrackCombination << " : Combination rejected " << endmsg;
    return 0;
  }
  
  float omega = trkState.omega;
  float tanlambda = trkState.tanLambda;
  float phi0 = trkState.phi;
  float d0 = trkState.D0;
  float z0 = trkState.Z0;
  
  OutputTrack = new TrackExtended();

  GroupTracks * group = new GroupTracks();
  OutputTrack->setGroupTracks(group);
  
  group->addTrackExtended(siTrack);
  group->addTrackExtended(tpcTrack);
  
  // note OutputTrack which is of type TrackExtended, only takes fits set for ref point = 0,0,0
  OutputTrack->setOmega(omega);
  OutputTrack->setTanLambda(tanlambda);
  OutputTrack->setPhi(phi0);
  OutputTrack->setZ0(z0);
  OutputTrack->setD0(d0);
  OutputTrack->setChi2(chi2_D);
  OutputTrack->setNDF(ndf);
  
  float cov[15];
  
  for (int i = 0 ; i<15 ; ++i) {
    cov[i] = trkState.covMatrix[i];
  }
  
  OutputTrack->setCovMatrix(cov);
  
  // if this is not just a test of the combination add the hits to the combined track
  if ( testCombinationOnly == false ) {

    for (unsigned ihit=0; ihit<siHitInFit.size(); ++ihit) {
      TrackerHitExtended * hitExt = siHitInFit[ihit];
      OutputTrack->addTrackerHitExtended(hitExt);
      hitExt->setUsedInFit(true);
    }
    
    for (unsigned ihit=0; ihit<siOutliers.size(); ++ihit) {
      TrackerHitExtended * hitExt = siOutliers[ihit];
      OutputTrack->addTrackerHitExtended(hitExt);
      hitExt->setUsedInFit(false);
    }

    for (unsigned ihit=0; ihit<tpcHitInFit.size(); ++ihit) {
      TrackerHitExtended * hitExt = tpcHitInFit[ihit];
      OutputTrack->addTrackerHitExtended(hitExt);
      hitExt->setUsedInFit(true);
    }
    
    for (unsigned ihit=0; ihit<tpcOutliers.size(); ++ihit) {
      TrackerHitExtended * hitExt = tpcOutliers[ihit];
      OutputTrack->addTrackerHitExtended(hitExt);
      hitExt->setUsedInFit(false);
    }
  }
    
  debug() << "FullLDCTrackingAlg::CombineTracks: merged track created  " << OutputTrack << " with " << OutputTrack->getTrackerHitExtendedVec().size() << " hits, nhits tpc " << nTPCHits << " nSiHits " << nSiHits << ", testCombinationOnly = " << testCombinationOnly << endmsg;
  
  return OutputTrack;
  
}


void FullLDCTrackingAlg::SortingTrackHitPairs(TrackHitPairVec & trackHitPairVec) {
  
  int sizeOfVector = int(trackHitPairVec.size());
  TrackHitPair *one,*two,*Temp;
  
  for (int i = 0 ; i < sizeOfVector-1; i++)
    for (int j = 0; j < sizeOfVector-i-1; j++) {
      one = trackHitPairVec[j];
      two = trackHitPairVec[j+1];
      float oneQ = one->getDistance();
      float twoQ = two->getDistance();
      if( oneQ > twoQ ) {
        Temp = trackHitPairVec[j];
        trackHitPairVec[j] = trackHitPairVec[j+1];
        trackHitPairVec[j+1] = Temp;
      }
    }  
  
  
}

/*
 
 Sorts all tracks in the vector by Chi2/NDF
 
 */

void FullLDCTrackingAlg::Sorting(TrackExtendedVec & trackVec) {
  
  int sizeOfVector = int(trackVec.size());
  TrackExtended *one,*two,*Temp;
  
  for (int i = 0 ; i < sizeOfVector-1; i++)
    for (int j = 0; j < sizeOfVector-i-1; j++) {
      one = trackVec[j];
      two = trackVec[j+1];
      float oneQ = one->getChi2()/float(one->getNDF());
      float twoQ = two->getChi2()/float(two->getNDF());
      if( oneQ > twoQ ) {
        Temp = trackVec[j];
        trackVec[j] = trackVec[j+1];
        trackVec[j+1] = Temp;
      }
    }  
}

/*
 
 this function is used to select only the combined tracks which have been formed from excatly 2 sub tracks which themselves are not formed from any other tracks.
 
 */

void FullLDCTrackingAlg::SelectCombinedTracks() {
  
  int nCombTrk = int(_allCombinedTracks.size());
  
  debug() << " **SelectCombinedTracks - check " << nCombTrk << " comb. tracks " << endmsg;

  // loop over all combined tracks ...
  for (int i=0; i<nCombTrk;++i) {

    TrackExtended * trkExt = _allCombinedTracks[i];

    debug() << " **SelectCombinedTracks - Check Track " << trkExt << endmsg;
    
    // get the sub tracks which have been combined to form this track
    GroupTracks * group = trkExt->getGroupTracks();
    TrackExtendedVec tracks = group->getTrackExtendedVec();

    // check that there are only 2 sub tracks, as we are after Si <-> TPC mergers only
    int nTracks = int(tracks.size());

    debug() << " **SelectCombinedTracks - nTracks = " << nTracks << endmsg;
    
    if (nTracks == 2) {
      
      TrackExtended * firstTrack = tracks[0];
      TrackExtended * secondTrack = tracks[1];

      // check that the two sub tracks in question are not themselves a merger of tracks
      if ((firstTrack->getGroupTracks() == NULL) && (secondTrack->getGroupTracks() == NULL) ) {

        debug() << " **SelectCombinedTracks - firstTrack->getGroupTracks() == NULL ... "  << endmsg;

        // associate the current group to the two sub tracks  
        firstTrack->setGroupTracks(group);
        secondTrack->setGroupTracks(group);     

        // get the tracker hits ...
        TrackerHitExtendedVec firstVec = firstTrack->getTrackerHitExtendedVec();
        TrackerHitExtendedVec secondVec = secondTrack->getTrackerHitExtendedVec();

        // number of hits for first and second sub track
        int nFirst = int(firstVec.size());
        int nSecond = int(secondVec.size());

        // use these to store the min and max z positions for the combination.
        float edges[2];
        edges[0] = 1.0e+20;
        edges[1] = -1.0e+20;

        // get min and max z for the first sub track
        for (int iF=0;iF<nFirst;++iF) {
          TrackerHitExtended * trkHitExt = firstVec[iF];
	  edm4hep::ConstTrackerHit trkHit = trkHitExt->getTrackerHit();
          float zpos = float(trkHit.getPosition()[2]);
          if (zpos>edges[1])
            edges[1] = zpos;
          if (zpos<edges[0])
            edges[0] = zpos;      
        }

        // get min and max z for the second sub track
        for (int iS=0;iS<nSecond;++iS) {
          TrackerHitExtended * trkHitExt = secondVec[iS];
	  edm4hep::ConstTrackerHit trkHit = trkHitExt->getTrackerHit();
          float zpos = float(trkHit.getPosition()[2]);
          if (zpos>edges[1])
            edges[1] = zpos;
          if (zpos<edges[0])
            edges[0] = zpos;
        }

        // record the z extreams to the group ...
        group->setEdges(edges);
        // ... and add the combined track to the list.
        _trkImplVec.push_back(trkExt);
        debug() << " add track " << trkExt << " to combined final list " << endmsg;
        
	if (_debug >= 2) {
          int iopt = 1;
	  // here it is assumed that the tpc tracks is the secondTrack ...
          // PrintOutMerging(secondTrack,firstTrack,iopt);
        }       
      }
    }
    else { // if(nTracks>2) 
      debug() << " *****************  SelectCombinedTracks: MORE THAN TWO TRACKS " << nCombTrk << endmsg;
    }
  }
}

void FullLDCTrackingAlg::AddNotCombinedTracks() {  
  
  int nTPCTrk = int(_allTPCTracks.size());
  int nSiTrk = int(_allSiTracks.size());
  
  // we need some buffer vector
  TrackExtendedVec allMergedTracks;
  allMergedTracks.clear();
  
  // forcing merging of Si and TPC track segments
  if (_forceMerging==1) { 

    // loop over all TPC tracks
    for (int i=0;i<nTPCTrk;++i) {

      // get the tracks assigned to this TPC track
      TrackExtended * trkExtTPC = _allTPCTracks[i];
      GroupTracks * groupTPC = trkExtTPC->getGroupTracks();

      // if no tracks have been grouped with this TPC track
      if (groupTPC == NULL) {

        float diffMin = 1.0e+20;

        TrackExtended * siTrkToAttach = NULL;

        // loop over all Silicon Tracks
        for (int j=0;j<nSiTrk;++j) {
          TrackExtended * trkExtSi = _allSiTracks[j];
          GroupTracks * groupSi = trkExtSi->getGroupTracks();

          // only consider ungrouped Silicon Tracks
          if (groupSi == NULL) {

            int iComp = 0;
            //      float deltaP = CompareTrk(trkExtSi,trkExtTPC,_d0CutForForcedMerging,_z0CutForForcedMerging,iComp);
            float angle(0.);
            float angleSignificance(0.);
            
            // try to merge tracks using looser cuts
            float dOmega = CompareTrkII(trkExtSi,trkExtTPC,_d0CutForForcedMerging,_z0CutForForcedMerging,iComp,angle);

            float significance = CompareTrkIII(trkExtSi,trkExtTPC,_d0CutForForcedMerging,_z0CutForForcedMerging,iComp,angleSignificance);

            //      if (deltaP < _dPCutForForcedMerging) {

            if ( ((dOmega<_dOmegaForForcedMerging) && (angle<_angleForForcedMerging)) ||
                ((significance<5)                 && (angleSignificance<5))
                ) {

              float chi2O = dOmega/_dOmegaForForcedMerging;
              float chi2A = angle/_angleForForcedMerging;
              float deltaP = chi2O*chi2O + chi2A*chi2A; 

              // if this is the best match (diffMin) set the possible merger 
              if (deltaP<diffMin) {
                diffMin = deltaP;
                siTrkToAttach = trkExtSi;
              }
            } else {
              if (_debug >= 3) {
                int  iopt = 7;
                debug() << significance << " " << angleSignificance << endmsg;
                // PrintOutMerging(trkExtTPC,trkExtSi,iopt);
              }
            }
          }
        }
        
        if (siTrkToAttach!=NULL) {

          TrackExtended * trkExtSi = siTrkToAttach;
          TrackExtended * OutputTrack = new TrackExtended();
          GroupTracks * group = new GroupTracks();

          group->addTrackExtended(trkExtSi);
          group->addTrackExtended(trkExtTPC);

          OutputTrack->setGroupTracks(group);
          //        trkExtSi->setGroupTracks(group);
          //        trkExtTPC->setGroupTracks(group);         

          OutputTrack->setOmega(trkExtTPC->getOmega());
          OutputTrack->setTanLambda(trkExtSi->getTanLambda());
          OutputTrack->setPhi(trkExtSi->getPhi());
          OutputTrack->setZ0(trkExtSi->getZ0());
          OutputTrack->setD0(trkExtSi->getD0());

          float covMatTPC[15];
          float covMatSi[15];
          float covMat[15];
          for (int iCov=0;iCov<15;++iCov) {
            covMatTPC[iCov] = trkExtTPC->getCovMatrix()[iCov];