diff --git a/Reconstruction/CMakeLists.txt b/Reconstruction/CMakeLists.txt
index 6dd63b02cda864eb8f587f8a9f8f6bc57f1b5030..6bb53e061183577afbe0a7bd5572c109c985793d 100644
--- a/Reconstruction/CMakeLists.txt
+++ b/Reconstruction/CMakeLists.txt
@@ -4,3 +4,4 @@ add_subdirectory(PFA)
 add_subdirectory(SiliconTracking)
 add_subdirectory(Tracking)
 add_subdirectory(RecGenfitAlg)
+add_subdirectory(RecAssociationMaker)
diff --git a/Reconstruction/RecAssociationMaker/CMakeLists.txt b/Reconstruction/RecAssociationMaker/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..d854356b8be3c4ed9f5c0913c2e4836bdab14cb7
--- /dev/null
+++ b/Reconstruction/RecAssociationMaker/CMakeLists.txt
@@ -0,0 +1,20 @@
+# Modules
+gaudi_add_module(RecAssociationMaker
+                 SOURCES src/TrackParticleRelationAlg.cpp
+
+                 LINK GearSvc
+                      EventSeeder
+                      TrackSystemSvcLib
+                      DataHelperLib
+                      KiTrackLib
+                      Gaudi::GaudiKernel
+                      k4FWCore::k4FWCore
+                      ${GEAR_LIBRARIES}
+                      ${GSL_LIBRARIES}
+                      ${LCIO_LIBRARIES}
+)
+install(TARGETS RecAssociationMaker
+  EXPORT CEPCSWTargets
+  RUNTIME DESTINATION "${CMAKE_INSTALL_BINDIR}" COMPONENT bin
+  LIBRARY DESTINATION "${CMAKE_INSTALL_LIBDIR}" COMPONENT shlib
+  COMPONENT dev)
diff --git a/Reconstruction/RecAssociationMaker/src/TrackParticleRelationAlg.cpp b/Reconstruction/RecAssociationMaker/src/TrackParticleRelationAlg.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..1c764257d182405df4f2af436777bb92c4a4fd02
--- /dev/null
+++ b/Reconstruction/RecAssociationMaker/src/TrackParticleRelationAlg.cpp
@@ -0,0 +1,150 @@
+#include "TrackParticleRelationAlg.h"
+
+DECLARE_COMPONENT(TrackParticleRelationAlg)
+
+TrackParticleRelationAlg::TrackParticleRelationAlg(const std::string& name, ISvcLocator* svcLoc)
+: GaudiAlgorithm(name, svcLoc){
+  declareProperty("MCParticleCollection", m_inMCParticleColHdl, "Handle of the Input MCParticle collection");
+}
+
+StatusCode TrackParticleRelationAlg::initialize() {
+  for(auto name : m_inTrackCollectionNames) {
+    m_inTrackColHdls.push_back(new DataHandle<edm4hep::TrackCollection> (name, Gaudi::DataHandle::Reader, this));
+    m_outColHdls.push_back(new DataHandle<edm4hep::MCRecoTrackParticleAssociationCollection> (name+"ParticleAssociation", Gaudi::DataHandle::Writer, this));
+  }
+  
+  for(unsigned i=0; i<m_inAssociationCollectionNames.size(); i++) {
+    m_inAssociationColHdls.push_back(new DataHandle<edm4hep::MCRecoTrackerAssociationCollection> (m_inAssociationCollectionNames[i], Gaudi::DataHandle::Reader, this));
+  }
+  
+  if(m_inAssociationColHdls.size()==0) {
+    warning() << "no Association Collection input" << endmsg;
+    return StatusCode::FAILURE;
+  }
+
+  m_nEvt = 0;
+  return GaudiAlgorithm::initialize();
+}
+
+StatusCode TrackParticleRelationAlg::execute() {
+  info() << "start Event " << m_nEvt << endmsg;
+  
+  // MCParticle
+  const edm4hep::MCParticleCollection* mcpCol = nullptr;
+  try {
+    mcpCol = m_inMCParticleColHdl.get();
+    for (auto mcp : *mcpCol) {
+      debug() << "id=" << mcp.id() << " pdgid=" << mcp.getPDG() << " charge=" << mcp.getCharge() << " genstat=" << mcp.getGeneratorStatus() << " simstat=" << mcp.getSimulatorStatus()
+	      << " p=" << mcp.getMomentum() << endmsg;
+      int nparents = mcp.parents_size();
+      int ndaughters = mcp.daughters_size();
+      for (int i=0; i<nparents; i++) {
+	debug() << "<<<<<<" << mcp.getParents(i).id() << endmsg;
+      }
+      for (int i=0; i<ndaughters; i++) {
+        debug() << "<<<<<<" << mcp.getDaughters(i).id() << endmsg;
+      }
+    }
+  }
+  catch ( GaudiException &e ) {
+    debug() << "Collection " << m_inMCParticleColHdl.fullKey() << " is unavailable in event " << m_nEvt << endmsg;
+  }
+  if(!mcpCol) {
+    warning() << "pass this event because MCParticle collection can not read" << endmsg; 
+    return StatusCode::SUCCESS;
+  }
+
+  // Prepare map from hit to MCParticle
+  std::map<edm4hep::TrackerHit, edm4hep::MCParticle> mapHitParticle;
+  debug() << "reading Association" << endmsg;
+  for (auto hdl : m_inAssociationColHdls) {
+    const edm4hep::MCRecoTrackerAssociationCollection* assCol = nullptr;
+    try {
+      assCol = hdl->get();
+    }
+    catch ( GaudiException &e ) {
+      debug() << "Collection " << hdl->fullKey() << " is unavailable in event " << m_nEvt << endmsg;
+      return StatusCode::FAILURE;
+    }
+    for (auto ass: *assCol) {
+      auto hit = ass.getRec();
+      auto particle = ass.getSim().getMCParticle();
+      mapHitParticle[hit] = particle;
+    }
+  }
+  
+  // Handle all input TrackCollection
+  for (unsigned icol=0; icol<m_inTrackColHdls.size(); icol++) {
+    auto hdl = m_inTrackColHdls[icol];
+
+    const edm4hep::TrackCollection* trkCol = nullptr;
+    try {
+      trkCol = hdl->get();
+    }
+    catch ( GaudiException &e ) {
+      debug() << "Collection " << hdl->fullKey() << " is unavailable in event " << m_nEvt << endmsg;
+    }
+
+    auto outCol = m_outColHdls[icol]->createAndPut();
+    if(!outCol) {
+      error() << "Collection " << m_outColHdls[icol]->fullKey() << " cannot be created" << endmsg;
+      return StatusCode::FAILURE;
+    }
+    
+    if(trkCol) {
+      std::map<edm4hep::MCParticle, std::vector<edm4hep::TrackerHit> > mapParticleHits;
+
+      for (auto track: *trkCol) {
+	std::map<edm4hep::MCParticle, int> mapParticleNHits;
+	
+	// Count hits related to MCParticle
+	int nhits = track.trackerHits_size();
+	for (int ihit=0; ihit<nhits; ihit++) {
+	  auto hit = track.getTrackerHits(ihit);
+	  auto itHP  = mapHitParticle.find(hit);
+	  if (itHP==mapHitParticle.end()) {
+	    warning() << "track " << track.id() << "'s hit " << hit.id() << "not in association list, will be ignored" << endmsg;
+	  }
+	  else {
+	    auto particle = itHP->second;
+	    if(!particle.isAvailable()) continue;
+	    debug() << "track " << track.id() << "'s hit " << hit.id() << " link to MCParticle " << particle.id() << endmsg;
+	    auto itPN = mapParticleNHits.find(particle);
+	    if (itPN!=mapParticleNHits.end()) itPN->second++;
+	    else                             mapParticleNHits[particle] = 1;
+
+	    if (msgLevel(MSG::DEBUG)) mapParticleHits[particle].push_back(hit);
+	  }
+	}
+	debug() << "Total " << mapParticleNHits.size() << " particles link to the track " << track.id() << endmsg;
+
+	// Save to collection
+	for (std::map<edm4hep::MCParticle, int>::iterator it=mapParticleNHits.begin(); it!=mapParticleNHits.end(); it++) {
+	  auto ass = outCol->create();
+	  ass.setWeight(it->second);
+	  ass.setRec(track);
+	  ass.setSim(it->first);
+	}
+      }
+      
+      if (msgLevel(MSG::DEBUG)) {
+	for (std::map<edm4hep::MCParticle, std::vector<edm4hep::TrackerHit> >::iterator it=mapParticleHits.begin(); it!=mapParticleHits.end(); it++) {
+	  auto particle = it->first;
+	  auto hits     = it->second;
+	  debug() << "=== MCPaticle ===" << particle << endmsg;
+	  for (auto hit : hits) {
+	    debug() << hit << endmsg;
+	  }
+	}
+      }
+    }
+  }
+
+  m_nEvt++;
+  return StatusCode::SUCCESS;
+}
+
+StatusCode TrackParticleRelationAlg::finalize() {
+  
+  return StatusCode::SUCCESS;
+}
diff --git a/Reconstruction/RecAssociationMaker/src/TrackParticleRelationAlg.h b/Reconstruction/RecAssociationMaker/src/TrackParticleRelationAlg.h
new file mode 100644
index 0000000000000000000000000000000000000000..9f19212bd6d054d8b6d562bf282b0391748327a3
--- /dev/null
+++ b/Reconstruction/RecAssociationMaker/src/TrackParticleRelationAlg.h
@@ -0,0 +1,38 @@
+#ifndef TrackParticleRelationAlg_h
+#define TrackParticleRelationAlg_h 1
+
+#include "k4FWCore/DataHandle.h"
+#include "GaudiAlg/GaudiAlgorithm.h"
+
+#include "edm4hep/MCParticleCollection.h"
+#include "edm4hep/TrackCollection.h"
+#include "edm4hep/MCRecoTrackerAssociationCollection.h"
+#include "edm4hep/MCRecoTrackParticleAssociationCollection.h"
+
+class TrackParticleRelationAlg : public GaudiAlgorithm {
+ public:
+  // Constructor of this form must be provided
+  TrackParticleRelationAlg( const std::string& name, ISvcLocator* pSvcLocator );
+
+  // Three mandatory member functions of any algorithm
+  StatusCode initialize() override;
+  StatusCode execute() override;
+  StatusCode finalize() override;
+
+ private:
+  // input MCParticle
+  DataHandle<edm4hep::MCParticleCollection>                              m_inMCParticleColHdl{"MCParticle", Gaudi::DataHandle::Reader, this};
+  // input Tracks to make relation
+  std::vector<DataHandle<edm4hep::TrackCollection>* >                    m_inTrackColHdls;
+  Gaudi::Property<std::vector<std::string> >                             m_inTrackCollectionNames{this, "TrackList", {"SiTracks"}};
+  // input TrackerAssociation to link TrackerHit and SimTrackerHit
+  std::vector<DataHandle<edm4hep::MCRecoTrackerAssociationCollection>* > m_inAssociationColHdls;
+  Gaudi::Property<std::vector<std::string> >                             m_inAssociationCollectionNames{this, "TrackerAssociationList", {"VXDTrackerHitAssociation",
+        "SITTrackerHitAssociation", "SETTrackerHitAssociation", "FTDTrackerHitAssociation"}};
+
+  // output TrackParticleAssociation
+  std::vector<DataHandle<edm4hep::MCRecoTrackParticleAssociationCollection>* > m_outColHdls;
+
+  int m_nEvt;
+};
+#endif
diff --git a/Reconstruction/SiliconTracking/CMakeLists.txt b/Reconstruction/SiliconTracking/CMakeLists.txt
index 9eb87350466e1836ee641b685b1a9ac24e084126..8fd9be94f130023a15e91f362d5297e5e248d4c5 100644
--- a/Reconstruction/SiliconTracking/CMakeLists.txt
+++ b/Reconstruction/SiliconTracking/CMakeLists.txt
@@ -7,6 +7,7 @@ gaudi_add_module(SiliconTracking
                          src/TrackSubsetAlg.cpp
                  LINK GearSvc
                       EventSeeder
+                      TrackingLib
                       TrackSystemSvcLib
                       DataHelperLib
                       KiTrackLib
diff --git a/Reconstruction/SiliconTracking/src/ForwardTrackingAlg.cpp b/Reconstruction/SiliconTracking/src/ForwardTrackingAlg.cpp
index 3eb06e29713317bc809ef615a31e801259e8fd56..3184563ffb2390fc231ae216da04cfc6ce7c6ba5 100644
--- a/Reconstruction/SiliconTracking/src/ForwardTrackingAlg.cpp
+++ b/Reconstruction/SiliconTracking/src/ForwardTrackingAlg.cpp
@@ -4,7 +4,7 @@
 #include "DataHelper/Navigation.h"
 
 #include "edm4hep/TrackerHit.h"
-#include "edm4hep/TrackerHit.h"
+#include "edm4hep/TrackerHitPlane.h"
 #include "edm4hep/Track.h"
 #if __has_include("edm4hep/EDM4hepVersion.h")
 #include "edm4hep/EDM4hepVersion.h"
@@ -74,12 +74,16 @@ StatusCode ForwardTrackingAlg::initialize(){
   // can be printed. As this is mainly used for debugging it is not a steerable parameter.
   //if( _useCED )MarlinCED::init(this) ;    //CED
 
+  // Set up the track fit tool
+  m_fitTool = ToolHandle<ITrackFitterTool>(m_fitToolName.value());
+
   if(m_dumpTime){
     NTuplePtr nt1(ntupleSvc(), "MyTuples/Time"+name());
     if ( !nt1 ) {
       m_tuple = ntupleSvc()->book("MyTuples/Time"+name(),CLID_ColumnWiseTuple,"Tracking time");
       if ( 0 != m_tuple ) {
 	m_tuple->addItem ("timeTotal",  m_timeTotal ).ignore();
+	m_tuple->addItem ("timeKalman", m_timeKalman ).ignore();
       }
       else {
 	fatal() << "Cannot book MyTuples/Time"+name() <<endmsg;
@@ -306,7 +310,9 @@ StatusCode ForwardTrackingAlg::execute(){
       _map_sector_hits[ ftdHit->getSector() ].push_back( ftdHit );         
     }
   }
-     
+
+  if(m_dumpTime&&m_tuple) m_timeKalman = 0;
+
   if( !_map_sector_hits.empty() ){
     /**********************************************************************************************/
     /*                Check if no sector is overflowing with hits                                 */
@@ -661,7 +667,7 @@ StatusCode ForwardTrackingAlg::execute(){
     debug() << "\t\t---Save Tracks---" << endmsg ;
       
     //auto trkCol = _outColHdl.createAndPut();
-    
+
     for (unsigned int i=0; i < tracks.size(); i++){
       FTDTrack* myTrack = dynamic_cast< FTDTrack* >( tracks[i] );
          
@@ -942,7 +948,7 @@ bool ForwardTrackingAlg::setCriteria( unsigned round ){
 }
 
 void ForwardTrackingAlg::finaliseTrack( edm4hep::MutableTrack* trackImpl ){
-     
+  auto stopwatch = TStopwatch();
   Fitter fitter( trackImpl , _trkSystem );
    
   //trackImpl->trackStates().clear();
@@ -1024,6 +1030,9 @@ void ForwardTrackingAlg::finaliseTrack( edm4hep::MutableTrack* trackImpl ){
   trackImpl->addToSubDetectorHitNumbers(hitNumbers[UTIL::ILDDetID::SET]);
   trackImpl->addToSubDetectorHitNumbers(hitNumbers[UTIL::ILDDetID::ETD]);
 #endif
+
+  if(m_dumpTime&&m_tuple) m_timeKalman += stopwatch.RealTime()*1000;
+
   return;
 }
 
diff --git a/Reconstruction/SiliconTracking/src/ForwardTrackingAlg.h b/Reconstruction/SiliconTracking/src/ForwardTrackingAlg.h
index bd7a5173be02214e734579e3d4c025f8006ec1a2..fee28994bf66bd1f0c7510faf11588e681afd5cc 100644
--- a/Reconstruction/SiliconTracking/src/ForwardTrackingAlg.h
+++ b/Reconstruction/SiliconTracking/src/ForwardTrackingAlg.h
@@ -14,6 +14,7 @@
 
 #include "edm4hep/TrackerHit.h"
 #include "TrackSystemSvc/IMarlinTrkSystem.h"
+#include "Tracking/ITrackFitterTool.h"
 //#include "gear/BField.h"
 
 #include "KiTrack/Segment.h"
@@ -239,6 +240,7 @@ class ForwardTrackingAlg : public GaudiAlgorithm {
 
   NTuple::Tuple*       m_tuple;
   NTuple::Item<float>  m_timeTotal;
+  NTuple::Item<float>  m_timeKalman;
 
   /** B field in z direction */
   double _Bz;
@@ -261,6 +263,7 @@ class ForwardTrackingAlg : public GaudiAlgorithm {
   Gaudi::Property<std::vector<float> > _critMinimaInit{this, "CriteriaMin", {} };
   Gaudi::Property<std::vector<float> > _critMaximaInit{this, "CriteriaMax", {} };
   Gaudi::Property<bool>   m_dumpTime{this, "DumpTime", true};
+  Gaudi::Property<std::string> m_fitToolName{this, "FitterTool", "KalTestTool/KalTest110"};
 
   std::map<std::string, std::vector<float> > _critMinima;
   std::map<std::string, std::vector<float> > _critMaxima;
@@ -291,6 +294,7 @@ class ForwardTrackingAlg : public GaudiAlgorithm {
   unsigned _nTrackCandidatesPlus;
      
   MarlinTrk::IMarlinTrkSystem* _trkSystem;
+  ToolHandle<ITrackFitterTool> m_fitTool;
   
   /** The quality of the output track collection */
   int _output_track_col_quality ; 
diff --git a/Reconstruction/SiliconTracking/src/SiliconTrackingAlg.cpp b/Reconstruction/SiliconTracking/src/SiliconTrackingAlg.cpp
index 1ef104fa4b479dde6ce3be62b98ad6a6feb31b4a..df203553f486e813a95b88c99feb094154a8cd31 100644
--- a/Reconstruction/SiliconTracking/src/SiliconTrackingAlg.cpp
+++ b/Reconstruction/SiliconTracking/src/SiliconTrackingAlg.cpp
@@ -113,13 +113,17 @@ StatusCode  SiliconTrackingAlg::initialize() {
   _nRun = -1 ;
   _nEvt = 0 ;
   //printParameters() ;
-  if(m_dumpTime){
+
+  // Set up the track fit tool
+  m_fitTool = ToolHandle<ITrackFitterTool>(m_fitToolName.value());
+
+  if (m_dumpTime) {
     NTuplePtr nt1(ntupleSvc(), "MyTuples/Time"+name());
     if ( !nt1 ) {
       m_tuple = ntupleSvc()->book("MyTuples/Time"+name(),CLID_ColumnWiseTuple,"Tracking time");
       if ( 0 != m_tuple ) {
-	m_tuple->addItem ("timeTotal",  m_timeTotal ).ignore();
-	m_tuple->addItem ("timeKalman", m_timeKalman ).ignore();
+	m_tuple->addItem("timeTotal",  m_timeTotal).ignore();
+	m_tuple->addItem("timeKalman", m_timeKalman).ignore();
       }
       else {
 	fatal() << "Cannot book MyTuples/Time"+name() <<endmsg;
@@ -130,6 +134,27 @@ StatusCode  SiliconTrackingAlg::initialize() {
       m_tuple = nt1;
     }
   }
+
+  if (m_debug) {
+    NTuplePtr nt2(ntupleSvc(), "MyTuples/"+name());
+    if ( !nt2 ) {
+      m_tupleDebug = ntupleSvc()->book("MyTuples/"+name(),CLID_ColumnWiseTuple,"Tracking time");
+      if ( 0 != m_tuple ) {
+	m_tupleDebug->addItem("nhit", m_nhit, 0, 100).ignore();
+	m_tupleDebug->addIndexedItem("layer", m_nhit, m_layer).ignore();
+	m_tupleDebug->addIndexedItem("chi2",  m_nhit, m_chi2).ignore();
+	m_tupleDebug->addIndexedItem("used",  m_nhit, m_used).ignore();
+      }
+      else {
+        fatal() << "Cannot book MyTuples/"+name() <<endmsg;
+	return StatusCode::FAILURE;
+      }
+    }
+    else{
+      m_tupleDebug = nt2;
+    }
+  }
+
   // 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");
@@ -192,7 +217,7 @@ StatusCode SiliconTrackingAlg::execute(){
   
   // 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();
@@ -205,14 +230,13 @@ StatusCode SiliconTrackingAlg::execute(){
   //int runNo = header.getRunNumber();
   //debug() << "Processing Run[" << runNo << "]::Event[" << evtNo << "]" << endmsg;
 
-  _trackImplVec.reserve(100);
+  //_trackImplVec.reserve(100);
 
   int successVTX = InitialiseVTX();
-  //int successFTD = 0;
   int successFTD = InitialiseFTD();
   if (successVTX == 1) {
     
-    debug() << "      phi          theta        layer      nh o :   m :   i  :: o*m*i " << endmsg; 
+    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) {
@@ -225,7 +249,7 @@ StatusCode SiliconTrackingAlg::execute(){
   }
   
   if (successFTD == 1) {
-    debug() << "      phi          side        layer      nh o :   m :   i  :: o*m*i " << endmsg;
+    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;
   }
@@ -250,7 +274,17 @@ StatusCode SiliconTrackingAlg::execute(){
            trackIter < tracksWithNHits.end(); trackIter++) {
         CreateTrack( *trackIter );
       }
-      debug() <<  "End of creating "<< nHits << " hits tracks " << endmsg;
+      debug() <<  "End of creating "<< nHits << " hits tracks, total " << _trackImplVec.size() << " tracks now" << endmsg;
+
+      if (msgLevel(MSG::DEBUG)) {
+	for (auto trackAR : _trackImplVec) {
+	  debug() << "track " << trackAR << " has hits:" << endmsg;
+	  TrackerHitExtendedVec& hitVec = trackAR->getTrackerHitExtendedVec();
+	  for (auto hit : hitVec) {
+	    debug() << " " << hit->getTrackerHit().id().collectionID << "-" << hit->getTrackerHit().id().index << endmsg;
+	  }
+	}
+      }
     }
     
     if (_attachFast == 0) {
@@ -264,10 +298,6 @@ StatusCode SiliconTrackingAlg::execute(){
     
     debug() <<  "End of picking up remaining hits " << endmsg;
 
-    //edm4hep::TrackCollection* trkCol = nullptr; 
-    //edm4hep::LCRelationCollection* relCol = nullptr;
-    //auto trkCol = _outColHdl.createAndPut();
-    //auto relCol = _outRelColHdl.createAndPut();
     /*
     LCCollectionVec * trkCol = new LCCollectionVec(LCIO::TRACK);
     // if we want to point back to the hits we need to set the flag
@@ -539,7 +569,7 @@ int SiliconTrackingAlg::InitialiseFTD() {
       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 << " cellID = " << hit.getCellID() << endmsg;  
+      debug() << " FTD Pixel Hit " << hit.id().index << " added : @ " << pos[0] << " " << pos[1] << " " << pos[2] << " drphi " << hitExt->getResolutionRPhi() << " dz " << hitExt->getResolutionZ() << "  iPhi = " << iPhi << " iSemiSphere "  << iSemiSphere << " iCode = " << iCode << " layer = " << layer << " cellID = " << hit.getCellID() << endmsg;
     }
   }
   // Reading out FTD SpacePoint Collection
@@ -603,7 +633,7 @@ int SiliconTrackingAlg::InitialiseFTD() {
       
       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));
@@ -636,7 +666,7 @@ int SiliconTrackingAlg::InitialiseFTD() {
       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 << " cellID = " << hit.getCellID() << endmsg;  
+      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 << " cellID = " << hit.getCellID() << endmsg;
       
     }
   }
@@ -713,7 +743,7 @@ int SiliconTrackingAlg::InitialiseVTX() {
       // 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
@@ -733,7 +763,7 @@ int SiliconTrackingAlg::InitialiseVTX() {
       double Phi = atan2(pos[1],pos[0]);
       
       if (Phi < 0.) Phi = Phi + TWOPI;
-      
+
       // get the layer number
       int layer = getLayerID(hit);
       
@@ -747,7 +777,7 @@ int SiliconTrackingAlg::InitialiseVTX() {
       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;  
+      debug() << " VXD Hit " <<  hit.id().index << " added : @ " << pos[0] << " " << pos[1] << " " << pos[2] << " drphi " << hitExt->getResolutionRPhi() << " dz " << hitExt->getResolutionZ() << "  iPhi = " << iPhi <<  " iTheta "  << iTheta << " iCode = " << iCode << "  layer = " << layer << endmsg;
       
     }
   }
@@ -906,7 +936,7 @@ int SiliconTrackingAlg::InitialiseVTX() {
         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;  
+        debug() << " SIT Hit " <<  trkhit.id().index << " added : @ " << pos[0] << " " << pos[1] << " " << pos[2] << " drphi " << hitExt->getResolutionRPhi() << " dz " << hitExt->getResolutionZ() << "  iPhi = " << iPhi <<  " iTheta "  << iTheta << " iCode = " << iCode << "  layer = " << layer << endmsg;
         
       }
     }    
@@ -1063,13 +1093,13 @@ void SiliconTrackingAlg::ProcessOneSector(int iPhi, int iTheta) {
                 
                 if (nHitsInner > 0) {
                   
-                  debug() << " " 
-                  << std::setw(3) << iPhi       << " "   << std::setw(3) << ipMiddle << " "      << std::setw(3) << ipInner << "   " 
-                  << std::setw(3) << iTheta     << " "   << std::setw(3) << itMiddle << " "      << std::setw(3) << itInner << "  " 
-                  << std::setw(3) << nLR[0]     << " "   << std::setw(3) << nLR[1]   << " "      << std::setw(3) << nLR[2]  << "     " 
-                  << std::setw(3) << nHitsOuter << " : " << std::setw(3) << nHitsMiddle << " : " << std::setw(3) << nHitsInner << "  :: " 
-                  << std::setw(3) << nHitsOuter*nHitsMiddle* nHitsInner << endmsg;
-                  
+                  debug() << "pattern "
+			  << std::setw(3) << iPhi       << " "   << std::setw(3) << ipMiddle << " "      << std::setw(3) << ipInner << "   "
+			  << std::setw(3) << iTheta     << " "   << std::setw(3) << itMiddle << " "      << std::setw(3) << itInner << "  "
+			  << std::setw(3) << nLR[0]     << " "   << std::setw(3) << nLR[1]   << " "      << std::setw(3) << nLR[2]  << "     "
+			  << std::setw(3) << nHitsOuter << " : " << std::setw(3) << nHitsMiddle << " : " << std::setw(3) << nHitsInner << "  :: "
+			  << std::setw(3) << nHitsOuter*nHitsMiddle*nHitsInner << endmsg;
+
                   // test all triplets 
                   
                   for (int iOuter=0; iOuter<nHitsOuter; ++iOuter) { // loop over hits in the outer sector
@@ -1219,6 +1249,8 @@ TrackExtended * SiliconTrackingAlg::TestTriplet(TrackerHitExtended * outerHit,
   float d0 = par[3];
   float z0 = par[4];
 
+  debug() << "after fastHelixFit: chi2RPhi = " << chi2RPhi << " chi2Z = " << chi2Z
+	  << " tanlambda = " << tanlambda << " coslambda = " << cos(atan(tanlambda)) << endmsg;
   // chi2 is weighted here by a factor for both rphi and z
   float Chi2 = chi2RPhi*_chi2WRPhiTriplet+chi2Z*_chi2WZTriplet;
   int ndf = 2*NPT-5;
@@ -1476,6 +1508,7 @@ int SiliconTrackingAlg::BuildTrack(TrackerHitExtended * outerHit,
    */
   
   debug() << " BuildTrack starting " << endmsg;
+  debug() << outerHit->getTrackerHit().id().index << " " << middleHit->getTrackerHit().id().index << " " << innerHit->getTrackerHit().id().index << " innerLayer=" << innerLayer << endmsg;
   
   for (int layer = innerLayer-1; layer>=0; layer--) { // loop over remaining layers
     float distMin = 1.0e+20;
@@ -1571,7 +1604,7 @@ int SiliconTrackingAlg::BuildTrack(TrackerHitExtended * outerHit,
       float chi2RPhi;
       float chi2Z;
       
-      //debug() << "######## number of hits to fit with _fastfitter = " << NPT << endmsg; 
+      //debug() << "######## number of hits to fit with _fastfitter = " << NPT << endmsg;
       
       _fastfitter->fastHelixFit(NPT, xh, yh, rh, ph, wrh, zh, wzh,iopt, par, epar, chi2RPhi, chi2Z);
       par[3] = par[3]*par[0]/fabs(par[0]);
@@ -1623,7 +1656,8 @@ int SiliconTrackingAlg::BuildTrack(TrackerHitExtended * outerHit,
   } // endloop over remaining layers
   TrackerHitExtendedVec& hvec = trackAR->getTrackerHitExtendedVec();  
   int nTotalHits = int(hvec.size());
-  debug() << "######## number of hits to return = " << nTotalHits << endmsg; 
+  debug() << "######## number of hits to return = " << nTotalHits << endmsg;
+
   return nTotalHits;
 }
 
@@ -1654,15 +1688,20 @@ void SiliconTrackingAlg::CreateTrack(TrackExtended * trackAR ) {
    Method which creates Track out of TrackExtended objects. Checks for possible
    track splitting (separate track segments in VXD and FTD).
    */
-  
+  debug() << "CreateTrack " << trackAR << endmsg;
   
   TrackerHitExtendedVec& hitVec = trackAR->getTrackerHitExtendedVec();
   int nHits = int(hitVec.size());
   
   for (int i=0; i<nHits; ++i) {
     TrackExtendedVec& trackVec = hitVec[i]->getTrackExtendedVec();
-    if (trackVec.size() != 0) 
+    //edm4hep::TrackerHit trkHit = hitVec[i]->getTrackerHit();
+    //debug() << "Hit " << trkHit.id().collectionID << "-" << trkHit.id().index << endmsg;
+    if (trackVec.size() != 0) {
+      //debug() << "    used in TrackExtended " << endmsg;
       return ;
+      //continue;
+    }
   }
   
   // First check if the current track is piece of the split one
@@ -1674,7 +1713,7 @@ void SiliconTrackingAlg::CreateTrack(TrackExtended * trackAR ) {
   
   for (int itrk=0; itrk<nTrk; ++itrk) {
     TrackExtended * trackOld = _trackImplVec[itrk];
-    TrackerHitExtendedVec& hitVecOld = trackOld->getTrackerHitExtendedVec();
+    TrackerHitExtendedVec hitVecOld = trackOld->getTrackerHitExtendedVec();
     
     float phiNew = trackAR->getPhi();
     float phiOld = trackOld->getPhi();
@@ -1713,6 +1752,7 @@ void SiliconTrackingAlg::CreateTrack(TrackExtended * trackAR ) {
       }      
       for (int ih=0;ih<nHitsOld;++ih) {
 	edm4hep::TrackerHit trkHit = hitVecOld[ih]->getTrackerHit();
+	//debug() << "old track's hit " << trkHit.id().collectionID << " " << trkHit.id().index << endmsg;
         xh[ih+nHits] = trkHit.getPosition()[0];
         yh[ih+nHits] = trkHit.getPosition()[1];
         zh[ih+nHits] = float(trkHit.getPosition()[2]);
@@ -1809,6 +1849,15 @@ void SiliconTrackingAlg::CreateTrack(TrackExtended * trackAR ) {
       // Attach hits belonging to the current track segment to  
       // the track already created
       if (found == 1) {
+	if (iBad!=-1) {
+	  if (iBad>=nHits)
+	    debug() << "Bad hit " << iBad << " " << hitVecOld[iBad-nHits]->getTrackerHit().id().collectionID << "-" << hitVecOld[iBad-nHits]->getTrackerHit().id().index << endmsg;
+	  else
+	    debug() << "Bad hit " << iBad << " " << hitVec[iBad]->getTrackerHit().id().collectionID << "-" << hitVec[iBad]->getTrackerHit().id().index << endmsg;
+	}
+	else
+	  debug() << "Bad hit " << iBad << endmsg;
+
         trackOld->ClearTrackerHitExtendedVec();
         for (int i=0;i<nHits;++i) {
           TrackerHitExtended * trkHit = hitVec[i];
@@ -1823,6 +1872,7 @@ void SiliconTrackingAlg::CreateTrack(TrackExtended * trackAR ) {
         for (int i=0;i<nHitsOld;++i) {
           int icur = i+nHits;
           TrackerHitExtended * trkHit = hitVecOld[i];
+	  debug() << "add old hit " << i << " " << trkHit->getTrackerHit().id().collectionID << "-" << trkHit->getTrackerHit().id().index << endmsg;
           trkHit->clearTrackVec();
           if (icur == iBad) {
           }
@@ -1847,7 +1897,8 @@ void SiliconTrackingAlg::CreateTrack(TrackExtended * trackAR ) {
         trackOld->setChi2(chi2Min*float(ndf));  
         trackOld->setNDF(ndf);
         trackOld->setCovMatrix(eparmin);
-        //      trackOld->setReferencePoint(refPointMin);
+	// TODO: by fucd, removed why in ILCSoft
+	//trackOld->setReferencePoint(refPointMin);
       }
       
       delete[] xh;
@@ -2026,7 +2077,8 @@ void SiliconTrackingAlg::AttachRemainingVTXHitsFast() {
 }
 
 void SiliconTrackingAlg::AttachRemainingVTXHitsSlow() {
-  
+  debug() << "AttachRemainingVTXHitsSlow()" << endmsg;
+
   TrackerHitExtendedVec nonAttachedHits;
   nonAttachedHits.clear();
   
@@ -2049,11 +2101,9 @@ void SiliconTrackingAlg::AttachRemainingVTXHitsSlow() {
             if(isize>maxTrackSize)maxTrackSize = isize;
           }     
           if (maxTrackSize<=3) { 
-            debug() << " Add non attached hit to list: id = " << hit->getTrackerHit().id() << endmsg;
+	    debug() << " Add non attached hit to list: id = " << hit->getTrackerHit().id().index << endmsg;
             nonAttachedHits.push_back( hit );
           } 
-          
-          
         }
       }
     }
@@ -2064,7 +2114,7 @@ void SiliconTrackingAlg::AttachRemainingVTXHitsSlow() {
   int nTrk = int(_trackImplVec.size()); 
   for (int iHit=0; iHit<nNotAttached; ++iHit) {
     TrackerHitExtended * hit = nonAttachedHits[iHit];
-    debug() << " Try hit: id = " << hit->getTrackerHit().id() << endmsg;
+    debug() << " Try hit: id = " << hit->getTrackerHit().id().index << endmsg;
     int layer = getLayerID( hit->getTrackerHit() );
     if (layer > _minimalLayerToAttach) {
       float pos[3];
@@ -2096,7 +2146,7 @@ void SiliconTrackingAlg::AttachRemainingVTXHitsSlow() {
               distance = sqrt(distance);
               if (distance<_minDistToDelta) {
                 consider = false;
-                debug() << " hit: id = " << hit->getTrackerHit().id() << " condsidered delta together with hit " << trkhit2.id() << endmsg;
+                debug() << " hit: id = " << hit->getTrackerHit().id().index << " condsidered delta together with hit " << trkhit2.id().index << endmsg;
                 break;
               }
             }       
@@ -2109,12 +2159,13 @@ void SiliconTrackingAlg::AttachRemainingVTXHitsSlow() {
           float z0 = trackAR->getZ0();
           float omega = trackAR->getOmega();
           float tanlambda = trackAR->getTanLambda();
+	  //debug() << "attach hit now, field = " << _bField << " d0 = " << d0 << " phi0 = " << phi0 << " z0 = " << z0 << " omega = " << omega << " tanlambda = " << tanlambda << endmsg;
           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];
+	      minDist = distance[2];
               trackToAttach = trackAR;
             }
           }
@@ -2122,16 +2173,18 @@ void SiliconTrackingAlg::AttachRemainingVTXHitsSlow() {
       }
       if (minDist < _minDistCutAttach && trackToAttach != NULL) {
         int iopt = 2;
-        debug() << " Hit: id = " << hit->getTrackerHit().id() << " : try attachement"<< endmsg;
+        debug() << " Hit: id = " << hit->getTrackerHit().id().index << " : 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;
+        debug() << " Hit: id = " << hit->getTrackerHit().id().index << " rejected due to distance cut of " <<_minDistCutAttach<< " min distance = "  << minDist << endmsg;
       }      
     }
   }  
 }
 
 void SiliconTrackingAlg::AttachRemainingFTDHitsSlow() {
+  debug() << "AttachRemainingFTDHitsSlow()" << endmsg;
+
   TrackerHitExtendedVec nonAttachedHits;
   nonAttachedHits.clear();
   
@@ -2145,6 +2198,7 @@ void SiliconTrackingAlg::AttachRemainingFTDHitsSlow() {
           TrackerHitExtended * hit = hitVec[iH];
           TrackExtendedVec& trackVec = hit->getTrackExtendedVec();
           if (trackVec.size()==0)
+	    debug() << " Add non attached hit to list: id = " << hit->getTrackerHit().id().index << endmsg;
             nonAttachedHits.push_back( hit );
         }
       }
@@ -2172,7 +2226,7 @@ void SiliconTrackingAlg::AttachRemainingFTDHitsSlow() {
         // 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()) {
-          
+	  debug() << " hit: id = " << hit->getTrackerHit().id().index << " condsidered delta together with hit " << hitVector[IHIT]->getTrackerHit().id().index << endmsg;
           consider = false;
           break;
         }
@@ -2192,7 +2246,7 @@ void SiliconTrackingAlg::AttachRemainingFTDHitsSlow() {
           float time = helix.getDistanceToPoint(pos,distance);
           if (time < 1.0e+10) {
             if (distance[2] < minDist) {
-              minDist = distance[2];
+	      minDist = distance[2];
               trackToAttach = trackAR;
             }
           }
@@ -2201,8 +2255,12 @@ void SiliconTrackingAlg::AttachRemainingFTDHitsSlow() {
     }
     if (minDist < _minDistCutAttach && trackToAttach != NULL) {
       int iopt = 2;
+      debug() << " Hit: id = " << hit->getTrackerHit().id().index << " : try attachement"<< endmsg;
       AttachHitToTrack(trackToAttach,hit,iopt);
-    }      
+    }
+    else {
+      debug() << " Hit: id = " << hit->getTrackerHit().id().index << " rejected due to distance cut of " <<_minDistCutAttach<< " min distance = "  << minDist << endmsg;
+    }
   }  
 }
 
@@ -2320,6 +2378,7 @@ void SiliconTrackingAlg::TrackingInFTD() {
         
         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;
+	  info() << "iS=" << iS << " nLS=" << nLS[0] << " _nlayersFTD=" << _nlayersFTD << " ipOuter=" << ipOuter << endmsg;
           exit(1);
         }
         
@@ -2370,12 +2429,12 @@ void SiliconTrackingAlg::TrackingInFTD() {
                   //                  << "\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;
+                  debug() << "pattern "
+			  << std::setw(3) << ipOuter       << " "   << std::setw(3) << ipMiddle << " "      << std::setw(3) << ipInner << "       "
+			  << ((iS==0)?" +z      ":" -z      ")
+			  << 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);
@@ -2582,7 +2641,7 @@ void SiliconTrackingAlg::FinalRefit(edm4hep::TrackCollection* trk_col) {
     TrackerHitExtendedVec& hitVec = trackAR->getTrackerHitExtendedVec();
     
     int nHits = int(hitVec.size());
-    //debug() << "  Track " << iTrk << " has hits: " << nHits << endmsg;
+    debug() << "  Track " << iTrk << " has hits: " << nHits << endmsg;
     if( nHits >= _minimalHits) {
       //    int * lh = new int[nHits];
       std::vector<int> lh;
@@ -2700,10 +2759,12 @@ void SiliconTrackingAlg::FinalRefit(edm4hep::TrackCollection* trk_col) {
       
       int nFit = 0;
       for (int i=0; i<nHits; ++i) {
+	edm4hep::TrackerHit trkHit = hitVec[i]->getTrackerHit();
+	debug() << "TrackerHit " << i << " id = " << trkHit.id().collectionID << "-" << trkHit.id().index << endmsg;
         // 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();
-	  debug() << "TrackerHit " << i << " id = " << trkHit.id() << endmsg;
+	  //edm4hep::TrackerHit trkHit = hitVec[i]->getTrackerHit();
+	  debug() << "                  accept" << endmsg;
           nFit++;
           if(trkHit.isAvailable()) { 
             trkHits.push_back(trkHit);   
@@ -2722,6 +2783,10 @@ void SiliconTrackingAlg::FinalRefit(edm4hep::TrackCollection* trk_col) {
         debug() << "SiliconTrackingAlg::FinalRefit: Cannot fit less than 3 hits. Number of hits =  " << trkHits.size() << endmsg;
         continue ; 
       }
+
+      // test w/o fit
+      //continue;
+
       //TrackImpl* Track = new TrackImpl ;
       //auto track = trk_col->create();
       //fucd
@@ -2759,45 +2824,14 @@ void SiliconTrackingAlg::FinalRefit(edm4hep::TrackCollection* trk_col) {
       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;
       debug() << "call createFinalisedLCIOTrack now" << endmsg;
       try {
-        status = MarlinTrk::createFinalisedLCIOTrack(marlinTrk, trkHits, &track, fit_backwards, covMatrix, _bField, _maxChi2PerHit);
+        //status = MarlinTrk::createFinalisedLCIOTrack(marlinTrk, trkHits, &track, fit_backwards, covMatrix, _bField, _maxChi2PerHit);
+	status = m_fitTool->Fit(track, trkHits, covMatrix, _maxChi2PerHit, fit_backwards);
       } catch (...) {
 	//      delete Track;
         //      delete marlinTrk;
@@ -2818,29 +2852,48 @@ void SiliconTrackingAlg::FinalRefit(edm4hep::TrackCollection* trk_col) {
       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);
-      
+
+      UTIL::BitField64 cellID_encoder( UTIL::ILDCellID0::encoder_string ) ;
+
+      //marlinTrk->getHitsInFit(hits_in_fit);
+      hits_in_fit = m_fitTool->GetHitsInFit();
+
       for ( unsigned ihit = 0; ihit < hits_in_fit.size(); ++ihit) {
-	debug() << "Hit id =" << hits_in_fit[ihit].first.id() << endmsg;
-	edm4hep::TrackerHit trk = hits_in_fit[ihit].first;
-        all_hits.push_back(trk);//hits_in_fit[ihit].first);
+	edm4hep::TrackerHit hit = hits_in_fit[ihit].first;
+        //all_hits.push_back(hit);//hits_in_fit[ihit].first);
+	if (m_tupleDebug) {
+	  cellID_encoder.setValue(hit.getCellID());
+	  m_layer[m_nhit] = cellID_encoder[lcio::ILDCellID0::layer];
+	  m_chi2[m_nhit]  = hits_in_fit[ihit].second;
+	  m_used[m_nhit]  = true;
+	  debug() << "Hit id =" << hit.id().index << " layer = " << m_layer[m_nhit] << " det = " << cellID_encoder[lcio::ILDCellID0::subdet] << endmsg;
+	  m_nhit++;
+	}
+	all_hits.push_back(hit);
       }
       
-      UTIL::BitField64 cellID_encoder( UTIL::ILDCellID0::encoder_string ) ; 
-      
       MarlinTrk::addHitNumbersToTrack(&track, all_hits, true, cellID_encoder);
       
-      marlinTrk->getOutliers(outliers);
-      
+      //marlinTrk->getOutliers(outliers);
+      outliers = m_fitTool->GetOutliers();
+
       for ( unsigned ihit = 0; ihit < outliers.size(); ++ihit) {
         all_hits.push_back(outliers[ihit].first);
+	if (m_tupleDebug) {
+	  cellID_encoder.setValue(outliers[ihit].first.getCellID());
+	  m_layer[m_nhit] = cellID_encoder[lcio::ILDCellID0::layer];
+	  m_chi2[m_nhit]  = outliers[ihit].second;
+	  m_used[m_nhit]  = false;
+	  m_nhit++;
+	}
       }
       
       MarlinTrk::addHitNumbersToTrack(&track, all_hits, false, cellID_encoder);
-      
-      delete marlinTrk;
+      if (m_tupleDebug) m_tupleDebug->write();
+
+      //delete marlinTrk;
+      m_fitTool->Clear();
+
 #if EDM4HEP_BUILD_VERSION > EDM4HEP_VERSION(0, 9, 0)
       int nhits_in_vxd = track.getSubdetectorHitNumbers(0);
       int nhits_in_ftd = track.getSubdetectorHitNumbers(1);
@@ -2851,8 +2904,7 @@ void SiliconTrackingAlg::FinalRefit(edm4hep::TrackCollection* trk_col) {
       int nhits_in_sit = track.getSubDetectorHitNumbers(2);
 #endif
       
-      //debug() << " Hit numbers for Track "<< track.id() << ": "
-      debug() << " Hit numbers for Track "<< iTrk <<": "
+      debug() << " Hit numbers for Track "<< iTrk <<" (id=" << track.id().index << "): "
 	      << " vxd hits = " << nhits_in_vxd
 	      << " ftd hits = " << nhits_in_ftd
 	      << " sit hits = " << nhits_in_sit
@@ -2864,13 +2916,13 @@ void SiliconTrackingAlg::FinalRefit(edm4hep::TrackCollection* trk_col) {
 
       if( status != IMarlinTrack::success ) {       
         //delete track;
-        debug() << "FinalRefit: Track fit failed with error code " << status << " track dropped. Number of hits = "<< trkHits.size() << endmsg;       
+	debug() << "FinalRefit: Track fit failed with error code " << status << " track dropped. Number of hits = "<< trkHits.size() << endmsg;
         continue ;
       }
       
       if( track.getNdf() < 0) {       
         //delete track;
-        debug() << "FinalRefit: Track fit returns " << track.getNdf() << " degress of freedom track dropped. Number of hits = "<< trkHits.size() << endmsg;       
+	debug() << "FinalRefit: Track fit returns " << track.getNdf() << " degress of freedom track dropped. Number of hits = "<< trkHits.size() << endmsg;
 	//delete track;
         continue ;
       }
@@ -2884,7 +2936,7 @@ void SiliconTrackingAlg::FinalRefit(edm4hep::TrackCollection* trk_col) {
 	if(trkStateIP.location !=1) continue;
       /*
       if (trkStateIP == 0) {
-        debug() << "SiliconTrackingAlg::FinalRefit: Track fit returns " << track->getNdf() << " degress of freedom track dropped. Number of hits = "<< trkHits.size() << endmsg;       
+        debug() << "SiliconTrackingAlg::FinalRefit: Track fit returns " << track->getNdf() << " degress of freedom track dropped. Number of hits = "<< trkHits.size() << endmsg;
         throw EVENT::Exception( std::string("SiliconTrackingAlg::FinalRefit: trkStateIP pointer == NULL ")  ) ;
       }
       */
@@ -2897,8 +2949,8 @@ void SiliconTrackingAlg::FinalRefit(edm4hep::TrackCollection* trk_col) {
 	
 	float cov[15];
 	
-	for (int i = 0 ; i<15 ; ++i) {
-	  cov[i] = trkStateIP.covMatrix.operator[](i);
+	for (int icov = 0 ; icov < 15 ; ++icov) {
+	  cov[icov] = trkStateIP.covMatrix.operator[](icov);
 	}
       
 	trackAR->setCovMatrix(cov);
@@ -2924,8 +2976,7 @@ void SiliconTrackingAlg::FinalRefit(edm4hep::TrackCollection* trk_col) {
   }
   if(m_dumpTime&&m_tuple) m_timeKalman = stopwatch.RealTime()*1000;
 
-  debug() << " -> run " << _nRun << " event " << _nEvt << endmsg;
-  debug() << "Number of Si tracks = " << nSiSegments << endmsg;
+  info() << " -> run " << _nRun << " event " << _nEvt << " Number of Si tracks = " << nSiSegments << endmsg;
   debug() << "Total 4-momentum of Si Tracks : E = " << std::setprecision(7) << eTot
 	  << " Px = " << pxTot
 	  << " Py = " << pyTot
@@ -3075,6 +3126,9 @@ StatusCode SiliconTrackingAlg::setupGearGeom(){
       
     } 
   }
+
+  info() << "nvxd = " << _nLayersVTX << " nsit = " << _nLayersSIT << " nftd = " << _nlayersFTD << endmsg;
+
   return StatusCode::SUCCESS;
 }
 
diff --git a/Reconstruction/SiliconTracking/src/SiliconTrackingAlg.h b/Reconstruction/SiliconTracking/src/SiliconTrackingAlg.h
index e8e915b56530c595f1ed01040c2d5d6421af4bfb..c457d446bc20205b4f7a8bd5aa86597ec8acbfae 100644
--- a/Reconstruction/SiliconTracking/src/SiliconTrackingAlg.h
+++ b/Reconstruction/SiliconTracking/src/SiliconTrackingAlg.h
@@ -23,6 +23,7 @@
 #include "DataHelper/HelixClass.h"
 
 #include "TrackSystemSvc/IMarlinTrack.h"
+#include "Tracking/ITrackFitterTool.h"
 
 #include <UTIL/BitField64.h>
 #include <UTIL/ILDConf.h>
@@ -211,6 +212,7 @@ class SiliconTrackingAlg : public GaudiAlgorithm {
   /** pointer to the IMarlinTrkSystem instance 
    */
   MarlinTrk::IMarlinTrkSystem* _trksystem ;
+  ToolHandle<ITrackFitterTool> m_fitTool;
   //bool _runMarlinTrkDiagnostics;
   //std::string _MarlinTrkDiagnosticsName;
   typedef std::vector<int> IntVec;
@@ -238,7 +240,7 @@ class SiliconTrackingAlg : public GaudiAlgorithm {
   Gaudi::Property<float> _cutOnPt{this, "CutOnPt", 0.05};
   Gaudi::Property<int> _minimalHits{this, "MinimalHits",3};
   Gaudi::Property<int> _nHitsChi2{this, "NHitsChi2", 5};
-  Gaudi::Property<int> _max_hits_per_sector{this, "MaxHitsPerSector", 100};
+  Gaudi::Property<int> _max_hits_per_sector{this, "MaxHitsPerSector", 200};
   Gaudi::Property<int> _attachFast{this, "FastAttachment", 0};
   Gaudi::Property<bool> _useSIT{this, "UseSIT", true};
   Gaudi::Property<float> _initialTrackError_d0{this, "InitialTrackErrorD0",1e6};
@@ -253,8 +255,11 @@ class SiliconTrackingAlg : public GaudiAlgorithm {
   Gaudi::Property<bool> _ElossOn{this, "EnergyLossOn", true};
   Gaudi::Property<bool> _SmoothOn{this, "SmoothOn", true};
   Gaudi::Property<float> _helix_max_r{this, "HelixMaxR", 2000.};
-  Gaudi::Property<bool> m_dumpTime{this, "DumpTime", true}; 
+  Gaudi::Property<bool> m_dumpTime{this, "DumpTime", true};
+  Gaudi::Property<bool> m_debug{this, "Debug", false};
+
   //std::vector<int> _colours;  
+  Gaudi::Property<std::string> m_fitToolName{this, "FitterTool", "KalTestTool/KalTest111"};
   
   /** helper function to get collection using try catch block */
   //LCCollection* GetCollection(  LCEvent * evt, std::string colName ) ;
@@ -302,10 +307,15 @@ class SiliconTrackingAlg : public GaudiAlgorithm {
   std::vector<TrackerHitExtendedVec> _sectors;
   std::vector<TrackerHitExtendedVec> _sectorsFTD;
 
-  NTuple::Tuple*       m_tuple;
+  NTuple::Tuple*       m_tuple = nullptr;
   NTuple::Item<float>  m_timeTotal;
   NTuple::Item<float>  m_timeKalman;
 
+  NTuple::Tuple*       m_tupleDebug = nullptr;
+  NTuple::Item<int>    m_nhit;
+  NTuple::Array<int>   m_layer;
+  NTuple::Array<float> m_chi2;
+  NTuple::Array<bool>  m_used;
   /**
    * A helper class to allow good code readability by accessing tracks with N hits.
    * As the smalest valid track contains three hits, but the first index in a vector is 0,
diff --git a/Reconstruction/SiliconTracking/src/TrackSubsetAlg.cpp b/Reconstruction/SiliconTracking/src/TrackSubsetAlg.cpp
index d4741f2eb165767d51fdaf7afee0612dacf942b7..c8b6d1ea988b1a0daf139f2b793e4463824357b7 100644
--- a/Reconstruction/SiliconTracking/src/TrackSubsetAlg.cpp
+++ b/Reconstruction/SiliconTracking/src/TrackSubsetAlg.cpp
@@ -41,12 +41,16 @@ StatusCode TrackSubsetAlg::initialize() {
   _nRun = 0 ;
   _nEvt = 0 ;
 
+  // Set up the track fit tool
+  m_fitTool = ToolHandle<ITrackFitterTool>(m_fitToolName.value());
+
   if(m_dumpTime){
     NTuplePtr nt1(ntupleSvc(), "MyTuples/Time"+name());
     if ( !nt1 ) {
       m_tuple = ntupleSvc()->book("MyTuples/Time"+name(),CLID_ColumnWiseTuple,"Tracking time");
       if ( 0 != m_tuple ) {
 	m_tuple->addItem ("timeTotal", m_timeTotal ).ignore();
+	m_tuple->addItem ("timeKalman", m_timeKalman ).ignore();
       }
       else {
 	fatal() << "Cannot book MyTuples/Time"+name() <<endmsg;
@@ -220,7 +224,7 @@ StatusCode TrackSubsetAlg::execute(){
   debug() << "Fitting and saving of the tracks" << endmsg;
 
   //auto trkCol = _outColHdl.createAndPut();
-
+  auto stopwatch_kalman = TStopwatch();
   for( unsigned i=0; i < accepted.size(); i++ ){
     edm4hep::MutableTrack trackImpl;
     
@@ -266,14 +270,15 @@ StatusCode TrackSubsetAlg::execute(){
 
     bool fit_backwards = MarlinTrk::IMarlinTrack::backward;
     
-    MarlinTrk::IMarlinTrack* marlinTrk = _trkSystem->createTrack();
+    //MarlinTrk::IMarlinTrack* marlinTrk = _trkSystem->createTrack();
     
     int error = 0;
     
     try {
       
-      error = MarlinTrk::createFinalisedLCIOTrack(marlinTrk, trackerHits, &trackImpl, fit_backwards, covMatrix, _bField, _maxChi2PerHit);
-      
+      //error = MarlinTrk::createFinalisedLCIOTrack(marlinTrk, trackerHits, &trackImpl, fit_backwards, covMatrix, _bField, _maxChi2PerHit);
+      error = m_fitTool->Fit(trackImpl, trackerHits, covMatrix, _maxChi2PerHit, fit_backwards);
+
     } catch (...) {
       
       //      delete Track;
@@ -288,10 +293,11 @@ StatusCode TrackSubsetAlg::execute(){
     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);
+    //all_hits.reserve(300);
     
+    //marlinTrk->getHitsInFit(hits_in_fit);
+    hits_in_fit = m_fitTool->GetHitsInFit();
+
     for ( unsigned ihit = 0; ihit < hits_in_fit.size(); ++ihit) {
       all_hits.push_back(hits_in_fit[ihit].first);
     }
@@ -300,15 +306,17 @@ StatusCode TrackSubsetAlg::execute(){
     
     MarlinTrk::addHitNumbersToTrack(&trackImpl, all_hits, true, cellID_encoder);
     
-    marlinTrk->getOutliers(outliers);
-    
+    //marlinTrk->getOutliers(outliers);
+    outliers = m_fitTool->GetOutliers();
+
     for ( unsigned ihit = 0; ihit < outliers.size(); ++ihit) {
       all_hits.push_back(outliers[ihit].first);
     }
     
     MarlinTrk::addHitNumbersToTrack(&trackImpl, all_hits, false, cellID_encoder);
     
-    delete marlinTrk;
+    //delete marlinTrk;
+    m_fitTool->Clear();
         
     if( error != MarlinTrk::IMarlinTrack::success ) {
       //delete trackImpl;
@@ -355,6 +363,7 @@ StatusCode TrackSubsetAlg::execute(){
 
   if(m_dumpTime&&m_tuple){
     m_timeTotal = stopwatch.RealTime()*1000;
+    m_timeKalman = stopwatch_kalman.RealTime()*1000;
     m_tuple->write();
   }
 
diff --git a/Reconstruction/SiliconTracking/src/TrackSubsetAlg.h b/Reconstruction/SiliconTracking/src/TrackSubsetAlg.h
index 1023eba976e861461174d93a7158294328ef4854..534524f9b66fcac2c60858e7f3d29d033740f877 100644
--- a/Reconstruction/SiliconTracking/src/TrackSubsetAlg.h
+++ b/Reconstruction/SiliconTracking/src/TrackSubsetAlg.h
@@ -8,6 +8,7 @@
 #include "edm4hep/TrackerHitCollection.h"
 //#include "edm4hep/Track.h"
 #include "TrackSystemSvc/IMarlinTrkSystem.h"
+#include "Tracking/ITrackFitterTool.h"
 
 #include "Math/ProbFunc.h"
 
@@ -56,6 +57,7 @@ class TrackSubsetAlg : public GaudiAlgorithm {
   
  protected:
   MarlinTrk::IMarlinTrkSystem* _trkSystem;
+  ToolHandle<ITrackFitterTool> m_fitTool;
   /* Input collection */
   std::vector<DataHandle<edm4hep::TrackCollection>* > _inTrackColHdls;
   std::vector<DataHandle<edm4hep::TrackerHitCollection>* > _inTrackerHitColHdls;
@@ -76,6 +78,7 @@ class TrackSubsetAlg : public GaudiAlgorithm {
   Gaudi::Property<double> _maxChi2PerHit{this, "MaxChi2PerHit", 1e2};
   Gaudi::Property<double> _omega{this, "Omega", 0.75};
   Gaudi::Property<bool> m_dumpTime{this, "DumpTime", true};
+  Gaudi::Property<std::string> m_fitToolName{this, "FitterTool", "KalTestTool/KalTest111"};
   
   float _bField;
   
@@ -84,6 +87,7 @@ class TrackSubsetAlg : public GaudiAlgorithm {
 
   NTuple::Tuple*       m_tuple;
   NTuple::Item<float>  m_timeTotal;
+  NTuple::Item<float>  m_timeKalman;
 };
 
 /** A functor to return whether two tracks are compatible: The criterion is if the share a TrackerHit or more */
diff --git a/Reconstruction/Tracking/CMakeLists.txt b/Reconstruction/Tracking/CMakeLists.txt
index f3e77b4721987efb6a6cd5d7888f7455927bf61d..9ea9ecd6794902b915681f461703868feabd6909 100644
--- a/Reconstruction/Tracking/CMakeLists.txt
+++ b/Reconstruction/Tracking/CMakeLists.txt
@@ -1,9 +1,12 @@
+gaudi_add_header_only_library(TrackingLib)
+
 # Modules
 gaudi_add_module(Tracking
                  SOURCES src/Clupatra/ClupatraAlg.cpp
                          src/Clupatra/clupatra_new.cpp
                          src/FullLDCTracking/FullLDCTrackingAlg.cpp
                          src/TruthTracker/TruthTrackerAlg.cpp
+                         src/FitterTool/KalTestTool.cpp
                  LINK GearSvc
                       EventSeeder
                       TrackSystemSvcLib
diff --git a/Reconstruction/Tracking/include/Tracking/ITrackFitterTool.h b/Reconstruction/Tracking/include/Tracking/ITrackFitterTool.h
new file mode 100644
index 0000000000000000000000000000000000000000..4de2b46e80e807241e2898788fa03f1a64c085d2
--- /dev/null
+++ b/Reconstruction/Tracking/include/Tracking/ITrackFitterTool.h
@@ -0,0 +1,40 @@
+#ifndef ITrackFitterTool_h
+#define ITrackFitterTool_h
+
+/*
+ * Description:
+ *   ITrackFitterTool is used to fit a track candidate to obtain track parameter
+ *
+ * The interface:
+ *   * Fit: peform on tracker hits according prepared geometry
+ *
+ * Author: FU Chengdong <fucd@ihep.ac.cn>
+ */
+
+#include "GaudiKernel/IAlgTool.h"
+#include "edm4hep/TrackState.h"
+#include <vector>
+
+
+
+namespace edm4hep{
+  class MutableTrack;
+  class TrackerHit;
+}
+
+class ITrackFitterTool: virtual public IAlgTool {
+ public:
+
+  DeclareInterfaceID(ITrackFitterTool, 0, 1);
+  virtual ~ITrackFitterTool() {}
+
+  virtual int Fit(edm4hep::MutableTrack track, std::vector<edm4hep::TrackerHit>& trackHits,
+                  const decltype(edm4hep::TrackState::covMatrix)& covMatrix, double maxChi2perHit, bool backward) = 0;
+  virtual int Fit(edm4hep::MutableTrack track, std::vector<edm4hep::TrackerHit>& trackHits,
+                  edm4hep::TrackState trackState, double maxChi2perHit, bool backward) = 0;
+  virtual std::vector<std::pair<edm4hep::TrackerHit, double> >& GetHitsInFit() = 0;
+  virtual std::vector<std::pair<edm4hep::TrackerHit, double> >& GetOutliers() = 0;
+  virtual void Clear() = 0;
+};
+
+#endif
diff --git a/Reconstruction/Tracking/src/Clupatra/ClupatraAlg.cpp b/Reconstruction/Tracking/src/Clupatra/ClupatraAlg.cpp
index d218e041b47024844daf858f9abf23d77693143f..937adba6ab52508fd907b5ece1c64ca6a0b703c6 100644
--- a/Reconstruction/Tracking/src/Clupatra/ClupatraAlg.cpp
+++ b/Reconstruction/Tracking/src/Clupatra/ClupatraAlg.cpp
@@ -201,6 +201,9 @@ StatusCode ClupatraAlg::initialize() {
         // don't need, since Gaudi Algorithm will print all Property  
 	//printParameters() ;
 
+  // Set up the track fit tool
+  m_fitTool = ToolHandle<ITrackFitterTool>(m_fitToolName.value());
+
 	auto _trackSystemSvc = service<ITrackSystemSvc>("TrackSystemSvc");
 	if ( !_trackSystemSvc ) {
 		error() << "Fail to find TrackSystemSvc ..." << endmsg;
diff --git a/Reconstruction/Tracking/src/Clupatra/ClupatraAlg.h b/Reconstruction/Tracking/src/Clupatra/ClupatraAlg.h
index 962b1c544e1f3b67737e443d16f4d579695029b4..fb451483d2e383506ab345f756f04c68c86184cd 100644
--- a/Reconstruction/Tracking/src/Clupatra/ClupatraAlg.h
+++ b/Reconstruction/Tracking/src/Clupatra/ClupatraAlg.h
@@ -22,6 +22,7 @@
 #include "TrackSystemSvc/HelixTrack.h"
 #include "TrackSystemSvc/HelixFit.h"
 #include "TrackSystemSvc/IMarlinTrack.h"
+#include "Tracking/ITrackFitterTool.h"
 
 namespace MarlinTrk{
   class IMarlinTrkSystem ;
@@ -139,6 +140,7 @@ class ClupatraAlg : public GaudiAlgorithm {
   Gaudi::Property<bool> _MSOn{this, "MultipleScatteringOn", false};
   Gaudi::Property<bool> _ElossOn{this, "EnergyLossOn", true};
   Gaudi::Property<bool> _SmoothOn{this, "SmoothOn", false};
+  Gaudi::Property<std::string> m_fitToolName{this, "FitterTool", "KalTestTool/KalTest010"};
 
 
   DataHandle<edm4hep::TrackerHitCollection> _TPCHitCollectionHandle{"TPCTrackerHits", Gaudi::DataHandle::Reader, this};
@@ -154,6 +156,7 @@ class ClupatraAlg : public GaudiAlgorithm {
   int _nEvt ;
 
   MarlinTrk::IMarlinTrkSystem* _trksystem ;
+  ToolHandle<ITrackFitterTool> m_fitTool;
 
   const gear::TPCParameters*  _gearTPC ;
 
diff --git a/Reconstruction/Tracking/src/FitterTool/KalTestTool.cpp b/Reconstruction/Tracking/src/FitterTool/KalTestTool.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..773f1998a17af850fd5f06d42b1ef3d5d0d5a79a
--- /dev/null
+++ b/Reconstruction/Tracking/src/FitterTool/KalTestTool.cpp
@@ -0,0 +1,103 @@
+#include "KalTestTool.h"
+
+#include "TrackSystemSvc/ITrackSystemSvc.h"
+#include "TrackSystemSvc/MarlinTrkUtils.h"
+#include "TrackSystemSvc/IMarlinTrack.h"
+#include "DetInterface/IGeomSvc.h"
+
+#include "DD4hep/Detector.h"
+#include "DD4hep/DD4hepUnits.h"
+
+#include "edm4hep/TrackerHit.h"
+#include "edm4hep/TrackState.h"
+#include "edm4hep/MutableTrack.h"
+
+DECLARE_COMPONENT(KalTestTool)
+
+StatusCode KalTestTool::initialize() {
+  StatusCode sc;
+  always() << m_fitterName << endmsg;
+  if (m_fitterName=="KalTest") {
+    auto _trackSystemSvc = service<ITrackSystemSvc>("TrackSystemSvc");
+    if (!_trackSystemSvc) {
+      error() << "Failed to find TrackSystemSvc ..." << endmsg;
+      return StatusCode::FAILURE;
+    }
+    m_factoryMarlinTrk = _trackSystemSvc->getTrackSystem(this);
+    m_factoryMarlinTrk->setOption(MarlinTrk::IMarlinTrkSystem::CFG::useQMS, m_useQMS);
+    m_factoryMarlinTrk->setOption(MarlinTrk::IMarlinTrkSystem::CFG::usedEdx, m_usedEdx);
+    m_factoryMarlinTrk->setOption(MarlinTrk::IMarlinTrkSystem::CFG::useSmoothing, m_useSmoothing);
+    m_factoryMarlinTrk->init();
+  }
+  else {
+    error() << "fitter " << m_fitterName << " has not been imported" << endmsg;
+    return StatusCode::FAILURE;
+  }
+
+  auto _geomSvc = service<IGeomSvc>("GeomSvc");
+  if ( !_geomSvc ) {
+    error() << "Failed to find GeomSvc ..." << endmsg;
+    return StatusCode::FAILURE;
+  }
+
+  if (m_magneticField.value()==0) {
+    const dd4hep::Direction& field = _geomSvc->lcdd()->field().magneticField(dd4hep::Position(0,0,0));
+    double Bz = field.z()/dd4hep::tesla;
+    if (Bz==0) {
+      error() << "magnetic field = 0, KalmanFilter cannot run" << endmsg;
+      return StatusCode::FAILURE;
+    }
+    m_magneticField = Bz;
+  }
+
+  return sc;
+}
+
+StatusCode KalTestTool::finalize() {
+  StatusCode sc;
+  return sc;
+}
+
+int KalTestTool::Fit(edm4hep::MutableTrack track, std::vector<edm4hep::TrackerHit>& trackHits,
+			  const decltype(edm4hep::TrackState::covMatrix)& covMatrix, double maxChi2perHit, bool backward) {
+  if (m_hitsInFit.size()!=0 || m_outliers.size()!=0) {
+    error() << "Important! vector not clear, still store the data of last event!" << endmsg;
+    return 0;
+  }
+
+  if (m_fitterName=="KalTest") {
+    debug() << "start..." << endmsg;
+    std::shared_ptr<MarlinTrk::IMarlinTrack> marlinTrack(m_factoryMarlinTrk->createTrack());
+    debug() << "created MarlinKalTestTrack" << endmsg;
+    int status = MarlinTrk::createFinalisedLCIOTrack(marlinTrack.get(), trackHits, &track, backward, covMatrix, m_magneticField, maxChi2perHit);
+
+    marlinTrack->getHitsInFit(m_hitsInFit);
+    marlinTrack->getOutliers(m_outliers);
+    return status;
+  }
+  
+  error() << "Don't support the Fitter " << m_fitterName << endmsg;
+  return 0;
+}
+
+int KalTestTool::Fit(edm4hep::MutableTrack track, std::vector<edm4hep::TrackerHit>& trackHits,
+                          edm4hep::TrackState trackState, double maxChi2perHit, bool backward) {
+  if (m_hitsInFit.size()!=0 || m_outliers.size()!=0) {
+    error() << "Important! vector not clear, still store the data of last event!" << endmsg;
+    return 0;
+  }
+
+  if (m_fitterName=="KalTest") {
+    debug() << "start..." << endmsg;
+    std::shared_ptr<MarlinTrk::IMarlinTrack> marlinTrack(m_factoryMarlinTrk->createTrack());
+    debug() << "created MarlinKalTestTrack" << endmsg;
+    int status = MarlinTrk::createFinalisedLCIOTrack(marlinTrack.get(), trackHits, &track, backward, &trackState, m_magneticField, maxChi2perHit);
+
+    marlinTrack->getHitsInFit(m_hitsInFit);
+    marlinTrack->getOutliers(m_outliers);
+    return status;
+  }
+
+  error() << "Don't support the Fitter " << m_fitterName << endmsg;
+  return 0;
+}
diff --git a/Reconstruction/Tracking/src/FitterTool/KalTestTool.h b/Reconstruction/Tracking/src/FitterTool/KalTestTool.h
new file mode 100644
index 0000000000000000000000000000000000000000..0cce58bc46c56d8bc15c0a4670205e58c27aaf38
--- /dev/null
+++ b/Reconstruction/Tracking/src/FitterTool/KalTestTool.h
@@ -0,0 +1,43 @@
+#ifndef KalTestTool_h
+#define KalTestTool_h
+
+#include "GaudiKernel/AlgTool.h"
+#include "Tracking/ITrackFitterTool.h"
+#include <vector>
+
+namespace MarlinTrk {
+  class IMarlinTrkSystem ;
+}
+
+class KalTestTool : public extends<AlgTool, ITrackFitterTool> {
+ public:
+  using extends::extends;
+  //KalTestTool(void* p) { m_pAlgUsing=p; };
+  
+  virtual int Fit(edm4hep::MutableTrack track, std::vector<edm4hep::TrackerHit>& trackHits,
+		  const decltype(edm4hep::TrackState::covMatrix)& covMatrix, double maxChi2perHit, bool backward = true) override;
+  virtual int Fit(edm4hep::MutableTrack track, std::vector<edm4hep::TrackerHit>& trackHits,
+                  edm4hep::TrackState trackState, double maxChi2perHit, bool backward = true) override;
+
+  StatusCode initialize() override;
+  StatusCode finalize() override;
+
+  std::vector<std::pair<edm4hep::TrackerHit, double> >& GetHitsInFit() override {return m_hitsInFit;};
+  std::vector<std::pair<edm4hep::TrackerHit, double> >& GetOutliers() override {return m_outliers;};
+  void Clear() override {m_hitsInFit.clear(); m_outliers.clear();};
+
+ private:
+  Gaudi::Property<std::string>    m_fitterName{this, "Fitter", "KalTest"};
+  Gaudi::Property<bool>           m_useQMS{this, "MSOn", true};
+  Gaudi::Property<bool>           m_usedEdx{this, "EnergyLossOn", true};
+  Gaudi::Property<bool>           m_useSmoothing{this, "Smooth", true};
+  Gaudi::Property<double>         m_magneticField{this, "MagneticField", 0};
+  
+  MarlinTrk::IMarlinTrkSystem* m_factoryMarlinTrk = nullptr;
+
+  void* m_pAlgUsing = nullptr;
+  std::vector<std::pair<edm4hep::TrackerHit, double> > m_hitsInFit ;
+  std::vector<std::pair<edm4hep::TrackerHit, double> > m_outliers ;
+};
+
+#endif
diff --git a/Reconstruction/Tracking/src/FullLDCTracking/FullLDCTrackingAlg.cpp b/Reconstruction/Tracking/src/FullLDCTracking/FullLDCTrackingAlg.cpp
old mode 100644
new mode 100755
index ee5f1feb25c5cda5c95f0f4d9dab9801e943db04..95265a4ccef887ba450fe439b2c37fab1533f6e3
--- a/Reconstruction/Tracking/src/FullLDCTracking/FullLDCTrackingAlg.cpp
+++ b/Reconstruction/Tracking/src/FullLDCTracking/FullLDCTrackingAlg.cpp
@@ -6,7 +6,7 @@
 #include <GearSvc/IGearSvc.h>
 
 #include <edm4hep/TrackerHit.h>
-#include <edm4hep/TrackerHit.h>
+#include <edm4hep/TrackerHitPlane.h>
 #include <edm4hep/Track.h>
 #if __has_include("edm4hep/EDM4hepVersion.h")
 #include "edm4hep/EDM4hepVersion.h"
@@ -94,7 +94,11 @@ std::string toString( int iTrk, edm4hep::Track tpcTrack, float bField=3.5 ) {
   float pzTPC = helixTPC.getMomentum()[2];
   const float ptot = sqrt(pxTPC*pxTPC+pyTPC*pyTPC+pzTPC*pzTPC);
 
+#if EDM4HEP_BUILD_VERSION > EDM4HEP_VERSION(0, 9, 0)
+  sprintf(strg,"%3i   %5i %9.3f  %9.3f  %9.3f  %7.2f  %7.2f  %7.2f %4i %4i %8.3f %8i",iTrk,tpcTrack.id().index,
+#else
   sprintf(strg,"%3i   %5i %9.3f  %9.3f  %9.3f  %7.2f  %7.2f  %7.2f %4i %4i %8.3f %8i",iTrk,tpcTrack.id(),
+#endif
 	  ptot, d0TPC,z0TPC,pxTPC,pyTPC,pzTPC,nHits,ndfTPC,Chi2TPC,nlinkedTracks);
 
   return std::string( strg ) ;
@@ -154,6 +158,9 @@ StatusCode FullLDCTrackingAlg::initialize() {
   PIOVER2 = 0.5*PI;
   TWOPI = 2*PI;
 
+  // Set up the track fit tool
+  m_fitTool = ToolHandle<ITrackFitterTool>(m_fitToolName.value());
+
   if(m_dumpTime){
     NTuplePtr nt1(ntupleSvc(), "MyTuples/Time"+name());
     if ( !nt1 ) {
@@ -414,13 +421,14 @@ void FullLDCTrackingAlg::AddTrackColToEvt(TrackExtendedVec & trkVec, edm4hep::Tr
     debug() << "trkHits ready" << endmsg;
     bool fit_backwards = IMarlinTrack::backward;
     
-    MarlinTrk::IMarlinTrack* marlinTrk = _trksystem->createTrack();
-    debug() << "marlinTrk created " << endmsg;
+    //MarlinTrk::IMarlinTrack* marlinTrk = _trksystem->createTrack();
+    //debug() << "marlinTrk created " << endmsg;
     
     int error_code = 0;
     
     try {
-      error_code = MarlinTrk::createFinalisedLCIOTrack(marlinTrk, trkHits, &track, fit_backwards, &ts_initial, _bField, _maxChi2PerHit);
+      //error_code = MarlinTrk::createFinalisedLCIOTrack(marlinTrk, trkHits, &track, fit_backwards, &ts_initial, _bField, _maxChi2PerHit);
+      error_code = m_fitTool->Fit(track, trkHits, ts_initial, _maxChi2PerHit, fit_backwards);
     } catch (...) {
       
       //      delete track;
@@ -445,8 +453,9 @@ void FullLDCTrackingAlg::AddTrackColToEvt(TrackExtendedVec & trkVec, edm4hep::Tr
     std::vector<edm4hep::TrackerHit> all_hits;
     all_hits.reserve(hits_in_fit.size());
     
-    marlinTrk->getHitsInFit(hits_in_fit);
-    
+    //marlinTrk->getHitsInFit(hits_in_fit);
+    hits_in_fit = m_fitTool->GetHitsInFit();
+
     for ( unsigned ihit = 0; ihit < hits_in_fit.size(); ++ihit) {
       all_hits.push_back(hits_in_fit[ihit].first);
     }
@@ -455,17 +464,18 @@ void FullLDCTrackingAlg::AddTrackColToEvt(TrackExtendedVec & trkVec, edm4hep::Tr
     
     MarlinTrk::addHitNumbersToTrack(&track, all_hits, true, cellID_encoder);
     
-    marlinTrk->getOutliers(outliers);
-    
+    //marlinTrk->getOutliers(outliers);
+    outliers = m_fitTool->GetOutliers();
+
     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;
-    
+    //delete marlinTrk;
+    m_fitTool->Clear();
+
     if( error_code != IMarlinTrack::success ) {
       debug() << "FullLDCTrackingAlg::AddTrackColToEvt: Track fit failed with error code " << error_code << " track dropped. Number of hits = "<< trkHits.size() << endmsg;
       //delete Track;
@@ -534,6 +544,7 @@ void FullLDCTrackingAlg::AddTrackColToEvt(TrackExtendedVec & trkVec, edm4hep::Tr
     int nhits_in_tpc = track.getSubDetectorHitNumbers(3);
     int nhits_in_set = track.getSubDetectorHitNumbers(4);
 #endif
+
     //int nhits_in_vxd = Track->subdetectorHitNumbers()[ 2 * lcio::ILDDetID::VXD - 2 ];
     //int nhits_in_ftd = Track->subdetectorHitNumbers()[ 2 * lcio::ILDDetID::FTD - 2 ];
     //int nhits_in_sit = Track->subdetectorHitNumbers()[ 2 * lcio::ILDDetID::SIT - 2 ];
@@ -1264,7 +1275,11 @@ void FullLDCTrackingAlg::prepareVectors() {
       const float pTot = sqrt(pxSi*pxSi+pySi*pySi+pzSi*pzSi);
       const int ndfSi = trackExt->getNDF();
       float Chi2Si = trackExt->getChi2()/float(trackExt->getNDF());
+#if EDM4HEP_BUILD_VERSION > EDM4HEP_VERSION(0, 9, 0)
+      sprintf(strg,"%3i   %5i %9.3f  %9.3f  %9.3f  %7.2f  %7.2f  %7.2f %4i %4i %8.3f",iTrk, siTrack.id().index, pTot, d0Si,z0Si,pxSi,pySi,pzSi,nHits, ndfSi, Chi2Si);
+#else
       sprintf(strg,"%3i   %5i %9.3f  %9.3f  %9.3f  %7.2f  %7.2f  %7.2f %4i %4i %8.3f",iTrk, siTrack.id(), pTot, d0Si,z0Si,pxSi,pySi,pzSi,nHits, ndfSi, Chi2Si);
+#endif
       debug() << strg << endmsg;
       
       if(nHits>0){
diff --git a/Reconstruction/Tracking/src/FullLDCTracking/FullLDCTrackingAlg.h b/Reconstruction/Tracking/src/FullLDCTracking/FullLDCTrackingAlg.h
index cfc804879d44a7e453d069e79abe261640494120..2e9b9586bab5237f87738b97e28365e1ddaab305 100755
--- a/Reconstruction/Tracking/src/FullLDCTracking/FullLDCTrackingAlg.h
+++ b/Reconstruction/Tracking/src/FullLDCTracking/FullLDCTrackingAlg.h
@@ -19,6 +19,7 @@
 
 #include "TrackSystemSvc/MarlinTrkUtils.h"
 #include "TrackSystemSvc/IMarlinTrack.h"
+#include "Tracking/ITrackFitterTool.h"
 
 #include "edm4hep/SimTrackerHitCollection.h"
 #include "edm4hep/TrackerHitCollection.h"
@@ -319,6 +320,7 @@ protected:
   /** pointer to the IMarlinTrkSystem instance 
    */
   MarlinTrk::IMarlinTrkSystem* _trksystem ;
+  ToolHandle<ITrackFitterTool> m_fitTool;
   
   Gaudi::Property<bool> _MSOn{this, "MultipleScatteringOn", true};
   Gaudi::Property<bool> _ElossOn{this, "EnergyLossOn", true};
@@ -397,6 +399,7 @@ protected:
   Gaudi::Property<int>   _maxAllowedSiHitRejectionsForTrackCombination{this, "MaxAllowedSiHitRejectionsForTrackCombination", 2};
   Gaudi::Property<bool>  m_dumpTime{this, "DumpTime", true};
   //float _dPCutForForcedMerging;
+  Gaudi::Property<std::string> m_fitToolName{this, "FitterTool", "KalTestTool/KalTest111"};
   
   bool _runMarlinTrkDiagnostics = false;
   std::string _MarlinTrkDiagnosticsName;
diff --git a/Service/GearSvc/src/GearSvc.cpp b/Service/GearSvc/src/GearSvc.cpp
index 695e455499efff51aeca796bb0ee45bd1d8b7020..cf0d6ec9b44a4014462ec2ecf9f74499d3bdb90d 100644
--- a/Service/GearSvc/src/GearSvc.cpp
+++ b/Service/GearSvc/src/GearSvc.cpp
@@ -73,9 +73,15 @@ StatusCode GearSvc::initialize()
     info() << "Fill GEAR data from GeomSvc" << endmsg;
     m_gearMgr->setDetectorName("CRD_o1_v01");
 
-    const dd4hep::Direction& field = geomSvc->lcdd()->field().magneticField(dd4hep::Position(0,0,0));
-    gear::ConstantBField* b = new gear::ConstantBField(gear::Vector3D(field.x()/dd4hep::tesla, field.y()/dd4hep::tesla, field.z()/dd4hep::tesla));
-    m_gearMgr->setBField(b);
+    if (m_field.value()==0) {
+      const dd4hep::Direction& field = geomSvc->lcdd()->field().magneticField(dd4hep::Position(0,0,0));
+      gear::ConstantBField* b = new gear::ConstantBField(gear::Vector3D(field.x()/dd4hep::tesla, field.y()/dd4hep::tesla, field.z()/dd4hep::tesla));
+      m_gearMgr->setBField(b);
+    }
+    else {
+      gear::ConstantBField* b = new gear::ConstantBField(gear::Vector3D(0, 0, m_field.value()));
+      m_gearMgr->setBField(b);
+    }
 
     dd4hep::DetElement world = geomSvc->getDD4HepGeo();
     const std::map<std::string, dd4hep::DetElement>& subs = world.children();
@@ -211,8 +217,11 @@ StatusCode GearSvc::convertVXD(dd4hep::DetElement& vxd){
 
     dd4hep::rec::MaterialManager matMgr( dd4hep::Detector::getInstance().world().volume() ) ;
     const dd4hep::rec::ZPlanarData::LayerLayout& l = vxdData->layers[0] ;
-    dd4hep::rec::Vector3D a( l.distanceSensitive + l.thicknessSensitive, l.phi0 , 0. ,  dd4hep::rec::Vector3D::cylindrical ) ;
-    dd4hep::rec::Vector3D b( l.distanceSupport   + l.thicknessSupport,   l.phi0 , 0. ,  dd4hep::rec::Vector3D::cylindrical ) ;
+    double offset = l.offsetSupport;
+    //dd4hep::rec::Vector3D a( l.distanceSensitive + l.thicknessSensitive, l.phi0 , 0. ,  dd4hep::rec::Vector3D::cylindrical ) ;
+    //dd4hep::rec::Vector3D b( l.distanceSupport   + l.thicknessSupport,   l.phi0 , 0. ,  dd4hep::rec::Vector3D::cylindrical ) ;
+    dd4hep::rec::Vector3D a( l.distanceSensitive + l.thicknessSensitive, l.offsetSupport, 2.*dd4hep::mm);
+    dd4hep::rec::Vector3D b( l.distanceSupport   + l.thicknessSupport,   l.offsetSupport, 2.*dd4hep::mm);
     const dd4hep::rec::MaterialVec& materials = matMgr.materialsBetween( a , b  ) ;
     dd4hep::rec::MaterialData mat = ( materials.size() > 1  ? matMgr.createAveragedMaterial( materials ) : materials[0].first  ) ;
 
@@ -228,6 +237,25 @@ StatusCode GearSvc::convertVXD(dd4hep::DetElement& vxd){
 										mat.interactionLength()/dd4hep::mm);
     m_gearMgr->registerSimpleMaterial(VXDSupportMaterial);
 
+    if (vxdData->rOuterShell>vxdData->rInnerShell) {
+      dd4hep::rec::Vector3D a1( vxdData->rInnerShell, 0, 2.*dd4hep::mm);
+      dd4hep::rec::Vector3D b1( vxdData->rOuterShell, 0, 2.*dd4hep::mm);
+      const dd4hep::rec::MaterialVec& materials1 = matMgr.materialsBetween( a1 , b1  ) ;
+      dd4hep::rec::MaterialData mat1 = ( materials1.size() > 1  ? matMgr.createAveragedMaterial( materials1 ) : materials1[0].first  ) ;
+
+      std::cout << " ####### found materials between points : " << a1 << " and " << b1 << " : " ;
+      for( unsigned i=0,n=materials1.size();i<n;++i){
+	std::cout <<  materials1[i].first.name() << "[" <<   materials1[i].second << "], " ;
+      }
+      std::cout << std::endl ;
+      std::cout << "   averaged material : " << mat1 << std::endl ;
+      gear::SimpleMaterialImpl* VXDShellMaterial = new gear::SimpleMaterialImpl("VXDShellMaterial", mat1.A(), mat1.Z(),
+                                                                                mat1.density()/(dd4hep::kg/(dd4hep::g*dd4hep::m3)),
+                                                                                mat1.radiationLength()/dd4hep::mm,
+                                                                                mat1.interactionLength()/dd4hep::mm);
+      m_gearMgr->registerSimpleMaterial(VXDShellMaterial);
+    }
+
     info() << vxdData->rInnerShell << " " << vxdData->rOuterShell << " " << vxdData->zHalfShell << " " << vxdData->gapShell << endmsg;
     for(int i=0,n=vxdData->layers.size(); i<n; i++){
       const dd4hep::rec::ZPlanarData::LayerLayout& thisLayer = vxdData->layers[i];
@@ -702,7 +730,7 @@ StatusCode GearSvc::convertFTD(dd4hep::DetElement& ftd){
                        senRinner, senThickness, senLengthMin, senLengthMax, senWidth, 0);
   }
   m_gearMgr->setFTDParameters(ftdParam);
-
+  info() << "nftd = " << nLayers << endmsg;
   return StatusCode::SUCCESS;
 }
 
diff --git a/Service/GearSvc/src/GearSvc.h b/Service/GearSvc/src/GearSvc.h
index 3048526cb9d0c477519599941006cefb9ee6f3fc..312491eb4c6f7e3950e50830f1faa87da08be119 100644
--- a/Service/GearSvc/src/GearSvc.h
+++ b/Service/GearSvc/src/GearSvc.h
@@ -10,7 +10,7 @@ class GearSvc : public extends<Service, IGearSvc>
 {
     public:
         GearSvc(const std::string& name, ISvcLocator* svc);
-        ~GearSvc();
+        virtual ~GearSvc();
 
         gear::GearMgr* getGearMgr() override;
 
@@ -28,6 +28,7 @@ class GearSvc : public extends<Service, IGearSvc>
 	TGeoNode* FindNode(TGeoNode* mother, char* name);
 
         Gaudi::Property<std::string> m_gearFile{this, "GearXMLFile", ""};
+        Gaudi::Property<float>       m_field{this, "MagneticField", 0};
 
         gear::GearMgr* m_gearMgr;
 };
diff --git a/Service/TrackSystemSvc/src/MarlinTrkUtils.cc b/Service/TrackSystemSvc/src/MarlinTrkUtils.cc
index fcacb19cf8fead3e3204d70af7517f286372faef..bf2ab05d9f1987c37e7391621b36c427f42a6e1c 100644
--- a/Service/TrackSystemSvc/src/MarlinTrkUtils.cc
+++ b/Service/TrackSystemSvc/src/MarlinTrkUtils.cc
@@ -608,7 +608,16 @@ namespace MarlinTrk {
         trkStateCalo.location = MarlinTrk::Location::AtCalorimeter;
         track->addToTrackStates(trkStateCalo);
       } else {
+#ifdef DEBUG
 	std::cout << "  >>>>>>>>>>> MarlinTrk::finaliseLCIOTrack:  could not get TrackState at Calo Face "  << std::endl ;
+	if (last_constrained_hit.isAvailable()) {
+	  auto pos = last_constrained_hit.getPosition();
+	  std::cout << " last_constrained_hit = " << pos.x << "," << pos.y << "," << pos.z << std::endl;
+	}
+	else {
+	  std::cout << " last_constrained_hit not Available" << std::endl;
+	}
+#endif
         //delete trkStateCalo;
       }
     } else {
diff --git a/Simulation/DetSimSD/src/GenericTrackerSensitiveDetector.cpp b/Simulation/DetSimSD/src/GenericTrackerSensitiveDetector.cpp
index 5e9ba1a6af4cf2dbe994a1bf7d686f3bfcbf19f7..c1b8aca9a71ea25c560cf75966ed67ee5bb0e92d 100644
--- a/Simulation/DetSimSD/src/GenericTrackerSensitiveDetector.cpp
+++ b/Simulation/DetSimSD/src/GenericTrackerSensitiveDetector.cpp
@@ -33,10 +33,12 @@ G4bool GenericTrackerSensitiveDetector::ProcessHits(G4Step* step, G4TouchableHis
   dd4hep::Position direction = postPos - prePos;
   dd4hep::Position position  = mean_direction(prePos,postPos);
   double   hit_len   = direction.R();
+  if (hit_len < 1E-9) return true;
   if (hit_len > 0) {
     double new_len = mean_length(h.preMom(),h.postMom())/hit_len;
     direction *= new_len/hit_len;
   }
+
   dd4hep::sim::Geant4TrackerHit* hit = nullptr;
   hit = new dd4hep::sim::Geant4TrackerHit(h.track->GetTrackID(),
 					  h.track->GetDefinition()->GetPDGEncoding(),
diff --git a/Utilities/KalDet/src/ild/common/MaterialDataBase.cc b/Utilities/KalDet/src/ild/common/MaterialDataBase.cc
index f181b8b53de84a990f3b4351d455241e0a344bf8..277f17aead1dc94636e271a6f88c55343a642830 100644
--- a/Utilities/KalDet/src/ild/common/MaterialDataBase.cc
+++ b/Utilities/KalDet/src/ild/common/MaterialDataBase.cc
@@ -202,7 +202,6 @@ void MaterialDataBase::createMaterials(const gear::GearMgr& gearMgr, IGeomSvc* g
   TMaterial &ftdsupport = *new TMaterial(name.c_str(), "", A, Z, density, radlen, 0.) ;
   this->addMaterial(&ftdsupport, name);
 
-  
   // VXD Support Material
   if(geoSvc){
     //obselete
@@ -222,9 +221,18 @@ void MaterialDataBase::createMaterials(const gear::GearMgr& gearMgr, IGeomSvc* g
       name    = vxd_sup_mat.getName() ;
       TMaterial &vxdsupport = *new TMaterial(name.c_str(), "", A, Z, density, radlen, 0.);
       this->addMaterial(&vxdsupport, name);
+
+      const gear::SimpleMaterial& vxd_shell_mat = gearMgr.getSimpleMaterial("VXDShellMaterial");
+      A       = vxd_shell_mat.getA();
+      Z       = vxd_shell_mat.getZ();
+      density = vxd_shell_mat.getDensity() * (1000.0/ 1000000.0); // kg/m^3 -> g/cm^3
+      radlen  = vxd_shell_mat.getRadLength() / 10.0 ; // mm -> cm
+      name    = vxd_shell_mat.getName() ;
+      TMaterial &vxdshell = *new TMaterial(name.c_str(), "", A, Z, density, radlen, 0.);
+      this->addMaterial(&vxdshell, name);
     }
     catch( gear::UnknownParameterException& e){   
-      std::cout << "Error while read material from GeomSvc!" << std::endl;
+      std::cout << "Warning! cannot get material VXDSupportMaterial and VXDShellMaterial from GeomSvc! GearMgr=" << &gearMgr << std::endl;
     }
   }
 }
diff --git a/Utilities/KalDet/src/ild/vxd/ILDVXDKalDetector.cc b/Utilities/KalDet/src/ild/vxd/ILDVXDKalDetector.cc
index 3043a7f392acd79700a441c826b3cbd3f5f32522..3cf666966b6115eda29a39cb3a1c9f59d8843c48 100644
--- a/Utilities/KalDet/src/ild/vxd/ILDVXDKalDetector.cc
+++ b/Utilities/KalDet/src/ild/vxd/ILDVXDKalDetector.cc
@@ -45,7 +45,6 @@ ILDVXDKalDetector::ILDVXDKalDetector( const gear::GearMgr& gearMgr, IGeomSvc* ge
   TMaterial & beryllium = *MaterialDataBase::Instance().getMaterial("beryllium");
 
   // needed for cryostat
-  
   TMaterial & aluminium = *MaterialDataBase::Instance().getMaterial("aluminium");
   
   _vxd_Cryostat.exists = false;
@@ -215,6 +214,12 @@ ILDVXDKalDetector::ILDVXDKalDetector( const gear::GearMgr& gearMgr, IGeomSvc* ge
     }
   }
   
+  if (_vxd_Cryostat.shellInnerR>0&&_vxd_Cryostat.shellThickness>0) {
+    TMaterial & shell     = *MaterialDataBase::Instance().getMaterial("VXDShellMaterial");
+    Add( new ILDCylinderMeasLayer(air, shell , _vxd_Cryostat.shellInnerR, _vxd_Cryostat.shelllHalfZ, 0, 0, 0, _bZ, dummy,-1,"VXDShellInnerWall" ) );
+    Add( new ILDCylinderMeasLayer(shell, air , _vxd_Cryostat.shellInnerR+_vxd_Cryostat.shellThickness, _vxd_Cryostat.shelllHalfZ, 0, 0, 0, _bZ, dummy,-1,"VXDShellOuterWall" ) );
+  }
+
   if (_vxd_Cryostat.exists) {
     // build Cryostat according to mokka driver vxd04.cc
     
@@ -357,6 +362,11 @@ void ILDVXDKalDetector::setupGearGeom( const gear::GearMgr& gearMgr ){
     _relative_position_of_measurement_surface =  pVXDDetMain.getDoubleVal( "relative_position_of_measurement_surface"  );
   }
   catch (gear::UnknownParameterException& e) {}
+
+  _vxd_Cryostat.shellInnerR    = pVXDDetMain.getShellInnerRadius();
+  _vxd_Cryostat.shellThickness = pVXDDetMain.getShellOuterRadius() - pVXDDetMain.getShellInnerRadius();
+  _vxd_Cryostat.shelllHalfZ    = pVXDDetMain.getShellHalfLength();
+
   try {
     const gear::GearParameters& pVXDInfra = gearMgr.getGearParameters("VXDInfra");                                                                                                  
     _vxd_Cryostat.alRadius    = pVXDInfra.getDoubleVal( "CryostatAlRadius"  );
@@ -365,16 +375,16 @@ void ILDVXDKalDetector::setupGearGeom( const gear::GearMgr& gearMgr ){
     _vxd_Cryostat.alZEndCap   = pVXDInfra.getDoubleVal( "CryostatAlZEndCap"  );
     _vxd_Cryostat.alHalfZ     = pVXDInfra.getDoubleVal( "CryostatAlHalfZ"  );
 
-    _vxd_Cryostat.shellInnerR    = pVXDDetMain.getShellInnerRadius();
-    _vxd_Cryostat.shellThickness = pVXDDetMain.getShellOuterRadius() - _vxd_Cryostat.shellInnerR;
-    _vxd_Cryostat.shelllHalfZ    = pVXDDetMain.getShellHalfLength();
+    //_vxd_Cryostat.shellInnerR    = pVXDDetMain.getShellInnerRadius();
+    //_vxd_Cryostat.shellThickness = pVXDDetMain.getShellOuterRadius() - _vxd_Cryostat.shellInnerR;
+    //_vxd_Cryostat.shelllHalfZ    = pVXDDetMain.getShellHalfLength();
 
     _vxd_Cryostat.exists = true;
     //std::cout << "VXDInfra: " << _vxd_Cryostat.alRadius << " " << _vxd_Cryostat.alThickness << " " << _vxd_Cryostat.alInnerR << " " << _vxd_Cryostat.alZEndCap << " " 
     //	      << _vxd_Cryostat.alHalfZ << " " << _vxd_Cryostat.shellInnerR << " " << _vxd_Cryostat.shellThickness << " " << _vxd_Cryostat.shelllHalfZ << std::endl;
   }    
   catch (gear::UnknownParameterException& e) {
-    std::cout << e.what() << std::endl ;
+    //std::cout << "ILDVXDKalDetector: " << e.what() << ", will not be built" << std::endl ;
     _vxd_Cryostat.exists = false;
 
   }
diff --git a/Utilities/KalTest/src/kaltracklib/TKalDetCradle.cxx b/Utilities/KalTest/src/kaltracklib/TKalDetCradle.cxx
index 0ff61c7073b50eea9ce7f33d1397f0b21b7bcaae..010face67afbfa9d24e1a533f3c9d906886ca9a1 100644
--- a/Utilities/KalTest/src/kaltracklib/TKalDetCradle.cxx
+++ b/Utilities/KalTest/src/kaltracklib/TKalDetCradle.cxx
@@ -100,8 +100,11 @@ void TKalDetCradle::Transport(const TKalTrackSite  &from,   // site from
   const TVMeasLayer& ml_to = to.GetHit().GetMeasLayer() ;
   TVector3  x0;
   this->Transport(from, ml_to, x0, sv, F, Q ) ;
-  
-  THelicalTrack hel(sv, x0, to.GetHit().GetBfield()) ;
+
+  double bfield = to.GetHit().GetBfield();
+  TVTrack* trk = 0;
+  if (bfield==0) trk = new TStraightTrack(sv, x0);
+  else           trk = new THelicalTrack(sv, x0, bfield);
   
   // ---------------------------------------------------------------------
   //  Move pivot from last expected hit to actural hit at site to
@@ -113,14 +116,14 @@ void TKalDetCradle::Transport(const TKalTrackSite  &from,   // site from
     Int_t sdim = sv.GetNrows();              // number of track parameters
     TKalMatrix DF(sdim, sdim);               // propagator matrix segment
     
-    hel.MoveTo(to.GetPivot(), fid, &DF);     // move pivot to actual hit (to)
+    trk->MoveTo(to.GetPivot(), fid, &DF);     // move pivot to actual hit (to)
     F = DF * F;                              // update F accordingly
-    hel.PutInto(sv);                         // save updated hel to sv
+    trk->PutInto(sv);                         // save updated hel to sv
     
   } else {
     to.SetPivot(x0);                         // if it is a 1-dim hit
   }
-  
+  delete trk;
 }
 
 //