FullLDCTrackingAlg.cpp

#include "FullLDCTrackingAlg.h"

#include "DataHelper/Navigation.h"
#include "Tracking/TrackingHelper.h"

#include <GearSvc/IGearSvc.h>

#include <edm4hep/TrackerHitConst.h>
#include <edm4hep/TrackerHit.h>
#include <edm4hep/TrackConst.h>
#include <edm4hep/Track.h>

#include <iostream>
#include <algorithm>
#include <memory>

#include <math.h>
#include <map>

//#include "DataHelper/ClusterShapes.h"
#include <gear/GEAR.h>
#include <gear/GearParameters.h>
#include <gear/BField.h>
#include <gear/VXDLayerLayout.h>
#include <gear/VXDParameters.h>
#include "gear/FTDLayerLayout.h"
#include "gear/FTDParameters.h"
#include <gear/TPCParameters.h>
#include <gear/PadRowLayout2D.h>

#include "TrackSystemSvc/IMarlinTrack.h"
#include "TrackSystemSvc/IMarlinTrkSystem.h"
#include "TrackSystemSvc/MarlinTrkUtils.h"
#include "TrackSystemSvc/LCIOTrackPropagators.h"

#include <UTIL/LCTOOLS.h>
//#include <UTIL/LCRelationNavigator.h>

// #include "MarlinTrk/MarlinTrkDiagnostics.h"
#ifdef MARLINTRK_DIAGNOSTICS_ON
#include "MarlinTrk/DiagnosticsController.h"
#endif

#include <UTIL/BitField64.h>
//#include <UTIL/BitSet32.h>
#include <UTIL/ILDConf.h>

#include <climits>
#include <cmath>

#include "gsl/gsl_randist.h"
#include "gsl/gsl_cdf.h"

#include <vector>
#include <bitset>

typedef std::vector<edm4hep::ConstTrackerHit> ConstTrackerHitVec;

using namespace edm4hep ;
using namespace MarlinTrk ;

/** debug printout helper method */
std::string toString( int iTrk, edm4hep::ConstTrack tpcTrack, float bField=3.5 ) {
  
  int   nHits    = int( tpcTrack.trackerHits_size() );
  float d0TPC    = getD0(tpcTrack);
  float z0TPC    = getZ0(tpcTrack);
  float omegaTPC = getOmega(tpcTrack);
  float phi0TPC  = getPhi(tpcTrack);
  float tanLTPC  = getTanLambda(tpcTrack);
  float Chi2TPC  = tpcTrack.getChi2()/float(tpcTrack.getNdf());
  int   ndfTPC   = tpcTrack.getNdf();

  int nlinkedTracks = tpcTrack.tracks_size();
  
  
  HelixClass helixTPC;
  
  helixTPC.Initialize_Canonical(phi0TPC,d0TPC,z0TPC,omegaTPC,tanLTPC, bField);
  
  char strg[200];
  
  float pxTPC = helixTPC.getMomentum()[0];
  float pyTPC = helixTPC.getMomentum()[1];
  float pzTPC = helixTPC.getMomentum()[2];
  const float ptot = sqrt(pxTPC*pxTPC+pyTPC*pyTPC+pzTPC*pzTPC);

  sprintf(strg,"%3i   %5i %9.3f  %9.3f  %9.3f  %7.2f  %7.2f  %7.2f %4i %4i %8.3f %8i",iTrk,tpcTrack.id(),
	  ptot, d0TPC,z0TPC,pxTPC,pyTPC,pzTPC,nHits,ndfTPC,Chi2TPC,nlinkedTracks);

  return std::string( strg ) ;
}

DECLARE_COMPONENT( FullLDCTrackingAlg )
FullLDCTrackingAlg::FullLDCTrackingAlg(const std::string& name, ISvcLocator* svcLoc) : GaudiAlgorithm(name, svcLoc) {  
  // _description = "Performs full tracking in ILD detector" ;  
  
  _encoder = new UTIL::BitField64(lcio::ILDCellID0::encoder_string);
  
  // Input tracker hit collections
  declareProperty("FTDPixelTrackerHits", _FTDPixelTrackerHitColHdl, "handler of FTD Pixel Hit Collection");
  declareProperty("FTDSpacePoints", _FTDSpacePointColHdl, "FTD FTDSpacePoint Collection");
  declareProperty("VTXTrackerHits", _VTXTrackerHitColHdl, "VTX Hit Collection");
  declareProperty("SITTrackerHits", _SITTrackerHitColHdl, "SIT Hit Collection");
  declareProperty("SETTrackerHits", _SETTrackerHitColHdl, "SET Hit Collection");
  //declareProperty("ETDTrackerHits", _ETDTrackerHitColHdl, "ETD Hit Collection");
  declareProperty("TPCTrackerHits", _TPCTrackerHitColHdl, "TPC Hit Collection");
  declareProperty("SITRawHits", _inSITRawColHdl, "SIT Raw Hit Collection of SpacePoints");
  declareProperty("SETRawHits", _inSETRawColHdl, "SET Raw Hit Collection of SpacePoints");
  declareProperty("FTDRawHits", _inFTDRawColHdl, "FTD Raw Hit Collection of SpacePoints");
  //declareProperty("VTXRawHits", _inVXDRawColHdl, "VXD SimTrackerHit collection");
  
  // Input track collections
  declareProperty("TPCTracks", _TPCTrackColHdl, "TPC Track Collection"); 
  declareProperty("SiTracks", _SiTrackColHdl, "Si Track Collection");
  
  // Input relation collections
  
  /*
  registerInputCollection(LCIO::LCRELATION,
                          "TPCTracksMCPRelColl",
                          "TPC Track to MCP Relation Collection Name",
                          _TPCTrackMCPCollName,
                          std::string("TPCTracksMCP"));
  
  registerInputCollection(LCIO::LCRELATION,
                          "SiTracksMCPRelColl",
                          "Si Track to Collection",
                          _SiTrackMCPCollName,
                          std::string("SiTracksMCP"));
  */ 
  // Output track collection
  declareProperty("OutputTracks", _OutputTrackColHdl, "Full LDC track collection name");
}



