SiliconTracking.cpp~

              }
            }
          }
        }
        if (minDist < _minDistCutAttach && trackToAttach != NULL) {
          int iopt = 2;
          AttachHitToTrack(trackToAttach,hit,iopt);
        }      
      }
    }
  }
}

void SiliconTracking::AttachRemainingVTXHitsSlow() {
  
  TrackerHitExtendedVec nonAttachedHits;
  nonAttachedHits.clear();
  
  for (int il=0;il<_nLayers;++il) {
    for (int ip=0;ip<_nDivisionsInPhi;++ip) {
      for (int it=0;it<_nDivisionsInTheta; ++it) {
        int iCode = il + _nLayers*ip + _nLayers*_nDivisionsInPhi*it;      
        TrackerHitExtendedVec& hitVec = _sectors[iCode];
        int nH = int(hitVec.size());
        for (int iH=0; iH<nH; ++iH) {
          TrackerHitExtended * hit = hitVec[iH];
          TrackExtendedVec& trackVec = hit->getTrackExtendedVec();
          // if (trackVec.size()==0)
          // nonAttachedHits.push_back( hit );
          //-- allow hits that are only used in triplets to be re-attached 
          unsigned int maxTrackSize = 0;
          for(unsigned int itrack = 0;itrack < trackVec.size();itrack++){
            TrackerHitExtendedVec hitVec_tmp= trackVec[itrack]->getTrackerHitExtendedVec();
            unsigned int isize = hitVec_tmp.size();
            if(isize>maxTrackSize)maxTrackSize = isize;
          }     
          if (maxTrackSize<=3) { 
            debug() << " Add non attached hit to list: id = " << hit->getTrackerHit()->id() << endmsg;
            nonAttachedHits.push_back( hit );
          } 
          
          
        }
      }
    }
  }
  
  int nNotAttached = int(nonAttachedHits.size());
  
  int nTrk = int(_trackImplVec.size()); 
  for (int iHit=0; iHit<nNotAttached; ++iHit) {
    TrackerHitExtended * hit = nonAttachedHits[iHit];
    debug() << " Try hit: id = " << hit->getTrackerHit()->id() << endmsg;
    int layer = getLayerID( hit->getTrackerHit() );
    if (layer > _minimalLayerToAttach) {
      float pos[3];
      for (int i=0; i<3; ++i) 
        pos[i] = hit->getTrackerHit()->getPosition()[i];      
      float minDist = 1e+10;
      TrackExtended * trackToAttach = NULL;
      for (int iTrk=0; iTrk<nTrk; ++iTrk) {
        TrackExtended * trackAR = _trackImplVec[iTrk];
        bool consider = true;
        if (_checkForDelta) {
          TrackerHitExtendedVec& hitVector = trackAR->getTrackerHitExtendedVec();
          int NHITS = int(hitVector.size());
          for (int IHIT=0;IHIT<NHITS;++IHIT) {
            
            // Here we are trying to find if a hits are too close i.e. closer than _minDistToDelta
	    edm4hep::TrackerHit* trkhit1 = hit->getTrackerHit();
	    edm4hep::TrackerHit* trkhit2 = hitVector[IHIT]->getTrackerHit();                  
            
            if ( trkhit1->getCellID() == trkhit2->getCellID() ){ // i.e. they are in the same sensor
              
              float distance = 0.;
              for (int iC=0;iC<3;++iC) {
                float posFirst = float(hit->getTrackerHit()->getPosition()[iC]);
                float posSecond = float(hitVector[IHIT]->getTrackerHit()->getPosition()[iC]);
                float deltaPos = posFirst - posSecond;
                distance += deltaPos*deltaPos;
              }
              distance = sqrt(distance);
              if (distance<_minDistToDelta) {
                consider = false;
                debug() << " hit: id = " << hit->getTrackerHit()->id() << " condsidered delta together with hit " << trkhit2->id() << endmsg;
                break;
              }
            }       
          }
        }
        if (consider) {
          HelixClass helix;
          float phi0 = trackAR->getPhi();
          float d0 = trackAR->getD0();
          float z0 = trackAR->getZ0();
          float omega = trackAR->getOmega();
          float tanlambda = trackAR->getTanLambda();
          helix.Initialize_Canonical(phi0,d0,z0,omega,tanlambda,_bField);
          float distance[3];
          float time = helix.getDistanceToPoint(pos,distance);
          if (time < 1.0e+10) {
            if (distance[2] < minDist) {
              minDist = distance[2];
              trackToAttach = trackAR;
            }
          }
        }
      }
      if (minDist < _minDistCutAttach && trackToAttach != NULL) {
        int iopt = 2;
        debug() << " Hit: id = " << hit->getTrackerHit()->id() << " : try attachement"<< endmsg;
        AttachHitToTrack(trackToAttach,hit,iopt);
      } else {
        debug() << " Hit: id = " << hit->getTrackerHit()->id() << " rejected due to distance cut of " <<_minDistCutAttach<< " min distance = "  << minDist << endmsg;
      }      
    }
  }  
}

