FullLDCTrackingAlg.cpp

        helix.Initialize_Canonical(phi0,d0,z0,omega,tanLambda,_bField);
        float distance = helix.getDistanceToPoint(pos,dcut);
        
        debug() << "AssignSiHitsToTracks : distance =  " << distance << " cut = " << dcut << endmsg;
        
        if (distance<dcut) {
          TrackHitPair * trkHitPair = 
          new TrackHitPair(trkExt,trkHitExt,distance);
          pairs.push_back(trkHitPair);
          flagTrack[trkExt] = true;
          flagHit[trkHitExt] = true;
        }
      }
    }
  }
  
  int nPairs = int(pairs.size());
  
  debug() << "AssignSiHitsToTracks : Number of track hit pairs to try =  " << nPairs << endmsg;
  
  if (nPairs>0) {
    
    SortingTrackHitPairs(pairs);
    
    for (int iP=0;iP<nPairs;++iP) {
      
      TrackHitPair * trkHitPair = pairs[iP];
      TrackExtended * trkExt = trkHitPair->getTrackExtended();
      TrackerHitExtended * trkHitExt = 
      
      trkHitPair->getTrackerHitExtended();
      
      if (flagTrack[trkExt] && flagHit[trkHitExt]) {              
        
        // get the hits already assigned to the track
        TrackerHitExtendedVec hitsInTrack = trkExt->getTrackerHitExtendedVec();
        
        int nTotH = int(hitsInTrack.size());
        int nHitsInFit = 0;
        
        for (int iTH=0;iTH<nTotH;++iTH) {
          
          TrackerHitExtended * hitInTrack = hitsInTrack[iTH];
          
          // count the number of hits used in the fit
          if (hitInTrack->getUsedInFit()) {
            nHitsInFit++; 
          }
        }
        
        int iHitInFit = 0;
        
        // add the previously used hits from the track to the vectors 
        
        ConstTrackerHitVec trkHits;
        
        for (int iHit=0;iHit<nTotH;++iHit) {
          
          TrackerHitExtended * hitInTrack = hitsInTrack[iHit];
          if (hitInTrack->getUsedInFit()) {
	    edm4hep::ConstTrackerHit hit = hitInTrack->getTrackerHit();
            iHitInFit++;
            if(hit.isAvailable()) { 
              trkHits.push_back(hit);   
            }
            else{
              throw EVENT::Exception( std::string("FullLDCTrackingAlg::AssignSiHitsToTracks: TrackerHit pointer == NULL ")  ) ;
            }
          }
        }
        
        // add the hit to be attached to the vectors 
	edm4hep::ConstTrackerHit remainHit = trkHitExt->getTrackerHit();
        iHitInFit++;
        trkHits.push_back(remainHit);
        
        
        double chi2_D;
        int ndf;
        
        if( trkHits.size() < 3 ) return ;
        
        // sort the hits in R
        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() << "AssignSiHitsToTracks: Start Fitting: AddHits: number of hits to fit " << trkHits.size() << endmsg;
        
        MarlinTrk::IMarlinTrack* marlin_trk = _trksystem->createTrack();
        
        TrackState pre_fit ;
        pre_fit.D0 = trkExt->getD0();
        pre_fit.phi = trkExt->getPhi();
        pre_fit.Z0 = trkExt->getZ0();
        pre_fit.omega = trkExt->getOmega();
        pre_fit.tanLambda = trkExt->getTanLambda();
        
        float ref[3];
        ref[0]=ref[1]=ref[2]=0.0;
        
        pre_fit.referencePoint = ref;
        
        pre_fit.location = 1/*lcio::TrackState::AtIP*/;
        
        // 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;
        
        int error = MarlinTrk::createFit( trkHits, marlin_trk, &pre_fit, _bField, IMarlinTrack::backward , _maxChi2PerHit );
        
        if ( error != IMarlinTrack::success ) {
          debug() << "FullLDCTrackingAlg::AssignSiHitsToTracks: creation of fit fails with error " << error << endmsg;
          
          delete marlin_trk ;
          continue ;
	}
        
        std::vector<std::pair<edm4hep::ConstTrackerHit , double> > outliers ;
        marlin_trk->getOutliers(outliers);
        
        float outlier_pct = outliers.size()/float(trkHits.size());
        
        debug()<< "FullLDCTrackingAlg::AssignSiHitsToTracks: percentage of outliers " << outlier_pct << endmsg;
        
        if ( outlier_pct > _maxAllowedPercentageOfOutliersForTrackCombination) {
	  debug() << "FullLDCTrackingAlg::AssignSiHitsToTracks: percentage of outliers " << outlier_pct << " is greater than cut maximum: " << _maxAllowedPercentageOfOutliersForTrackCombination << endmsg;
          delete marlin_trk ;
          continue ;
          
        }
                
        edm4hep::Vector3d point(0.,0.,0.); // nominal IP
        int return_code = 0;
        
        TrackState trkState ;
        return_code = marlin_trk->propagate(point, trkState, chi2_D, ndf ) ;
        
        delete marlin_trk ;
        
        if ( error != IMarlinTrack::success ) {
	  debug() << "FullLDCTrackingAlg::AssignSiHitsToTracks: propagate to IP fails with error " << error << endmsg;
	  continue ;
	}
        
        if ( ndf < 0  ) {
	  debug() << "FullLDCTrackingAlg::AssignSiHitsToTracks: Fit failed : NDF is less that zero  " << ndf << endmsg;
	  continue ;
	}
        
        float chi2Fit = chi2_D/float(ndf);
        
        if ( chi2Fit > _chi2FitCut ) {
	  debug() << "FullLDCTrackingAlg::AssignSiHitsToTracks: track fail Chi2 cut of " << _chi2FitCut << " chi2 of track = " <<  chi2Fit << endmsg;
	  continue ;
	}
                
        // note trackAR which is of type TrackExtended, only takes fits with ref point = 0,0,0 
        trkExt->setOmega(trkState.omega);
        trkExt->setTanLambda(trkState.tanLambda);
        trkExt->setPhi(trkState.phi);
        trkExt->setD0(trkState.D0);
        trkExt->setZ0(trkState.Z0);
        
        float cov[15];
        
        for (int i = 0 ; i<15 ; ++i) {
          cov[i] = trkState.covMatrix[i];
        }
        
        trkExt->setCovMatrix(cov);
        trkExt->setChi2(chi2_D);
        trkExt->setNDF(ndf);
        
        trkExt->addTrackerHitExtended( trkHitExt );
        trkHitExt->setTrackExtended( trkExt );
        trkHitExt->setUsedInFit( true );
        flagTrack[trkExt] = false;
        flagHit[trkHitExt] = false;
                
        debug() << "AssignSiHitsToTracks: Hit " << trkHitExt << " successfully assigned to track " << trkExt << endmsg;
      }
    }
    
    for (int iP=0;iP<nPairs;++iP) {
      TrackHitPair * trkHitPair = pairs[iP];
      delete trkHitPair;
    }
    
    pairs.clear();
    
  }
}

