SiliconTrackingAlg.cpp~

#include "SiliconTrackingAlg.h"
#include "GearSvc/IGearSvc.h"
#include "EventSeeder/IEventSeeder.h"
#include "TrackSystemSvc/ITrackSystemSvc.h"
#include "edm4hep/MCParticle.h"
#include "edm4hep/TrackerHit.h"
//#include "edm4hep/TrackerHitPlane.h"
#include "edm4hep/Track.h"
#include "edm4hep/TrackState.h"

//#include "DataHelper/ClusterExtended.h"
//#include "DataHelper/TrackExtended.h"
//#include "DataHelper/TrackerHitExtended.h"
//#include "DataHelper/HelixClass.h"

#include <iostream>
#include <algorithm>
#include <cmath>
#include <climits>

#include <gear/GEAR.h>
#include <gear/GearMgr.h>
#include <gear/GearParameters.h>
#include <gear/VXDLayerLayout.h>
#include <gear/VXDParameters.h>
#include "gear/FTDLayerLayout.h"
#include "gear/FTDParameters.h"

#include <gear/BField.h>

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

#include "TrackSystemSvc/MarlinTrkUtils.h"
#include "TrackSystemSvc/HelixTrack.h"
#include "TrackSystemSvc/HelixFit.h"
#include "TrackSystemSvc/IMarlinTrack.h"

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

//#include "MarlinCED.h"

//#include "marlin/AIDAProcessor.h"

//---- ROOT -----
#include "TH1F.h"
#include "TH2F.h"

using namespace edm4hep ;
//using namespace marlin ;
using namespace MarlinTrk ;

using std::min;
using std::max;
using std::abs;

const int SiliconTrackingAlg::_output_track_col_quality_GOOD = 1;
const int SiliconTrackingAlg::_output_track_col_quality_FAIR = 2;
const int SiliconTrackingAlg::_output_track_col_quality_POOR = 3;

const double SiliconTrackingAlg::TWOPI = 2*M_PI;

DECLARE_COMPONENT( SiliconTrackingAlg )

SiliconTrackingAlg::SiliconTrackingAlg(const std::string& name, ISvcLocator* svcLoc)
: GaudiAlgorithm(name, svcLoc) {
  
  //_description = "Pattern recognition in silicon trackers";
  
  _fastfitter = new MarlinTrk::HelixFit();
  
  _encoder = new UTIL::BitField64(lcio::ILDCellID0::encoder_string);
  
  _petalBasedFTDWithOverlaps = false;
  
  // zero triplet counters
  _ntriplets = _ntriplets_good = _ntriplets_2MCP = _ntriplets_3MCP = _ntriplets_1MCP_Bad = _ntriplets_bad = 0;

  // Input Collections
  // ^^^^^^^^^^^^^^^^^
  declareProperty("HeaderCol", _headerColHdl);
  declareProperty("MCParticleCollection", _inMCColHdl, "Handle of the Input MCParticle collection");
  declareProperty("VTXHitCollection", _inVTXColHdl, "Handle of the Input VTX TrackerHits collection");
  declareProperty("FTDPixelHitCollection", _inFTDPixelColHdl, "Handle of the Input FTD TrackerHits collection");
  declareProperty("FTDSpacePointCollection", _inFTDSpacePointColHdl, "Handle of the Input FTD SpacePoints collection");
  declareProperty("SITHitCollection", _inSITColHdl, "Handle of the Input SIT TrackerHits collection");
    
  // Output Collections
  // ^^^^^^^^^^^^^^^^^^
  declareProperty("SiTrackCollection", _outColHdl, "Handle of the SiTrack output collection");
  //declareProperty("TrkHitRelCollection", _outRelColHdl, "Handle of TrackerHit Track relation collection");
  // Steering parameters
  // ^^^^^^^^^^^^^^^^^^^
  
  _output_track_col_quality = _output_track_col_quality_GOOD;
  
}



StatusCode  SiliconTrackingAlg::initialize() { 
  
  _nRun = -1 ;
  _nEvt = 0 ;
  //printParameters() ;
  
  // 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() << "Failed to find TrackSystemSvc ..." << endmsg;
    return StatusCode::FAILURE;
  }
  _trksystem =  _trackSystemSvc->getTrackSystem();
  
  if( _trksystem == 0 ){
    error() << "Cannot initialize MarlinTrkSystem of Type: KalTest" <<endmsg;
    return StatusCode::FAILURE;
  }
  
  _trksystem->setOption( IMarlinTrkSystem::CFG::useQMS,        _MSOn ) ;
  _trksystem->setOption( IMarlinTrkSystem::CFG::usedEdx,       _ElossOn) ;
  _trksystem->setOption( IMarlinTrkSystem::CFG::useSmoothing,  _SmoothOn) ;
  _trksystem->init() ;  
  std::cout << "fucd ==============" << _trksystem << std::endl;