void SiliconTracking::AttachRemainingFTDHitsSlow() {
  TrackerHitExtendedVec nonAttachedHits;
  nonAttachedHits.clear();
  
  for (int iS=0;iS<2;++iS) {
    for (unsigned int layer=0;layer<_nlayersFTD;++layer) {
      for (int ip=0;ip<_nPhiFTD;++ip) {
        int iCode = iS + 2*layer + 2*_nlayersFTD*ip;      
        TrackerHitExtendedVec& hitVec = _sectorsFTD[iCode];
        int nH = int(hitVec.size());
        for (int iH=0; iH<nH; ++iH) {
          TrackerHitExtended * hit = hitVec[iH];
          TrackExtendedVec& trackVec = hit->getTrackExtendedVec();
          if (trackVec.size()==0)
            nonAttachedHits.push_back( hit );
        }
      }
    }
  }
  
  int nNotAttached = int(nonAttachedHits.size());
  
  int nTrk = int(_trackImplVec.size()); 
  for (int iHit=0; iHit<nNotAttached; ++iHit) {
    TrackerHitExtended * hit = nonAttachedHits[iHit];
    float pos[3];
    for (int i=0; i<3; ++i) 
      pos[i] = hit->getTrackerHit()->getPosition()[i];      
    float minDist = 1e+10;
    TrackExtended * trackToAttach = NULL;
    for (int iTrk=0; iTrk<nTrk; ++iTrk) {
      TrackExtended * trackAR = _trackImplVec[iTrk];
      bool consider = true;
      TrackerHitExtendedVec& hitVector = trackAR->getTrackerHitExtendedVec();
      int NHITS = int(hitVector.size());
      
      for (int IHIT=0;IHIT<NHITS;++IHIT) {
        
        // SJA:FIXME: check to see if allowing no hits in the same sensor vs no hits in the same layer works 
        //        if (hit->getTrackerHit()->getType() == hitVector[IHIT]->getTrackerHit()->getType()) {
        if (hit->getTrackerHit()->getCellID() == hitVector[IHIT]->getTrackerHit()->getCellID()) {
          
          consider = false;
          break;
        }
      }
      
      
      if (consider) {
        HelixClass helix;
        float phi0 = trackAR->getPhi();
        float d0 = trackAR->getD0();
        float z0 = trackAR->getZ0();
        float omega = trackAR->getOmega();
        float tanlambda = trackAR->getTanLambda();
        if (tanlambda*float(getSideID(hit->getTrackerHit())) > 0) {
          helix.Initialize_Canonical(phi0,d0,z0,omega,tanlambda,_bField);
          float distance[3];
          float time = helix.getDistanceToPoint(pos,distance);
          if (time < 1.0e+10) {
            if (distance[2] < minDist) {
              minDist = distance[2];
              trackToAttach = trackAR;
            }
          }
        }
      }
    }
    if (minDist < _minDistCutAttach && trackToAttach != NULL) {
      int iopt = 2;
      AttachHitToTrack(trackToAttach,hit,iopt);
    }      
  }  
}


void SiliconTracking::AttachRemainingFTDHitsFast() {
  int nTrk = _trackImplVec.size();
  
  for (int iTrk=0; iTrk<nTrk; ++iTrk) {
    TrackExtended * trackAR = _trackImplVec[iTrk];
    HelixClass helix;
    float phi0 = trackAR->getPhi();
    float d0 = trackAR->getD0();
    float z0 = trackAR->getZ0();
    float omega = trackAR->getOmega();
    float tanlambda = trackAR->getTanLambda();
    helix.Initialize_Canonical(phi0,d0,z0,omega,tanlambda,_bField);
    int iSemiSphere = 0;
    if (tanlambda > 0) 
      iSemiSphere = 1;
    float ref[3];
    for (int i=0;i<3;++i) 
      ref[i] = helix.getReferencePoint()[i];
    // Start loop over FTD layers
    for (unsigned int layer=0;layer<_nlayersFTD;layer++) {
      float ZL = _zLayerFTD[layer];
      if (iSemiSphere == 0)
        ZL = - ZL;
      float point[3];
      helix.getPointInZ(ZL,ref,point);
      float Phi = atan2(point[1],point[0]);
      if (Phi < 0) 
        Phi = Phi + TWOPI;
      int iPhi = int(Phi/_dPhiFTD);
      float distMin = 1e+10;
      TrackerHitExtended * attachedHit = NULL;     
      for (int iP=iPhi-1;iP<=iPhi+1;++iP) {
        int iPP = iP;
        if (iP < 0) 
          iPP = iP + _nPhiFTD;
        if (iP >= _nPhiFTD)
          iPP = iP - _nPhiFTD;  
        int iCode = iSemiSphere + 2*layer + 2*_nlayersFTD*iPP;
        int nHits = int(_sectorsFTD[iCode].size());
        for (int iHit=0;iHit<nHits;++iHit) {
          TrackerHitExtended * hit = _sectorsFTD[iCode][iHit];
          bool consider = true;
          TrackerHitExtendedVec& hitVector = trackAR->getTrackerHitExtendedVec();
          int NHITS = int(hitVector.size());
          
          // SJA:FIXME: check to see if allowing no hits in the same sensor vs no hits in the same layer works 
          for (int IHIT=0;IHIT<NHITS;++IHIT) {
            //            if (hit->getTrackerHit()->getType() == hitVector[IHIT]->getTrackerHit()->getType()) {
            if (hit->getTrackerHit()->getCellID() == hitVector[IHIT]->getTrackerHit()->getCellID()) {
              consider = false;
              break;
            }
          }
          
          
          if (consider) {
            float pos[3];
            for (int i=0;i<3;++i) {
              pos[i] = hit->getTrackerHit()->getPosition()[i];
            }
            float distance[3];
            float time = helix.getDistanceToPoint(pos,distance);
            if (time < 1.0e+10) {
              if (distance[2] < distMin) {
                distMin = distance[2];
                attachedHit = hit;
              }
            }
          }
        }
      }
      if (distMin < _minDistCutAttach && attachedHit != NULL) {
        int iopt = 2;
        AttachHitToTrack(trackAR,attachedHit, iopt);
      }
    }
  }
}