/*
void FullLDCTrackingAlg::PrintOutMerging(TrackExtended * firstTrackExt, TrackExtended * secondTrackExt, int iopt) {
  // iopt = 1 merged Si and TPC merging
  // iopt = 2 merged Si and TPC using forced merging
  // iopt = 3 merged TPC segments merging
  // iopt = 4 merged Comb Si and TPC merging
  // iopt = 5 merged Comb TPC and TPC merging
  // iopt = 6 unmerged TPC and Si segments ( when using soft merging)
  // iopt = 7 unmerged TPC and Si segments ( when using forced merging)
  // iopt = 8 unmerged Comb and TPC
  // iopt = 9 unmerged TPC segments
  
  char strg[200];
  
  try {
    
    Track * firstTrack = firstTrackExt->getTrack();
    Track * secondTrack = secondTrackExt->getTrack();
    
    std::string firstColName  = _TPCTrackMCPCollName;
    std::string secondColName = _TPCTrackMCPCollName;
    
    if (iopt==1) {
      secondColName = _SiTrackMCPCollName;
    }
    else if (iopt==2) {
      secondColName = _SiTrackMCPCollName;
    }
    else if (iopt==3) {
      secondColName = _TPCTrackMCPCollName;
    }
    else if (iopt==4) {
      secondColName = _SiTrackMCPCollName;
    }
    else if (iopt==5) {
      secondColName = _TPCTrackMCPCollName;
    }
    else if (iopt==6) {
      secondColName = _SiTrackMCPCollName;
    }
    else if (iopt==7) {
      secondColName = _SiTrackMCPCollName;
    }
    else if (iopt==8) {
      secondColName = _SiTrackMCPCollName;
    }    
    else {
      secondColName = _TPCTrackMCPCollName;
    }
    
    
    // get the relation collections, this should be done only once for each event ...
    LCCollection * firstCol = _evt->getCollection(firstColName.c_str());
    LCCollection * secondCol = _evt->getCollection(secondColName.c_str());
    
    // get navigators
    LCRelationNavigator firstNav(firstCol);
    LCRelationNavigator secondNav(secondCol);

    
    // get the MCParticles with the greatest weight for the first track
    LCObjectVec firstVec = firstNav.getRelatedToObjects(firstTrack);
    FloatVec firstWeights = firstNav.getRelatedToWeights(firstTrack);

    LCObject * firstMCP = NULL;

    float firstWght = 0;
    int nObj = firstVec.size();
    for (int iObj=0;iObj<nObj;++iObj) {
      if (firstWeights[iObj]>firstWght) {
        firstWght = firstWeights[iObj];
        firstMCP = firstVec[iObj];
      }
    }
    
    
    // get the MCParticles with the greatest weight for the second track
    LCObjectVec secondVec = secondNav.getRelatedToObjects(secondTrack);
    FloatVec secondWeights = secondNav.getRelatedToWeights(secondTrack);

    LCObject * secondMCP = NULL;

    float secondWght = 0;
    nObj = secondVec.size();
    for (int iObj=0;iObj<nObj;++iObj) {
      if (secondWeights[iObj]>secondWght) {
        secondWght = secondWeights[iObj];
        secondMCP = secondVec[iObj];
      }
    }
    

    // get the track parameters for both tracks and get the 3-momentum using the HelixClass
    float d0First = firstTrackExt->getD0();
    float z0First = firstTrackExt->getZ0();
    float omegaFirst = firstTrackExt->getOmega();
    float tanLFirst = firstTrackExt->getTanLambda();
    float phi0First = firstTrackExt->getPhi();
    
    float d0Second = secondTrackExt->getD0();
    float z0Second = secondTrackExt->getZ0();
    float omegaSecond = secondTrackExt->getOmega();
    float tanLSecond = secondTrackExt->getTanLambda();
    float phi0Second = secondTrackExt->getPhi();            
    
    HelixClass firstHelix;
    firstHelix.Initialize_Canonical(phi0First,d0First,z0First,omegaFirst,tanLFirst,_bField);
    float pxFirst = firstHelix.getMomentum()[0];
    float pyFirst = firstHelix.getMomentum()[1];
    float pzFirst = firstHelix.getMomentum()[2];            
    
    HelixClass secondHelix;
    secondHelix.Initialize_Canonical(phi0Second,d0Second,z0Second,omegaSecond,tanLSecond,_bField);
    float pxSecond = secondHelix.getMomentum()[0];
    float pySecond = secondHelix.getMomentum()[1];
    float pzSecond = secondHelix.getMomentum()[2];          

    
    // get the momentum differences
    float dPx = pxFirst + pxSecond;
    float dPy = pyFirst + pySecond;
    float dPz = pzFirst + pzSecond;
    
    float dPplus  = sqrt(dPx*dPx+dPy*dPy+dPz*dPz);
    
    dPx = pxFirst - pxSecond;
    dPy = pyFirst - pySecond;
    dPz = pzFirst - pzSecond;
    
    float dPminus = sqrt(dPx*dPx+dPy*dPy+dPz*dPz);
    
    // get momentum for each track
    float pFirst  = sqrt(pxFirst * pxFirst+ pyFirst* pyFirst+ pzFirst*pzFirst);
    float pSecond = sqrt(pxSecond*pxSecond+pySecond*pySecond+pzSecond*pzSecond);
    
    float ptFirst  = sqrt(pxFirst * pxFirst+ pyFirst* pyFirst);
    float ptSecond = sqrt(pxSecond*pxSecond+pySecond*pySecond);
    
    
    const float sigmaPOverPFirst  = sqrt( firstTrackExt->getCovMatrix()[5])/fabs(omegaFirst);
    const float sigmaPOverPSecond = sqrt(secondTrackExt->getCovMatrix()[5])/fabs(omegaSecond);

    const float sigmaPFirst  =  pFirst*sigmaPOverPFirst;
    const float sigmaPSecond = pSecond*sigmaPOverPSecond;
    
    
    float den = pFirst;

    if (pSecond<pFirst)
      den = pSecond;
    
    dPplus  = dPplus/den;
    dPminus = dPminus/den; 
    
    // now check if this was a Erroneous merger ...
    if (firstMCP!=secondMCP && iopt < 6) {
      
      if (iopt==1) {
        streamlog_out(DEBUG4) << "Erroneous merging of Si and TPC segments (iopt=1) mcp first = " << firstMCP << " mcp second = " << secondMCP << " --->" << endmsg;
      }
      else if (iopt==2) {
        streamlog_out(DEBUG4) << "Erroneous merging of Si and TPC segments (iopt=2) mcp first = " << firstMCP << " mcp second = " << secondMCP << " --->" << endmsg;
      }
      else if (iopt==3) {
        streamlog_out(DEBUG4) << "Erroneous merging of TPC segments (iopt=3) mcp first = " << firstMCP << " mcp second = " << secondMCP << " ---> " << endmsg;
      }
      else if (iopt==4) {
        streamlog_out(DEBUG4) << "Erroneous merging of combSi segment with uncombTPC segment (iopt=4) mcp first = " << firstMCP << " mcp second = " << secondMCP << " ---> " << endmsg;
      }
      else {
        streamlog_out(DEBUG4) << "Erroneous merging of combTPC segment with uncombTPC segment (iopt=5) mcp first = " << firstMCP << " mcp second = " << secondMCP << " --->" << endmsg;
      }
      
      
      streamlog_out(DEBUG4) << "    p         error      pt       D0      Z0     Px      Py      Pz      wieght" << endmsg;
      
      sprintf(strg,"%7.2f +- %7.2f   %7.2f  %7.1f %7.1f %7.2f %7.2f %7.2f  ",
              pFirst, sigmaPFirst, ptFirst, d0First,z0First,pxFirst,pyFirst,pzFirst);
      streamlog_out(DEBUG4) << strg;
      
      sprintf(strg,"  %5.3f\n",firstWght);
      streamlog_out(DEBUG4) << strg;
      
      
      sprintf(strg,"%7.2f +- %7.2f   %7.2f  %7.1f %7.1f %7.2f %7.2f %7.2f  ",
              pSecond, sigmaPSecond, ptSecond, d0Second,z0Second,pxSecond,pySecond,pzSecond);
      streamlog_out(DEBUG4) << strg;
      sprintf(strg,"  %5.3f\n",secondWght);
      streamlog_out(DEBUG4) << strg;
      
      streamlog_out(DEBUG4) << "Difference in +P = " << dPplus << "  -P = " << dPminus << endmsg;
      
      TrackExtended * combinedTrack = CombineTracks(firstTrackExt,secondTrackExt, _maxAllowedPercentageOfOutliersForTrackCombination, true);
      
      if(combinedTrack != NULL){
        streamlog_out(DEBUG4) << "CombinedTrack " << combinedTrack->getNDF() << " c.f. " << firstTrackExt->getNDF()+secondTrackExt->getNDF()+5 << endmsg;
        delete combinedTrack->getGroupTracks();
        delete combinedTrack;
      }else{
        streamlog_out(DEBUG4) << "Could not combine track " << endmsg;
      }
      streamlog_out(DEBUG4) << " Overlap = " << SegmentRadialOverlap(firstTrackExt, secondTrackExt) << " veto = " << VetoMerge(firstTrackExt, secondTrackExt) << endmsg;
      
      streamlog_out(DEBUG4) << endmsg;
      
    }
    
    // ... or if it was an incorrect TPC to TPC rejection ...
    else if (firstMCP==secondMCP && ( (iopt==8) || (iopt==9) ) ) {
      
      float deltaOmega = _dOmegaForMerging;
      float deltaAngle = _angleForMerging;
      if (iopt==8) {
        streamlog_out(DEBUG4) << "Unmerged TPC and Comb segments (iopt=8) --->" << endmsg;
      }
      else {
        streamlog_out(DEBUG4) << "Unmerged TPC segments (iopt=9) --->" << endmsg;
      }
      
      float qFirst = PIOVER2 - atan(tanLFirst);
      float qSecond = PIOVER2 - atan(tanLSecond);
      float dOmega = fabs((omegaFirst-omegaSecond)/omegaSecond);
      float angle = (cos(phi0First)*cos(phi0Second)+sin(phi0First)*sin(phi0Second))*
      sin(qFirst)*sin(qSecond)+cos(qFirst)*cos(qSecond);
      angle = acos(angle);
      
      
      streamlog_out(DEBUG4) << " dOmegaCut = " << deltaOmega
      << " AngleCut = " << deltaAngle
      << " dOmega = " << dOmega
      << " angle = " << angle << endmsg;
      
      
      streamlog_out(DEBUG4) << "    p         error      pt       D0      Z0     Px      Py      Pz      wieght" << endmsg;
      
      sprintf(strg,"%7.2f +- %7.2f   %7.2f  %7.1f %7.1f %7.2f %7.2f %7.2f  ",
              pFirst, sigmaPFirst, ptFirst, d0First,z0First,pxFirst,pyFirst,pzFirst);
      streamlog_out(DEBUG4) << strg;

      sprintf(strg,"  %5.3f\n",firstWght);
      streamlog_out(DEBUG4) << strg;

      
      sprintf(strg,"%7.2f +- %7.2f   %7.2f  %7.1f %7.1f %7.2f %7.2f %7.2f  ",
              pSecond, sigmaPSecond, ptSecond, d0Second,z0Second,pxSecond,pySecond,pzSecond);
      streamlog_out(DEBUG4) << strg;

      sprintf(strg,"  %5.3f\n",secondWght);
      streamlog_out(DEBUG4) << strg;
      
      streamlog_out(DEBUG4) << "Difference in +P = " << dPplus << "  -P = " << dPminus << endmsg;
      
      TrackExtended * combinedTrack = CombineTracks(firstTrackExt,secondTrackExt, _maxAllowedPercentageOfOutliersForTrackCombination, true);
      if(combinedTrack != NULL){
        streamlog_out(DEBUG4) << "CombinedTrack " << combinedTrack->getNDF() << " c.f. " << firstTrackExt->getNDF()+secondTrackExt->getNDF()+5 << endmsg;
        delete combinedTrack->getGroupTracks();
        delete combinedTrack;
      }else{
        streamlog_out(DEBUG4) << "Could not combine track " << endmsg;
      }
      streamlog_out(DEBUG4) << " Overlap = " << SegmentRadialOverlap(firstTrackExt, secondTrackExt) << " veto = " << VetoMerge(firstTrackExt, secondTrackExt) << endmsg;
      
      streamlog_out(DEBUG4) << endmsg;
      
    }    

    // ... or if it was an incorrect TPC to Si rejection ...
    else if (firstMCP==secondMCP && ( (iopt == 6) || (iopt == 7) ) ) {
      
      float deltaOmega = _dOmegaForMerging;
      float deltaAngle = _angleForMerging;
      
      if (iopt ==6) {
        streamlog_out(DEBUG4) << "Unmerged TPC and Si segments (iopt=6) mcp first = " << firstMCP << " mcp second = " << secondMCP << " --->" << endmsg;
      }
      else {
        streamlog_out(DEBUG4) << "Unmerged TPC and Si segments (iopt=7) mcp first = " << firstMCP << " mcp second = " << secondMCP << " --->" << endmsg;
        deltaOmega = _dOmegaForForcedMerging;
        deltaAngle = _angleForForcedMerging;
      }
      
      float qFirst = PIOVER2 - atan(tanLFirst);
      float qSecond = PIOVER2 - atan(tanLSecond);
      
      float dOmega = fabs((omegaFirst-omegaSecond)/omegaSecond);
      float angle = (cos(phi0First)*cos(phi0Second)+sin(phi0First)*sin(phi0Second))*
      sin(qFirst)*sin(qSecond)+cos(qFirst)*cos(qSecond);
      angle = acos(angle);
      
      
      streamlog_out(DEBUG4) << " dOmegaCut = " << deltaOmega
      << " AngleCut = " << deltaAngle
      << " dOmega = " << dOmega
      << " angle = " << angle << endmsg;

      streamlog_out(DEBUG4) << "    p         error      pt       D0      Z0     Px      Py      Pz      wieght" << endmsg;
      
      sprintf(strg,"%7.2f +- %7.2f   %7.2f  %7.1f %7.1f %7.2f %7.2f %7.2f  ",
              pFirst, sigmaPFirst, ptFirst, d0First,z0First,pxFirst,pyFirst,pzFirst);
      streamlog_out(DEBUG4) << strg;

      sprintf(strg,"  %5.3f\n",firstWght);
      streamlog_out(DEBUG4) << strg;

      sprintf(strg,"%7.2f +- %7.2f   %7.2f  %7.1f %7.1f %7.2f %7.2f %7.2f  ",
              pSecond, sigmaPSecond, ptSecond, d0Second,z0Second,pxSecond,pySecond,pzSecond);
      streamlog_out(DEBUG4) << strg;

      sprintf(strg,"  %5.3f\n",secondWght);
      streamlog_out(DEBUG4) << strg;
      
      streamlog_out(DEBUG4) << "Difference in +P = " << dPplus << "  -P = " << dPminus << endmsg;      
      
      TrackExtended * combinedTrack = CombineTracks(firstTrackExt,secondTrackExt, _maxAllowedPercentageOfOutliersForTrackCombination, true);
      
      if(combinedTrack != NULL){
        streamlog_out(DEBUG4) << "CombinedTrack " << combinedTrack->getNDF() << " c.f. " << firstTrackExt->getNDF()+secondTrackExt->getNDF()+5 << endmsg;
        delete combinedTrack->getGroupTracks();
        delete combinedTrack;
      }else{
        streamlog_out(DEBUG4) << "Could not combine track " << endmsg;
      }
      streamlog_out(DEBUG4) << " Overlap = " << SegmentRadialOverlap(firstTrackExt, secondTrackExt) << " veto = " << VetoMerge(firstTrackExt, secondTrackExt) << endmsg;
      streamlog_out(DEBUG4) << endmsg;      
      
    }
    // ... or if it was an correct merger ...
    else if (firstMCP==secondMCP && iopt < 6 && _debug > 3) {
      //      return;
      if (iopt==1) {
        streamlog_out(DEBUG4) << "Correctly combining Si and TPC segments (iopt=1) mcp first = " << firstMCP << " mcp second = " << secondMCP << " --->" << endmsg;
      }
      else if (iopt==2) {
        streamlog_out(DEBUG4) << "Correctly merging of Si and TPC segments (iopt=2) mcp first = " << firstMCP << " mcp second = " << secondMCP << " --->" << endmsg;
      }
      else if (iopt==3) {
        streamlog_out(DEBUG4) << "Correctly merging of TPC segments (iopt=3) mcp first = " << firstMCP << " mcp second = " << secondMCP << " ---> " << endmsg;
      }
      else if (iopt==4) {
        streamlog_out(DEBUG4) << "Correctly merging of combSi segment with uncombTPC segment (iopt=4) mcp first = " << firstMCP << " mcp second = " << secondMCP << " ---> " << endmsg;
      }
      else {
        streamlog_out(DEBUG4) << "Correctly merging of combTPC segment with uncombTPC segment (iopt=5) mcp first = " << firstMCP << " mcp second = " << secondMCP << " --->" << endmsg;
      }

      streamlog_out(DEBUG4) << "    p         error      pt       D0      Z0     Px      Py      Pz      wieght" << endmsg;
      
      sprintf(strg,"%7.2f +- %7.2f   %7.2f  %7.1f %7.1f %7.2f %7.2f %7.2f  ",
              pFirst, sigmaPFirst, ptFirst, d0First,z0First,pxFirst,pyFirst,pzFirst);
      streamlog_out(DEBUG4) << strg;

      sprintf(strg,"  %5.3f\n",firstWght);
      streamlog_out(DEBUG4) << strg;
      
      sprintf(strg,"%7.2f +- %7.2f   %7.2f  %7.1f %7.1f %7.2f %7.2f %7.2f  ",
              pSecond, sigmaPSecond, ptSecond, d0Second,z0Second,pxSecond,pySecond,pzSecond);
      streamlog_out(DEBUG4) << strg;

      sprintf(strg,"  %5.3f\n",secondWght);
      streamlog_out(DEBUG4) << strg;
      
      streamlog_out(DEBUG4) << "Difference in +P = " << dPplus << "  -P = " << dPminus << endmsg;

      TrackExtended * combinedTrack = CombineTracks(firstTrackExt,secondTrackExt, _maxAllowedPercentageOfOutliersForTrackCombination, true);
      
      if(combinedTrack != NULL){
        streamlog_out(DEBUG4) << "CombinedTrack " << combinedTrack->getNDF() << " c.f. " << firstTrackExt->getNDF()+secondTrackExt->getNDF()+5 << endmsg;
      }else{
        streamlog_out(DEBUG4) << "Could not combine track " << endmsg;
      }
      delete combinedTrack->getGroupTracks();
      delete combinedTrack;
      streamlog_out(DEBUG4) << " Overlap = " << SegmentRadialOverlap(firstTrackExt, secondTrackExt) << " veto = " << VetoMerge(firstTrackExt, secondTrackExt) << endmsg;
      
      streamlog_out(DEBUG4) << endmsg;
      
    }
  }
  
  catch( ... ){};
  
}    
*/