#ifdef MARLINTRK_DIAGNOSTICS_ON
  
  void * dcv = _trksystem->getDiagnositicsPointer();
  DiagnosticsController* dc = static_cast<DiagnosticsController*>(dcv);
  dc->init(_MarlinTrkDiagnosticsName,_MarlinTrkDiagnosticsName, _runMarlinTrkDiagnostics);
  
#endif
  
  if(setupGearGeom()==StatusCode::FAILURE) return StatusCode::FAILURE;
  
  if (_useSIT == 0)
    _nLayers = _nLayersVTX;
  else 
    _nLayers = _nLayersVTX + _nLayersSIT;
  
  // initialise the container to have separate vectors for up to _nHitsChi2 hits.
  _tracksWithNHitsContainer.resize(_nHitsChi2);
  
  _dPhi = TWOPI/_nDivisionsInPhi;
  _dTheta = 2.0/_nDivisionsInTheta;
  _dPhiFTD = TWOPI/_nPhiFTD;
  // I leave this for the moment, but 0.3 is c/1e9.
  // For the cut it does not make too much of a difference
  double cutOnR = _cutOnPt/(0.3*_bField);
  cutOnR = 1000.*cutOnR;
  _cutOnOmega = 1/cutOnR;
  
  _output_track_col_quality = 0;
  
  return GaudiAlgorithm::initialize();
}