void SiliconTracking::TrackingInFTD() {

  int nComb = int(_CombinationsFTD.size()) / 3;

  for (int iComb=0;iComb<nComb;++iComb) {

    int nLS[3];
    nLS[0] = _CombinationsFTD[3*iComb];
    nLS[1] = _CombinationsFTD[3*iComb+1];
    nLS[2] = _CombinationsFTD[3*iComb+2];

    for (int iS=0;iS<2;++iS) { // loop over +z and -z
      
      //      std::cout << "Combinations : " << iS << " " << nLS[0] << " " << nLS[1] << " " << nLS[2] << endmsg;
      //      int iC = iS + 2*nLS[0];
      //      TrackerHitExtendedVec& hitVec = _sectorsFTD[iC];
      //      int nO = int(hitVec.size());
      //      iC = iS + 2*nLS[1];
      //      hitVec = _sectorsFTD[iC];
      //      int nM = int(hitVec.size());
      //      iC = iS + 2*nLS[2];
      //      hitVec = _sectorsFTD[iC];
      //      int nI = int(hitVec.size());
      //      std::cout << nO << " " << nM << " " << nI << endmsg;

      for (int ipOuter=0;ipOuter<_nPhiFTD;++ipOuter) { 

        int ipMiddleLow = ipOuter - 1;
        int ipMiddleUp  = ipOuter + 1;
        
        unsigned int iCodeOuter = iS + 2*nLS[0] + 2*_nlayersFTD*ipOuter;
        
        if( iCodeOuter >= _sectorsFTD.size()){          
          error() << "iCodeOuter index out of range: iCodeOuter =   " << iCodeOuter << " _sectorsFTD.size() = " << _sectorsFTD.size() << " exit(1) called from file " << __FILE__ << " line " << __LINE__<< endmsg;
          exit(1);
        }
        
        TrackerHitExtendedVec& hitVecOuter = _sectorsFTD[iCodeOuter];
        
        int nOuter = int(hitVecOuter.size());

        for (int iOuter=0;iOuter<nOuter;++iOuter) {
          
          TrackerHitExtended * hitOuter = hitVecOuter[iOuter];
        
          for (int ipMiddle=ipMiddleLow;ipMiddle<=ipMiddleUp;++ipMiddle) {
            //for(int ipMiddle=0;ipMiddle<_nPhiFTD;++ipMiddle) {
            int ipM = ipMiddle;
            if (ipM < 0) 
              ipM = ipMiddle + _nPhiFTD;
            if (ipM >= _nPhiFTD)
              ipM = ipMiddle - _nPhiFTD;
            int iCodeMiddle = iS + 2*nLS[1] + 2*_nlayersFTD*ipM;
          
            TrackerHitExtendedVec& hitVecMiddle = _sectorsFTD[iCodeMiddle];
            int ipInnerLow,ipInnerUp;       
            ipInnerLow = ipMiddle - 1;
            ipInnerUp =  ipMiddle + 1;
            
            int nMiddle = int(hitVecMiddle.size());
            
            for (int iMiddle=0;iMiddle<nMiddle;++iMiddle) {
              TrackerHitExtended * hitMiddle = hitVecMiddle[iMiddle];
              for (int ipInner=ipInnerLow;ipInner<=ipInnerUp;++ipInner) {
                //for (int ipInner=0;ipInner<_nPhiFTD;++ipInner) {
                int ipI = ipInner;
                if (ipI < 0)
                  ipI = ipInner + _nPhiFTD;
                if (ipI >= _nPhiFTD) 
                  ipI = ipInner - _nPhiFTD;
                int iCodeInner = iS + 2*nLS[2] + 2*_nlayersFTD*ipI;
                TrackerHitExtendedVec& hitVecInner = _sectorsFTD[iCodeInner];
            
                int nInner = int(hitVecInner.size());
                
                for (int iInner=0;iInner<nInner;++iInner) {
                
                  TrackerHitExtended * hitInner = hitVecInner[iInner];
                  HelixClass helix;
                  //                  std::cout << endmsg;
                  //                  std::cout << "Outer Hit Type " << hitOuter->getTrackerHit()->getType() << " z = " << hitOuter->getTrackerHit()->getPosition()[2] 
                  //                  << "\nMiddle Hit Type "<< hitMiddle->getTrackerHit()->getType() << " z = " << hitMiddle->getTrackerHit()->getPosition()[2]  
                  //                  << "\nInner Hit Type "<< hitInner->getTrackerHit()->getType() << " z = " << hitInner->getTrackerHit()->getPosition()[2]  << endmsg;

                  debug() << " "
                  << std::setw(3) << ipOuter       << " "   << std::setw(3) << ipMiddle << " "      << std::setw(3) << ipInner << "       "
                  << std::setw(3) << iS << "     "
                  << std::setw(3) << nLS[0]     << " "   << std::setw(3) << nLS[1]   << " "      << std::setw(3) << nLS[2]  << "     "
                  << std::setw(3) << nOuter << " : " << std::setw(3) << nMiddle << " : " << std::setw(3) << nInner << "  :: "
                  << std::setw(3) << nOuter*nMiddle* nInner << endmsg;

                  
                  TrackExtended * trackAR = TestTriplet(hitOuter,hitMiddle,hitInner,helix);
                  if (trackAR != NULL) {
                    //                    std::cout << "FTD triplet found" << endmsg;
                    int nHits = BuildTrackFTD(trackAR,nLS,iS);
                    
                    _tracksWithNHitsContainer.getTracksWithNHitsVec( nHits ).push_back( trackAR );
                  }
                }
              }
            }
          }       
        }
      }
    }
  }
}