StatusCode FullLDCTrackingAlg::initialize() { 
  
  // usually a good idea to
  // printParameters();  
  _nRun = -1 ;
  _nEvt = 0 ;
  PI = acos(-1.);
  PIOVER2 = 0.5*PI;
  TWOPI = 2*PI;
  
  // set up the geometery needed by KalTest
  //FIXME: for now do KalTest only - make this a steering parameter to use other fitters
  auto _trackSystemSvc = service<ITrackSystemSvc>("TrackSystemSvc");
  if ( !_trackSystemSvc ) {
    error() << "Fail to find TrackSystemSvc ..." << endmsg;
  } 
  
  _trksystem = _trackSystemSvc->getTrackSystem(this);
  if( _trksystem == 0 ){
    error() << "Cannot initialize MarlinTrkSystem of Type: KalTest" <<endmsg;
    return StatusCode::FAILURE;
  }

  _trksystem->setOption( MarlinTrk::IMarlinTrkSystem::CFG::useQMS,        _MSOn ) ;
  _trksystem->setOption( MarlinTrk::IMarlinTrkSystem::CFG::usedEdx,       _ElossOn) ;
  _trksystem->setOption( MarlinTrk::IMarlinTrkSystem::CFG::useSmoothing,  _SmoothOn) ;
  _trksystem->init() ;
    
  this->setupGearGeom();
  return GaudiAlgorithm::initialize();
}

/*
void FullLDCTrackingAlg::processRunHeader( LCRunHeader* run) { 
  
  _nRun++ ;
  _nEvt = 0;
  streamlog_out(DEBUG5) << endmsg;
  streamlog_out(DEBUG5) << "FullLDCTrackingAlg ---> new run : run number = " << run->getRunNumber() << endmsg;
  
} 
*/

StatusCode FullLDCTrackingAlg::execute() { 
  
  // debug() << endmsg;
  debug() << "FullLDCTrackingAlg -> run = " << 0/*evt->getRunNumber()*/ << "  event = " << _nEvt << endmsg;
  // debug() << endmsg;
  auto outCol = _OutputTrackColHdl.createAndPut();
  
  prepareVectors();
  debug() << "************************************PrepareVectors done..." << endmsg;

  debug() << "************************************Merge TPC/Si ..." << endmsg;
  
  MergeTPCandSiTracks();
  debug() << "************************************Merging done ..." << endmsg;

  MergeTPCandSiTracksII();
  debug() << "************************************Merging II done ..." << endmsg;

  Sorting(_allCombinedTracks);
  debug() << "************************************Sorting by Chi2/NDF done ..." << endmsg;

  debug() << "************************************Selection of all 2 track combininations ..." << endmsg;
  SelectCombinedTracks();
  debug() << "************************************Selection of all 2 track combininations done ..." << endmsg;

  debug() << "************************************Trying non combined tracks ..." << endmsg;
  AddNotCombinedTracks( );
  debug() << "************************************Non combined tracks added ..." << endmsg;
  CheckTracks( );

  debug() << "************************************Add Non assigned hits ..." << endmsg;
  AddNotAssignedHits();
  debug() << "************************************Non assigned hits added ..." << endmsg;

  AddTrackColToEvt(_trkImplVec, outCol);
  debug() << "Collections added to event ..." << endmsg;
  CleanUp();
  debug() << "Cleanup is done." << endmsg;
  _nEvt++;
  //  getchar();
  // streamlog_out(DEBUG5) << endmsg;
  // streamlog_out(DEBUG5) << endmsg;
  return StatusCode::SUCCESS;
  
}