StatusCode SiliconTrackingAlg::execute(){ 
  
  //_current_event = evt;
  //_allHits.reserve(1000);

  _output_track_col_quality = _output_track_col_quality_GOOD;
  
  // zero triplet counters
  _ntriplets = _ntriplets_good = _ntriplets_2MCP = _ntriplets_3MCP = _ntriplets_1MCP_Bad = _ntriplets_bad = 0;
  
  // Clearing the working containers from the previous event
  // FIXME: partly done at the end of the event, in CleanUp. Make it consistent.
  //_tracksWithNHitsContainer.clear();
  //_trackImplVec.clear();
  
  //_colTrackerHits.clear();
  //_colNamesTrackerHits.clear();
  //auto header = _headerColHdl.get()->at(0);
  //int evtNo = header.getEventNumber();
  //int runNo = header.getRunNumber();
  //debug() << "Processing Run[" << runNo << "]::Event[" << evtNo << "]" << endmsg;

  _trackImplVec.reserve(100);
  _allHits.reserve(1000);

  int successVTX = InitialiseVTX();
  int successFTD = 0;
  //int successFTD = InitialiseFTD();
  if (successVTX == 1) {
    
    debug() << "      phi          theta        layer      nh o :   m :   i  :: o*m*i " << endmsg; 
    
    for (int iPhi=0; iPhi<_nDivisionsInPhi; ++iPhi) { 
      for (int iTheta=0; iTheta<_nDivisionsInTheta;++iTheta) {
        ProcessOneSector(iPhi,iTheta); // Process one VXD sector     
      }
    }
    
    debug() << "End of Processing VXD and SIT sectors" << endmsg;
    
  }
  
  if (successFTD == 1) {
    debug() << "      phi          side        layer      nh o :   m :   i  :: o*m*i " << endmsg;
    TrackingInFTD(); // Perform tracking in the FTD
    debug() << "End of Processing FTD sectors" << endmsg;
  }

  //if(0){
  if (successVTX == 1 || successFTD == 1) {
    //if (successVTX == 1 ) {
    for (int nHits = _nHitsChi2; nHits >= 3 ;// the three is hard coded, sorry.
         // It's the minimal number to form a track
         nHits--) {
      Sorting( _tracksWithNHitsContainer.getTracksWithNHitsVec( nHits ) );
      
    }
    debug() <<  "End of Sorting " << endmsg;
        
    for (int nHits = _nHitsChi2; nHits >= 3 ;// the three is hard coded, sorry.
         // It's the minimal number to form a track
         nHits--) {
      
      TrackExtendedVec &tracksWithNHits = _tracksWithNHitsContainer.getTracksWithNHitsVec( nHits );
      for (TrackExtendedVec::iterator trackIter = tracksWithNHits.begin();
           trackIter < tracksWithNHits.end(); trackIter++) {
        CreateTrack( *trackIter );
      }
      debug() <<  "End of creating "<< nHits << " hits tracks " << endmsg;
    }
    
    if (_attachFast == 0) {
      if(successVTX) AttachRemainingVTXHitsSlow();
      if(successFTD) AttachRemainingFTDHitsSlow();
    }
    else {
      if(successVTX) AttachRemainingVTXHitsFast();
      if(successFTD) AttachRemainingFTDHitsFast();
    }
    
    debug() <<  "End of picking up remaining hits " << endmsg;

    //edm4hep::TrackCollection* trkCol = nullptr; 
    //edm4hep::LCRelationCollection* relCol = nullptr;
    auto trkCol = _outColHdl.createAndPut();
    //auto relCol = _outRelColHdl.createAndPut();
    //std::cout << "fucd------------------" << std::endl;
    /*
    LCCollectionVec * trkCol = 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 ) ;
    trkCol->setFlag( trkFlag.getFlag()  ) ;
    
    LCCollectionVec * relCol = NULL;
    */
    //FinalRefit(trkCol, relCol);
    FinalRefit(trkCol);
    //std::cout << "fucd------------------" << std::endl;
    // set the quality of the output collection
    switch (_output_track_col_quality) {

      case _output_track_col_quality_FAIR:
        //trkCol->parameters().setValue( "QualityCode" , "Fair"  ) ;
        break;

      case _output_track_col_quality_POOR:
        //trkCol->parameters().setValue( "QualityCode" , "Poor"  ) ;
        break;
        
      default:
        //trkCol->parameters().setValue( "QualityCode" , "Good"  ) ;
        break;
    }
    /*
    if (_UseEventDisplay) {
      this->drawEvent();
    }
    */
  }
  
  // fill event based histogram
  /*
  if (_createDiagnosticsHistograms) {
  
    // triplet histos
    _histos->fill1D(DiagnosticsHistograms::hntriplets, _ntriplets);
    _histos->fill1D(DiagnosticsHistograms::hntriplets_good, _ntriplets_good);
    _histos->fill1D(DiagnosticsHistograms::hntriplets_2MCP, _ntriplets_2MCP);
    _histos->fill1D(DiagnosticsHistograms::hntriplets_3MCP, _ntriplets_3MCP);
    _histos->fill1D(DiagnosticsHistograms::hntriplets_1MCP_Bad, _ntriplets_1MCP_Bad);
    _histos->fill1D(DiagnosticsHistograms::hntriplets_bad, _ntriplets_bad);
  
  }
  */
  /*
  const edm4hep::MCParticleCollection* mcCol = nullptr;
  try{
    mcCol =_inMCColHdl.get();
  }
  catch(...){
  }
  if(mcCol){
    int id = 0;
    for(auto mcP : *mcCol){
      float pos[3];
      float mom[3];
      pos[0] = mcP.vertex()[0];
      pos[1] = mcP.vertex()[1];
      pos[2] = mcP.vertex()[2];
      mom[0] = mcP.momentum()[0];
      mom[1] = mcP.momentum()[1];
      mom[2] = mcP.momentum()[2];
      float           charge = mcP.getCharge();
      HelixClass helix;
      helix.Initialize_VP(pos,mom,charge,_bField);
      float d0 = helix.getD0();
      float z0 = helix.getZ0();
      float omega = helix.getOmega();
      float phi0 = helix.getPhi0();
      float tanLambda = helix.getTanLambda();
      std::cout <<"MCParticle: " << evtNo << " " << id << " " << sqrt(mom[0]*mom[0]+mom[1]*mom[1]) << " " << acos(mom[2]/sqrt(mom[0]*mom[0]+mom[1]*mom[1]+mom[2]*mom[2]))
		<< " " << atan2(mom[1],mom[0])
		<< " " << d0 << " " << phi0 << " " << omega << " " << z0 << " " << tanLambda << " " << mcP.vertex() << std::endl; 
      id++;
    }
  }
  
  const edm4hep::TrackCollection* trkCol = nullptr;
  try{
    trkCol = _outColHdl.get();
  }
  catch(...){
  }
  if(trkCol){
    int id = 0;
    for(auto track : *trkCol){
      int nstate = track->trackStates_size();
      for(int i=0;i<nstate;i++){
	edm4hep::TrackState trkState = track->getTrackStates(i);
	if(trkState.location != 1) continue;
	HelixClass helix_final;
        helix_final.Initialize_Canonical(trkState.phi,trkState.D0,trkState.Z0,trkState.omega,trkState.tanLambda,_bField);
        float trkPx = helix_final.getMomentum()[0];
        float trkPy = helix_final.getMomentum()[1];
	float trkPz = helix_final.getMomentum()[2];
        float trkPt = sqrt(trkPx*trkPx+trkPy*trkPy);
	std::cout << "Track parameter: " << evtNo << " " << id << " " << trkPt << " " << acos(trkPz/sqrt(trkPt*trkPt+trkPz*trkPz)) << " " << atan2(trkPy,trkPx)
		  << " " << trkState.D0 << " " << trkState.phi << " " << trkState.omega << " " << trkState.Z0 << " " << trkState.tanLambda
		  << " " << sqrt(trkState.covMatrix[0]) << " " << sqrt(trkState.covMatrix[2]) << " " << sqrt(trkState.covMatrix[5])
		  << " " << sqrt(trkState.covMatrix[9]) << " " << sqrt(trkState.covMatrix[14]) << " " << std::endl;
	id++;
	break;
      }
    }
  }
  */
  CleanUp();
  debug() << "Event is done " << endmsg;
  _nEvt++;
  return StatusCode::SUCCESS;
}