int SiliconTracking::BuildTrackFTD(TrackExtended * trackAR, int * nLR, int iS) {
  //  std::cout << "BuildTrackFTD: Layers = " << nLR[0] << " " << nLR[1] << " " << nLR[2] << endmsg;
  
  // initialise a helix from the track
  HelixClass helix;
  const float d0 = trackAR->getD0();
  const float z0 = trackAR->getZ0();
  const float phi0 = trackAR->getPhi();
  const float tanlambda = trackAR->getTanLambda();
  const float omega = trackAR->getOmega();
  helix.Initialize_Canonical(phi0,d0,z0,omega,tanlambda,_bField);
  float ref[3] = {helix.getReferencePoint()[0],
    helix.getReferencePoint()[1],
    helix.getReferencePoint()[2]};
  
  for (int iL=0; iL < static_cast<int>(_nlayersFTD); ++iL) {
    if (iL != nLR[0] && iL != nLR[1] && iL != nLR[2]) {
      float point[3];
      float ZL = _zLayerFTD[iL];
      if (iS == 0) 
        ZL = - ZL;
      helix.getPointInZ(ZL,ref,point);
      //      float Phi = atan2(point[1],point[0]);
      //      int iPhi = int(Phi/_dPhiFTD);
      float distMin = 1e+6;
      TrackerHitExtended * attachedHit = NULL;
      for (int ip=0;ip<=_nPhiFTD;++ip) {
        int iP = ip;
        if (iP < 0)
          iP = ip + _nPhiFTD;
        if (iP >= _nPhiFTD)
          iP = ip - _nPhiFTD;   
        int iCode = iS + 2*iL + 2*_nlayersFTD*iP;
        TrackerHitExtendedVec& hitVec = _sectorsFTD[iCode];
        int nH = int(hitVec.size());
        for (int iH=0; iH<nH; ++iH) {
          TrackerHitExtended * hit = hitVec[iH];
	  edm4hep::TrackerHit * trkHit = hit->getTrackerHit();
          float pos[3];
          for (int i=0;i<3;++i)
            pos[i] = float(trkHit->getPosition()[i]);
          float distance[3];
          float time = helix.getDistanceToPoint(pos,distance);
          if (time < 1.0e+10) {
            if (distance[2] < distMin) {
              distMin = distance[2];
              attachedHit = hit;
            }
          }
        }
      }
      //      std::cout << "Layer = " << iL << "  distMin = " << distMin << endmsg;
      if (distMin < _minDistCutAttach && attachedHit != NULL) {
        int iopt = 2;
        AttachHitToTrack( trackAR, attachedHit, iopt);
      }
    }
  }
  TrackerHitExtendedVec& hitVec = trackAR->getTrackerHitExtendedVec();
  int nH = int (hitVec.size());
  return nH;
}