void FullLDCTrackingAlg::AddTrackColToEvt(TrackExtendedVec & trkVec, edm4hep::TrackCollection* colTRK) {
  
  //LCCollectionVec * colTRK = new LCCollectionVec(LCIO::TRACK);
  // if we want to point back to the hits we need to set the flag
  //LCFlagImpl trkFlag(0) ;
  //trkFlag.setBit( LCIO::TRBIT_HITS ) ;
  //colTRK->setFlag( trkFlag.getFlag()  ) ;  
    
  //  LCCollectionVec * colRel = NULL;
    
  int nTrkCand = int(trkVec.size());
  
  int nTotTracks = 0;
  float eTot  = 0.0;
  float pxTot = 0.0;
  float pyTot = 0.0;
  float pzTot = 0.0;
  
  //SJA:FIXME: So here we are going to do one final refit. This can certainly be optimised, but rather than worry about the mememory management right now lets make it work, and optimise it later ...
  
  
  for (int iTRK=0;iTRK<nTrkCand;++iTRK) {
    
    TrackExtended * trkCand = trkVec[iTRK];
    TrackerHitExtendedVec& hitVec = trkCand->getTrackerHitExtendedVec();
    
    std::vector<edm4hep::ConstTrackerHit> trkHits;
    
    debug() << " Trying to add track " << trkCand << " to final lcio collection " << endmsg;
        
    int nHits = int(hitVec.size());
    
    debug() << " Trying to add track " << trkCand << " to final lcio collection nHits = " << nHits << endmsg;
    
    for (int ihit=0;ihit<nHits;++ihit) {
    
      if( hitVec[ihit]->getUsedInFit() == false )  {
        debug() << "rejecting hit for track " << trkCand << " at zhit  " <<  hitVec[ihit]->getTrackerHit().getPosition()[2] << endmsg;
        continue;
      }

      edm4hep::ConstTrackerHit trkHit = hitVec[ihit]->getTrackerHit();
      
      if(trkHit.isAvailable()) {
        trkHits.push_back(trkHit);   
      }
      else{
        throw EVENT::Exception( std::string("FullLDCTrackingAlg::AddTrackColToEvt: TrackerHit pointer == NULL ")  ) ;
      }
      
    }
    
    
    if( trkHits.size() < 3 ) {
      debug() << "FullLDCTrackingAlg::AddTrackColToEvt: Cannot fit less than 3 hits. Number of hits =  " << trkHits.size() << endmsg;
      continue ; 
    }
    
    edm4hep::Track track;// = new edm4hep::Track;
    
    // 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
    
    // get the track state at the last hit at the outer most hit
    
    GroupTracks * group = trkCand->getGroupTracks();

    edm4hep::TrackState ts_initial;
    
    bool prefit_set = false;
    
    debug() << "Track Group = " << group << endmsg;
    
    if( group ) debug() << "Track Group size = " << group->getTrackExtendedVec().size() << endmsg;

    if (group != NULL && group->getTrackExtendedVec().size() > 0) {
    
      // get the second track as this must be the one furthest from the IP

      TrackExtended* te = 0;
      
      if(group->getTrackExtendedVec().size()==1) {
        te = group->getTrackExtendedVec()[0];
      } else {
        te = group->getTrackExtendedVec()[1];
      }
      
      if(hasTrackStateAt(te->getTrack(), 3/*lcio::TrackState::AtLastHit*/)){

        debug() << "Initialise Fit with trackstate from last hit" << group << endmsg;
	
        ts_initial = getTrackStateAt(te->getTrack(), 3/*lcio::TrackState::AtLastHit*/);
                        
        prefit_set = true;
      }
    }
    
    if( !prefit_set ) { // use parameters at IP
      debug() << "Initialise Fit with trackstate from IP " << group << endmsg;
      
      ts_initial.D0        = trkCand->getD0();
      ts_initial.phi       = trkCand->getPhi();
      ts_initial.Z0        = trkCand->getZ0();
      ts_initial.omega     = trkCand->getOmega();
      ts_initial.tanLambda = trkCand->getTanLambda();
      
      edm4hep::Vector3f ref(0,0,0);
      
      ts_initial.referencePoint = ref;
      
      ts_initial.location = 1/*lcio::TrackState::AtIP*/;
    }
    
    ts_initial.covMatrix = covMatrix;
        
    // sort hits in R
    std::vector< std::pair<float, edm4hep::ConstTrackerHit> > r2_values;
    r2_values.reserve(trkHits.size());
    
    for (std::vector<edm4hep::ConstTrackerHit>::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);
    }
    
    bool fit_backwards = IMarlinTrack::backward;
    
    MarlinTrk::IMarlinTrack* marlinTrk = _trksystem->createTrack();
    
    
    int error = 0;
    
    try {
      
      error = MarlinTrk::createFinalisedLCIOTrack(marlinTrk, trkHits, &track, fit_backwards, &ts_initial, _bField, _maxChi2PerHit);                              
      
    } catch (...) {
      
      //      delete track;
      //      delete marlinTrk;
      
      throw ;
      
    }
    
    
#ifdef MARLINTRK_DIAGNOSTICS_ON
    if ( error != IMarlinTrack::success && _runMarlinTrkDiagnostics ) {        
      void * dcv = _trksystem->getDiagnositicsPointer();
      DiagnosticsController* dc = static_cast<DiagnosticsController*>(dcv);
      dc->skip_current_track();
    }        
#endif
    
    
    std::vector<std::pair<edm4hep::ConstTrackerHit , double> > hits_in_fit ;  
    std::vector<std::pair<edm4hep::ConstTrackerHit , double> > outliers ;
    std::vector<edm4hep::ConstTrackerHit> all_hits;    
    all_hits.reserve(300);
    
    marlinTrk->getHitsInFit(hits_in_fit);
    
    for ( unsigned ihit = 0; ihit < hits_in_fit.size(); ++ihit) {
      all_hits.push_back(hits_in_fit[ihit].first);
    }
    
    UTIL::BitField64 cellID_encoder( lcio::ILDCellID0::encoder_string ) ; 
    
    MarlinTrk::addHitNumbersToTrack(&track, all_hits, true, cellID_encoder);
    
    marlinTrk->getOutliers(outliers);
    
    for ( unsigned ihit = 0; ihit < outliers.size(); ++ihit) {
      all_hits.push_back(outliers[ihit].first);
    }
    
    MarlinTrk::addHitNumbersToTrack(&track, all_hits, false, cellID_encoder);
    
    
    delete marlinTrk;
    
    if( error != IMarlinTrack::success ) {       
      debug() << "FullLDCTrackingAlg::AddTrackColToEvt: Track fit failed with error code " << error << " track dropped. Number of hits = "<< trkHits.size() << endmsg;  
      
      //delete Track;      
      continue ;
    }
    
    if( track.getNdf() < 0) {       
      debug() << "FullLDCTrackingAlg::AddTrackColToEvt: Track fit returns " << track.getNdf() << " degress of freedom track dropped. Number of hits = "<< trkHits.size() << endmsg;       
      //delete Track;
      continue ;
    }

    edm4hep::TrackState trkStateIP;
    for(int i=0;i<track.trackStates_size();i++){
      trkStateIP = track.getTrackStates(i);
      if(trkStateIP.location ==1/*lcio::TrackState::AtIP*/) break;
    }
    
    if (trkStateIP.location != 1/*lcio::TrackState::AtIP*/) {
      debug() << "FullLDCTrackingAlg::AddTrackColToEvt: Track fit returns " << track.getNdf() << " degress of freedom track dropped. Number of hits = "<< trkHits.size() << endmsg;
      throw EVENT::Exception( std::string("FullLDCTracking_MarlinTrk::AddTrackColToEvt: trkStateIP pointer == NULL ")  ) ;
    }
         
    if (group != NULL) {
      TrackExtendedVec trkVecGrp = group->getTrackExtendedVec();
      int nGrTRK = int(trkVecGrp.size());
      for (int iGr=0;iGr<nGrTRK;++iGr) {
        TrackExtended * subTrack = trkVecGrp[iGr];
        track.addToTracks(subTrack->getTrack());

        // check if it is a tpc looper ...
        if( UTIL::BitSet32( subTrack->getTrack().getType() )[  lcio::ILDDetID::TPC   ] )  {
          
          //const TrackVec segments = subTrack->getTrack().getTracks();
	  std::vector<edm4hep::ConstTrack> segments;
	  std::copy(subTrack->getTrack().tracks_begin(), subTrack->getTrack().tracks_end(), std::back_inserter(segments));
          if ( segments.empty() == false ) {
            
            for (unsigned iSeg=0;iSeg<segments.size();++iSeg) {
              track.addToTracks(segments[iSeg]);
            }

          }

        }
      }
    }
    
    float d0TrkCand = trkCand->getD0();
    float z0TrkCand = trkCand->getZ0();
    //    float phi0TrkCand = trkCand->getPhi();
    // FIXME, fucd
    int nhits_in_vxd = track.getSubDetectorHitNumbers(0);
    int nhits_in_ftd = track.getSubDetectorHitNumbers(1);
    int nhits_in_sit = track.getSubDetectorHitNumbers(2);
    int nhits_in_tpc = track.getSubDetectorHitNumbers(3);
    int nhits_in_set = track.getSubDetectorHitNumbers(4);
    //int nhits_in_vxd = Track->subdetectorHitNumbers()[ 2 * lcio::ILDDetID::VXD - 2 ];
    //int nhits_in_ftd = Track->subdetectorHitNumbers()[ 2 * lcio::ILDDetID::FTD - 2 ];
    //int nhits_in_sit = Track->subdetectorHitNumbers()[ 2 * lcio::ILDDetID::SIT - 2 ];
    //int nhits_in_tpc = Track->subdetectorHitNumbers()[ 2 * lcio::ILDDetID::TPC - 2 ];
    //int nhits_in_set = Track->subdetectorHitNumbers()[ 2 * lcio::ILDDetID::SET - 2 ];
    
    int nHitsSi = nhits_in_vxd + nhits_in_ftd + nhits_in_sit;
    
    debug() << " Hit numbers for Track "<< track.id() << ": "
	    << " vxd hits = " << nhits_in_vxd
	    << " ftd hits = " << nhits_in_ftd
	    << " sit hits = " << nhits_in_sit
	    << " tpc hits = " << nhits_in_tpc
	    << " set hits = " << nhits_in_set
	    << endmsg;
    
    if (nhits_in_vxd > 0) track.setType( track.getType()| (1<<lcio::ILDDetID::VXD) ) ;
    if (nhits_in_ftd > 0) track.setType( track.getType()| (1<<lcio::ILDDetID::FTD) ) ;
    if (nhits_in_sit > 0) track.setType( track.getType()| (1<<lcio::ILDDetID::SIT) ) ;
    if (nhits_in_tpc > 0) track.setType( track.getType()| (1<<lcio::ILDDetID::TPC) ) ;
    if (nhits_in_set > 0) track.setType( track.getType()| (1<<lcio::ILDDetID::SET) ) ;
    
    bool rejectTrack_onTPCHits = (nhits_in_tpc < _cutOnTPCHits) && (nHitsSi<=0);
    
    bool rejectTrackonSiliconHits = ( (nhits_in_tpc<=0) && (nHitsSi<_cutOnSiHits) );
    bool rejectTrackonImpactParameters =  ( fabs(d0TrkCand) > _d0TrkCut ) || ( fabs(z0TrkCand) > _z0TrkCut );
    
    if ( rejectTrack_onTPCHits || rejectTrackonSiliconHits  || rejectTrackonImpactParameters) {

      debug() << " Track " << trkCand
	      << " rejected : rejectTrack_onTPCHits = " << rejectTrack_onTPCHits
	      << " rejectTrackonSiliconHits " << rejectTrackonSiliconHits
	      << " rejectTrackonImpactParameters " << rejectTrackonImpactParameters
	      << endmsg;
      //delete Track;
    }
    else {
      float omega = trkStateIP.omega;
      float tanLambda = trkStateIP.tanLambda;
      float phi0 = trkStateIP.phi;
      float d0 = trkStateIP.D0;
      float z0 = trkStateIP.Z0;
      
      HelixClass helix;
      helix.Initialize_Canonical(phi0,d0,z0,omega,tanLambda,_bField);
      
      float trkPx = helix.getMomentum()[0];
      float trkPy = helix.getMomentum()[1];
      float trkPz = helix.getMomentum()[2];
      float trkP = sqrt(trkPx*trkPx+trkPy*trkPy+trkPz*trkPz);
      
      eTot += trkP;
      pxTot += trkPx;
      pyTot += trkPy;
      pzTot += trkPz;   
      nTotTracks++;
   
      colTRK->push_back(track);
      debug() << " Add Track to final Collection: ID = " << track.id() << " for trkCand "<< trkCand << endmsg;
    }
  }
  
  // streamlog_out(DEBUG5) << endmsg;
  debug() << "Number of accepted " << _OutputTrackColHdl.fullKey() << " = " << nTotTracks << endmsg;
  debug() << "Total 4-momentum of " << _OutputTrackColHdl.fullKey() << " : E = " << eTot
	  << " Px = " << pxTot
	  << " Py = " << pyTot
	  << " Pz = " << pzTot << endmsg;

}