/*
void FullLDCTrackingAlg::GeneralSorting(int * index, float * val, int direct, int nVal) {
   // Sorting of index vector in ascending (0) /descending (!=0) order of val
  
  float valOne, valTwo, valTemp;
  int   indTemp;
  for (int i=0; i<nVal; ++i) {
    index[i] = i;
  }
  
  for (int i = 0 ; i < nVal-1; i++) {
    for (int j = 0; j < nVal-i-1; j++) {      
      valOne = val[j];
      valTwo = val[j+1];
      bool order = valOne > valTwo;
      if (direct>0) 
        order = valOne <= valTwo;
      if( order )
      {
        valTemp = val[j];
        val[j] = val[j+1];
        val[j+1] = valTemp;
        indTemp = index[j];
        index[j] = index[j+1];
        index[j+1] = indTemp;
      }
    }  
  }
  
  
}
*/

int FullLDCTrackingAlg::SegmentRadialOverlap(TrackExtended* first, TrackExtended* second){
  
  
  int nTrkGrpFirst = 0;
  int nTrkGrpSecond = 0;
  ConstTrackerHitVec hitvecFirst;
  ConstTrackerHitVec hitvecSecond;
  GroupTracks * groupFirst = first->getGroupTracks();
  GroupTracks * groupSecond = second->getGroupTracks();
  
  if(groupFirst!=NULL){
    
    TrackExtendedVec tracksInGroupFirst = groupFirst->getTrackExtendedVec();
    nTrkGrpFirst = int(tracksInGroupFirst.size());
    
    for (int iTrkGrp=0;iTrkGrp<nTrkGrpFirst;++iTrkGrp) {
      
      TrackExtended * trkGrp = tracksInGroupFirst[iTrkGrp];
      TrackerHitExtendedVec hitVec = trkGrp->getTrackerHitExtendedVec();
      
      for(unsigned int i =0; i<hitVec.size(); ++i){
        hitvecFirst.push_back(hitVec[i]->getTrackerHit());        
      }
    }
  }
  if(groupSecond!=NULL){
    
    TrackExtendedVec tracksInGroupSecond = groupSecond->getTrackExtendedVec();
    nTrkGrpSecond = int(tracksInGroupSecond.size());
    
    for (int iTrkGrp=0;iTrkGrp<nTrkGrpSecond;++iTrkGrp) {
      TrackExtended * trkGrp = tracksInGroupSecond[iTrkGrp];
      TrackerHitExtendedVec hitVec = 
      trkGrp->getTrackerHitExtendedVec();
      
      for(unsigned int i=0;i<hitVec.size();++i){
        hitvecSecond.push_back(hitVec[i]->getTrackerHit());
      }
    }
  }
  
  
  int nhitsFirst = (int)hitvecFirst.size();
  int nhitsSecond = (int)hitvecSecond.size();
  int count = 0;
  for(int i =0;i<nhitsFirst;++i){
    float xi = (float)hitvecFirst[i].getPosition()[0];
    float yi = (float)hitvecFirst[i].getPosition()[1];
    float ri = sqrt(xi*xi+yi*yi);
    if(ri < _tpc_inner_r || ri > _tpc_pad_height)continue;
    for(int j =0;j<nhitsSecond;++j){
      float xj = (float)hitvecSecond[j].getPosition()[0];
      float yj = (float)hitvecSecond[j].getPosition()[1];
      float rj = sqrt(xj*xj+yj*yj);
      if(fabs(ri-rj)<_tpc_pad_height/2.0)count++;
    }
  }  
  return count;
}