int SiliconTracking::AttachHitToTrack(TrackExtended * trackAR, TrackerHitExtended * hit, int iopt) {
  
  int attached = 0;
  TrackerHitExtendedVec& hitVec = trackAR->getTrackerHitExtendedVec();
  int nHits = int(hitVec.size());
  
  double * xh = new double[nHits+1];
  double * yh = new double[nHits+1];
  float  * zh = new float[nHits+1];
  double * wrh = new double[nHits+1];
  float * wzh = new float[nHits+1];
  float * rh = new float[nHits+1];
  float * ph = new float[nHits+1];
  float par[5];
  float epar[15];
  
  for (int i=0; i<nHits; ++i) {
    edm4hep::TrackerHit * trkHit = hitVec[i]->getTrackerHit();
    xh[i] = double(trkHit->getPosition()[0]);
    yh[i] = double(trkHit->getPosition()[1]);
    zh[i] = float(trkHit->getPosition()[2]);
    ph[i] = float(atan2(yh[i],xh[i]));
    rh[i] = float(sqrt(xh[i]*xh[i]+yh[i]*yh[i]));
    float rR = hitVec[i]->getResolutionRPhi();
    float rZ = hitVec[i]->getResolutionZ();
    wrh[i] = double(1.0/(rR*rR));
    wzh[i] = 1.0/(rZ*rZ);
  }
  
  edm4hep::TrackerHit * trkHit = hit->getTrackerHit();
  xh[nHits] = double(trkHit->getPosition()[0]);
  yh[nHits] = double(trkHit->getPosition()[1]);
  zh[nHits] = float(trkHit->getPosition()[2]);
  ph[nHits] = float(atan2(yh[nHits],xh[nHits]));
  rh[nHits] = float(sqrt(xh[nHits]*xh[nHits]+yh[nHits]*yh[nHits]));
  
  float rR = hit->getResolutionRPhi();
  float rZ = hit->getResolutionZ();
  wrh[nHits] = double(1.0/(rR*rR));
  wzh[nHits] = 1.0/(rZ*rZ);
  
  
  int NPT = nHits + 1;
  
  // SJA:FIXME the newtonian part is giving crazy results for FTD so just use iopt 2 for simply attaching hits 
  // using SIT and VXD doesn't seem to give any problems, so make it a function parameter and let the caller decide
  //  int iopt = 3;
  
  float chi2RPhi = 0 ;
  float chi2Z = 0 ;
  
  
  int error = _fastfitter->fastHelixFit(NPT, xh, yh, rh, ph, wrh, zh, wzh,iopt, par, epar, chi2RPhi, chi2Z);
  par[3] = par[3]*par[0]/fabs(par[0]);
  
  
  float omega = par[0];
  float tanlambda = par[1];
  float phi0 = par[2];
  float d0 = par[3];
  float z0 = par[4];
  float chi2 = FLT_MAX;
  int ndf = INT_MAX;
  
  if (NPT == 3) {
    chi2 = chi2RPhi*_chi2WRPhiTriplet+chi2Z*_chi2WZTriplet;
  }
  if (NPT == 4) {
    chi2 = chi2RPhi*_chi2WRPhiQuartet+chi2Z*_chi2WZQuartet;
  }
  if (NPT > 4) {
    chi2 = chi2RPhi*_chi2WRPhiSeptet+chi2Z*_chi2WZSeptet;
  }
  ndf = 2*NPT-5;
  
  
  if ( error == 0 && chi2/float(ndf) < _chi2FitCut ) {
    trackAR->addTrackerHitExtended(hit);
    hit->addTrackExtended( trackAR );
    trackAR->setChi2( chi2 );
    trackAR->setOmega( omega );
    trackAR->setTanLambda( tanlambda );
    trackAR->setD0( d0 );
    trackAR->setZ0( z0 );
    trackAR->setPhi( phi0 );
    trackAR->setNDF( ndf );
    trackAR->setCovMatrix( epar );
    attached = 1;
    debug() << "Attachement succeeded chi2/float(ndf) = " << chi2/float(ndf) << "  cut = " <<  _chi2FitCut  << " chi2RPhi = " << chi2RPhi << " chi2Z = " << chi2Z << " error = " << error << endmsg;
  } else {
    debug() << "Attachement failed chi2/float(ndf) = " << chi2/float(ndf) << "  cut = " <<  _chi2FitCut  << " chi2RPhi = " << chi2RPhi << " chi2Z = " << chi2Z << " error = " << error << endmsg;
  }
  
  delete[] xh;
  delete[] yh;
  delete[] zh;
  delete[] wrh;
  delete[] wzh;
  delete[] rh;
  delete[] ph;
  
  return attached;
  
  
}