void SiliconTrackingAlg::CleanUp() {
  
  _tracksWithNHitsContainer.clear();
  
  for (int il=0;il<_nLayers;++il) {
    for (int ip=0;ip<_nDivisionsInPhi;++ip) {
      for (int it=0;it<_nDivisionsInTheta; ++it) {
        unsigned int iCode = il + _nLayers*ip + _nLayers*_nDivisionsInPhi*it;      
        
        if( iCode >= _sectors.size()){          
          error() << "iCode index out of range: iCode =   " << iCode << " _sectors.size() = " << _sectors.size() << " exit(1) called from file " << __FILE__ << " line " << __LINE__<< endmsg;
          continue;
        }
        
        TrackerHitExtendedVec& hitVec = _sectors[iCode];
        int nH = int(hitVec.size());
        for (int iH=0; iH<nH; ++iH) {
          TrackerHitExtended * hit = hitVec[iH];
          delete hit;
        }
      }
    }
  }
  
  for (int iS=0;iS<2;++iS) {
    for (unsigned int layer=0;layer<_nlayersFTD;++layer) {
      for (int ip=0;ip<_nPhiFTD;++ip) {
        unsigned int iCode = iS + 2*layer + 2*_nlayersFTD*ip;
        
        if( iCode >= _sectorsFTD.size()){
          //error() << "iCode index out of range: iCode =   " << iCode << " _sectorsFTD.size() = " << _sectorsFTD.size() << " exit(1) called from file " << __FILE__ << " line " << __LINE__<< endmsg;
          continue;
        }
        
        TrackerHitExtendedVec& hitVec = _sectorsFTD[iCode];
        int nH = int(hitVec.size());
        for (int iH=0; iH<nH; ++iH) {
          TrackerHitExtended * hit = hitVec[iH];
          delete hit;
        }
      }
    }
  }
  _trackImplVec.clear();
  _allHits.clear();
}