/*
 
 veto merger 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
 
 NOTE: This function will try a full fit using CombineTracks if the momentum of both tracks is greater than VetoMergeMomentumCut
 
 */

bool FullLDCTrackingAlg::VetoMerge(TrackExtended* firstTrackExt, TrackExtended* secondTrackExt){
    
  debug() << "FullLDCTrackingAlg::VetoMerge called for " << firstTrackExt << " and " << secondTrackExt << endmsg;
  
  const float d0First = firstTrackExt->getD0();
  const float z0First = firstTrackExt->getZ0();
  const float omegaFirst = firstTrackExt->getOmega();
  const float tanLFirst = firstTrackExt->getTanLambda();
  const float phi0First = firstTrackExt->getPhi();
  
  const float d0Second = secondTrackExt->getD0();
  const float z0Second = secondTrackExt->getZ0();
  const float omegaSecond = secondTrackExt->getOmega();
  const float tanLSecond = secondTrackExt->getTanLambda();
  const float phi0Second = secondTrackExt->getPhi();        
  
  HelixClass firstHelix;
  firstHelix.Initialize_Canonical(phi0First,d0First,z0First,omegaFirst,tanLFirst,_bField);
  const float pxFirst = firstHelix.getMomentum()[0];
  const float pyFirst = firstHelix.getMomentum()[1];
  const float pzFirst = firstHelix.getMomentum()[2];        
  
  HelixClass secondHelix;
  secondHelix.Initialize_Canonical(phi0Second,d0Second,z0Second,omegaSecond,tanLSecond,_bField);
  const float pxSecond = secondHelix.getMomentum()[0];
  const float pySecond = secondHelix.getMomentum()[1];
  const float pzSecond = secondHelix.getMomentum()[2];      
  const float pFirst = sqrt(pxFirst*pxFirst+pyFirst*pyFirst+pzFirst*pzFirst);
  const float pSecond = sqrt(pxSecond*pxSecond+pySecond*pySecond+pzSecond*pzSecond);
  
  if(pFirst<_vetoMergeMomentumCut || pSecond<_vetoMergeMomentumCut) {
    debug() << "FullLDCTrackingAlg::VetoMerge do not veto as below momentum cut of 2.5 : pFirst = " << pFirst << " pSecond = " << pSecond << endmsg;
    return false;
  }

  bool veto = false;
  
  bool testCombinationOnly=true;
  TrackExtended * combinedTrack = CombineTracks(firstTrackExt,secondTrackExt,_maxAllowedPercentageOfOutliersForTrackCombination,testCombinationOnly);
  
  if(combinedTrack!=NULL){
    //SJA:FIXME hardcoded cut: here the check is that no more than 7 hits have been rejected in the combined fit.
    if( combinedTrack->getNDF()+15 < firstTrackExt->getNDF() + secondTrackExt->getNDF()+5 ) {
      debug() << "FullLDCTrackingAlg::VetoMerge fails NDF cut " << endmsg;
      veto=true ;
    }
  
    delete combinedTrack->getGroupTracks();
    delete combinedTrack;

  }
  else {
    debug() << "FullLDCTrackingAlg::VetoMerge fails CombineTracks(firstTrackExt,secondTrackExt,true) test" << endmsg;
    veto = true;
  }
  
  if(SegmentRadialOverlap(firstTrackExt,secondTrackExt)>10) {
    debug() << "FullLDCTrackingAlg::VetoMerge fails SegmentRadialOverlap test " << endmsg;
    veto=true;
  }

  return veto;
}