void SiliconTracking::FinalRefit(edm4hep::TrackCollection* trk_col) {
  
  int nTracks = int(_trackImplVec.size());
  
  int nSiSegments = 0;        
  float eTot = 0.;
  float pxTot = 0.;
  float pyTot = 0.;
  float pzTot = 0.;
  std::cout << "fucd============" << nTracks << std::endl;
  for (int iTrk=0;iTrk<nTracks;++iTrk) {
    
    TrackExtended * trackAR = _trackImplVec[iTrk];    
    TrackerHitExtendedVec& hitVec = trackAR->getTrackerHitExtendedVec();
    
    int nHits = int(hitVec.size());
    std::cout << "fucd-------------" << iTrk << ": " << nHits << std::endl;
    if( nHits >= _minimalHits) {
      //    int * lh = new int[nHits];
      std::vector<int> lh;
      lh.resize(nHits);
      
      for (int i=0; i<nHits; ++i) {
        lh[i]=0;
      }
      
      float d0 = trackAR->getD0();
      float z0 = trackAR->getZ0();
      float omega = trackAR->getOmega();
      float tanlambda = trackAR->getTanLambda();
      float phi0 = trackAR->getPhi();
      
      HelixClass * helix = new HelixClass();
      helix->Initialize_Canonical(phi0, d0, z0, omega, 
                                  tanlambda, _bField);
      
      
      // get the point of closest approach to the reference point
      // here it is implicitly assumed that the reference point is the origin 
      float Pos[3];
      Pos[0] = -d0*sin(phi0);
      Pos[1] = d0*cos(phi0);
      Pos[2] = z0;
      
      std::cout << "fucd------------------1" << std::endl; 
      // at this point is is possible to have hits from the same layer ...
      // so a check is made to ensure that the hit with the smallest distance to the 
      // current helix hypothosis is used, the other hit has lh set to 0 
      
      // start loop over the hits to
      for (int ihit=0;ihit<nHits;++ihit) {
        
        lh[ihit] = 1; // only hits which have lh=1 will be used for the fit
        
        // get the pointer to the lcio trackerhit for this hit
	edm4hep::TrackerHit * trkHit = hitVec[ihit]->getTrackerHit();
        
        int det = getDetectorID(trkHit);
        
        if (det == lcio::ILDDetID::VXD || det == lcio::ILDDetID::FTD || det == lcio::ILDDetID::SIT) { // only accept VXD, FTD or SIT
          
          
          //        int layer = getLayerID(trkHit);
          //        int moduleIndex = getModuleID(trkHit);
          
          // start a double loop over the hits which have already been checked 
          for (int lhit=0;lhit<ihit;++lhit) {
            
            // get the pointer to the lcio trackerhit for the previously checked hit
	    edm4hep::TrackerHit * trkHitS = hitVec[lhit]->getTrackerHit();
            
            
            //          int layerS = getLayerID(trkHitS);
            //          int moduleIndexS = getModuleID(trkHitS);
            
            // SJA:FIXME: check to see if allowing no hits in the same sensor vs no hits in the same layer works 
            // if they are on the same layer and the previously checked hits has been declared good for fitting
            //          if ((trkHitS->getType() == trkHit->getType()) && (lh[lhit] == 1)) {
            // check if the hits have the same layer and petal number
            //          hitVec[ihit]->
            //          if ((layer == layerS) && (moduleIndex==moduleIndexS) && (lh[lhit] == 1)) {
            if ( (trkHit->getCellID() == trkHitS->getCellID()) && (lh[lhit] == 1)) {
              
              // get the position of the hits 
              float xP[3];
              float xPS[3];
              for (int iC=0;iC<3;++iC) {
                xP[iC] = float(trkHit->getPosition()[iC]);
                xPS[iC] = float(trkHitS->getPosition()[iC]);
              }
              
              // get the intersection of the helix with the either the cylinder or plane containing the hit
              float Point[6];
              float PointS[6];
              
              if (det == lcio::ILDDetID::FTD) {

                float time = helix->getPointInZ(xP[2],Pos,Point);
                time = helix->getPointInZ(xPS[2],Pos,PointS);

              } else {

                float RAD = sqrt(xP[0]*xP[0]+xP[1]*xP[1]);
                float RADS = sqrt(xPS[0]*xPS[0]+xPS[1]*xPS[1]);
                float time = helix->getPointOnCircle(RAD,Pos,Point);
                time = helix->getPointOnCircle(RADS,Pos,PointS);

              }
              
              float DIST = 0;
              float DISTS = 0;
              
              // get the euclidean distance between the hit and the point of intersection
              for (int iC=0;iC<3;++iC) {
                DIST += (Point[iC]-xP[iC])*(Point[iC]-xP[iC]);
                DISTS += (PointS[iC]-xPS[iC])*(PointS[iC]-xPS[iC]);
              }
              if (DIST < DISTS) {
                lh[lhit] = 0;
              }
              else {
                lh[ihit] = 0;
              }
              break;
            }
          }
        }
      }
      
      delete helix;
      
      std::vector<TrackerHit*> trkHits;
      std::vector<TrackerHit*> trkHits_used_inFit;
      
      int nFit = 0;
      for (int i=0; i<nHits; ++i) {
        // check if the hit has been rejected as being on the same layer and further from the helix lh==0
        if (lh[i] == 1) {
	  edm4hep::TrackerHit * trkHit = hitVec[i]->getTrackerHit();
          nFit++;
          if(trkHit) { 
            trkHits.push_back(trkHit);   
          }
          else{
            throw EVENT::Exception( std::string("SiliconTracking::FinalRefit: TrackerHit pointer == NULL ")  ) ;
          }
        }
        else { // reject hit 
               // SJA:FIXME missuse of type find a better way to signal rejected hits
          hitVec[i]->setType(int(0));
        }
      }
      
      if( trkHits.size() < 3 ) {
        debug() << "SiliconTracking::FinalRefit: Cannot fit less than 3 hits. Number of hits =  " << trkHits.size() << endmsg;
        continue ; 
      }
      std::cout << "fucd------------------2" << std::endl;
      //TrackImpl* Track = new TrackImpl ;
      auto track = trk_col->create();
      //fucd
      //edm4hep::Track track;// = new edm4hep::Track;
      std::cout << "fucd------------------3" << std::endl;
      // setup initial dummy covariance matrix
      //std::vector<float> covMatrix;
      //covMatrix.resize(15);
      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
      
      
      std::vector< std::pair<float, edm4hep::TrackerHit*> > r2_values;
      r2_values.reserve(trkHits.size());
      
      for (std::vector<edm4hep::TrackerHit*>::iterator it=trkHits.begin(); it!=trkHits.end(); ++it) {
        edm4hep::TrackerHit* 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());
      //std::cout << "fucd------------------3" << std::endl;
      trkHits.clear();
      trkHits.reserve(r2_values.size());

      for (std::vector< std::pair<float, edm4hep::TrackerHit*> >::iterator it=r2_values.begin(); it!=r2_values.end(); ++it) {
        trkHits.push_back(it->second);
      }
      //std::cout << "fucd------------------3 " << _trksystem << std::endl;
      //for (unsigned ihit_indx=0 ; ihit_indx < trkHits.size(); ++ihit_indx) {
      //  std::cout << "fucd trk hit " << *trkHits[ihit_indx] << " " << trkHits[ihit_indx]->getCovMatrix()[0]
      //		  << " " << BitSet32(trkHits[ihit_indx]->getType())[ ILDTrkHitTypeBit::COMPOSITE_SPACEPOINT ] << endmsg;
      //}
      /*
      auto _trackSystemSvc = service<ITrackSystemSvc>("TrackSystemSvc");
      if ( !_trackSystemSvc ) {
	error() << "Failed to find TrackSystemSvc ..." << endmsg;
	return;
      }
      _trksystem =  _trackSystemSvc->getTrackSystem();

      if( _trksystem == 0 ){
	error() << "Cannot initialize MarlinTrkSystem of Type: KalTest" <<endmsg;
	return;
      }
      debug() << "_trksystem pointer " << _trksystem << endmsg;
      
      _trksystem->setOption( IMarlinTrkSystem::CFG::useQMS,        _MSOn ) ;
      _trksystem->setOption( IMarlinTrkSystem::CFG::usedEdx,       _ElossOn) ;
      _trksystem->setOption( IMarlinTrkSystem::CFG::useSmoothing,  _SmoothOn) ;
      _trksystem->init() ;
      */
      bool fit_backwards = IMarlinTrack::backward;
      
      MarlinTrk::IMarlinTrack* marlinTrk = nullptr;
      try{
	marlinTrk = _trksystem->createTrack();
      }
      catch(...){
	error() << "Cannot create MarlinTrack ! " << endmsg;
	return;
      }
      
      int status = 0;
      std::cout << "fucd------------------3" << std::endl;
      try {
        status = MarlinTrk::createFinalisedLCIOTrack(marlinTrk, trkHits, &track, fit_backwards, covMatrix, _bField, _maxChi2PerHit);
      } catch (...) {
        
        //      delete Track;
        //      delete marlinTrk;
        error() << "MarlinTrk::createFinalisedLCIOTrack " << endmsg;
        throw ;
        
      }
      std::cout << "fucd------------------4" << std::endl;
      /*
#ifdef MARLINTRK_DIAGNOSTICS_ON
      if ( status != IMarlinTrack::success && _runMarlinTrkDiagnostics ) {        
        void * dcv = _trksystem->getDiagnositicsPointer();
        DiagnosticsController* dc = static_cast<DiagnosticsController*>(dcv);
        dc->skip_current_track();
      }        
#endif
      */
      
      std::vector<std::pair<edm4hep::TrackerHit* , double> > hits_in_fit ;  
      std::vector<std::pair<edm4hep::TrackerHit* , double> > outliers ;
      std::vector<edm4hep::TrackerHit*> 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;

      int nhits_in_vxd = track.getSubDetectorHitNumbers(0);
      int nhits_in_ftd = track.getSubDetectorHitNumbers(1);
      int nhits_in_sit = track.getSubDetectorHitNumbers(2);
      
      //debug() << " Hit numbers for Track "<< track->id() << ": "
      debug() << " Hit numbers for Track "<< iTrk <<": "
	      << " vxd hits = " << nhits_in_vxd
	      << " ftd hits = " << nhits_in_ftd
	      << " sit hits = " << nhits_in_sit
	      << endmsg;
      
      //if (nhits_in_vxd > 0) Track->setTypeBit( lcio::ILDDetID::VXD ) ;
      //if (nhits_in_ftd > 0) Track->setTypeBit( lcio::ILDDetID::FTD ) ;
      //if (nhits_in_sit > 0) Track->setTypeBit( lcio::ILDDetID::SIT ) ;

      
      
      if( status != IMarlinTrack::success ) {       
        
        //delete track;
        debug() << "SiliconTracking::FinalRefit: Track fit failed with error code " << status << " track dropped. Number of hits = "<< trkHits.size() << endmsg;       
        continue ;
      }
      
      if( track.getNdf() < 0) {       
        //delete track;
        debug() << "SiliconTracking::FinalRefit: Track fit returns " << track.getNdf() << " degress of freedom track dropped. Number of hits = "<< trkHits.size() << endmsg;       
	//delete track;
        continue ;
      }
      
      //trk_col->addElement(Track);     
      //fucd
      //trk_col->push_back(track);
      for(int i=0;i<track.trackStates_size();i++){
	// 1 = lcio::EVENT::TrackState::AtIP
	edm4hep::TrackState trkStateIP = track.getTrackStates(i);
	if(trkStateIP.location !=1) continue;
      /*
      if (trkStateIP == 0) {
        debug() << "SiliconTracking::FinalRefit: Track fit returns " << track->getNdf() << " degress of freedom track dropped. Number of hits = "<< trkHits.size() << endmsg;       
        throw EVENT::Exception( std::string("SiliconTracking::FinalRefit: trkStateIP pointer == NULL ")  ) ;
      }
      */
      // note trackAR which is of type TrackExtended, only takes fits set for ref point = 0,0,0
	trackAR->setOmega(trkStateIP.omega);
	trackAR->setTanLambda(trkStateIP.tanLambda);
	trackAR->setPhi(trkStateIP.phi);
	trackAR->setD0(trkStateIP.D0);
	trackAR->setZ0(trkStateIP.Z0);
	
	float cov[15];
	
	for (int i = 0 ; i<15 ; ++i) {
	  cov[i] = trkStateIP.covMatrix.operator[](i);
	}
      
	trackAR->setCovMatrix(cov);
	trackAR->setChi2(track.getChi2());
	trackAR->setNDF(track.getNdf());
      
	nSiSegments++;
      
	HelixClass helix_final;
      
	helix_final.Initialize_Canonical(trkStateIP.phi,trkStateIP.D0,trkStateIP.Z0,trkStateIP.omega,trkStateIP.tanLambda,_bField);
      
	float trkPx = helix_final.getMomentum()[0];
	float trkPy = helix_final.getMomentum()[1];
	float trkPz = helix_final.getMomentum()[2];
	float trkP = sqrt(trkPx*trkPx+trkPy*trkPy+trkPz*trkPz);
	eTot += trkP;
	pxTot += trkPx;
	pyTot += trkPy;
	pzTot += trkPz;
      }
    }
  }
  
  debug() << "SiliconTracking -> run " << _nRun
	  << " event " << _nEvt << endmsg;
  debug() << "Number of Si tracks = " << nSiSegments << endmsg;
  debug() << "Total 4-momentum of Si Tracks : E = " << eTot
	  << " Px = " << pxTot
	  << " Py = " << pyTot
	  << " Pz = " << pzTot << endmsg;
  
  
}