void FullLDCTrackingAlg::prepareVectors() {
  
  _allTPCHits.clear();
  _allVTXHits.clear();
  _allFTDHits.clear();
  _allSITHits.clear();
  _allSETHits.clear();
  _allETDHits.clear();
  _allTPCTracks.clear();
  _allSiTracks.clear();
  _allCombinedTracks.clear();
  _allNonCombinedTPCTracks.clear();
  _allNonCombinedSiTracks.clear();
  _trkImplVec.clear();
  _candidateCombinedTracks.clear();
  
  
  std::map <edm4hep::ConstTrackerHit,TrackerHitExtended*> mapTrackerHits;
  
  // Reading TPC hits
  const edm4hep::TrackerHitCollection* hitTPCCol = nullptr;
  try {
    hitTPCCol = _TPCTrackerHitColHdl.get();
  }
  catch ( GaudiException &e ) {
    debug() << "Collection " << _TPCTrackerHitColHdl.fullKey() << " is unavailable in event " << _nEvt << endmsg;
  }
  if(hitTPCCol){
    int nelem = hitTPCCol->size();
    debug() << "Number of TPC hits = " << nelem << endmsg;
    for (edm4hep::ConstTrackerHit hit : *hitTPCCol) {
      TrackerHitExtended * hitExt = new TrackerHitExtended(hit);
      
      // Covariance Matrix in LCIO is defined in XYZ convert to R-Phi-Z
      // For no error in r
      
      double tpcRPhiRes = sqrt(hit.getCovMatrix()[0] + hit.getCovMatrix()[2]);
      double tpcZRes = sqrt(hit.getCovMatrix()[5]);
      
      hitExt->setResolutionRPhi(float(tpcRPhiRes));
      hitExt->setResolutionZ(float(tpcZRes));
      
      // 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));
      _allTPCHits.push_back( hitExt );
      mapTrackerHits[hit] = hitExt;

      double pos[3];
      
      for (int i=0; i<3; ++i) {
        pos[i] = hit.getPosition()[i];
      }
      
      unsigned int layer = static_cast<unsigned int>(getLayerID(hit));
      
      debug() << " TPC Hit added : @ " << pos[0] << " " << pos[1] << " " << pos[2]  << " drphi " << tpcRPhiRes << " dz " << tpcZRes << "  layer = " << layer << endmsg;
    
    }
  }
  
  // Reading in FTD Pixel Hits Collection
  //^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
  const edm4hep::TrackerHitCollection* hitFTDPixelCol = nullptr;
  try {
    hitFTDPixelCol = _FTDPixelTrackerHitColHdl.get();
  }
  catch ( GaudiException &e ) {
    debug() << "Collection " << _FTDPixelTrackerHitColHdl.fullKey() << " is unavailable in event " << _nEvt << endmsg;
  }

  if(hitFTDPixelCol){
    int nelem = hitFTDPixelCol->size();
    debug() << "Number of FTD Pixel Hits = " << nelem << endmsg;
    for(edm4hep::ConstTrackerHit hit : *hitFTDPixelCol){
      if ( UTIL::BitSet32( hit.getType() )[ UTIL::ILDTrkHitTypeBit::ONE_DIMENSIONAL ] ) continue;

      TrackerHitExtended * hitExt = new TrackerHitExtended( hit );
      
      // double point_res_rphi = sqrt( hit->getdU()*hit->getdU() + hit->getdV()*hit->getdV() );
      // FIXME to calculate the correct resolution;
      double point_res_rphi = sqrt( hit.getCovMatrix()[2]*hit.getCovMatrix()[2] + hit.getCovMatrix()[5]*hit.getCovMatrix()[5] );
      hitExt->setResolutionRPhi( point_res_rphi );
      
      // SJA:FIXME why is this needed? 
      hitExt->setResolutionZ(0.1);
      
      // 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));
      
      _allFTDHits.push_back( hitExt );
      mapTrackerHits[hit] = hitExt;
      
      // get the layer number
      unsigned int layer = static_cast<unsigned int>(getLayerID(hit));
      unsigned int petalIndex = static_cast<unsigned int>(getModuleID(hit));
      
      if ( _petalBasedFTDWithOverlaps == true ) {
        
        // as we are dealing with staggered petals we will use 2*nlayers in each directions +/- z
        // the layers will follow the even odd numbering of the petals 
        if ( petalIndex % 2 == 0 ) {
          layer = 2*layer;
        }
        else {
          layer = 2*layer + 1;
        }
        
      }
      
      if (layer >= _nLayersFTD) {
        fatal() << "FullLDCTrackingAlg => fatal error in FTD : layer is outside allowed range : " << layer << " number of layers = " << _nLayersFTD << endmsg;
        exit(1);
      }
      
      double pos[3];
      
      for (int i=0; i<3; ++i) {
        pos[i] = hit.getPosition()[i];
      }      
      
      debug() << " FTD Pixel Hit added : @ " << pos[0] << " " << pos[1] << " " << pos[2]  << " drphi " << hitExt->getResolutionRPhi() << " dz " << hitExt->getResolutionZ() << "  layer = " << layer << endmsg;
      
    }
  }
  
  // Reading in FTD SpacePoint Collection
  //^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
  const edm4hep::TrackerHitCollection* hitFTDSpacePointCol = nullptr;
  try {
    hitFTDSpacePointCol = _FTDSpacePointColHdl.get();
  }
  catch ( GaudiException &e ) {
    debug() << "Collection " << _FTDSpacePointColHdl.fullKey() << " is unavailable in event " << _nEvt << endmsg;
  }

  const edm4hep::TrackerHitCollection* rawFTDCol = nullptr;
  if(hitFTDSpacePointCol){
    try{
      rawFTDCol = _inFTDRawColHdl.get();
    }
    catch ( GaudiException &e ) {
      fatal() << "Collection " << _inFTDRawColHdl.fullKey() << " is unavailable in event " << _nEvt << endmsg;
    }
  }

  if(hitFTDSpacePointCol&&rawFTDCol){
    Navigation::Instance()->AddTrackerHitCollection(rawFTDCol);
    int nelem = hitFTDSpacePointCol->size();
    debug() << "Number of FTD SpacePoints = " << nelem << endmsg;
    for(edm4hep::ConstTrackerHit hit : *hitFTDSpacePointCol){
      TrackerHitExtended * hitExt = new TrackerHitExtended( hit );
      
      // SJA:FIXME: fudge for now by a factor of two and ignore covariance
      double point_res_rphi = 2 * sqrt( hit.getCovMatrix()[0] + hit.getCovMatrix()[2] );
      
      hitExt->setResolutionRPhi( point_res_rphi );
      
      // SJA:FIXME why is this needed? 
      hitExt->setResolutionZ(0.1);
      
      // 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));
      
      _allFTDHits.push_back( hitExt );
      mapTrackerHits[hit] = hitExt;
      
      // get the layer number
      unsigned int layer = static_cast<unsigned int>(getLayerID(hit));
      unsigned int petalIndex = static_cast<unsigned int>(getModuleID(hit));
      
      if ( _petalBasedFTDWithOverlaps == true ) {
        
        // as we are dealing with staggered petals we will use 2*nlayers in each directions +/- z
        // the layers will follow the even odd numbering of the petals 
        if ( petalIndex % 2 == 0 ) {
          layer = 2*layer;
        }
        else {
          layer = 2*layer + 1;
        }
        
      }
      
      if (layer >= _nLayersFTD) {
        fatal() << "FullLDCTrackingAlg => fatal error in FTD : layer is outside allowed range : " << layer << " number of layers = " << _nLayersFTD <<  endmsg;
        exit(1);
      }
      
      double pos[3];
      
      for (int i=0; i<3; ++i) {
        pos[i] = hit.getPosition()[i];
      }      
      
      debug() << " FTD SpacePoint Hit added : @ " << pos[0] << " " << pos[1] << " " << pos[2]  << " drphi " << hitExt->getResolutionRPhi() << " dz " << hitExt->getResolutionZ() << "  layer = " << layer << endmsg;
      
      
    }
  }
  
  const edm4hep::TrackerHitCollection* hitSITCol = nullptr;
  try {
    hitSITCol = _SITTrackerHitColHdl.get();
  }
  catch ( GaudiException &e ) {
    debug() << "Collection " << _SITTrackerHitColHdl.fullKey() << " is unavailable in event " << _nEvt << endmsg;
  }
  
  const edm4hep::TrackerHitCollection* rawSITCol = nullptr;
  if(hitSITCol){
    try{
      rawSITCol = _inSITRawColHdl.get();
    }
    catch ( GaudiException &e ) {
      warning() << "Collection " << _inSITRawColHdl.fullKey() << " is unavailable in event " << _nEvt << ", if SIT is Space Point, it needed " << endmsg;
    }
  }
  
  if(hitSITCol){
    if(rawSITCol) Navigation::Instance()->AddTrackerHitCollection(rawSITCol);
    
    int nelem = hitSITCol->size();

    debug() << "Number of SIT hits = " << nelem << endmsg;
    
    double drphi(NAN);
    double dz(NAN);
    
    for(edm4hep::ConstTrackerHit trkhit : *hitSITCol){
      
      // hit could be of the following type
      // 1) TrackerHit, either ILDTrkHitTypeBit::COMPOSITE_SPACEPOINT or just standard TrackerHit
      // 2) TrackerHitPlane, either 1D or 2D
      // 3) TrackerHitZCylinder, if coming from a simple cylinder design as in the LOI
      
      // Establish which of these it is in the following order of likelyhood
      //    i)   ILDTrkHitTypeBit::ONE_DIMENSIONAL (TrackerHitPlane) Should Never Happen: SpacePoints Must be Used Instead
      //    ii)  ILDTrkHitTypeBit::COMPOSITE_SPACEPOINT (TrackerHit)
      //    iii) TrackerHitPlane (Two dimentional)
      //    iv)  TrackerHitZCylinder 
      //    v)   Must be standard TrackerHit
      
      int layer = getLayerID(trkhit);
      
      if (layer < 0 || (unsigned)layer >= _nLayersSIT) {
        fatal() << "FullLDCTrackingAlg => fatal error in SIT : layer is outside allowed range : " << layer << endmsg;
        exit(1);
      }
      
      // first check that we have not been given 1D hits by mistake, as they won't work here
      if ( UTIL::BitSet32( trkhit.getType() )[ UTIL::ILDTrkHitTypeBit::ONE_DIMENSIONAL ] ) {
        fatal() << "FullLDCTrackingAlg: SIT Hit cannot be of type UTIL::ILDTrkHitTypeBit::ONE_DIMENSIONAL COMPOSITE SPACEPOINTS must be use instead. \n\n  exit(1) called from file " << __FILE__ << " and line " << __LINE__ << endmsg;
        exit(1);
      } 
      // most likely case: COMPOSITE_SPACEPOINT hits formed from stereo strip hits
      else if ( UTIL::BitSet32( trkhit.getType() )[ UTIL::ILDTrkHitTypeBit::COMPOSITE_SPACEPOINT ] ) {
        
        // SJA:FIXME: fudge for now by a factor of two and ignore covariance
        drphi =  2 * sqrt(trkhit.getCovMatrix()[0] + trkhit.getCovMatrix()[2]);         
        dz    =      sqrt(trkhit.getCovMatrix()[5]);         
        
      } 
      // or a PIXEL based SIT, using 2D TrackerHitPlane like the VXD above
      else if ( UTIL::BitSet32( trkhit.getType() )[3])  {
        // FIXME Should make it correct
	// first we need to check if the measurement vectors are aligned with the global coordinates 
	gear::Vector3D U(1.0,trkhit.getCovMatrix()[1],trkhit.getCovMatrix()[0],gear::Vector3D::spherical);
	gear::Vector3D V(1.0,trkhit.getCovMatrix()[4],trkhit.getCovMatrix()[3],gear::Vector3D::spherical);
	gear::Vector3D Z(0.0,0.0,1.0);
        
        const float eps = 1.0e-07;
        // V must be the global z axis 
        if( fabs(1.0 - V.dot(Z)) > eps ) {
          error() << "FullLDCTrackingAlg: PIXEL SIT Hit measurment vectors V is not equal to the global Z axis. \n\n  exit(1) called from file " << __FILE__ << " and line " << __LINE__ << endmsg;
          exit(1);
        }
        
        // U must be normal to the global z axis
        if( fabs(U.dot(Z)) > eps ) {
          error() << "FullLDCTrackingAlg: PIXEL SIT Hit measurment vectors U is not in the global X-Y plane. \n\n exit(1) called from file " << __FILE__ << " and line " << __LINE__ << endmsg;
          exit(1);
        }
        
        // FIXME should make it correct
        // drphi = trkhit_P->getdU();
        // dz    = trkhit_P->getdV();                                                 
        drphi = trkhit.getCovMatrix()[2];
	dz    = trkhit.getCovMatrix()[5];
      } 
      // or a simple cylindrical design, as used in the LOI
      /* FIXME, fucd
      else if ( true ) {
        trkhit_C = hitCollection->at( ielem );
        // FIXME
        // drphi = trkhit_C->getdRPhi();
        // dz    = trkhit_C->getdZ();
        drphi = 1.0;
        dz = 1.0; 
      } 
      */
      // this would be very unlikely, but who knows ... just an ordinary TrackerHit, which is not a COMPOSITE_SPACEPOINT
      else {
        
        // SJA:FIXME: fudge for now by a factor of two and ignore covariance
        drphi =  2 * sqrt(trkhit.getCovMatrix()[0] + trkhit.getCovMatrix()[2]);         
        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));
      
      _allSITHits.push_back( hitExt );
      mapTrackerHits[trkhit] = hitExt;
      
      
      double pos[3];
      
      for (int i=0; i<3; ++i) {
        pos[i] = trkhit.getPosition()[i];
      }
      
      debug() << " SIT Hit " <<  trkhit.id() << " added : @ " << pos[0] << " " << pos[1] << " " << pos[2] << " drphi " << hitExt->getResolutionRPhi() << " dz " << hitExt->getResolutionZ() << "  layer = " << layer << endmsg;
    }
  }
  
  const edm4hep::TrackerHitCollection* hitSETCol = nullptr;
  try {
    hitSETCol = _SETTrackerHitColHdl.get();
  }
  catch ( GaudiException &e ) {
    debug() << "Collection " << _SETTrackerHitColHdl.fullKey() << " is unavailable in event " << _nEvt << endmsg;
  }

  const edm4hep::TrackerHitCollection* rawSETCol = nullptr;
  if(hitSETCol){
    try{
      rawSETCol = _inSETRawColHdl.get();
    }
    catch ( GaudiException &e ) {
      warning() << "Collection " << _inSETRawColHdl.fullKey() << " is unavailable in event " << _nEvt << ", if SIT is Space Point, it needed " << endmsg;
    }
  }

  if(hitSETCol){
    if(rawSETCol) Navigation::Instance()->AddTrackerHitCollection(rawSETCol);

    int nelem = hitSETCol->size();

    debug() << "Number of SET hits = " << nelem << endmsg;

    double drphi(NAN);
    double dz(NAN);

    for(edm4hep::ConstTrackerHit trkhit : *hitSETCol){
      // hit could be of the following type
      // 1) TrackerHit, either ILDTrkHitTypeBit::COMPOSITE_SPACEPOINT or just standard TrackerHit
      // 2) TrackerHitPlane, either 1D or 2D
      // 3) TrackerHitZCylinder, if coming from a simple cylinder design as in the LOI
      
      // Establish which of these it is in the following order of likelyhood
      //    i)   ILDTrkHitTypeBit::ONE_DIMENSIONAL (TrackerHitPlane) Should Never Happen: SpacePoints Must be Used Instead
      //    ii)  ILDTrkHitTypeBit::COMPOSITE_SPACEPOINT (TrackerHit)
      //    iii) TrackerHitPlane (Two dimentional)
      //    iv)  TrackerHitZCylinder
      //    v)   Must be standard TrackerHit
      int layer = getLayerID(trkhit);
      
      if (layer < 0 || (unsigned)layer >= _nLayersSET) {
        fatal() << "FullLDCTrackingAlg => fatal error in SET : layer is outside allowed range : " << layer << endmsg;
        exit(1);
      }
      
      // first check that we have not been given 1D hits by mistake, as they won't work here
      if ( UTIL::BitSet32( trkhit.getType() )[ UTIL::ILDTrkHitTypeBit::ONE_DIMENSIONAL ] ) {
	fatal() << "SiliconTrackingAlg => fatal error in SIT : layer is outside allowed range : " << layer << endmsg;
        exit(1);
      }
      // most likely case: COMPOSITE_SPACEPOINT hits formed from stereo strip hits
      else if ( UTIL::BitSet32( trkhit.getType() )[ UTIL::ILDTrkHitTypeBit::COMPOSITE_SPACEPOINT ] ) {
	// SJA:FIXME: fudge for now by a factor of two and ignore covariance
        drphi =  2 * sqrt(trkhit.getCovMatrix()[0] + trkhit.getCovMatrix()[2]);
        dz    =      sqrt(trkhit.getCovMatrix()[5]);
      }
      // or a PIXEL based SET, using 2D TrackerHitPlane like the VXD above
      else if ( UTIL::BitSet32( trkhit.getType() )[3] )  {
        // FIXME should consider this carefully
        
        // first we need to check if the measurement vectors are aligned with the global coordinates
        // gear::Vector3D U(1.0,trkhit_P->getU()[1],trkhit_P->getU()[0],gear::Vector3D::spherical);
        // gear::Vector3D V(1.0,trkhit_P->getV()[1],trkhit_P->getV()[0],gear::Vector3D::spherical);
        // FIXME Should calculate it correctly
	gear::Vector3D U(1.0,trkhit.getCovMatrix()[1],trkhit.getCovMatrix()[0],gear::Vector3D::spherical);
	gear::Vector3D V(1.0,trkhit.getCovMatrix()[4],trkhit.getCovMatrix()[3],gear::Vector3D::spherical);
	gear::Vector3D Z(0.0,0.0,1.0);
        
        const float eps = 1.0e-07;
        // V must be the global z axis
        if( fabs(1.0 - V.dot(Z)) > eps ) {
          fatal() << "FullLDCTrackingAlg: PIXEL SET Hit measurment vectors V is not equal to the global Z axis. \n\n  exit(1) called from file " << __FILE__ << " and line " << __LINE__ << endmsg;
          exit(1);
        }
        
        // U must be normal to the global z axis
        if( fabs(U.dot(Z)) > eps ) {
          fatal() << "FullLDCTrackingAlg: PIXEL SET Hit measurment vectors U is not in the global X-Y plane. \n\n exit(1) called from file " << __FILE__ << " and line " << __LINE__ << endmsg;
          exit(1);
        }
	
        // FIXME should use the correct 
        // drphi = trkhit_P->getdU();
        // dz    = trkhit_P->getdV();
        drphi = trkhit.getCovMatrix()[2];
	dz    = trkhit.getCovMatrix()[5];
      }
      // or a simple cylindrical design, as used in the LOI
      /*FIXME, fucd
      else if ( true ) {
        trkhit_C = hitCollection->at( ielem );
        // FIXME
        // drphi = trkhit_C->getdRPhi();
        // dz    = trkhit_C->getdZ();
        drphi = 1.0;
        dz = 1.0;
      }
      */
      // this would be very unlikely, but who knows ... just an ordinary TrackerHit, which is not a COMPOSITE_SPACEPOINT
      else {
        
        // SJA:FIXME: fudge for now by a factor of two and ignore covariance
        drphi =  2 * sqrt(trkhit.getCovMatrix()[0] + trkhit.getCovMatrix()[2]);