int SiliconTrackingAlg::InitialiseFTD() {
  
  int success = 1;
  
  _nTotalFTDHits = 0;
  _sectorsFTD.clear();
  _sectorsFTD.resize(2*_nlayersFTD*_nPhiFTD);
  
  // Reading in FTD Pixel Hits Collection
  //^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
  const edm4hep::TrackerHitCollection* hitFTDPixelCol = nullptr;
  try {
    hitFTDPixelCol = _inFTDPixelColHdl.get();
  }
  catch ( GaudiException &e ) {
    debug() << "Collection " << _inFTDPixelColHdl.fullKey() << " is unavailable in event " << _nEvt << endmsg;
    success = 0;
  }
  
  if(hitFTDPixelCol){
    //LCCollection * hitCollection = evt->getCollection(_FTDPixelHitCollection.c_str());
    
    //_colNamesTrackerHits[hitCollection] = _FTDPixelHitCollection;    
    // _colTrackerHits.push_back(hitCollection);
    
    int nelem = hitFTDPixelCol->size();
    
    debug() << "Number of FTD Pixel Hits = " << nelem << endmsg;
    _nTotalFTDHits = nelem;
    
    //for (int ielem=0; ielem<nelem; ++ielem) {
    for(auto hit : *hitFTDPixelCol){  
    // edm4hep::TrackerHit* hit = hitFTDPixelCol->at(ielem);
      
      TrackerHitExtended * hitExt = new TrackerHitExtended( hit );
      
      //gear::Vector3D U(1.0,hit->getU()[1],hit->getU()[0],gear::Vector3D::spherical);
      //gear::Vector3D V(1.0,hit->getV()[1],hit->getV()[0],gear::Vector3D::spherical);
      gear::Vector3D U(1.0,hit.getCovMatrix()[1],hit.getCovMatrix()[0],gear::Vector3D::spherical);
      gear::Vector3D V(1.0,hit.getCovMatrix()[4],hit.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(V.dot(Z)) > eps ) {
	error() << "SiliconTrackingAlg: VXD Hit measurment vectors V is not in the global X-Y plane. \n\n  exit(1) called from file " << __FILE__ << " and line " << __LINE__ << endmsg;
      exit(1);
      }
      
      if( fabs(U.dot(Z)) > eps ) {
	error() << "SiliconTrackingAlg: VXD 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);
      }
      
      // SJA:FIXME Here dU and dV are almost certainly dX and dY ... should test ...
      //double point_res_rphi = sqrt( hit->getdU()*hit->getdU() + hit->getdV()*hit->getdV() );
      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 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));
    
      double pos[3];
      
      for (int i=0; i<3; ++i) {
	pos[i] = hit.getPosition()[i];
      }
      
      double Phi = atan2(pos[1],pos[0]);
      if (Phi < 0.) Phi = Phi + TWOPI;
      
      // 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) {
	error() << "SiliconTrackingAlg => fatal error in FTD : layer is outside allowed range : " << layer << " number of layers = " << _nlayersFTD <<  endmsg;
	exit(1);
      }
      
      int iPhi = int(Phi/_dPhiFTD);
      
      int side = getSideID(&hit);
      int iSemiSphere = 0;
    
      if (side > 0) 
	iSemiSphere = 1;
      
      int iCode = iSemiSphere + 2*layer + 2*_nlayersFTD*iPhi;
      _sectorsFTD[iCode].push_back( hitExt );
      
      debug() << " FTD Pixel Hit added : @ " << pos[0] << " " << pos[1] << " " << pos[2] << " drphi " << hitExt->getResolutionRPhi() << " dz " << hitExt->getResolutionZ() << "  iPhi = " << iPhi <<  " iSemiSphere "  << iSemiSphere << " iCode = " << iCode << "  layer = " << layer << endmsg;  
    }
  }
  // Reading out FTD SpacePoint Collection
  //^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
  const edm4hep::TrackerHitCollection* hitFTDSpacePointCol = nullptr;
  try {
    hitFTDSpacePointCol = _inFTDSpacePointColHdl.get();
  }
  catch ( GaudiException &e ) {
    debug() << "Collection " << _inFTDSpacePointColHdl.fullKey() << " is unavailable in event " << _nEvt << endmsg;
    success = 0;
  }

  if(hitFTDSpacePointCol){
    //LCCollection * hitCollection = evt->getCollection(_FTDSpacePointCollection.c_str());
    
    //_colNamesTrackerHits[hitCollection] = _FTDSpacePointCollection;
    //_colTrackerHits.push_back(hitCollection);
    
    int nelem = hitFTDSpacePointCol->size();
    
    debug() << "Number of FTD SpacePoints = " << nelem << endmsg;
    _nTotalFTDHits += nelem;
    
    //for (int ielem=0; ielem<nelem; ++ielem) {
    for(auto hit : *hitFTDSpacePointCol){
    //edm4hep::TrackerHit* hit =  hitFTDSpacePointCol->at(ielem);
      
      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));
      
      double pos[3];
      
      for (int i=0; i<3; ++i) {
        pos[i] = hit.getPosition()[i];
      }
      
      double Phi = atan2(pos[1],pos[0]);
      if (Phi < 0.) Phi = Phi + TWOPI;
      
      // 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) {
        error() << "SiliconTrackingAlg => fatal error in FTD : layer is outside allowed range : " << layer << " number of layers = " << _nlayersFTD <<  endmsg;
        exit(1);
      }
      
      int iPhi = int(Phi/_dPhiFTD);
      
      int side = getSideID(&hit);
      int iSemiSphere = 0;
      
      if (side > 0) 
        iSemiSphere = 1;
      
      int iCode = iSemiSphere + 2*layer + 2*_nlayersFTD*iPhi;
      _sectorsFTD[iCode].push_back( hitExt );
      
      debug() << " FTD SpacePoint Hit added : @ " << pos[0] << " " << pos[1] << " " << pos[2] << " drphi " << hitExt->getResolutionRPhi() << " dz " << hitExt->getResolutionZ() << "  iPhi = " << iPhi <<  " iSemiSphere "  << iSemiSphere << " iCode = " << iCode << "  layer = " << layer << endmsg;  
      
    }
  }
  
  for (unsigned i=0; i<_sectorsFTD.size(); ++i) {
    int nhits = _sectorsFTD[i].size();
    if( nhits != 0 ) debug() << " Number of Hits in FTD Sector " << i << " = " << _sectorsFTD[i].size() << endmsg;
    if (nhits > _max_hits_per_sector) {
      for (unsigned ihit=0; ihit<_sectorsFTD[i].size(); ++ihit) {
        delete _sectorsFTD[i][ihit];
      } 
      _sectorsFTD[i].clear();
      if( nhits != 0 ) error()  << " \n ### Number of Hits in FTD Sector " << i << " = " << nhits << " : Limit is set to " << _max_hits_per_sector << " : This sector will be dropped from track search, and QualityCode set to \"Poor\" " << endmsg;
      
      _output_track_col_quality = _output_track_col_quality_POOR;
      
    }
    
  }
  debug() << "FTD initialized" << endmsg;
  return success;
}