StatusCode FullLDCTrackingAlg::finalize() { 
  delete _encoder ;
  return StatusCode::SUCCESS;
}

void FullLDCTrackingAlg::setupGearGeom(){
  
  auto _gear = service<IGearSvc>("GearSvc");
  gearMgr = _gear->getGearMgr();

  _bField = gearMgr->getBField().at( gear::Vector3D( 0.,0.,0.)  ).z() ;
  
  //-- VXD Parameters--
  _nLayersVTX = 0 ;
  const gear::VXDParameters* pVXDDetMain = 0;
  const gear::VXDLayerLayout* pVXDLayerLayout = 0;
  
  try{
    debug() << " filling VXD parameters from gear::SITParameters " << endmsg;
    
    pVXDDetMain = &(gearMgr->getVXDParameters());
    pVXDLayerLayout = &(pVXDDetMain->getVXDLayerLayout());
    _nLayersVTX = pVXDLayerLayout->getNLayers();
  }
  catch( ... ){
    debug() << " ### gear::VXDParameters Not Present in GEAR FILE" << endmsg;
  }
    
  //-- SIT Parameters--
  _nLayersSIT = 0 ;
  const gear::ZPlanarParameters* pSITDetMain = 0;
  const gear::ZPlanarLayerLayout* pSITLayerLayout = 0;
  
  try{
    debug() << " filling SIT parameters from gear::SITParameters " << endmsg;
    
    pSITDetMain = &(gearMgr->getSITParameters());
    pSITLayerLayout = &(pSITDetMain->getZPlanarLayerLayout());
    _nLayersSIT = pSITLayerLayout->getNLayers();
  }
  catch( ... ){
    debug() << " ### gear::SITParameters Not Present in GEAR FILE" << endmsg;
  }
  
  if( _nLayersSIT == 0 ){
    // try the old LOI style key value pairs as defined in the SSit03 Mokka drive
    try{
      debug() << "  FullLDCTrackingAlg - Simple Cylinder Based SIT using parameters defined by SSit03 Mokka driver " << endmsg;
      
      // SIT
      const gear::GearParameters& pSIT = gearMgr->getGearParameters("SIT");
      
      const EVENT::DoubleVec& SIT_r   =  pSIT.getDoubleVals("SITLayerRadius" )  ;
      const EVENT::DoubleVec& SIT_hl  =  pSIT.getDoubleVals("SITSupportLayerHalfLength" )  ;
      
      _nLayersSIT = SIT_r.size() ; 
      
      if (_nLayersSIT != SIT_r.size() || _nLayersSIT != SIT_hl.size()) {
	fatal() << "FullLDCTrackingAlg Miss-match between DoubleVec and nlayers exit(1) called from file " << __FILE__ << " line " << __LINE__  << endmsg;
        exit(1);
      }
    }
    catch( ... ){
      debug() << " ### gear::SIT Parameters from as defined in SSit03 Not Present in GEAR FILE" << endmsg;
    } 
  }
  
  //-- SET Parameters--
  _nLayersSET = 0 ;
  const gear::ZPlanarParameters* pSETDetMain = 0;
  const gear::ZPlanarLayerLayout* pSETLayerLayout = 0;
  
  try{
    debug() << " filling SET parameters from gear::SETParameters " << endmsg;
    
    pSETDetMain = &(gearMgr->getSETParameters());
    pSETLayerLayout = &(pSETDetMain->getZPlanarLayerLayout());
    _nLayersSET = pSETLayerLayout->getNLayers();
  }
  catch( ... ){
    debug() << " ### gear::SETParameters Not Present in GEAR FILE" << endmsg;
  }
  
  if( _nLayersSET == 0 ){
    // try the old LOI style key value pairs as defined in the SSet02 Mokka drive
    try{
      debug() << "  FullLDCTrackingAlg - Simple Cylinder Based SET using parameters defined by SSet02 Mokka driver " << endmsg;
      
      // SET
      const gear::GearParameters& pSET = gearMgr->getGearParameters("SET");
      
      const EVENT::DoubleVec& SET_r   =  pSET.getDoubleVals("SETLayerRadius" )  ;
      const EVENT::DoubleVec& SET_hl  =  pSET.getDoubleVals("SETSupportLayerHalfLength" )  ;
      
      _nLayersSET = SET_r.size() ;
      
      if (_nLayersSET != SET_r.size() || _nLayersSET != SET_hl.size()) {
	fatal() << "FullLDCTrackingAlg Miss-match between DoubleVec and nlayers exit(1) called from file " << __FILE__ << " line " << __LINE__  << endmsg;
        exit(1);
      }
    }
    catch( ... ){
      debug() << " ### gear::SET Parameters from as defined in SSet02 Not Present in GEAR FILE" << endmsg;
    }
  }
    
  //-- FTD Parameters--
  _petalBasedFTDWithOverlaps = false;  
  _nLayersFTD = 0;
  
  try{
    debug() << " filling FTD parameters from gear::FTDParameters " << endmsg;
    
    const gear::FTDParameters&   pFTD      =gearMgr->getFTDParameters();
    const gear::FTDLayerLayout&  ftdlayers = pFTD.getFTDLayerLayout() ;
    
    _nLayersFTD = ftdlayers.getNLayers() ;
    
    for (unsigned int disk=0; disk < _nLayersFTD; ++disk) {
      
      _zLayerFTD.push_back( ftdlayers.getSensitiveZposition(disk, 0, 1) ); // front petal even numbered
      
      if ( ftdlayers.getNPetals(disk) > 0) {
        _zLayerFTD.push_back( ftdlayers.getSensitiveZposition(disk, 1, 1) );  // front petal odd numbered
        _petalBasedFTDWithOverlaps = true;
      }
      
    }
    
    // SJA: Here we increase the size of _nlayersFTD as we are treating the 
    _nLayersFTD =_zLayerFTD.size() ;     
    
  }
  catch( ... ){
    debug() << " ### gear::FTDParameters Not Present in GEAR FILE" << endmsg;
  } 
  
  if( _nLayersFTD == 0 ){
    // FTD
    try{
      debug() << "  FullLDCTrackingAlg - Simple Disc Based FTD using parameters defined by SFtd05 Mokka driver " << endmsg;
      
      const gear::GearParameters& pFTD = gearMgr->getGearParameters("FTD");
      
      const EVENT::DoubleVec* pFTD_z   = NULL;
      
      pFTD_z = &pFTD.getDoubleVals("FTDZCoordinate" )  ;
      
      _nLayersFTD = pFTD_z->size();
      
      for (unsigned int i = 0; i<_nLayersFTD; ++i) {
        _zLayerFTD.push_back((*pFTD_z)[i]);
      }
    }
    catch( ... ){
      debug() << " ### gear::FTD Parameters as defined in SFtd05 Not Present in GEAR FILE" << endmsg;
    } 
  }
}