StatusCode SiliconTracking::setupGearGeom(){
  auto _gear = service<IGearSvc>("GearSvc");
  if ( !_gear ) {
    error() << "Failed to find GearSvc ..." << endmsg;
    return StatusCode::FAILURE;
  }
  gear::GearMgr* gearMgr = _gear->getGearMgr();
  _bField = gearMgr->getBField().at( gear::Vector3D( 0.,0.,0.)  ).z() ;
  debug() << "Field " << _bField << endmsg;
  //-- 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( gear::UnknownParameterException& e){
    
    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( gear::UnknownParameterException& e){
    
    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{
      
      info() << "  SiliconTracking - Simple Cylinder Based SIT using parameters defined by SSit03 Mokka driver " << endmsg ;
      
      // SIT
      
      const gear::GearParameters& pSIT = gearMgr->getGearParameters("SIT");
      
      const std::vector<double>& SIT_r   =  pSIT.getDoubleVals("SITLayerRadius" )  ;
      const std::vector<double>& SIT_hl  =  pSIT.getDoubleVals("SITSupportLayerHalfLength" )  ;
      
      _nLayersSIT = SIT_r.size() ; 
      
      if (_nLayersSIT != SIT_r.size() || _nLayersSIT != SIT_hl.size()) {
        
        error() << "ILDSITCylinderKalDetector miss-match between DoubleVec and nlayers exit(1) called from file " << __FILE__ << " line " << __LINE__  << endmsg ;
        exit(1);
        
      }
    }
    catch( gear::UnknownParameterException& e){
      
      debug() << " ### gear::SIT Parameters from as defined in SSit03 Not Present in GEAR FILE" << endmsg ;
      
    }