int SiliconTrackingAlg::InitialiseVTX() {
  //std::cout << "fucd================" << std::endl;
  _nTotalVTXHits = 0;
  _nTotalSITHits = 0;
  _sectors.clear();
  _sectors.resize(_nLayers*_nDivisionsInPhi*_nDivisionsInTheta);
  //std::cout << "fucd================" << std::endl;
  int success = 1;
  // Reading out VTX Hits Collection
  //^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^  
  const edm4hep::TrackerHitCollection* hitVTXCol = nullptr;
  try {
    hitVTXCol = _inVTXColHdl.get();
  }
  catch ( GaudiException &e ) {
    debug() << "Collection " << _inVTXColHdl.fullKey() << " is unavailable in event " << _nEvt << endmsg;
    success = 0;
  }
  if(hitVTXCol){
    //LCCollection * hitCollection = evt->getCollection(_VTXHitCollection.c_str());
    
    //_colNamesTrackerHits[hitCollection] = _VTXHitCollection;
    //_colTrackerHits.push_back(hitCollection);
    //std::cout << "fucd================1" << std::endl;
    int nelem = hitVTXCol->size();
    //std::cout << "fucd================2" << std::endl;
    debug() << "Number of VTX hits = " << nelem << endmsg;
    _nTotalVTXHits = nelem;
    
    for (int ielem=0; ielem<nelem; ++ielem) {
    //for(auto hit : *hitVTXCol){ 
      edm4hep::TrackerHit hit = hitVTXCol->at(ielem);
      //_allHits.push_back(hit);
      //gear::Vector3D U(1.0,hit->getU()[1],hit->getU()[0],gear::Vector3D::spherical);
      //gear::Vector3D V(1.0,hit->getV()[1],hit->getV()[0],gear::Vector3D::spherical);
      gear::Vector3D U(1.0,hit.getCovMatrix()[1],hit.getCovMatrix()[0],gear::Vector3D::spherical);
      gear::Vector3D V(1.0,hit.getCovMatrix()[4],hit.getCovMatrix()[3],gear::Vector3D::spherical);
      gear::Vector3D Z(0.0,0.0,1.0);
      //debug() << "covMatrix : " << hit->getCovMatrix()[0] << " " << hit->getCovMatrix()[1] << endmsg;
      const float eps = 1.0e-07;
      // V must be the global z axis 
      if( fabs(1.0 - V.dot(Z)) > eps ) {
        error() << "SiliconTrackingAlg: VXD 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);
      }
      
      if( fabs(U.dot(Z)) > eps ) {
        error() << "SiliconTrackingAlg: VXD 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);
      }
      //std::cout << "fucd: " << &hit << " " << &_allHits.back() << std::endl;
      //TrackerHitExtended * hitExt = new TrackerHitExtended( &_allHits.back() );
      TrackerHitExtended * hitExt = new TrackerHitExtended(hit);
      std::cout << "Saved TrackerHit pointer in TrackerHitExtended " << ielem << ": " << hitExt->getTrackerHit() << std::endl;
      //std::cout << (&_allHits.back())->getPosition()[0] << " " << hit.getCovMatrix()[2] << " " << hit.getCovMatrix()[5] << std::endl;
            
      // SJA:FIXME: just use planar res for now
      hitExt->setResolutionRPhi(hit.getCovMatrix()[2]);
      hitExt->setResolutionZ(hit.getCovMatrix()[5]);
      
      // 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));
      
      double pos[3];
      double radius = 0;
      
      for (int i=0; i<3; ++i) {
        pos[i] = hit.getPosition()[i];
        radius += pos[i]*pos[i];
      }
      
      radius = sqrt(radius);
      
      double cosTheta = pos[2]/radius;
      double Phi = atan2(pos[1],pos[0]);
      
      if (Phi < 0.) Phi = Phi + TWOPI;
      
      // get the layer number
      int layer = getLayerID(&hit);
      
      if (layer < 0 || layer >= _nLayers) {
        error() << "SiliconTrackingAlg => fatal error in VTX : layer is outside allowed range : " << layer << endmsg;
        exit(1);
      }
      
      int iPhi = int(Phi/_dPhi);
      int iTheta = int ((cosTheta + double(1.0))/_dTheta);
      int iCode = layer + _nLayers*iPhi + _nLayers*_nDivisionsInPhi*iTheta;      
      _sectors[iCode].push_back( hitExt );
      
      debug() << " VXD Hit " <<  hit.id() << " added : @ " << pos[0] << " " << pos[1] << " " << pos[2] << " drphi " << hitExt->getResolutionRPhi() << " dz " << hitExt->getResolutionZ() << "  iPhi = " << iPhi <<  " iTheta "  << iTheta << " iCode = " << iCode << "  layer = " << layer << endmsg;  
      
    }
  }
  
  if (_useSIT > 0 ) {
    const edm4hep::TrackerHitCollection* hitSITCol = nullptr;
    try {
      hitSITCol = _inSITColHdl.get();
    }
    catch ( GaudiException &e ) {
      debug() << "Collection " << _inSITColHdl.fullKey() << " is unavailable in event " << _nEvt << endmsg;
      success = 0;
    }
    if(hitSITCol){
      //LCCollection *hitCollection = evt->getCollection(_SITHitCollection.c_str());
      
      //_colNamesTrackerHits[hitCollection] = _SITHitCollection;
      //_colTrackerHits.push_back(hitCollection);
      
      int nelem = hitSITCol->size();
      
      debug() << "Number of SIT hits = " << nelem << endmsg;
      _nTotalSITHits = nelem;
      
      //TrackerHit*          trkhit   = 0;
      //TrackerHitPlane*     trkhit_P = 0;
      //TrackerHitZCylinder* trkhit_C = 0;
      
      double drphi(NAN);
      double dz(NAN);
      
      //for (int ielem=0; ielem<nelem; ++ielem) {
      for(auto 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
        
	//const edm4hep::TrackerHit* trkhit = hitSITCol->at(ielem);
        
        int layer = getLayerID(&trkhit);
        
        // VXD and SIT are treated as one system so SIT layers start from _nLayersVTX
        layer = layer + _nLayersVTX;
        
        if (layer < 0 || layer >= _nLayers) {
          error() << "SiliconTrackingAlg => 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 ] ) {
          
          error() << "SiliconTrackingAlg: 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
	// by fucd
        //else if ( ( trkhit_P = dynamic_cast<TrackerHitPlane*>( hitCollection->getElementAt( ielem ) ) ) )  {
	else if( UTIL::BitSet32( trkhit.getType() )[ 31 ]){
          // 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);
	  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() << "SiliconTrackingAlg: 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() << "SiliconTrackingAlg: 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);
          }
          
          //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      
	/* by fucd
        else if ( ( trkhit_C = dynamic_cast<TrackerHitZCylinder*>( hitCollection->getElementAt( ielem ) ) ) ) {
          
          drphi = trkhit_C->getdRPhi();
          dz    = trkhit_C->getdZ();
          
        } 
	*/
        // 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));
        
        double pos[3];
        double radius = 0;
        
        for (int i=0; i<3; ++i) {
          pos[i] = trkhit.getPosition()[i];
          radius += pos[i]*pos[i];
        }
        
        radius = sqrt(radius);
        
        double cosTheta = pos[2]/radius;
        double Phi = atan2(pos[1],pos[0]);
        
        if (Phi < 0.) Phi = Phi + TWOPI;
        
        int iPhi = int(Phi/_dPhi);
        int iTheta = int ((cosTheta + double(1.0))/_dTheta);
        int iCode = layer + _nLayers*iPhi + _nLayers*_nDivisionsInPhi*iTheta;      
        _sectors[iCode].push_back( hitExt );
        
        debug() << " SIT Hit " <<  trkhit.id() << " added : @ " << pos[0] << " " << pos[1] << " " << pos[2] << " drphi " << hitExt->getResolutionRPhi() << " dz " << hitExt->getResolutionZ() << "  iPhi = " << iPhi <<  " iTheta "  << iTheta << " iCode = " << iCode << "  layer = " << layer << endmsg;  
        
      }
    }    
  }
  
  
  for (unsigned i=0; i<_sectors.size(); ++i) {
    int nhits = _sectors[i].size();
    if( nhits != 0 ) debug() << " Number of Hits in VXD/SIT Sector " << i << " = " << _sectors[i].size() << endmsg;
    if (nhits > _max_hits_per_sector) {
      for (unsigned ihit=0; ihit<_sectors[i].size(); ++ihit) {
        delete _sectors[i][ihit];
      }
      _sectors[i].clear();
      if( nhits != 0 ) error()  << " \n ### Number of Hits in VXD/SIT Sector " << i << " = " << nhits << " : Limit is set to " << _max_hits_per_sector << " : This sector will be dropped from track search, and QualityCode set to \"Poor\" " << endmsg;
      
      _output_track_col_quality = _output_track_col_quality_POOR;
    }
  }
  debug() << "VXD initialized" << endmsg;
  return success; 
}

StatusCode  SiliconTrackingAlg::finalize(){
  
  delete _fastfitter ; _fastfitter = 0;
  delete _encoder ; _encoder = 0;
  //delete _trksystem ; _trksystem = 0;
  //delete _histos ; _histos = 0;
  info() << "Processed " << _nEvt << " events " << endmsg;
  return GaudiAlgorithm::finalize();
}


void SiliconTrackingAlg::ProcessOneSector(int iPhi, int iTheta) {
  
  int counter = 0 ;
  
  int iPhi_Up    = iPhi + 1;
  int iPhi_Low   = iPhi - 1;
  int iTheta_Up  = iTheta + 1; 
  int iTheta_Low = iTheta - 1;
  if (iTheta_Low < 0) iTheta_Low = 0;
  if (iTheta_Up  >= _nDivisionsInTheta) iTheta_Up = _nDivisionsInTheta-1;
  
  int nComb = int( _Combinations.size() / 3 ); // number of triplet combinations
                                               //  std::cout << iPhi << " " << iTheta << " " << _nEvt << endmsg;
  int iNC = 0;
  
  for (int iComb=0; iComb < nComb; ++iComb) { // loop over triplets
    
    int nLR[3];
    
    for (int iS=0; iS<3; ++iS) {
      nLR[iS] = _Combinations[iNC];
      iNC++;
    }    
    
    //std::cout << iPhi << " " << iTheta << " " << nLR[0] << " " << nLR[1] << " " << nLR[2] << " " << std::endl;
    
    // index of theta-phi bin of outer most layer
    int iCode = nLR[0] + _nLayers*iPhi +  _nLayers*_nDivisionsInPhi*iTheta;
    
    //std::cout << "size of vector = " << _sectors.size() << " iCode = " << iCode << std::endl;
    
    // get the all the hits in the outer most theta-phi bin 
    
    TrackerHitExtendedVec& hitVecOuter =  _sectors[iCode];
    
    int nHitsOuter = int(hitVecOuter.size());
    if (nHitsOuter > 0) {
      
      //std::cout << " " << iPhi << " " << iTheta << " " << nLR[0] << " " << nLR[1] << " " << nLR[2] << " size of vector = " << hitVecOuter.size() << std::endl;
      
      for (int ipMiddle=iPhi_Low; ipMiddle<iPhi_Up+1;ipMiddle++) { // loop over phi in the Middle
        
        for (int itMiddle=iTheta_Low; itMiddle<iTheta_Up+1;itMiddle++) { // loop over theta in the Middle 
          
          int iPhiMiddle = ipMiddle;
          
          // catch wrap-around
          if (ipMiddle < 0) iPhiMiddle = _nDivisionsInPhi-1;          
          if (ipMiddle >= _nDivisionsInPhi) iPhiMiddle = ipMiddle - _nDivisionsInPhi;
          
          // index of current theta-phi bin of middle layer
          iCode = nLR[1] + _nLayers*iPhiMiddle +  _nLayers*_nDivisionsInPhi*itMiddle;
          
          // get the all the hits in the current middle theta-phi bin 
          TrackerHitExtendedVec& hitVecMiddle = _sectors[iCode];
          
          int nHitsMiddle = int(hitVecMiddle.size());
          
          // determine which inner theta-phi bins to look in
          
          int iPhiLowInner = iPhi_Low;
          int iPhiUpInner = iPhi_Up;
          int iThetaLowInner = iTheta_Low;
          int iThetaUpInner = iTheta_Up;        
          
          // test to see if this is the core bin of the current search
          // if so, look into the neigboring bins in the inner layer
          if (ipMiddle == iPhi && itMiddle==iTheta) { 
            iPhiLowInner = iPhi_Low;
            iPhiUpInner  = iPhi_Up;
            iThetaLowInner = iTheta_Low;
            iThetaUpInner = iTheta_Up;
          }
          else { 
            int difP = abs(ipMiddle-iPhi); //  number of phi bins from core: can only be 1 or 0 due to hard coded 1 above
            int difT = abs(itMiddle-iTheta);// number of theta bins from core: can only be 1 or 0 due to hard coded 1 above
            int minP = min(ipMiddle,iPhi);   // min phi: core bin or current phi bin middle
            int minT = min(itMiddle,iTheta); // min theta: core bin or current theta bin middle
            int maxP = max(ipMiddle,iPhi);   // max phi: core bin or current phi bin middle
            int maxT = max(itMiddle,iTheta); // max theta: core bin or current theta bin middle
            
            if (difP==1 && difT==1) { // if the diffence is a single bin in both phi and theta : only look in the bin adjacent to the core bin  
              iPhiLowInner = minP;
              iPhiUpInner = maxP;
              iThetaLowInner = minT;
              iThetaUpInner = maxT;
            }
            if (difP==0) { // must be +/-1 theta : only look in bins adjacent to the middle bin