diff --git a/Reconstruction/CMakeLists.txt b/Reconstruction/CMakeLists.txt
index 793d8828b8f2d6ee08f04c20dcef10b7e041c78f..20058b1c4d08665cfe763d841edfb80f86c37794 100644
--- a/Reconstruction/CMakeLists.txt
+++ b/Reconstruction/CMakeLists.txt
@@ -6,3 +6,4 @@ add_subdirectory(Tracking)
 add_subdirectory(RecGenfitAlg)
 add_subdirectory(RecAssociationMaker)
 add_subdirectory(ParticleID)
+add_subdirectory(CrystalCaloRec)
diff --git a/Reconstruction/CrystalCaloRec/CMakeLists.txt b/Reconstruction/CrystalCaloRec/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..d577cb45df57b60dce98f29e7cba5766c95c670a
--- /dev/null
+++ b/Reconstruction/CrystalCaloRec/CMakeLists.txt
@@ -0,0 +1,68 @@
+# Modules
+gaudi_add_library(CrystalCaloRecLib
+                  SOURCES src/Objects/CaloUnit.cc
+                          src/Objects/CaloHit.cc
+                          src/Objects/Calo1DCluster.cc
+                          src/Objects/Calo2DCluster.cc
+                          src/Objects/Calo3DCluster.cc
+                          src/Objects/CaloHalfCluster.cc
+                          src/Objects/Track.cc
+                          src/Objects/HoughSpace.cc
+                          src/Objects/HoughObject.cc
+                          src/Objects/PFObject.cc
+                  LINK    k4FWCore::k4FWCore
+                          GearSvc
+                          CrystalEcalSvcLib
+                          DetInterface
+                          Gaudi::GaudiKernel
+                          Gaudi::GaudiAlgLib
+                          ${CLHEP_LIBRARIES}
+                          ${GEAR_LIBRARIES}
+                          ${GSL_LIBRARIES}
+                          ${LCIO_LIBRARIES}
+                          ${ROOT_LIBRARIES}
+                          EDM4HEP::edm4hep EDM4HEP::edm4hepDict
+
+)
+
+gaudi_add_module(CrystalCaloRec
+                 SOURCES src/PandoraPlusPFAlg.cpp
+                         src/PandoraPlusDataCol.cpp
+                         src/Tools/MCParticleCreator.cpp
+                         src/Tools/TrackCreator.cpp
+                         src/Tools/CaloHitsCreator.cpp
+                         src/Tools/OutputCreator.cpp
+                         src/Tools/TrackFitInEcal.cpp
+                         src/Algorithm/ExampleAlg.cpp
+                         src/Algorithm/TrackExtrapolatingAlg.cpp
+                         src/Algorithm/GlobalClusteringAlg.cpp
+                         src/Algorithm/LocalMaxFindingAlg.cpp
+                         src/Algorithm/TrackMatchingAlg.cpp
+                         src/Algorithm/HoughClusteringAlg.cpp
+                         src/Algorithm/ConeClustering2DAlg.cpp
+                         src/Algorithm/AxisMergingAlg.cpp
+                         src/Algorithm/EnergySplittingAlg.cpp
+                         src/Algorithm/EnergyTimeMatchingAlg.cpp
+                         src/Algorithm/HcalClusteringAlg.cpp
+                         src/Algorithm/ConeClusteringAlg.cpp
+                         src/Algorithm/PFOCreatingAlg.cpp
+                         src/Algorithm/TrackClusterConnectingAlg.cpp
+                         src/Algorithm/PFOReclusteringAlg.cpp
+                         src/Algorithm/TruthTrackMatchingAlg.cpp
+                         src/Algorithm/TruthMatchingAlg.cpp
+                         src/Algorithm/TruthPatternRecAlg.cpp
+                         src/Algorithm/TruthEnergySplittingAlg.cpp
+                         src/Algorithm/TruthClusteringAlg.cpp
+                         src/Algorithm/TruthClusterMergingAlg.cpp
+                 LINK    CrystalCaloRecLib
+)
+target_include_directories(CrystalCaloRec PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_LIST_DIR}>/include
+  $<INSTALL_INTERFACE:${CMAKE_INSTALL_INCLUDEDIR}>)
+
+install(TARGETS CrystalCaloRecLib CrystalCaloRec
+  EXPORT CEPCSWTargets
+  RUNTIME DESTINATION "${CMAKE_INSTALL_BINDIR}" COMPONENT bin
+  LIBRARY DESTINATION "${CMAKE_INSTALL_LIBDIR}" COMPONENT shlib
+  COMPONENT dev)
+
diff --git a/Reconstruction/CrystalCaloRec/include/Algorithm/AxisMergingAlg.h b/Reconstruction/CrystalCaloRec/include/Algorithm/AxisMergingAlg.h
new file mode 100644
index 0000000000000000000000000000000000000000..b93c40255dd0e41c26695570166fb923ec2a3362
--- /dev/null
+++ b/Reconstruction/CrystalCaloRec/include/Algorithm/AxisMergingAlg.h
@@ -0,0 +1,48 @@
+#ifndef _AXISMERGING_ALG_H
+#define _AXISMERGING_ALG_H
+
+#include "Tools/Algorithm.h"
+
+using namespace PandoraPlus;
+class AxisMergingAlg: public PandoraPlus::Algorithm{
+public: 
+
+  AxisMergingAlg(){};
+  ~AxisMergingAlg(){};
+
+  class Factory : public PandoraPlus::AlgorithmFactory
+  {
+  public: 
+    PandoraPlus::Algorithm* CreateAlgorithm() const{ return new AxisMergingAlg(); } 
+
+  };
+
+  StatusCode ReadSettings( PandoraPlus::Settings& m_settings );
+  StatusCode Initialize( PandoraPlusDataCol& m_datacol );
+  StatusCode RunAlgorithm( PandoraPlusDataCol& m_datacol );
+  StatusCode ClearAlgorithm();
+
+  //Self defined algorithms
+  StatusCode TrkMatchedMerging( std::vector<PandoraPlus::CaloHalfCluster*>& m_axisCol );
+  StatusCode OverlapMerging   ( std::vector<PandoraPlus::CaloHalfCluster*>& m_axisCol );
+  StatusCode BranchMerging   ( std::vector<PandoraPlus::CaloHalfCluster*>& m_axisCol );     // yyy: trying to merge fake photon to track axis
+  StatusCode ConeMerging      ( std::vector<PandoraPlus::CaloHalfCluster*>& m_axisCol );
+  StatusCode FragmentsMerging ( std::vector<PandoraPlus::CaloHalfCluster*>& m_axisCol );
+  bool MergeToClosestCluster( PandoraPlus::CaloHalfCluster* m_badaxis, std::vector<PandoraPlus::CaloHalfCluster*>& m_axisCol );
+
+private: 
+
+  std::vector<std::shared_ptr<PandoraPlus::CaloHalfCluster>>* p_HalfClusterU = nullptr;
+  std::vector<std::shared_ptr<PandoraPlus::CaloHalfCluster>>* p_HalfClusterV = nullptr;
+
+  std::vector<const PandoraPlus::CaloHalfCluster*> m_axisUCol;
+  std::vector<const PandoraPlus::CaloHalfCluster*> m_axisVCol;
+  std::vector<PandoraPlus::CaloHalfCluster*> m_newAxisUCol;
+  std::vector<PandoraPlus::CaloHalfCluster*> m_newAxisVCol;
+
+  static bool compLayer( const PandoraPlus::CaloHalfCluster* sh1, const PandoraPlus::CaloHalfCluster* sh2 )
+    { return sh1->getBeginningDlayer() < sh2->getBeginningDlayer(); }
+
+};
+
+#endif
diff --git a/Reconstruction/CrystalCaloRec/include/Algorithm/ClusterMergingAlg.h b/Reconstruction/CrystalCaloRec/include/Algorithm/ClusterMergingAlg.h
new file mode 100644
index 0000000000000000000000000000000000000000..8669f61eb1be4566831609cc2e96636fd60800a5
--- /dev/null
+++ b/Reconstruction/CrystalCaloRec/include/Algorithm/ClusterMergingAlg.h
@@ -0,0 +1,31 @@
+#ifndef _CLUSTERMERGING_ALG_H
+#define _CLUSTERMERGING_ALG_H
+
+#include "Tools/Algorithm.h"
+
+using namespace PandoraPlus;
+class ClusterMergingAlg: public PandoraPlus::Algorithm{
+public: 
+
+  ClusterMergingAlg(){};
+  ~ClusterMergingAlg(){};
+
+  class Factory : public PandoraPlus::AlgorithmFactory
+  {
+  public: 
+    PandoraPlus::Algorithm* CreateAlgorithm() const{ return new ClusterMergingAlg(); } 
+
+  };
+
+  StatusCode ReadSettings(PandoraPlus::Settings& m_settings);
+  StatusCode Initialize( PandoraPlusDataCol& m_datacol );
+  StatusCode RunAlgorithm( PandoraPlusDataCol& m_datacol );
+  StatusCode ClearAlgorithm();
+
+  //Self defined algorithms
+  StatusCode SelfAlg1(); 
+
+private: 
+
+};
+#endif
diff --git a/Reconstruction/CrystalCaloRec/include/Algorithm/ConeClustering2DAlg.h b/Reconstruction/CrystalCaloRec/include/Algorithm/ConeClustering2DAlg.h
new file mode 100644
index 0000000000000000000000000000000000000000..cf1d9ada6dd9cbfd4b3cb467cfc11ef93a0fca20
--- /dev/null
+++ b/Reconstruction/CrystalCaloRec/include/Algorithm/ConeClustering2DAlg.h
@@ -0,0 +1,45 @@
+#ifndef _CONECLUSTERING2D_ALG_H
+#define _CONECLUSTERING2D_ALG_H
+
+#include "PandoraPlusDataCol.h"
+#include "Tools/Algorithm.h"
+#include "TMath.h"
+
+using namespace PandoraPlus;
+class ConeClustering2DAlg: public PandoraPlus::Algorithm{
+public: 
+
+  ConeClustering2DAlg(){};
+  ~ConeClustering2DAlg(){};
+
+  class Factory : public PandoraPlus::AlgorithmFactory
+  {
+  public: 
+    PandoraPlus::Algorithm* CreateAlgorithm() const{ return new ConeClustering2DAlg(); } 
+
+  };
+
+  StatusCode ReadSettings(PandoraPlus::Settings& m_settings);
+  StatusCode Initialize( PandoraPlusDataCol& m_datacol );
+  StatusCode RunAlgorithm( PandoraPlusDataCol& m_datacol );
+  StatusCode ClearAlgorithm();
+
+  //Self defined algorithms
+  StatusCode LongiConeLinking( std::map<int, std::vector<const PandoraPlus::Calo1DCluster*> >& orderedShower, 
+                               std::vector<PandoraPlus::CaloHalfCluster*>& ClusterCol, 
+                               std::vector<std::shared_ptr<PandoraPlus::CaloHalfCluster>>& bk_HFclus );
+  TVector2 GetProjectedAxis( const PandoraPlus::CaloHalfCluster* m_shower );
+  TVector2 GetProjectedRelR( const PandoraPlus::Calo1DCluster* m_shower1, const PandoraPlus::Calo1DCluster* m_shower2 );
+
+private: 
+
+  std::vector<PandoraPlus::CaloHalfCluster*> p_HalfClusterV;
+  std::vector<PandoraPlus::CaloHalfCluster*> p_HalfClusterU;
+
+  std::vector<const PandoraPlus::Calo1DCluster*> m_localMaxVCol;
+  std::vector<const PandoraPlus::Calo1DCluster*> m_localMaxUCol;
+  std::vector<const PandoraPlus::CaloHalfCluster*> const_longiClusVCol;
+  std::vector<const PandoraPlus::CaloHalfCluster*> const_longiClusUCol;
+
+};
+#endif
diff --git a/Reconstruction/CrystalCaloRec/include/Algorithm/ConeClusteringAlg.h b/Reconstruction/CrystalCaloRec/include/Algorithm/ConeClusteringAlg.h
new file mode 100644
index 0000000000000000000000000000000000000000..56484d8972629691b156e1174291f2b8a057a2e5
--- /dev/null
+++ b/Reconstruction/CrystalCaloRec/include/Algorithm/ConeClusteringAlg.h
@@ -0,0 +1,36 @@
+#ifndef _CONECLUSTERING_ALG_H
+#define _CONECLUSTERING_ALG_H
+
+#include "PandoraPlusDataCol.h"
+#include "Tools/Algorithm.h"
+#include "TMath.h"
+using namespace PandoraPlus;
+
+class ConeClusteringAlg: public PandoraPlus::Algorithm {
+public: 
+
+  ConeClusteringAlg(){};
+  ~ConeClusteringAlg(){};
+
+  class Factory : public PandoraPlus::AlgorithmFactory
+  {
+    PandoraPlus::Algorithm* CreateAlgorithm() const{ return new ConeClusteringAlg(); }
+  };
+
+  StatusCode ReadSettings(PandoraPlus::Settings& m_settings);
+  StatusCode Initialize( PandoraPlusDataCol& m_datacol );
+  StatusCode RunAlgorithm( PandoraPlusDataCol& m_datacol);
+  StatusCode ClearAlgorithm(); 
+
+  StatusCode LongiConeLinking( const std::map<int, std::vector<const PandoraPlus::CaloHit*> >& orderedShower, std::vector<std::shared_ptr<PandoraPlus::Calo3DCluster>>& ClusterCol );
+  //StatusCode MergeGoodClusters( std::vector<std::shared_ptr<PandoraPlus::Calo3DCluster>>& m_clusCol); 
+  //StatusCode MergeBadToGoodCluster( std::vector<std::shared_ptr<PandoraPlus::Calo3DCluster>>& m_goodClusCol, std::shared_ptr<PandoraPlus::Calo3DCluster> m_badClus );
+  //PandoraPlus::Calo3DCluster* GetClosestGoodCluster( std::vector< PandoraPlus::Calo3DCluster* >& m_goodClusCol, PandoraPlus::Calo3DCluster* m_badClus );
+
+  //static bool compBegin( PandoraPlus::Calo3DCluster* clus1, PandoraPlus::Calo3DCluster* clus2 ) { return clus1->getBeginningDlayer() < clus2->getBeginningDlayer(); }
+
+private: 
+
+
+};
+#endif
diff --git a/Reconstruction/CrystalCaloRec/include/Algorithm/EnergySplittingAlg.h b/Reconstruction/CrystalCaloRec/include/Algorithm/EnergySplittingAlg.h
new file mode 100644
index 0000000000000000000000000000000000000000..94ff532da0ff2b605f3b9083c0309b01defca7a5
--- /dev/null
+++ b/Reconstruction/CrystalCaloRec/include/Algorithm/EnergySplittingAlg.h
@@ -0,0 +1,77 @@
+#ifndef _ENERGYSPLITTING_ALG_H
+#define _ENERGYSPLITTING_ALG_H
+
+#include "PandoraPlusDataCol.h"
+#include "Tools/Algorithm.h"
+#include "TVector3.h"
+#include "TVector.h"
+#include "TMatrix.h"
+
+using namespace PandoraPlus;
+class EnergySplittingAlg: public PandoraPlus::Algorithm{
+public: 
+
+  class Factory : public PandoraPlus::AlgorithmFactory
+  {
+  public:
+    PandoraPlus::Algorithm* CreateAlgorithm() const{ return new EnergySplittingAlg(); }
+
+  };
+
+  EnergySplittingAlg(){};
+  ~EnergySplittingAlg(){};
+
+  StatusCode ReadSettings( PandoraPlus::Settings& m_settings );
+  StatusCode Initialize( PandoraPlusDataCol& m_datacol );
+  StatusCode RunAlgorithm( PandoraPlusDataCol& m_datacol );
+  StatusCode ClearAlgorithm(); 
+
+
+  StatusCode LongitudinalLinking( std::vector<std::shared_ptr<PandoraPlus::Calo1DCluster>>& m_showers, 
+                                  std::vector<const PandoraPlus::CaloHalfCluster*>& m_oldClusCol, 
+                                  std::vector<std::shared_ptr<PandoraPlus::CaloHalfCluster>>& m_newClusCol );
+
+  StatusCode HalfClusterToTowers( std::vector<PandoraPlus::CaloHalfCluster*>& m_halfClusU, 
+                                  std::vector<PandoraPlus::CaloHalfCluster*>& m_halfClusV, 
+                                  std::vector<PandoraPlus::CaloHalfCluster*>& m_emptyClusU,
+                                  std::vector<PandoraPlus::CaloHalfCluster*>& m_emptyClusV,
+                                  std::vector<std::shared_ptr<PandoraPlus::Calo3DCluster>>& m_towers );
+
+  StatusCode ClusterSplitting( const PandoraPlus::Calo1DCluster* m_cluster, 
+                               std::vector<std::shared_ptr<PandoraPlus::Calo1DCluster>>& outshCol );
+
+  StatusCode SplitOverlapCluster( std::vector<std::shared_ptr<PandoraPlus::CaloHalfCluster>>& m_HFClusCol );
+
+  StatusCode MergeToClosestCluster( PandoraPlus::Calo1DCluster* iclus, std::vector<std::shared_ptr<PandoraPlus::Calo1DCluster>>& clusvec );
+
+  StatusCode MergeToClosestCluster( const PandoraPlus::Calo1DCluster* m_shower, std::vector<std::shared_ptr<PandoraPlus::CaloHalfCluster>>& m_clusters );
+
+  void CalculateInitialEseed( const std::vector<const PandoraPlus::CaloUnit*>& Seeds, const TVector3* pos, double* Eseed);
+
+  double GetShowerProfile(const TVector3& p_bar, const TVector3& p_seed );
+
+
+private: 
+  std::vector<PandoraPlus::CaloHalfCluster*> p_HalfClusterU;
+  std::vector<PandoraPlus::CaloHalfCluster*> p_HalfClusterV;
+  std::vector<PandoraPlus::CaloHalfCluster*> p_emptyHalfClusterU;
+  std::vector<PandoraPlus::CaloHalfCluster*> p_emptyHalfClusterV;  
+  std::vector<const PandoraPlus::CaloHalfCluster*> m_axisUCol; 
+  std::vector<const PandoraPlus::CaloHalfCluster*> m_axisVCol; 
+
+  std::vector<std::shared_ptr<PandoraPlus::CaloHalfCluster>> m_newClusUCol; 
+  std::vector<std::shared_ptr<PandoraPlus::CaloHalfCluster>> m_newClusVCol; 
+  std::vector<std::shared_ptr<PandoraPlus::Calo1DCluster>> m_1dShowerUCol; 
+  std::vector<std::shared_ptr<PandoraPlus::Calo1DCluster>> m_1dShowerVCol; 
+  std::vector<std::shared_ptr<PandoraPlus::Calo3DCluster>> m_towerCol;
+
+  PandoraPlusDataCol m_bkCol;
+
+  //static bool compBar( const PandoraPlus::CaloUnit* bar1, const PandoraPlus::CaloUnit* bar2 )
+  //  { return bar1->getBar() < bar2->getBar(); }
+  static bool compLayer( const PandoraPlus::Calo1DCluster* sh1, const PandoraPlus::Calo1DCluster* sh2 )
+    { return sh1->getDlayer() < sh2->getDlayer(); }
+
+};
+
+#endif
diff --git a/Reconstruction/CrystalCaloRec/include/Algorithm/EnergyTimeMatchingAlg.h b/Reconstruction/CrystalCaloRec/include/Algorithm/EnergyTimeMatchingAlg.h
new file mode 100644
index 0000000000000000000000000000000000000000..46d6df6ca3174c8e7c835042aef974674e0d23d4
--- /dev/null
+++ b/Reconstruction/CrystalCaloRec/include/Algorithm/EnergyTimeMatchingAlg.h
@@ -0,0 +1,83 @@
+#ifndef ETMATCHING_ALG_H
+#define ETMATCHING_ALG_H
+
+#include "PandoraPlusDataCol.h"
+#include "Tools/Algorithm.h"
+
+#include "TVector3.h"
+using namespace PandoraPlus;
+
+class EnergyTimeMatchingAlg: public PandoraPlus::Algorithm{
+
+public: 
+
+  class Factory : public PandoraPlus::AlgorithmFactory
+  {
+  public:
+    PandoraPlus::Algorithm* CreateAlgorithm() const{ return new EnergyTimeMatchingAlg(); }
+
+  };
+
+  EnergyTimeMatchingAlg(){};
+  ~EnergyTimeMatchingAlg(){};
+
+  StatusCode ReadSettings( PandoraPlus::Settings& m_settings);
+  StatusCode Initialize( PandoraPlusDataCol& m_datacol );
+  StatusCode RunAlgorithm( PandoraPlusDataCol& m_datasvc );
+  StatusCode ClearAlgorithm(); 
+
+  StatusCode Matching(std::vector<const PandoraPlus::CaloHalfCluster*> m_HFClusUCol, 
+                      std::vector<const PandoraPlus::CaloHalfCluster*> m_HFClusVCol,
+                      std::vector<std::shared_ptr<PandoraPlus::Calo3DCluster>>& m_clusterCol );
+
+  StatusCode PatternMatrixCalculation(std::vector<const PandoraPlus::CaloHalfCluster*> m_HFClusUCol,
+                                      std::vector<const PandoraPlus::CaloHalfCluster*> m_HFClusVCol,
+                                      vector<vector<int>>& matrix );
+
+  StatusCode Chi2MatrixCalculation( std::vector<const PandoraPlus::CaloHalfCluster*> m_HFClusUCol,
+                                    std::vector<const PandoraPlus::CaloHalfCluster*> m_HFClusVCol,
+                                    vector<vector<double>>& matrix, 
+                                    std::vector< std::pair<int, int> >& chi2order); 
+
+  StatusCode ParameterMatrixCalculation(const int& M, const int& N, vector<vector<int>>& parMatrix);
+
+  StatusCode SimplityMatrix(vector<vector<int>>& parMatrix, vector<double>& Eij, vector<vector<int>>& pattern);
+
+  double SolveMatrix(const int& M, const int& N, vector<vector<int>>& parMatrix, vector<double>& Eij, std::vector<double>& En_clusters); 
+
+  StatusCode leastSquares(const vector<vector<int>>& A, const vector<double>& b, vector<double>& x);
+
+  vector<vector<double>> GetClusterChi2Map( std::vector<std::vector<const PandoraPlus::Calo1DCluster*>>& barShowerUCol,
+                                            std::vector<std::vector<const PandoraPlus::Calo1DCluster*>>& barShowerVCol );
+
+  StatusCode ClusterBuilding( std::vector<std::vector<double>> Ematrix, 
+                              std::vector<const PandoraPlus::CaloHalfCluster*>& m_HFClusUCol, 
+                              std::vector<const PandoraPlus::CaloHalfCluster*>& m_HFClusVCol, 
+                              std::vector<std::shared_ptr<PandoraPlus::Calo3DCluster>>& m_clusterCol);
+
+  StatusCode XYClusterMatchingL0( const PandoraPlus::CaloHalfCluster* m_longiClU,
+                                  const PandoraPlus::CaloHalfCluster* m_longiClV,
+                                  std::shared_ptr<PandoraPlus::Calo3DCluster>& m_clus );
+
+  StatusCode GetMatchedShowersL0( const PandoraPlus::Calo1DCluster* barShowerU,
+                                  const PandoraPlus::Calo1DCluster* barShowerV,
+                                  PandoraPlus::Calo2DCluster* outsh );
+
+  StatusCode GetMatchedShowersL1( const PandoraPlus::Calo1DCluster* shower1,
+                                  std::vector<const PandoraPlus::Calo1DCluster*>& showerNCol,
+                                  std::vector<PandoraPlus::Calo2DCluster*>& outshCol );
+
+  StatusCode ClusterReconnecting(std::vector<std::shared_ptr<PandoraPlus::Calo3DCluster>>& m_clusterCol);
+
+
+private: 
+  std::vector<PandoraPlus::Calo3DCluster*> m_towerCol; 
+
+  std::vector<const PandoraPlus::CaloHalfCluster*> m_HFClusUCol;
+  std::vector<const PandoraPlus::CaloHalfCluster*> m_HFClusVCol;
+  std::vector<std::shared_ptr<PandoraPlus::Calo3DCluster>> m_clusterCol; 
+
+  PandoraPlusDataCol m_bkCol;
+
+};
+#endif
diff --git a/Reconstruction/CrystalCaloRec/include/Algorithm/ExampleAlg.h b/Reconstruction/CrystalCaloRec/include/Algorithm/ExampleAlg.h
new file mode 100644
index 0000000000000000000000000000000000000000..d4d0733a1d8947414889b24ca7d7c20ac53768c5
--- /dev/null
+++ b/Reconstruction/CrystalCaloRec/include/Algorithm/ExampleAlg.h
@@ -0,0 +1,31 @@
+#ifndef _EXAMPLE_ALG_H
+#define _EXAMPLE_ALG_H
+
+#include "Tools/Algorithm.h"
+
+using namespace PandoraPlus;
+class ExampleAlg: public PandoraPlus::Algorithm{
+public: 
+
+  ExampleAlg(){};
+  ~ExampleAlg(){};
+
+  class Factory : public PandoraPlus::AlgorithmFactory
+  {
+  public: 
+    PandoraPlus::Algorithm* CreateAlgorithm() const{ return new ExampleAlg(); } 
+
+  };
+
+  StatusCode ReadSettings(PandoraPlus::Settings& m_settings);
+  StatusCode Initialize( PandoraPlusDataCol& m_datacol );
+  StatusCode RunAlgorithm( PandoraPlusDataCol& m_datacol );
+  StatusCode ClearAlgorithm();
+
+  //Self defined algorithms
+  StatusCode SelfAlg1(); 
+
+private: 
+
+};
+#endif
diff --git a/Reconstruction/CrystalCaloRec/include/Algorithm/GlobalClusteringAlg.h b/Reconstruction/CrystalCaloRec/include/Algorithm/GlobalClusteringAlg.h
new file mode 100644
index 0000000000000000000000000000000000000000..5c4642b9f388941b2e86cbd20c92d436d21c6a6d
--- /dev/null
+++ b/Reconstruction/CrystalCaloRec/include/Algorithm/GlobalClusteringAlg.h
@@ -0,0 +1,53 @@
+#ifndef GLOBALCLUSTERING_ALG_H
+#define GLOBALCLUSTERING_ALG_H
+
+#include "Tools/Algorithm.h"
+
+#include "time.h"
+#include <TTimeStamp.h> 
+#include <ctime>
+
+#include <cstdlib>
+
+using namespace PandoraPlus;
+
+class GlobalClusteringAlg : public PandoraPlus::Algorithm{
+public: 
+
+  GlobalClusteringAlg(){};
+  ~GlobalClusteringAlg(){};
+ 
+  class Factory : public PandoraPlus::AlgorithmFactory
+  {
+  public:
+    PandoraPlus::Algorithm* CreateAlgorithm() const{ return new GlobalClusteringAlg(); }
+ 
+  };
+ 
+  StatusCode ReadSettings(PandoraPlus::Settings& m_settings); 
+  StatusCode Initialize( PandoraPlusDataCol& m_datacol );
+  StatusCode RunAlgorithm( PandoraPlusDataCol& m_datacol );
+  StatusCode ClearAlgorithm();
+
+  template<typename T1, typename T2> StatusCode Clustering(std::vector<std::shared_ptr<T1>>& m_input, std::vector<std::shared_ptr<T2>>& m_output);
+  
+  //geometry construction
+/*   int m_module = settings.map_intPars["m_module"];
+  int m_modulestart = settings.map_intPars["m_modulestart"];
+  int m_part = settings.map_intPars["m_part"];
+  int m_stave = settings.map_intPars["m_stave"];
+  int m_superlayer = settings.map_intPars["m_superlayer"];
+  int m_startnumber = settings.map_intPars["m_startnumber"];
+  int m_phibarnumber = settings.map_intPars["m_phibarnumber"];
+  int m_zbarnumber = settings.map_intPars["m_zbarnumber"]; */
+
+private:
+
+  std::vector<std::shared_ptr<PandoraPlus::CaloUnit>> m_bars; 
+  std::vector<std::shared_ptr<PandoraPlus::CaloUnit>> m_processbars;        
+  std::vector<std::shared_ptr<PandoraPlus::CaloUnit>> m_restbars;           
+  std::vector<std::shared_ptr<PandoraPlus::Calo1DCluster>> m_1dclusters;    
+  std::vector<std::shared_ptr<PandoraPlus::CaloHalfCluster>> m_halfclusters; 
+  
+};
+#endif
diff --git a/Reconstruction/CrystalCaloRec/include/Algorithm/HcalClusteringAlg.h b/Reconstruction/CrystalCaloRec/include/Algorithm/HcalClusteringAlg.h
new file mode 100644
index 0000000000000000000000000000000000000000..31c4c76274ccb8fc4862d1ab9421987b2a4e76bc
--- /dev/null
+++ b/Reconstruction/CrystalCaloRec/include/Algorithm/HcalClusteringAlg.h
@@ -0,0 +1,33 @@
+#ifndef HCALCLUSTERING_ALG_H
+#define HCALCLUSTERING_ALG_H
+
+#include "Tools/Algorithm.h"
+
+using namespace PandoraPlus;
+
+class HcalClusteringAlg : public PandoraPlus::Algorithm{
+public: 
+
+  HcalClusteringAlg(){};
+  ~HcalClusteringAlg(){};
+ 
+  class Factory : public PandoraPlus::AlgorithmFactory
+  {
+  public:
+    PandoraPlus::Algorithm* CreateAlgorithm() const{ return new HcalClusteringAlg(); }
+ 
+  };
+ 
+  StatusCode ReadSettings(PandoraPlus::Settings& m_settings); 
+  StatusCode Initialize( PandoraPlusDataCol& m_datacol );
+  StatusCode RunAlgorithm( PandoraPlusDataCol& m_datacol );
+  StatusCode ClearAlgorithm();
+  
+  template<typename T1, typename T2> StatusCode Clustering(std::vector<T1*>& m_input, std::vector<std::shared_ptr<T2>>& m_output);
+  StatusCode LongiConeLinking( const std::map<int, std::vector<PandoraPlus::CaloHit*> >& orderedHit, std::vector<std::shared_ptr<PandoraPlus::Calo3DCluster> >& ClusterCol );
+
+private:
+
+//   std::vector<std::shared_ptr<PandoraPlus::CaloHit>> m_hcalHits;
+};
+#endif
\ No newline at end of file
diff --git a/Reconstruction/CrystalCaloRec/include/Algorithm/HoughClusteringAlg.h b/Reconstruction/CrystalCaloRec/include/Algorithm/HoughClusteringAlg.h
new file mode 100644
index 0000000000000000000000000000000000000000..1f3940fbb01e949a23a6d8095811133e3fd6532b
--- /dev/null
+++ b/Reconstruction/CrystalCaloRec/include/Algorithm/HoughClusteringAlg.h
@@ -0,0 +1,49 @@
+#ifndef HOUGHCLUSTERINGALG_H
+#define HOUGHCLUSTERINGALG_H
+
+#include "PandoraPlusDataCol.h"
+#include "Tools/Algorithm.h"
+#include "TVector2.h"
+#include <vector>
+using namespace PandoraPlus;
+
+class HoughClusteringAlg: public PandoraPlus::Algorithm{
+
+public: 
+
+  HoughClusteringAlg () {};
+  ~HoughClusteringAlg() {};
+
+  class Factory : public PandoraPlus::AlgorithmFactory
+  {
+  public:
+    PandoraPlus::Algorithm* CreateAlgorithm() const{ return new HoughClusteringAlg(); }
+  };
+
+  StatusCode ReadSettings(PandoraPlus::Settings& m_settings);
+  StatusCode Initialize( PandoraPlusDataCol& m_datacol );
+  StatusCode RunAlgorithm( PandoraPlusDataCol& m_datacol );
+  StatusCode ClearAlgorithm();
+
+  StatusCode HoughTransformation( std::vector<PandoraPlus::HoughObject>& Hobjects );
+  //StatusCode SetLineRange( int module, int slayer, double *range12, double* range34 );
+  StatusCode FillHoughSpace( std::vector<PandoraPlus::HoughObject>& Hobjects, 
+                             PandoraPlus::HoughSpace& Hspace );
+  StatusCode ClusterFinding( std::vector<PandoraPlus::HoughObject>& Hobjects, 
+                             PandoraPlus::HoughSpace& Hspace, 
+                             std::vector<std::shared_ptr<PandoraPlus::CaloHalfCluster>>& longiClusCol );
+  StatusCode CleanClusters( std::vector<std::shared_ptr<PandoraPlus::CaloHalfCluster>>& m_longiClusCol);
+  
+
+private:
+	std::vector<PandoraPlus::CaloHalfCluster*> p_HalfClusterV;
+  std::vector<PandoraPlus::CaloHalfCluster*> p_HalfClusterU;
+
+  std::vector<const PandoraPlus::Calo1DCluster*> m_localMaxVCol;
+  std::vector<const PandoraPlus::Calo1DCluster*> m_localMaxUCol;
+  std::vector<std::shared_ptr<PandoraPlus::CaloHalfCluster>> m_longiClusVCol;
+  std::vector<std::shared_ptr<PandoraPlus::CaloHalfCluster>> m_longiClusUCol;
+
+
+};
+#endif
diff --git a/Reconstruction/CrystalCaloRec/include/Algorithm/LocalMaxFindingAlg.h b/Reconstruction/CrystalCaloRec/include/Algorithm/LocalMaxFindingAlg.h
new file mode 100644
index 0000000000000000000000000000000000000000..0ad55daff87e1c742fcc2a20cc8a4fe1c4468e60
--- /dev/null
+++ b/Reconstruction/CrystalCaloRec/include/Algorithm/LocalMaxFindingAlg.h
@@ -0,0 +1,39 @@
+#ifndef _LOCALMAXFINDING_ALG_H
+#define _LOCALMAXFINDING_ALG_H
+
+#include <set>
+#include "Tools/Algorithm.h"
+using namespace PandoraPlus;
+
+class LocalMaxFindingAlg: public PandoraPlus::Algorithm{
+public: 
+
+  LocalMaxFindingAlg(){};
+  ~LocalMaxFindingAlg(){};
+
+  class Factory : public PandoraPlus::AlgorithmFactory
+  {
+  public: 
+    PandoraPlus::Algorithm* CreateAlgorithm() const { return new LocalMaxFindingAlg(); }
+
+  };
+
+  StatusCode ReadSettings(PandoraPlus::Settings& m_settings);
+  StatusCode Initialize( PandoraPlusDataCol& m_datacol );
+  StatusCode RunAlgorithm( PandoraPlusDataCol& m_datacol );
+  StatusCode ClearAlgorithm();
+
+  StatusCode GetLocalMax( const PandoraPlus::Calo1DCluster* m_1dClus, std::vector<std::shared_ptr<PandoraPlus::Calo1DCluster>>& m_output);
+  StatusCode GetLocalMaxBar( std::vector<const PandoraPlus::CaloUnit*>& barCol, std::vector<const PandoraPlus::CaloUnit*>& localMaxCol );
+  std::vector<const PandoraPlus::CaloUnit*>  getNeighbors(const PandoraPlus::CaloUnit* seed, std::vector<const PandoraPlus::CaloUnit*>& barCol);
+
+private: 
+
+  std::vector<std::shared_ptr<PandoraPlus::CaloHalfCluster>>* p_HalfClusU = nullptr;
+  std::vector<std::shared_ptr<PandoraPlus::CaloHalfCluster>>* p_HalfClusV = nullptr;
+
+  static bool compBar( const PandoraPlus::CaloUnit* bar1, const PandoraPlus::CaloUnit* bar2 )
+      { return bar1->getBar() < bar2->getBar(); }
+
+};
+#endif
diff --git a/Reconstruction/CrystalCaloRec/include/Algorithm/PFOCreatingAlg.h b/Reconstruction/CrystalCaloRec/include/Algorithm/PFOCreatingAlg.h
new file mode 100644
index 0000000000000000000000000000000000000000..0d9b36e728def2c340176d959a74dc37e1491d03
--- /dev/null
+++ b/Reconstruction/CrystalCaloRec/include/Algorithm/PFOCreatingAlg.h
@@ -0,0 +1,64 @@
+#ifndef _PFOCREATING_ALG_H
+#define _PFOCREATING_ALG_H
+
+#include "Tools/Algorithm.h"
+
+using namespace PandoraPlus;
+class PFOCreatingAlg: public PandoraPlus::Algorithm{
+public: 
+
+  PFOCreatingAlg(){};
+  ~PFOCreatingAlg(){};
+
+  class Factory : public PandoraPlus::AlgorithmFactory
+  {
+  public: 
+    PandoraPlus::Algorithm* CreateAlgorithm() const{ return new PFOCreatingAlg(); } 
+
+  };
+
+  StatusCode ReadSettings( PandoraPlus::Settings& m_settings );
+  StatusCode Initialize( PandoraPlusDataCol& m_datacol );
+  StatusCode RunAlgorithm( PandoraPlusDataCol& m_datacol );
+  StatusCode ClearAlgorithm();
+
+  std::vector<PandoraPlus::Track*> getTracks() const { return m_tracks; }
+  std::vector<PandoraPlus::Calo3DCluster*> getECALClusters() const { return m_ecal_clusters; }
+  std::vector<PandoraPlus::Calo3DCluster*> getHCALClusters() const { return m_hcal_clusters; }
+
+  //Self defined algorithms
+  // Get canditate clusters in HCAL for charged particles
+  StatusCode GetChargedHCALCandidates(const PandoraPlus::Track* _track,
+                                      std::vector<PandoraPlus::Calo3DCluster*>& _hcal_clusters,
+                                      std::vector<PandoraPlus::Calo3DCluster*>& _hcal_clus_candidate);
+  // Get nearby HCAL clusters
+  StatusCode GetNearbyHCALCandidates( PandoraPlus::Calo3DCluster* _ecal_cluster,
+                                  std::vector<PandoraPlus::Calo3DCluster*>& _hcal_clusters,
+                                  std::vector<PandoraPlus::Calo3DCluster*>& _hcal_clus_candidate);
+  // If a neutral cluster in ECAL reach the outermost ECAL boundary
+  bool isReachOuterMostECAL(PandoraPlus::Calo3DCluster* _ecal_cluster);
+  // erase the used_elements in the left_elements
+  template<typename T1, typename T2> StatusCode CleanUsedElements(std::vector<T1>& _used_elements,
+                                                    std::vector<T2>& _left_elements);
+  template<typename T1, typename T2> StatusCode CleanUsedElement(T1 _used_elements,
+                                                    std::vector<T2>& _left_elements);
+  // Create PFO with:
+  //   1. tracks with no clusters in ECAL and HCAL
+  //   2. HCAL clusters
+  StatusCode CreateLeftPFO( std::vector<PandoraPlus::Track*>& _tracks,
+                            std::vector<PandoraPlus::Calo3DCluster*>& _hcal_clusters,
+                            std::vector<std::shared_ptr<PandoraPlus::PFObject>>& _pfobjects);
+  
+  
+
+private: 
+  std::vector<PandoraPlus::Track*> m_tracks;
+  std::vector<PandoraPlus::Calo3DCluster*> m_ecal_clusters;
+  std::vector<PandoraPlus::Calo3DCluster*> m_hcal_clusters;
+
+  std::vector<std::shared_ptr<PandoraPlus::PFObject>> m_pfobjects;
+  
+
+};
+
+#endif
diff --git a/Reconstruction/CrystalCaloRec/include/Algorithm/PFOReclusteringAlg.h b/Reconstruction/CrystalCaloRec/include/Algorithm/PFOReclusteringAlg.h
new file mode 100644
index 0000000000000000000000000000000000000000..97bd7048a2f44808e9b3fe80f3cae92b21acbc19
--- /dev/null
+++ b/Reconstruction/CrystalCaloRec/include/Algorithm/PFOReclusteringAlg.h
@@ -0,0 +1,40 @@
+#ifndef _PFORECLUSTERING_ALG_H
+#define _PFORECLUSTERING_ALG_H
+
+#include "Tools/Algorithm.h"
+
+using namespace PandoraPlus;
+class PFOReclusteringAlg: public PandoraPlus::Algorithm{
+public: 
+
+  PFOReclusteringAlg(){};
+  ~PFOReclusteringAlg(){};
+
+  class Factory : public PandoraPlus::AlgorithmFactory
+  {
+  public: 
+    PandoraPlus::Algorithm* CreateAlgorithm() const{ return new PFOReclusteringAlg(); } 
+
+  };
+
+  StatusCode ReadSettings(PandoraPlus::Settings& m_settings);
+  StatusCode Initialize( PandoraPlusDataCol& m_datacol );
+  StatusCode RunAlgorithm( PandoraPlusDataCol& m_datacol );
+  StatusCode ClearAlgorithm();
+
+  StatusCode ReCluster_MergeToChg( std::vector< std::shared_ptr<PandoraPlus::PFObject> >& m_chargedPFOs,
+                                   std::vector< std::shared_ptr<PandoraPlus::PFObject> >& m_neutralPFOs );
+
+  StatusCode ReCluster_SplitFromChg( std::vector< std::shared_ptr<PandoraPlus::PFObject> >& m_chargedPFOs,
+                                     std::vector< std::shared_ptr<PandoraPlus::PFObject> >& m_neutralPFOs );
+
+private: 
+  
+  std::vector<std::shared_ptr<PandoraPlus::PFObject>>* p_PFObjects; 
+
+  PandoraPlusDataCol m_bkCol;
+
+  static bool compTrkP( std::shared_ptr<PandoraPlus::PFObject> pfo1, std::shared_ptr<PandoraPlus::PFObject> pfo2 )
+    { return pfo1->getTrackMomentum() > pfo2->getTrackMomentum(); }
+};
+#endif
diff --git a/Reconstruction/CrystalCaloRec/include/Algorithm/TrackClusterConnectingAlg.h b/Reconstruction/CrystalCaloRec/include/Algorithm/TrackClusterConnectingAlg.h
new file mode 100644
index 0000000000000000000000000000000000000000..483440ca84f371c52abb418407b29f8e7b2519c8
--- /dev/null
+++ b/Reconstruction/CrystalCaloRec/include/Algorithm/TrackClusterConnectingAlg.h
@@ -0,0 +1,52 @@
+#ifndef _TRACKCLUSTERCONNECTING_ALG_H
+#define _TRACKCLUSTERCONNECTING_ALG_H
+
+#include "Tools/Algorithm.h"
+
+using namespace PandoraPlus;
+class TrackClusterConnectingAlg: public PandoraPlus::Algorithm{
+public: 
+
+  TrackClusterConnectingAlg(){};
+  ~TrackClusterConnectingAlg(){};
+
+  class Factory : public PandoraPlus::AlgorithmFactory
+  {
+  public: 
+    PandoraPlus::Algorithm* CreateAlgorithm() const{ return new TrackClusterConnectingAlg(); } 
+
+  };
+
+  StatusCode ReadSettings(PandoraPlus::Settings& m_settings);
+  StatusCode Initialize( PandoraPlusDataCol& m_datacol );
+  StatusCode RunAlgorithm( PandoraPlusDataCol& m_datacol );
+  StatusCode ClearAlgorithm();
+
+  //Self defined algorithms
+  double GetMinR2Trk( const PandoraPlus::Calo3DCluster* p_clus, const PandoraPlus::Track* m_trk);
+  StatusCode PFOCreating( std::vector<const PandoraPlus::Calo3DCluster*>& m_clusters,
+                          std::vector<const PandoraPlus::Track*>& m_trks,
+                          std::vector<std::shared_ptr<PandoraPlus::PFObject>>& m_PFOs );
+
+  StatusCode EcalChFragAbsorption( std::vector<const PandoraPlus::Calo3DCluster*>& m_clusters,
+                                   std::vector<const PandoraPlus::Track*>& m_trks,
+                                   std::vector<std::shared_ptr<PandoraPlus::Calo3DCluster>>& m_newclusCol ); 
+
+  StatusCode HcalExtrapolatingMatch(std::vector<const PandoraPlus::Calo3DCluster*>& m_clusters, std::vector<std::shared_ptr<PandoraPlus::PFObject>>& m_PFOs);
+
+private: 
+
+  std::vector<const PandoraPlus::Calo3DCluster*> m_EcalClusters;
+  std::vector<const PandoraPlus::Calo3DCluster*> m_HcalClusters;
+  std::vector<const PandoraPlus::Track*> m_tracks; 
+
+  std::vector<std::shared_ptr<PandoraPlus::Calo3DCluster>> m_absorbedEcal; 
+  std::vector<std::shared_ptr<PandoraPlus::PFObject>> m_PFObjects; 
+
+  PandoraPlusDataCol m_bkCol;
+
+  static bool compTrkP( std::shared_ptr<PandoraPlus::PFObject> pfo1, std::shared_ptr<PandoraPlus::PFObject> pfo2 )
+    { return pfo1->getTrackMomentum() > pfo2->getTrackMomentum(); }
+
+};
+#endif
diff --git a/Reconstruction/CrystalCaloRec/include/Algorithm/TrackExtrapolatingAlg.h b/Reconstruction/CrystalCaloRec/include/Algorithm/TrackExtrapolatingAlg.h
new file mode 100644
index 0000000000000000000000000000000000000000..af6db11ea1fe20a74e2b4bdcbc15e248a84e59a6
--- /dev/null
+++ b/Reconstruction/CrystalCaloRec/include/Algorithm/TrackExtrapolatingAlg.h
@@ -0,0 +1,76 @@
+#ifndef _TRACKEXTRAPOLATING_ALG_H
+#define _TRACKEXTRAPOLATING_ALG_H
+
+#include "PandoraPlusDataCol.h"
+#include "Tools/Algorithm.h"
+#include "Objects/Track.h"
+
+using namespace PandoraPlus;
+class TrackExtrapolatingAlg: public PandoraPlus::Algorithm{
+public: 
+
+  TrackExtrapolatingAlg(){};
+  ~TrackExtrapolatingAlg(){};
+
+  class Factory : public PandoraPlus::AlgorithmFactory
+  {
+  public: 
+    PandoraPlus::Algorithm* CreateAlgorithm() const{ return new TrackExtrapolatingAlg(); } 
+
+  };
+
+  StatusCode ReadSettings(PandoraPlus::Settings& m_settings);
+  StatusCode Initialize( PandoraPlusDataCol& m_datacol );
+  StatusCode RunAlgorithm( PandoraPlusDataCol& m_datacol );
+  StatusCode ClearAlgorithm();
+
+  //Self defined algorithms
+  // StatusCode SelfAlg1(); 
+
+  // Get normal vectors of planes in each module
+  StatusCode GetPlaneNormalVector(std::vector<TVector2> & normal_vectors_Ecal, std::vector<TVector2> & normal_vectors_Hcal );
+  // Get points in each plane of layer
+  StatusCode GetLayerPoints(const std::vector<TVector2> & normal_vectors_Ecal, 
+                            const std::vector<TVector2> & normal_vectors_Hcal,
+                            std::vector<std::vector<TVector2>> & ECAL_layer_points, 
+                            std::vector<std::vector<TVector2>> & HCAL_layer_points);
+  // If the track reach barrel ECAL
+  bool IsReachECAL(PandoraPlus::Track * track);
+  // Get track state at calorimeter
+  StatusCode GetTrackStateAtCalo(PandoraPlus::Track * track, 
+                           PandoraPlus::TrackState & trk_state_at_calo);
+  // get extrapolated points
+  StatusCode ExtrapolateByLayer(const std::vector<TVector2> & normal_vectors_Ecal,
+                              const std::vector<TVector2> & normal_vectors_Hcal,
+                              const std::vector<std::vector<TVector2>> & ECAL_layer_points,
+                              const std::vector<std::vector<TVector2>> & HCAL_layer_points,
+                              const PandoraPlus::TrackState & ECAL_trk_state, 
+                              PandoraPlus::Track* p_track);
+  // Get the radius rho 
+  float GetRho(const PandoraPlus::TrackState & trk_state);
+  // Get coordinates of the center of the circle
+  TVector2 GetCenterOfCircle(const PandoraPlus::TrackState & trk_state, const float & rho);
+  // phase from center to reference point
+  float GetRefAlpha0(const PandoraPlus::TrackState & trk_state, const TVector2 & center);
+  // If the charged particle return back 
+  bool IsReturn(float rho, TVector2 & center);
+
+  std::vector<std::vector<float>> GetDeltaPhi(float rho, TVector2 center, float alpha0,
+                                              const std::vector<TVector2> & normal_vectors,
+                                              const std::vector<std::vector<TVector2>> & layer_points, 
+                                              const PandoraPlus::TrackState & CALO_trk_state);
+  std::vector<TVector3> GetExtrapoPoints(std::string calo_name, 
+                                         float rho, TVector2 center, float alpha0, 
+                                         const PandoraPlus::TrackState & CALO_trk_state,
+                                         const std::vector<std::vector<float>>& delta_phi);
+
+    // Get phi0 of extrapolated points. Note that this phi0 is not same as the definition of the phi0 in TrackState, but will be stored in TrackState
+  float GetExtrapolatedPhi0(float Kappa, float ECAL_phi0, TVector2 center, TVector3 ext_point);
+  // To sort the extrapolatedpoints, define the following comparison function
+  static bool SortByPhi0(const PandoraPlus::TrackState& trk_state1, const PandoraPlus::TrackState& trk_state2 ) 
+  { return TMath::Abs(trk_state1.phi0) < TMath::Abs(trk_state2.phi0); }
+  
+private: 
+
+};
+#endif
diff --git a/Reconstruction/CrystalCaloRec/include/Algorithm/TrackMatchingAlg.h b/Reconstruction/CrystalCaloRec/include/Algorithm/TrackMatchingAlg.h
new file mode 100644
index 0000000000000000000000000000000000000000..78e65a0a142470f07cf8aee64402a52b0e0f7b2b
--- /dev/null
+++ b/Reconstruction/CrystalCaloRec/include/Algorithm/TrackMatchingAlg.h
@@ -0,0 +1,54 @@
+#ifndef TRACKMATCHING_H
+#define TRACKMATCHING_H
+
+#include "PandoraPlusDataCol.h"
+#include "Tools/Algorithm.h"
+#include "TVector3.h"
+using namespace PandoraPlus;
+
+class TrackMatchingAlg: public PandoraPlus::Algorithm{
+
+public:
+  TrackMatchingAlg () {};
+  ~TrackMatchingAlg() {};
+
+  class Factory : public PandoraPlus::AlgorithmFactory
+  {
+  public:
+    PandoraPlus::Algorithm* CreateAlgorithm() const{ return new TrackMatchingAlg(); }
+  };
+
+  StatusCode ReadSettings(PandoraPlus::Settings& m_settings);
+  StatusCode Initialize( PandoraPlusDataCol& m_datacol );
+  StatusCode RunAlgorithm( PandoraPlusDataCol& m_datacol );
+  StatusCode ClearAlgorithm();
+
+
+  StatusCode GetExtrpoECALPoints(const PandoraPlus::Track* track, std::vector<TVector3>& extrapo_points);
+  StatusCode CreateTrackAxis(vector<TVector3>& extrapo_points, std::vector<const PandoraPlus::Calo1DCluster*>& localMaxVCol,
+                             PandoraPlus::CaloHalfCluster* t_track_axis);
+  StatusCode GetNearby(const std::vector<std::shared_ptr<PandoraPlus::CaloHalfCluster>>* p_HalfClusterV, 
+                    const std::vector<TVector3>& extrapo_points, 
+                    std::vector<PandoraPlus::CaloHalfCluster*>& t_nearbyHalfClusters, 
+                    std::vector<const PandoraPlus::Calo1DCluster*>& t_nearbyLocalMax);
+  StatusCode LongiConeLinking(const std::vector<TVector3>& extrapo_points, 
+                            std::vector<const PandoraPlus::Calo1DCluster*>& nearbyLocalMax, 
+                            std::vector<const PandoraPlus::Calo1DCluster*>& cone_axis);
+  bool isStopLinking(const std::vector<TVector3>& extrapo_points, 
+                    const PandoraPlus::Calo1DCluster* final_cone_hit);
+  TVector2 GetProjectedRelR(const PandoraPlus::Calo1DCluster* m_shower1, const PandoraPlus::Calo1DCluster* m_shower2 );
+  TVector2 GetProjectedAxis(const std::vector<TVector3>& extrapo_points, const PandoraPlus::Calo1DCluster* m_shower);
+  StatusCode CreatConeAxis(PandoraPlusDataCol& m_datacol, PandoraPlus::Track* track, std::vector<PandoraPlus::CaloHalfCluster*>& nearbyHalfClusters, 
+                          std::vector<const PandoraPlus::Calo1DCluster*>& cone_axis);
+
+private:
+  std::vector<PandoraPlus::Track*> m_TrackCol;
+  std::vector<std::shared_ptr<PandoraPlus::CaloHalfCluster>>* p_HalfClusterV;
+  std::vector<std::shared_ptr<PandoraPlus::CaloHalfCluster>>* p_HalfClusterU;
+
+  // std::vector<const PandoraPlus::CaloHalfCluster*> m_trackAxisVCol;
+  // std::vector<const PandoraPlus::CaloHalfCluster*> m_trackAxisUCol;
+
+
+};
+#endif
diff --git a/Reconstruction/CrystalCaloRec/include/Algorithm/TruthClusterMergingAlg.h b/Reconstruction/CrystalCaloRec/include/Algorithm/TruthClusterMergingAlg.h
new file mode 100644
index 0000000000000000000000000000000000000000..baf9c15099e98458f1e8a53fb3a6cbbec553c972
--- /dev/null
+++ b/Reconstruction/CrystalCaloRec/include/Algorithm/TruthClusterMergingAlg.h
@@ -0,0 +1,35 @@
+#ifndef _TRUTHCLUSTERMERGING_ALG_H
+#define _TRUTHCLUSTERMERGING_ALG_H
+
+#include "Tools/Algorithm.h"
+
+using namespace PandoraPlus;
+class TruthClusterMergingAlg: public PandoraPlus::Algorithm{
+public: 
+
+  TruthClusterMergingAlg(){};
+  ~TruthClusterMergingAlg(){};
+
+  class Factory : public PandoraPlus::AlgorithmFactory
+  {
+  public: 
+    PandoraPlus::Algorithm* CreateAlgorithm() const{ return new TruthClusterMergingAlg(); } 
+
+  };
+
+  StatusCode ReadSettings(PandoraPlus::Settings& m_settings);
+  StatusCode Initialize( PandoraPlusDataCol& m_datacol );
+  StatusCode RunAlgorithm( PandoraPlusDataCol& m_datacol );
+  StatusCode ClearAlgorithm();
+
+
+private: 
+
+  std::vector<const PandoraPlus::Calo3DCluster*> m_EcalClusterCol;
+  std::vector<const PandoraPlus::Calo3DCluster*> m_HcalClusterCol;
+  std::vector<std::shared_ptr<PandoraPlus::Calo3DCluster>> merged_EcalClusterCol;
+  std::vector<std::shared_ptr<PandoraPlus::Calo3DCluster>> merged_HcalClusterCol;
+  std::vector<std::shared_ptr<PandoraPlus::PFObject>> merged_CombClusterCol;
+
+};
+#endif
diff --git a/Reconstruction/CrystalCaloRec/include/Algorithm/TruthClusteringAlg.h b/Reconstruction/CrystalCaloRec/include/Algorithm/TruthClusteringAlg.h
new file mode 100644
index 0000000000000000000000000000000000000000..24434fb80d316d8e8a53ed0e0d50bed71e22a8e1
--- /dev/null
+++ b/Reconstruction/CrystalCaloRec/include/Algorithm/TruthClusteringAlg.h
@@ -0,0 +1,46 @@
+#ifndef _TRUTHCLUS_ALG_H
+#define _TRUTHCLUS_ALG_H
+
+#include "Tools/Algorithm.h"
+
+using namespace PandoraPlus;
+class TruthClusteringAlg: public PandoraPlus::Algorithm{
+public: 
+
+  TruthClusteringAlg(){};
+  ~TruthClusteringAlg(){};
+
+  class Factory : public PandoraPlus::AlgorithmFactory
+  {
+  public: 
+    PandoraPlus::Algorithm* CreateAlgorithm() const{ return new TruthClusteringAlg(); } 
+
+  };
+
+  StatusCode ReadSettings(PandoraPlus::Settings& m_settings);
+  StatusCode Initialize( PandoraPlusDataCol& m_datacol );
+  StatusCode RunAlgorithm( PandoraPlusDataCol& m_datacol );
+  StatusCode ClearAlgorithm();
+
+  StatusCode HalfClusterToTowers( std::vector<std::shared_ptr<PandoraPlus::CaloHalfCluster>>& m_halfClusU,
+                                  std::vector<std::shared_ptr<PandoraPlus::CaloHalfCluster>>& m_halfClusV,
+                                  std::vector<std::shared_ptr<PandoraPlus::Calo3DCluster>>& m_towers );
+
+private: 
+
+  PandoraPlusDataCol m_bkCol;
+
+  std::vector<std::shared_ptr<PandoraPlus::Track>> m_TrackCol;
+  //For ECAL
+  std::vector<std::shared_ptr<PandoraPlus::CaloUnit>> m_bars;
+  std::vector<std::shared_ptr<PandoraPlus::Calo1DCluster>> m_1dclusterUCol;
+  std::vector<std::shared_ptr<PandoraPlus::Calo1DCluster>> m_1dclusterVCol;
+  std::vector<std::shared_ptr<PandoraPlus::CaloHalfCluster>> m_halfclusterU;
+  std::vector<std::shared_ptr<PandoraPlus::CaloHalfCluster>> m_halfclusterV;
+  std::vector<std::shared_ptr<PandoraPlus::Calo3DCluster>> m_towers;
+  //For HCAL
+  std::vector<std::shared_ptr<PandoraPlus::CaloHit>> m_hits;
+  std::vector<std::shared_ptr<PandoraPlus::Calo3DCluster>> m_clusters;
+
+};
+#endif
diff --git a/Reconstruction/CrystalCaloRec/include/Algorithm/TruthEnergySplittingAlg.h b/Reconstruction/CrystalCaloRec/include/Algorithm/TruthEnergySplittingAlg.h
new file mode 100644
index 0000000000000000000000000000000000000000..99a6190dc9fa61b73fb82a8629f1674ec3741c81
--- /dev/null
+++ b/Reconstruction/CrystalCaloRec/include/Algorithm/TruthEnergySplittingAlg.h
@@ -0,0 +1,39 @@
+#ifndef _TRUTHENERGYSPLITTING_ALG_H
+#define _TRUTHENERGYSPLITTING_ALG_H
+
+#include "Tools/Algorithm.h"
+
+using namespace PandoraPlus;
+class TruthEnergySplittingAlg: public PandoraPlus::Algorithm{
+public: 
+
+  TruthEnergySplittingAlg(){};
+  ~TruthEnergySplittingAlg(){};
+
+  class Factory : public PandoraPlus::AlgorithmFactory
+  {
+  public: 
+    PandoraPlus::Algorithm* CreateAlgorithm() const{ return new TruthEnergySplittingAlg(); } 
+
+  };
+
+  StatusCode ReadSettings(PandoraPlus::Settings& m_settings);
+  StatusCode Initialize( PandoraPlusDataCol& m_datacol );
+  StatusCode RunAlgorithm( PandoraPlusDataCol& m_datacol );
+  StatusCode ClearAlgorithm();
+
+  StatusCode HalfClusterToTowers( std::vector<std::shared_ptr<PandoraPlus::CaloHalfCluster>>& m_halfClusU,
+                                  std::vector<std::shared_ptr<PandoraPlus::CaloHalfCluster>>& m_halfClusV,
+                                  std::vector<std::shared_ptr<PandoraPlus::Calo3DCluster>>& m_towers );
+
+private: 
+  std::vector<PandoraPlus::CaloHalfCluster*> p_HalfClusterU;
+  std::vector<PandoraPlus::CaloHalfCluster*> p_HalfClusterV;
+
+  std::vector<std::shared_ptr<PandoraPlus::CaloHalfCluster>> m_newClusUCol;
+  std::vector<std::shared_ptr<PandoraPlus::CaloHalfCluster>> m_newClusVCol;
+  std::vector<std::shared_ptr<PandoraPlus::Calo3DCluster>> m_towerCol;
+
+  PandoraPlusDataCol m_bkCol;
+};
+#endif
diff --git a/Reconstruction/CrystalCaloRec/include/Algorithm/TruthMatchingAlg.h b/Reconstruction/CrystalCaloRec/include/Algorithm/TruthMatchingAlg.h
new file mode 100644
index 0000000000000000000000000000000000000000..f33dc4ff53ef328aedd8efc48e1f50f75e865047
--- /dev/null
+++ b/Reconstruction/CrystalCaloRec/include/Algorithm/TruthMatchingAlg.h
@@ -0,0 +1,52 @@
+#ifndef _TRUTHMATCHING_ALG_H
+#define _TRUTHMATCHING_ALG_H
+
+#include "Tools/Algorithm.h"
+
+using namespace PandoraPlus;
+class TruthMatchingAlg: public PandoraPlus::Algorithm{
+public: 
+
+  TruthMatchingAlg(){};
+  ~TruthMatchingAlg(){};
+
+  class Factory : public PandoraPlus::AlgorithmFactory
+  {
+  public: 
+    PandoraPlus::Algorithm* CreateAlgorithm() const{ return new TruthMatchingAlg(); } 
+
+  };
+
+  StatusCode ReadSettings(PandoraPlus::Settings& m_settings);
+  StatusCode Initialize( PandoraPlusDataCol& m_datacol );
+  StatusCode RunAlgorithm( PandoraPlusDataCol& m_datacol );
+  StatusCode ClearAlgorithm();
+
+  StatusCode TruthMatching( std::vector<const PandoraPlus::CaloHalfCluster*>& m_ClUCol,
+                            std::vector<const PandoraPlus::CaloHalfCluster*>& m_ClVCol,
+                            std::vector<std::shared_ptr<PandoraPlus::Calo3DCluster>>& m_clusters );
+
+  StatusCode XYClusterMatchingL0( const PandoraPlus::CaloHalfCluster* m_longiClX,
+                                  const PandoraPlus::CaloHalfCluster* m_longiClY,
+                                  std::shared_ptr<PandoraPlus::Calo3DCluster>& m_clus );
+
+  StatusCode GetMatchedShowersL0( const PandoraPlus::Calo1DCluster* barShowerX,
+                                  const PandoraPlus::Calo1DCluster* barShowerY,
+                                  PandoraPlus::Calo2DCluster* outsh); //1*1  
+
+  StatusCode GetMatchedShowersL1( const PandoraPlus::Calo1DCluster* shower1,
+                                  std::vector<const PandoraPlus::Calo1DCluster*>& showerNCol,
+                                  std::vector<PandoraPlus::Calo2DCluster*>& outshCol ); //1*N
+
+private: 
+
+  std::vector<PandoraPlus::Calo3DCluster*> m_towerCol;
+
+  std::vector<const PandoraPlus::CaloHalfCluster*> m_HFClusUCol;
+  std::vector<const PandoraPlus::CaloHalfCluster*> m_HFClusVCol;
+  std::vector<std::shared_ptr<PandoraPlus::Calo3DCluster>> m_clusterCol;
+
+  PandoraPlusDataCol m_bkCol;
+
+};
+#endif
diff --git a/Reconstruction/CrystalCaloRec/include/Algorithm/TruthPatternRecAlg.h b/Reconstruction/CrystalCaloRec/include/Algorithm/TruthPatternRecAlg.h
new file mode 100644
index 0000000000000000000000000000000000000000..e04e7be77d396cb7230c70413be0a8b396322e9a
--- /dev/null
+++ b/Reconstruction/CrystalCaloRec/include/Algorithm/TruthPatternRecAlg.h
@@ -0,0 +1,33 @@
+#ifndef _TRUTHPATTERNREC_ALG_H
+#define _TRUTHPATTERNREC_ALG_H
+
+#include "Tools/Algorithm.h"
+
+using namespace PandoraPlus;
+class TruthPatternRecAlg: public PandoraPlus::Algorithm{
+public: 
+
+  TruthPatternRecAlg(){};
+  ~TruthPatternRecAlg(){};
+
+  class Factory : public PandoraPlus::AlgorithmFactory
+  {
+  public: 
+    PandoraPlus::Algorithm* CreateAlgorithm() const{ return new TruthPatternRecAlg(); } 
+
+  };
+
+  StatusCode ReadSettings(PandoraPlus::Settings& m_settings);
+  StatusCode Initialize( PandoraPlusDataCol& m_datacol );
+  StatusCode RunAlgorithm( PandoraPlusDataCol& m_datacol );
+  StatusCode ClearAlgorithm();
+
+  StatusCode OverlapMerging   ( std::vector<std::shared_ptr<PandoraPlus::CaloHalfCluster>>& m_axisCol );
+
+private: 
+
+  std::vector<PandoraPlus::CaloHalfCluster*> p_HalfClusterV;
+  std::vector<PandoraPlus::CaloHalfCluster*> p_HalfClusterU;
+
+};
+#endif
diff --git a/Reconstruction/CrystalCaloRec/include/Algorithm/TruthTrackMatchingAlg.h b/Reconstruction/CrystalCaloRec/include/Algorithm/TruthTrackMatchingAlg.h
new file mode 100644
index 0000000000000000000000000000000000000000..580206ffb37e5fe972083169ee1bba95e5c60112
--- /dev/null
+++ b/Reconstruction/CrystalCaloRec/include/Algorithm/TruthTrackMatchingAlg.h
@@ -0,0 +1,37 @@
+#ifndef _TRUTHTRKMATCHING_ALG_H
+#define _TRUTHTRKMATCHING_ALG_H
+
+#include "Tools/Algorithm.h"
+
+using namespace PandoraPlus;
+class TruthTrackMatchingAlg: public PandoraPlus::Algorithm{
+public: 
+
+  TruthTrackMatchingAlg(){};
+  ~TruthTrackMatchingAlg(){};
+
+  class Factory : public PandoraPlus::AlgorithmFactory
+  {
+  public: 
+    PandoraPlus::Algorithm* CreateAlgorithm() const{ return new TruthTrackMatchingAlg(); } 
+
+  };
+
+  StatusCode ReadSettings(PandoraPlus::Settings& m_settings);
+  StatusCode Initialize( PandoraPlusDataCol& m_datacol );
+  StatusCode RunAlgorithm( PandoraPlusDataCol& m_datacol );
+  StatusCode ClearAlgorithm();
+
+  //Self defined algorithms
+  StatusCode SelfAlg1(); 
+
+private: 
+
+  std::vector<PandoraPlus::Track*> m_TrackCol;
+  std::vector<std::shared_ptr<PandoraPlus::CaloHalfCluster>>* p_HalfClusterV;
+  std::vector<std::shared_ptr<PandoraPlus::CaloHalfCluster>>* p_HalfClusterU;  
+
+
+
+};
+#endif
diff --git a/Reconstruction/CrystalCaloRec/include/Objects/Calo1DCluster.h b/Reconstruction/CrystalCaloRec/include/Objects/Calo1DCluster.h
new file mode 100644
index 0000000000000000000000000000000000000000..5b74ef4912c1ed7b6d695e9fea547a63e7f591c7
--- /dev/null
+++ b/Reconstruction/CrystalCaloRec/include/Objects/Calo1DCluster.h
@@ -0,0 +1,84 @@
+#ifndef CALO_1DCLUSTER_H
+#define CALO_1DCLUSTER_H
+#include <iostream>
+#include <cstdio>
+#include <algorithm>
+#include <cmath>
+#include <vector>
+
+#include "Objects/CaloUnit.h"
+
+namespace PandoraPlus{
+  class Calo1DCluster{
+  public: 
+
+    Calo1DCluster() {}; 
+    Calo1DCluster( std::vector<const PandoraPlus::CaloUnit*> _bars, std::vector<const PandoraPlus::CaloUnit*> _seeds)
+    : Bars(_bars), Seeds(_seeds) {};
+    ~Calo1DCluster() { Clear(); }
+
+    void Clear();
+    void Clean();
+    void Check();
+    std::shared_ptr<PandoraPlus::Calo1DCluster> Clone() const; 
+
+    inline bool operator == (const Calo1DCluster &x) const{
+      return ( Bars == x.getBars()  );
+    }
+
+    bool isNeighbor(const PandoraPlus::CaloUnit* m_bar) const; 
+    bool inCluster(const PandoraPlus::CaloUnit* iBar) const;
+    void sortByPos() { std::sort(Bars.begin(), Bars.end()); }
+
+    double getEnergy() const; 
+    TVector3 getPos() const;
+    double getT1() const;
+    double getT2() const;
+    double getWidth() const;
+    double getScndMoment() const;
+
+    int getNseeds() const { return Seeds.size(); }
+    std::vector<const PandoraPlus::CaloUnit*> getBars()  const { return Bars;  }
+	  std::vector<const PandoraPlus::CaloUnit*> getCluster()  const { return Bars;  }
+    std::vector<const PandoraPlus::CaloUnit*> getSeeds() const { return Seeds; }
+    std::vector< const PandoraPlus::Calo1DCluster* > getCousinClusters() const { return CousinClusters; }
+    std::vector< const PandoraPlus::Calo1DCluster* > getChildClusters() const { return ChildClusters; }
+    bool getGlobalRange( double& xmin,  double& ymin, double& zmin, double& xmax, double& ymax, double& zmax ) const;
+    int  getLeftEdge();
+    int  getRightEdge();
+    std::vector< std::pair<edm4hep::MCParticle, float> > getLinkedMCP() const { return MCParticleWeight; }
+    std::vector< std::pair<edm4hep::MCParticle, float> > getLinkedMCPfromUnit();
+    edm4hep::MCParticle getLeadingMCP() const;
+    float getLeadingMCPweight() const;
+
+	  void addUnit(const PandoraPlus::CaloUnit* _bar );
+    void addSeed(const PandoraPlus::CaloUnit* _seed ) { Seeds.push_back(_seed); }
+    void setBars( std::vector<const PandoraPlus::CaloUnit*> _bars ) { Bars = _bars; }
+    void setSeeds( std::vector<const PandoraPlus::CaloUnit*> _seeds) { Seeds = _seeds; }
+    void addCousinCluster( const PandoraPlus::Calo1DCluster* clus ) { CousinClusters.push_back(clus); }
+    void addChildCluster( const PandoraPlus::Calo1DCluster* clus ) { ChildClusters.push_back(clus); }
+    void deleteCousinCluster( const PandoraPlus::Calo1DCluster* _cl );
+    void addLinkedMCP( std::pair<edm4hep::MCParticle, float> _pair ) { MCParticleWeight.push_back(_pair); }
+    void setLinkedMCP( std::vector<std::pair<edm4hep::MCParticle, float>> _pairVec ) { MCParticleWeight.clear(); MCParticleWeight = _pairVec; }
+    void setSeed();  //Set the most energitic unit as seed, Eseed>5 MeV (hardcoded). 
+    void setIDInfo(); 
+
+    int getDlayer() const { if(Bars.size()>0) return Bars[0]->getDlayer(); return -99;  }
+    int getSlayer() const { if(Bars.size()>0) return Bars[0]->getSlayer(); return -99;  }
+    std::vector< std::vector<int> > getTowerID() const { return towerID; }
+    
+  private: 
+    std::vector<const PandoraPlus::CaloUnit*> Bars;
+    std::vector<const PandoraPlus::CaloUnit*> Seeds;
+    double Energy;
+    TVector3 pos;
+
+    std::vector< std::vector<int> > towerID; //[module, stave]
+
+    std::vector< const PandoraPlus::Calo1DCluster* > CousinClusters;
+    std::vector< const PandoraPlus::Calo1DCluster* > ChildClusters;
+
+    std::vector< std::pair<edm4hep::MCParticle, float> > MCParticleWeight;
+  };
+}
+#endif
diff --git a/Reconstruction/CrystalCaloRec/include/Objects/Calo2DCluster.h b/Reconstruction/CrystalCaloRec/include/Objects/Calo2DCluster.h
new file mode 100644
index 0000000000000000000000000000000000000000..88bdd2ae6854f9a6608cdce5920384743e85be7e
--- /dev/null
+++ b/Reconstruction/CrystalCaloRec/include/Objects/Calo2DCluster.h
@@ -0,0 +1,65 @@
+#ifndef CALO_2DCLUSTER_H
+#define CALO_2DCLUSTER_H
+
+#include "Objects/CaloHit.h"
+#include "Objects/CaloUnit.h"
+#include "Objects/Calo1DCluster.h"
+
+namespace PandoraPlus {
+
+  class CaloHit;
+  class Calo2DCluster {
+  public: 
+    Calo2DCluster() {};
+    ~Calo2DCluster() { Clear(); };
+
+    void Clear();
+    void ClearShower();
+    void Clean();
+    void Check(); 
+
+    inline bool operator == (const Calo2DCluster &x) const{
+      return ( barUCol == x.getBarUCol() &&   barVCol == x.getBarVCol());
+    }
+
+    bool isNeighbor(const PandoraPlus::Calo1DCluster* m_1dcluster) const; 
+
+    int getDlayer() const { if(barUCol.size()>0) return barUCol[0]->getDlayer(); else if(barVCol.size()>0) return barVCol[0]->getDlayer(); else return -99; }
+    std::vector< std::vector<int> > getTowerID() const { return towerID; }
+
+    std::vector<const CaloUnit*> getBars() const;
+    std::vector<const CaloHit*> getCaloHits() const { return hits; }
+    std::vector<const CaloUnit *> getBarUCol() const { return barUCol; }
+    std::vector<const CaloUnit *> getBarVCol() const { return barVCol; }
+    std::vector<const Calo1DCluster*> getShowerUCol() const {return barShowerUCol;}
+    std::vector<const Calo1DCluster*> getShowerVCol() const {return barShowerVCol;}
+    std::vector<const Calo1DCluster*> getCluster() const;
+    double getEnergy() const; 
+    TVector3 getPos() const; 
+
+    void setCaloHits( std::vector<const CaloHit*> _hits) { hits = _hits; }
+    void addBar(const CaloUnit* _bar) { if(_bar->getSlayer()==0) barUCol.push_back(_bar); if(_bar->getSlayer()==1) barVCol.push_back(_bar); }
+    void setBarUCol( std::vector<const CaloUnit*> _bars ) { barUCol=_bars; }
+    void setBarVCol( std::vector<const CaloUnit*> _bars ) { barVCol=_bars; }
+    void addShowerU( const Calo1DCluster* _sh) { barShowerUCol.push_back(_sh); }
+    void addShowerV( const Calo1DCluster* _sh) { barShowerVCol.push_back(_sh); }
+    void setShowerUCol(std::vector<const Calo1DCluster*> _sh) { barShowerUCol=_sh; }
+    void setShowerVCol(std::vector<const Calo1DCluster*> _sh) { barShowerVCol=_sh; }
+    void addUnit(const Calo1DCluster* _1dcluster);
+    void addTowerID(int _m, int _p, int _s) { std::vector<int> id(3); id[0] = _m; id[1] = _p; id[2] = _s; towerID.push_back(id); }
+    void addTowerID(std::vector<int> id) { towerID.push_back(id); }
+    void setTowerID(std::vector<int> id) { towerID.clear(); towerID.push_back(id); }
+
+  private:
+    std::vector< std::vector<int> > towerID; //[module, stave]
+
+    std::vector<const CaloHit*> hits; 
+    std::vector<const CaloUnit*> barUCol;  //slayer == 0.
+    std::vector<const CaloUnit*> barVCol;  //slayer == 1.
+    std::vector<const Calo1DCluster*> barShowerUCol; 
+    std::vector<const Calo1DCluster*> barShowerVCol; 
+
+  };
+
+};
+#endif
diff --git a/Reconstruction/CrystalCaloRec/include/Objects/Calo3DCluster.h b/Reconstruction/CrystalCaloRec/include/Objects/Calo3DCluster.h
new file mode 100644
index 0000000000000000000000000000000000000000..5036e0257586be8cae4391cfc9ef0c03a42c34cc
--- /dev/null
+++ b/Reconstruction/CrystalCaloRec/include/Objects/Calo3DCluster.h
@@ -0,0 +1,116 @@
+#ifndef CALO_3DCLUSTER_H
+#define CALO_3DCLUSTER_H
+
+#include "Objects/Calo2DCluster.h"
+#include "Objects/Track.h"
+#include "Objects/CaloHalfCluster.h"
+//#include "Objects/CaloTower.h"
+#include "Tools/TrackFitInEcal.h"
+
+namespace PandoraPlus {
+
+  class Calo3DCluster {
+  public: 
+    Calo3DCluster() {};
+    ~Calo3DCluster() { Clear(); };
+
+    void Clear();
+    void Clean();
+    void Check(); 
+    void Clear2DClusters() { m_2dclusters.clear(); };
+    void CleanLongiClusters();
+    std::shared_ptr<PandoraPlus::Calo3DCluster> Clone() const;
+
+    inline bool operator == (const Calo3DCluster &x) const{
+      return ( m_2dclusters == x.getCluster()  );
+    }
+
+    std::vector< std::vector<int> > getTowerID() const { return towerID; }
+    //bool isNeighbor(const PandoraPlus::Calo2DCluster* m_2dcluster) const; 
+    std::vector<const PandoraPlus::CaloHit*> getCaloHits() const { return hits; }
+    std::vector<const Calo2DCluster*> getCluster() const { return m_2dclusters; }
+    std::vector<const Calo1DCluster*> getLocalMaxUCol(std::string name) const;
+    std::vector<const Calo1DCluster*> getLocalMaxVCol(std::string name) const;
+    std::map<std::string, std::vector<const PandoraPlus::Calo1DCluster*> >  getLocalMaxUMap() const { return map_localMaxU; }
+    std::map<std::string, std::vector<const PandoraPlus::Calo1DCluster*> >  getLocalMaxVMap() const { return map_localMaxV; }
+    std::vector<const CaloHalfCluster*> getHalfClusterUCol(std::string name) const;
+    std::vector<const CaloHalfCluster*> getHalfClusterVCol(std::string name) const; 
+    std::map<std::string, std::vector<const PandoraPlus::CaloHalfCluster*> > getHalfClusterUMap() const { return map_halfClusUCol; }
+    std::map<std::string, std::vector<const PandoraPlus::CaloHalfCluster*> > getHalfClusterVMap() const { return map_halfClusVCol; }
+    std::vector<const PandoraPlus::Track*> getAssociatedTracks() const { return m_TrackCol; }
+    std::vector< std::pair<edm4hep::MCParticle, float> > getLinkedMCP() const { return MCParticleWeight; }
+    std::vector< std::pair<edm4hep::MCParticle, float> > getLinkedMCPfromHFCluster(std::string name);
+    std::vector< std::pair<edm4hep::MCParticle, float> > getLinkedMCPfromHit();
+    edm4hep::MCParticle getLeadingMCP() const;
+    float getLeadingMCPweight() const;
+
+    std::vector<const Calo3DCluster*> getTowers() const {return m_towers; }
+    std::vector<const CaloUnit*> getBars() const;
+    double getEnergy() const; 
+    double getHitsE() const;
+    double getLongiE() const;
+    TVector3 getHitCenter() const;
+    TVector3 getShowerCenter() const; 
+    TVector3 getAxis() const { return axis; }
+    int getBeginningDlayer() const;
+    int getEndDlayer() const;
+    double getDepthToECALSurface() const;
+    int getType() const { return type; }
+
+    void setCaloHits( std::vector<const PandoraPlus::CaloHit*> _hits ) { hits = _hits; }
+    void setCaloHitsFrom2DCluster(); 
+    void setTowers(std::vector<const Calo3DCluster*> _t) { m_towers = _t; }
+    void setHalfClusters( std::string name1, std::vector<const CaloHalfCluster*>& _clU, 
+                           std::string name2, std::vector<const CaloHalfCluster*>& _clV )
+    { map_halfClusUCol[name1]=_clU; map_halfClusVCol[name2]=_clV;  }
+
+    void setLocalMax( std::string name1, std::vector<const Calo1DCluster*>& _colU,
+                      std::string name2, std::vector<const Calo1DCluster*>& _colV )
+    { map_localMaxU[name1]=_colU; map_localMaxV[name2]=_colV; }
+    void setClusters(std::vector<const Calo2DCluster*> _2dcol) { m_2dclusters = _2dcol; }
+    void setType(int _type) { type = _type; }
+
+    void addTowerID(int _m, int _p, int _s) { std::vector<int> id(3); id[0] = _m; id[1] = _p; id[2] = _s; towerID.push_back(id); }
+    void addTowerID( std::vector<int> id ) { towerID.push_back(id); }
+
+    void addUnit(const Calo2DCluster* _2dcluster);
+    void addHit(const PandoraPlus::CaloHit* _hit) { hits.push_back(_hit); };
+    void addTower( const Calo3DCluster* _tower ) { m_towers.push_back(_tower); }
+    void addHalfClusterU( std::string name, const CaloHalfCluster* _clU ) { map_halfClusUCol[name].push_back(_clU); }
+    void addHalfClusterV( std::string name, const CaloHalfCluster* _clV ) { map_halfClusVCol[name].push_back(_clV); }
+    void addLocalMaxU( std::string name, const Calo1DCluster* _shU ) { map_localMaxU[name].push_back(_shU); }
+    void addLocalMaxV( std::string name, const Calo1DCluster* _shV ) { map_localMaxV[name].push_back(_shV); }
+    void addAssociatedTrack(const PandoraPlus::Track* _track){ m_TrackCol.push_back(_track); }
+    void addLinkedMCP( std::pair<edm4hep::MCParticle, float> _pair ) { MCParticleWeight.push_back(_pair); }
+    void setLinkedMCP( std::vector<std::pair<edm4hep::MCParticle, float>> _pairVec ) { MCParticleWeight.clear(); MCParticleWeight = _pairVec; }
+    void mergeCluster( const PandoraPlus::Calo3DCluster* _clus );
+
+    //void FitProfile();
+    void FitAxis();
+    //void FitAxisHit();
+
+    bool isHCALNeighbor(const PandoraPlus::CaloHit* m_hit) const;
+
+  private:
+    std::vector<const PandoraPlus::CaloHit*> hits; 
+    std::vector<const PandoraPlus::Calo2DCluster*> m_2dclusters;
+    std::vector<const Calo3DCluster*> m_towers;
+    std::map<std::string, std::vector<const PandoraPlus::Calo1DCluster*> > map_localMaxU;
+    std::map<std::string, std::vector<const PandoraPlus::Calo1DCluster*> > map_localMaxV;
+    std::map<std::string, std::vector<const PandoraPlus::CaloHalfCluster*> > map_halfClusUCol;
+    std::map<std::string, std::vector<const PandoraPlus::CaloHalfCluster*> > map_halfClusVCol;
+    std::vector<const PandoraPlus::Track*> m_TrackCol;
+    std::vector< std::pair<edm4hep::MCParticle, float> > MCParticleWeight;
+
+    std::vector< std::vector<int> > towerID; //[module, part, stave]
+    TVector3 axis;
+    double chi2;
+    double alpha;
+    double beta;
+    int    type;  //0: MIP shower.  1: EM shower.  2: hadronic shower
+    TrackFitInEcal trackFitter;
+
+  };
+
+};
+#endif
diff --git a/Reconstruction/CrystalCaloRec/include/Objects/CaloHalfCluster.h b/Reconstruction/CrystalCaloRec/include/Objects/CaloHalfCluster.h
new file mode 100644
index 0000000000000000000000000000000000000000..116efd047e4925686f50556565b0fbd7213690d6
--- /dev/null
+++ b/Reconstruction/CrystalCaloRec/include/Objects/CaloHalfCluster.h
@@ -0,0 +1,108 @@
+#ifndef CALO_HALFCLUSTER_H
+#define CALO_HALFCLUSTER_H
+
+#include "Objects/CaloUnit.h"
+#include "Objects/Calo1DCluster.h"
+#include "Objects/Track.h"
+#include "Tools/TrackFitInEcal.h"
+
+namespace PandoraPlus {
+
+  class Track;
+  class CaloHalfCluster {
+  public: 
+    CaloHalfCluster() {};
+    ~CaloHalfCluster() { Clear(); };
+
+    void Clear();
+    void Clean();
+    void Check(); 
+
+    inline bool operator == (const CaloHalfCluster &x) const{
+      return m_1dclusters==x.getCluster();
+    }
+    std::shared_ptr<PandoraPlus::CaloHalfCluster> Clone() const; 
+
+    bool isNeighbor(const PandoraPlus::Calo1DCluster* m_1dcluster) const; 
+
+    double getEnergy() const; 
+    TVector3 getPos() const; 
+    TVector3 getAxis() const { return axis; }
+    TVector3 getEnergyCenter() const;
+    std::vector<int> getEnergyCenterTower() const;
+    int getSlayer() const { return slayer; }
+    std::vector< std::vector<int> > getTowerID() const { return towerID; }
+    double getHoughAlpha() const { return Hough_alpha; }
+    double getHoughRho() const { return Hough_rho; }
+    double getHoughIntercept() const { return Hough_intercept; }
+    int getType() const { return type; }
+
+    std::vector<const CaloUnit*> getBars() const;
+    std::vector<const Calo1DCluster*> getCluster() const { return m_1dclusters;};
+    std::vector<const Calo1DCluster*> getLocalMaxCol(std::string name) const;
+    std::vector<const Calo1DCluster*> getAllLocalMaxCol() const;
+    std::vector<const Calo1DCluster*> getClusterInLayer(int _layer) const;
+    std::vector<const CaloHalfCluster*> getHalfClusterCol(std::string name) const;
+    std::vector<const CaloHalfCluster*> getAllHalfClusterCol() const;
+    std::map<std::string, std::vector<const PandoraPlus::CaloHalfCluster*> > getHalfClusterMap() const {return map_halfClusCol; }
+    std::map<std::string, std::vector<const PandoraPlus::Calo1DCluster*> > getLocalMaxMap() const {return map_localMax; }
+    std::vector<const PandoraPlus::Track*> getAssociatedTracks() const { return m_TrackCol; }
+    std::vector< std::pair<edm4hep::MCParticle, float> > getLinkedMCP() const { return MCParticleWeight; }
+    std::vector< std::pair<edm4hep::MCParticle, float> > getLinkedMCPfromUnit();
+    edm4hep::MCParticle getLeadingMCP() const; 
+    float getLeadingMCPweight() const;
+
+    int getBeginningDlayer() const;
+    int getEndDlayer() const;
+    bool isContinue() const;
+    bool isContinueN(int n) const;
+    bool isSubset(const CaloHalfCluster* clus) const;
+    double OverlapRatioE( const CaloHalfCluster* clus ) const;
+
+    void fitAxis( std::string name );
+    void setType( int _type ) { type = _type; }
+    void sortBarShowersByLayer() { std::sort(m_1dclusters.begin(), m_1dclusters.end(), compLayer); }
+    void addUnit(const Calo1DCluster* _1dcluster);
+    void deleteUnit(const Calo1DCluster* _1dcluster);
+    void setLocalMax( std::string name, std::vector<const Calo1DCluster*> _col) { map_localMax[name]=_col; }
+    void setHalfClusters( std::string name, std::vector<const PandoraPlus::CaloHalfCluster*>& _cl) { map_halfClusCol[name]=_cl; }
+    void addHalfCluster(std::string name, const PandoraPlus::CaloHalfCluster* _cl) { map_halfClusCol[name].push_back(_cl); }
+    void addCousinCluster( const PandoraPlus::CaloHalfCluster* _cl ) { map_halfClusCol["CousinCluster"].push_back(_cl); }
+    void deleteCousinCluster( const PandoraPlus::CaloHalfCluster* _cl ); 
+    void setHoughPars(double _a, double _r) { Hough_alpha=_a; Hough_rho=_r; }
+    void setIntercept(double _in) { Hough_intercept=_in; }
+    void mergeHalfCluster( const CaloHalfCluster* clus );
+    void addTowerID(int _m, int _p, int _s) { std::vector<int> id(3); id[0] = _m; id[1] = _p; id[2] = _s; towerID.push_back(id); }
+    void addTowerID(std::vector<int> id) { towerID.push_back(id); }
+    void setTowerID(std::vector<int> id) { towerID.clear(); towerID.push_back(id); }
+    void addAssociatedTrack(const PandoraPlus::Track* _track){ m_TrackCol.push_back(_track); }
+    void addLinkedMCP( std::pair<edm4hep::MCParticle, float> _pair ) { MCParticleWeight.push_back(_pair); }
+    void setLinkedMCP( std::vector<std::pair<edm4hep::MCParticle, float>> _pairVec ) { MCParticleWeight.clear(); MCParticleWeight = _pairVec; }
+    void mergeClusterInLayer(); 
+
+
+  private:
+    int type; // yyy: new definition: track: 10000, Hough: 100, cone: 1, merge: sum them
+    std::vector< std::vector<int> > towerID; //[module, part, stave]
+    int slayer;
+    TVector3 axis;
+    double trk_dr;
+    double trk_dz;
+    double Hough_alpha;
+    double Hough_rho;
+    double Hough_intercept;
+    std::vector<const Calo1DCluster*> m_1dclusters; 
+    std::map<std::string, std::vector<const PandoraPlus::Calo1DCluster*> > map_localMax;
+    std::map<std::string, std::vector<const PandoraPlus::CaloHalfCluster*> > map_halfClusCol;  
+    std::vector<const PandoraPlus::Track*> m_TrackCol;
+    std::vector< std::pair<edm4hep::MCParticle, float> > MCParticleWeight;
+
+    TrackFitInEcal* track = new TrackFitInEcal();
+
+    static bool compLayer( const PandoraPlus::Calo1DCluster* hit1, const PandoraPlus::Calo1DCluster* hit2 )
+      { return hit1->getDlayer() < hit2->getDlayer(); }
+
+  };
+
+};
+#endif
diff --git a/Reconstruction/CrystalCaloRec/include/Objects/CaloHit.h b/Reconstruction/CrystalCaloRec/include/Objects/CaloHit.h
new file mode 100644
index 0000000000000000000000000000000000000000..ecb6565c149474d03acb81f4e1cd3d0d77c210e0
--- /dev/null
+++ b/Reconstruction/CrystalCaloRec/include/Objects/CaloHit.h
@@ -0,0 +1,52 @@
+#ifndef CALO_HIT_H
+#define CALO_HIT_H
+
+#include "TVector3.h"
+#include "edm4hep/MCParticle.h"
+#include "edm4hep/CalorimeterHit.h"
+#include "Objects/Calo2DCluster.h"
+
+namespace PandoraPlus{
+  class Calo2DCluster; 
+
+  class CaloHit{
+  public:
+    CaloHit() {};
+    ~CaloHit() { Clear(); };
+
+    void Clear() { cellID=0; position.SetXYZ(0.,0.,0.); energy=-1; module=-1; layer=-1; ParentShower=nullptr; }
+    std::shared_ptr<CaloHit> Clone() const;
+
+    void setOriginHit( edm4hep::CalorimeterHit& _hit ) { m_hit = _hit; }
+    TVector3 getPosition() const { return position; }
+    double   getEnergy() const { return energy; } 
+    int getLayer() const {return layer;}
+    int getModule() const {return module;}
+    std::vector< std::pair<edm4hep::MCParticle, float> > getLinkedMCP() const { return MCParticleWeight; }
+    edm4hep::MCParticle getLeadingMCP() const;
+    float getLeadingMCPweight() const;
+    edm4hep::CalorimeterHit getOriginHit() const { return m_hit; }
+
+    void setcellID(unsigned long long _id) { cellID=_id; }
+    void setcellID(int _m, int _l) { module=_m; layer=_l; }
+    void setEnergy(double _en) { energy=_en; }
+    void setPosition( TVector3 _vec ) { position=_vec; }
+    void setModule(int _m ) { module = _m; }
+    void setLayer(int _l) { layer = _l; }
+    void setParentShower( PandoraPlus::Calo2DCluster* _p ) { ParentShower=_p; }
+    void addLinkedMCP( std::pair<edm4hep::MCParticle, float> _pair ) {MCParticleWeight.push_back(_pair); } 
+    void setLinkedMCP( std::vector<std::pair<edm4hep::MCParticle, float>> _pairVec ) { MCParticleWeight.clear(); MCParticleWeight = _pairVec; }
+
+  private: 
+    int module; 
+    int layer; 
+    unsigned long long cellID; 
+    TVector3 position;
+    double   energy; 
+    edm4hep::CalorimeterHit m_hit;
+    PandoraPlus::Calo2DCluster* ParentShower; 
+    std::vector< std::pair<edm4hep::MCParticle, float> > MCParticleWeight;
+  };
+
+};
+#endif
diff --git a/Reconstruction/CrystalCaloRec/include/Objects/CaloUnit.h b/Reconstruction/CrystalCaloRec/include/Objects/CaloUnit.h
new file mode 100644
index 0000000000000000000000000000000000000000..96c5d89c002f6468aea06883c3d95dcfd6d4e98d
--- /dev/null
+++ b/Reconstruction/CrystalCaloRec/include/Objects/CaloUnit.h
@@ -0,0 +1,101 @@
+#ifndef CALOBAR_H
+#define CALOBAR_H
+
+#include <DD4hep/Objects.h>
+#include "edm4hep/MCParticle.h"
+#include "TVector3.h"
+
+namespace PandoraPlus{
+
+  class CaloUnit{
+
+  public:
+    CaloUnit(unsigned long long _cellID, int _system, int _module, int _slayer, int _dlayer, int _stave, int _bar, TVector3 _pos, double _Q1, double _Q2, double _T1, double _T2)
+    : cellID(_cellID), system(_system), module(_module), stave(_stave), dlayer(_dlayer), slayer(_slayer), bar(_bar), position(_pos), Q1(_Q1), Q2(_Q2), T1(_T1), T2(_T2) {}; 
+    CaloUnit() {};
+    ~CaloUnit() { Clear(); };
+    void Clear() { position.SetXYZ(0.,0.,0.); Q1=-1; Q2=-1; T1=-99; T2=-99; }
+
+    inline bool operator < (const CaloUnit &x) const {
+      if(x.cellID==cellID) return false; 
+
+      if(slayer==0) return ( (stave<x.stave) || (stave==x.stave && bar<x.bar) );
+      else{
+        if( module==Nmodule && x.module==0 ) return true;
+        else if( module==0 && x.module==Nmodule ) return false; 
+        else{
+          return ( (module<x.module) || (module==x.module && bar<x.bar) );
+        }
+      }
+    }
+
+    inline bool operator == (const CaloUnit &x) const{
+      return ( (cellID == x.cellID) && getEnergy()==x.getEnergy() );
+    }
+    unsigned long long getcellID() const { return cellID; }
+    int getSystem() const { return system; }
+    int getModule() const { return module; }
+    int getStave()  const { return stave;  }
+    int getDlayer() const { return dlayer; }
+    int getSlayer() const { return slayer; }
+    int getBar()    const { return bar;    }
+    double getQ1()  const { return Q1;     }
+    double getQ2()  const { return Q2;     }
+    double getT1()  const { return T1;     }
+    double getT2()  const { return T2;     }
+    double getBarLength() const {return barLength; }
+
+    TVector3 getPosition() const { return position; }
+    double getEnergy() const { return (Q1+Q2); }
+    std::vector< std::pair<edm4hep::MCParticle, float> > getLinkedMCP() const { return MCParticleWeight; }
+    edm4hep::MCParticle getLeadingMCP() const;
+    float getLeadingMCPweight() const;
+    bool isAtLowerEdgePhi() const; 
+    bool isAtUpperEdgePhi() const; 
+    bool isAtLowerEdgeZ() const; 
+    bool isAtUpperEdgeZ() const; 
+    bool isNeighbor(const CaloUnit* x) const;
+    //bool isModuleAdjacent( const CaloUnit* x ) const;
+    bool isLongiNeighbor(const CaloUnit* x) const;
+    //bool isLongiModuleAdjacent( const CaloUnit* x ) const;
+
+    void addLinkedMCP( std::pair<edm4hep::MCParticle, float> _pair ) { MCParticleWeight.push_back(_pair); }
+    void setLinkedMCP( std::vector<std::pair<edm4hep::MCParticle, float>> _pairVec ) { MCParticleWeight.clear(); MCParticleWeight = _pairVec; }
+    void setcellID(unsigned long long _cellid) { cellID = _cellid; }
+    void setcellID(int _system, int _module, int _stave, int _dlayer, int _slayer, int _bar) { system=_system; module=_module; stave=_stave; dlayer=_dlayer; slayer=_slayer; bar=_bar; }
+    void setPosition( TVector3 posv3) { position.SetXYZ( posv3.x(), posv3.y(), posv3.z() ); }
+    void setQ(double _q1, double _q2) { Q1=_q1; Q2=_q2; }
+    void setT(double _t1, double _t2) { T1=_t1; T2=_t2; }
+    void setBarLength(double _barLength) { barLength=_barLength; }
+    std::shared_ptr<CaloUnit> Clone() const;
+
+    static int Nmodule;
+    static int Nstave;
+    static int Nlayer;
+    static int NbarPhi_odd[14];
+    static int NbarPhi_even[14];
+    static int NbarZ;
+    static float barsize;
+    static float ecal_innerR;
+
+  private:
+		unsigned long long cellID;
+		int system;
+		int module;
+		int stave;
+		int dlayer;
+		int slayer;
+		int bar;
+		TVector3 position;
+		double Q1;      // Q in left readout
+		double Q2;      // Q in right readout;
+		double T1;    // T in left readout;
+		double T2;    // T in right readout;
+
+    double barLength;
+    std::vector< std::pair<edm4hep::MCParticle, float> > MCParticleWeight; 
+
+  };
+  
+}
+#endif
diff --git a/Reconstruction/CrystalCaloRec/include/Objects/HoughObject.h b/Reconstruction/CrystalCaloRec/include/Objects/HoughObject.h
new file mode 100644
index 0000000000000000000000000000000000000000..03a407ec259aec18ffaa8da887edf3ede8ef4a78
--- /dev/null
+++ b/Reconstruction/CrystalCaloRec/include/Objects/HoughObject.h
@@ -0,0 +1,60 @@
+#ifndef CALOHOUGHOBJECT_H
+#define CALOHOUGHOBJECT_H
+
+#include "Objects/Calo1DCluster.h"
+#include "TVector2.h"
+#include "TF1.h"
+
+
+namespace PandoraPlus {
+
+  class HoughObject{
+  public:
+    HoughObject() {};
+    HoughObject( const PandoraPlus::Calo1DCluster* _localmax, double _cellSize, double _ecal_inner_radius, double _phi=0.);
+    ~HoughObject() { };
+
+
+    TVector2 getCenterPoint() const { return m_center_point; }
+    TVector2 getUpperPoint() const { return m_center_point + TVector2( m_cell_size*cos(m_center_point.Phi() + TMath::PiOver2()), m_cell_size*sin(m_center_point.Phi() + TMath::PiOver2()) ); }
+    TVector2 getLowerPoint() const { return m_center_point + TVector2( m_cell_size*cos(m_center_point.Phi() + 3*TMath::PiOver2()), m_cell_size*sin(m_center_point.Phi() + 3*TMath::PiOver2()) ); }
+    //TVector2 getPointU()  const { return m_center_point + TVector2(0,  m_cell_size/TMath::Sqrt(2)); }
+    //TVector2 getPointD()  const { return m_center_point + TVector2(0, -m_cell_size/TMath::Sqrt(2)); }
+    //TVector2 getPointL()  const { return m_center_point + TVector2(-m_cell_size/TMath::Sqrt(2), 0); }
+    //TVector2 getPointR()  const { return m_center_point + TVector2( m_cell_size/TMath::Sqrt(2), 0); }
+    //TVector2 getPointUR() const { return m_center_point + TVector2( m_cell_size/2,  m_cell_size/2); }
+    //TVector2 getPointDL() const { return m_center_point + TVector2(-m_cell_size/2, -m_cell_size/2); }
+    //TVector2 getPointUL() const { return m_center_point + TVector2(-m_cell_size/2,  m_cell_size/2); }
+    //TVector2 getPointDR() const { return m_center_point + TVector2( m_cell_size/2, -m_cell_size/2); }
+
+    TF1 getHoughLine1() const { return m_Hough_line_1; }
+    TF1 getHoughLine2() const { return m_Hough_line_2; }
+    //TF1 getHoughLine3() const { return m_Hough_line_3; }
+    //TF1 getHoughLine4() const { return m_Hough_line_4; }
+
+    //int getModule() const { return (m_local_max->getTowerID())[0][0]; }
+    int getSlayer() const { return m_local_max->getSlayer(); }
+    double getE() const { return m_local_max->getEnergy(); }
+    double getCellSize() const { return m_cell_size; }
+    const PandoraPlus::Calo1DCluster* getLocalMax() const { return m_local_max; }
+
+    void setCellSize(double _cs) { m_cell_size=_cs; }
+    void setCenterPoint(double& _ecal_inner_radius, double _phi=0.);
+    void setHoughLine(TF1& line1, TF1& line2);
+
+
+  private:
+    const PandoraPlus::Calo1DCluster* m_local_max;  //Local max
+    double m_cell_size;
+    TVector2 m_center_point;  // center position
+
+    TF1 m_Hough_line_1;    // ur or u
+    TF1 m_Hough_line_2;    // dl or d
+    //TF1 m_Hough_line_3;    // ul or l
+    //TF1 m_Hough_line_4;    // dr or r
+    // The above conversion is only for the octagon barrel ECAL
+
+  };
+
+};
+#endif
diff --git a/Reconstruction/CrystalCaloRec/include/Objects/HoughSpace.h b/Reconstruction/CrystalCaloRec/include/Objects/HoughSpace.h
new file mode 100644
index 0000000000000000000000000000000000000000..3c3e284826c29466881af307cfbbe991cc07337a
--- /dev/null
+++ b/Reconstruction/CrystalCaloRec/include/Objects/HoughSpace.h
@@ -0,0 +1,60 @@
+#ifndef CALOHOUGHSPACE_H
+#define CALOHOUGHSPACE_H
+
+#include<map>
+#include<set>
+#include "TH2.h"
+
+using namespace std;
+
+namespace PandoraPlus {
+  class HoughSpace{
+  public:
+    HoughSpace(){};
+    HoughSpace(double _alpha_low, double _alpha_high, double _bin_width_alpha, int _Nbins_alpha, double _rho_low, double _rho_high, double _bin_width_rho, int _Nbins_rho){
+      alpha_low = _alpha_low;
+      alpha_high = _alpha_high;
+      bin_width_alpha = _bin_width_alpha;
+      Nbins_alpha = _Nbins_alpha;
+      rho_low = _rho_low;
+      rho_high = _rho_high;
+      bin_width_rho = _bin_width_rho;
+      Nbins_rho = _Nbins_rho;
+    };
+    ~HoughSpace() {};
+
+     // Functions
+    int getAlphaBin(double alpha) const;
+    double getAlphaBinCenter(int alpha_bin) const;
+    double getAlphaBinLowEdge(int alpha_bin) const;
+    double getAlphaBinUpEdge(int alpha_bin) const;
+    int getRhoBin(double rho) const;
+    double getRhoBinCenter(int rho_bin) const;
+    double getRhoBinLowEdge(int rho_bin) const;
+    double getRhoBinUpEdge(int rho_bin) const;
+
+    void AddBinHobj(int bin_alpha, int bin_rho, int index_Hobj);
+    map< pair<int, int>, set<int> > getHoughBins() const { return Hough_bins; }
+
+  private:
+    double alpha_low;
+    double alpha_high;
+    double bin_width_alpha;
+    int Nbins_alpha;
+
+    double rho_low;
+    double rho_high;
+    double bin_width_rho;
+    int Nbins_rho;
+
+    map< pair<int, int>, set<int> > Hough_bins;
+    // description of the above map:
+    // key: the pair represents index of alpha bin and rho bin. All start from 1 to Nbins
+    // value: a set of index of HoughObject
+    // With this map, we know the relationship of bin in Hough space and Hough object
+
+  };
+
+
+};
+#endif
diff --git a/Reconstruction/CrystalCaloRec/include/Objects/PFObject.h b/Reconstruction/CrystalCaloRec/include/Objects/PFObject.h
new file mode 100644
index 0000000000000000000000000000000000000000..889d3d058d6735d07a7eb01e5b3c2b2e48feda6f
--- /dev/null
+++ b/Reconstruction/CrystalCaloRec/include/Objects/PFObject.h
@@ -0,0 +1,44 @@
+#ifndef PFOBJECT_H
+#define PFOBJECT_H
+
+#include "Objects/Calo3DCluster.h"
+#include "Objects/Track.h"
+
+namespace PandoraPlus{
+  class PFObject{
+  public:
+    PFObject () {};
+    ~PFObject() { Clear(); }
+
+    void Clear();
+    std::shared_ptr<PandoraPlus::PFObject> Clone() const; 
+
+    void addTrack(const Track* _track);
+    void addECALCluster(const Calo3DCluster* _ecal_cluster);
+    void addHCALCluster(const Calo3DCluster* _hcal_cluster);
+
+    void setPID(int _pid) { m_pid = _pid; }
+    void setTrack(std::vector<const Track*> _trkCol) { m_tracks = _trkCol; } 
+    void setECALCluster( std::vector<const Calo3DCluster*> _cluCol ) { m_ecal_clusters = _cluCol; }
+    void setHCALCluster( std::vector<const Calo3DCluster*> _cluCol ) { m_hcal_clusters = _cluCol; }
+
+    std::vector<const Track*> getTracks() const { return m_tracks; }
+    std::vector<const Calo3DCluster*> getECALClusters() const { return m_ecal_clusters; }
+    std::vector<const Calo3DCluster*> getHCALClusters() const { return m_hcal_clusters; }
+
+    int getPID() const { return m_pid; }
+    double getECALClusterEnergy() const;
+    double getHCALClusterEnergy() const;
+    double getTrackMomentum() const;
+
+
+  private:
+    int m_pid; 
+    std::vector<const Track*> m_tracks;
+    std::vector<const Calo3DCluster*> m_ecal_clusters;
+    std::vector<const Calo3DCluster*> m_hcal_clusters;
+
+  };
+
+};
+#endif
diff --git a/Reconstruction/CrystalCaloRec/include/Objects/Track.h b/Reconstruction/CrystalCaloRec/include/Objects/Track.h
new file mode 100644
index 0000000000000000000000000000000000000000..49dc55e2109ed9754a51f688ef94ca55a77da2a8
--- /dev/null
+++ b/Reconstruction/CrystalCaloRec/include/Objects/Track.h
@@ -0,0 +1,58 @@
+#ifndef _TRACK_H
+#define _TRACK_H
+
+#include "edm4hep/Track.h"
+#include "Objects/TrackState.h"
+#include "Objects/CaloHalfCluster.h"
+#include "TVector3.h"
+
+namespace PandoraPlus {
+
+  class CaloHalfCluster;
+  class Track{
+  public:
+    Track() {}
+    ~Track() { Clear(); }; 
+  void Clear() { m_trackStates.clear(); m_halfClusterUCol.clear(); m_halfClusterVCol.clear(); }
+
+  void setOriginTrack(edm4hep::Track& _trk) { m_track = _trk; }
+  int trackStates_size(std::string name) const;
+  int trackStates_size() const ;
+  edm4hep::Track getOriginTrack() const { return m_track; }
+  std::vector<TrackState> getTrackStates(std::string name) const;
+  std::vector<TrackState> getAllTrackStates() const;
+  std::map<std::string, std::vector<TrackState> > getTrackStatesMap() const { return m_trackStates; }
+  std::vector<PandoraPlus::CaloHalfCluster*> getAssociatedHalfClustersU() const { return m_halfClusterUCol; }  
+  std::vector<PandoraPlus::CaloHalfCluster*> getAssociatedHalfClustersV() const { return m_halfClusterVCol; }  
+  std::vector< std::pair<edm4hep::MCParticle, float> > getLinkedMCP() const { return MCParticleWeight; }
+  edm4hep::MCParticle getLeadingMCP() const;
+  float getLeadingMCPweight() const;
+  float getPt() const;
+  float getPz() const;
+  float getMomentum() const { return sqrt( getPt()*getPt() + getPz()*getPz() ); } 
+  TVector3 getP3() const; 
+  float getCharge() const;
+
+  void setTrackStates( std::string name, std::vector<TrackState>& _states ) { m_trackStates[name]=_states; }
+  void addAssociatedHalfClusterU( PandoraPlus::CaloHalfCluster* _cl ) { m_halfClusterUCol.push_back(_cl); }
+  void addAssociatedHalfClusterV( PandoraPlus::CaloHalfCluster* _cl ) { m_halfClusterVCol.push_back(_cl); }
+  void addLinkedMCP( std::pair<edm4hep::MCParticle, float> _pair ) { MCParticleWeight.push_back(_pair); }
+  void setLinkedMCP( std::vector<std::pair<edm4hep::MCParticle, float>> _pairVec ) { MCParticleWeight.clear(); MCParticleWeight = _pairVec; }
+
+  void setType(int _type) { m_type=_type; }
+  int getType() const { return m_type; }
+
+  private:
+    edm4hep::Track m_track;
+    static const double B ; //direction of magnetic field and charge need to check
+
+    std::map<std::string, std::vector<TrackState> > m_trackStates; // name = Input, Ecal, Hcal
+    std::vector<PandoraPlus::CaloHalfCluster*> m_halfClusterUCol; 
+    std::vector<PandoraPlus::CaloHalfCluster*> m_halfClusterVCol; 
+
+    int m_type;
+    std::vector< std::pair<edm4hep::MCParticle, float> > MCParticleWeight;
+  };
+
+};
+#endif
diff --git a/Reconstruction/CrystalCaloRec/include/Objects/TrackState.h b/Reconstruction/CrystalCaloRec/include/Objects/TrackState.h
new file mode 100644
index 0000000000000000000000000000000000000000..280404f26f17716f6c82efe4e5593c3ce03fa2ec
--- /dev/null
+++ b/Reconstruction/CrystalCaloRec/include/Objects/TrackState.h
@@ -0,0 +1,33 @@
+#ifndef _TRACKSTATE_H
+#define _TRACKSTATE_H
+#include "TVector3.h"
+
+namespace PandoraPlus{
+
+  class TrackState{
+  public: 
+    TrackState() {}
+    ~TrackState() {};
+    void Clear() {};
+
+    int location; 
+    float D0;
+    float Z0; 
+    float phi0; 
+    float Kappa;  //Kappa = omega*1000/(0.3*B[T]) = 1/pT
+    float tanLambda; 
+    float Omega;  
+    TVector3 referencePoint; 
+
+    static const int AtOther = 0 ; // any location other than the ones defined below
+    static const int AtIP = 1 ;
+    static const int AtFirstHit = 2 ;
+    static const int AtLastHit = 3 ;
+    static const int AtCalorimeter = 4 ;
+    static const int AtVertex = 5 ;
+    static const int LastLocation = AtVertex  ;
+    
+
+  };
+};
+#endif
diff --git a/Reconstruction/CrystalCaloRec/include/PandoraPlusDataCol.h b/Reconstruction/CrystalCaloRec/include/PandoraPlusDataCol.h
new file mode 100644
index 0000000000000000000000000000000000000000..03a9676aada6a38fea30301f9e38f343caf6f567
--- /dev/null
+++ b/Reconstruction/CrystalCaloRec/include/PandoraPlusDataCol.h
@@ -0,0 +1,70 @@
+#ifndef _PANDORAPLUS_DATA_H
+#define _PANDORAPLUS_DATA_H
+#include <iostream>
+#include <algorithm>
+#include <map>
+
+#include <CrystalEcalSvc/ICrystalEcalSvc.h>
+#include "Objects/CaloHit.h"
+#include "Objects/CaloUnit.h"
+#include "Objects/Calo1DCluster.h"
+#include "Objects/Calo2DCluster.h"
+#include "Objects/CaloHalfCluster.h"
+#include "Objects/Calo3DCluster.h"
+#include "Objects/HoughObject.h"
+#include "Objects/HoughSpace.h"
+#include "Objects/PFObject.h"
+#include "Objects/Track.h"
+
+#include "k4FWCore/DataHandle.h"
+#include "edm4hep/MCParticleCollection.h"
+#include "edm4hep/MCParticle.h"
+#include "edm4hep/Track.h"
+#include "edm4hep/TrackCollection.h"
+#include "edm4hep/CalorimeterHit.h"
+#include "edm4hep/CalorimeterHitCollection.h"
+#include "edm4hep/Vertex.h"
+#include "edm4hep/VertexCollection.h"
+#include "edm4hep/ClusterCollection.h"
+#include "edm4hep/ReconstructedParticleCollection.h"
+#include "edm4hep/MCRecoCaloAssociation.h"
+#include "edm4hep/MCRecoTrackerAssociation.h"
+#include "edm4hep/MCRecoParticleAssociationCollection.h"
+#include "edm4hep/MCRecoCaloParticleAssociationCollection.h"
+#include "edm4hep/MCRecoTrackParticleAssociationCollection.h"
+
+#define PI 3.141592653
+//#define C 299.79  // unit: mm/ns
+using namespace std;
+const double C = 299.79;
+class PandoraPlusDataCol{
+public:
+
+  PandoraPlusDataCol() {}; 
+  ~PandoraPlusDataCol() { Clear(); }
+  StatusCode Clear(); 
+
+  //Readin CollectionMap
+  std::map<std::string, std::vector<edm4hep::MCParticle> >       collectionMap_MC;
+  std::map<std::string, std::vector<edm4hep::CalorimeterHit> >   collectionMap_CaloHit;
+  std::map<std::string, std::vector<edm4hep::Vertex> >           collectionMap_Vertex;
+  std::map<std::string, std::vector<edm4hep::Track> >            collectionMap_Track;
+  std::map<std::string, std::vector<edm4hep::MCRecoCaloAssociation> > collectionMap_CaloRel;
+  std::map<std::string, std::vector<edm4hep::MCRecoTrackerAssociation> > collectionMap_TrkRel;
+
+  //Self used objects
+  //General objects for all PFA
+  std::vector<std::shared_ptr<PandoraPlus::Track>>       TrackCol;
+  std::map<std::string, std::vector<std::shared_ptr<PandoraPlus::CaloHit>>> map_CaloHit; //Hit
+  std::map<std::string, std::vector<std::shared_ptr<PandoraPlus::CaloUnit>>> map_BarCol; 
+  std::map<std::string, std::vector<std::shared_ptr<PandoraPlus::Calo1DCluster>>> map_1DCluster; 
+  std::map<std::string, std::vector<std::shared_ptr<PandoraPlus::CaloHalfCluster>>> map_HalfCluster;
+  std::map<std::string, std::vector<std::shared_ptr<PandoraPlus::Calo2DCluster>>> map_2DCluster; 
+  std::map<std::string, std::vector<std::shared_ptr<PandoraPlus::Calo3DCluster>>> map_CaloCluster; //Cluster
+  std::map<std::string, std::vector<std::shared_ptr<PandoraPlus::PFObject>>> map_PFObjects;
+
+  //Energy calibration service
+  SmartIF<ICrystalEcalSvc> EnergyCorrSvc; 
+
+};
+#endif
diff --git a/Reconstruction/CrystalCaloRec/include/PandoraPlusPFAlg.h b/Reconstruction/CrystalCaloRec/include/PandoraPlusPFAlg.h
new file mode 100644
index 0000000000000000000000000000000000000000..b5836d94ce99df0903d75671d50737ccd9f13517
--- /dev/null
+++ b/Reconstruction/CrystalCaloRec/include/PandoraPlusPFAlg.h
@@ -0,0 +1,321 @@
+#ifndef PANDORAPLUS_ALG_H
+#define PANDORAPLUS_ALG_H
+
+#include <string>
+#include "k4FWCore/DataHandle.h"
+#include "GaudiAlg/GaudiAlgorithm.h"
+#include <DDRec/DetectorData.h>
+#include <DDRec/CellIDPositionConverter.h>
+#include <DD4hep/Segmentations.h>
+#include "DetInterface/IGeomSvc.h"
+#include <CrystalEcalSvc/ICrystalEcalSvc.h>
+
+#include "k4FWCore/PodioDataSvc.h"
+#include "podio/CollectionBase.h"
+#include "podio/ROOTFrameWriter.h"
+
+#include "PandoraPlusDataCol.h"
+#include "Tools/MCParticleCreator.h"
+#include "Tools/TrackCreator.h"
+#include "Tools/CaloHitsCreator.h"
+#include "Tools/OutputCreator.h"
+#include "Tools/AlgorithmManager.h"
+#include "Algorithm/ExampleAlg.h"
+#include "Algorithm/GlobalClusteringAlg.h"
+#include "Algorithm/HcalClusteringAlg.h"
+#include "Algorithm/LocalMaxFindingAlg.h"
+#include "Algorithm/TrackMatchingAlg.h"
+#include "Algorithm/HoughClusteringAlg.h"
+#include "Algorithm/ConeClustering2DAlg.h"
+#include "Algorithm/AxisMergingAlg.h"
+#include "Algorithm/EnergySplittingAlg.h"
+#include "Algorithm/EnergyTimeMatchingAlg.h"
+#include "Algorithm/ConeClusteringAlg.h"
+#include "Algorithm/TrackExtrapolatingAlg.h"
+#include "Algorithm/PFOCreatingAlg.h"
+#include "Algorithm/TrackClusterConnectingAlg.h"
+#include "Algorithm/PFOReclusteringAlg.h"
+
+#include "Algorithm/TruthClusteringAlg.h"
+#include "Algorithm/TruthTrackMatchingAlg.h"
+#include "Algorithm/TruthPatternRecAlg.h"
+#include "Algorithm/TruthEnergySplittingAlg.h"
+#include "Algorithm/TruthMatchingAlg.h"
+#include "Algorithm/TruthClusterMergingAlg.h"
+
+#include "TVector3.h"
+#include "TRandom3.h"
+#include "TFile.h"
+#include "TTree.h"
+#include "TBranch.h"
+
+#include <cstdlib>
+
+using namespace PandoraPlus;
+using namespace std;
+
+class PandoraPlusPFAlg : public GaudiAlgorithm
+{
+ 
+public:
+ 
+  PandoraPlusPFAlg(const std::string& name, ISvcLocator* svcLoc);
+ 
+  /** Called at the begin of the job before anything is read.
+   * Use to initialize the processor, e.g. book histograms.
+   */
+  virtual StatusCode initialize() ;
+ 
+  /** Called for every event - the working horse.
+   */
+  virtual StatusCode execute() ; 
+ 
+  /** Called after data processing for clean up.
+   */
+  virtual StatusCode finalize() ;
+  
+
+protected:
+
+  int _nEvt ;
+  TRandom3 rndm;
+
+  SmartIF<IGeomSvc> m_geosvc;
+  SmartIF<ICrystalEcalSvc> m_energycorsvc;
+  std::map<std::string, dd4hep::DDSegmentation::BitFieldCoder*> map_readout_decoder;
+
+  //DataCollection: moved into execute() to ensure everything can be cleand after one event. 
+  //PandoraPlusDataCol     m_DataCol; 
+
+
+  //Creators and their setting
+  MCParticleCreator       *m_pMCParticleCreator;
+  TrackCreator            *m_pTrackCreator;
+  CaloHitsCreator         *m_pCaloHitsCreator;
+  OutputCreator           *m_pOutputCreator;
+
+  Settings   m_pMCParticleCreatorSettings;
+  Settings   m_pTrackCreatorSettings;
+  Settings   m_CaloHitsCreatorSettings;
+  Settings   m_OutputCreatorSettings;
+
+
+  //Parameters for PFA algorithm
+  Settings m_GlobalSettings; 
+
+
+  //Algorithm for PFA
+  PandoraPlus::AlgorithmManager m_algorithmManager; 
+
+
+  //Readin collection names
+  Gaudi::Property< std::string > name_MCParticleCol{ this, "MCParticleCollection", "MCParticle" };
+  Gaudi::Property< std::string > name_MCPTrkAssoCol{this, "MCRecoTrackParticleAssociationCollection", "MarlinTrkAssociation"};
+  Gaudi::Property< std::vector<std::string> > name_TrackCol{ this, "TrackCollections", {"MarlinTrkTracks"} };
+  Gaudi::Property< std::vector<std::string> > name_EcalHits{ this, "ECalCaloHitCollections", {"ECALBarrel"} };
+  Gaudi::Property< std::vector<std::string> > name_EcalReadout{ this, "ECalReadOutNames", {"EcalBarrelCollection"} }; 
+  Gaudi::Property< std::vector<std::string> > name_EcalMCPAssociation{ this, "ECalMCPAssociationName", {"ECALBarrelParticleAssoCol"} };
+  Gaudi::Property< std::vector<std::string> > name_HcalHits{ this, "HCalCaloHitCollections", {"HCALBarrel"} };
+  Gaudi::Property< std::vector<std::string> > name_HcalReadout{ this, "HCalReadOutNames", {"HcalBarrelCollection"} }; 
+  Gaudi::Property< std::vector<std::string> > name_HcalMCPAssociation{ this, "HCalMCPAssociationName", {"HCALBarrelParticleAssoCol"} };
+  
+
+  //---Readin collections
+  typedef DataHandle<edm4hep::TrackCollection>                          TrackType; 
+  typedef DataHandle<edm4hep::CalorimeterHitCollection>                 CaloType; 
+  typedef DataHandle<edm4hep::MCRecoCaloParticleAssociationCollection>  CaloParticleAssoType; 
+  DataHandle<edm4hep::MCParticleCollection>* r_MCParticleCol; 
+  DataHandle<edm4hep::MCRecoTrackParticleAssociationCollection>* r_MCPTrkAssoCol;  
+  std::vector<TrackType*> r_TrackCols; 
+  //std::vector<CaloType*>  r_ECalHitCols; 
+  //std::vector<CaloType*>  r_HCalHitCols; 
+  std::vector<CaloType*>  r_CaloHitCols; 
+  std::map<std::string, CaloParticleAssoType*> map_CaloMCPAssoCols;
+
+  //Global parameters.
+  Gaudi::Property<float>  m_BField{this,  "BField", 3., "Magnetic field"};
+  Gaudi::Property<float>  m_seed{this,    "Seed", 2131, "Random Seed"};
+  Gaudi::Property<int>    m_Debug{this,   "Debug", 0, "Debug level"};
+  Gaudi::Property<int>    m_Nskip{this,   "SkipEvt", 0, "Skip event"};
+  Gaudi::Property<std::string>   m_EcalType{this, "EcalType", "BarEcal", "ECAL type"};
+  Gaudi::Property<bool>   m_useTruthTrk{this,   "UseTruthTrack", 1, "Use truth track or reconstructed track"};
+  Gaudi::Property<float>  m_EcalCalib{this,  "EcalGlobalCalib", 1.02, "ECAL global calibration"};
+  Gaudi::Property<float>  m_HcalCalib{this,  "HcalGlobalCalib", 65.,  "HCAL global calibration"};
+  
+  //Algorithms: 
+  typedef std::vector<std::string> StringVector;
+  Gaudi::Property< StringVector > name_Algs{ this, "AlgList", {} };
+  Gaudi::Property< std::vector<StringVector> > name_AlgPars{ this, "AlgParNames", {} };
+  Gaudi::Property< std::vector<StringVector> > type_AlgPars{ this, "AlgParTypes", {} };
+  Gaudi::Property< std::vector<StringVector> > value_AlgPars{this, "AlgParValues", {} };
+
+
+  // Output collections
+  DataHandle<edm4hep::CalorimeterHitCollection>             w_RecEcalCol{"RecECALBarrel", Gaudi::DataHandle::Writer, this};
+  DataHandle<edm4hep::CalorimeterHitCollection>             w_RecCoreCol{"RecECALBarrelCore", Gaudi::DataHandle::Writer, this};
+  DataHandle<edm4hep::CalorimeterHitCollection>             w_RecHcalCol{"RecHCALBarrel", Gaudi::DataHandle::Writer, this};
+  DataHandle<edm4hep::TrackCollection>                      w_RecTrkCol{"RecTracks", Gaudi::DataHandle::Writer, this};
+  //Gaudi::Property< std::string > name_EcalCluster{this, "OutputEcalCluster", "TrkMergedECAL"};
+  //Gaudi::Property< std::string > name_EcalCore   {this, "OutputEcalCore", "EcalCore"};
+  //Gaudi::Property< std::string > name_HcalCluster{this, "OutputHcalCluster", "HcalCluster"};
+  Gaudi::Property< std::string > name_PFObject   {this, "OutputPFO", "outputPFO"};
+  DataHandle<edm4hep::ReconstructedParticleCollection>      w_ReconstructedParticleCollection {"PandoraPFOs"    ,Gaudi::DataHandle::Writer, this};
+  std::map<std::string, DataHandle<edm4hep::ClusterCollection>* > w_ClusterCollection;
+
+
+  //For Ana
+  Gaudi::Property<bool>  m_WriteAna {this, "WriteAna", false, "Write Ntuples for analysis"};
+  Gaudi::Property<std::string> m_filename{this, "AnaFileName", "testout.root", "Output file name"};
+
+  typedef std::vector<float> FloatVec;
+  typedef std::vector<int>   IntVec;
+
+  TFile* m_wfile;
+
+  // MC particle
+  TTree *t_MCParticle;
+  int m_Nmc;
+  IntVec m_mcPdgid, m_mcStatus;
+  FloatVec m_mcPx, m_mcPy, m_mcPz, m_mcEn, m_mcMass, m_mcCharge, m_mcEPx, m_mcEPy, m_mcEPz, m_depEn_ecal, m_depEn_hcal;
+
+  //Raw bars and hits
+  TTree* t_SimBar;
+  float m_totE_EcalSim, m_totE_HcalSim;
+  FloatVec m_simBar_x, m_simBar_y, m_simBar_z, m_simBar_T1, m_simBar_T2, m_simBar_Q1, m_simBar_Q2; 
+  FloatVec m_simBar_truthMC_tag, m_simBar_truthMC_pid, m_simBar_truthMC_px, m_simBar_truthMC_py, m_simBar_truthMC_pz, m_simBar_truthMC_E, 
+           m_simBar_truthMC_EPx, m_simBar_truthMC_EPy, m_simBar_truthMC_EPz, m_simBar_truthMC_weight;
+  IntVec m_simBar_dlayer, m_simBar_stave, m_simBar_slayer, m_simBar_module, m_simBar_bar;
+  FloatVec m_HcalHit_x, m_HcalHit_y, m_HcalHit_z, m_HcalHit_E, 
+           m_HcalHit_truthMC_tag, m_HcalHit_truthMC_pid, m_HcalHit_truthMC_px, m_HcalHit_truthMC_py, m_HcalHit_truthMC_pz, m_HcalHit_truthMC_E,
+           m_HcalHit_truthMC_EPx, m_HcalHit_truthMC_EPy, m_HcalHit_truthMC_EPz, m_HcalHit_truthMC_weight;
+  IntVec   m_HcalHit_layer;
+
+  //localMax
+  TTree *t_LocalMax;
+  int m_NlmU, m_NlmV;
+  IntVec m_localMaxU_mc_pdg, m_localMaxV_mc_pdg, m_localMaxU_mc_tag, m_localMaxV_mc_tag;
+  FloatVec m_localMaxU_tag, m_localMaxU_x, m_localMaxU_y, m_localMaxU_z, m_localMaxU_E,
+            m_localMaxU_mc_px, m_localMaxU_mc_py, m_localMaxU_mc_pz, m_localMaxU_mc_weight;
+  FloatVec m_localMaxV_tag, m_localMaxV_x, m_localMaxV_y, m_localMaxV_z, m_localMaxV_E,
+            m_localMaxV_mc_px, m_localMaxV_mc_py, m_localMaxV_mc_pz, m_localMaxV_mc_weight;
+
+
+  //1D showers
+  TTree *t_Layers;
+  int m_NshowerU, m_NshowerV;
+  IntVec m_barShowerU_mc_pdg, m_barShowerV_mc_pdg, m_barShowerU_mc_tag, m_barShowerV_mc_tag;
+  FloatVec m_barShowerU_tag, m_barShowerU_x, m_barShowerU_y, m_barShowerU_z, m_barShowerU_E,
+            m_barShowerU_mc_px, m_barShowerU_mc_py, m_barShowerU_mc_pz, m_barShowerU_mc_weight;
+  FloatVec m_barShowerV_tag, m_barShowerV_x, m_barShowerV_y, m_barShowerV_z, m_barShowerV_E,
+            m_barShowerV_mc_px, m_barShowerV_mc_py, m_barShowerV_mc_pz, m_barShowerV_mc_weight;
+  // Hough axis
+  TTree * t_Hough;
+  IntVec m_houghU_type, m_houghV_type;
+  FloatVec  m_houghU_tag, m_houghU_x, m_houghU_y, m_houghU_z, m_houghU_E,
+            m_houghU_truth_tag, m_houghU_truth_MC_px, m_houghU_truth_MC_py, m_houghU_truth_MC_pz, m_houghU_truth_MC_E, m_houghU_truth_MC_weight,
+            m_houghU_hit_tag, m_houghU_hit_x, m_houghU_hit_y, m_houghU_hit_z, m_houghU_hit_E;
+  FloatVec  m_houghV_tag, m_houghV_x, m_houghV_y, m_houghV_z, m_houghV_E, m_houghV_alpha, m_houghV_rho,
+            m_houghV_truth_tag, m_houghV_truth_MC_px, m_houghV_truth_MC_py, m_houghV_truth_MC_pz, m_houghV_truth_MC_E, m_houghV_truth_MC_weight,
+            m_houghV_hit_tag, m_houghV_hit_x, m_houghV_hit_y, m_houghV_hit_z, m_houghV_hit_E;
+  // Cone axis
+  TTree * t_Cone;
+  IntVec m_coneU_type, m_coneV_type;
+  FloatVec  m_coneU_tag, m_coneU_x, m_coneU_y, m_coneU_z, m_coneU_E,
+            m_coneU_truth_tag, m_coneU_truth_MC_px, m_coneU_truth_MC_py, m_coneU_truth_MC_pz, m_coneU_truth_MC_E, m_coneU_truth_MC_weight,
+            m_coneU_hit_tag, m_coneU_hit_x, m_coneU_hit_y, m_coneU_hit_z, m_coneU_hit_E;
+  FloatVec  m_coneV_tag, m_coneV_x, m_coneV_y, m_coneV_z, m_coneV_E,
+            m_coneV_truth_tag, m_coneV_truth_MC_px, m_coneV_truth_MC_py, m_coneV_truth_MC_pz, m_coneV_truth_MC_E, m_coneV_truth_MC_weight,
+            m_coneV_hit_tag, m_coneV_hit_x, m_coneV_hit_y, m_coneV_hit_z, m_coneV_hit_E;
+  // Track axis
+  TTree * t_TrackAxis;
+  IntVec m_trackU_type, m_trackV_type;
+  FloatVec  m_trackU_tag, m_trackU_x, m_trackU_y, m_trackU_z, m_trackU_E,
+            m_trackU_truth_tag, m_trackU_truth_MC_px, m_trackU_truth_MC_py, m_trackU_truth_MC_pz, m_trackU_truth_MC_E, m_trackU_truth_MC_weight,
+            m_trackU_hit_tag, m_trackU_hit_x, m_trackU_hit_y, m_trackU_hit_z, m_trackU_hit_E;
+  FloatVec  m_trackV_tag, m_trackV_x, m_trackV_y, m_trackV_z, m_trackV_E,
+            m_trackV_truth_tag, m_trackV_truth_MC_px, m_trackV_truth_MC_py, m_trackV_truth_MC_pz, m_trackV_truth_MC_E, m_trackV_truth_MC_weight,
+            m_trackV_hit_tag, m_trackV_hit_x, m_trackV_hit_y, m_trackV_hit_z, m_trackV_hit_E;
+  // axis
+  TTree *t_Axis;
+  IntVec m_axisU_type, m_axisV_type;
+  FloatVec  m_axisU_tag, m_axisU_x, m_axisU_y, m_axisU_z, m_axisU_E,
+            m_axisU_truth_tag, m_axisU_truth_MC_px, m_axisU_truth_MC_py, m_axisU_truth_MC_pz, m_axisU_truth_MC_E, m_axisU_truth_MC_weight,
+            m_axisU_hit_tag, m_axisU_hit_x, m_axisU_hit_y, m_axisU_hit_z, m_axisU_hit_E;
+  FloatVec  m_axisV_tag, m_axisV_x, m_axisV_y, m_axisV_z, m_axisV_E,
+            m_axisV_truth_tag, m_axisV_truth_MC_px, m_axisV_truth_MC_py, m_axisV_truth_MC_pz, m_axisV_truth_MC_E, m_axisV_truth_MC_weight,
+            m_axisV_hit_tag, m_axisV_hit_x, m_axisV_hit_y, m_axisV_hit_z, m_axisV_hit_E;
+  FloatVec  m_emptyAxisU_tag, m_emptyAxisU_x, m_emptyAxisU_y, m_emptyAxisU_z, m_emptyAxisU_E; 
+  FloatVec  m_emptyAxisV_tag, m_emptyAxisV_x, m_emptyAxisV_y, m_emptyAxisV_z, m_emptyAxisV_E; 
+
+
+  //HalfCluster after energy splitting
+  TTree *t_HalfCluster;
+  float m_totE_HFClusU, m_totE_HFClusV;
+  FloatVec m_HalfClusterV_x, m_HalfClusterV_y, m_HalfClusterV_z, m_HalfClusterV_E, m_HalfClusterV_tag, m_HalfClusterV_type, m_HalfClusterV_nTrk;
+  FloatVec m_HalfClusterV_hit_x, m_HalfClusterV_hit_y, m_HalfClusterV_hit_z, m_HalfClusterV_hit_E, m_HalfClusterV_hit_tag;
+  FloatVec m_HalfClusterV_truth_tag, m_HalfClusterV_truthMC_px, m_HalfClusterV_truthMC_py, m_HalfClusterV_truthMC_pz, m_HalfClusterV_truthMC_E, m_HalfClusterV_truthMC_weight;
+  FloatVec m_HalfClusterU_x, m_HalfClusterU_y, m_HalfClusterU_z, m_HalfClusterU_E, m_HalfClusterU_tag, m_HalfClusterU_type, m_HalfClusterU_nTrk;
+  FloatVec m_HalfClusterU_hit_x, m_HalfClusterU_hit_y, m_HalfClusterU_hit_z, m_HalfClusterU_hit_E, m_HalfClusterU_hit_tag;
+  FloatVec m_HalfClusterU_truth_tag, m_HalfClusterU_truthMC_px, m_HalfClusterU_truthMC_py, m_HalfClusterU_truthMC_pz, m_HalfClusterU_truthMC_E, m_HalfClusterU_truthMC_weight;
+
+
+  //Tower
+  TTree *t_Tower; 
+  int towerID[3];
+  int m_NclusU, m_NclusV; 
+  float m_totEn, m_totEn_U, m_totEn_V;
+
+
+  //3D clusters
+  TTree *t_Cluster;
+  float m_totE_Ecal, m_totE_Hcal;
+  int m_Nclus_Ecal, m_Nclus_Hcal;
+  IntVec m_EcalClus_trk_location, m_EcalClus_trk_tag;
+  FloatVec m_EcalClus_x, m_EcalClus_y, m_EcalClus_z, m_EcalClus_E, m_EcalClus_Escale, m_EcalClus_nTrk, m_EcalClus_pTrk, m_EcalClus_typeU, m_EcalClus_typeV,
+          m_EcalClus_hitU_x, m_EcalClus_hitU_y, m_EcalClus_hitU_z, m_EcalClus_hitU_E, m_EcalClus_hitU_tag,
+          m_EcalClus_hitV_x, m_EcalClus_hitV_y, m_EcalClus_hitV_z, m_EcalClus_hitV_E, m_EcalClus_hitV_tag,
+          m_EcalClus_trk_d0, m_EcalClus_trk_z0, m_EcalClus_trk_phi, m_EcalClus_trk_tanL, m_EcalClus_trk_omega, m_EcalClus_trk_kappa,
+          m_EcalClus_truthMC_tag, m_EcalClus_truthMC_pid, m_EcalClus_truthMC_px, m_EcalClus_truthMC_py, m_EcalClus_truthMC_pz, m_EcalClus_truthMC_E,
+          m_EcalClus_truthMC_EPx, m_EcalClus_truthMC_EPy, m_EcalClus_truthMC_EPz, m_EcalClus_truthMC_weight;
+  FloatVec m_HcalClus_x, m_HcalClus_y, m_HcalClus_z, m_HcalClus_E, m_HcalClus_nTrk, m_HcalClus_pTrk,
+          m_HcalClus_hit_x, m_HcalClus_hit_y, m_HcalClus_hit_z, m_HcalClus_hit_E, m_HcalClus_hit_tag,
+          m_HcalClus_truthMC_tag, m_HcalClus_truthMC_pid, m_HcalClus_truthMC_px, m_HcalClus_truthMC_py, m_HcalClus_truthMC_pz, m_HcalClus_truthMC_E,
+          m_HcalClus_truthMC_EPx, m_HcalClus_truthMC_EPy, m_HcalClus_truthMC_EPz, m_HcalClus_truthMC_weight;
+  FloatVec m_SimpleHcalClus_x, m_SimpleHcalClus_y, m_SimpleHcalClus_z, m_SimpleHcalClus_E, m_SimpleHcalClus_nTrk, m_SimpleHcalClus_pTrk,
+          m_SimpleHcalClus_hit_x, m_SimpleHcalClus_hit_y, m_SimpleHcalClus_hit_z, m_SimpleHcalClus_hit_E, m_SimpleHcalClus_hit_tag,
+          m_SimpleHcalClus_truthMC_tag, m_SimpleHcalClus_truthMC_pid, m_SimpleHcalClus_truthMC_px, m_SimpleHcalClus_truthMC_py, m_SimpleHcalClus_truthMC_pz, m_SimpleHcalClus_truthMC_E,
+          m_SimpleHcalClus_truthMC_EPx, m_SimpleHcalClus_truthMC_EPy, m_SimpleHcalClus_truthMC_EPz, m_SimpleHcalClus_truthMC_weight;
+
+  TTree *t_Track;
+  int m_Ntrk; 
+  FloatVec m_trkstate_d0, m_trkstate_z0, m_trkstate_phi, m_trkstate_tanL, m_trkstate_omega, m_trkstate_kappa;
+  FloatVec m_trkstate_refx, m_trkstate_refy, m_trkstate_refz; 
+  IntVec m_trkstate_tag, m_trkstate_location, m_type;
+
+  // yyy: output PFO information
+  TTree *t_PFO;
+  IntVec pfo_tag, pfo_n_track, pfo_n_ecal_clus, pfo_n_hcal_clus, pfo_ecal_tag, pfo_hcal_tag;
+  IntVec pfo_trk_location, pfo_trk_tag;
+  FloatVec pfo_trk_d0, pfo_trk_z0, pfo_trk_phi, pfo_trk_tanL, pfo_trk_omega, pfo_trk_kappa;
+  FloatVec  pfo_ecal_clus_x, pfo_ecal_clus_y, pfo_ecal_clus_z, pfo_ecal_clus_E, pfo_ecal_clus_Escale,
+            pfo_hcal_clus_x, pfo_hcal_clus_y, pfo_hcal_clus_z, pfo_hcal_clus_E;
+
+
+  void ClearMCParticle();
+  void ClearBar();
+  void ClearLocalMax();
+  void ClearLayer();
+  void ClearHough();
+  void ClearCone();
+  void ClearTrackAxis();
+  void ClearAxis();
+  void ClearHalfCluster();
+  void ClearTower();
+  void ClearCluster();
+  void ClearTrack();
+  void ClearPFO(); // yyy
+
+  double GetParticleDepEnergy(edm4hep::MCParticle& mcp, std::vector<std::shared_ptr<PandoraPlus::CaloUnit>>& barcol);
+  double GetParticleDepEnergy(edm4hep::MCParticle& mcp, std::vector<std::shared_ptr<PandoraPlus::CaloHit>>& hitcol);
+};
+#endif
diff --git a/Reconstruction/CrystalCaloRec/include/Tools/Algorithm.h b/Reconstruction/CrystalCaloRec/include/Tools/Algorithm.h
new file mode 100644
index 0000000000000000000000000000000000000000..14cd8fad00408b88a3cba7adb031b0f3e65dc7a2
--- /dev/null
+++ b/Reconstruction/CrystalCaloRec/include/Tools/Algorithm.h
@@ -0,0 +1,46 @@
+#ifndef ALGORITHM_TEMP_H
+#define ALGORITHM_TEMP_H
+
+#include "GaudiAlg/GaudiAlgorithm.h"
+
+#include "PandoraPlusDataCol.h"
+
+namespace PandoraPlus{
+
+  class Settings{
+  public: 
+    Settings(){};
+    ~Settings(){ Clear(); }
+   
+    void Clear() { map_intPars.clear();  map_floatPars.clear(); map_boolPars.clear(); map_stringPars.clear(); map_stringVecPars.clear(); }
+    std::map<std::string, int>    map_intPars; 
+    std::map<std::string, double> map_floatPars; 
+    std::map<std::string, bool>   map_boolPars; 
+    std::map<std::string, std::string> map_stringPars; 
+    std::map<std::string, std::vector<std::string> > map_stringVecPars;
+
+  };
+
+  class Algorithm{
+  public:
+    Algorithm() {};
+    virtual ~Algorithm() {};
+
+    virtual StatusCode ReadSettings(Settings& m_settings) = 0;
+    virtual StatusCode Initialize( PandoraPlusDataCol& m_datacol ) = 0;
+    virtual StatusCode RunAlgorithm( PandoraPlusDataCol& m_datacol ) = 0;
+    virtual StatusCode ClearAlgorithm() = 0;
+
+
+    Settings settings; 
+  };
+
+  class AlgorithmFactory{
+  public:
+
+    virtual ~AlgorithmFactory(){}; 
+    virtual Algorithm* CreateAlgorithm() const = 0;
+  };
+
+};
+#endif
diff --git a/Reconstruction/CrystalCaloRec/include/Tools/AlgorithmManager.h b/Reconstruction/CrystalCaloRec/include/Tools/AlgorithmManager.h
new file mode 100644
index 0000000000000000000000000000000000000000..1089aaf14d57c51252ffe7368ba16c4fa182865e
--- /dev/null
+++ b/Reconstruction/CrystalCaloRec/include/Tools/AlgorithmManager.h
@@ -0,0 +1,66 @@
+#ifndef ALGORITHM_MANAGER_H
+#define ALGORITHM_MANAGER_H
+
+#include "Tools/Algorithm.h"
+
+namespace PandoraPlus{
+
+  class Algorithm;
+  class AlgorithmFactory;
+
+  class AlgorithmManager{
+  public: 
+
+    AlgorithmManager(){};
+    ~AlgorithmManager(){ Clean(); };
+   
+    void Clean(){
+      m_algorithmNames.clear();
+      for(auto iter : m_algorithmFactoryMap) delete iter.second;
+      for(auto iter : m_algorithmMap) delete iter.second;
+   
+      m_algorithmFactoryMap.clear();
+      m_algorithmMap.clear();
+    }
+   
+    StatusCode RegisterAlgorithmFactory(const std::string& algorithmName, AlgorithmFactory *const algorithmFactory)
+    {
+      if(!m_algorithmFactoryMap.insert(AlgorithmFactoryMap::value_type(algorithmName, algorithmFactory)).second ){
+        std::cout<<"ERROR in Register AlgorithmFactory: can not register "<<algorithmName<<std::endl;
+        return StatusCode::FAILURE; 
+      }
+      return StatusCode::SUCCESS;
+    }
+   
+    StatusCode RegisterAlgorithm( const std::string& algorithmName, Settings& m_algorithmSetting ){
+      m_algorithmNames.push_back(algorithmName);
+      m_algorithmMap.insert(AlgorithmMap::value_type(algorithmName, m_algorithmFactoryMap[algorithmName]->CreateAlgorithm()) );
+      m_algorithmMap[algorithmName]->ReadSettings(m_algorithmSetting);
+   
+      return StatusCode::SUCCESS;
+    }
+   
+    //StatusCode CreateAlgorithm(const std::map<std::string, Settings>& m_algorithmSettings); 
+    StatusCode RunAlgorithm( PandoraPlusDataCol& m_datacol ){
+      for(auto iter : m_algorithmNames){
+cout<<"Processing Algorithm: "<<iter<<endl;
+        m_algorithmMap[iter]->Initialize(m_datacol);
+        m_algorithmMap[iter]->RunAlgorithm(m_datacol);
+        m_algorithmMap[iter]->ClearAlgorithm();
+      }
+      return StatusCode::SUCCESS;
+    }
+
+  private: 
+    typedef std::map<const std::string, Algorithm *const> AlgorithmMap;
+    typedef std::map< std::string, AlgorithmFactory *const> AlgorithmFactoryMap;
+
+    AlgorithmMap              m_algorithmMap;
+    AlgorithmFactoryMap       m_algorithmFactoryMap;
+    std::vector<std::string>  m_algorithmNames;
+
+  };
+
+};
+
+#endif
diff --git a/Reconstruction/CrystalCaloRec/include/Tools/CaloHitsCreator.h b/Reconstruction/CrystalCaloRec/include/Tools/CaloHitsCreator.h
new file mode 100644
index 0000000000000000000000000000000000000000..26b477716fc3edea771d8000cb8c125363bcc031
--- /dev/null
+++ b/Reconstruction/CrystalCaloRec/include/Tools/CaloHitsCreator.h
@@ -0,0 +1,37 @@
+#ifndef ECALHIT_CREATOR_H
+#define ECALHIT_CREATOR_H
+
+#include "k4FWCore/DataHandle.h"
+#include "PandoraPlusDataCol.h"
+#include "Tools/Algorithm.h"
+#include <DDRec/DetectorData.h>
+#include <DDRec/CellIDPositionConverter.h>
+#include <DD4hep/Segmentations.h>
+
+namespace PandoraPlus{
+
+  class CaloHitsCreator{
+
+  public: 
+    //initialize a CaloHitCreator
+    CaloHitsCreator( const Settings& m_settings );
+    ~CaloHitsCreator() {};
+   
+    StatusCode CreateCaloHits( PandoraPlusDataCol& m_DataCol,
+                               std::vector<DataHandle<edm4hep::CalorimeterHitCollection>*>& r_CaloHitCols, 
+                               std::map<std::string, dd4hep::DDSegmentation::BitFieldCoder*>& map_decoder, 
+                               std::map<std::string, DataHandle<edm4hep::MCRecoCaloParticleAssociationCollection>*>& map_CaloParticleAssoCol ); 
+
+    //StatusCode CreateMCParticleCaloHitsAsso( std::vector<DataHandle<edm4hep::CalorimeterHitCollection>*>& r_CaloHitCols, 
+    //                                         DataHandle<edm4hep::MCRecoCaloParticleAssociationCollection>* r_MCParticleRecoCaloCol );
+
+    //StatusCode Clustering( PandoraPlusDataCol& m_DataCol ) { return StatusCode::SUCCESS; };
+   
+    StatusCode Reset() { return StatusCode::SUCCESS; };
+
+  private: 
+    const PandoraPlus::Settings  settings; 
+
+  };
+};
+#endif
diff --git a/Reconstruction/CrystalCaloRec/include/Tools/MCParticleCreator.h b/Reconstruction/CrystalCaloRec/include/Tools/MCParticleCreator.h
new file mode 100644
index 0000000000000000000000000000000000000000..9ed7795ed3a93bfb0dc53182bf6d2eae1e7e44ac
--- /dev/null
+++ b/Reconstruction/CrystalCaloRec/include/Tools/MCParticleCreator.h
@@ -0,0 +1,34 @@
+#ifndef MCPARTICLE_CREATOR_H
+#define MCPARTICLE_CREATOR_H
+
+#include "PandoraPlusDataCol.h"
+#include "Tools/Algorithm.h"
+
+namespace PandoraPlus{
+
+  class MCParticleCreator{
+
+  public: 
+
+    //initialize a CaloHitCreator
+    MCParticleCreator( const Settings& m_settings );
+    ~MCParticleCreator() {};
+
+    StatusCode CreateMCParticle( PandoraPlusDataCol& m_DataCol, 
+                                 DataHandle<edm4hep::MCParticleCollection>& r_MCParticleCol ); 
+
+    //StatusCode CreateTrackMCParticleRelation(){ return StatusCode::SUCCESS; };
+
+    //StatusCode CreateEcalBarMCParticleRelation(){ return StatusCode::SUCCESS; };
+
+    //StatusCode CreateHcalHitsMCParticleRelation(){ return StatusCode::SUCCESS; };
+
+    StatusCode Reset() { return StatusCode::SUCCESS; };
+
+  private: 
+    const PandoraPlus::Settings   settings;  
+
+  };
+
+};
+#endif
diff --git a/Reconstruction/CrystalCaloRec/include/Tools/OutputCreator.h b/Reconstruction/CrystalCaloRec/include/Tools/OutputCreator.h
new file mode 100644
index 0000000000000000000000000000000000000000..485dc7c21b0dc23d6dcfa66573909502d3da75b7
--- /dev/null
+++ b/Reconstruction/CrystalCaloRec/include/Tools/OutputCreator.h
@@ -0,0 +1,37 @@
+#ifndef OUTPUT_CREATOR_H
+#define OUTPUT_CREATOR_H
+
+#include "k4FWCore/DataHandle.h"
+#include "edm4hep/MutableCalorimeterHit.h"
+#include "edm4hep/Vector3f.h"
+#include "PandoraPlusDataCol.h"
+#include "Tools/Algorithm.h"
+//#include "Tools/HelixClassD.h"
+
+namespace PandoraPlus{
+
+  class OutputCreator{
+  public: 
+
+    OutputCreator( const Settings& m_settings);
+    ~OutputCreator() {};
+
+    StatusCode CreateOutputCollections( PandoraPlusDataCol& m_DataCol, 
+                                        DataHandle<edm4hep::CalorimeterHitCollection>& m_outRecHitsHandler, 
+                                        DataHandle<edm4hep::CalorimeterHitCollection>& m_outRecCoreHandler, 
+                                        DataHandle<edm4hep::CalorimeterHitCollection>& m_outRecHcalHitsHandler, 
+                                        DataHandle<edm4hep::TrackCollection>& m_outTrkHandler, 
+                                        std::map<std::string, DataHandle<edm4hep::ClusterCollection>*>& m_outClusterColHandler, 
+                                        DataHandle<edm4hep::ReconstructedParticleCollection>& m_recPFOHandler );
+
+
+    StatusCode Reset() { return StatusCode::SUCCESS; }
+
+    edm4hep::Track TruthTrack(edm4hep::MCParticle _mcp, edm4hep::TrackCollection* _trkCol );
+
+  private: 
+    const PandoraPlus::Settings   settings;
+  
+  };
+};
+#endif
diff --git a/Reconstruction/CrystalCaloRec/include/Tools/TrackCreator.h b/Reconstruction/CrystalCaloRec/include/Tools/TrackCreator.h
new file mode 100644
index 0000000000000000000000000000000000000000..a35cfaa5af79ed9ce46aa4fb12e0ce10155836ac
--- /dev/null
+++ b/Reconstruction/CrystalCaloRec/include/Tools/TrackCreator.h
@@ -0,0 +1,36 @@
+#ifndef TRACK_CREATOR_H
+#define TRACK_CREATOR_H
+
+#include "PandoraPlusDataCol.h"
+#include "Tools/Algorithm.h"
+#include "Algorithm/TrackExtrapolatingAlg.h"
+#include "TVector3.h"
+
+namespace PandoraPlus{
+  class TrackCreator{
+
+  public: 
+
+    //initialize a CaloHitCreator
+    TrackCreator( const Settings& m_settings );
+    ~TrackCreator() { delete m_TrkExtraAlg; };
+   
+    StatusCode CreateTracks( PandoraPlusDataCol& m_DataCol, 
+                             std::vector<DataHandle<edm4hep::TrackCollection>*>& r_TrackCols, 
+                             DataHandle<edm4hep::MCRecoTrackParticleAssociationCollection>* r_MCParticleTrkCol ); 
+   
+    StatusCode CreateTracksFromMCParticle(PandoraPlusDataCol& m_DataCol, 
+                                          DataHandle<edm4hep::MCParticleCollection>& r_MCParticleCol);
+
+
+    StatusCode Reset(){};
+
+  private: 
+    const PandoraPlus::Settings  settings; 
+    PandoraPlus::Algorithm*      m_TrkExtraAlg; 
+    PandoraPlus::Settings        m_TrkExtraSettings;  
+
+
+  };
+};
+#endif
diff --git a/Reconstruction/CrystalCaloRec/include/Tools/TrackFitInEcal.h b/Reconstruction/CrystalCaloRec/include/Tools/TrackFitInEcal.h
new file mode 100644
index 0000000000000000000000000000000000000000..3d9e9e1f66cd70705b14a1b0d1d86ad2e9be96a5
--- /dev/null
+++ b/Reconstruction/CrystalCaloRec/include/Tools/TrackFitInEcal.h
@@ -0,0 +1,59 @@
+#ifndef TRACKFITINECAL_H
+#define TRACKFITINECAL_H
+
+#include <iostream>
+#include <vector>
+
+#include "Rtypes.h"
+
+class TrackFitInEcal{
+public:
+	 TrackFitInEcal(double barAngle=0.);
+	 ~TrackFitInEcal();
+
+   void setBarAngle(double barAngle) { m_barAngle=barAngle; }
+   void setPoint(int flagUZ, double uzPos, double uzPosErr, double depth, double depthErr);
+   void setGlobalPoint(int flagUZ, double x, double xerr, double y, double yerr, double z, double zerr);
+   void setImpactParameter( double dr, double dz ) { m_IPvar[0]=dr; m_IPvar[1]=dz; m_FixIP=true; } 
+	 bool fitTrack();
+	 void clear();
+
+	 bool fit2D();				/* fit in x-y and w-z respectively */
+	 bool mnFit3D();			/* fit with minuit */
+
+	 double getChisquare() const{return m_chisq;}
+	 double getTrkPar(int i) const;
+	 double getTrkParErr(int i) const{return m_trkParErr[i];}
+	 double getCovariance(int i, int k) const{return m_covariance[i][k];}
+
+	 double getDr() const{return m_trkPar[0];}
+	 double getDz() const{return m_trkPar[1];}
+	 double getPhi() const{return m_trkPar[2]+m_barAngle;}
+	 double getTheta() const{return m_trkPar[3];}
+
+	 static const int NTRKPAR = 4;
+
+	 static void fcnTrk(Int_t &npar, Double_t *gin, Double_t &f, Double_t *par, Int_t iflag);
+
+	 static std::vector<int> m_flagUZ; /* direction in transverse plane, 0 for u, 1 for z */
+	 static std::vector<double> m_uzPos; /* transverse position */
+	 static std::vector<double> m_uzPosErr; /* transverse position error */
+	 static std::vector<double> m_depth; /* longitudinal position */
+	 static std::vector<double> m_depthErr; /* longitudinal position error */
+
+private:
+	 double m_barAngle;
+	 double m_chisq;
+	 double m_trkPar[NTRKPAR];		/* dr, dz, phi, theta */
+	 double m_trkParErr[NTRKPAR];
+	 double m_covariance[NTRKPAR][NTRKPAR];
+    double m_IPvar[2];  //dr, dz
+    bool   m_FixIP; 
+};
+
+inline double TrackFitInEcal::getTrkPar(int i) const{
+	 if(2==i) return m_trkPar[i]+m_barAngle;
+	 else return m_trkPar[i];
+}
+
+#endif
diff --git a/Reconstruction/CrystalCaloRec/script/digi.py b/Reconstruction/CrystalCaloRec/script/digi.py
new file mode 100644
index 0000000000000000000000000000000000000000..b587c61d0a659b86a42a10309f0c96392a497757
--- /dev/null
+++ b/Reconstruction/CrystalCaloRec/script/digi.py
@@ -0,0 +1,115 @@
+#!/usr/bin/env python
+import os
+from Gaudi.Configuration import *
+
+from Configurables import k4DataSvc
+dsvc = k4DataSvc("EventDataSvc", input="Tracking_TDR_o1_v01_Pi-_10GeV.root")
+
+from Configurables import RndmGenSvc, HepRndm__Engine_CLHEP__RanluxEngine_
+seed = [12340]
+# rndmengine = HepRndm__Engine_CLHEP__RanluxEngine_() # The default engine in Gaudi                                                                                                                                            
+rndmengine = HepRndm__Engine_CLHEP__HepJamesRandom_("RndmGenSvc.Engine") # The default engine in Geant4                                                                                                                        
+rndmengine.SetSingleton = True
+rndmengine.Seeds = seed
+
+rndmgensvc = RndmGenSvc("RndmGenSvc")
+rndmgensvc.Engine = rndmengine.name()
+
+geometry_option = "TDR_o1_v01/TDR_o1_v01.xml"
+
+if not os.getenv("DETCRDROOT"):
+    print("Can't find the geometry. Please setup envvar DETCRDROOT." )
+    sys.exit(-1)
+
+geometry_path = os.path.join(os.getenv("DETCRDROOT"), "compact", geometry_option)
+if not os.path.exists(geometry_path):
+    print("Can't find the compact geometry file: %s"%geometry_path)
+    sys.exit(-1)
+
+from Configurables import GeomSvc
+geosvc = GeomSvc("GeomSvc")
+geosvc.compact = geometry_path
+
+from Configurables import MarlinEvtSeeder
+evtseeder = MarlinEvtSeeder("EventSeeder")
+
+from Configurables import GearSvc
+gearsvc = GearSvc("GearSvc")
+
+from Configurables import TrackSystemSvc
+tracksystemsvc = TrackSystemSvc("TrackSystemSvc")
+
+from Configurables import SimplePIDSvc
+pidsvc = SimplePIDSvc("SimplePIDSvc")
+cepcswdatatop = "/cvmfs/cepcsw.ihep.ac.cn/prototype/releases/data/latest"
+pidsvc.ParFile = os.path.join(cepcswdatatop, "CEPCSWData/offline-data/Service/SimplePIDSvc/data/dNdx_TPC.root")
+
+from Configurables import PodioInput
+podioinput = PodioInput("PodioReader", collections=[
+#    "EventHeader",
+    "MCParticle",
+    "EcalBarrelCollection", 
+    "EcalBarrelContributionCollection", 
+    "HcalBarrelCollection", 
+    "HcalBarrelContributionCollection", 
+    "CompleteTracks", 
+    "CompleteTracksParticleAssociation"
+    ])
+
+########## Digitalization ################
+
+##ECAL##
+from Configurables import EcalDigiAlg
+EcalDigi = EcalDigiAlg("EcalDigiAlg")
+EcalDigi.ReadOutName = "EcalBarrelCollection"
+EcalDigi.SimCaloHitCollection = "EcalBarrelCollection"
+EcalDigi.CaloHitCollection = "ECALBarrel"
+EcalDigi.CaloAssociationCollection = "ECALBarrelAssoCol"
+EcalDigi.CaloMCPAssociationCollection = "ECALBarrelParticleAssoCol"
+EcalDigi.SkipEvt = 0
+EcalDigi.Seed = 2079
+#Digitalization parameters
+EcalDigi.CalibrECAL = 1.
+EcalDigi.AttenuationLength = 7e10
+EcalDigi.TimeResolution = 0.5        #unit: ns
+EcalDigi.ChargeThresholdFrac = 0.05
+EcalDigi.Debug=1
+EcalDigi.WriteNtuple = 0
+EcalDigi.OutFileName = "Digi_ECAL.root"
+#########################################
+
+##HCAL##
+from Configurables import HcalDigiAlg
+HcalDigi = HcalDigiAlg("HcalDigiAlg")
+HcalDigi.ReadOutName = "HcalBarrelCollection"
+HcalDigi.SimCaloHitCollection = "HcalBarrelCollection"
+HcalDigi.CaloHitCollection = "HCALBarrel"
+HcalDigi.CaloAssociationCollection = "HCALBarrelAssoCol"
+HcalDigi.CaloMCPAssociationCollection = "HCALBarrelParticleAssoCol"
+HcalDigi.SkipEvt = 0
+HcalDigi.Seed = 2079
+#Digitalization parameters
+HcalDigi.MIPResponse = 0.01  # 0.5 MeV / MIP
+HcalDigi.MIPThreshold = 0.5    # Unit: MIP
+HcalDigi.CalibrHCAL = 1.
+HcalDigi.Debug=0
+HcalDigi.WriteNtuple = 0
+HcalDigi.OutFileName = "Digi_HCAL.root"
+
+
+# output
+from Configurables import PodioOutput
+out = PodioOutput("outputalg")
+out.filename = "CaloDigi_TDR_o1_v01_Pi-_10GeV.root"
+out.outputCommands = ["keep *"]
+
+# ApplicationMgr
+from Configurables import ApplicationMgr
+mgr = ApplicationMgr(
+    TopAlg = [podioinput, EcalDigi, HcalDigi, out],
+    EvtSel = 'NONE',
+    EvtMax = 10,
+    ExtSvc = [dsvc, rndmengine, rndmgensvc, geosvc],
+    HistogramPersistency = 'ROOT',
+    OutputLevel = ERROR
+)
diff --git a/Reconstruction/CrystalCaloRec/script/rec.py b/Reconstruction/CrystalCaloRec/script/rec.py
new file mode 100644
index 0000000000000000000000000000000000000000..51f1e6d654723991f162257196d82e5676047e49
--- /dev/null
+++ b/Reconstruction/CrystalCaloRec/script/rec.py
@@ -0,0 +1,139 @@
+import os
+from Gaudi.Configuration import *
+
+############## GeomSvc #################
+geometry_option = "TDR_o1_v01/TDR_o1_v01.xml"
+
+if not os.getenv("DETCRDROOT"):
+    print("Can't find the geometry. Please setup envvar DETCRDROOT." )
+    sys.exit(-1)
+
+geometry_path = os.path.join(os.getenv("DETCRDROOT"), "compact", geometry_option)
+if not os.path.exists(geometry_path):
+    print("Can't find the compact geometry file: %s"%geometry_path)
+    sys.exit(-1)
+
+from Configurables import GeomSvc
+geomsvc = GeomSvc("GeomSvc")
+geomsvc.compact = geometry_path
+#######################################
+
+########### k4DataSvc ####################
+from Configurables import k4DataSvc
+podioevent = k4DataSvc("EventDataSvc", input="CaloDigi_TDR_o1_v01_Pi-_10GeV.root")
+##########################################
+
+
+########## Podio Input ###################
+from Configurables import PodioInput
+inp = PodioInput("InputReader")
+inp.collections = [ "EcalBarrelCollection",
+                    "EcalBarrelContributionCollection",  
+                    "ECALBarrel", 
+                    "ECALBarrelParticleAssoCol",
+                    "HcalBarrelCollection", 
+                    "HcalBarrelContributionCollection", 
+                    "HCALBarrel",
+                    "HCALBarrelParticleAssoCol",
+                    "MCParticle", 
+                    "CompleteTracks", 
+                    "CompleteTracksParticleAssociation"]
+##########################################
+
+######### Reconstruction ################
+from Configurables import PandoraPlusPFAlg
+PandoraPlusPFAlg = PandoraPlusPFAlg("PandoraPlusPFAlg")
+##----Global parameters----
+PandoraPlusPFAlg.Seed = 1024
+PandoraPlusPFAlg.BField = 3.
+PandoraPlusPFAlg.Debug = 0
+PandoraPlusPFAlg.SkipEvt = 0
+PandoraPlusPFAlg.WriteAna = 1
+PandoraPlusPFAlg.AnaFileName = "RecAnaTuple_TDR_o1_v01_Pi-_10GeV.root"
+PandoraPlusPFAlg.UseTruthTrack = 0
+PandoraPlusPFAlg.EcalGlobalCalib = 1.05
+PandoraPlusPFAlg.HcalGlobalCalib = 4.5
+##----Readin collections----
+PandoraPlusPFAlg.MCParticleCollection = "MCParticle"
+PandoraPlusPFAlg.TrackCollections = ["CompleteTracks"]
+PandoraPlusPFAlg.MCRecoTrackParticleAssociationCollection = "CompleteTracksParticleAssociation"
+PandoraPlusPFAlg.ECalCaloHitCollections = ["ECALBarrel"]
+PandoraPlusPFAlg.ECalReadOutNames = ["EcalBarrelCollection"]
+PandoraPlusPFAlg.ECalMCPAssociationName = ["ECALBarrelParticleAssoCol"]
+PandoraPlusPFAlg.HCalCaloHitCollections = ["HCALBarrel"]
+PandoraPlusPFAlg.HCalReadOutNames = ["HcalBarrelCollection"]
+PandoraPlusPFAlg.HCalMCPAssociationName = ["HCALBarrelParticleAssoCol"]
+
+##--- Output collections ---
+PandoraPlusPFAlg.OutputPFO = "outputPFO";
+
+#----Algorithms----
+PandoraPlusPFAlg.AlgList = ["GlobalClusteringAlg",      #1
+                            "LocalMaxFindingAlg",       #2
+                            "TrackMatchingAlg",         #3
+                            "HoughClusteringAlg",       #4
+                            "ConeClustering2DAlg",      #5
+                            "AxisMergingAlg",           #6
+                            "EnergySplittingAlg",       #9
+                            "EnergyTimeMatchingAlg",    #11
+                            "HcalClusteringAlg",        #12
+                            "TruthClusteringAlg",       #15
+                            "TrackClusterConnectingAlg",  #16
+                            "PFOReclusteringAlg" ]  #17
+PandoraPlusPFAlg.AlgParNames = [ ["InputECALBars","OutputECAL1DClusters","OutputECALHalfClusters"],#1
+                                 ["OutputLocalMaxName"],#2
+                                 ["ReadinLocalMaxName","OutputLongiClusName"],#3
+                                 ["ReadinLocalMaxName","LeftLocalMaxName","OutputLongiClusName"],#4
+                                 ["ReadinLocalMaxName", "OutputLongiClusName"], #5
+                                 ["OutputAxisName"], #6
+                                 ["ReadinAxisName", "OutputClusName", "OutputTowerName"],  #9
+                                 ["ReadinHFClusterName", "ReadinTowerName","OutputClusterName"], #11
+                                 ["InputHCALHits", "OutputHCALClusters"], #12
+                                 ["DoECALClustering","DoHCALClustering","InputHCALHits","OutputHCALClusters"], #15
+                                 ["ReadinECALClusterName", "ReadinHCALClusterName", "OutputCombPFO"],  #16
+                                 ["ECALCalib", "HCALCalib", "MinAngleForNeuMerge"] ]#17
+PandoraPlusPFAlg.AlgParTypes = [ ["string","string","string"],#1
+                                 ["string"],#2
+                                 ["string","string"],#3
+                                 ["string","string","string"],#4
+                                 ["string","string"], #5
+                                 ["string"], #6
+                                 ["string","string","string"],  #9
+                                 ["string","string","string"], #11
+                                 ["string", "string"], #12
+                                 ["bool","bool","string","string"], #15
+                                 ["string","string","string"],  #16
+                                 ["double","double","double"] ]#17
+PandoraPlusPFAlg.AlgParValues = [ ["BarCol","Cluster1DCol","HalfClusterCol"],#1
+                                  ["AllLocalMax"],#2
+                                  ["AllLocalMax","TrackAxis"],#3
+                                  ["AllLocalMax","LeftLocalMax","HoughAxis"],#4
+                                  ["LeftLocalMax","ConeAxis"], #5
+                                  ["MergedAxis"], #6
+                                  ["MergedAxis","ESHalfCluster","ESTower"],  #9
+                                  ["ESHalfCluster","ESTower","EcalCluster"], #11
+                                  ["HCALBarrel", "SimpleHCALCluster"], #12
+                                  ["0","1","HCALBarrel","HCALCluster"], #15
+                                  ["EcalCluster", "SimpleHCALCluster", "outputPFO"],  #16
+                                  ["1.05","4.5","0.12"]  ]#17
+
+
+##############################################################################
+# POD I/O
+##############################################################################
+from Configurables import PodioOutput
+out = PodioOutput("outputalg")
+out.filename = "Rec_TDR_o1_v01_Pi-_10GeV.root"
+out.outputCommands = ["keep *"]
+
+
+########################################
+
+from Configurables import ApplicationMgr
+ApplicationMgr( 
+    TopAlg=[inp, PandoraPlusPFAlg, out ],
+    EvtSel="NONE",
+    EvtMax=10,
+    ExtSvc=[podioevent, geomsvc],
+    #OutputLevel=DEBUG
+)
diff --git a/Reconstruction/CrystalCaloRec/script/sim.py b/Reconstruction/CrystalCaloRec/script/sim.py
new file mode 100644
index 0000000000000000000000000000000000000000..3ffd55dac942f2bde67089aa8654a7afec9cfb98
--- /dev/null
+++ b/Reconstruction/CrystalCaloRec/script/sim.py
@@ -0,0 +1,103 @@
+#!/usr/bin/env python
+import os
+from Gaudi.Configuration import *
+
+from Configurables import k4DataSvc
+dsvc = k4DataSvc("EventDataSvc")
+
+from Configurables import RndmGenSvc, HepRndm__Engine_CLHEP__RanluxEngine_
+seed = [12340]
+# rndmengine = HepRndm__Engine_CLHEP__RanluxEngine_() # The default engine in Gaudi
+rndmengine = HepRndm__Engine_CLHEP__HepJamesRandom_("RndmGenSvc.Engine") # The default engine in Geant4
+rndmengine.SetSingleton = True
+rndmengine.Seeds = seed
+
+rndmgensvc = RndmGenSvc("RndmGenSvc")
+rndmgensvc.Engine = rndmengine.name()
+
+# option for standalone tracker study
+geometry_option = "TDR_o1_v01/TDR_o1_v01.xml"
+
+if not os.getenv("DETCRDROOT"):
+    print("Can't find the geometry. Please setup envvar DETCRDROOT." )
+    sys.exit(-1)
+
+geometry_path = os.path.join(os.getenv("DETCRDROOT"), "compact", geometry_option)
+if not os.path.exists(geometry_path):
+    print("Can't find the compact geometry file: %s"%geometry_path)
+    sys.exit(-1)
+
+from Configurables import GeomSvc
+geosvc = GeomSvc("GeomSvc")
+geosvc.compact = geometry_path
+
+##############################################################################
+# Physics Generator
+##############################################################################
+from Configurables import GenAlgo
+from Configurables import GtGunTool
+from Configurables import StdHepRdr
+from Configurables import SLCIORdr
+from Configurables import HepMCRdr
+from Configurables import GenPrinter
+
+gun = GtGunTool("GtGunTool")
+gun.PositionXs = [0]
+gun.PositionYs = [0]
+gun.PositionZs = [0]
+gun.Particles = ["pi-"]
+gun.EnergyMins = [10]
+gun.EnergyMaxs = [10]
+gun.ThetaMins  = [60]
+gun.ThetaMaxs  = [120]
+gun.PhiMins    = [0]
+gun.PhiMaxs    = [360]
+
+genprinter = GenPrinter("GenPrinter")
+
+genalg = GenAlgo("GenAlgo")
+genalg.GenTools = ["GtGunTool"]
+
+##############################################################################
+# Detector Simulation
+##############################################################################
+from Configurables import DetSimSvc
+detsimsvc = DetSimSvc("DetSimSvc")
+
+from Configurables import DetSimAlg
+detsimalg = DetSimAlg("DetSimAlg")
+detsimalg.RandomSeeds = seed
+# detsimalg.VisMacs = ["vis.mac"]
+detsimalg.RunCmds = [
+#    "/tracking/verbose 1",
+]
+detsimalg.AnaElems = [
+    # example_anatool.name()
+    # "ExampleAnaElemTool"
+    "Edm4hepWriterAnaElemTool"
+]
+detsimalg.RootDetElem = "WorldDetElemTool"
+
+from Configurables import TimeProjectionChamberSensDetTool
+tpc_sensdettool = TimeProjectionChamberSensDetTool("TimeProjectionChamberSensDetTool")
+tpc_sensdettool.TypeOption = 1
+
+from Configurables import MarlinEvtSeeder
+evtseeder = MarlinEvtSeeder("EventSeeder")
+    
+# output
+from Configurables import PodioOutput
+out = PodioOutput("outputalg")
+out.filename = "Sim_TDR_o1_v01_Pi-_10GeV.root"
+out.outputCommands = ["keep *"]
+
+# ApplicationMgr
+from Configurables import ApplicationMgr
+mgr = ApplicationMgr(
+    TopAlg = [genalg, detsimalg, out],
+    EvtSel = 'NONE',
+    EvtMax = 10,
+    ExtSvc = [rndmengine, rndmgensvc, dsvc, geosvc],
+    HistogramPersistency = 'ROOT',
+    OutputLevel = ERROR
+)
diff --git a/Reconstruction/CrystalCaloRec/script/tracking.py b/Reconstruction/CrystalCaloRec/script/tracking.py
new file mode 100644
index 0000000000000000000000000000000000000000..38f47ad30cfb343bad4c1fc8e21e6a2dcab218c3
--- /dev/null
+++ b/Reconstruction/CrystalCaloRec/script/tracking.py
@@ -0,0 +1,249 @@
+#!/usr/bin/env python
+import os
+from Gaudi.Configuration import *
+
+from Configurables import k4DataSvc
+dsvc = k4DataSvc("EventDataSvc", input="Sim_TDR_o1_v01_Pi-_10GeV.root")
+
+from Configurables import RndmGenSvc, HepRndm__Engine_CLHEP__RanluxEngine_
+seed = [12340]
+# rndmengine = HepRndm__Engine_CLHEP__RanluxEngine_() # The default engine in Gaudi                                                                                                                                            
+rndmengine = HepRndm__Engine_CLHEP__HepJamesRandom_("RndmGenSvc.Engine") # The default engine in Geant4                                                                                                                        
+rndmengine.SetSingleton = True
+rndmengine.Seeds = seed
+
+rndmgensvc = RndmGenSvc("RndmGenSvc")
+rndmgensvc.Engine = rndmengine.name()
+
+geometry_option = "TDR_o1_v01/TDR_o1_v01.xml"
+
+if not os.getenv("DETCRDROOT"):
+    print("Can't find the geometry. Please setup envvar DETCRDROOT." )
+    sys.exit(-1)
+
+geometry_path = os.path.join(os.getenv("DETCRDROOT"), "compact", geometry_option)
+if not os.path.exists(geometry_path):
+    print("Can't find the compact geometry file: %s"%geometry_path)
+    sys.exit(-1)
+
+from Configurables import GeomSvc
+geosvc = GeomSvc("GeomSvc")
+geosvc.compact = geometry_path
+
+from Configurables import MarlinEvtSeeder
+evtseeder = MarlinEvtSeeder("EventSeeder")
+
+from Configurables import GearSvc
+gearsvc = GearSvc("GearSvc")
+
+from Configurables import TrackSystemSvc
+tracksystemsvc = TrackSystemSvc("TrackSystemSvc")
+
+from Configurables import SimplePIDSvc
+pidsvc = SimplePIDSvc("SimplePIDSvc")
+cepcswdatatop = "/cvmfs/cepcsw.ihep.ac.cn/prototype/releases/data/latest"
+pidsvc.ParFile = os.path.join(cepcswdatatop, "CEPCSWData/offline-data/Service/SimplePIDSvc/data/dNdx_TPC.root")
+
+from Configurables import PodioInput
+podioinput = PodioInput("PodioReader", collections=[
+#    "EventHeader",
+    "MCParticle",
+    "VXDCollection",
+    "SITCollection",
+    "TPCCollection",
+    "SETCollection",
+    "FTDCollection"
+    ])
+
+# digitization
+vxdhitname  = "VXDTrackerHits"
+sithitname  = "SITTrackerHits"
+gashitname  = "TPCTrackerHits"
+sethitname  = "SETTrackerHits"
+setspname   = "SETSpacePoints"
+ftdhitname  = "FTDTrackerHits"
+ftdspname   = "FTDSpacePoints"
+from Configurables import PlanarDigiAlg
+digiVXD = PlanarDigiAlg("VXDDigi")
+digiVXD.SimTrackHitCollection = "VXDCollection"
+digiVXD.TrackerHitCollection = vxdhitname
+digiVXD.TrackerHitAssociationCollection = "VXDTrackerHitAssociation"
+digiVXD.ResolutionU = [0.004, 0.004, 0.004, 0.004, 0.004, 0.004]
+digiVXD.ResolutionV = [0.004, 0.004, 0.004, 0.004, 0.004, 0.004]
+digiVXD.UsePlanarTag = True
+digiVXD.ParameterizeResolution = False
+digiVXD.ParametersU = [5.60959e-03, 5.74913e-03, 7.03433e-03, 1.99516, -663.952, 3.752e-03, 0, -0.0704734, 0.0454867e-03, 1.07359]
+digiVXD.ParametersV = [5.60959e-03, 5.74913e-03, 7.03433e-03, 1.99516, -663.952, 3.752e-03, 0, -0.0704734, 0.0454867e-03, 1.07359]
+#digiVXD.OutputLevel = DEBUG
+
+digiSIT = PlanarDigiAlg("SITDigi")
+digiSIT.IsStrip = False
+digiSIT.SimTrackHitCollection = "SITCollection"
+digiSIT.TrackerHitCollection = sithitname
+digiSIT.TrackerHitAssociationCollection = "SITTrackerHitAssociation"
+digiSIT.ResolutionU = [0.0072]
+digiSIT.ResolutionV = [0.086]
+digiSIT.UsePlanarTag = True
+digiSIT.ParameterizeResolution = False
+digiSIT.ParametersU = [2.29655e-03, 0.965899e-03, 0.584699e-03, 17.0856, 84.566, 12.4695e-03, -0.0643059, 0.168662, 1.87998e-03, 0.514452]
+digiSIT.ParametersV = [1.44629e-02, 2.20108e-03, 1.03044e-02, 4.39195e+00, 3.29641e+00, 1.55167e+18, -5.41954e+01, 5.72986e+00, -6.80699e-03, 5.04095e-01]
+#digiSIT.OutputLevel = DEBUG
+
+digiSET = PlanarDigiAlg("SETDigi")
+digiSET.IsStrip = False
+digiSET.SimTrackHitCollection = "SETCollection"
+digiSET.TrackerHitCollection = sethitname
+digiSET.TrackerHitAssociationCollection = "SETTrackerHitAssociation"
+digiSET.ResolutionU = [0.0072]
+digiSET.ResolutionV = [0.086]
+digiSET.UsePlanarTag = True
+digiSET.ParameterizeResolution = False
+digiSET.ParametersU = [2.29655e-03, 0.965899e-03, 0.584699e-03, 17.0856, 84.566, 12.4695e-03, -0.0643059, 0.168662, 1.87998e-03, 0.514452]
+digiSET.ParametersV = [1.44629e-02, 2.20108e-03, 1.03044e-02, 4.39195e+00, 3.29641e+00, 1.55167e+18, -5.41954e+01, 5.72986e+00, -6.80699e-03, 5.04095e-01]
+#digiSET.OutputLevel = DEBUG
+
+digiFTD = PlanarDigiAlg("FTDDigi")
+digiFTD.IsStrip = False
+digiFTD.SimTrackHitCollection = "FTDCollection"
+digiFTD.TrackerHitCollection = ftdhitname
+digiFTD.TrackerHitAssociationCollection = "FTDTrackerHitAssociation"
+digiFTD.ResolutionU = [0.0072]
+digiFTD.ResolutionV = [0.086]
+digiFTD.UsePlanarTag = True
+digiFTD.ParameterizeResolution = False
+digiFTD.ParametersU = [2.29655e-03, 0.965899e-03, 0.584699e-03, 17.0856, 84.566, 12.4695e-03, -0.0643059, 0.168662, 1.87998e-03, 0.514452]
+digiFTD.ParametersV = [1.44629e-02, 2.20108e-03, 1.03044e-02, 4.39195e+00, 3.29641e+00, 1.55167e+18, -5.41954e+01, 5.72986e+00, -6.80699e-03, 5.04095e-01]
+#digiFTD.OutputLevel = DEBUG
+
+from Configurables import TPCDigiAlg
+digiTPC = TPCDigiAlg("TPCDigi")
+digiTPC.TPCCollection = "TPCCollection"
+digiTPC.TPCLowPtCollection = "TPCLowPtCollection"
+digiTPC.TPCTrackerHitsCol = gashitname
+#digiTPC.OutputLevel = DEBUG
+
+# tracking
+from Configurables import KalTestTool
+# Close multiple scattering and smooth, used by clupatra
+kt010 = KalTestTool("KalTest010")
+kt010.MSOn = False
+kt010.Smooth = False
+#kt010.OutputLevel = DEBUG
+
+# Open multiple scattering, energy loss and smooth (default)
+kt111 = KalTestTool("KalTest111")
+#kt111.OutputLevel = DEBUG
+
+# Close smooth
+kt110 = KalTestTool("KalTest110")
+kt110.Smooth = False
+#kt110.OutputLevel = DEBUG
+
+from Configurables import SiliconTrackingAlg
+tracking = SiliconTrackingAlg("SiliconTracking")
+tracking.LayerCombinationsFTD = []
+tracking.HeaderCol = "EventHeader"
+tracking.VTXHitCollection = vxdhitname
+tracking.SITHitCollection = sithitname
+tracking.FTDPixelHitCollection = ftdhitname
+tracking.FTDSpacePointCollection = ftdspname
+tracking.SITRawHitCollection = "NotNeedForPixelSIT"
+tracking.FTDRawHitCollection = ftdhitname
+tracking.UseSIT = True
+tracking.SmoothOn = False
+tracking.NDivisionsInTheta = 10
+tracking.NDivisionsInPhi = 60
+tracking.NDivisionsInPhiFTD = 16
+tracking.MinDistCutAttach = 50
+tracking.Chi2FitCut = 200
+tracking.MaxChi2PerHit = 200
+tracking.Chi2WZTriplet = 0.1
+tracking.Chi2WZQuartet = 0.1
+tracking.Chi2WZSeptet  = 0.1
+#tracking.FitterTool = "KalTestTool/KalTest111"
+#tracking.OutputLevel = DEBUG
+
+from Configurables import ForwardTrackingAlg
+forward = ForwardTrackingAlg("ForwardTracking")
+forward.FTDPixelHitCollection = ftdhitname
+forward.FTDSpacePointCollection = ftdspname
+forward.FTDRawHitCollection = ftdhitname
+forward.Chi2ProbCut = 0.0
+forward.HitsPerTrackMin = 3
+forward.BestSubsetFinder = "SubsetSimple"
+forward.Criteria = ["Crit2_DeltaPhi","Crit2_StraightTrackRatio","Crit3_3DAngle","Crit3_ChangeRZRatio","Crit3_IPCircleDist","Crit4_3DAngleChange","Crit4_DistToExtrapolation",
+                    "Crit2_DeltaRho","Crit2_RZRatio","Crit3_PT"]
+forward.CriteriaMin = [0,  0.9,  0,  0.995, 0,  0.8, 0,   20,  1.002, 0.1,      0,   0.99, 0,    0.999, 0,   0.99, 0]
+forward.CriteriaMax = [30, 1.02, 10, 1.015, 20, 1.3, 1.0, 150, 1.08,  99999999, 0.8, 1.01, 0.35, 1.001, 1.5, 1.01, 0.05]
+#forward.FitterTool = "KalTestTool/KalTest110"
+#forward.OutputLevel = DEBUG
+
+from Configurables import TrackSubsetAlg
+subset = TrackSubsetAlg("TrackSubset")
+subset.TrackInputCollections = ["ForwardTracks", "SiTracks"]
+subset.RawTrackerHitCollections = [vxdhitname, sithitname, ftdhitname, ftdspname]
+subset.TrackSubsetCollection = "SubsetTracks"
+#subset.FitterTool = "KalTestTool/KalTest111"
+#subset.OutputLevel = DEBUG
+
+from Configurables import ClupatraAlg
+clupatra = ClupatraAlg("Clupatra")
+clupatra.TPCHitCollection = gashitname
+#clupatra.OutputLevel = DEBUG
+
+from Configurables import FullLDCTrackingAlg
+full = FullLDCTrackingAlg("FullTracking")
+full.VTXTrackerHits = vxdhitname
+full.SITTrackerHits = sithitname
+full.TPCTrackerHits = gashitname
+full.SETTrackerHits = sethitname
+full.FTDPixelTrackerHits = ftdhitname
+full.FTDSpacePoints = ftdspname
+full.SITRawHits     = "NotNeedForPixelSIT"
+full.SETRawHits     = "NotNeedForPixelSET"
+full.FTDRawHits     = ftdhitname
+full.TPCTracks = "ClupatraTracks" # add standalone TPC track
+full.SiTracks  = "SubsetTracks"
+full.OutputTracks  = "CompleteTracks" # default name
+full.VTXHitToTrackDistance = 5.
+full.FTDHitToTrackDistance = 5.
+full.SITHitToTrackDistance = 3.
+full.SETHitToTrackDistance = 5.
+full.MinChi2ProbForSiliconTracks = 0
+#full.OutputLevel = DEBUG
+
+from Configurables import TPCDndxAlg
+tpc_dndx = TPCDndxAlg("TPCDndxAlg")
+tpc_dndx.Method = "Simple"
+
+from Configurables import TrackParticleRelationAlg
+tpr = TrackParticleRelationAlg("Track2Particle")
+tpr.MCParticleCollection = "MCParticle"
+tpr.TrackList = ["CompleteTracks", "ClupatraTracks"]
+tpr.TrackerAssociationList = ["VXDTrackerHitAssociation", "SITTrackerHitAssociation", "SETTrackerHitAssociation", "FTDTrackerHitAssociation", "TPCTrackerHitAss"]
+#tpr.OutputLevel = DEBUG
+
+from Configurables import TrueMuonTagAlg
+tmt = TrueMuonTagAlg("TrueMuonTag")
+tmt.MCParticleCollection = "MCParticle"
+tmt.TrackList = ["CompleteTracks"]
+tmt.MuonTagEfficiency = 0.95 # muon true tag efficiency, default is 1.0 (100%)
+tmt.MuonDetTanTheta = 1.2 # muon det barrel/endcap separation tan(theta)
+#tmt.OutputLevel = DEBUG
+
+# output
+from Configurables import PodioOutput
+out = PodioOutput("outputalg")
+out.filename = "Tracking_TDR_o1_v01_Pi-_10GeV.root"
+out.outputCommands = ["keep *"]
+
+# ApplicationMgr
+from Configurables import ApplicationMgr
+mgr = ApplicationMgr(
+    TopAlg = [podioinput, digiVXD, digiSIT, digiSET, digiFTD, digiTPC, tracking, forward, subset, clupatra, full, tpr, tpc_dndx, tmt, out],
+    EvtSel = 'NONE',
+    EvtMax = 10,
+    ExtSvc = [rndmengine, rndmgensvc, dsvc, evtseeder, geosvc, gearsvc, tracksystemsvc, pidsvc],
+    HistogramPersistency = 'ROOT',
+    OutputLevel = ERROR
+)
diff --git a/Reconstruction/CrystalCaloRec/src/Algorithm/ArborClusteringAlg.cpp b/Reconstruction/CrystalCaloRec/src/Algorithm/ArborClusteringAlg.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..1d03dcf07449b462283dab4ac3f7f78c290dbcab
--- /dev/null
+++ b/Reconstruction/CrystalCaloRec/src/Algorithm/ArborClusteringAlg.cpp
@@ -0,0 +1,509 @@
+#ifndef _ARBORCLUSTERING_ALG_C
+#define _ARBORCLUSTERING_ALG_C
+
+#include "Algorithm/ArborClusteringAlg.h"
+#include <map>
+
+void ArborClusteringAlg::Settings::SetInitialValue(){
+
+    clusType="";
+
+    Lth_start = 2; 
+    Rth_start = 30; 
+    Rth_value = 40; 
+    Rth_slope = 0.5; 
+
+    wiB = 1;
+    wiF = 1;
+    pTheta = 1;
+    pR = 1;
+    pE = 1; 
+    Rth_nbrRoot = 40; // Root node distance threshold when merging neighbor trees.
+    Debug = 0; 
+}
+
+void ArborClusteringAlg::Settings::PrintSettings() const{
+  std::cout<<"  ArborClusteringAlg Settings: "<<std::endl;
+  std::cout<<"Rth_value: "<<'\t'<<Rth_value<<std::endl; 
+  std::cout<<"Rth_slope: "<<'\t'<<Rth_slope<<std::endl; 
+  std::cout<<"wiB: "<<'\t'<<wiB<<std::endl; 
+  std::cout<<"wiF: "<<'\t'<<wiF<<std::endl; 
+  std::cout<<"pTheta: "<<'\t'<<pTheta<<std::endl; 
+  std::cout<<"pR: "<<'\t'<<pR<<std::endl; 
+  std::cout<<"pE: "<<'\t'<<pE<<std::endl; 
+  std::cout<<"Rth_nbrRoot: "<<'\t'<<Rth_nbrRoot<<std::endl; 
+  std::cout<<"Debug: "<<'\t'<<Debug<<std::endl; 
+
+}
+
+StatusCode ArborClusteringAlg::Initialize(){
+
+  return StatusCode::SUCCESS;
+}
+
+StatusCode ArborClusteringAlg::RunAlgorithm( ArborClusteringAlg::Settings& m_settings, PandoraPlusDataCol& m_datacol ){
+  settings = m_settings;
+
+  std::vector<CRDEcalEDM::CRDCaloHit2DShower> m_2DshowerCol; m_2DshowerCol.clear();
+  if( settings.clusType=="MIP" )     m_2DshowerCol = m_datacol.MIPShower2DCol;
+  else if( settings.clusType=="EM" ) m_2DshowerCol = m_datacol.EMShower2DCol;
+  else m_2DshowerCol = m_datacol.Shower2DCol;
+
+
+  if(m_2DshowerCol.size()==0){ 
+    std::cout<<"Warning: Empty input in ArborClusteringAlg. Please check previous algorithm!"<<endl;  
+    m_datacol.ClearArbor(); 
+    return StatusCode::SUCCESS; 
+  }
+//m_datacol.PrintShower(); 
+
+  std::map<int, std::vector<CRDEcalEDM::CRDArborNode*> > m_orderedNodes;  m_orderedNodes.clear(); //map<layer, showers>
+
+  //Transform showers to nodes, order the nodes by layer. 
+  for(int is=0; is<m_2DshowerCol.size(); is++ ){
+    int m_dlayer = m_2DshowerCol[is].getDlayer();
+    //CRDEcalEDM::CRDArborNode m_node(m_2DshowerCol[is]);
+    CRDEcalEDM::CRDArborNode* m_node = new CRDEcalEDM::CRDArborNode(m_2DshowerCol[is]);
+    m_orderedNodes[m_dlayer].push_back(m_node);
+  }
+
+  int NtotNodes=0; 
+  std::map<int, std::vector<CRDEcalEDM::CRDArborNode*> >::iterator iter_count = m_orderedNodes.begin(); 
+  for(iter_count; iter_count!=m_orderedNodes.end(); iter_count++){
+    NtotNodes += iter_count->second.size(); 
+  }
+  if(settings.Debug>=1) std::cout<<"Total node size: "<<NtotNodes<<std::endl; 
+
+/*
+std::cout<<"Print ordered nodes: "<<std::endl;
+for(auto iter=m_orderedNodes.begin(); iter!=m_orderedNodes.end(); iter++){
+  std::cout<<"#Layer: "<<iter->first<<'\t';
+  for(int i=0; i<iter->second.size(); i++) printf("(%.2f, %.2f, %.2f, %d) \t", iter->second[i].GetPosition().x(), iter->second[i].GetPosition().y(), iter->second[i].GetPosition().z(), iter->second[i].GetType());
+  std::cout<<std::endl;
+}
+*/
+  std::vector<CRDEcalEDM::CRDArborTree> m_ArborTreeCol; m_ArborTreeCol.clear();
+  std::vector<CRDEcalEDM::CRDArborNode*> m_isoNodes; m_isoNodes.clear();
+  
+
+  //Build tree
+  InitArborTree(m_orderedNodes, m_ArborTreeCol, m_isoNodes);
+
+  if(settings.Debug>=1) std::cout<<"Initial Ntrees = "<<m_ArborTreeCol.size()<<std::endl;
+  if(settings.Debug>=2){
+    for(int it=0; it<m_ArborTreeCol.size(); it++) m_ArborTreeCol[it].PrintTree();
+    std::cout<<"Print Isolated Nodes: "<<std::endl;
+    for(int i=0; i<m_isoNodes.size(); i++)
+      printf("(%.2f, %.2f, %.2f, %d) \n", m_isoNodes[i]->GetPosition().x(), m_isoNodes[i]->GetPosition().y(), m_isoNodes[i]->GetPosition().z(), m_isoNodes[i]->GetType());
+    std::cout<<std::endl;
+  }  
+
+
+  std::vector<CRDEcalEDM::CRDArborTree> tmpTrees; tmpTrees.clear();
+  MergeConnectedTrees( m_ArborTreeCol, tmpTrees );
+  m_ArborTreeCol.clear(); m_ArborTreeCol = tmpTrees;
+
+  if(settings.Debug>=1) std::cout<<"After ConnectedTree merging: Ntree = "<<m_ArborTreeCol.size()<<std::endl;
+  if(settings.Debug>=2){
+    for(int it=0; it<m_ArborTreeCol.size(); it++) m_ArborTreeCol[it].PrintTree();
+    std::cout<<"Print Isolated Nodes: "<<std::endl;
+    for(int i=0; i<m_isoNodes.size(); i++)
+      printf("(%.2f, %.2f, %.2f, %d) \n", m_isoNodes[i]->GetPosition().x(), m_isoNodes[i]->GetPosition().y(), m_isoNodes[i]->GetPosition().z(), m_isoNodes[i]->GetType());
+    std::cout<<std::endl;
+  }
+
+  //Clean connection
+  for(int it=0; it<m_ArborTreeCol.size(); it++) CleanConnection(m_ArborTreeCol[it]);
+
+  if(settings.Debug>=1) std::cout<<"After connection cleaning: Ntree = "<<m_ArborTreeCol.size()<<std::endl;
+  if(settings.Debug>=2){ 
+    for(int it=0; it<m_ArborTreeCol.size(); it++) m_ArborTreeCol[it].PrintTree();
+    std::cout<<"Print Isolated Nodes: "<<std::endl;
+    for(int i=0; i<m_isoNodes.size(); i++)
+      printf("(%.2f, %.2f, %.2f, %d) \n", m_isoNodes[i]->GetPosition().x(), m_isoNodes[i]->GetPosition().y(), m_isoNodes[i]->GetPosition().z(), m_isoNodes[i]->GetType());
+    std::cout<<std::endl;
+  }
+
+  //Depart trees after connection cleaning. 
+  std::vector<CRDEcalEDM::CRDArborTree> m_departedTrees; m_departedTrees.clear(); 
+  for(int it=0; it<m_ArborTreeCol.size(); it++){ 
+    tmpTrees.clear(); 
+    DepartArborTree(m_ArborTreeCol[it], tmpTrees, m_isoNodes);
+    m_departedTrees.insert(m_departedTrees.end(), tmpTrees.begin(), tmpTrees.end());
+    //m_ArborTreeCol.clear(); m_ArborTreeCol = tmpTrees; 
+  }
+  m_ArborTreeCol.clear(); m_ArborTreeCol = m_departedTrees; m_departedTrees.clear(); 
+
+  if(settings.Debug>=1) std::cout<<"After DepartArborTree: Ntree = "<<m_ArborTreeCol.size()<<std::endl;
+  if(settings.Debug>=2){ 
+    for(int it=0; it<m_ArborTreeCol.size(); it++) m_ArborTreeCol[it].PrintTree();
+    std::cout<<"Print Isolated Nodes: "<<std::endl;
+    for(int i=0; i<m_isoNodes.size(); i++)
+      printf("(%.2f, %.2f, %.2f, %d) \n", m_isoNodes[i]->GetPosition().x(), m_isoNodes[i]->GetPosition().y(), m_isoNodes[i]->GetPosition().z(), m_isoNodes[i]->GetType());
+    std::cout<<std::endl;
+  }
+
+  //Merge Neighbor tree (roots are close within the same layer)
+  //tmpTrees.clear();
+  //MergeNeighborTree(m_ArborTreeCol, tmpTrees);
+  //m_ArborTreeCol.clear(); m_ArborTreeCol = tmpTrees;
+
+  m_datacol.ArborTreeCol = m_ArborTreeCol; 
+  m_datacol.IsoNodes = m_isoNodes; 
+
+  int Nnodes_end = 0; 
+  for(int it=0; it<m_ArborTreeCol.size(); it++) Nnodes_end += m_ArborTreeCol[it].GetNodes().size(); 
+  Nnodes_end += m_isoNodes.size(); 
+  if(settings.Debug>=1) std::cout<<"At the end: Node size = "<<NtotNodes<<std::endl;
+  if(Nnodes_end != NtotNodes) 
+    std::cout<<"WARNING!  ArborClusteringAlg: Initial node size("<<NtotNodes<<") is not equal to final node size("<<Nnodes_end<<")! May have memory leakage! "<<std::endl; 
+
+  return StatusCode::SUCCESS;
+}
+
+StatusCode ArborClusteringAlg::ClearAlgorithm() {
+
+  return StatusCode::SUCCESS;
+}
+
+
+StatusCode ArborClusteringAlg::InitArborTree( std::map<int, std::vector<CRDEcalEDM::CRDArborNode*> >& m_orderedNodes, 
+                                              std::vector<CRDEcalEDM::CRDArborTree>& m_treeCol,
+                                              std::vector<CRDEcalEDM::CRDArborNode*>& m_isoNodes )
+{
+
+//std::cout<<"Ordered node collection size: "<<m_orderedNodes.size()<<std::endl;
+//std::cout<<"  Layer(Nnode): ";
+//for(auto iter = m_orderedNodes.begin(); iter!=m_orderedNodes.end(); iter++) std::cout<<iter->first<<"("<<iter->second.size()<<")  ";
+//std::cout<<std::endl;
+
+  if( m_orderedNodes.size()==0 ) return StatusCode::SUCCESS;
+  m_treeCol.clear();
+  m_isoNodes.clear();
+
+  std::map<int, std::vector<CRDEcalEDM::CRDArborNode*> >::iterator iter = m_orderedNodes.begin();
+
+  //Nodes in First layer: initialize the trees. 
+  for(int is=0; is<iter->second.size(); is++){
+//std::cout<<"#Layer: "<<iter->first<<", Nnode: "<<iter->second.size()<<endl;
+    int m_dlayer = iter->first; 
+    double Rth; 
+    if(m_dlayer<=settings.Lth_start) Rth = settings.Rth_start; 
+    else Rth = settings.Rth_value + (double)m_dlayer * settings.Rth_slope; 
+
+    CRDEcalEDM::CRDArborTree m_tree; 
+    std::vector<CRDEcalEDM::CRDArborNode*>* m_nextLayer = &m_orderedNodes[iter->first+1]; 
+    for(int js=0; js<m_nextLayer->size(); js++)
+      if( (iter->second[is]->GetPosition()-m_nextLayer->at(js)->GetPosition()).Mag()<Rth ) 
+        iter->second[is]->ConnectDaughter( (m_nextLayer->at(js)) ); 
+
+    m_tree.AddNode( (iter->second[is]) );
+    m_tree.AddNode( iter->second[is]->GetDaughterNodes() );
+    m_treeCol.push_back( m_tree );
+  }
+  iter++;
+//std::cout<<"End tree seeding. Tree number: "<< m_treeCol.size()<<", Next layer: "<<iter->first<<std::endl;
+//for(int it=0; it<m_treeCol.size(); it++) m_treeCol[it].PrintTree();
+
+  //In later layers
+  for(iter; iter!=m_orderedNodes.end(); iter++){
+
+//std::cout<<"  #Layer: "<<iter->first<<", Nnode: "<<iter->second.size()<<", Present Ntrees: "<<m_treeCol.size()<<std::endl;
+//for(int it=0; it<m_treeCol.size(); it++) m_treeCol[it].PrintTree();
+
+    int m_dlayer = iter->first;
+    double Rth;
+    if(m_dlayer<=settings.Lth_start) Rth = settings.Rth_start;
+    else Rth = settings.Rth_value + (double)m_dlayer * settings.Rth_slope;
+
+    for(int in=0; in<iter->second.size(); in++){
+      CRDEcalEDM::CRDArborNode* m_node = iter->second[in];
+//printf("    #Node: (%.2f, %.2f, %.2f) \n", m_node->GetPosition().x(),  m_node->GetPosition().y(),  m_node->GetPosition().z());
+      //Loop all trees:
+      for(int it=0; it<m_treeCol.size(); it++){
+//std::cout<<"    Loop in tree: "<<std::endl;
+//m_treeCol[it].PrintTree();
+//std::cout<<"    Node in this tree: "<<m_node->isInTree(m_treeCol[it])<<std::endl; 
+        //If node in this tree: 
+        if( m_node->isInTree(m_treeCol[it]) ){          
+          //Get daughter nodes and add them in tree
+          std::vector<CRDEcalEDM::CRDArborNode*>* m_nextLayer = &m_orderedNodes[m_dlayer+1];
+          for(int jn=0; jn<m_nextLayer->size(); jn++)
+            if( (m_node->GetPosition()-m_nextLayer->at(jn)->GetPosition()).Mag()<Rth ) 
+              m_node->ConnectDaughter( (m_nextLayer->at(jn)) );
+          m_treeCol[it].AddNode( m_node->GetDaughterNodes() );
+          break;
+        }
+      }//end loop trees
+//std::cout<<"End loop nodes in this layer! "<<std::endl; 
+//std::cout<<std::endl;
+    } //end loop nodes in this layer
+
+    //if collection is not empty, create a new tree.
+
+    for(int in=0; in<iter->second.size(); in++){
+      CRDEcalEDM::CRDArborNode* m_node = iter->second[in];
+      if( m_node->GetDaughterNodes().size()==0 && m_node->GetParentNodes().size()==0){
+        CRDEcalEDM::CRDArborTree m_tree;
+        m_tree.AddNode( m_node );
+        std::vector<CRDEcalEDM::CRDArborNode*>* m_nextLayer = &m_orderedNodes[m_dlayer+1];
+        for(int jn=0; jn<m_nextLayer->size(); jn++)
+          if( (m_node->GetPosition() - m_nextLayer->at(jn)->GetPosition()).Mag()<Rth ) 
+            m_node->ConnectDaughter( (m_nextLayer->at(jn)) );        
+        m_tree.AddNode( m_node->GetDaughterNodes() );
+        m_treeCol.push_back( m_tree );
+      }
+    } //end new tree.
+
+  }//end loop all nodes.
+
+//std::cout<<"End loop all nodes! Print all trees: "<<std::endl;
+//for(int it=0; it<m_treeCol.size(); it++) m_treeCol[it].PrintTree();
+
+  int NtotNodes=0;
+//  for(int it=0; it<m_treeCol.size(); it++){
+//    NtotNodes += m_treeCol[it].GetNodes().size();
+//  }
+//  std::cout<<"Total nodes before picking out isoNodes: "<<NtotNodes<<std::endl;
+//  std::cout<<"Print all trees: "<<std::endl;
+//  for(int it=0; it<m_treeCol.size(); it++) m_treeCol[it].PrintTree();
+
+  //Loop treeCol, pickout isoNodes, update the orderedNodes.
+  for(int it=0; it<m_treeCol.size(); it++){
+    if(m_treeCol[it].GetNodes().size()==1){ 
+      CRDEcalEDM::CRDArborNode* m_node = m_treeCol[it].GetNodes()[0];
+      m_node->SetType(0);
+      m_isoNodes.push_back( m_node );
+      m_treeCol.erase( m_treeCol.begin()+it );
+      it--;
+    }
+  }
+
+  NtotNodes=0;
+  for(int it=0; it<m_treeCol.size(); it++){
+    NtotNodes += m_treeCol[it].GetNodes().size();
+  }
+//std::cout<<"Final tree size: "<<m_treeCol.size(); 
+//std::cout<<"  Total nodes in tree: "<<NtotNodes; 
+//std::cout<<"  Isolated note size: "<< m_isoNodes.size()<<std::endl;
+
+  return StatusCode::SUCCESS;
+}
+
+
+StatusCode ArborClusteringAlg::MergeConnectedTrees( std::vector<CRDEcalEDM::CRDArborTree>& m_inTreeCol,
+                                                    std::vector<CRDEcalEDM::CRDArborTree>& m_outTreeCol )
+{
+  m_outTreeCol.clear(); 
+  if(m_inTreeCol.size()==0){ return StatusCode::SUCCESS;}
+  if(m_inTreeCol.size()==1){ m_outTreeCol=m_inTreeCol; return StatusCode::SUCCESS;}
+
+  //Create a tree with topological link, with one node as seed. 
+  for(int it=0; it<m_inTreeCol.size(); it++){
+    m_inTreeCol[it].SortNodes(); 
+    std::vector<CRDEcalEDM::CRDArborNode*> m_nodeCol = m_inTreeCol[it].GetNodes(); 
+    if(m_nodeCol.size()==0) continue; 
+
+    CRDEcalEDM::CRDArborTree m_tree; 
+    m_tree.CreateWithTopo( m_nodeCol[0] );
+    m_outTreeCol.push_back(m_tree);
+    break;
+  }
+
+  //Check if other nodes in this tree. If not, create a new tree. 
+  for(int it=0; it<m_inTreeCol.size(); it++){
+    std::vector<CRDEcalEDM::CRDArborNode*> m_nodeCol = m_inTreeCol[it].GetNodes();
+    for(int in=0; in<m_nodeCol.size(); in++){
+      bool f_inTree = false; 
+      for(int jt=0; jt<m_outTreeCol.size(); jt++)
+        if( m_nodeCol[in]->isInTree(m_outTreeCol[jt]) ) {f_inTree=true; break;}
+      if(!f_inTree){
+        CRDEcalEDM::CRDArborTree m_tree;
+        m_tree.CreateWithTopo( m_nodeCol[in] );
+        m_outTreeCol.push_back(m_tree);
+      }      
+    }
+
+  }
+//std::cout<<"Ntrees before: "<<m_inTreeCol.size()<<", Ntrees after: "<<m_outTreeCol.size()<<std::endl;
+
+  return StatusCode::SUCCESS;
+}
+
+
+StatusCode ArborClusteringAlg::CleanConnection( CRDEcalEDM::CRDArborTree& m_tree ){
+
+  m_tree.SortNodes(); 
+//std::cout<<"First node: Layer="<<m_tree.GetNodes()[0]->GetDlayer()<<" Type="<<m_tree.GetNodes()[0]->GetType()<<std::endl;
+
+  std::vector<CRDEcalEDM::CRDArborNode*> m_nodeCol = m_tree.GetNodes();
+  std::vector< std::pair<CRDEcalEDM::CRDArborNode*, CRDEcalEDM::CRDArborNode*> > m_connectorCol; m_connectorCol.clear();  
+  for(int in=0; in<m_nodeCol.size(); in++){
+    if(in==0 && m_nodeCol[in]->GetDaughterNodes().size()!=0 ){ std::cout<<"WARNING in ArborClusteringAlg: Last node still have daughters! Check it!"<<std::endl; continue; }
+
+//std::cout<<"#Layer: "<<m_nodeCol[in]->GetDlayer()<<" Parent size:"<<m_nodeCol[in]->GetParentNodes().size()<<", Daughter size: "<<m_nodeCol[in]->GetDaughterNodes().size()<<std::endl;
+
+    //Get the connection with minimum kappa order
+    TVector3 Cref = m_nodeCol[in]->GetRefDir(settings.wiF, settings.wiB);
+    double kappaMin = 9999.;
+    double deltaMax = -999.; 
+    double deltaEmax = -999.; 
+    CRDEcalEDM::CRDArborNode* m_foundnode = nullptr;
+    std::vector<CRDEcalEDM::CRDArborNode*> m_parentNodes = m_nodeCol[in]->GetParentNodes();
+    if(m_parentNodes.size()<=1) continue; 
+
+    for(int ip=0; ip<m_parentNodes.size(); ip++){
+      TVector3 relP = m_nodeCol[in]->GetPosition() - m_parentNodes[ip]->GetPosition();
+      if(relP.Mag()>deltaMax) deltaMax = relP.Mag();
+      double deltaE = fabs( m_nodeCol[in]->GetEnergy()-m_parentNodes[ip]->GetEnergy() ); 
+      if(deltaE>deltaEmax) deltaEmax = deltaE; 
+    }
+
+    for(int ip=0; ip<m_parentNodes.size(); ip++){
+      TVector3 relP = m_nodeCol[in]->GetPosition() - m_parentNodes[ip]->GetPosition();
+      double theta  = relP.Angle(Cref)/(PI) ;
+      double deltaR = relP.Mag();
+      double deltaE = fabs( m_nodeCol[in]->GetEnergy()-m_parentNodes[ip]->GetEnergy() ); 
+
+      double kappa = pow(theta, settings.pTheta) * pow(deltaR/deltaMax, settings.pR ) * pow(deltaE/deltaEmax, settings.pE ); 
+//std::cout<<"   Node kappa order: "<<theta<<"  "<<deltaR<<"  "<<kappa<<std::endl;
+      if(kappa<kappaMin) { kappaMin=kappa; m_foundnode=m_parentNodes[ip]; }
+    }
+//std::cout<<"  Min Kappa order: "<<kappaMin<<std::endl;
+    if( m_foundnode==nullptr ){ std::cout<<"WARNING in ArborClusteringAlg: Did not find minimum kappa order!"<<std::endl; continue; }
+
+    //Save two nodes into connector collection
+    for(int ip=0; ip<m_parentNodes.size(); ip++){
+      if(m_parentNodes[ip]==m_foundnode) continue; 
+      std::pair<CRDEcalEDM::CRDArborNode*, CRDEcalEDM::CRDArborNode*> m_connector;
+      m_connector.first  = m_nodeCol[in]; 
+      m_connector.second = m_parentNodes[ip]; 
+      m_connectorCol.push_back( m_connector );
+    }
+  }
+
+  //Clean the connections in tree
+//std::cout<<" Connection size: "<<m_connectorCol.size()<<std::endl;
+
+  m_tree.CleanConnection( m_connectorCol );
+  m_tree.NodeClassification(); 
+
+  return StatusCode::SUCCESS;
+}
+
+
+StatusCode ArborClusteringAlg::DepartArborTree( CRDEcalEDM::CRDArborTree& m_tree, 
+                                                std::vector<CRDEcalEDM::CRDArborTree>& m_departedTrees,
+                                                std::vector<CRDEcalEDM::CRDArborNode*>& m_isoNodes ) 
+{
+//std::cout<<std::endl;
+//std::cout<<"ArborClusteringAlg::DepartArborTree  Input tree node size: "<<m_tree.GetNodes().size()<<std::endl;
+//std::cout<<"ArborClusteringAlg::DepartArborTree  Input isonode size: "<<m_isoNodes.size()<<std::endl;
+
+  std::vector<CRDEcalEDM::CRDArborNode*> m_rootNodes; m_rootNodes.clear(); 
+  for(int in=0; in<m_tree.GetNodes().size(); in++){
+    CRDEcalEDM::CRDArborNode* m_node = m_tree.GetNodes()[in];
+//printf("  Debug: Loop in node: (%.2f, %.2f, %.2f, %d) \n", m_node->GetPosition().x(), m_node->GetPosition().y(), m_node->GetPosition().z(), m_node->GetType() );
+    if(m_node->GetType()==0){ m_isoNodes.push_back(m_node); m_tree.CleanNode(m_node); in--; }
+    if(m_node->GetType()==4 || m_node->GetType()==5) m_rootNodes.push_back(m_node); 
+//std::cout<<"  Debug: Iso node size: "<<m_isoNodes.size()<<"  Root node size: "<<m_rootNodes.size()<<std::endl;
+  }
+//std::cout<<"ArborClusteringAlg::DepartArborTree  isonode size after classification: "<<m_isoNodes.size()<<std::endl;
+//std::cout<<"ArborClusteringAlg::DepartArborTree  tree node size after classification: "<<m_tree.GetNodes().size()<<std::endl;
+//std::cout<<"ArborClusteringAlg::DepartArborTree  Root node size: "<<m_rootNodes.size()<<std::endl;
+//std::cout<<std::endl;
+
+  //Only one tree: 
+  if( m_rootNodes.size()<=1 ){ 
+    if(m_rootNodes.size()==0) std::cout<<"ERROR in ArborClusteringAlg::DepartArborTree: Did not find root node! Check it!"<<std::endl; 
+    m_departedTrees.push_back(m_tree);
+    return StatusCode::SUCCESS;
+  }
+
+  //Multiple trees: 
+  else{
+  for(int ir=0; ir<m_rootNodes.size(); ir++){
+    CRDEcalEDM::CRDArborTree m_newTree; m_newTree.Clear(); 
+    m_newTree.CreateWithRoot( m_rootNodes[ir] ); //Create a tree with recursion 
+    m_departedTrees.push_back(m_newTree);
+  }}
+
+  return StatusCode::SUCCESS;
+}
+
+
+StatusCode ArborClusteringAlg::MergeNeighborTree( std::vector<CRDEcalEDM::CRDArborTree>& m_inTreeCol, 
+                                                  std::vector<CRDEcalEDM::CRDArborTree>& m_outTreeCol )
+{
+  if(m_inTreeCol.size()<=1){ m_outTreeCol=m_inTreeCol; return StatusCode::SUCCESS; }
+
+  std::vector<CRDEcalEDM::CRDArborNode*> m_rootCol; m_rootCol.clear();
+  m_rootCol.resize(m_inTreeCol.size());
+  for(int it=0; it<m_inTreeCol.size(); it++)
+    m_rootCol[it] = m_inTreeCol[it].GetRootNode();
+
+  //Merge trees whose root nodes are in the same layer and close to each other. 
+  for(int in=0; in<m_rootCol.size(); in++){
+  for(int jn=in+1; jn<m_rootCol.size(); jn++){
+    if( m_rootCol[in]->GetDlayer() != m_rootCol[jn]->GetDlayer() ) continue; 
+    if( (m_rootCol[in]->GetPosition()-m_rootCol[jn]->GetPosition()).Mag()<settings.Rth_nbrRoot ){
+      CRDEcalEDM::CRDArborTree m_newTree;
+      m_newTree.AddNode( m_inTreeCol[in].GetNodes() );
+      m_newTree.AddNode( m_inTreeCol[jn].GetNodes() );
+      m_outTreeCol.push_back( m_newTree );
+    }
+  }}
+
+  return StatusCode::SUCCESS;
+}
+
+/*
+StatusCode ArborClusteringAlg::MergeBranches( std::vector<CRDEcalEDM::CRDArborTree>& m_inTreeCol, 
+                                              std::vector<CRDEcalEDM::CRDArborTree>& m_goodTreeCol, 
+                                              std::vector<CRDEcalEDM::CRDArborTree>& m_badTreeCol)
+{
+  if(m_inTreeCol.size()==0) return StatusCode::SUCCESS; 
+
+  m_goodTreeCol.clear(); m_badTreeCol.clear(); 
+
+  for(int it=0; it<m_inTreeCol.size(); it++){
+    if( (m_inTreeCol[it].GetMaxDlayer()-m_inTreeCol[it].GetMinDlayer()) >= settings.fl_GoodTreeLevel && 
+         m_inTreeCol[it].GetNodes().size()>=settings.th_GoodTreeNodes) 
+      m_goodTreeCol.push_back( m_inTreeCol[it] ); 
+    else m_badTreeCol.push_back( m_inTreeCol[it] );
+  }
+
+//std::cout<<"  ArborClusteringAlg::MergeBranches  Ngoodtrees: "<<m_goodTreeCol.size()<<"  Nbadtrees: "<<m_badTreeCol.size()<<std::endl;
+
+//  for(int it=0; it<m_badTreeCol.size(); it++){
+//    CRDEcalEDM::CRDArborTree m_tree = GetClosestTree( m_badTreeCol[it], m_goodTreeCol );
+//    std::vector<CRDEcalEDM::CRDArborTree>::iterator iter = find(m_goodTreeCol.begin(), m_goodTreeCol.end(), m_tree);
+//    if( iter==m_goodTreeCol.end() ) { cout<<"Warning in MergeBranches: Tree Merging Fail!"<<std::endl; continue; }
+
+//    iter->AddNode( m_badTreeCol[it].GetNodes() );
+//  }
+
+
+  return StatusCode::SUCCESS;
+}
+
+
+CRDEcalEDM::CRDArborTree ArborClusteringAlg::GetClosestTree( CRDEcalEDM::CRDArborTree m_badTree, std::vector<CRDEcalEDM::CRDArborTree> m_goodTreeCol ){
+
+  CRDEcalEDM::CRDArborTree m_tree; m_tree.Clear(); 
+
+  TVector3 cent_bad = m_badTree.GetBarycenter();
+  double minR = 9999; 
+  int index = -1; 
+  for(int it=0; it<m_goodTreeCol.size(); it++){
+    TVector3 cent_good = m_goodTreeCol[it].GetBarycenter();
+    double dis = (cent_bad-cent_good).Mag();
+    if(dis<minR) { minR = dis; index = it;}
+  }
+
+  if(index<0){ std::cout<<"Warning in ArborClusteringAlg::GetClosestTree: Can not find closest good tree!"<<std::endl; return m_tree; }
+  m_tree = m_goodTreeCol[index];
+  return m_tree; 
+}
+*/
+#endif
diff --git a/Reconstruction/CrystalCaloRec/src/Algorithm/ArborTreeMergingAlg.cpp b/Reconstruction/CrystalCaloRec/src/Algorithm/ArborTreeMergingAlg.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..beade7ae81b709a6a931c27c7b0cd18f9a30f8c6
--- /dev/null
+++ b/Reconstruction/CrystalCaloRec/src/Algorithm/ArborTreeMergingAlg.cpp
@@ -0,0 +1,281 @@
+#ifndef _ARBORTREEMERGING_ALG_C
+#define _ARBORTREEMERGING_ALG_C
+#include "Algorithm/ArborTreeMergingAlg.h"
+void ArborTreeMergingAlg::Settings::SetInitialValue(){
+
+    th_Nsigma = 2; 
+    th_daughterR = 40.; 
+    th_GoodTreeLayer1 = 6;
+    th_GoodTreeLayer2 = 3;
+    th_GoodTreeNodes = 10;
+    th_MergeR = 50; 
+    th_MergeTheta = PI/10.;
+    fl_MergeTrees = true; 
+    fl_overwrite = true;
+    clusType = "";
+}
+
+StatusCode ArborTreeMergingAlg::Initialize(){
+
+  return StatusCode::SUCCESS;
+}
+
+StatusCode ArborTreeMergingAlg::RunAlgorithm( ArborTreeMergingAlg::Settings& m_settings, PandoraPlusDataCol& m_datacol){
+  settings = m_settings; 
+
+  std::vector<CRDEcalEDM::CRDCaloHit2DShower> m_2DshowerCol; m_2DshowerCol.clear();
+  if( settings.clusType=="MIP" )      m_2DshowerCol = m_datacol.MIPShower2DCol;
+  else if( settings.clusType=="EM")  m_2DshowerCol = m_datacol.EMShower2DCol;
+  else m_2DshowerCol = m_datacol.Shower2DCol;
+  std::vector<CRDEcalEDM::CRDArborTree> m_ArborTreeCol = m_datacol.ArborTreeCol; 
+  std::vector<CRDEcalEDM::CRDArborNode*> m_isoNodes = m_datacol.IsoNodes; 
+
+  if(m_2DshowerCol.size()==0){ 
+    std::cout<<"Warning: Empty input in ArborTreeMergingAlg. Please check previous algorithm!"<<endl;  
+//std::cout<<"  Tree size: "<<m_ArborTreeCol.size()<<"  isoNode size: "<<m_isoNodes.size()<<std::endl;
+//  std::cout<<"Print Tree: "<<std::endl;
+//  for(int it=0; it<m_ArborTreeCol.size(); it++) m_ArborTreeCol[it].PrintTree();
+//  std::cout<<"Print Isolated Nodes: "<<std::endl;
+//  for(int i=0; i<m_isoNodes.size(); i++)
+//    printf("(%.2f, %.2f, %.2f, %d) \n", m_isoNodes[i]->GetPosition().x(), m_isoNodes[i]->GetPosition().y(), m_isoNodes[i]->GetPosition().z(), m_isoNodes[i]->GetType());
+    for(int i=0; i<m_ArborTreeCol.size(); i++) { m_ArborTreeCol[i].Clear(); }
+    for(int i=0; i<m_isoNodes.size(); i++)    { delete m_isoNodes[i]; }
+    return StatusCode::SUCCESS; 
+  }
+
+//std::cout<<"  Tree size: "<<m_ArborTreeCol.size()<<"  isoNode size: "<<m_isoNodes.size()<<std::endl;
+//  std::cout<<"Print Tree: "<<std::endl;
+//  for(int it=0; it<m_ArborTreeCol.size(); it++) m_ArborTreeCol[it].PrintTree();
+//  std::cout<<"Print Isolated Nodes: "<<std::endl;
+//  for(int i=0; i<m_isoNodes.size(); i++)
+//    printf("(%.2f, %.2f, %.2f, %d) \n", m_isoNodes[i]->GetPosition().x(), m_isoNodes[i]->GetPosition().y(), m_isoNodes[i]->GetPosition().z(), m_isoNodes[i]->GetType());
+
+
+  //Classify good and bad Tree.
+  std::vector<CRDEcalEDM::CRDArborTree*> m_goodTreeCol; m_goodTreeCol.clear();
+  std::vector<CRDEcalEDM::CRDArborTree*> m_badTreeCol; m_badTreeCol.clear();
+  for(int it=0; it<m_ArborTreeCol.size(); it++){
+//std::cout<<"    MinDlayer: "<<m_ArborTreeCol[it].GetMinDlayer()<<", MaxDlayer: "<<m_ArborTreeCol[it].GetMaxDlayer()<<", Node size: "<<m_ArborTreeCol[it].GetNodes().size()<<std::endl;
+
+    if(  ( (m_ArborTreeCol[it].GetMaxDlayer()-m_ArborTreeCol[it].GetMinDlayer()) >= settings.th_GoodTreeLayer1 ) ||
+         ( (m_ArborTreeCol[it].GetMaxDlayer()-m_ArborTreeCol[it].GetMinDlayer()) >= settings.th_GoodTreeLayer2 &&
+         m_ArborTreeCol[it].GetNodes().size()>=settings.th_GoodTreeNodes) )
+      m_goodTreeCol.push_back( &m_ArborTreeCol[it] );
+    else m_badTreeCol.push_back( &m_ArborTreeCol[it] );
+  }
+//std::cout<<"  Good tree size: "<<m_goodTreeCol.size()<<", Bad tree size: "<<m_badTreeCol.size()<<std::endl;
+
+  if(settings.fl_MergeTrees){
+  std::map<CRDEcalEDM::CRDArborNode*, CRDEcalEDM::CRDArborTree*> m_svtx; m_svtx.clear();
+  //Type 1 secondary vertex: large variation in variance. 
+  GetSecondaryVtxT1(m_ArborTreeCol, m_2DshowerCol, m_svtx);
+  //Type 2 secondary vertex: branch node in a tree & large distance between daughter nodes.
+  //GetSecondaryVtxT2(m_ArborTreeCol, m_svtx);
+
+//std::cout<<"  Vtx size: "<<m_svtx.size()<<std::endl;
+  //Merge clusters pointing to the vertex. 
+  std::map<CRDEcalEDM::CRDArborNode*, CRDEcalEDM::CRDArborTree*>::iterator iter = m_svtx.begin(); 
+  for(iter; iter!=m_svtx.end(); iter++){
+
+    //Get vertex
+    CRDEcalEDM::CRDArborNode* m_node = iter->first; 
+    int m_layer = m_node->GetDlayer(); 
+    TVector3 m_vtxPos = m_node->GetPosition(); 
+
+//printf("vtx: (%.2f, %.2f, %.2f) (Layer %d) \n", m_vtxPos.x(), m_vtxPos.y(), m_vtxPos.z(), m_layer);
+
+    for(int it=0; it<m_goodTreeCol.size(); it++){
+//std::cout<<"  MinDlayer: "<<m_goodTreeCol[it]->GetMinDlayer()<<", MaxDlayer: "<<m_goodTreeCol[it]->GetMaxDlayer()<<std::endl;
+      if( m_goodTreeCol[it]->GetMinDlayer()<0      || m_goodTreeCol[it]->GetMaxDlayer()<0 ||  
+          (m_layer>m_goodTreeCol[it]->GetMinDlayer() && m_layer<m_goodTreeCol[it]->GetMaxDlayer()) ) continue; 
+
+      m_goodTreeCol[it]->SortNodes();  //Sort from outer layer to inner layer. 
+//m_goodTreeCol[it]->PrintTree();
+
+      TVector3 m_nodePos; 
+      if( m_layer<=m_goodTreeCol[it]->GetMinDlayer() )   m_nodePos = m_goodTreeCol[it]->GetNodes().back()->GetPosition(); 
+      if( m_layer>=m_goodTreeCol[it]->GetMaxDlayer() )   m_nodePos = m_goodTreeCol[it]->GetNodes().front()->GetPosition(); 
+
+      CRDEcalEDM::CRDCaloHit3DCluster m_clus = m_goodTreeCol[it]->ConvertTreeToCluster(); 
+      m_clus.FitAxis(); 
+      TVector3 m_treeAxis = m_clus.getAxis(); 
+
+//printf("  Root/leaf node: (%.2f, %.2f, %.2f) \t", m_nodePos.x(), m_nodePos.y(), m_nodePos.z());
+//printf("  cluster axis: (%.2f, %.2f, %.2f) \n", m_treeAxis.x(), m_treeAxis.y(), m_treeAxis.z()); 
+
+      //Merge clusters
+      if( (m_vtxPos-m_nodePos).Mag()<settings.th_MergeR && 
+          ( m_treeAxis.Angle(m_vtxPos-m_nodePos)<settings.th_MergeTheta || (PI-m_treeAxis.Angle(m_vtxPos-m_nodePos))<settings.th_MergeTheta ) ){
+        iter->second->AddNode( m_goodTreeCol[it]->GetNodes() );
+        m_goodTreeCol.erase( m_goodTreeCol.begin()+it );
+        it--; 
+      }
+    }
+  }
+  }
+
+
+  //Re-classify trees after merging: 
+  for(int it=0; it<m_badTreeCol.size(); it++){
+    if(  ( (m_badTreeCol[it]->GetMaxDlayer()-m_badTreeCol[it]->GetMinDlayer()) >= settings.th_GoodTreeLayer1 ) ||
+         ( (m_badTreeCol[it]->GetMaxDlayer()-m_badTreeCol[it]->GetMinDlayer()) >= settings.th_GoodTreeLayer2 &&
+         m_badTreeCol[it]->GetNodes().size()>=settings.th_GoodTreeNodes) ){
+      m_goodTreeCol.push_back( m_badTreeCol[it] );
+      m_badTreeCol.erase( m_badTreeCol.begin()+it );
+      it--; 
+    }
+  }
+
+//std::cout<<"  Good tree size: "<<m_goodTreeCol.size()<<", Bad tree size: "<<m_badTreeCol.size()<<std::endl;
+  //Save Trees into CRDCaloHit3DCluster
+  std::vector<CRDEcalEDM::CRDCaloHit3DCluster> m_goodClusterCol;  m_goodClusterCol.clear();
+  std::vector<CRDEcalEDM::CRDCaloHit3DCluster> m_badClusterCol;  m_badClusterCol.clear();
+  std::vector<CRDEcalEDM::CRDCaloHit3DCluster> m_ClusterCol;  m_ClusterCol.clear();
+
+  for(int it=0; it<m_goodTreeCol.size(); it++)
+    m_goodClusterCol.push_back( m_goodTreeCol[it]->ConvertTreeToCluster() );
+  for(int it=0; it<m_badTreeCol.size(); it++)
+    m_badClusterCol.push_back( m_badTreeCol[it]->ConvertTreeToCluster() );
+  for(int in=0; in<m_isoNodes.size(); in++){
+    CRDEcalEDM::CRDCaloHit3DCluster clus_isonodes;
+    CRDEcalEDM::CRDCaloHit2DShower m_shower = m_isoNodes[in]->GetOriginShower();
+    clus_isonodes.AddShower( m_shower );
+    m_badClusterCol.push_back( clus_isonodes );
+  }
+
+  m_ClusterCol.insert(m_ClusterCol.end(), m_goodClusterCol.begin(), m_goodClusterCol.end());
+  m_ClusterCol.insert(m_ClusterCol.end(), m_badClusterCol.begin(), m_badClusterCol.end());
+
+//  std::cout<<"Print Good Tree: "<<std::endl;
+//  for(int it=0; it<m_goodTreeCol.size(); it++) m_goodTreeCol[it]->PrintTree();
+//  std::cout<<"Print Bad Tree: "<<std::endl;
+//  for(int it=0; it<m_badTreeCol.size(); it++) m_badTreeCol[it]->PrintTree();
+//  std::cout<<"Print Isolated Nodes: "<<std::endl;
+//  for(int i=0; i<m_isoNodes.size(); i++)
+//    printf("(%.2f, %.2f, %.2f, %d) \n", m_isoNodes[i]->GetPosition().x(), m_isoNodes[i]->GetPosition().y(), m_isoNodes[i]->GetPosition().z(), m_isoNodes[i]->GetType());  
+//  cout<<"After convertion: goodClus: "<<m_goodClusterCol.size()<<"  badClus: "<<m_badClusterCol.size()<<"  total: "<<m_ClusterCol.size()<<std::endl;
+
+
+  if(settings.fl_overwrite) m_datacol.ClearCluster(); 
+  m_datacol.GoodClus3DCol.insert(m_datacol.GoodClus3DCol.end(), m_goodClusterCol.begin(), m_goodClusterCol.end() );
+  m_datacol.BadClus3DCol.insert( m_datacol.BadClus3DCol.end(),  m_badClusterCol.begin(), m_badClusterCol.end() );
+  m_datacol.Clus3DCol.insert( m_datacol.Clus3DCol.end(), m_ClusterCol.begin(), m_ClusterCol.end() );
+
+  //Clear nodes pointer. 
+  for(int i=0; i<m_goodTreeCol.size(); i++) { m_goodTreeCol[i]->Clear(); }
+  for(int i=0; i<m_badTreeCol.size(); i++)  { m_badTreeCol[i]->Clear(); }
+  for(int i=0; i<m_isoNodes.size(); i++)    { delete m_isoNodes[i]; }
+
+  return StatusCode::SUCCESS;
+}
+
+StatusCode ArborTreeMergingAlg::ClearAlgorithm(){
+
+  return StatusCode::SUCCESS;
+}
+
+StatusCode ArborTreeMergingAlg::GetSecondaryVtxT1(  std::vector<CRDEcalEDM::CRDArborTree>& m_ArborTreeCol,  
+                                                    std::vector<CRDEcalEDM::CRDCaloHit2DShower>& m_2DshowerCol,
+                                                    std::map<CRDEcalEDM::CRDArborNode*, CRDEcalEDM::CRDArborTree*>& m_svtx )
+{
+
+  if( m_ArborTreeCol.size()==0 || m_2DshowerCol.size()==0 )  return StatusCode::SUCCESS;
+
+  //Get the variance in each layer:
+  std::vector<float> m_scndM; m_scndM.clear(); m_scndM.resize(14);
+  std::map<int, std::vector<CRDEcalEDM::CRDCaloHit2DShower> > m_orderedShower; m_orderedShower.clear();
+  for(int is=0;is<m_2DshowerCol.size();is++){
+    m_orderedShower[m_2DshowerCol[is].getDlayer()].push_back(m_2DshowerCol[is]);
+  }
+  for(int il=0; il<14; il++){
+    std::vector<CRDEcalEDM::CRDCaloHit2DShower> m_showers = m_orderedShower[il+1];
+    if(m_showers.size()==0){ m_scndM[il]=0; continue; }
+
+    double sumE=0;
+    TVector3 cent(0,0,0);
+    for(int is=0; is<m_showers.size(); is++){
+      sumE+=m_showers[is].getShowerE();
+      TVector3 pos(m_showers[is].getPos().x(), m_showers[is].getPos().y(), m_showers[is].getPos().z());
+      cent += pos;
+    }
+    cent = cent*(1./(double)m_showers.size());
+
+    double scndM=0;
+    for(int is=0; is<m_showers.size(); is++){
+      TVector3 pos(m_showers[is].getPos().x(), m_showers[is].getPos().y(), m_showers[is].getPos().z());
+      double rpos = (pos-cent).Mag2();
+      double wi = m_showers[is].getShowerE()/sumE;
+      scndM += wi*rpos;
+    }
+    m_scndM[il] = scndM;
+//std::cout<<"  Layer: "<<il<<"  variance: "<<m_scndM[il]<<std::endl;
+  }
+//std::cout<<std::endl; 
+
+  //Get secondary vertex layer and position
+  vector<int> Layer_svtx; Layer_svtx.clear();
+  for(int il=1; il<14; il++)
+    if( m_scndM[il-1]!=0 && (fabs(m_scndM[il]-m_scndM[il-1]) > settings.th_Nsigma * m_scndM[il-1]) ){ Layer_svtx.push_back(il-1); }
+
+//std::cout<<std::endl; 
+//std::cout<<"  Layer with svtx: "; 
+//for(int i=0; i<Layer_svtx.size(); i++) std::cout<<Layer_svtx[i]<<'\t'; 
+//std::cout<<std::endl; 
+
+
+  for(int iv=0; iv<Layer_svtx.size(); iv++){
+    for(int it=0; it<m_ArborTreeCol.size(); it++){
+      if( Layer_svtx[iv]>m_ArborTreeCol[it].GetMaxDlayer() || Layer_svtx[iv]<m_ArborTreeCol[it].GetMinDlayer() ||
+          m_ArborTreeCol[it].GetMinDlayer()==-1 || m_ArborTreeCol[it].GetMaxDlayer()==-1 ) continue;
+
+      std::vector<CRDEcalEDM::CRDArborNode*> m_nodes = m_ArborTreeCol[it].GetNodes(Layer_svtx[iv]);
+      for(int in=0; in<m_nodes.size(); in++){
+//printf("  vtx node: (%.2f, %.2f, %.2f) \n", m_nodes[in]->GetPosition().x(), m_nodes[in]->GetPosition().y(), m_nodes[in]->GetPosition().z());
+        m_svtx[m_nodes[in]] = &m_ArborTreeCol[it];
+      }
+    }
+  }
+
+  return StatusCode::SUCCESS;
+}
+
+
+StatusCode ArborTreeMergingAlg::GetSecondaryVtxT2(  std::vector<CRDEcalEDM::CRDArborTree>& m_ArborTreeCol,
+                                                    std::map<CRDEcalEDM::CRDArborNode*, CRDEcalEDM::CRDArborTree*>& m_svtx )
+{
+
+  if(m_ArborTreeCol.size()==0) return StatusCode::SUCCESS;
+
+  //Get first branch node
+  for(int it=0; it<m_ArborTreeCol.size(); it++){
+    m_ArborTreeCol[it].SortNodes(); 
+    std::vector<CRDEcalEDM::CRDArborNode*> m_nodes = m_ArborTreeCol[it].GetNodes(); 
+
+    std::vector<CRDEcalEDM::CRDArborNode*> m_BrNodes; m_BrNodes.clear(); 
+    for(int in=0; in<m_nodes.size(); in++){
+      int nodeType = m_nodes[in]->GetType(); 
+      if(nodeType!=3) continue; 
+
+      double maxR_daughter = -999;
+      std::vector<CRDEcalEDM::CRDArborNode*> m_dtrNodes = m_nodes[in]->GetDaughterNodes(); 
+      for(int id=0; id<m_dtrNodes.size(); id++){
+      for(int jd=id+1; jd<m_dtrNodes.size(); jd++){
+        double m_Dis = (m_dtrNodes[id]->GetPosition() - m_dtrNodes[jd]->GetPosition()).Mag(); 
+        if( m_Dis>maxR_daughter ) maxR_daughter = m_Dis; 
+      }}
+      if(maxR_daughter<0){ std::cout<<"Error in GetSecondaryVtxT2: failed to get daughter node distance! Check it!"<<std::endl; continue; }
+
+      if( nodeType==3 && maxR_daughter>settings.th_daughterR ) m_BrNodes.push_back(m_nodes[in]); 
+    }
+
+    for(int in=0; in<m_BrNodes.size(); in++){
+      std::map<CRDEcalEDM::CRDArborNode*, CRDEcalEDM::CRDArborTree*>::iterator it_find = m_svtx.find(m_BrNodes[in]);
+      if( it_find==m_svtx.end() ) m_svtx[m_BrNodes[in]] = &m_ArborTreeCol[it]; 
+    }
+
+  }
+
+  return StatusCode::SUCCESS;
+}
+
+#endif
diff --git a/Reconstruction/CrystalCaloRec/src/Algorithm/AxisMergingAlg.cpp b/Reconstruction/CrystalCaloRec/src/Algorithm/AxisMergingAlg.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..1b43f0fc2b48d01fe48ca2f0cc9b3b006ea23a58
--- /dev/null
+++ b/Reconstruction/CrystalCaloRec/src/Algorithm/AxisMergingAlg.cpp
@@ -0,0 +1,982 @@
+#ifndef _AXISMERGING_ALG_C
+#define _AXISMERGING_ALG_C
+
+#include "Algorithm/AxisMergingAlg.h"
+
+StatusCode AxisMergingAlg::ReadSettings(Settings& m_settings){
+  settings = m_settings;
+
+  //Initialize parameters
+  if(settings.map_stringPars.find("OutputAxisName")==settings.map_stringPars.end()) settings.map_stringPars["OutputAxisName"] = "MergedAxis";
+  if(settings.map_floatPars.find("th_overlap")==settings.map_floatPars.end()) settings.map_floatPars["th_overlap"] = 0.5;
+  if(settings.map_intPars.find("th_CoreNhit")==settings.map_intPars.end()) settings.map_intPars["th_CoreNhit"] = 3;
+  if(settings.map_floatPars.find("axis_Angle")==settings.map_floatPars.end()) settings.map_floatPars["axis_Angle"] = TMath::Pi()/4.;
+  if(settings.map_floatPars.find("relP_Angle")==settings.map_floatPars.end()) settings.map_floatPars["relP_Angle"] = TMath::Pi()/4.;
+  if(settings.map_floatPars.find("relP_Dis")==settings.map_floatPars.end()) settings.map_floatPars["relP_Dis"] = 5*PandoraPlus::CaloUnit::barsize;
+  if(settings.map_intPars.find("th_Nhit")==settings.map_intPars.end()) settings.map_intPars["th_Nhit"] = 5;
+  if(settings.map_floatPars.find("th_branch_distance")==settings.map_floatPars.end()) settings.map_floatPars["th_branch_distance"] = 30;
+
+  if(settings.map_intPars.find("th_Nhit_low")==settings.map_intPars.end()) settings.map_intPars["th_Nhit_low"] = 1;
+  if(settings.map_intPars.find("th_Eclus_low")==settings.map_intPars.end()) settings.map_intPars["th_Eclus_low"] = 0.005;
+
+  return StatusCode::SUCCESS;
+};
+
+StatusCode AxisMergingAlg::Initialize( PandoraPlusDataCol& m_datacol ){
+  m_axisUCol.clear();
+  m_axisVCol.clear();
+  m_newAxisUCol.clear();
+  m_newAxisVCol.clear();
+
+  p_HalfClusterU = nullptr;
+  p_HalfClusterV = nullptr;
+
+  p_HalfClusterU = &(m_datacol.map_HalfCluster["HalfClusterColU"]);
+  p_HalfClusterV = &(m_datacol.map_HalfCluster["HalfClusterColV"]);
+
+  return StatusCode::SUCCESS;
+};
+
+StatusCode AxisMergingAlg::RunAlgorithm( PandoraPlusDataCol& m_datacol ){
+  //cout << "yyy: ----------------------------  Running Axis MergingAlg  -----------------------------------" << endl;
+
+  if( p_HalfClusterU->size() + p_HalfClusterV->size()==0 ) {
+    std::cout<<"AxisMergingAlg: No HalfCluster input"<<std::endl;
+    return StatusCode::SUCCESS;
+  }
+
+  //cout << "yyy: Readin halfcluster size in U, V: "<<p_HalfClusterU->size()<<", "<<p_HalfClusterV->size()<<endl;
+
+  //cout << "yyy: Merge axis in U pHalfClusterU" << endl;
+  //Merge axis in HalfClusterU: 
+  std::vector<std::shared_ptr<PandoraPlus::CaloHalfCluster>> p_emptyHFClusU; p_emptyHFClusU.clear();
+  for(int ih=0; ih<p_HalfClusterU->size(); ih++){
+    m_axisUCol.clear(); m_newAxisUCol.clear();
+    m_axisUCol = p_HalfClusterU->at(ih)->getAllHalfClusterCol();
+
+//printf("  HalfClusterU #%d: Energy %.4f, axis size %d, 1DCluster size %d, address %p \n", ih, p_HalfClusterU->at(ih)->getEnergy(), m_axisUCol.size(), p_HalfClusterU->at(ih)->getCluster().size(), p_HalfClusterU->at(ih).get() );
+    if(m_axisUCol.size()==0){ //No axis: save out and merge to other HFClusters later.
+      //if(p_HalfClusterU->at(ih)->getEnergy()>settings.map_intPars["th_Eclus_low"] || p_HalfClusterU->at(ih)->getCluster().size()>settings.map_intPars["th_Nhit_low"])
+      //  m_datacol.map_HalfCluster["emptyHalfClusterU"].push_back(p_HalfClusterU->at(ih));
+      p_emptyHFClusU.push_back(p_HalfClusterU->at(ih));
+      p_HalfClusterU->erase(p_HalfClusterU->begin()+ih);
+      ih--;
+      continue;
+    }
+
+    for(int ic=0; ic<m_axisUCol.size(); ic++){ 
+      std::shared_ptr<CaloHalfCluster> ptr_cloned = m_axisUCol[ic]->Clone();
+      m_datacol.map_HalfCluster["bkHalfCluster"].push_back(ptr_cloned);
+      m_newAxisUCol.push_back( ptr_cloned.get() );
+    }
+
+    // cout << "  yyy: For p_HalfClusterU[" << ih << "], m_newAxisUCol.size() = " << m_newAxisUCol.size() 
+    //      << ", if size<2, no need to merge" << endl;
+    if(m_newAxisUCol.size()<2){ //No need to merge, save into OutputAxis. 
+      std::vector<const PandoraPlus::CaloHalfCluster*> tmp_axisCol; tmp_axisCol.clear();
+      for(int ic=0; ic<m_newAxisUCol.size(); ic++) tmp_axisCol.push_back(m_newAxisUCol[ic]);
+      p_HalfClusterU->at(ih)->setHalfClusters(settings.map_stringPars["OutputAxisName"], tmp_axisCol);
+      continue; 
+    }
+
+
+    std::sort( m_newAxisUCol.begin(), m_newAxisUCol.end(), compLayer );
+
+
+//printf("  In HalfClusterU #%d: readin axis size %d \n", ih, m_newAxisUCol.size());
+/*
+std::map<std::string, std::vector<const PandoraPlus::CaloHalfCluster*> > tmp_HClusMap =  p_HalfClusterU->at(ih)->getHalfClusterMap();
+cout<<"Print Readin AxisU: "<<endl;
+for(auto iter : tmp_HClusMap){
+  cout<<"  Axis name: "<<iter.first<<endl;
+  for(int ia=0; ia<iter.second.size(); ia++){
+    cout<<"    No. #"<<ia<<endl;
+    for(int il=0 ;il<iter.second[ia]->getCluster().size(); il++)
+    printf("    LocalMax %d: (%.3f, %.3f, %.3f, %.3f), towerID [%d, %d, %d], %p \n", il, 
+        iter.second[ia]->getCluster()[il]->getPos().x(), 
+        iter.second[ia]->getCluster()[il]->getPos().y(), 
+        iter.second[ia]->getCluster()[il]->getPos().z(), 
+        iter.second[ia]->getCluster()[il]->getEnergy(), 
+        iter.second[ia]->getCluster()[il]);
+  }
+cout<<endl;
+}
+*/
+/*
+cout<<"Print Readin AxisU: "<<endl;
+for(int ia=0; ia<m_newAxisUCol.size(); ia++){
+cout<<"  Axis #"<<ia<<": type "<<m_newAxisUCol[ia]->getType()<<endl;
+for(int il=0 ;il<m_newAxisUCol[ia]->getCluster().size(); il++)
+  printf("    LocalMax %d: (%.3f, %.3f, %.3f, %.3f), towerID [%d, %d, %d], %p \n", il, 
+    m_newAxisUCol[ia]->getCluster()[il]->getPos().x(), 
+    m_newAxisUCol[ia]->getCluster()[il]->getPos().y(), 
+    m_newAxisUCol[ia]->getCluster()[il]->getPos().z(), 
+    m_newAxisUCol[ia]->getCluster()[il]->getTowerID()[0][0],
+    m_newAxisUCol[ia]->getCluster()[il]->getTowerID()[0][1],
+    m_newAxisUCol[ia]->getCluster()[il]->getTowerID()[0][2],
+    m_newAxisUCol[ia]->getCluster()[il]->getEnergy(), 
+    m_newAxisUCol[ia]->getCluster()[il]);
+cout<<endl;
+}
+*/
+    //Case1: Merge axes associated to the same track. 
+    TrkMatchedMerging(m_newAxisUCol);    
+    // cout << "  yyy: after TrkMatchedMerging(), axis size = " <<  m_newAxisUCol.size() << endl;
+
+    //Case2: Merge axes that share same localMax.
+    OverlapMerging(m_newAxisUCol);
+    // cout << "  yyy: after OverlapMerging(), axis size = " <<  m_newAxisUCol.size() << endl;
+
+    // Case3: Merge fake photon to track axis.
+    BranchMerging(m_newAxisUCol);
+    // cout << "  yyy: after BranchMerging(), axis size = " <<  m_newAxisUCol.size() << endl;
+
+    //Case4: Merge fragments to core axes. 
+    FragmentsMerging(m_newAxisUCol);
+    // cout << "  yyy: after FragmentsMerging(), axis size = " <<  m_newAxisUCol.size() << endl;
+
+    //Case4: Merge nearby axes. 
+    //ConeMerging(m_newAxisUCol);
+    //printf("  In HalfClusterU #%d: After Step4: axis size %d \n", ih, m_newAxisUCol.size());
+
+
+    // cout<<"  yyy: after all merging: axis size "<<m_newAxisUCol.size()<<endl;
+    // for(int ia=0; ia<m_newAxisUCol.size(); ia++){
+    //   cout<<"  yyy: axis #"<<ia<<endl;
+    //   for(int il=0 ;il<m_newAxisUCol[ia]->getCluster().size(); il++)
+    //     printf("         LocalMax %d: (%.2f, %.2f, %.2f, %.4f), towerID [%d, %d, %d], %p \n", il, 
+    //       m_newAxisUCol[ia]->getCluster()[il]->getPos().x(), 
+    //       m_newAxisUCol[ia]->getCluster()[il]->getPos().y(), 
+    //       m_newAxisUCol[ia]->getCluster()[il]->getPos().z(), 
+    //       m_newAxisUCol[ia]->getCluster()[il]->getEnergy(), 
+    //       m_newAxisUCol[ia]->getCluster()[il]->getTowerID()[0][0], 
+    //       m_newAxisUCol[ia]->getCluster()[il]->getTowerID()[0][1], 
+    //       m_newAxisUCol[ia]->getCluster()[il]->getTowerID()[0][2], 
+    //       m_newAxisUCol[ia]->getCluster()[il]);
+    // }
+
+
+//cout << "  yyy: check axis quality" << endl;
+//printf("  In HalfClusterU #%d: After Step4: axis size %d \n", ih, m_newAxisUCol.size());
+
+/*
+cout<<"  After merging: axis size "<<m_newAxisUCol.size()<<", Check the overlap"<<endl;
+cout<<"Print Merged AxisU: "<<endl;
+for(int ia=0; ia<m_newAxisUCol.size(); ia++){
+cout<<"  Axis #"<<ia<<": type "<<m_newAxisUCol[ia]->getType()<<endl;
+for(int il=0 ;il<m_newAxisUCol[ia]->getCluster().size(); il++)
+  printf("    LocalMax %d: (%.3f, %.3f, %.3f, %.3f), towerID [%d, %d, %d], %p \n", il, 
+    m_newAxisUCol[ia]->getCluster()[il]->getPos().x(), 
+    m_newAxisUCol[ia]->getCluster()[il]->getPos().y(), 
+    m_newAxisUCol[ia]->getCluster()[il]->getPos().z(), 
+    m_newAxisUCol[ia]->getCluster()[il]->getEnergy(), 
+    m_newAxisUCol[ia]->getCluster()[il]->getTowerID()[0][0], 
+    m_newAxisUCol[ia]->getCluster()[il]->getTowerID()[0][1], 
+    m_newAxisUCol[ia]->getCluster()[il]->getTowerID()[0][2], 
+    m_newAxisUCol[ia]->getCluster()[il]);
+cout<<endl;
+}
+*/
+    //Check axis quality
+    std::vector<PandoraPlus::CaloHalfCluster*> tmp_goodAxis; tmp_goodAxis.clear(); 
+    std::vector<PandoraPlus::CaloHalfCluster*> tmp_badAxis; tmp_badAxis.clear();
+    for(int ic=0; ic<m_newAxisUCol.size(); ic++){
+      m_newAxisUCol[ic]->getLinkedMCPfromUnit();
+      if( (m_newAxisUCol[ic]->getType()==100 && m_newAxisUCol[ic]->getCluster().size()>=settings.map_intPars["th_CoreNhit"] ) ||
+          m_newAxisUCol[ic]->getType()>100 ){
+            tmp_goodAxis.push_back( m_newAxisUCol[ic] );
+          }
+        
+
+      else {
+        tmp_badAxis.push_back( m_newAxisUCol[ic] );
+      }
+    }  
+
+    for(int ic=0; ic<tmp_badAxis.size(); ic++){
+      MergeToClosestCluster( tmp_badAxis[ic], tmp_goodAxis );
+    }
+    
+    //Split the double-used 1DClusters in axis  
+    //SplitOverlapCluster(p_HalfClusterU->at(ih), tmp_goodAxis, m_datacol.map_1DCluster["bk1DCluster"], m_datacol.map_BarCol["bkBar"]);
+
+    //convert to constant object and save in HalfCluster: 
+    std::vector<const PandoraPlus::CaloHalfCluster*> tmp_axisCol; tmp_axisCol.clear();
+    for(int ic=0; ic<tmp_goodAxis.size(); ic++) tmp_axisCol.push_back(tmp_goodAxis[ic]);
+    // for(int ic=0; ic<m_newAxisUCol.size(); ic++) tmp_axisCol.push_back(m_newAxisUCol[ic]);
+    p_HalfClusterU->at(ih)->setHalfClusters(settings.map_stringPars["OutputAxisName"], tmp_axisCol);
+
+  }
+
+  //Merge empty HalfCluster into the existing clusters. 
+  for(int ih=0; ih<p_emptyHFClusU.size(); ih++){
+    if(!p_emptyHFClusU[ih]) continue;
+    std::vector<PandoraPlus::CaloHalfCluster*> m_HalfCluster; m_HalfCluster.clear();
+    for(int i=0; i<p_HalfClusterU->size(); i++) m_HalfCluster.push_back(p_HalfClusterU->at(i).get());
+    if(!MergeToClosestCluster(p_emptyHFClusU[ih].get(), m_HalfCluster)){
+      if(p_emptyHFClusU[ih]->getEnergy()>settings.map_intPars["th_Eclus_low"] || p_emptyHFClusU[ih]->getCluster().size()>settings.map_intPars["th_Nhit_low"]){
+cout<<"  Save cluster #"<<ih<<" as empty, energy "<<p_emptyHFClusU[ih]->getEnergy()<<endl;
+        m_datacol.map_HalfCluster["emptyHalfClusterU"].push_back(p_emptyHFClusU[ih]);
+      }
+    }
+  }
+
+  //cout << "yyy: Merge axis in V pHalfClusterV" << endl;
+  //Merge axis in HalfClusterV:
+  std::vector<std::shared_ptr<PandoraPlus::CaloHalfCluster>> p_emptyHFClusV; p_emptyHFClusV.clear();
+  for(int ih=0; ih<p_HalfClusterV->size(); ih++){
+    m_axisVCol.clear(); m_newAxisVCol.clear();
+    m_axisVCol = p_HalfClusterV->at(ih)->getAllHalfClusterCol();
+//printf("  HalfClusterV #%d: Energy %.4f, axis size %d, 1DCluster size %d \n", ih, p_HalfClusterV->at(ih)->getEnergy(), m_axisVCol.size(), p_HalfClusterV->at(ih)->getCluster().size());
+
+    if(m_axisVCol.size()==0){ //No axis: save out and merge to other HFClusters later.
+      //m_datacol.map_HalfCluster["emptyHalfClusterV"].push_back(p_HalfClusterV->at(ih));
+      p_emptyHFClusV.push_back(p_HalfClusterV->at(ih));
+      p_HalfClusterV->erase(p_HalfClusterV->begin()+ih);
+      ih--;
+      continue;
+    }
+
+    for(int ic=0; ic<m_axisVCol.size(); ic++){
+      std::shared_ptr<CaloHalfCluster> ptr_cloned = m_axisVCol[ic]->Clone();
+      m_datacol.map_HalfCluster["bkHalfCluster"].push_back(ptr_cloned);
+      m_newAxisVCol.push_back( ptr_cloned.get() );
+    }
+
+    // cout << "  yyy: For p_HalfClusterV[" << ih << "], m_newAxisVCol.size() = " << m_newAxisVCol.size() 
+    //      << ", if size<2, no need to merge" << endl;
+    if(m_newAxisVCol.size()<2){
+      std::vector<const PandoraPlus::CaloHalfCluster*> tmp_axisCol; tmp_axisCol.clear();
+      for(int ic=0; ic<m_newAxisVCol.size(); ic++) tmp_axisCol.push_back(m_newAxisVCol[ic]);
+      p_HalfClusterV->at(ih)->setHalfClusters(settings.map_stringPars["OutputAxisName"], tmp_axisCol);
+      continue;
+    }
+    std::sort( m_newAxisVCol.begin(), m_newAxisVCol.end(), compLayer );
+    //Case1: Merge axes associated to the same track. 
+    TrkMatchedMerging(m_newAxisVCol);
+    // cout << "  yyy: after TrkMatchedMerging(), axis size = " <<  m_newAxisVCol.size() << endl;
+
+    //Case2: Merge axes that share same localMax.
+    OverlapMerging(m_newAxisVCol);
+    // cout << "  yyy: after OverlapMerging(), axis size = " <<  m_newAxisVCol.size() << endl;
+
+    //Case3: Merge fake photon to track axis.
+    BranchMerging(m_newAxisVCol);
+    // cout << "  yyy: after BranchMerging(), axis size = " <<  m_newAxisVCol.size() << endl;
+
+    //Case4: Merge fragments to core axes. 
+    FragmentsMerging(m_newAxisVCol);
+    // cout << "  yyy: after FragmentsMerging(), axis size = " <<  m_newAxisVCol.size() << endl;
+
+    //Case4: Merge nearby axes.
+    //ConeMerging(m_newAxisVCol);
+    //printf("  In HalfClusterV #%d: After Step4: axis size %d \n", ih, m_newAxisVCol.size());
+
+    // cout<<"  yyy: after all merging: axis size "<<m_newAxisVCol.size()<<endl;
+    // for(int ia=0; ia<m_newAxisVCol.size(); ia++){
+    //   cout<<"  yyy: axis #"<<ia<<endl;
+    //   for(int il=0 ;il<m_newAxisVCol[ia]->getCluster().size(); il++)
+    //     printf("         LocalMax %d: (%.2f, %.2f, %.2f, %.4f), towerID [%d, %d, %d], %p \n", il, 
+    //       m_newAxisVCol[ia]->getCluster()[il]->getPos().x(), 
+    //       m_newAxisVCol[ia]->getCluster()[il]->getPos().y(), 
+    //       m_newAxisVCol[ia]->getCluster()[il]->getPos().z(), 
+    //       m_newAxisVCol[ia]->getCluster()[il]->getEnergy(), 
+    //       m_newAxisVCol[ia]->getCluster()[il]->getTowerID()[0][0], 
+    //       m_newAxisVCol[ia]->getCluster()[il]->getTowerID()[0][1], 
+    //       m_newAxisVCol[ia]->getCluster()[il]->getTowerID()[0][2], 
+    //       m_newAxisVCol[ia]->getCluster()[il]);
+    // }
+
+    //cout << "  yyy: check axis quality" << endl;
+    //Check axis quality
+    std::vector<PandoraPlus::CaloHalfCluster*> tmp_goodAxis; tmp_goodAxis.clear();
+    std::vector<PandoraPlus::CaloHalfCluster*> tmp_badAxis; tmp_badAxis.clear();
+    for(int ic=0; ic<m_newAxisVCol.size(); ic++){
+      m_newAxisVCol[ic]->getLinkedMCPfromUnit();
+      if( (m_newAxisVCol[ic]->getType()==100 && m_newAxisVCol[ic]->getCluster().size()>=settings.map_intPars["th_CoreNhit"] ) ||
+          m_newAxisVCol[ic]->getType()>100 ){
+            tmp_goodAxis.push_back( m_newAxisVCol[ic] );
+          }
+        
+
+      else {
+        tmp_badAxis.push_back( m_newAxisVCol[ic] );
+      }
+    }
+
+    for(int ic=0; ic<tmp_badAxis.size(); ic++){
+      MergeToClosestCluster( tmp_badAxis[ic], tmp_goodAxis );
+    }
+      
+    //Split the double-used 1DClusters in axis  
+    //SplitOverlapCluster(p_HalfClusterV->at(ih), tmp_goodAxis, m_datacol.map_1DCluster["bk1DCluster"], m_datacol.map_BarCol["bkBar"]);
+
+    //convert to constant object and save in HalfCluster:
+    std::vector<const PandoraPlus::CaloHalfCluster*> tmp_axisCol; tmp_axisCol.clear();
+    for(int ic=0; ic<tmp_goodAxis.size(); ic++) tmp_axisCol.push_back(tmp_goodAxis[ic]);
+    // for(int ic=0; ic<m_newAxisVCol.size(); ic++) tmp_axisCol.push_back(m_newAxisVCol[ic]);
+    p_HalfClusterV->at(ih)->setHalfClusters(settings.map_stringPars["OutputAxisName"], tmp_axisCol);
+
+  }
+
+  //Merge empty HalfClusters into the existing cluster.
+  for(int ih=0; ih<p_emptyHFClusV.size(); ih++){
+    if(!p_emptyHFClusV[ih]) continue;
+    std::vector<PandoraPlus::CaloHalfCluster*> m_HalfCluster; m_HalfCluster.clear();
+    for(int i=0; i<p_HalfClusterV->size(); i++) m_HalfCluster.push_back(p_HalfClusterV->at(i).get());
+    if(!MergeToClosestCluster(p_emptyHFClusV[ih].get(), m_HalfCluster)){
+      if(p_emptyHFClusV[ih]->getEnergy()>settings.map_intPars["th_Eclus_low"] || p_emptyHFClusV[ih]->getCluster().size()>settings.map_intPars["th_Nhit_low"])
+        m_datacol.map_HalfCluster["emptyHalfClusterV"].push_back(p_emptyHFClusV[ih]);
+    }
+  }
+
+  return StatusCode::SUCCESS;
+};
+
+StatusCode AxisMergingAlg::ClearAlgorithm(){
+  p_HalfClusterU = nullptr;
+  p_HalfClusterV = nullptr;
+  m_axisUCol.clear();
+  m_axisVCol.clear();
+  m_newAxisUCol.clear();
+  m_newAxisVCol.clear();
+
+  return StatusCode::SUCCESS;
+};
+
+
+StatusCode AxisMergingAlg::TrkMatchedMerging( std::vector<PandoraPlus::CaloHalfCluster*>& m_axisCol ){
+  // cout << "  yyy: calling TrkMatchedMerging(), m_axisCol.size()=" << m_axisCol.size() << endl;
+  if(m_axisCol.size()<2) return StatusCode::SUCCESS;
+
+  for(int iax=0; iax<m_axisCol.size(); iax++){
+    std::vector<const PandoraPlus::Track*> m_trkCol = m_axisCol[iax]->getAssociatedTracks();
+    for(int jax=iax+1; jax<m_axisCol.size(); jax++){
+      std::vector<const PandoraPlus::Track*> p_trkCol = m_axisCol[jax]->getAssociatedTracks();
+
+
+      bool fl_match = false;
+      for(int itrk=0; itrk<m_trkCol.size(); itrk++){
+        if( find(p_trkCol.begin(), p_trkCol.end(), m_trkCol[itrk])!=p_trkCol.end() ){
+          fl_match = true; break;
+        }
+      }
+
+      if(fl_match){
+        // cout << "    yyy: m_axisCol[" << iax << "] with type=" << m_axisCol[iax]->getType() 
+        //      << " and [" << jax << "] with type=" << m_axisCol[jax]->getType() << "share the same associated track, merge them" << endl;
+        // cout << "         hit in m_axisCol["<<iax<<"]:"<<endl;
+        // for(int yii=0; yii<m_axisCol[iax]->getCluster().size(); yii++){
+        //   cout << "           (" << std::fixed << std::setprecision(2) << m_axisCol[iax]->getCluster()[yii]->getPos().x()
+        //        << ", " << std::fixed << std::setprecision(2) << m_axisCol[iax]->getCluster()[yii]->getPos().y()
+        //        << ", " << std::fixed << std::setprecision(2) << m_axisCol[iax]->getCluster()[yii]->getPos().z()
+        //        << ")" << endl;
+        // }
+        // cout << "         hit in m_axisCol["<<jax<<"]:"<<endl;
+        // for(int yii=0; yii<m_axisCol[jax]->getCluster().size(); yii++){
+        //   cout << "           (" << std::fixed << std::setprecision(2) << m_axisCol[jax]->getCluster()[yii]->getPos().x()
+        //        << ", " << std::fixed << std::setprecision(2) << m_axisCol[jax]->getCluster()[yii]->getPos().y()
+        //        << ", " << std::fixed << std::setprecision(2) << m_axisCol[jax]->getCluster()[yii]->getPos().z()
+        //        << ")" << endl;
+        // }
+
+        m_axisCol[iax]->mergeHalfCluster( m_axisCol[jax] );
+        // cout << "    yyy: after merge them, new type=" << m_axisCol[iax]->getType() << endl;
+        //delete m_axisCol[jax]; m_axisCol[jax]=nullptr;
+        m_axisCol.erase(m_axisCol.begin()+jax);
+        jax--;
+        if(iax>jax+1) iax--;
+      }
+    }
+  }
+  return StatusCode::SUCCESS;
+};
+
+
+StatusCode AxisMergingAlg::OverlapMerging( std::vector<PandoraPlus::CaloHalfCluster*>& m_axisCol ){
+  // cout << "  yyy: calling OverlapMerging(), m_axisCol.size()=" << m_axisCol.size() << endl;
+  if(m_axisCol.size()<2) return StatusCode::SUCCESS;
+
+  // cout << "  yyy: first interate" << endl;
+  for(int iax=0; iax<m_axisCol.size(); iax++){
+    const PandoraPlus::CaloHalfCluster* m_axis = m_axisCol[iax];
+    for(int jax=iax+1; jax<m_axisCol.size(); jax++){
+      const PandoraPlus::CaloHalfCluster* p_axis = m_axisCol[jax];
+
+      // Do not merge two different track axes
+      if ( m_axis->getAssociatedTracks().size()>0 && p_axis->getAssociatedTracks().size()>0 ) continue;
+
+      std::vector<const Calo1DCluster*> tmp_localMax = p_axis->getCluster();
+
+      int nsharedHits = 0;
+      for(int ihit=0; ihit<m_axis->getCluster().size(); ihit++)
+        if( find(tmp_localMax.begin(), tmp_localMax.end(), m_axis->getCluster()[ihit])!=tmp_localMax.end() ) nsharedHits++;
+
+      
+      if( (m_axis->getCluster().size()<=p_axis->getCluster().size() && (float)nsharedHits/m_axis->getCluster().size()>settings.map_floatPars["th_overlap"] ) || 
+          (p_axis->getCluster().size()<m_axis->getCluster().size() && (float)nsharedHits/p_axis->getCluster().size()>settings.map_floatPars["th_overlap"] ) ){
+
+        // cout << "    yyy: m_axisCol[" << iax << "] with type=" << m_axisCol[iax]->getType() 
+        //      << " and [" << jax << "] with type=" << m_axisCol[jax]->getType() 
+        //      << " share " << nsharedHits << " hits, merge them" << endl;
+        // cout << "         hit in m_axisCol["<<iax<<"]:"<<endl;
+        // for(int yii=0; yii<m_axisCol[iax]->getCluster().size(); yii++){
+        //   cout << "           (" << std::fixed << std::setprecision(2) << m_axisCol[iax]->getCluster()[yii]->getPos().x()
+        //        << ", " << std::fixed << std::setprecision(2) << m_axisCol[iax]->getCluster()[yii]->getPos().y()
+        //        << ", " << std::fixed << std::setprecision(2) << m_axisCol[iax]->getCluster()[yii]->getPos().z()
+        //        << ")" << endl;
+        // }
+        // cout << "         hit in m_axisCol["<<jax<<"]:"<<endl;
+        // for(int yii=0; yii<m_axisCol[jax]->getCluster().size(); yii++){
+        //   cout << "           (" << std::fixed << std::setprecision(2) << m_axisCol[jax]->getCluster()[yii]->getPos().x()
+        //        << ", " << std::fixed << std::setprecision(2) << m_axisCol[jax]->getCluster()[yii]->getPos().y()
+        //        << ", " << std::fixed << std::setprecision(2) << m_axisCol[jax]->getCluster()[yii]->getPos().z()
+        //        << ")" << endl;
+        // }
+
+        m_axisCol[iax]->mergeHalfCluster( m_axisCol[jax] );
+
+        // track axis + neutral axis = track axis
+        int axis_type = m_axisCol[iax]->getType() + m_axisCol[jax]->getType();
+        m_axisCol[iax]->setType(axis_type);
+
+        // cout << "    yyy: after merge them, new type=" << m_axisCol[iax]->getType() << endl;
+        //delete m_axisCol[jax]; m_axisCol[jax]=nullptr;
+        m_axisCol.erase(m_axisCol.begin()+jax);
+        jax--;
+        if(iax>jax+1) iax--;
+
+      }
+
+      p_axis=nullptr;
+    }
+    m_axis=nullptr;
+  }
+
+
+  // cout << "  yyy: second interate" << endl;
+  //iterate
+  for(int iax=0; iax<m_axisCol.size(); iax++){
+    const PandoraPlus::CaloHalfCluster* m_axis = m_axisCol[iax];
+    for(int jax=iax+1; jax<m_axisCol.size(); jax++){
+      const PandoraPlus::CaloHalfCluster* p_axis = m_axisCol[jax];
+
+      // Do not merge two different track axes
+      if ( m_axis->getAssociatedTracks().size()>0 && p_axis->getAssociatedTracks().size()>0 ) continue;
+      
+      std::vector<const Calo1DCluster*> tmp_localMax = p_axis->getCluster();
+
+      int nsharedHits = 0;
+      for(int ihit=0; ihit<m_axis->getCluster().size(); ihit++)
+        if( find(tmp_localMax.begin(), tmp_localMax.end(), m_axis->getCluster()[ihit])!=tmp_localMax.end() ) nsharedHits++;
+
+  //printf("  In pair (%d, %d): hit size (%d, %d), shared hit size %d \n",iax, jax, m_axis->getCluster().size(), p_axis->getCluster().size(), nsharedHits);
+
+      if( (m_axis->getCluster().size()<=p_axis->getCluster().size() && (float)nsharedHits/m_axis->getCluster().size()>settings.map_floatPars["th_overlap"] ) ||
+          (p_axis->getCluster().size()<m_axis->getCluster().size() && (float)nsharedHits/p_axis->getCluster().size()>settings.map_floatPars["th_overlap"] ) ){
+
+  //cout<<"  Merge: Yes. "<<endl;
+        // cout << "    yyy: m_axisCol[" << iax << "] with type=" << m_axisCol[iax]->getType() 
+        //      << " and [" << jax << "] with type=" << m_axisCol[jax]->getType() 
+        //      << " share " << nsharedHits << " hits, merge them" << endl;
+        // cout << "         hit in m_axisCol["<<iax<<"]:"<<endl;
+        // for(int yii=0; yii<m_axisCol[iax]->getCluster().size(); yii++){
+        //   cout << "           (" << std::fixed << std::setprecision(2) << m_axisCol[iax]->getCluster()[yii]->getPos().x()
+        //        << ", " << std::fixed << std::setprecision(2) << m_axisCol[iax]->getCluster()[yii]->getPos().y()
+        //        << ", " << std::fixed << std::setprecision(2) << m_axisCol[iax]->getCluster()[yii]->getPos().z()
+        //        << ")" << endl;
+        // }
+        // cout << "         hit in m_axisCol["<<jax<<"]:"<<endl;
+        // for(int yii=0; yii<m_axisCol[jax]->getCluster().size(); yii++){
+        //   cout << "           (" << std::fixed << std::setprecision(2) << m_axisCol[jax]->getCluster()[yii]->getPos().x()
+        //        << ", " << std::fixed << std::setprecision(2) << m_axisCol[jax]->getCluster()[yii]->getPos().y()
+        //        << ", " << std::fixed << std::setprecision(2) << m_axisCol[jax]->getCluster()[yii]->getPos().z()
+        //        << ")" << endl;
+        // }
+
+        m_axisCol[iax]->mergeHalfCluster( m_axisCol[jax] );  
+
+        int axis_type = m_axisCol[iax]->getType() + m_axisCol[jax]->getType();
+        m_axisCol[iax]->setType(axis_type);
+
+        // cout << "    yyy: after merge them, new type=" << m_axisCol[iax]->getType() << endl;
+
+        //delete m_axisCol[jax]; m_axisCol[jax]=nullptr;
+        m_axisCol.erase(m_axisCol.begin()+jax);
+        jax--;
+        if(iax>jax+1) iax--;
+
+      }
+
+      p_axis=nullptr;
+    }
+    m_axis=nullptr;
+  }
+
+
+  return StatusCode::SUCCESS;
+};
+
+
+StatusCode AxisMergingAlg::BranchMerging( std::vector<PandoraPlus::CaloHalfCluster*>& m_axisCol ){
+  // cout << "  yyy: calling BranchMerging(), m_axisCol.size()=" << m_axisCol.size() << endl;
+  if(m_axisCol.size()<2) return StatusCode::SUCCESS;
+
+  // Merge fake Hough axis to track axis
+  std::sort( m_axisCol.begin(), m_axisCol.end(), compLayer );
+  for(int iax=0; iax<m_axisCol.size(); iax++){  // select track axis
+    const PandoraPlus::CaloHalfCluster* m_axis = m_axisCol[iax];
+
+    if (m_axis->getType()<10000){
+      // cout << "yyy:  m_axisCol[" << iax << "] is not a track axis. skip" << endl;
+      continue;
+    }
+
+    for(int jax=0; jax<m_axisCol.size(); jax++){ // select Hough axis
+      if(jax==iax) continue;
+      const PandoraPlus::CaloHalfCluster* p_axis = m_axisCol[jax];
+      
+      if (p_axis->getType()>=10000) continue; // Do not merge two track axis
+      if (p_axis->getType()/100%100<1){
+        // cout << "yyy:  m_axisCol[" << jax << "] is not a Hough axis. skip" << endl;
+        continue;
+      }
+
+      // // Determine if two axes are close to each other
+      // bool is_close = false;
+      // for(int ibar=0; ibar<m_axis->getCluster().size(); ibar++){
+      //   for(int jbar=0; jbar<p_axis->getCluster().size(); jbar++){
+      //     double distance = ( m_axis->getCluster()[ibar]->getPos() - p_axis->getCluster()[jbar]->getPos() ).Mag();
+      //     if(distance<100){ // yyy: hard coding here. min distance of the two axes must be < 50 mm
+      //       is_close = true;
+      //       break;
+      //     }
+      //   }
+      //   if(is_close) break;
+      // }
+
+
+      // if(!is_close) continue;  // if the two axes are not close to each other, no need to merge
+
+      double hough_rho = p_axis->getHoughRho();
+      double hough_alpha = p_axis->getHoughAlpha();
+
+      // V plane
+      if(m_axis->getSlayer()==1){
+        double x0 = m_axis->getEnergyCenter().x();
+        double y0 = m_axis->getEnergyCenter().y();
+        double distance = TMath::Abs( x0*TMath::Cos(hough_alpha) + y0*TMath::Sin(hough_alpha) - hough_rho );
+
+        // cout << "    yyy:V rho = " << hough_rho << ", alpha = " << hough_alpha << ", x0 = " << x0 << ", y0 = " << y0 << endl;
+        // cout << "         distanceV = " << distance << endl;
+
+        if (distance<settings.map_floatPars["th_branch_distance"]){
+          m_axisCol[iax]->mergeHalfCluster( m_axisCol[jax] );
+          int axis_type = m_axisCol[iax]->getType() + m_axisCol[jax]->getType();
+          m_axisCol[iax]->setType(axis_type);
+          m_axisCol.erase(m_axisCol.begin()+jax);
+          jax--;
+          if(iax>jax+1) iax--;
+          // cout << "  yyy: axis " << jax << " is merged into axis " << iax << endl;
+        }
+      }
+
+      // U plane
+      else{
+        int m_module = m_axis->getEnergyCenterTower()[0];
+        // cout << "  yyy:branceU: m_module = " << m_module << endl;
+        int p_module = p_axis->getTowerID()[0][0];
+        // cout << "  yyy:branceU: p_module = " << p_module << endl;
+        // Do not merge the two axis in two different modules
+        //if (m_module != p_module) continue;
+
+        TVector3 t_pos = m_axis->getEnergyCenter();
+        // cout << "  yyy:branceU: t_pos = " << t_pos.x() << ", " << t_pos.y() << ", " << t_pos.z() << endl;
+        t_pos.RotateZ( TMath::TwoPi()/PandoraPlus::CaloUnit::Nmodule*(int(PandoraPlus::CaloUnit::Nmodule*3./4.)-m_module) );
+        // cout << "  yyy:branceU: t_pos after rotate to module 6 = " << t_pos.x() << ", " << t_pos.y() << ", " << t_pos.z() << endl;
+        double x0 = t_pos.x();
+        double y0 = t_pos.z();
+        double distance = TMath::Abs( x0*TMath::Cos(hough_alpha) + y0*TMath::Sin(hough_alpha) - hough_rho );
+
+        // cout << "    yyy:U rho = " << hough_rho << ", alpha = " << hough_alpha << ", x0 = " << x0 << ", z0 = " << y0 << endl;
+        // cout << "         distanceU = " << distance << endl;
+
+        if (distance<settings.map_floatPars["th_branch_distance"]){
+          m_axisCol[iax]->mergeHalfCluster( m_axisCol[jax] );
+          int axis_type = m_axisCol[iax]->getType() + m_axisCol[jax]->getType();
+          m_axisCol[iax]->setType(axis_type);
+          m_axisCol.erase(m_axisCol.begin()+jax);
+          jax--;
+          if(iax>jax+1) iax--;
+          // cout << "  yyy: axis " << jax << " is merged into axis " << iax << endl;
+        }
+
+
+      }
+
+      p_axis=nullptr;
+    }
+    m_axis=nullptr;
+  }
+}
+
+
+StatusCode AxisMergingAlg::FragmentsMerging( std::vector<PandoraPlus::CaloHalfCluster*>& m_axisCol ){
+  // cout << "  yyy: calling FragmentsMerging(), m_axisCol.size()=" << m_axisCol.size() << endl;
+  if(m_axisCol.size()<2) return StatusCode::SUCCESS;
+
+  /*
+  cout<<"FragmentsMerging: print readin axes "<<endl;
+  for(int ia=0; ia<m_axisCol.size(); ia++){
+  cout<<"  Axis #"<<ia<<", Energy center ";
+  printf("(%.3f, %.3f, %.3f, %.3f) \n", m_axisCol[ia]->getEnergyCenter().x(), m_axisCol[ia]->getEnergyCenter().y(), m_axisCol[ia]->getEnergyCenter().z());
+  for(int il=0 ;il<m_axisCol[ia]->getCluster().size(); il++)
+    printf("    LocalMax %d: (%.3f, %.3f, %.3f, %.3f) \n", il,
+      m_axisCol[ia]->getCluster()[il]->getPos().x(),
+      m_axisCol[ia]->getCluster()[il]->getPos().y(),
+      m_axisCol[ia]->getCluster()[il]->getPos().z(),
+      m_axisCol[ia]->getCluster()[il]->getEnergy() );
+  cout<<endl;
+  }
+  */
+
+  //Merge fragments to core.
+  std::sort( m_axisCol.begin(), m_axisCol.end(), compLayer );
+  for(int iax=0; iax<m_axisCol.size(); iax++){
+    const PandoraPlus::CaloHalfCluster* m_axis = m_axisCol[iax];
+    //Define the Core: Hough+Nhit || Trk+Hough || Trk+Cone.
+    //cout<<"  Readin core axis #"<<iax<<" type: "<<m_axis->getType()<<", Nclus: "<<m_axis->getCluster().size();
+    if( !( (m_axis->getType()==100 && m_axis->getCluster().size()>=settings.map_intPars["th_CoreNhit"] ) ||
+        m_axis->getType()>100 ) ) 
+      { m_axis=nullptr; continue; }
+
+    // cout << "    yyy: m_axisCol[" << iax << "] is a core" << endl;
+
+    for(int jax=0; jax<m_axisCol.size(); jax++){
+      if(jax==iax) continue;
+      const PandoraPlus::CaloHalfCluster* p_axis = m_axisCol[jax];
+      //Do not merge 2 cores. 
+      if( (p_axis->getType()==100 && p_axis->getCluster().size()>=settings.map_intPars["th_CoreNhit"] ) ||
+          p_axis->getType()>100 ){
+            // cout << "    yyy: m_axisCol[" << jax << "] is also a core, do not merge 2 cores" << endl;
+            // cout << "         hit in m_axisCol["<<iax<<"]:"<<endl;
+            //for(int yii=0; yii<m_axisCol[iax]->getCluster().size(); yii++){
+            //  cout << "           (" << std::fixed << std::setprecision(2) << m_axisCol[iax]->getCluster()[yii]->getPos().x()
+            //      << ", " << std::fixed << std::setprecision(2) << m_axisCol[iax]->getCluster()[yii]->getPos().y()
+            //      << ", " << std::fixed << std::setprecision(2) << m_axisCol[iax]->getCluster()[yii]->getPos().z()
+            //      << ")" << endl;
+            //}
+            // cout << "         hit in m_axisCol["<<jax<<"]:"<<endl;
+            //for(int yii=0; yii<m_axisCol[jax]->getCluster().size(); yii++){
+            //  cout << "           (" << std::fixed << std::setprecision(2) << m_axisCol[jax]->getCluster()[yii]->getPos().x()
+            //      << ", " << std::fixed << std::setprecision(2) << m_axisCol[jax]->getCluster()[yii]->getPos().y()
+            //      << ", " << std::fixed << std::setprecision(2) << m_axisCol[jax]->getCluster()[yii]->getPos().z()
+            //      << ")" << endl;
+            //}
+            
+            p_axis=nullptr; 
+            continue; 
+          }
+
+      TVector3 relP1 = p_axis->getClusterInLayer( p_axis->getBeginningDlayer() )[0]->getPos() - m_axis->getClusterInLayer( m_axis->getEndDlayer() )[0]->getPos();
+      TVector3 relP2 = p_axis->getClusterInLayer( p_axis->getBeginningDlayer() )[0]->getPos() - m_axis->getEnergyCenter();
+      TVector3 relP3 = m_axis->getEnergyCenter() - p_axis->getClusterInLayer( p_axis->getEndDlayer() )[0]->getPos();
+      TVector3 relP4 = p_axis->getPos() - m_axis->getPos();
+
+      // cout << "    yyy: m_axisCol[" << jax << "] is NOT a core, comparing them" << endl;
+      // cout << "    yyy: relP1 = (" << std::fixed << std::setprecision(2) << relP1.x() 
+      //      <<                 ", " << std::fixed << std::setprecision(2) << relP1.y() 
+      //      <<                 ", " << std::fixed << std::setprecision(2) << relP1.z()
+      //      << endl
+      //      << "         relP2 = (" << std::fixed << std::setprecision(2) << relP2.x() 
+      //      <<                 ", " << std::fixed << std::setprecision(2) << relP2.y() 
+      //      <<                 ", " << std::fixed << std::setprecision(2) << relP2.z() 
+      //      << endl
+      //      << "         relP3 = (" << std::fixed << std::setprecision(2) << relP3.x() 
+      //      <<                 ", " << std::fixed << std::setprecision(2) << relP3.y() 
+      //      <<                 ", " << std::fixed << std::setprecision(2) << relP3.z() 
+      //      << endl
+      //      << "         relP4 = (" << std::fixed << std::setprecision(2) << relP4.x() 
+      //      <<                 ", " << std::fixed << std::setprecision(2) << relP4.y() 
+      //      <<                 ", " << std::fixed << std::setprecision(2) << relP4.z() 
+      //      << endl
+      //      << "         p_axis->getAxis() = (" << std::fixed << std::setprecision(2) << p_axis->getAxis().x() 
+      //      <<                             ", " << std::fixed << std::setprecision(2) << p_axis->getAxis().y() 
+      //      <<                             ", " << std::fixed << std::setprecision(2) << p_axis->getAxis().z() 
+      //      << endl
+      //      << "         m_axis->getAxis() = (" << std::fixed << std::setprecision(2) << m_axis->getAxis().x() 
+      //      <<                             ", " << std::fixed << std::setprecision(2) << m_axis->getAxis().y() 
+      //      <<                             ", " << std::fixed << std::setprecision(2) << m_axis->getAxis().z() 
+      //      << endl;
+
+      if( (relP1.Angle(p_axis->getAxis()) < settings.map_floatPars["axis_Angle"] && relP1.Mag()<=settings.map_floatPars["relP_Dis"]) ||
+          (relP2.Angle(p_axis->getAxis()) < settings.map_floatPars["axis_Angle"] && relP2.Mag()<=settings.map_floatPars["relP_Dis"]) ||
+          (relP3.Angle(p_axis->getAxis()) < settings.map_floatPars["axis_Angle"] && relP3.Mag()<=settings.map_floatPars["relP_Dis"]) || 
+          ( p_axis->getType()<100 && relP4.Angle(m_axis->getAxis())<settings.map_floatPars["axis_Angle"] && relP4.Mag()<=settings.map_floatPars["relP_Dis"]) )
+      {
+        // cout << "    yyy: m_axisCol[" << iax << "] (core) with type=" << m_axisCol[iax]->getType() 
+        //      << " and [" << jax << "] (branch) with type=" << m_axisCol[jax]->getType() 
+        //      << " are merged" << endl;
+        // cout << "         hit in m_axisCol["<<iax<<"]:"<<endl;
+        // for(int yii=0; yii<m_axisCol[iax]->getCluster().size(); yii++){
+        //   cout << "           (" << std::fixed << std::setprecision(2) << m_axisCol[iax]->getCluster()[yii]->getPos().x()
+        //        << ", " << std::fixed << std::setprecision(2) << m_axisCol[iax]->getCluster()[yii]->getPos().y()
+        //        << ", " << std::fixed << std::setprecision(2) << m_axisCol[iax]->getCluster()[yii]->getPos().z()
+        //        << ")" << endl;
+        // }
+        // cout << "         hit in m_axisCol["<<jax<<"]:"<<endl;
+        // for(int yii=0; yii<m_axisCol[jax]->getCluster().size(); yii++){
+        //   cout << "           (" << std::fixed << std::setprecision(2) << m_axisCol[jax]->getCluster()[yii]->getPos().x()
+        //        << ", " << std::fixed << std::setprecision(2) << m_axisCol[jax]->getCluster()[yii]->getPos().y()
+        //        << ", " << std::fixed << std::setprecision(2) << m_axisCol[jax]->getCluster()[yii]->getPos().z()
+        //        << ")" << endl;
+        // }
+ 
+        m_axisCol[iax]->mergeHalfCluster( m_axisCol[jax] );
+
+        int axis_type = m_axisCol[iax]->getType() + m_axisCol[jax]->getType();
+        m_axisCol[iax]->setType(axis_type);
+
+        //delete m_axisCol[jax]; m_axisCol[jax]=nullptr;
+        m_axisCol.erase(m_axisCol.begin()+jax);
+        jax--;
+        if(iax>jax+1) iax--;
+      }
+
+      p_axis=nullptr;
+    }
+    m_axis = nullptr;
+  }
+
+
+  return StatusCode::SUCCESS;
+};
+
+
+StatusCode AxisMergingAlg::ConeMerging( std::vector<PandoraPlus::CaloHalfCluster*>& m_axisCol ){
+  if(m_axisCol.size()<2) return StatusCode::SUCCESS;
+
+  std::sort( m_axisCol.begin(), m_axisCol.end(), compLayer );
+  for(int iax=0; iax<m_axisCol.size(); iax++){
+    const PandoraPlus::CaloHalfCluster* m_axis = m_axisCol[iax];
+    for(int jax=iax+1; jax<m_axisCol.size(); jax++){
+      const PandoraPlus::CaloHalfCluster* p_axis = m_axisCol[jax];
+
+  //printf("  Layer range: axis %d: [%d, %d], axis %d: [%d, %d] \n", iax, m_axis->getBeginningDlayer(), m_axis->getEndDlayer(), jax, p_axis->getBeginningDlayer(), p_axis->getEndDlayer());
+
+      TVector3 m_beginPoint = m_axis->getClusterInLayer( m_axis->getBeginningDlayer() )[0]->getPos();
+      TVector3 m_endPoint   = m_axis->getClusterInLayer( m_axis->getEndDlayer() )[0]->getPos();
+      TVector3 p_beginPoint = p_axis->getClusterInLayer( p_axis->getBeginningDlayer() )[0]->getPos();
+      TVector3 p_endPoint   = p_axis->getClusterInLayer( p_axis->getEndDlayer() )[0]->getPos();
+      double relDis = -1.; 
+      if( m_endPoint.Mag()<p_beginPoint.Mag() )      relDis = p_beginPoint.Mag() - m_endPoint.Mag(); 
+      else if( p_endPoint.Mag()<m_beginPoint.Mag() ) relDis = m_beginPoint.Mag() - p_endPoint.Mag();
+      else if( m_beginPoint.Mag()>p_beginPoint.Mag() && m_endPoint.Mag()>p_endPoint.Mag() )  relDis = p_endPoint.Mag() - m_beginPoint.Mag();
+      else if( p_beginPoint.Mag()>m_beginPoint.Mag() && p_endPoint.Mag()>m_endPoint.Mag() )  relDis = m_endPoint.Mag() - p_beginPoint.Mag();
+      else if( (m_beginPoint.Mag()>p_beginPoint.Mag() && m_endPoint.Mag()<p_endPoint.Mag()) ||
+               (m_beginPoint.Mag()<p_beginPoint.Mag() && m_endPoint.Mag()>p_endPoint.Mag()) ) relDis = 9999.;
+
+      double minRelAngle = min( sin( (m_axis->getPos()-p_axis->getPos()).Angle(m_axis->getAxis())),
+                                sin( (m_axis->getPos()-p_axis->getPos()).Angle(p_axis->getAxis())) );
+  //      int skipLayer = m_axis->getBeginningDlayer()<p_axis->getBeginningDlayer() ?
+  //                      (p_axis->getBeginningDlayer()-m_axis->getEndDlayer()) : (m_axis->getBeginningDlayer()-p_axis->getEndDlayer());
+
+  //printf("  AxisMerging: axes pair (%d, %d): axisAngel = %.3f, RelPAngle = %.3f, dis = %d \n", iax, jax, m_axis->getAxis().Angle(p_axis->getAxis()), minRelAngle, relDis);
+
+
+      if( ( sin( m_axis->getAxis().Angle(p_axis->getAxis()) ) < sin(settings.map_floatPars["axis_Angle"]) ||
+            sin(minRelAngle) < sin(settings.map_floatPars["relP_Angle"]) )  &&
+          relDis <= settings.map_floatPars["relP_Dis"] && relDis>=0 ){
+  //cout<<"    Merge! "<<endl;
+
+        m_axisCol[iax]->mergeHalfCluster( m_axisCol[jax] );
+        if(m_axisCol[iax]->getType()==1 || m_axisCol[jax]->getType()==1 || m_axisCol[iax]->getType()==5 || m_axisCol[jax]->getType()==5) m_axisCol[iax]->setType(5);
+        else if(m_axisCol[iax]->getType()==3 || m_axisCol[jax]->getType()==3 || m_axisCol[iax]->getType()==6 || m_axisCol[jax]->getType()==6) m_axisCol[iax]->setType(6);
+        else if(m_axisCol[iax]->getType()==4 || m_axisCol[jax]->getType()==4) m_axisCol[iax]->setType(4);
+        else m_axisCol[iax]->setType(7);
+
+        //delete m_axisCol[jax]; m_axisCol[jax]=nullptr;
+        m_axisCol.erase(m_axisCol.begin()+jax);
+        jax--;
+        if(iax>jax+1) iax--;
+      }
+      p_axis = nullptr;
+    }
+    m_axis = nullptr;
+  }
+
+  //cout<<"Iteration merge"<<endl;
+
+  //iterate
+  for(int iax=0; iax<m_axisCol.size(); iax++){
+    const PandoraPlus::CaloHalfCluster* m_axis = m_axisCol[iax];
+    for(int jax=iax+1; jax<m_axisCol.size(); jax++){
+      const PandoraPlus::CaloHalfCluster* p_axis = m_axisCol[jax];
+
+      TVector3 m_beginPoint = m_axis->getClusterInLayer( m_axis->getBeginningDlayer() )[0]->getPos();
+      TVector3 m_endPoint   = m_axis->getClusterInLayer( m_axis->getEndDlayer() )[0]->getPos();
+      TVector3 p_beginPoint = p_axis->getClusterInLayer( p_axis->getBeginningDlayer() )[0]->getPos();
+      TVector3 p_endPoint   = p_axis->getClusterInLayer( p_axis->getEndDlayer() )[0]->getPos();
+      double relDis = -1.;
+      if( m_endPoint.Mag()<p_beginPoint.Mag() )      relDis = p_beginPoint.Mag() - m_endPoint.Mag();
+      else if( p_endPoint.Mag()<m_beginPoint.Mag() ) relDis = m_beginPoint.Mag() - p_endPoint.Mag();
+      else if( m_beginPoint.Mag()>p_beginPoint.Mag() && m_endPoint.Mag()>p_endPoint.Mag() )  relDis = p_endPoint.Mag() - m_beginPoint.Mag();
+      else if( p_beginPoint.Mag()>m_beginPoint.Mag() && p_endPoint.Mag()>m_endPoint.Mag() )  relDis = m_endPoint.Mag() - p_beginPoint.Mag();
+      else if( (m_beginPoint.Mag()>p_beginPoint.Mag() && m_endPoint.Mag()<p_endPoint.Mag()) ||
+               (m_beginPoint.Mag()<p_beginPoint.Mag() && m_endPoint.Mag()>p_endPoint.Mag()) ) relDis = 9999.;
+      double minRelAngle = min( sin( (m_axis->getEnergyCenter()-p_axis->getEnergyCenter()).Angle(m_axis->getAxis())),
+                                sin( (m_axis->getEnergyCenter()-p_axis->getEnergyCenter()).Angle(p_axis->getAxis())) );
+      //int skipLayer = m_axis->getBeginningDlayer()<p_axis->getBeginningDlayer() ?
+      //                (p_axis->getBeginningDlayer()-m_axis->getEndDlayer()) : (m_axis->getBeginningDlayer()-p_axis->getEndDlayer());
+
+  //printf("  AxisMerging: axes pair (%d, %d): axisAngel = %.3f, RelPAngle = %.3f, dis = %d \n", iax, jax, m_axis->getAxis().Angle(p_axis->getAxis()), minRelAngle, relDis);
+  //printf("  Layer range: axis %d: [%d, %d], axis %d: [%d, %d] \n", iax, m_axis->getBeginningDlayer(), m_axis->getEndDlayer(), jax, p_axis->getBeginningDlayer(), p_axis->getEndDlayer());
+
+      if( ( sin( m_axis->getAxis().Angle(p_axis->getAxis()) ) < sin(settings.map_floatPars["axis_Angle"]) ||
+            sin(minRelAngle) < sin(settings.map_floatPars["relP_Angle"]) ) &&
+          relDis <= settings.map_floatPars["relP_Dis"] && relDis>=0 ){
+
+        m_axisCol[iax]->mergeHalfCluster( m_axisCol[jax] );
+        if(m_axisCol[iax]->getType()==1 || m_axisCol[jax]->getType()==1 || m_axisCol[iax]->getType()==5 || m_axisCol[jax]->getType()==5) m_axisCol[iax]->setType(5);
+        else if(m_axisCol[iax]->getType()==3 || m_axisCol[jax]->getType()==3 || m_axisCol[iax]->getType()==6 || m_axisCol[jax]->getType()==6) m_axisCol[iax]->setType(6);
+        else if(m_axisCol[iax]->getType()==4 || m_axisCol[jax]->getType()==4) m_axisCol[iax]->setType(4);
+        else m_axisCol[iax]->setType(7);
+
+        //delete m_axisCol[jax]; m_axisCol[jax]=nullptr;
+        m_axisCol.erase(m_axisCol.begin()+jax);
+        jax--;
+        if(iax>jax+1) iax--;
+      }
+      p_axis = nullptr;
+    }
+    m_axis = nullptr;
+  }
+
+  return StatusCode::SUCCESS;
+};
+
+
+bool AxisMergingAlg::MergeToClosestCluster( PandoraPlus::CaloHalfCluster* m_badaxis, std::vector<PandoraPlus::CaloHalfCluster*>& m_axisCol ){
+  if(m_axisCol.size()==0){ 
+    //m_axisCol.push_back(m_badaxis);
+    // cout << "  yyy: no good axis, treat the bad axis as good axis" << endl;
+    return false;
+  }
+
+  float minR = 9999;
+  int index = -1;
+  for(int iax=0; iax<m_axisCol.size(); iax++){
+    float tmp_R = (m_badaxis->getEnergyCenter()-m_axisCol[iax]->getEnergyCenter()).Mag();
+    if( tmp_R<minR ){
+      minR = tmp_R;
+      index = iax;
+    }
+  }
+  if(index>=0){
+    m_axisCol[index]->mergeHalfCluster(m_badaxis);
+
+    int axis_type = m_axisCol[index]->getType() + m_badaxis->getType();
+    m_axisCol[index]->setType(axis_type);
+
+    return true;
+  } 
+  return false;
+};
+
+/*
+StatusCode AxisMergingAlg::SplitOverlapCluster( std::shared_ptr<PandoraPlus::CaloHalfCluster>& m_HFClus,
+                                                std::vector<PandoraPlus::CaloHalfCluster*>& m_axisCol,
+                                                std::vector<std::shared_ptr<PandoraPlus::Calo1DCluster>>& bk_1Dclus,
+                                                std::vector<std::shared_ptr<PandoraPlus::CaloUnit>>& bk_Unit){
+
+  if(m_axisCol.size()<2) return StatusCode::SUCCESS;
+
+  for(int ic=0; ic<m_axisCol.size()-1; ic++){
+    for(int jc=ic+1; jc<m_axisCol.size(); jc++){
+      double E_ratio = m_axisCol[ic]->OverlapRatioE(m_axisCol[jc]);
+      if(E_ratio>0){
+
+        std::vector<const Calo1DCluster*> m_ClusCol1 =  m_axisCol[ic]->getCluster();
+        std::vector<const Calo1DCluster*> m_ClusCol2 =  m_axisCol[jc]->getCluster();
+        std::vector<const Calo1DCluster*> m_overlapCl; m_overlapCl.clear();
+        for(int i1d=0; i1d<m_ClusCol1.size(); i1d++){
+          if(find(m_ClusCol2.begin(), m_ClusCol2.end(), m_ClusCol1[i1d])!=m_ClusCol2.end() ) m_overlapCl.push_back(m_ClusCol1[i1d]);
+        }
+
+        for(int io=0; io<m_overlapCl.size(); io++){
+
+          //Split the cluster with +-1 layer cluster energy.
+          int dlayer = m_overlapCl[io]->getDlayer();
+
+          //  For  m_axisCol[ic]
+          std::vector<const Calo1DCluster*> m_clus1_front = m_axisCol[ic]->getClusterInLayer(dlayer-1);
+          std::vector<const Calo1DCluster*> m_clus1_back  = m_axisCol[ic]->getClusterInLayer(dlayer+1);
+          std::vector<const Calo1DCluster*> m_clus2_front = m_axisCol[jc]->getClusterInLayer(dlayer-1);
+          std::vector<const Calo1DCluster*> m_clus2_back  = m_axisCol[jc]->getClusterInLayer(dlayer+1);
+
+          if( m_clus1_front.size()==0 && m_clus1_back.size()==0 && m_clus2_front.size()==0 && m_clus2_back.size()==0 ){
+            //An isolated hit in both: delete in m_axisCol[ic]. TODO: maybe need to optimize based on profile?
+            m_axisCol[ic]->deleteUnit(m_overlapCl[io]);
+            continue;
+          }
+          else if( m_clus1_front.size()==0 && m_clus1_back.size()==0 ){
+            //Only isolated in m_axisCol[ic]: delete in m_axisCol[ic].
+            m_axisCol[ic]->deleteUnit(m_overlapCl[io]);
+            continue;
+          }
+          else if( m_clus2_front.size()==0 && m_clus2_back.size()==0 ){
+            m_axisCol[jc]->deleteUnit(m_overlapCl[io]);
+            continue;
+          }
+          else{
+            //Use the average energy
+            double aveEn1 = 0.;
+            for(int i1d=0; i1d<m_clus1_front.size(); i1d++) aveEn1 += m_clus1_front[i1d]->getEnergy();
+            for(int i1d=0; i1d<m_clus1_back.size(); i1d++) aveEn1 += m_clus1_back[i1d]->getEnergy();
+            aveEn1 = aveEn1/(m_clus1_front.size()+m_clus1_back.size());
+
+            double aveEn2 = 0.;
+            for(int i1d=0; i1d<m_clus2_front.size(); i1d++) aveEn2 += m_clus2_front[i1d]->getEnergy();
+            for(int i1d=0; i1d<m_clus2_back.size(); i1d++) aveEn2 += m_clus2_back[i1d]->getEnergy();
+            aveEn2 = aveEn2/(m_clus2_front.size()+m_clus2_back.size());
+
+            std::shared_ptr<PandoraPlus::Calo1DCluster> m_splitCl1 = std::make_shared<PandoraPlus::Calo1DCluster>();
+            std::shared_ptr<PandoraPlus::Calo1DCluster> m_splitCl2 = std::make_shared<PandoraPlus::Calo1DCluster>();
+
+            for(int ib=0; ib<m_overlapCl[io]->getBars().size(); ib++){
+              auto bar1 = m_overlapCl[io]->getBars()[ib]->Clone();
+              bar1->setQ( bar1->getQ1()*aveEn1/(aveEn1+aveEn2), bar1->getQ2()*aveEn1/(aveEn1+aveEn2) );
+              m_splitCl1->addUnit(bar1.get());
+
+              auto bar2 = m_overlapCl[io]->getBars()[ib]->Clone();
+              bar2->setQ( bar2->getQ1()*aveEn2/(aveEn1+aveEn2), bar2->getQ2()*aveEn2/(aveEn1+aveEn2) );
+              m_splitCl2->addUnit(bar2.get());
+
+              bk_Unit.push_back(bar1);
+              bk_Unit.push_back(bar2);
+            }
+
+            m_axisCol[ic]->deleteUnit(m_overlapCl[io]);
+            m_axisCol[ic]->addUnit(m_splitCl1.get());
+
+            m_axisCol[jc]->deleteUnit(m_overlapCl[io]);
+            m_axisCol[jc]->addUnit(m_splitCl2.get());
+
+            //m_HFClus->deleteUnit(m_overlapCl[io]);
+            //m_HFClus->addUnit(m_splitCl1.get());
+            //m_HFClus->addUnit(m_splitCl2.get());
+
+            bk_1Dclus.push_back(m_splitCl1);
+            bk_1Dclus.push_back(m_splitCl2);
+          }
+
+
+        }//end loop m_axisCol
+      }
+
+    }
+  }
+  return StatusCode::SUCCESS;
+}
+*/
+#endif
diff --git a/Reconstruction/CrystalCaloRec/src/Algorithm/ClusterMergingAlg.cpp b/Reconstruction/CrystalCaloRec/src/Algorithm/ClusterMergingAlg.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c0dcbf9a9f0edb475b40195f81568e330444682d
--- /dev/null
+++ b/Reconstruction/CrystalCaloRec/src/Algorithm/ClusterMergingAlg.cpp
@@ -0,0 +1,32 @@
+#ifndef _CLUSTERMERGING_ALG_C
+#define _CLUSTERMERGING_ALG_C
+
+#include "Algorithm/ClusterMergingAlg.h"
+
+StatusCode ClusterMergingAlg::ReadSettings(Settings& m_settings){
+  settings = m_settings;
+
+  //Initialize parameters
+
+  return StatusCode::SUCCESS;
+};
+
+StatusCode ClusterMergingAlg::Initialize( PandoraPlusDataCol& m_datacol ){
+
+  return StatusCode::SUCCESS;
+};
+
+StatusCode ClusterMergingAlg::RunAlgorithm( PandoraPlusDataCol& m_datacol ){
+
+
+  return StatusCode::SUCCESS;
+};
+
+StatusCode ClusterMergingAlg::ClearAlgorithm(){
+
+
+  return StatusCode::SUCCESS;
+};
+
+
+#endif
diff --git a/Reconstruction/CrystalCaloRec/src/Algorithm/ConeClustering2DAlg.cpp b/Reconstruction/CrystalCaloRec/src/Algorithm/ConeClustering2DAlg.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4696277c301dd1a4a0aee80970915340aaad678c
--- /dev/null
+++ b/Reconstruction/CrystalCaloRec/src/Algorithm/ConeClustering2DAlg.cpp
@@ -0,0 +1,212 @@
+#ifndef _CONECLUSTERING2D_ALG_C
+#define _CONECLUSTERING2D_ALG_C
+
+#include "Algorithm/ConeClustering2DAlg.h"
+
+StatusCode ConeClustering2DAlg::ReadSettings(Settings& m_settings){
+  settings = m_settings;
+
+  //Initialize parameters
+  if(settings.map_floatPars.find("th_beginLayer")==settings.map_floatPars.end())   settings.map_floatPars["th_beginLayer"] = 1;
+  if(settings.map_floatPars.find("th_stopLayer")==settings.map_floatPars.end())    settings.map_floatPars["th_stopLayer"] = 15;
+  if(settings.map_floatPars.find("th_ConeTheta")==settings.map_floatPars.end())    settings.map_floatPars["th_ConeTheta"] = TMath::Pi()/4.;
+  if(settings.map_floatPars.find("th_ConeR")==settings.map_floatPars.end())        settings.map_floatPars["th_ConeR"] = 50;
+  if(settings.map_intPars.find("th_Nshowers")==settings.map_intPars.end())       settings.map_intPars["th_Nshowers"] = 4;
+  if(settings.map_stringPars.find("ReadinLocalMaxName")==settings.map_stringPars.end()) settings.map_stringPars["ReadinLocalMaxName"] = "LeftLocalMax";
+  if(settings.map_stringPars.find("OutputLongiClusName")==settings.map_stringPars.end()) settings.map_stringPars["OutputLongiClusName"] = "ConeAxis";
+
+  return StatusCode::SUCCESS;
+};
+
+
+StatusCode ConeClustering2DAlg::Initialize( PandoraPlusDataCol& m_datacol ){
+  p_HalfClusterU.clear();
+  p_HalfClusterV.clear();
+
+
+  for(int ih=0; ih<m_datacol.map_HalfCluster["HalfClusterColU"].size(); ih++)
+    p_HalfClusterU.push_back( m_datacol.map_HalfCluster["HalfClusterColU"][ih].get() );
+  for(int ih=0; ih<m_datacol.map_HalfCluster["HalfClusterColV"].size(); ih++)
+    p_HalfClusterV.push_back( m_datacol.map_HalfCluster["HalfClusterColV"][ih].get() );
+
+  return StatusCode::SUCCESS;
+}
+
+StatusCode ConeClustering2DAlg::RunAlgorithm( PandoraPlusDataCol& m_datacol ){
+
+  if( (p_HalfClusterU.size()+p_HalfClusterV.size())<1 ){
+    std::cout << "ConeClustering2DAlg: No HalfCluster input"<<std::endl;
+    return StatusCode::SUCCESS;
+  }   
+ 
+  std::vector<PandoraPlus::CaloHalfCluster*> tmp_longiClusCol; tmp_longiClusCol.clear(); 
+  std::map<int, std::vector<const PandoraPlus::Calo1DCluster*> > m_orderedLocalMax; 
+
+//cout<<"  ConeClustering: Input HalfCluster size "<<p_HalfClusterU.size()<<", "<<p_HalfClusterV.size()<<endl;
+
+  //Processing U plane:
+  for(int ic=0; ic<p_HalfClusterU.size(); ic++){
+    //Get LocalMax
+    m_localMaxUCol.clear(); 
+    m_localMaxUCol = p_HalfClusterU[ic]->getLocalMaxCol(settings.map_stringPars["ReadinLocalMaxName"]);
+
+//cout<<"    In ClusU #"<<ic<<": localMax size "<<m_localMaxUCol.size()<<endl;
+
+    //Get ordered LocalMax
+    m_orderedLocalMax.clear();
+    for(int is=0; is<m_localMaxUCol.size(); is++)
+      m_orderedLocalMax[m_localMaxUCol[is]->getDlayer()].push_back(m_localMaxUCol[is]);
+
+    tmp_longiClusCol.clear();
+    LongiConeLinking( m_orderedLocalMax, tmp_longiClusCol, m_datacol.map_HalfCluster["bkHalfCluster"] );
+
+//cout<<"    Cluster size after ConeLinking: "<<tmp_longiClusCol.size()<<endl;
+
+    //Convert LongiClusters to const object.    
+    const_longiClusUCol.clear();
+    for(int ic=0; ic<tmp_longiClusCol.size(); ic++){
+      tmp_longiClusCol[ic]->Check();
+      if(tmp_longiClusCol[ic]->getCluster().size()>=settings.map_intPars["th_Nshowers"]) const_longiClusUCol.push_back(tmp_longiClusCol[ic]);
+    }
+//cout<<"    Cluster size after requiring Nhit: "<<const_longiClusUCol.size()<<endl;
+
+    p_HalfClusterU[ic]->setHalfClusters(settings.map_stringPars["OutputLongiClusName"], const_longiClusUCol);    
+  }
+
+  //Processing V plane:
+  for(int ic=0; ic<p_HalfClusterV.size(); ic++){
+    m_localMaxVCol.clear();
+    m_localMaxVCol = p_HalfClusterV[ic]->getLocalMaxCol(settings.map_stringPars["ReadinLocalMaxName"]);
+
+    m_orderedLocalMax.clear();
+    for(int is=0; is<m_localMaxVCol.size(); is++)
+      m_orderedLocalMax[m_localMaxVCol[is]->getDlayer()].push_back(m_localMaxVCol[is]);
+
+    tmp_longiClusCol.clear();
+    LongiConeLinking(m_orderedLocalMax, tmp_longiClusCol, m_datacol.map_HalfCluster["bkHalfCluster"]);
+
+
+    //Convert LongiClusters to const object.
+    const_longiClusVCol.clear();
+    for(int ic=0; ic<tmp_longiClusCol.size(); ic++){
+      tmp_longiClusCol[ic]->Check();
+      if(tmp_longiClusCol[ic]->getCluster().size()>=settings.map_intPars["th_Nshowers"]) const_longiClusVCol.push_back(tmp_longiClusCol[ic]);
+    }
+
+    p_HalfClusterV[ic]->setHalfClusters(settings.map_stringPars["OutputLongiClusName"], const_longiClusVCol);
+  }
+
+  tmp_longiClusCol.clear();
+  m_orderedLocalMax.clear();
+  return StatusCode::SUCCESS;
+}
+
+StatusCode ConeClustering2DAlg::ClearAlgorithm(){
+  p_HalfClusterV.clear(); 
+  p_HalfClusterU.clear();
+  m_localMaxVCol.clear();
+  m_localMaxUCol.clear();
+  const_longiClusVCol.clear();
+  const_longiClusUCol.clear();
+
+  return StatusCode::SUCCESS;
+}
+
+
+StatusCode ConeClustering2DAlg::LongiConeLinking(  std::map<int, std::vector<const PandoraPlus::Calo1DCluster*> >& orderedShower,
+                                                   std::vector<PandoraPlus::CaloHalfCluster*>& ClusterCol, 
+                                                   std::vector<std::shared_ptr<PandoraPlus::CaloHalfCluster>>& bk_HFclus )
+{
+  if(orderedShower.size()==0) return StatusCode::SUCCESS;
+ 
+  vector<std::shared_ptr<PandoraPlus::CaloHalfCluster>> m_clusCol; m_clusCol.clear();
+  std::map<int, std::vector<const PandoraPlus::Calo1DCluster*>>::iterator iter = orderedShower.begin();
+  //In first layer: initial clusters. All showers in the first layer are regarded as cluster seed.
+  //cluster initial direction = R.
+  std::vector<const PandoraPlus::Calo1DCluster*> ShowersinFirstLayer;  ShowersinFirstLayer.clear();
+  ShowersinFirstLayer = iter->second;
+  for(int i=0;i<ShowersinFirstLayer.size(); i++){
+    if(iter->first < settings.map_floatPars["th_beginLayer"] || iter->first > settings.map_floatPars["th_stopLayer"] ) continue; 
+    std::shared_ptr<PandoraPlus::CaloHalfCluster> m_clus = std::make_shared<PandoraPlus::CaloHalfCluster>();
+    m_clus->addUnit(ShowersinFirstLayer[i]);
+    m_clus->setType(1);
+    m_clusCol.push_back(m_clus);
+  }
+  iter++;
+
+
+  for(iter; iter!=orderedShower.end(); iter++){
+    if(iter->first < settings.map_floatPars["th_beginLayer"] || iter->first > settings.map_floatPars["th_stopLayer"] ) continue; 
+    std::vector<const PandoraPlus::Calo1DCluster*> ShowersinLayer = iter->second;
+//cout<<"  In Layer "<<iter->first<<": hit size "<<ShowersinLayer.size()<<endl;
+
+    for(int ic=0; ic<m_clusCol.size(); ic++){
+      const PandoraPlus::Calo1DCluster* shower_in_clus = m_clusCol[ic].get()->getCluster().back();
+      if(!shower_in_clus) continue;
+      for(int is=0; is<ShowersinLayer.size(); is++){
+        TVector2 relR = GetProjectedRelR(shower_in_clus, ShowersinLayer[is]);  //Return vec: 1->2.
+        TVector2 clusaxis = GetProjectedAxis( m_clusCol[ic].get() );
+//printf("    Cluster axis (%.3f, %.3f), last hit (%.3f, %.3f, %.3f), coming hit (%.3f, %.3f, %.3f), projected relR (%.3f, %.3f) \n",
+//clusaxis.Px(), clusaxis.Py(), shower_in_clus->getPos().x(),  shower_in_clus->getPos().y(),  shower_in_clus->getPos().z(),
+//ShowersinLayer[is]->getPos().x(), ShowersinLayer[is]->getPos().y(), ShowersinLayer[is]->getPos().z(), relR.Px(), relR.Py() );
+
+        if( relR.DeltaPhi(clusaxis)<settings.map_floatPars["th_ConeTheta"] && relR.Mod()<settings.map_floatPars["th_ConeR"] ){
+          m_clusCol[ic].get()->addUnit(ShowersinLayer[is]);
+          ShowersinLayer.erase(ShowersinLayer.begin()+is);
+          is--;
+        }
+      }
+    }//end loop showers in layer.
+
+    if(ShowersinLayer.size()>0){
+      for(int i=0;i<ShowersinLayer.size(); i++){
+        std::shared_ptr<PandoraPlus::CaloHalfCluster> m_clus = std::make_shared<PandoraPlus::CaloHalfCluster>();
+        m_clus->addUnit(ShowersinLayer[i]);
+        m_clus->setType(1);
+        m_clusCol.push_back(m_clus);
+    }}//end new cluster
+  }//end loop layers.
+
+  bk_HFclus.insert(bk_HFclus.end(), m_clusCol.begin(), m_clusCol.end());
+  for(int icl=0; icl<m_clusCol.size(); icl++){
+    m_clusCol[icl]->getLinkedMCPfromUnit();
+    ClusterCol.push_back( m_clusCol[icl].get() );  
+  }
+
+  return StatusCode::SUCCESS;
+}
+
+
+
+TVector2 ConeClustering2DAlg::GetProjectedAxis( const PandoraPlus::CaloHalfCluster* m_shower ){
+  TVector2 axis(0., 0.);
+  TVector3 m_axis = m_shower->getAxis();
+  if( m_shower->getSlayer()==1 )  axis.Set(m_axis.x(), m_axis.y()); //For V-bars: (x, y) 
+  else                            axis.Set( sqrt(m_axis.x()*m_axis.x() + m_axis.y()*m_axis.y()), m_axis.z()); //For U-bars: (R, z)
+  axis.Unit();
+
+  return axis;
+}
+
+TVector2 ConeClustering2DAlg::GetProjectedRelR( const PandoraPlus::Calo1DCluster* m_shower1, const PandoraPlus::Calo1DCluster* m_shower2 ){
+  TVector2 paxis1, paxis2;
+  if(m_shower1->getSlayer()==1){ //For V-bars
+    paxis1.Set(m_shower1->getPos().x(), m_shower1->getPos().y());
+    paxis2.Set(m_shower2->getPos().x(), m_shower2->getPos().y());
+  }
+  else{
+    float rotAngle = -m_shower1->getTowerID()[0][0]*TMath::TwoPi()/PandoraPlus::CaloUnit::Nmodule;
+    TVector3 raw_pos1 = m_shower1->getPos(); 
+    TVector3 raw_pos2 = m_shower2->getPos(); 
+    raw_pos1.RotateZ(rotAngle); raw_pos2.RotateZ(rotAngle);
+    paxis1.Set(raw_pos1.y(), raw_pos1.z());
+    paxis2.Set(raw_pos2.y(), raw_pos2.z());
+
+    //paxis1.Set( sqrt(m_shower1->getPos().x()*m_shower1->getPos().x()+m_shower1->getPos().y()*m_shower1->getPos().y()), m_shower1->getPos().z());
+    //paxis2.Set( sqrt(m_shower2->getPos().x()*m_shower2->getPos().x()+m_shower2->getPos().y()*m_shower2->getPos().y()), m_shower2->getPos().z());
+  }
+
+  return paxis2 - paxis1;
+}
+
+#endif
diff --git a/Reconstruction/CrystalCaloRec/src/Algorithm/ConeClusteringAlg.cpp b/Reconstruction/CrystalCaloRec/src/Algorithm/ConeClusteringAlg.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..95d6f440291a1d540452cfec9d99f6133c01c483
--- /dev/null
+++ b/Reconstruction/CrystalCaloRec/src/Algorithm/ConeClusteringAlg.cpp
@@ -0,0 +1,220 @@
+#ifndef _CONECLUSTERING_ALG_C
+#define _CONECLUSTERING_ALG_C
+
+#include "Algorithm/ConeClusteringAlg.h"
+
+StatusCode ConeClusteringAlg::ReadSettings(Settings& m_settings){
+  settings = m_settings;
+
+  //Option: readin information
+  if(settings.map_stringPars.find("ReadinHit")==settings.map_stringPars.end())        settings.map_stringPars["ReadinHit"] = "EcalTransShower";
+  if(settings.map_stringPars.find("OutputCluster")==settings.map_stringPars.end())    settings.map_stringPars["OutputCluster"] = "EcalBarCluster";
+
+
+  //Set initial values
+  if(settings.map_floatPars.find("th_ConeTheta_l1")==settings.map_floatPars.end())    settings.map_floatPars["th_ConeTheta_l1"] = TMath::Pi()/2.;
+  if(settings.map_floatPars.find("th_ConeR_l1")==settings.map_floatPars.end())        settings.map_floatPars["th_ConeR_l1"] = 70.;
+  if(settings.map_floatPars.find("th_ConeTheta_l2")==settings.map_floatPars.end())    settings.map_floatPars["th_ConeTheta_l2"] = TMath::Pi()/3.;
+  if(settings.map_floatPars.find("th_ConeR_l2")==settings.map_floatPars.end())        settings.map_floatPars["th_ConeR_l2"] = 120.;
+  if(settings.map_floatPars.find("th_ClusChi2")==settings.map_floatPars.end())        settings.map_floatPars["th_ClusChi2"] = 10e17;
+  if(settings.map_floatPars.find("fl_GoodClusLevel")==settings.map_floatPars.end())   settings.map_floatPars["fl_GoodClusLevel"] = 10;
+  //For Cluster merging 
+  if(settings.map_floatPars.find("axis_Angle")==settings.map_floatPars.end())         settings.map_floatPars["axis_Angle"] = TMath::Pi()/6.;
+  if(settings.map_floatPars.find("relP_Angle")==settings.map_floatPars.end())         settings.map_floatPars["relP_Angle"] = TMath::Pi()/5.;
+  if(settings.map_floatPars.find("skipLayer")==settings.map_floatPars.end())          settings.map_floatPars["skipLayer"] = 3;
+  if(settings.map_floatPars.find("fl_MergeGoodClus")==settings.map_floatPars.end())   settings.map_floatPars["fl_MergeGoodClus"] = 1;
+  if(settings.map_floatPars.find("fl_MergeBadClus")==settings.map_floatPars.end())    settings.map_floatPars["fl_MergeBadClus"] = 1;
+  if(settings.map_floatPars.find("fl_MergeEMTail")==settings.map_floatPars.end())     settings.map_floatPars["fl_MergeEMTail"] = 1;
+
+  return StatusCode::SUCCESS;
+};
+
+StatusCode ConeClusteringAlg::Initialize( PandoraPlusDataCol& m_datacol ){
+
+  return StatusCode::SUCCESS;
+}
+
+
+StatusCode ConeClusteringAlg::RunAlgorithm( PandoraPlusDataCol& m_datacol){
+
+  std::vector<std::shared_ptr<PandoraPlus::CaloHit>> m_hitCol; m_hitCol.clear(); 
+  //Get readin info
+  if(settings.map_stringPars["ReadinHit"] == "EcalTransShower"){
+
+    std::vector<std::shared_ptr<PandoraPlus::Calo2DCluster>>* p_Tshowers = &(m_datacol.map_2DCluster["EcalShowerInLayer"]);
+    if(p_Tshowers->size()==0){ 
+      std::cout<<"Warning: Empty input in ConeClusteringAlg. Please check previous algorithm!"<<endl;  
+      return StatusCode::SUCCESS; 
+    }
+    //Convert transShower to hit
+    for(int is=0; is<p_Tshowers->size(); is++){
+      std::shared_ptr<PandoraPlus::CaloHit> m_hit = std::make_shared<PandoraPlus::CaloHit>();
+      m_hit->setEnergy( p_Tshowers->at(is)->getEnergy() );
+      m_hit->setPosition( p_Tshowers->at(is)->getPos() );
+      m_hit->setLayer( p_Tshowers->at(is)->getDlayer() );
+      //m_hit->setParentShower( p_Tshowers->at(is) );
+      m_hitCol.push_back(m_hit);
+      m_datacol.map_CaloHit["bkHit"].push_back(m_hit);
+    }
+    p_Tshowers = nullptr;
+  }
+  else m_hitCol = m_datacol.map_CaloHit[settings.map_stringPars["ReadinHit"]]; 
+  if(m_hitCol.size()==0) { std::cout<<"Warning: Empty input in ConeClusteringAlg. Please check previous algorithm!"<<endl; return StatusCode::SUCCESS; }
+
+cout<<"  ConeClusteringAlg: input hit size "<<m_hitCol.size()<<endl;
+/*
+cout<<"ConeClustering: Print Input Hits"<<endl;
+for(int i=0; i<m_hitCol.size(); i++)
+  printf("  Hit #%d: pos/E (%.2f, %.2f, %.2f, %.3f), Layer %d. \n", i,  
+        m_hitCol[i]->getPosition().x(), m_hitCol[i]->getPosition().y(), m_hitCol[i]->getPosition().z(), m_hitCol[i]->getEnergy(), m_hitCol[i]->getLayer() );
+cout<<"ConeClustering: End Print"<<endl;
+*/
+
+  //Store the ordered hits.
+  std::map<int, std::vector<const PandoraPlus::CaloHit*> > m_orderedHit;  m_orderedHit.clear(); //map<layer, showers>
+  for(int ih=0;ih<m_hitCol.size();ih++)
+    m_orderedHit[m_hitCol[ih]->getLayer()].push_back(m_hitCol[ih].get());
+
+
+  //Longitudinal linking
+  std::vector<std::shared_ptr<PandoraPlus::Calo3DCluster>> m_clusterCol;  m_clusterCol.clear();
+  LongiConeLinking( m_orderedHit, m_clusterCol );
+  for(int icl=0; icl<m_clusterCol.size(); icl++) m_clusterCol[icl]->getLinkedMCPfromHit();
+  m_datacol.map_CaloCluster["bk3DCluster"].insert(  m_datacol.map_CaloCluster["bk3DCluster"].end(), m_clusterCol.begin(), m_clusterCol.end() );
+
+cout<<"  Cluster size: "<<m_clusterCol.size()<<endl;
+
+
+  //Check cluster quality.
+/*
+  std::vector<PandoraPlus::Calo3DCluster*>  goodClus;
+  std::vector<PandoraPlus::Calo3DCluster*>  badClus;
+  for(int icl=0; icl<m_clusterCol.size(); icl++){
+    if( m_clusterCol[icl]->getCaloHits().size() >= settings.map_floatPars["fl_GoodClusLevel"]) goodClus.push_back(m_clusterCol[icl]);
+    else badClus.push_back(m_clusterCol[icl]);
+  }
+
+cout<<"  Good cluster: "<<goodClus.size()<<endl;
+cout<<"  Bad cluster: "<<badClus.size()<<endl;
+
+  //Merge clusters
+  //MergeGoodClusters( goodClus );
+  //for(int icl=0;icl<badClus.size();icl++) MergeBadToGoodCluster(goodClus, badClus[icl] ); 
+  //MergeGoodClusters( goodClus );
+  //MergeEMTail( goodClus );
+
+  //m_datacol.GoodClus3DCol.insert(m_datacol.GoodClus3DCol.end(), goodClus.begin(), goodClus.end() );
+  //m_datacol.BadClus3DCol.insert( m_datacol.BadClus3DCol.end(),  badClus.begin(), badClus.end() );
+  //m_datacol.Clus3DCol.insert( m_datacol.Clus3DCol.end(), m_clusterCol.begin(), m_clusterCol.end() );
+*/
+  m_datacol.map_CaloCluster[settings.map_stringPars["OutputCluster"]] = m_clusterCol;
+
+  return StatusCode::SUCCESS;
+}
+
+StatusCode ConeClusteringAlg::ClearAlgorithm(){
+
+  return StatusCode::SUCCESS;
+}
+
+StatusCode ConeClusteringAlg::LongiConeLinking(  const std::map<int, std::vector<const PandoraPlus::CaloHit*> >& orderedHit, 
+                                                 std::vector<std::shared_ptr<PandoraPlus::Calo3DCluster>>& ClusterCol)
+{
+
+  if(orderedHit.size()==0) return StatusCode::SUCCESS;
+
+  auto iter = orderedHit.begin();
+  //In first layer: initial clusters. All showers in the first layer are regarded as cluster seed.
+  //cluster initial direction = R.
+  std::vector<const PandoraPlus::CaloHit*> HitsinFirstLayer = iter->second;
+  for(int i=0;i<HitsinFirstLayer.size(); i++){
+    std::shared_ptr<PandoraPlus::Calo3DCluster> m_clus = std::make_shared<PandoraPlus::Calo3DCluster>();
+    m_clus->addHit(HitsinFirstLayer[i]);
+    ClusterCol.push_back(m_clus);
+  }
+  iter++;
+
+//cout<<"    LongiConeLinking: Cluster seed in first layer: "<<ClusterCol.size()<<endl;
+
+  //Use different cone angle for 1->2/2->3 and 3->n case
+  //Loop later layers
+  for(iter; iter!=orderedHit.end(); iter++){
+    std::vector<const PandoraPlus::CaloHit*> HitsinLayer = iter->second;
+//cout<<"    In Layer: "<<iter->first<<"  Hit size: "<<HitsinLayer.size()<<endl;
+
+    for(int is=0; is<HitsinLayer.size(); is++){
+      const PandoraPlus::CaloHit* m_hit = HitsinLayer[is];
+//printf("     New Hit: (%.3f, %.3f, %.3f), Layer %d \n", m_hit->getPosition().x(), m_hit->getPosition().y(), m_hit->getPosition().z(),  m_hit->getLayer() );
+//cout<<"     Cluster size: "<<ClusterCol.size()<<endl;
+
+      for(int ic=0; ic<ClusterCol.size(); ic++ ){
+        int m_Nhits = ClusterCol[ic]->getCaloHits().size();
+        const PandoraPlus::CaloHit* hit_in_clus = ClusterCol[ic]->getCaloHits().back();
+        TVector3 relR_vec = m_hit->getPosition() - hit_in_clus->getPosition();
+//printf("      New hit: (%.2f, %.2f, %.2f), Cluster last: (%.2f, %.2f, %.2f, %d), Cluster axis: (%.2f, %.2f, %.2f) \n",
+//    m_hit->getPosition().x(), m_hit->getPosition().y(),m_hit->getPosition().z(),
+//    hit_in_clus->getPosition().x(), hit_in_clus->getPosition().y(), hit_in_clus->getPosition().z(), hit_in_clus->getLayer(), 
+//    ClusterCol[ic]->getAxis().x(), ClusterCol[ic]->getAxis().y(), ClusterCol[ic]->getAxis().z() );
+
+        if(  (m_Nhits<3 && m_Nhits>0 && relR_vec.Angle(ClusterCol[ic]->getAxis())< settings.map_floatPars["th_ConeTheta_l1"] && relR_vec.Mag()< settings.map_floatPars["th_ConeR_l1"]) ||
+             (m_Nhits>=3             && relR_vec.Angle(ClusterCol[ic]->getAxis())< settings.map_floatPars["th_ConeTheta_l2"] && relR_vec.Mag()< settings.map_floatPars["th_ConeR_l2"])  ){
+
+          ClusterCol[ic]->addHit(m_hit);
+          HitsinLayer.erase(HitsinLayer.begin()+is);
+          is--;
+          break;
+        }
+      }
+    }//end loop showers in layer.
+    if(HitsinLayer.size()>0){
+      for(int i=0;i<HitsinLayer.size(); i++){
+        std::shared_ptr<PandoraPlus::Calo3DCluster> m_clus = std::make_shared<PandoraPlus::Calo3DCluster>();
+        m_clus->addHit(HitsinLayer[i]);
+        ClusterCol.push_back(m_clus);
+    }}//end new cluster
+  }//end loop layers.
+
+
+  return StatusCode::SUCCESS;
+}
+
+/*
+StatusCode ConeClusteringAlg::MergeGoodClusters( std::vector<PandoraPlus::Calo3DCluster*>& m_clusCol ){
+
+  return StatusCode::SUCCESS;
+}
+
+StatusCode ConeClusteringAlg::MergeBadToGoodCluster( std::vector<PandoraPlus::Calo3DCluster*>& m_goodClusCol, PandoraPlus::Calo3DCluster* m_badClus ){
+  PandoraPlus::Calo3DCluster* m_clus = GetClosestGoodCluster( m_goodClusCol, m_badClus );
+  if(!m_clus) return StatusCode::FAILURE;
+
+  auto iter = find( m_goodClusCol.begin(), m_goodClusCol.end(), m_clus );
+  if(iter==m_goodClusCol.end()) return StatusCode::FAILURE;
+  else m_clus->mergeCluster( m_badClus );
+    
+  return StatusCode::SUCCESS;
+}
+
+
+
+PandoraPlus::Calo3DCluster* ConeClusteringAlg::GetClosestGoodCluster( std::vector< PandoraPlus::Calo3DCluster* >& m_goodClusCol, PandoraPlus::Calo3DCluster* m_badClus ){
+
+  TVector3 m_clusCent = m_badClus->getShowerCenter();
+  PandoraPlus::Calo3DCluster* m_clus = nullptr;
+  double minTheta=999;
+  for(int i=0;i<m_goodClusCol.size();i++){
+     TVector3 vec_goodClus = m_goodClusCol[i]->getShowerCenter();
+     TVector3 vec_goodAxis = m_goodClusCol[i]->getAxis();
+     double theta = vec_goodAxis.Cross(m_clusCent-vec_goodClus).Mag();
+
+     if(theta<minTheta){
+        minTheta=theta;
+        m_clus = m_goodClusCol[i];
+     }
+  }
+
+  return m_clus;
+}
+*/
+
+#endif
diff --git a/Reconstruction/CrystalCaloRec/src/Algorithm/EnergySplittingAlg.cpp b/Reconstruction/CrystalCaloRec/src/Algorithm/EnergySplittingAlg.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..58f00c70e8a620a32bfd69498abf838f298c990b
--- /dev/null
+++ b/Reconstruction/CrystalCaloRec/src/Algorithm/EnergySplittingAlg.cpp
@@ -0,0 +1,1289 @@
+#ifndef _ENERGYSPLITTING_ALG_C
+#define _ENERGYSPLITTING_ALG_C
+
+#include "Algorithm/EnergySplittingAlg.h"
+
+StatusCode EnergySplittingAlg::ReadSettings(Settings& m_settings){
+  settings = m_settings;
+
+  if(settings.map_floatPars.find("th_split")==settings.map_floatPars.end()) settings.map_floatPars["th_split"] = -1;
+  //if(settings.map_floatPars.find("Eth_Seed")==settings.map_floatPars.end()) settings.map_floatPars["Eth_Seed"] = 0.005;
+  //if(settings.map_floatPars.find("Eth_ShowerAbs")==settings.map_floatPars.end()) settings.map_floatPars["Eth_ShowerAbs"] = 0.005;
+  if(settings.map_floatPars.find("Eth_unit")==settings.map_floatPars.end())  settings.map_floatPars["Eth_unit"] = 0.001;
+  if(settings.map_floatPars.find("Eth_HFClus")==settings.map_floatPars.end())        settings.map_floatPars["Eth_HFClus"] = 0.05;
+  if(settings.map_intPars.find("th_Nhit")==settings.map_intPars.end()) settings.map_intPars["th_Nhit"] = 2;
+  if(settings.map_boolPars.find("CompactHFCluster")==settings.map_boolPars.end()) settings.map_boolPars["CompactHFCluster"] = true;
+  if(settings.map_stringPars.find("ReadinAxisName")==settings.map_stringPars.end()) settings.map_stringPars["ReadinAxisName"] = "MergedAxis";
+  if(settings.map_stringPars.find("OutputClusName")==settings.map_stringPars.end()) settings.map_stringPars["OutputClusName"] = "ESHalfCluster";
+  if(settings.map_stringPars.find("OutputTowerName")==settings.map_stringPars.end()) settings.map_stringPars["OutputTowerName"] = "ESTower";
+
+  return StatusCode::SUCCESS;
+};
+
+
+StatusCode EnergySplittingAlg::Initialize( PandoraPlusDataCol& m_datacol ){
+  m_axisUCol.clear();
+  m_axisVCol.clear();
+  m_newClusUCol.clear();
+  m_newClusVCol.clear();
+  m_1dShowerUCol.clear();
+  m_1dShowerVCol.clear();
+  m_towerCol.clear(); 
+  m_bkCol.Clear();
+
+  p_HalfClusterU.clear();
+  p_HalfClusterV.clear(); 
+
+  for(int ih=0; ih<m_datacol.map_HalfCluster["HalfClusterColU"].size(); ih++)
+    p_HalfClusterU.push_back( m_datacol.map_HalfCluster["HalfClusterColU"][ih].get() );
+  for(int ih=0; ih<m_datacol.map_HalfCluster["HalfClusterColV"].size(); ih++)
+    p_HalfClusterV.push_back( m_datacol.map_HalfCluster["HalfClusterColV"][ih].get() );
+  for(int ih=0; ih<m_datacol.map_HalfCluster["emptyHalfClusterU"].size(); ih++){
+    p_emptyHalfClusterU.push_back( m_datacol.map_HalfCluster["emptyHalfClusterU"][ih].get() );
+  }
+  for(int ih=0; ih<m_datacol.map_HalfCluster["emptyHalfClusterV"].size(); ih++)
+    p_emptyHalfClusterV.push_back( m_datacol.map_HalfCluster["emptyHalfClusterV"][ih].get() );
+
+  return StatusCode::SUCCESS;
+}
+
+
+StatusCode EnergySplittingAlg::RunAlgorithm( PandoraPlusDataCol& m_datacol ){
+
+  //Input: HalfCluster (with 1D clusters and HoughAxis)
+  //Output: Create Towers for the matching. 
+
+  if( p_HalfClusterU.size() + p_HalfClusterV.size()==0 ) {
+    if( p_emptyHalfClusterU.size() + p_emptyHalfClusterV.size() == 0 ){
+      std::cout<<"EnergySplittingAlg: No HalfCluster input"<<std::endl;
+      return StatusCode::SUCCESS;
+    }
+    m_towerCol.clear();
+    std::vector<PandoraPlus::CaloHalfCluster*> tmp_newClusUCol, tmp_newClusVCol;
+    HalfClusterToTowers( tmp_newClusUCol, tmp_newClusVCol, p_emptyHalfClusterU, p_emptyHalfClusterV, m_towerCol );
+
+    m_datacol.map_CaloCluster[settings.map_stringPars["OutputTowerName"]] = m_towerCol;
+
+    m_datacol.map_BarCol["bkBar"].insert( m_datacol.map_BarCol["bkBar"].end(), m_bkCol.map_BarCol["bkBar"].begin(), m_bkCol.map_BarCol["bkBar"].end() );
+    m_datacol.map_1DCluster["bk1DCluster"].insert( m_datacol.map_1DCluster["bk1DCluster"].end(), m_bkCol.map_1DCluster["bk1DCluster"].begin(), m_bkCol.map_1DCluster["bk1DCluster"].end() );
+    m_datacol.map_2DCluster["bk2DCluster"].insert( m_datacol.map_2DCluster["bk2DCluster"].end(), m_bkCol.map_2DCluster["bk2DCluster"].begin(), m_bkCol.map_2DCluster["bk2DCluster"].end() );
+    m_datacol.map_HalfCluster["bkHalfCluster"].insert( m_datacol.map_HalfCluster["bkHalfCluster"].end(), m_bkCol.map_HalfCluster["bkHalfCluster"].begin(), m_bkCol.map_HalfCluster["bkHalfCluster"].end() );
+
+    return StatusCode::SUCCESS;
+  }
+
+/*
+float tmp_totE_U = 0;
+for(int ih=0; ih<p_HalfClusterU.size(); ih++) tmp_totE_U += p_HalfClusterU[ih]->getEnergy();
+float tmp_totE_V = 0;
+for(int ih=0; ih<p_HalfClusterV.size(); ih++) tmp_totE_V += p_HalfClusterV[ih]->getEnergy();
+cout<<"Total energy in HFCluster: "<<tmp_totE_U<<'\t'<<tmp_totE_V<<endl;
+cout<<"Print readin axesU"<<endl;
+for(int ih=0; ih<p_HalfClusterU.size(); ih++){
+  cout<<"  HalfCluster #"<<ih<<": cluster En "<<p_HalfClusterU[ih]->getEnergy()<<endl;
+  auto tmp_axisUCol = p_HalfClusterU[ih]->getHalfClusterCol(settings.map_stringPars["ReadinAxisName"]);
+  for(int icl=0; icl<tmp_axisUCol.size(); icl++){
+    cout<<"  Axis #"<<icl<<": cluster En "<<tmp_axisUCol[icl]->getEnergy()<<", type "<<tmp_axisUCol[icl]->getType()<<endl;
+    for(auto ish : tmp_axisUCol[icl]->getCluster()){
+      printf("    Shower layer %d, Pos+E (%.3f, %.3f, %.3f, %.3f), Address %p \n", ish->getDlayer(), ish->getPos().x(), ish->getPos().y(), ish->getPos().z(), ish->getEnergy(), ish );
+    }
+  }
+}
+cout<<"Print readin axesV"<<endl;
+for(int ih=0; ih<p_HalfClusterV.size(); ih++){
+  cout<<"  HalfCluster #"<<ih<<": cluster En "<<p_HalfClusterV[ih]->getEnergy()<<endl;
+  auto tmp_axisVCol = p_HalfClusterV[ih]->getHalfClusterCol(settings.map_stringPars["ReadinAxisName"]);
+  for(int icl=0; icl<tmp_axisVCol.size(); icl++){
+    cout<<"  Axis #"<<icl<<": cluster En "<<tmp_axisVCol[icl]->getEnergy()<<", type "<<tmp_axisVCol[icl]->getType()<<endl;
+    for(auto ish : tmp_axisVCol[icl]->getCluster()){
+      printf("    Shower layer %d, Pos+E (%.3f, %.3f, %.3f, %.3f), Address %p \n", ish->getDlayer(), ish->getPos().x(), ish->getPos().y(), ish->getPos().z(), ish->getEnergy(), ish );
+    }
+  }
+}
+
+
+cout<<"Readin empty half cluster size "<<p_emptyHalfClusterU.size()<<", "<<p_emptyHalfClusterV.size()<<endl;
+tmp_totE_U = 0;
+for(int ih=0; ih<p_emptyHalfClusterU.size(); ih++) tmp_totE_U += p_emptyHalfClusterU[ih]->getEnergy();
+tmp_totE_V = 0;
+for(int ih=0; ih<p_emptyHalfClusterV.size(); ih++) tmp_totE_V += p_emptyHalfClusterV[ih]->getEnergy();
+cout<<"Empty half cluster energy "<<tmp_totE_U<<", "<<tmp_totE_V<<endl;
+*/
+
+  //Start in U direction. 
+  m_newClusUCol.clear();
+  for(int ih=0; ih<p_HalfClusterU.size(); ih++){
+
+    //Get all axis in U cluster.
+    m_axisUCol.clear();
+    if( settings.map_stringPars["ReadinAxisName"] == "AllAxis" ) m_axisUCol = p_HalfClusterU[ih]->getAllHalfClusterCol(); 
+    else m_axisUCol = p_HalfClusterU[ih]->getHalfClusterCol(settings.map_stringPars["ReadinAxisName"]);
+   
+    //  Loop for 1DClusters.
+    m_1dShowerUCol.clear();
+    std::vector<const PandoraPlus::Calo1DCluster*> m_1dclusCol = p_HalfClusterU[ih]->getCluster();
+    for(int icl=0; icl<m_1dclusCol.size(); icl++){
+      std::vector<const PandoraPlus::CaloUnit*> m_bars = m_1dclusCol[icl]->getBars();
+
+      //Find the seed with axis in 1DCluster: 
+      for(auto iaxis: m_axisUCol){
+        for(auto iseed: iaxis->getCluster() ){
+
+          if(iseed->getBars().size()!=1) {
+            std::cout<<"WARNING: Axis has more than one bars! Check!"<<'\t'<<iseed->getBars().size()<<std::endl;
+            continue;
+          }
+
+          bool findSeed = false;
+          if(find( m_bars.begin(), m_bars.end(), iseed->getBars()[0]) != m_bars.end()) findSeed = true;
+          for(int ib=0; ib<m_bars.size(); ib++){
+            if(findSeed) break;
+            if(m_bars[ib]->getcellID()==iseed->getBars()[0]->getcellID()) {findSeed=true; break; }
+          }
+          if(findSeed) const_cast<PandoraPlus::Calo1DCluster*>(m_1dclusCol[icl])->addSeed( iseed->getBars()[0] );
+
+        }
+      }
+      //if(m_1dclusCol[icl]->getNseeds()==0) const_cast<PandoraPlus::Calo1DCluster*>(m_1dclusCol[icl])->setSeed();   
+
+      //Split cluster to showers
+      std::vector<std::shared_ptr<PandoraPlus::Calo1DCluster>> tmp_showers; tmp_showers.clear();
+      ClusterSplitting( m_1dclusCol[icl], tmp_showers );
+      //if(tmp_showers.size()==0) continue;
+      m_1dShowerUCol.insert( m_1dShowerUCol.end(), tmp_showers.begin(), tmp_showers.end() );
+      tmp_showers.clear();
+    }
+
+//double tmp_En = 0.;
+//for(int i1d=0; i1d<m_1dShowerUCol.size(); i1d++) tmp_En += m_1dShowerUCol[i1d]->getEnergy();
+//cout<<"  HalfClusterU #"<<ih<<": total En after energy splitting "<<tmp_En<<endl;
+
+    //Clean showers without seed.
+    for(int ic=0; ic<m_1dShowerUCol.size(); ic++){
+      if(m_1dShowerUCol[ic]->getNseeds()==0){
+        if(MergeToClosestCluster( m_1dShowerUCol[ic].get(), m_1dShowerUCol )==StatusCode::SUCCESS){
+          m_1dShowerUCol.erase(m_1dShowerUCol.begin()+ic);
+          ic--;
+        }
+        else
+          m_1dShowerUCol[ic]->setSeed();
+      }
+    }
+    for(int i1d=0; i1d<m_1dShowerUCol.size(); i1d++) m_1dShowerUCol[i1d]->getLinkedMCPfromUnit();
+    m_datacol.map_1DCluster["bk1DCluster"].insert( m_datacol.map_1DCluster["bk1DCluster"].end(), m_1dShowerUCol.begin(), m_1dShowerUCol.end() );
+
+//tmp_En = 0.;
+//for(int i1d=0; i1d<m_1dShowerUCol.size(); i1d++) tmp_En += m_1dShowerUCol[i1d]->getEnergy();
+//cout<<"  HalfClusterU #"<<ih<<": total En after shower cleaning "<<tmp_En<<endl;
+
+    //  Longitudinal linking: update clusters' energy.
+    std::vector<std::shared_ptr<PandoraPlus::CaloHalfCluster>> tmp_newClus; tmp_newClus.clear();
+    LongitudinalLinking(m_1dShowerUCol, m_axisUCol, tmp_newClus);
+    m_newClusUCol.insert(m_newClusUCol.end(), tmp_newClus.begin(), tmp_newClus.end());
+
+//double tmp_En = 0.;
+//for(int acl=0; acl<tmp_newClus.size(); acl++) tmp_En += tmp_newClus[acl]->getEnergy();
+//cout<<"  HalfClusterU #"<<ih<<": total En after LongiLinking "<<tmp_En<<endl;
+    tmp_newClus.clear();
+  }
+
+  //Start in V direction
+  m_newClusVCol.clear();
+  for(int ih=0; ih<p_HalfClusterV.size(); ih++){
+    m_axisVCol.clear();
+//cout<<"Readin axis in V: "<<settings.map_stringPars["ReadinAxisName"]<<endl;
+    if( settings.map_stringPars["ReadinAxisName"] == "AllAxis" ) m_axisVCol = p_HalfClusterV[ih]->getAllHalfClusterCol(); 
+    else m_axisVCol = p_HalfClusterV[ih]->getHalfClusterCol(settings.map_stringPars["ReadinAxisName"]);
+
+    m_1dShowerVCol.clear();
+    std::vector<const PandoraPlus::Calo1DCluster*> m_1dclusCol = p_HalfClusterV[ih]->getCluster();
+
+//cout<<"1D Cluster size: "<<m_1dclusCol.size()<<", axis size: "<<m_axisVCol.size()<<endl;
+    for(int icl=0; icl<m_1dclusCol.size(); icl++){
+      std::vector<const PandoraPlus::CaloUnit*> m_bars = m_1dclusCol[icl]->getBars();
+//cout<<"  In 1DCluster #"<<icl<<": layer = "<<m_1dclusCol[icl]->getDlayer();   
+    
+      //Find the seed with axis in 1DCluster:
+      for(auto iaxis: m_axisVCol){
+        for(auto iseed: iaxis->getCluster() ){
+          if(iseed->getBars().size()!=1) {
+            std::cout<<"WARNING: Axis has more than one bars! Check!"<<'\t'<<iseed->getBars().size()<<std::endl;
+            continue;
+          }
+          if(find( m_bars.begin(), m_bars.end(), iseed->getBars()[0]) != m_bars.end())
+            const_cast<PandoraPlus::Calo1DCluster*>(m_1dclusCol[icl])->addSeed( iseed->getBars()[0] );
+        }
+      }
+//cout<<", matched seed size "<<m_1dclusCol[icl]->getNseeds()<<endl;
+      //if(m_1dclusCol[icl]->getNseeds()==0) const_cast<PandoraPlus::Calo1DCluster*>(m_1dclusCol[icl])->setSeed();   
+
+      //Split cluster to showers
+      std::vector<std::shared_ptr<PandoraPlus::Calo1DCluster>> tmp_showers; tmp_showers.clear();
+      ClusterSplitting( m_1dclusCol[icl], tmp_showers );
+//cout<<"  In 1DCluster #"<<icl<<": splitted into "<<tmp_showers.size ()<<" showers: ";
+//for(int is=0; is<tmp_showers.size(); is++){
+//  printf(" (%.3f, %.3f, %.3f, %.3f) \t", tmp_showers[is].get()->getPos().x(), tmp_showers[is].get()->getPos().y(), tmp_showers[is].get()->getPos().z(), tmp_showers[is].get()->getEnergy());
+//}
+//cout<<endl;
+//cout<<endl;
+      //if(tmp_showers.size()==0) continue;
+      m_1dShowerVCol.insert( m_1dShowerVCol.end(), tmp_showers.begin(), tmp_showers.end() );
+      tmp_showers.clear();
+    }
+
+//double tmp_En = 0.;
+//for(int i1d=0; i1d<m_1dShowerVCol.size(); i1d++) tmp_En += m_1dShowerVCol[i1d]->getEnergy();
+//cout<<"  HalfClusterV #"<<ih<<": total En after energy splitting "<<tmp_En<<endl;
+
+    //Clean showers without seed.
+    for(int ic=0; ic<m_1dShowerVCol.size(); ic++){
+      if(m_1dShowerVCol[ic]->getNseeds()==0){
+        if(MergeToClosestCluster( m_1dShowerVCol[ic].get(), m_1dShowerVCol )==StatusCode::SUCCESS){
+          m_1dShowerVCol.erase(m_1dShowerVCol.begin()+ic);
+          ic--;
+        }
+        else
+          m_1dShowerVCol[ic]->setSeed();
+      }
+    }
+
+//cout<<"After splitting: print all 1D showers"<<endl;
+//for(int is=0; is<m_1dShowerVCol.size(); is++){
+//printf("  Shower layer %d, Pos+E (%.3f, %.3f, %.3f, %.3f), Nbars %d, NSeed %d, address %p \n", m_1dShowerVCol[is].get()->getDlayer(), m_1dShowerVCol[is].get()->getPos().x(), m_1dShowerVCol[is].get()->getPos().y(), m_1dShowerVCol[is].get()->getPos().z(), m_1dShowerVCol[is].get()->getEnergy(), m_1dShowerVCol[is].get()->getBars().size(), m_1dShowerVCol[is].get()->getNseeds(), m_1dShowerVCol[is].get() );
+//}
+//cout<<endl;
+//tmp_En = 0.;
+//for(int i1d=0; i1d<m_1dShowerVCol.size(); i1d++) tmp_En += m_1dShowerVCol[i1d]->getEnergy();
+//cout<<"  HalfClusterV #"<<ih<<": total En after shower cleaning "<<tmp_En<<endl;
+
+    for(int i1d=0; i1d<m_1dShowerVCol.size(); i1d++) m_1dShowerVCol[i1d]->getLinkedMCPfromUnit();
+    m_datacol.map_1DCluster["bk1DCluster"].insert( m_datacol.map_1DCluster["bk1DCluster"].end(), m_1dShowerVCol.begin(), m_1dShowerVCol.end() );
+
+
+    //  Longitudinal linking: update clusters' energy.
+    std::vector<std::shared_ptr<CaloHalfCluster>> tmp_newClus; tmp_newClus.clear();
+    LongitudinalLinking(m_1dShowerVCol, m_axisVCol, tmp_newClus);
+    m_newClusVCol.insert(m_newClusVCol.end(), tmp_newClus.begin(), tmp_newClus.end());
+//double tmp_En = 0.;
+//for(int acl=0; acl<tmp_newClus.size(); acl++) tmp_En += tmp_newClus[acl]->getEnergy();
+//cout<<"  HalfClusterV #"<<ih<<": total En after LongiLinking "<<tmp_En<<endl;
+    tmp_newClus.clear();
+  }
+
+
+  //Assign MCtruth info to the HalfCluster. 
+  for(int ic=0; ic<m_newClusUCol.size(); ic++) m_newClusUCol[ic].get()->getLinkedMCPfromUnit();
+  for(int ic=0; ic<m_newClusVCol.size(); ic++) m_newClusVCol[ic].get()->getLinkedMCPfromUnit();
+
+  m_datacol.map_HalfCluster[settings.map_stringPars["OutputClusName"]+"U"] = m_newClusUCol; 
+  m_datacol.map_HalfCluster[settings.map_stringPars["OutputClusName"]+"V"] = m_newClusVCol; 
+
+/*
+cout<<"  After splitting: HalfCluster U size "<<m_newClusUCol.size()<<", print check"<<endl;
+for(int icl=0; icl<m_newClusUCol.size(); icl++){
+  cout<<"      In HFClusU #"<<icl<<": shower size = "<<m_newClusUCol[icl]->getCluster().size()<<endl;
+  for(auto ish : m_newClusUCol[icl]->getCluster()){
+    printf("          Shower layer %d, Pos+E (%.3f, %.3f, %.3f, %.3f), Nbars %d, NSeed %d, MC map size %d \n", ish->getDlayer(), ish->getPos().x(), ish->getPos().y(), ish->getPos().z(), ish->getEnergy(), ish->getBars().size(),  ish->getNseeds(), ish->getLinkedMCP().size() );
+    //printf(", cover tower size: %d: ", ish->getTowerID().size());
+    //for(int atw=0; atw<ish->getTowerID().size(); atw++) printf("[%d, %d, %d], ", ish->getTowerID()[atw][0], ish->getTowerID()[atw][1], ish->getTowerID()[atw][2] );
+    //cout<<endl;
+  }
+}
+cout<<endl;
+
+cout<<"  After splitting: HalfCluster V size "<<m_newClusVCol.size()<<", print check"<<endl;
+for(int icl=0; icl<m_newClusVCol.size(); icl++){
+  cout<<"      In HFClusV #"<<icl<<": shower size = "<<m_newClusVCol[icl]->getCluster().size()<<endl;
+  for(auto ish : m_newClusVCol[icl]->getCluster()){
+    printf("          Shower layer %d, Pos+E (%.3f, %.3f, %.3f, %.3f), Nbars %d, NSeed %d, MC map size %d \n", ish->getDlayer(), ish->getPos().x(), ish->getPos().y(), ish->getPos().z(), ish->getEnergy(), ish->getBars().size(),  ish->getNseeds(), ish->getLinkedMCP().size() );
+    //printf(", cover tower size: %d: ", ish->getTowerID().size());
+    //for(int atw=0; atw<ish->getTowerID().size(); atw++) printf("[%d, %d, %d], ", ish->getTowerID()[atw][0], ish->getTowerID()[atw][1], ish->getTowerID()[atw][2] );
+    //cout<<endl;
+  }
+}
+
+
+float totE_U = 0.;
+float totE_V = 0.;
+for(auto iter : m_newClusUCol) totE_U += iter->getEnergy();
+for(auto iter : m_newClusVCol) totE_V += iter->getEnergy();
+printf("    Before split to tower: HalfCluster size: (%d, %d), energy (%.3f, %.3f) \n", m_newClusUCol.size(), m_newClusVCol.size(), totE_U, totE_V );
+cout<<"    Loop print HalfClusterU: "<<endl;
+for(int icl=0; icl<m_newClusUCol.size(); icl++){
+  cout<<"      In HFClusU #"<<icl<<": shower size = "<<m_newClusUCol[icl]->getCluster().size()<<", En = "<<m_newClusUCol[icl]->getEnergy()<<", type "<<m_newClusUCol[icl]->getType();
+  printf(", Position (%.3f, %.3f, %.3f), address %p ",m_newClusUCol[icl]->getPos().x(), m_newClusUCol[icl]->getPos().y(), m_newClusUCol[icl]->getPos().z(), m_newClusUCol[icl]);
+  printf(", cousin size %d, address: ", m_newClusUCol[icl]->getHalfClusterCol("CousinCluster").size());
+  for(int ics=0; ics<m_newClusUCol[icl]->getHalfClusterCol("CousinCluster").size(); ics++) printf("%p, ", m_newClusUCol[icl]->getHalfClusterCol("CousinCluster")[ics]);
+  printf(", track size %d, address: ", m_newClusUCol[icl]->getAssociatedTracks().size());
+  for(int itrk=0; itrk<m_newClusUCol[icl]->getAssociatedTracks().size(); itrk++) printf("%p, ", m_newClusUCol[icl]->getAssociatedTracks()[itrk]);
+  cout<<endl;
+  for(auto ish : m_newClusUCol[icl]->getCluster()){
+    printf("          Shower layer %d, Pos+E (%.3f, %.3f, %.3f, %.3f), Nbars %d, NSeed %d, seedID [%d, %d, %d, %d], Address %p \n", ish->getDlayer(), ish->getPos().x(), ish->getPos().y(), ish->getPos().z(), ish->getEnergy(), ish->getBars().size(),  ish->getNseeds(), ish->getSeeds()[0]->getModule(), ish->getSeeds()[0]->getPart(), ish->getSeeds()[0]->getStave(), ish->getSeeds()[0]->getBar(), ish );
+  }
+}
+cout<<endl;
+
+cout<<"    Loop print HalfClusterV: "<<endl;
+for(int icl=0; icl<m_newClusVCol.size(); icl++){
+  cout<<"      In HFClusV #"<<icl<<": shower size = "<<m_newClusVCol[icl]->getCluster().size()<<", En = "<<m_newClusVCol[icl]->getEnergy()<<", type "<<m_newClusVCol[icl]->getType();
+  printf(", Position (%.3f, %.3f, %.3f), address %p ",m_newClusVCol[icl]->getPos().x(), m_newClusVCol[icl]->getPos().y(), m_newClusVCol[icl]->getPos().z(), m_newClusVCol[icl]);
+  printf(", cousin size %d, address: ", m_newClusVCol[icl]->getHalfClusterCol("CousinCluster").size());
+  for(int ics=0; ics<m_newClusVCol[icl]->getHalfClusterCol("CousinCluster").size(); ics++) printf("%p, ", m_newClusVCol[icl]->getHalfClusterCol("CousinCluster")[ics]);
+  printf(", track size %d, address: ", m_newClusVCol[icl]->getAssociatedTracks().size());
+  for(int itrk=0; itrk<m_newClusVCol[icl]->getAssociatedTracks().size(); itrk++) printf("%p, ", m_newClusVCol[icl]->getAssociatedTracks()[itrk]);
+  cout<<endl;
+  for(auto ish : m_newClusVCol[icl]->getCluster()){
+    printf("          Shower layer %d, Pos+E (%.3f, %.3f, %.3f, %.3f), Nbars %d, NSeed %d, seedID [%d, %d, %d, %d], Address %p \n", ish->getDlayer(), ish->getPos().x(), ish->getPos().y(), ish->getPos().z(), ish->getEnergy(), ish->getBars().size(), ish->getNseeds(), ish->getSeeds()[0]->getModule(), ish->getSeeds()[0]->getPart(), ish->getSeeds()[0]->getStave(), ish->getSeeds()[0]->getBar(), ish );
+  }
+}
+cout<<endl;
+*/
+
+
+  //Make tower 
+  m_towerCol.clear(); 
+  std::vector<PandoraPlus::CaloHalfCluster*> tmp_newClusUCol, tmp_newClusVCol;
+  tmp_newClusUCol.clear(); tmp_newClusVCol.clear();
+  for(int icl=0; icl<m_newClusUCol.size(); icl++) tmp_newClusUCol.push_back( m_newClusUCol[icl].get() );
+  for(int icl=0; icl<m_newClusVCol.size(); icl++) tmp_newClusVCol.push_back( m_newClusVCol[icl].get() );
+  HalfClusterToTowers( tmp_newClusUCol, tmp_newClusVCol, p_emptyHalfClusterU, p_emptyHalfClusterV, m_towerCol );
+
+  m_datacol.map_CaloCluster[settings.map_stringPars["OutputTowerName"]] = m_towerCol;
+
+  m_datacol.map_BarCol["bkBar"].insert( m_datacol.map_BarCol["bkBar"].end(), m_bkCol.map_BarCol["bkBar"].begin(), m_bkCol.map_BarCol["bkBar"].end() );
+  m_datacol.map_1DCluster["bk1DCluster"].insert( m_datacol.map_1DCluster["bk1DCluster"].end(), m_bkCol.map_1DCluster["bk1DCluster"].begin(), m_bkCol.map_1DCluster["bk1DCluster"].end() );
+  m_datacol.map_2DCluster["bk2DCluster"].insert( m_datacol.map_2DCluster["bk2DCluster"].end(), m_bkCol.map_2DCluster["bk2DCluster"].begin(), m_bkCol.map_2DCluster["bk2DCluster"].end() );
+  m_datacol.map_HalfCluster["bkHalfCluster"].insert( m_datacol.map_HalfCluster["bkHalfCluster"].end(), m_bkCol.map_HalfCluster["bkHalfCluster"].begin(), m_bkCol.map_HalfCluster["bkHalfCluster"].end() );
+
+  return StatusCode::SUCCESS;
+}
+
+
+StatusCode EnergySplittingAlg::ClearAlgorithm(){
+  p_HalfClusterU.clear();
+  p_HalfClusterV.clear();
+  p_emptyHalfClusterU.clear();
+  p_emptyHalfClusterV.clear();
+
+  m_axisUCol.clear();
+  m_axisVCol.clear();
+
+  m_newClusUCol.clear();
+  m_newClusVCol.clear();  
+  m_1dShowerUCol.clear();
+  m_1dShowerVCol.clear();
+  m_towerCol.clear(); 
+  m_bkCol.Clear();
+
+  return StatusCode::SUCCESS;
+}
+
+
+StatusCode EnergySplittingAlg::LongitudinalLinking( std::vector<std::shared_ptr<PandoraPlus::Calo1DCluster>>& m_showers,
+                                                    std::vector<const PandoraPlus::CaloHalfCluster*>& m_oldClusCol,
+                                                    std::vector<std::shared_ptr<PandoraPlus::CaloHalfCluster>>& m_newClusCol )
+
+{
+  if(m_showers.size()==0 || m_oldClusCol.size()==0) return StatusCode::SUCCESS;
+  m_newClusCol.clear();
+
+//double tmp_totE = 0;
+//for(int i=0; i<m_showers.size(); i++) tmp_totE += m_showers[i]->getEnergy();
+//cout<<"  In LongitudinalLinking: input 1DShower total energy : "<<tmp_totE<<". old axis size "<<m_oldClusCol.size()<<endl;
+//cout<<"  Print all 1DShower (address)"<<endl;
+//for(int i=0; i<m_showers.size(); i++){
+//  printf("    Shower #%d, pos+E [%.3f, %.3f, %.3f, %.3f], address %p \n", i, m_showers[i]->getPos().x(), m_showers[i]->getPos().y(), m_showers[i]->getPos().z(), m_showers[i]->getEnergy(), m_showers[i] );
+//}
+
+  //Update old Hough clusters
+  for(int ic=0; ic<m_oldClusCol.size(); ic++){
+    std::shared_ptr<PandoraPlus::CaloHalfCluster> m_newClus = std::make_shared<PandoraPlus::CaloHalfCluster>();
+
+    for(int is=0; is<m_oldClusCol[ic]->getCluster().size(); is++){
+      const PandoraPlus::Calo1DCluster* m_shower = m_oldClusCol[ic]->getCluster()[is];
+
+      const PandoraPlus::Calo1DCluster* m_selshower = NULL;
+      bool fl_foundshower = false; 
+      for(int js=0; js<m_showers.size(); js++){
+        bool fl_inTower = false;
+        for(int itw=0; itw<m_showers[js].get()->getTowerID().size() && !fl_inTower; itw++){
+        for(int jtw=0; jtw<m_shower->getTowerID().size(); jtw++){
+          if(m_showers[js].get()->getTowerID()[itw]==m_shower->getTowerID()[jtw]) {fl_inTower=true; break;}
+        }}
+
+        if( fl_inTower &&
+            m_showers[js].get()->getDlayer() == m_shower->getDlayer() && 
+            m_showers[js].get()->getSeeds().size()==1 && 
+            (m_showers[js].get()->getSeeds()[0]->getPosition()-m_shower->getPos()).Mag()<10 ) 
+        {m_selshower = m_showers[js].get(); fl_foundshower=true; break; }
+      }
+      if(fl_foundshower && m_selshower!=NULL) m_newClus->addUnit( m_selshower );
+    }
+
+    for(int itrk=0; itrk<m_oldClusCol[ic]->getAssociatedTracks().size(); itrk++) m_newClus->addAssociatedTrack( m_oldClusCol[ic]->getAssociatedTracks()[itrk] );
+    m_newClus->setType( m_oldClusCol[ic]->getType() );
+    m_newClus->setHoughPars(m_oldClusCol[ic]->getHoughAlpha(), m_oldClusCol[ic]->getHoughRho() );
+    m_newClus->setIntercept(m_oldClusCol[ic]->getHoughIntercept());
+    m_newClus->fitAxis("");
+    m_newClus->addHalfCluster(settings.map_stringPars["ReadinAxisName"], m_oldClusCol[ic]);
+    m_newClusCol.push_back( m_newClus );
+  }
+
+//tmp_totE = 0.;
+//for(int i=0; i<m_newClusCol.size(); i++) tmp_totE += m_newClusCol[i]->getEnergy();
+//cout<<"  In Longi-Linking: raw new HFCluster size : "<<m_newClusCol.size()<<", totE "<<tmp_totE<<endl;
+/*
+cout<<"  Print 1DShower in new HFCluster"<<endl;
+for(int ih=0; ih<m_newClusCol.size(); ih++){
+  cout<<"    In new HFCluster #"<<ih<<endl;
+  for(int i=0; i<m_newClusCol[ih]->getCluster().size(); i++){
+    printf("      Shower #%d, pos+E [%.3f, %.3f, %.3f, %.3f], address %p \n", i, m_newClusCol[ih]->getCluster()[i]->getPos().x(), m_newClusCol[ih]->getCluster()[i]->getPos().y(), m_newClusCol[ih]->getCluster()[i]->getPos().z(), m_newClusCol[ih]->getCluster()[i]->getEnergy(), m_newClusCol[ih]->getCluster()[i] );
+  }
+}
+*/
+
+  std::vector<const PandoraPlus::Calo1DCluster*> m_leftshowers; m_leftshowers.clear(); 
+  for(int ish=0; ish<m_showers.size(); ish++){
+    bool fl_inclus = false; 
+    for(int icl=0; icl<m_newClusCol.size(); icl++){
+      std::vector<const Calo1DCluster*> p_showerCol = m_newClusCol[icl]->getCluster();
+      if( find(p_showerCol.begin(), p_showerCol.end(), m_showers[ish].get())!=p_showerCol.end() ){ fl_inclus=true; break; }
+    }
+
+    if(!fl_inclus) m_leftshowers.push_back( m_showers[ish].get() );
+  }
+
+//tmp_totE = 0.;
+//for(int i=0; i<m_leftshowers.size(); i++) tmp_totE += m_leftshowers[i]->getEnergy();
+//cout<<"  In Longi-Linking: left 1D showers size: "<<m_leftshowers.size()<<", totE "<<tmp_totE<<endl;
+//cout<<"  Print left 1DShower (address)"<<endl;
+//for(int i=0; i<m_leftshowers.size(); i++){
+//  printf("    Shower #%d, pos+E [%.3f, %.3f, %.3f, %.3f], address %p \n", i, m_leftshowers[i]->getPos().x(), m_leftshowers[i]->getPos().y(), m_leftshowers[i]->getPos().z(), m_leftshowers[i]->getEnergy(), m_leftshowers[i] );
+//}
+
+
+  //Merge showers into closest Half cluster
+  std::sort( m_leftshowers.begin(), m_leftshowers.end(), compLayer );
+  for(int is=0; is<m_leftshowers.size(); is++) 
+    MergeToClosestCluster( m_leftshowers[is], m_newClusCol );
+
+//tmp_totE = 0.;
+//for(int i=0; i<m_newClusCol.size(); i++) tmp_totE += m_newClusCol[i]->getEnergy();
+//cout<<"  In Longi-Linking: after merge left showers: HFCluster size "<<m_newClusCol.size()<<", totE "<<tmp_totE<<endl;
+
+ //Split the double-used 1DClusters  
+  SplitOverlapCluster(m_newClusCol);
+  for(int is=0; is<m_newClusCol.size(); is++) m_newClusCol[is]->sortBarShowersByLayer();
+
+  //Merge showers in the same layer
+  if(settings.map_boolPars["CompactHFCluster"]){
+    for(int icl=0; icl<m_newClusCol.size(); icl++) 
+      m_newClusCol[icl].get()->mergeClusterInLayer();
+  }
+/*
+cout<<"  Print 1DShower in new HFCluster"<<endl;
+for(int ih=0; ih<m_newClusCol.size(); ih++){
+  cout<<"    In new HFCluster #"<<ih<<endl;
+  for(int i=0; i<m_newClusCol[ih]->getCluster().size(); i++){
+    printf("      Shower #%d, pos+E [%.3f, %.3f, %.3f, %.3f], address %p \n", i, m_newClusCol[ih]->getCluster()[i]->getPos().x(), m_newClusCol[ih]->getCluster()[i]->getPos().y(), m_newClusCol[ih]->getCluster()[i]->getPos().z(), m_newClusCol[ih]->getCluster()[i]->getEnergy(), m_newClusCol[ih]->getCluster()[i] );
+  }
+}
+*/
+  return StatusCode::SUCCESS;
+}
+
+
+StatusCode EnergySplittingAlg::HalfClusterToTowers( std::vector<PandoraPlus::CaloHalfCluster*>& m_halfClusU,
+                                                    std::vector<PandoraPlus::CaloHalfCluster*>& m_halfClusV,
+                                                    std::vector<PandoraPlus::CaloHalfCluster*>& m_emptyClusU,
+                                                    std::vector<PandoraPlus::CaloHalfCluster*>& m_emptyClusV,
+                                                    std::vector<std::shared_ptr<PandoraPlus::Calo3DCluster>>& m_towers )
+{
+//cout<<"  HalfClusterToTowers: Input halfcluster size "<<m_halfClusU.size()<<", "<<m_halfClusV.size()<<endl;
+//cout<<"    Input empty cluster size "<<m_emptyClusU.size()<<", "<<m_emptyClusV.size()<<endl;
+
+  m_towers.clear(); 
+
+  std::map<std::vector<int>, std::vector<const PandoraPlus::Calo2DCluster*> > map_2DCluster;
+  std::map<std::vector<int>, std::vector<PandoraPlus::CaloHalfCluster*> > map_HalfClusterU; 
+  std::map<std::vector<int>, std::vector<PandoraPlus::CaloHalfCluster*> > map_HalfClusterV; 
+
+  std::map<std::vector<int>, std::vector<PandoraPlus::CaloHalfCluster*> > map_emptyHalfClusterU;
+  std::map<std::vector<int>, std::vector<PandoraPlus::CaloHalfCluster*> > map_emptyHalfClusterV;  
+
+  //Assign the empty clusters into tower
+  for(int il=0; il<m_emptyClusU.size(); il++){
+//cout<<"  Empty clusterU #"<<il<<": towerID size "<<m_emptyClusU[il]->getTowerID().size()<<endl;
+    map_emptyHalfClusterU[m_emptyClusU[il]->getTowerID()[0]].push_back(m_emptyClusU[il]);
+  }
+  for(int il=0; il<m_emptyClusV.size(); il++){
+//cout<<"  Empty clusterV #"<<il<<": towerID size "<<m_emptyClusV[il]->getTowerID().size()<<endl;
+    map_emptyHalfClusterV[m_emptyClusV[il]->getTowerID()[0]].push_back(m_emptyClusV[il]);
+  }
+
+
+  //Split CaloHalfClusterU
+  for(int il=0; il<m_halfClusU.size(); il++){
+    if(m_halfClusU[il]->getCluster().size()==0) {std::cout<<"WARNING: Have an empty CaloHalfCluster! Skip it! "<<std::endl; continue;}
+
+    //HalfCluster does not cover tower: 
+    if( m_halfClusU[il]->getTowerID().size()==1 && m_halfClusU[il]->getCluster().size()>=settings.map_intPars["th_Nhit"]){
+      std::vector<int> cl_towerID =  m_halfClusU[il]->getTowerID()[0]; 
+      if(settings.map_boolPars["CompactHFCluster"]) m_halfClusU[il]->mergeClusterInLayer();
+      map_HalfClusterU[cl_towerID].push_back(m_halfClusU[il]);
+      continue; 
+    }
+
+    //CaloHalfCluster covers towers: Loop check showers. 
+    std::map<std::vector<int>, PandoraPlus::CaloHalfCluster* > tmp_LongiClusMaps; tmp_LongiClusMaps.clear();
+    for(int is=0; is<m_halfClusU[il]->getCluster().size(); is++){
+      const PandoraPlus::Calo1DCluster* p_shower = m_halfClusU[il]->getCluster()[is];
+
+      std::map<std::vector<int>, PandoraPlus::Calo1DCluster* > tmp_1DClusMaps; tmp_1DClusMaps.clear();
+      for(int ib=0; ib<p_shower->getBars().size(); ib++){
+        std::vector<int> towerID(2);
+        towerID[0] = p_shower->getBars()[ib]->getModule();
+        towerID[1] = p_shower->getBars()[ib]->getStave();        
+
+        if(tmp_1DClusMaps.find(towerID)!=tmp_1DClusMaps.end()){
+          tmp_1DClusMaps[towerID]->addUnit(p_shower->getBars()[ib]);
+          tmp_1DClusMaps[towerID]->setIDInfo();
+        }
+        else{
+          std::shared_ptr<PandoraPlus::Calo1DCluster> tmp_clus = std::make_shared<PandoraPlus::Calo1DCluster>();
+          tmp_clus->addUnit( p_shower->getBars()[ib] );
+          tmp_clus->setIDInfo();
+          tmp_1DClusMaps[towerID] = tmp_clus.get();
+          m_bkCol.map_1DCluster["bk1DCluster"].push_back( tmp_clus );
+        }
+      }
+
+      if(tmp_1DClusMaps.size()>1){
+        for(auto &iter : tmp_1DClusMaps){
+          for(auto &iter1 : tmp_1DClusMaps){
+            if(iter!= iter1) iter.second->addCousinCluster(iter1.second);
+          }
+        }
+      }
+
+      for(auto &iter: tmp_1DClusMaps){
+        std::vector<int> towerID = iter.first;
+        iter.second->setSeed();
+        iter.second->setIDInfo();
+        iter.second->getLinkedMCPfromUnit();
+
+        if( tmp_LongiClusMaps.find( towerID )!=tmp_LongiClusMaps.end() ){
+          tmp_LongiClusMaps[towerID]->addUnit( iter.second );
+          tmp_LongiClusMaps[towerID]->setTowerID( towerID );
+        }
+        else{
+          std::shared_ptr<PandoraPlus::CaloHalfCluster> tmp_clus = std::make_shared<PandoraPlus::CaloHalfCluster>();
+          tmp_clus->addUnit( iter.second );
+          tmp_clus->setTowerID( towerID );
+          tmp_LongiClusMaps[towerID] = tmp_clus.get();
+          m_bkCol.map_HalfCluster["bkHalfCluster"].push_back( tmp_clus );
+        }        
+      }
+
+      p_shower = nullptr;
+    }
+
+    //Connect cousins
+    if(tmp_LongiClusMaps.size()>1){
+      for(auto &iter : tmp_LongiClusMaps){
+        for(auto &iter1 : tmp_LongiClusMaps){
+          if(iter!= iter1 && 
+             iter.second->getEnergy()>settings.map_floatPars["Eth_HFClus"] &&
+             iter1.second->getEnergy()>settings.map_floatPars["Eth_HFClus"] &&
+             iter.second->getCluster().size()>=settings.map_intPars["th_Nhit"] && 
+             iter1.second->getCluster().size()>=settings.map_intPars["th_Nhit"] ){ iter.second->addCousinCluster(iter1.second); }
+        }
+      }
+    }
+    for(auto &iter : tmp_LongiClusMaps){
+      iter.second->setType(m_halfClusU[il]->getType());
+      if(m_halfClusU[il]->getHalfClusterCol(settings.map_stringPars["ReadinAxisName"]).size()>0 ) 
+        iter.second->addHalfCluster(settings.map_stringPars["ReadinAxisName"], m_halfClusU[il]->getHalfClusterCol(settings.map_stringPars["ReadinAxisName"])[0]);
+
+      if(iter.second->getEnergy()<settings.map_floatPars["Eth_HFClus"]) continue;
+      iter.second->addHalfCluster("ParentCluster", m_halfClusU[il]);
+      for(int itrk=0; itrk<m_halfClusU[il]->getAssociatedTracks().size(); itrk++) 
+        iter.second->addAssociatedTrack( m_halfClusU[il]->getAssociatedTracks()[itrk] );
+      if(iter.second->getCluster().size()>=settings.map_intPars["th_Nhit"]){
+        if(settings.map_boolPars["CompactHFCluster"]){ 
+          iter.second->mergeClusterInLayer();
+          iter.second->setTowerID(iter.first);
+        }
+        map_HalfClusterU[iter.first].push_back(iter.second);
+      }
+    }
+
+  }
+
+
+  //Split CaloHalfClusterV
+  for(int il=0; il<m_halfClusV.size(); il++){
+    if(m_halfClusV[il]->getCluster().size()==0) {std::cout<<"WARNING: Have an empty CaloHalfCluster! Skip it! "<<std::endl; continue;}
+
+    //HalfCluster does not cover tower:
+    if( m_halfClusV[il]->getTowerID().size()==1 && m_halfClusV[il]->getCluster().size()>=settings.map_intPars["th_Nhit"]){
+      std::vector<int> cl_towerID =  m_halfClusV[il]->getTowerID()[0];
+      if(settings.map_boolPars["CompactHFCluster"]) m_halfClusV[il]->mergeClusterInLayer();
+      map_HalfClusterV[cl_towerID].push_back(m_halfClusV[il]);
+      continue;
+    }
+
+    //CaloHalfCluster covers towers: Loop check showers.
+    std::map<std::vector<int>, PandoraPlus::CaloHalfCluster* > tmp_LongiClusMaps; tmp_LongiClusMaps.clear();
+    for(int is=0; is<m_halfClusV[il]->getCluster().size(); is++){
+      const PandoraPlus::Calo1DCluster* p_shower = m_halfClusV[il]->getCluster()[is];
+
+      std::map<std::vector<int>, PandoraPlus::Calo1DCluster* > tmp_1DClusMaps; tmp_1DClusMaps.clear();
+      for(int ib=0; ib<p_shower->getBars().size(); ib++){
+        std::vector<int> towerID(2);
+        towerID[0] = p_shower->getBars()[ib]->getModule();
+        towerID[1] = p_shower->getBars()[ib]->getStave();
+
+        if(tmp_1DClusMaps.find(towerID)!=tmp_1DClusMaps.end()){
+          tmp_1DClusMaps[towerID]->addUnit(p_shower->getBars()[ib]);
+          tmp_1DClusMaps[towerID]->setIDInfo();
+        }
+        else{
+          std::shared_ptr<PandoraPlus::Calo1DCluster> tmp_clus = std::make_shared<PandoraPlus::Calo1DCluster>();
+          tmp_clus->addUnit( p_shower->getBars()[ib] );
+          tmp_clus->setIDInfo();
+          tmp_1DClusMaps[towerID] = tmp_clus.get();
+          m_bkCol.map_1DCluster["bk1DCluster"].push_back( tmp_clus );
+        }
+      }
+
+      if(tmp_1DClusMaps.size()>1){
+        for(auto &iter : tmp_1DClusMaps){
+          for(auto &iter1 : tmp_1DClusMaps){
+            if(iter!= iter1) iter.second->addCousinCluster(iter1.second);
+          }
+        }
+      }
+
+      for(auto &iter: tmp_1DClusMaps){
+        std::vector<int> towerID = iter.first;
+        iter.second->setSeed();
+        iter.second->setIDInfo();
+        iter.second->getLinkedMCPfromUnit();
+
+        if( tmp_LongiClusMaps.find( towerID )!=tmp_LongiClusMaps.end() ){
+          tmp_LongiClusMaps[towerID]->addUnit( iter.second );
+          tmp_LongiClusMaps[towerID]->setTowerID( towerID );
+        }
+        else{
+          std::shared_ptr<PandoraPlus::CaloHalfCluster> tmp_clus = std::make_shared<PandoraPlus::CaloHalfCluster>();
+          tmp_clus->addUnit( iter.second );
+          tmp_clus->setTowerID( towerID );
+          tmp_LongiClusMaps[towerID] = tmp_clus.get();
+          m_bkCol.map_HalfCluster["bkHalfCluster"].push_back( tmp_clus );
+        }
+      }
+      p_shower = nullptr;
+
+    }
+
+    //Connect cousins
+    if(tmp_LongiClusMaps.size()>1){
+      for(auto &iter : tmp_LongiClusMaps){
+        for(auto &iter1 : tmp_LongiClusMaps){
+          if(iter!= iter1 &&
+             iter.second->getEnergy()>settings.map_floatPars["Eth_HFClus"] &&
+             iter1.second->getEnergy()>settings.map_floatPars["Eth_HFClus"] &&
+             iter.second->getCluster().size()>=settings.map_intPars["th_Nhit"] &&
+             iter1.second->getCluster().size()>=settings.map_intPars["th_Nhit"] ){ iter.second->addCousinCluster(iter1.second); }
+        }
+      }
+    }
+    for(auto &iter : tmp_LongiClusMaps){
+      iter.second->setType(m_halfClusV[il]->getType());
+      if(m_halfClusV[il]->getHalfClusterCol(settings.map_stringPars["ReadinAxisName"]).size()>0 )
+        iter.second->addHalfCluster(settings.map_stringPars["ReadinAxisName"], m_halfClusV[il]->getHalfClusterCol(settings.map_stringPars["ReadinAxisName"])[0]);
+
+      if(iter.second->getEnergy()<settings.map_floatPars["Eth_HFClus"]) continue;
+      iter.second->addHalfCluster("ParentCluster", m_halfClusV[il]);
+      for(int itrk=0; itrk<m_halfClusV[il]->getAssociatedTracks().size(); itrk++)
+        iter.second->addAssociatedTrack( m_halfClusV[il]->getAssociatedTracks()[itrk] );
+      if(iter.second->getCluster().size()>=settings.map_intPars["th_Nhit"]){
+        if(settings.map_boolPars["CompactHFCluster"]){ 
+          iter.second->mergeClusterInLayer();
+          iter.second->setTowerID(iter.first);
+        }
+        map_HalfClusterV[iter.first].push_back(iter.second);
+      }
+    }
+
+  }
+
+  //Build 2DCluster
+  for(auto &iterU : map_HalfClusterU){
+    if( map_HalfClusterV.find(iterU.first)==map_HalfClusterV.end() ){ 
+      iterU.second.clear();
+      continue; 
+    }
+
+    std::vector<PandoraPlus::CaloHalfCluster*> p_halfClusU = iterU.second; 
+    std::vector<PandoraPlus::CaloHalfCluster*> p_halfClusV = map_HalfClusterV[iterU.first]; 
+
+    //Get ordered showers for looping in layers. 
+    std::map<int, std::vector<const PandoraPlus::Calo1DCluster*>> m_orderedShowerU; m_orderedShowerU.clear();
+    std::map<int, std::vector<const PandoraPlus::Calo1DCluster*>> m_orderedShowerV; m_orderedShowerV.clear();
+
+    for(int ic=0; ic<p_halfClusU.size(); ic++){
+      for(int is=0; is<p_halfClusU.at(ic)->getCluster().size(); is++)
+        m_orderedShowerU[p_halfClusU.at(ic)->getCluster()[is]->getDlayer()].push_back( p_halfClusU.at(ic)->getCluster()[is] );
+    }
+    for(int ic=0; ic<p_halfClusV.size(); ic++){
+      for(int is=0; is<p_halfClusV.at(ic)->getCluster().size(); is++)
+        m_orderedShowerV[p_halfClusV.at(ic)->getCluster()[is]->getDlayer()].push_back( p_halfClusV.at(ic)->getCluster()[is] );
+    }
+    p_halfClusU.clear(); p_halfClusV.clear();
+
+
+    //Create super-layers (block)
+    std::vector<const PandoraPlus::Calo2DCluster*> m_blocks; m_blocks.clear();
+    for(auto &iter1 : m_orderedShowerU){
+      if( m_orderedShowerV.find( iter1.first )==m_orderedShowerV.end() ) continue; 
+      std::shared_ptr<PandoraPlus::Calo2DCluster> tmp_block = std::make_shared<PandoraPlus::Calo2DCluster>();
+      for(int is=0; is<iter1.second.size(); is++) tmp_block->addUnit( iter1.second.at(is) );
+      for(int is=0; is<m_orderedShowerV[iter1.first].size(); is++) tmp_block->addUnit( m_orderedShowerV[iter1.first].at(is) );
+      tmp_block->setTowerID( iterU.first );
+      m_blocks.push_back( tmp_block.get() );
+      m_bkCol.map_2DCluster["bk2DCluster"].push_back( tmp_block );
+    }
+    map_2DCluster[iterU.first] = m_blocks;
+  }
+  for(auto &iterV : map_HalfClusterV){
+    if( map_HalfClusterU.find(iterV.first)==map_HalfClusterU.end() ){
+      iterV.second.clear();
+    }
+  }
+
+  //Form a tower:
+  for(auto &iter : map_2DCluster){
+    std::vector<int> m_towerID = iter.first;
+//printf("  In tower: [%d, %d, %d] \n", m_towerID[0], m_towerID[1], m_towerID[2]);
+    //Check cousin clusters: 
+    std::vector<PandoraPlus::CaloHalfCluster*> m_HFClusUInTower = map_HalfClusterU[m_towerID];
+    for(auto &m_HFclus : m_HFClusUInTower){
+      std::vector<const CaloHalfCluster*> tmp_delClus; tmp_delClus.clear();
+      for(int ics=0; ics<m_HFclus->getHalfClusterCol("CousinCluster").size(); ics++){
+        std::vector<int> tmp_towerID = m_HFclus->getHalfClusterCol("CousinCluster")[ics]->getTowerID()[0];
+
+        if( map_2DCluster.find( tmp_towerID )==map_2DCluster.end() )
+          tmp_delClus.push_back( m_HFclus->getHalfClusterCol("CousinCluster")[ics] );
+      }
+      for(int ics=0; ics<tmp_delClus.size(); ics++) m_HFclus->deleteCousinCluster( tmp_delClus[ics] );
+    }
+
+    std::vector<PandoraPlus::CaloHalfCluster*> m_HFClusVInTower = map_HalfClusterV[m_towerID];
+    for(auto &m_HFclus : m_HFClusVInTower){
+      std::vector<const CaloHalfCluster*> tmp_delClus; tmp_delClus.clear();
+      for(int ics=0; ics<m_HFclus->getHalfClusterCol("CousinCluster").size(); ics++){
+        std::vector<int> tmp_towerID = m_HFclus->getHalfClusterCol("CousinCluster")[ics]->getTowerID()[0];
+
+        if( map_2DCluster.find( tmp_towerID )==map_2DCluster.end() )
+          tmp_delClus.push_back( m_HFclus->getHalfClusterCol("CousinCluster")[ics] );
+      }
+      for(int ics=0; ics<tmp_delClus.size(); ics++) m_HFclus->deleteCousinCluster( tmp_delClus[ics] );
+    }
+
+    std::vector<const PandoraPlus::CaloHalfCluster*> const_emptyClusU; const_emptyClusU.clear();
+    std::vector<const PandoraPlus::CaloHalfCluster*> const_emptyClusV; const_emptyClusV.clear();
+    if(map_emptyHalfClusterU.find(m_towerID)!=map_emptyHalfClusterU.end()){
+      for(int icl=0; icl<map_emptyHalfClusterU[m_towerID].size(); icl++){
+        map_emptyHalfClusterU[m_towerID][icl]->getLinkedMCPfromUnit();
+        const_emptyClusU.push_back(map_emptyHalfClusterU[m_towerID][icl]);
+      }
+      map_emptyHalfClusterU.erase(m_towerID);
+    }
+    if(map_emptyHalfClusterV.find(m_towerID)!=map_emptyHalfClusterV.end()){
+      for(int icl=0; icl<map_emptyHalfClusterV[m_towerID].size(); icl++){
+        map_emptyHalfClusterV[m_towerID][icl]->getLinkedMCPfromUnit();
+        const_emptyClusV.push_back(map_emptyHalfClusterV[m_towerID][icl]);
+      }
+      map_emptyHalfClusterV.erase(m_towerID);
+    }
+//cout<<"  Found empty half cluster size: "<<const_emptyClusU.size()<<", "<<const_emptyClusV.size()<<endl;
+
+    //Convert to const
+    std::vector<const PandoraPlus::CaloHalfCluster*> const_HFClusU; const_HFClusU.clear();
+    std::vector<const PandoraPlus::CaloHalfCluster*> const_HFClusV; const_HFClusV.clear();
+    for(int ics=0; ics<m_HFClusUInTower.size(); ics++){ m_HFClusUInTower[ics]->getLinkedMCPfromUnit(); const_HFClusU.push_back(m_HFClusUInTower[ics]); }
+    for(int ics=0; ics<m_HFClusVInTower.size(); ics++){ m_HFClusVInTower[ics]->getLinkedMCPfromUnit(); const_HFClusV.push_back(m_HFClusVInTower[ics]); }
+
+    std::shared_ptr<PandoraPlus::Calo3DCluster> m_tower = std::make_shared<PandoraPlus::Calo3DCluster>();
+//printf("  Creating tower: [%d, %d, %d] \n", m_towerID[0], m_towerID[1], m_towerID[2]);
+    m_tower->addTowerID( m_towerID );
+    for(int i2d=0; i2d<map_2DCluster[m_towerID].size(); i2d++) m_tower->addUnit(map_2DCluster[m_towerID][i2d]);
+    m_tower->setHalfClusters( settings.map_stringPars["OutputClusName"]+"U", const_HFClusU, 
+                              settings.map_stringPars["OutputClusName"]+"V", const_HFClusV );
+    m_tower->setHalfClusters( "emptyHalfClusterU", const_emptyClusU,
+                              "emptyHalfClusterV", const_emptyClusV );
+    m_towers.push_back(m_tower);
+  }
+
+  if(map_emptyHalfClusterU.size()>0 && map_emptyHalfClusterV.size()>0){
+    for(auto iter: map_emptyHalfClusterU){
+      std::vector<int> m_towerID = iter.first;
+
+      if( map_emptyHalfClusterV.find(m_towerID)==map_emptyHalfClusterV.end() ){
+        iter.second.clear();
+        //map_emptyHalfClusterU.erase(m_towerID);
+        continue;
+      }
+
+      std::vector<const PandoraPlus::CaloHalfCluster*> const_emptyClusU; const_emptyClusU.clear();
+      std::vector<const PandoraPlus::CaloHalfCluster*> const_emptyClusV; const_emptyClusV.clear();
+      for(int icl=0; icl<map_emptyHalfClusterU[m_towerID].size(); icl++){
+        map_emptyHalfClusterU[m_towerID][icl]->getLinkedMCPfromUnit();
+        const_emptyClusU.push_back(map_emptyHalfClusterU[m_towerID][icl]);
+      }
+      //map_emptyHalfClusterU.erase(m_towerID);
+      for(int icl=0; icl<map_emptyHalfClusterV[m_towerID].size(); icl++){
+        map_emptyHalfClusterV[m_towerID][icl]->getLinkedMCPfromUnit();
+        const_emptyClusV.push_back(map_emptyHalfClusterV[m_towerID][icl]);
+      }
+      //map_emptyHalfClusterV.erase(m_towerID);
+
+      std::shared_ptr<PandoraPlus::Calo3DCluster> m_tower = std::make_shared<PandoraPlus::Calo3DCluster>();
+      m_tower->addTowerID( m_towerID );
+      m_tower->setHalfClusters( "emptyHalfClusterU", const_emptyClusU,
+                                "emptyHalfClusterV", const_emptyClusV );
+      m_towers.push_back(m_tower);
+    }
+  }
+
+/*
+cout<<"  After splitting: tower size "<<m_towers.size()<<". Print Tower: "<<endl;
+for(auto it : m_towers){
+  std::vector<const CaloHalfCluster*> m_HFClusUInTower = it->getHalfClusterUCol(settings.map_stringPars["OutputClusName"]+"U");
+  std::vector<const CaloHalfCluster*> m_HFClusVInTower = it->getHalfClusterVCol(settings.map_stringPars["OutputClusName"]+"V");
+  //std::vector<const CaloHalfCluster*> m_HFClusUInTower = it->getHalfClusterUCol("emptyHalfClusterU");
+  //std::vector<const CaloHalfCluster*> m_HFClusVInTower = it->getHalfClusterVCol("emptyHalfClusterV");
+
+
+cout<<"Check tower ID: ";
+for(int i=0; i<it->getTowerID().size(); i++) printf("[%d, %d, %d], ", it->getTowerID()[i][0], it->getTowerID()[i][1], it->getTowerID()[i][2]);
+cout<<endl;
+
+  printf("    In Tower [%d, %d, %d], ", it->getTowerID()[0][0], it->getTowerID()[0][1], it->getTowerID()[0][2] );
+  printf("    HalfCluster size: (%d, %d) \n", m_HFClusUInTower.size(), m_HFClusVInTower.size() );
+  cout<<"    Loop print HalfClusterU: "<<endl;
+  for(int icl=0; icl<m_HFClusUInTower.size(); icl++){
+    cout<<"      In HFClusU #"<<icl<<": shower size = "<<m_HFClusUInTower[icl]->getCluster().size()<<", En = "<<m_HFClusUInTower[icl]->getEnergy()<<", type "<<m_HFClusUInTower[icl]->getType();
+    printf(", Position (%.3f, %.3f, %.3f), address %p ",m_HFClusUInTower[icl]->getPos().x(), m_HFClusUInTower[icl]->getPos().y(), m_HFClusUInTower[icl]->getPos().z(), m_HFClusUInTower[icl]);
+    printf(", cousin size %d, address: ", m_HFClusUInTower[icl]->getHalfClusterCol("CousinCluster").size());
+    for(int ics=0; ics<m_HFClusUInTower[icl]->getHalfClusterCol("CousinCluster").size(); ics++) printf("%p, ", m_HFClusUInTower[icl]->getHalfClusterCol("CousinCluster")[ics]);
+    printf(", track size %d, address: ", m_HFClusUInTower[icl]->getAssociatedTracks().size());
+    for(int itrk=0; itrk<m_HFClusUInTower[icl]->getAssociatedTracks().size(); itrk++) printf("%p, ", m_HFClusUInTower[icl]->getAssociatedTracks()[itrk]);
+    cout<<endl;
+    for(auto ish : m_HFClusUInTower[icl]->getCluster()){
+      printf("          Shower layer %d, Pos+E (%.3f, %.3f, %.3f, %.3f), Nbars %d, NSeed %d, Address %p \n", ish->getDlayer(), ish->getPos().x(), ish->getPos().y(), ish->getPos().z(), ish->getEnergy(), ish->getBars().size(),  ish->getNseeds(), ish );
+    }
+  }
+  cout<<endl;
+
+  cout<<"    Loop print HalfClusterV: "<<endl;
+  for(int icl=0; icl<m_HFClusVInTower.size(); icl++){
+    cout<<"      In HFClusV #"<<icl<<": shower size = "<<m_HFClusVInTower[icl]->getCluster().size()<<", En = "<<m_HFClusVInTower[icl]->getEnergy()<<", type "<<m_HFClusVInTower[icl]->getType();
+    printf(", Position (%.3f, %.3f, %.3f), address %p ",m_HFClusVInTower[icl]->getPos().x(), m_HFClusVInTower[icl]->getPos().y(), m_HFClusVInTower[icl]->getPos().z(), m_HFClusVInTower[icl]);
+    printf(", cousin size %d, address: ", m_HFClusVInTower[icl]->getHalfClusterCol("CousinCluster").size());
+    for(int ics=0; ics<m_HFClusVInTower[icl]->getHalfClusterCol("CousinCluster").size(); ics++) printf("%p, ", m_HFClusVInTower[icl]->getHalfClusterCol("CousinCluster")[ics]);
+    printf(", track size %d, address: ", m_HFClusVInTower[icl]->getAssociatedTracks().size());
+    for(int itrk=0; itrk<m_HFClusVInTower[icl]->getAssociatedTracks().size(); itrk++) printf("%p, ", m_HFClusVInTower[icl]->getAssociatedTracks()[itrk]);
+    cout<<endl;
+    for(auto ish : m_HFClusVInTower[icl]->getCluster()){
+      printf("          Shower layer %d, Pos+E (%.3f, %.3f, %.3f, %.3f), Nbars %d, NSeed %d, Address %p \n", ish->getDlayer(), ish->getPos().x(), ish->getPos().y(), ish->getPos().z(), ish->getEnergy(), ish->getBars().size(), ish->getNseeds(), ish );
+    }
+  }
+  cout<<endl;
+}
+*/
+
+  return StatusCode::SUCCESS;
+}
+
+
+StatusCode EnergySplittingAlg::ClusterSplitting(const PandoraPlus::Calo1DCluster* m_cluster, std::vector<std::shared_ptr<PandoraPlus::Calo1DCluster>>& outshCol ){
+
+//cout<<"ClusterSplitting: input cluster seed size = "<<m_cluster->getSeeds().size()<<endl;
+//cout<<"ClusterSplitting: input bar size = "<<m_cluster->getBars().size()<<endl;
+//cout<<"Seed position and E: "<<endl;
+//for(int a=0; a<m_cluster->getNseeds(); a++) printf("\t (%.3f, %.3f, %.3f, %.3f), barID %d \n", m_cluster->getSeeds()[a]->getPosition().x(), 
+//                                                                                             m_cluster->getSeeds()[a]->getPosition().y(),
+//                                                                                             m_cluster->getSeeds()[a]->getPosition().z(),
+//                                                                                             m_cluster->getSeeds()[a]->getEnergy(), 
+//                                                                                             m_cluster->getSeeds()[a]->getBar() );
+//cout<<endl;
+//double tmp_totE = 0.;
+//for(int i=0; i<m_cluster->getBars().size(); i++) tmp_totE += m_cluster->getBars()[i]->getEnergy();
+//cout<<"Total bar energy: "<<tmp_totE<<endl;
+
+  //No seed in cluster: return origin cluster.
+  if(m_cluster->getNseeds()==0) { 
+    auto shower = m_cluster->Clone();
+    outshCol.push_back(shower);
+    //std::cout<<"WARNING: Still have no-seed cluster!!"<<std::endl; 
+    return StatusCode::SUCCESS; 
+  }
+
+  //1 seed or second moment less than threshold: Not split. Turn cluster to shower and return
+  else if(m_cluster->getNseeds()<2 || m_cluster->getScndMoment()<settings.map_floatPars["th_split"]){
+    auto shower = m_cluster->Clone();
+    outshCol.push_back(shower);
+    return StatusCode::SUCCESS;
+  }
+
+  
+  //Separated bars: 
+  else if( m_cluster->getNseeds()>=m_cluster->getBars().size() ){
+    for(int ish=0; ish<m_cluster->getNseeds(); ish++){
+      std::shared_ptr<PandoraPlus::Calo1DCluster> shower = std::make_shared<PandoraPlus::Calo1DCluster>();
+      shower->addUnit(m_cluster->getSeeds()[ish]);
+      shower->addSeed(m_cluster->getSeeds()[ish]);
+      shower->setIDInfo();
+
+      outshCol.push_back(shower);
+    }
+    return StatusCode::SUCCESS;
+  }
+  
+  //2 or more seeds, large second moment: Split
+  int Nshower = m_cluster->getNseeds();
+  int Nbars = m_cluster->getBars().size();
+  double Eseed[Nshower] = {0};
+  double weight[Nbars][Nshower] = {0};
+  TVector3 SeedPos[Nshower];
+  TVector3 SeedPos_Origin[Nshower];
+  for(int is=0;is<Nshower;is++){ 
+    SeedPos[is] = m_cluster->getSeeds()[is]->getPosition(); 
+    SeedPos_Origin[is]=SeedPos[is]; 
+    Eseed[is]=m_cluster->getSeeds()[is]->getEnergy(); 
+  }
+  //CalculateInitialEseed(m_cluster->getSeeds(), SeedPos, Eseed);
+/*
+  bool isConverge = false;
+  bool isShifted = false; 
+  int iter=0;
+  TVector3 SeedPos_prev[Nshower];
+  do{
+
+    for(int ibar=0;ibar<m_cluster->getBars().size();ibar++){
+      double Eexp[Nshower];
+      double Eexp_tot=0;
+      for(int is=0;is<Nshower;is++){ Eexp[is] = Eseed[is]*GetShowerProfile(m_cluster->getBars()[ibar]->getPosition(), SeedPos[is] ); Eexp_tot+= Eexp[is];}
+      for(int is=0;is<Nshower;is++) weight[ibar][is] = Eexp[is]/Eexp_tot;
+    }
+    for(int is=0;is<Nshower;is++){
+      SeedPos_prev[is]=SeedPos[is];
+
+      double Etot=0;
+      double Ebar[Nbars] = {0};
+      double Emax = -99;
+      for(int ib=0;ib<Nbars;ib++){
+        Ebar[ib] = ( m_cluster->getBars()[ib]->getQ1()*weight[ib][is] + m_cluster->getBars()[ib]->getQ2()*weight[ib][is] )/2.; 
+        Etot += Ebar[ib];
+        if(Ebar[ib]>Emax) Emax = Ebar[ib];
+      }
+
+      TVector3 pos(0,0,0);
+      for(int ib=0; ib<Nbars; ib++) pos += m_cluster->getBars()[ib]->getPosition() * (Ebar[ib]/Etot);
+      SeedPos[is] = pos; 
+      Eseed[is] = Emax;
+    }
+    isConverge=true;
+    for(int is=0;is<Nshower;is++) if( (SeedPos_prev[is]-SeedPos[is]).Mag2()>2.89 ){ isConverge=false; break;}
+ 
+    isShifted = false;
+    for(int is=0;is<Nshower;is++) if( (SeedPos[is]-SeedPos_Origin[is]).Mag2()<15 ){ isShifted=true; break;}
+    iter++;
+
+  }
+  while(iter<20 && !isConverge && !isShifted );
+  if(iter>=20){
+    std::cout<<"WARNING: Iteration time larger than 20! Might not converge!"<<std::endl;
+    std::cout<<"  For Check: NBars: "<<m_cluster->getBars().size()<<"  Nseeds: "<<Nshower<<std::endl;
+  }
+*/
+
+
+//Check: no iteration, just calculate the weight.
+  for(int ibar=0;ibar<m_cluster->getBars().size();ibar++){
+    double Eexp[Nshower];
+    double Eexp_tot=0;
+    for(int is=0;is<Nshower;is++){ Eexp[is] = Eseed[is]*GetShowerProfile(m_cluster->getBars()[ibar]->getPosition(), SeedPos[is] ); Eexp_tot+= Eexp[is];}
+    for(int is=0;is<Nshower;is++) weight[ibar][is] = Eexp[is]/Eexp_tot;
+  }
+
+
+
+//cout<<"Print weight: "<<endl;
+//for(int abar=0; abar<Nbars; abar++){
+//for(int ash=0; ash<Nshower; ash++){
+//  printf("%.3f \t",weight[abar][ash]);
+//}
+//cout<<endl;
+//}
+
+//cout<<"After weight calculation. "<<endl;
+  //for(int is=0;is<Nshower;is++) SeedPos[is] = m_cluster->getSeeds()[is]->getPosition();
+  for(int is=0;is<Nshower;is++){
+//cout<<"  In shower #"<<is<<endl;
+    std::vector<const PandoraPlus::CaloUnit*> Bars; Bars.clear();
+    int iseed=-1;
+    int icount=0;
+    double _Emax = -99;
+    
+    for(int ib=0;ib<Nbars;ib++){
+      double barEn = (m_cluster->getBars()[ib]->getQ1()*weight[ib][is] + m_cluster->getBars()[ib]->getQ2()*weight[ib][is])/2.;
+//cout<<"    bar#"<<ib<<" barID "<<m_cluster->getBars()[ib]->getBar()<<" En: "<<barEn<<endl;
+      if(barEn<settings.map_floatPars["Eth_unit"]) continue;
+
+      auto bar = m_cluster->getBars()[ib]->Clone();
+      bar->setQ( bar->getQ1()*weight[ib][is], bar->getQ2()*weight[ib][is]  );
+      if( bar->getEnergy()>_Emax ) { _Emax=bar->getEnergy(); iseed=icount; }
+      Bars.push_back(bar.get());
+      icount++;
+      m_bkCol.map_BarCol["bkBar"].push_back( bar );
+//cout<<"      Found seed: iseed="<<iseed<<", icount="<<icount<<endl;
+    }
+    if(iseed<0) { std::cout<<"ERROR: Can not find seed(max energy bar) in this shower! Nbars = "<<Nbars<<". Please Check!"<<std::endl; continue; }
+    //if( (Bars[iseed]->getPosition()-SeedPos[is]).Mag()>15 ) { std::cout<<"ERROR: MaxEnergy bar is too far with original seed! Please Check! iSeed = "<<iseed<<std::endl; }
+
+    std::shared_ptr<PandoraPlus::Calo1DCluster> shower = std::make_shared<PandoraPlus::Calo1DCluster>();
+    shower->setBars(Bars);
+    shower->addSeed(Bars[iseed]);
+    shower->setIDInfo(); 
+
+
+    outshCol.push_back(shower);
+  }
+  
+  return StatusCode::SUCCESS;
+}
+
+
+StatusCode EnergySplittingAlg::SplitOverlapCluster( std::vector<std::shared_ptr<PandoraPlus::CaloHalfCluster>>& m_HFClusCol ){
+
+  if(m_HFClusCol.size()<2) return StatusCode::SUCCESS;
+
+  for(int ic=0; ic<m_HFClusCol.size()-1; ic++){
+    for(int jc=ic+1; jc<m_HFClusCol.size(); jc++){
+      double E_ratio = m_HFClusCol[ic]->OverlapRatioE(m_HFClusCol[jc].get());
+      if(E_ratio>0){
+
+        std::vector<const Calo1DCluster*> m_ClusCol1 =  m_HFClusCol[ic]->getCluster();
+        std::vector<const Calo1DCluster*> m_ClusCol2 =  m_HFClusCol[jc]->getCluster();
+        std::vector<const Calo1DCluster*> m_overlapCl; m_overlapCl.clear();
+        for(int i1d=0; i1d<m_ClusCol1.size(); i1d++){
+          if(find(m_ClusCol2.begin(), m_ClusCol2.end(), m_ClusCol1[i1d])!=m_ClusCol2.end() ) m_overlapCl.push_back(m_ClusCol1[i1d]);
+        }
+
+        for(int io=0; io<m_overlapCl.size(); io++){
+
+          //Split the cluster with +-1 layer cluster energy.
+          int dlayer = m_overlapCl[io]->getDlayer();
+
+          //  For  m_HFClusCol[ic]
+          std::vector<const Calo1DCluster*> m_clus1_front = m_HFClusCol[ic]->getClusterInLayer(dlayer-1);
+          std::vector<const Calo1DCluster*> m_clus1_back  = m_HFClusCol[ic]->getClusterInLayer(dlayer+1);
+          std::vector<const Calo1DCluster*> m_clus2_front = m_HFClusCol[jc]->getClusterInLayer(dlayer-1);
+          std::vector<const Calo1DCluster*> m_clus2_back  = m_HFClusCol[jc]->getClusterInLayer(dlayer+1);
+
+          if( m_clus1_front.size()==0 && m_clus1_back.size()==0 && m_clus2_front.size()==0 && m_clus2_back.size()==0 ){
+            //An isolated hit in both: delete in m_HFClusCol[ic]. TODO: maybe need to optimize based on profile?
+            m_HFClusCol[ic]->deleteUnit(m_overlapCl[io]);
+            continue;
+          }
+          else if( m_clus1_front.size()==0 && m_clus1_back.size()==0 ){
+            //Only isolated in m_HFClusCol[ic]: delete in m_HFClusCol[ic].
+            m_HFClusCol[ic]->deleteUnit(m_overlapCl[io]);
+            continue;
+          }
+          else if( m_clus2_front.size()==0 && m_clus2_back.size()==0 ){
+            m_HFClusCol[jc]->deleteUnit(m_overlapCl[io]);
+            continue;
+          }
+          else{
+            //Use the average energy
+            double aveEn1 = 0.;
+            for(int i1d=0; i1d<m_clus1_front.size(); i1d++) aveEn1 += m_clus1_front[i1d]->getEnergy();
+            for(int i1d=0; i1d<m_clus1_back.size(); i1d++) aveEn1 += m_clus1_back[i1d]->getEnergy();
+            aveEn1 = aveEn1/(m_clus1_front.size()+m_clus1_back.size());
+
+            double aveEn2 = 0.;
+            for(int i1d=0; i1d<m_clus2_front.size(); i1d++) aveEn2 += m_clus2_front[i1d]->getEnergy();
+            for(int i1d=0; i1d<m_clus2_back.size(); i1d++) aveEn2 += m_clus2_back[i1d]->getEnergy();
+            aveEn2 = aveEn2/(m_clus2_front.size()+m_clus2_back.size());
+
+            std::shared_ptr<PandoraPlus::Calo1DCluster> m_splitCl1 = std::make_shared<PandoraPlus::Calo1DCluster>();
+            std::shared_ptr<PandoraPlus::Calo1DCluster> m_splitCl2 = std::make_shared<PandoraPlus::Calo1DCluster>();
+
+            for(int ib=0; ib<m_overlapCl[io]->getBars().size(); ib++){
+              auto bar1 = m_overlapCl[io]->getBars()[ib]->Clone();
+              bar1->setQ( bar1->getQ1()*aveEn1/(aveEn1+aveEn2), bar1->getQ2()*aveEn1/(aveEn1+aveEn2) );
+              m_splitCl1->addUnit(bar1.get());
+
+              auto bar2 = m_overlapCl[io]->getBars()[ib]->Clone();
+              bar2->setQ( bar2->getQ1()*aveEn2/(aveEn1+aveEn2), bar2->getQ2()*aveEn2/(aveEn1+aveEn2) );
+              m_splitCl2->addUnit(bar2.get());
+
+              m_bkCol.map_BarCol["bkBar"].push_back(bar1);
+              m_bkCol.map_BarCol["bkBar"].push_back(bar2);
+            }
+            m_splitCl1->setSeed();
+            m_splitCl1->setIDInfo();
+            m_splitCl1->getLinkedMCPfromUnit();
+            m_splitCl2->setSeed();
+            m_splitCl2->setIDInfo();
+            m_splitCl2->getLinkedMCPfromUnit();
+
+            m_HFClusCol[ic]->deleteUnit(m_overlapCl[io]);
+            m_HFClusCol[ic]->addUnit(m_splitCl1.get());
+
+            m_HFClusCol[jc]->deleteUnit(m_overlapCl[io]);
+            m_HFClusCol[jc]->addUnit(m_splitCl2.get());
+
+            m_bkCol.map_1DCluster["bk1DCluster"].push_back(m_splitCl1);
+            m_bkCol.map_1DCluster["bk1DCluster"].push_back(m_splitCl2);
+          }
+
+
+        }//end loop m_HFClusCol
+      }
+
+    }
+  }
+  return StatusCode::SUCCESS;
+}
+
+
+StatusCode EnergySplittingAlg::MergeToClosestCluster( PandoraPlus::Calo1DCluster* iclus, std::vector<std::shared_ptr<PandoraPlus::Calo1DCluster>>& clusvec ){
+
+  int cLedge = iclus->getLeftEdge();
+  int cRedge = iclus->getRightEdge();
+
+//printf("ClusterMerging: input cluster layer %d, energy %.4f, edge [%d, %d]\n", iclus->getDlayer(), iclus->getEnergy(), cLedge, cRedge);
+
+  //Find the closest cluster with iclus.
+  int minD = 99;
+  int index = -1;
+  for(int icl=0; icl<clusvec.size(); icl++){
+    if(clusvec[icl].get()->getNseeds()==0) continue;
+    if( iclus->getTowerID() != clusvec[icl].get()->getTowerID() ) continue;
+    if( iclus->getDlayer() != clusvec[icl].get()->getDlayer() ) continue;
+
+    int iLedge = clusvec[icl].get()->getLeftEdge();
+    int iRedge = clusvec[icl].get()->getRightEdge();
+
+    //int dis = (cLedge-iRedge>0 ? cLedge-iRedge : iLedge-cRedge );
+    int dis = min( abs(cLedge-iRedge), abs(iLedge-cRedge) );
+
+//printf("  Loop in clusterCol #%d: range [%d, %d], distance %d, energy ratio %.3f \n", icl, iLedge, iRedge, dis, iclus->getEnergy()/clusvec[icl]->getEnergy());
+    if(dis>10) continue; //Don't merge to a too far cluster.
+    if(dis<minD){ minD = dis; index=icl; }
+  }
+//cout<<"Selected closest cluster #"<<index<<endl;
+
+  if(index<0) return StatusCode::FAILURE;
+
+  //Merge to the selected cluster
+  for(int icl=0; icl<iclus->getBars().size(); icl++)  clusvec[index].get()->addUnit(iclus->getBars()[icl]);
+
+
+  return StatusCode::SUCCESS;
+}
+
+
+StatusCode EnergySplittingAlg::MergeToClosestCluster( const PandoraPlus::Calo1DCluster* m_shower, 
+                                                      std::vector<std::shared_ptr<PandoraPlus::CaloHalfCluster>>& m_clusters )
+{
+  if(m_clusters.size()==0) return StatusCode::SUCCESS;
+
+
+  int minLayer = 99;
+  int maxLayer = -99;
+  for(int ic=0; ic<m_clusters.size(); ic++){
+    if(minLayer>m_clusters[ic].get()->getBeginningDlayer())  minLayer = m_clusters[ic].get()->getBeginningDlayer();
+    if(maxLayer<m_clusters[ic].get()->getEndDlayer())        maxLayer = m_clusters[ic].get()->getEndDlayer();
+  }
+  if(minLayer==99 || minLayer<0 || maxLayer<0) return StatusCode::SUCCESS;
+
+  int dlayer = m_shower->getDlayer();
+  TVector3 sh_pos = m_shower->getPos();
+
+//cout<<"  Cluster range: ("<<minLayer<<", "<<maxLayer<<") "<<endl;
+//cout<<"  Merging shower into cluster: Input shower ";
+//printf(" (%.3f, %.3f, %.3f, %.3f), towerID[%d, %d, %d], layer #%d \n", sh_pos.X(), sh_pos.Y(), sh_pos.Z(), m_shower->getEnergy(), m_shower->getTowerID()[0][0], m_shower->getTowerID()[0][1], m_shower->getTowerID()[0][2], dlayer);
+
+  double minR = 999;
+  int index_cluster = -1;
+  double m_distance = 0;
+  for(int ic=0; ic<m_clusters.size(); ic++ ){
+//printf("    Cluster #%d: pos (%.2f, %.2f, %.2f) \n", ic, m_clusters[ic]->getPos().x(), m_clusters[ic]->getPos().y(), m_clusters[ic]->getPos().z());
+    if(dlayer<=minLayer)
+      m_distance = (sh_pos-m_clusters[ic].get()->getClusterInLayer(m_clusters[ic].get()->getBeginningDlayer())[0]->getPos()).Mag();     
+    else if(dlayer>=maxLayer)
+      m_distance = (sh_pos-m_clusters[ic].get()->getClusterInLayer(m_clusters[ic].get()->getEndDlayer())[0]->getPos()).Mag();    
+    else
+      m_distance = (sh_pos-m_clusters[ic].get()->getPos()).Mag();
+
+//cout<<"    Cluster #"<<ic<<": distance with shower = "<<m_distance<<endl;
+    
+    if( m_distance<minR )  { minR=m_distance; index_cluster=ic; }
+  }
+
+//printf("  minR = %.3f, in #cl %d, cluster size %d \n", minR, index_cluster, m_clusters.size());
+
+  if(index_cluster>=0) m_clusters[index_cluster].get()->addUnit( m_shower );
+
+  return StatusCode::SUCCESS;
+}
+
+
+void EnergySplittingAlg::CalculateInitialEseed( const std::vector<const PandoraPlus::CaloUnit*>& Seeds, const TVector3* pos, double* Eseed){
+//Calculate Eseed by solving a linear function:
+// [ f(11) .. f(1mu)  .. ]   [E_seed 1 ]   [E_bar 1]
+// [  ..   ..   ..    .. ] * [...      ] = [...    ]
+// [ f(i1) .. f(imu)  .. ]   [E_seed mu]   [E_bar i]
+// [ f(N1) ..   ..  f(NN)]   [E_seed N ]   [E_bar N]
+
+  const int Nele = Seeds.size();
+  std::vector<double> Eratio;
+  std::vector<double> vec_Etot; //bar energy
+
+  TVector vecE(Nele);
+  TMatrix matrixR(Nele, Nele);
+
+  for(int i=0;i<Nele;i++){ //Loop bar
+    vecE[i] = Seeds[i]->getEnergy();
+    for(int j=0;j<Nele;j++) matrixR[i][j] = GetShowerProfile(Seeds[i]->getPosition(), pos[j]);
+  }
+
+
+  matrixR.Invert();
+  TVector sol = matrixR*vecE;
+
+  for(int i=0;i<Nele;i++) Eseed[i] = sol[i];
+
+//cout<<"Print initial Eseed: "<<endl;
+//for(int i=0;i<Nele;i++) cout<<Eseed[i]<<'\t';
+//cout<<endl;
+
+}
+
+
+double EnergySplittingAlg::GetShowerProfile(const TVector3& p_bar, const TVector3& p_seed ){
+  TVector3 rpos = p_bar-p_seed;
+  double dis = sqrt(rpos.Mag2());
+  double a1, a2, b1, b2, Rm;
+  a1=0.037; a2=0.265; b1=0.101; b2=0.437; Rm=1.868;
+
+  return a1*exp(-b1*dis/Rm)+a2*exp(-b2*dis/Rm);
+}
+#endif
diff --git a/Reconstruction/CrystalCaloRec/src/Algorithm/EnergyTimeMatchingAlg.cpp b/Reconstruction/CrystalCaloRec/src/Algorithm/EnergyTimeMatchingAlg.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..2bfc0c2a783c30c23b408ed5c1283416f48f4e4a
--- /dev/null
+++ b/Reconstruction/CrystalCaloRec/src/Algorithm/EnergyTimeMatchingAlg.cpp
@@ -0,0 +1,1445 @@
+#ifndef ETMATCHING_ALG_C
+#define ETMATCHING_ALG_C
+
+#include "Algorithm/EnergyTimeMatchingAlg.h"
+StatusCode EnergyTimeMatchingAlg::ReadSettings(Settings& m_settings){
+  settings = m_settings;
+
+  //Set initial value
+  if(settings.map_floatPars.find("chi2Wi_E")==settings.map_floatPars.end())          settings.map_floatPars["chi2Wi_E"] = 1.;
+  if(settings.map_floatPars.find("chi2Wi_T")==settings.map_floatPars.end())          settings.map_floatPars["chi2Wi_T"] = 10.;
+  if(settings.map_floatPars.find("sigmaE")==settings.map_floatPars.end())            settings.map_floatPars["sigmaE"] = 0.10;
+  if(settings.map_floatPars.find("sigmaPos")==settings.map_floatPars.end())          settings.map_floatPars["sigmaPos"] = 34.89;
+  if(settings.map_floatPars.find("nMat")==settings.map_floatPars.end())              settings.map_floatPars["nMat"] = 2.15;
+  if(settings.map_floatPars.find("Eth_HFClus")==settings.map_floatPars.end())        settings.map_floatPars["Eth_HFClus"] = 0.05;
+  if(settings.map_floatPars.find("th_overlapE")==settings.map_floatPars.end())       settings.map_floatPars["th_overlapE"] = 0.3;
+  if(settings.map_floatPars.find("th_UVdeltaE")==settings.map_floatPars.end())       settings.map_floatPars["th_UVdeltaE"] = 0.3;
+  if(settings.map_intPars.find("compressLayer")==settings.map_intPars.end())       settings.map_intPars["compressLayer"] = 3;
+  if(settings.map_floatPars.find("th_UVdeltaEinLayer")==settings.map_floatPars.end()) settings.map_floatPars["th_UVdeltaEinLayer"] = 0.5;
+  if(settings.map_floatPars.find("th_ConeTheta")==settings.map_floatPars.end())      settings.map_floatPars["th_ConeTheta"] = TMath::Pi()/6.;
+  if(settings.map_floatPars.find("th_ConeR")==settings.map_floatPars.end())          settings.map_floatPars["th_ConeR"] = 30.;
+
+  if(settings.map_stringPars.find("ReadinHFClusterName")==settings.map_stringPars.end()) settings.map_stringPars["ReadinHFClusterName"] = "ESHalfCluster";
+  if(settings.map_stringPars.find("ReadinTowerName")==settings.map_stringPars.end()) settings.map_stringPars["ReadinTowerName"] = "ESTower";
+  if(settings.map_stringPars.find("OutputClusterName")==settings.map_stringPars.end()) settings.map_stringPars["OutputClusterName"] = "EcalCluster";
+
+  return StatusCode::SUCCESS;
+};
+
+
+StatusCode EnergyTimeMatchingAlg::Initialize( PandoraPlusDataCol& m_datacol ){
+  m_HFClusUCol.clear();
+  m_HFClusVCol.clear();
+  m_clusterCol.clear();
+  m_towerCol.clear();
+  m_bkCol.Clear();
+
+  int ntower = m_datacol.map_CaloCluster[settings.map_stringPars["ReadinTowerName"]].size(); 
+  for(int it=0; it<ntower; it++)
+    m_towerCol.push_back( m_datacol.map_CaloCluster[settings.map_stringPars["ReadinTowerName"]][it].get() );
+
+//cout<<"  EnergyTimeMatchingAlg: Readin tower size: "<<m_towerCol.size()<<endl;
+	return StatusCode::SUCCESS;
+};
+
+
+StatusCode EnergyTimeMatchingAlg::RunAlgorithm( PandoraPlusDataCol& m_datacol ){
+
+
+  m_clusterCol.clear();
+  std::vector<const PandoraPlus::CaloHalfCluster*> m_leftHFClusterUCol; 
+  std::vector<const PandoraPlus::CaloHalfCluster*> m_leftHFClusterVCol; 
+
+  //Loop for towers:  
+  for(int it=0; it<m_towerCol.size(); it++){
+    m_HFClusUCol.clear(); m_HFClusVCol.clear();
+
+//cout<<"Check tower ID: ";
+//for(int i=0; i<m_towerCol[it]->getTowerID().size(); i++) printf("[%d, %d, %d], ", m_towerCol[it]->getTowerID()[i][0], m_towerCol[it]->getTowerID()[i][1], m_towerCol[it]->getTowerID()[i][2]);
+//cout<<endl;
+
+    m_HFClusUCol = m_towerCol.at(it)->getHalfClusterUCol(settings.map_stringPars["ReadinHFClusterName"]+"U");
+    m_HFClusVCol = m_towerCol.at(it)->getHalfClusterVCol(settings.map_stringPars["ReadinHFClusterName"]+"V");
+
+    Matching(m_HFClusUCol, m_HFClusVCol, m_clusterCol);
+
+  }//End loop towers
+
+  //Re-loop tower for empty half-clusters
+//cout<<"Match clusters without axis"<<endl;
+  for(int it=0; it<m_towerCol.size(); it++){
+    m_HFClusUCol.clear(); m_HFClusVCol.clear();
+
+    m_HFClusUCol = m_towerCol.at(it)->getHalfClusterUCol("emptyHalfClusterU");
+    m_HFClusVCol = m_towerCol.at(it)->getHalfClusterVCol("emptyHalfClusterV");
+
+    Matching(m_HFClusUCol, m_HFClusVCol, m_clusterCol);
+
+  }//End loop tower
+
+  ClusterReconnecting( m_clusterCol );
+  
+  m_datacol.map_CaloCluster[settings.map_stringPars["OutputClusterName"]] = m_clusterCol;
+
+//cout<<" Save backup collections in to main datacol. "<<endl;
+  //Save backup collections in to main datacol. 
+  m_datacol.map_CaloHit["bkHit"].insert( m_datacol.map_CaloHit["bkHit"].end(), m_bkCol.map_CaloHit["bkHit"].begin(), m_bkCol.map_CaloHit["bkHit"].end() );
+  m_datacol.map_BarCol["bkBar"].insert( m_datacol.map_BarCol["bkBar"].end(), m_bkCol.map_BarCol["bkBar"].begin(), m_bkCol.map_BarCol["bkBar"].end() );
+  m_datacol.map_1DCluster["bk1DCluster"].insert( m_datacol.map_1DCluster["bk1DCluster"].end(), m_bkCol.map_1DCluster["bk1DCluster"].begin(), m_bkCol.map_1DCluster["bk1DCluster"].end() );
+  m_datacol.map_2DCluster["bk2DCluster"].insert( m_datacol.map_2DCluster["bk2DCluster"].end(), m_bkCol.map_2DCluster["bk2DCluster"].begin(), m_bkCol.map_2DCluster["bk2DCluster"].end() );
+  m_datacol.map_HalfCluster["bkHalfCluster"].insert( m_datacol.map_HalfCluster["bkHalfCluster"].end(), m_bkCol.map_HalfCluster["bkHalfCluster"].begin(), m_bkCol.map_HalfCluster["bkHalfCluster"].end() );
+
+  return StatusCode::SUCCESS;
+}
+
+
+StatusCode EnergyTimeMatchingAlg::ClearAlgorithm(){
+  m_HFClusUCol.clear();
+  m_HFClusVCol.clear();
+  m_clusterCol.clear();
+  m_towerCol.clear();
+  m_bkCol.Clear();
+  
+
+  return StatusCode::SUCCESS;
+}
+
+
+StatusCode EnergyTimeMatchingAlg::Matching( std::vector<const PandoraPlus::CaloHalfCluster*> m_HFClusUCol,
+                                            std::vector<const PandoraPlus::CaloHalfCluster*> m_HFClusVCol,
+                                            std::vector<std::shared_ptr<PandoraPlus::Calo3DCluster>>& m_clusterCol )
+{
+
+  const int NclusU = m_HFClusUCol.size();
+  const int NclusV = m_HFClusVCol.size();
+
+//cout<<" cluster size ["<<NclusU<<", "<<NclusV<<"] "<<endl;
+//cout<<"ClusterU energy: ";
+//for(int i=0; i<NclusU; i++) cout<<m_HFClusUCol[i]->getEnergy()<<'\t';
+//cout<<endl;
+//cout<<"ClusterV energy: ";
+//for(int i=0; i<NclusV; i++) cout<<m_HFClusVCol[i]->getEnergy()<<'\t';
+//cout<<endl;
+
+
+  std::vector<std::shared_ptr<PandoraPlus::Calo3DCluster>> tmp_clusters; tmp_clusters.clear();
+  if(NclusU==0 || NclusV==0) return StatusCode::SUCCESS;
+
+  else if(NclusU==1 && NclusV==1){
+      std::shared_ptr<PandoraPlus::Calo3DCluster> tmp_clus = std::make_shared<PandoraPlus::Calo3DCluster>();
+      XYClusterMatchingL0(m_HFClusUCol[0], m_HFClusVCol[0], tmp_clus);
+      tmp_clusters.push_back(tmp_clus);
+  }  
+  else if(NclusU==1){
+    double sumE_V = 0.;
+    for(int icl=0; icl<NclusV; icl++) sumE_V += m_HFClusVCol[icl]->getEnergy();
+
+    //Split the HalfClusterU into NclusV part
+    for(int icl=0; icl<NclusV; icl++){
+      std::shared_ptr<PandoraPlus::CaloHalfCluster> newClus = std::make_shared<PandoraPlus::CaloHalfCluster>();
+      double frac = m_HFClusVCol[icl]->getEnergy()/sumE_V;
+      for(int ish=0; ish<m_HFClusUCol[0]->getCluster().size(); ish++){
+        const PandoraPlus::Calo1DCluster* p_shower = m_HFClusUCol[0]->getCluster()[ish];
+
+        std::vector<const PandoraPlus::CaloUnit*> Bars; Bars.clear();
+        for(int ibar=0; ibar<p_shower->getCluster().size(); ibar++){
+          auto bar = p_shower->getCluster()[ibar]->Clone();
+          bar->setQ(bar->getQ1()*frac, bar->getQ2()*frac );
+          Bars.push_back(bar.get());
+          m_bkCol.map_BarCol["bkBar"].push_back(bar);
+        }
+
+        std::shared_ptr<PandoraPlus::Calo1DCluster> shower = std::make_shared<PandoraPlus::Calo1DCluster>();
+        shower->setBars(Bars);
+        shower->setSeed();
+        shower->setIDInfo();
+
+        newClus->addUnit(shower.get());
+        m_bkCol.map_1DCluster["bk1DCluster"].push_back( shower );
+      }
+      newClus->setHoughPars(m_HFClusUCol[0]->getHoughAlpha(), m_HFClusUCol[0]->getHoughRho());
+      newClus->setIntercept(m_HFClusUCol[0]->getHoughIntercept());
+      for(auto iter: m_HFClusUCol[0]->getHalfClusterMap()) newClus->setHalfClusters( iter.first, iter.second );
+      for(int itrk=0; itrk<m_HFClusUCol[0]->getAssociatedTracks().size(); itrk++) newClus->addAssociatedTrack(m_HFClusUCol[0]->getAssociatedTracks()[itrk]);
+      newClus->setLinkedMCP(m_HFClusUCol[0]->getLinkedMCP());
+      newClus->setType(m_HFClusUCol[0]->getType());
+      m_bkCol.map_HalfCluster["bkHalfCluster"].push_back(newClus);
+
+      //Match the splitted HalfClusterU and HalfClusterV[icl]
+      std::shared_ptr<PandoraPlus::Calo3DCluster> tmp_clus = std::make_shared<PandoraPlus::Calo3DCluster>();
+      XYClusterMatchingL0(newClus.get(), m_HFClusVCol[icl], tmp_clus);
+      tmp_clusters.push_back(tmp_clus);
+    }
+  }
+
+  else if(NclusV==1){
+    double sumE_U = 0.;
+    for(int icl=0; icl<NclusU; icl++) sumE_U += m_HFClusUCol[icl]->getEnergy();
+
+    //Split the HalfClusterV into NclusU part
+    for(int icl=0; icl<NclusU; icl++){
+      std::shared_ptr<PandoraPlus::CaloHalfCluster> newClus = std::make_shared<PandoraPlus::CaloHalfCluster>();
+      double frac = m_HFClusUCol[icl]->getEnergy()/sumE_U;
+      for(int ish=0; ish<m_HFClusVCol[0]->getCluster().size(); ish++){
+        const PandoraPlus::Calo1DCluster* p_shower = m_HFClusVCol[0]->getCluster()[ish];
+
+        std::vector<const PandoraPlus::CaloUnit*> Bars; Bars.clear();
+        for(int ibar=0; ibar<p_shower->getCluster().size(); ibar++){
+          auto bar = p_shower->getCluster()[ibar]->Clone();
+          bar->setQ(bar->getQ1()*frac, bar->getQ2()*frac );
+          Bars.push_back(bar.get());
+          m_bkCol.map_BarCol["bkBar"].push_back(bar);
+        }
+
+        std::shared_ptr<PandoraPlus::Calo1DCluster> shower = std::make_shared<PandoraPlus::Calo1DCluster>();
+        shower->setBars(Bars);
+        shower->setSeed();
+        shower->setIDInfo();
+
+        newClus->addUnit(shower.get());
+        m_bkCol.map_1DCluster["bk1DCluster"].push_back( shower );
+      }
+      newClus->setHoughPars(m_HFClusVCol[0]->getHoughAlpha(), m_HFClusVCol[0]->getHoughRho());
+      newClus->setIntercept(m_HFClusVCol[0]->getHoughIntercept());
+      for(auto iter: m_HFClusVCol[0]->getHalfClusterMap()) newClus->setHalfClusters( iter.first, iter.second );
+      for(int itrk=0; itrk<m_HFClusVCol[0]->getAssociatedTracks().size(); itrk++) newClus->addAssociatedTrack(m_HFClusVCol[0]->getAssociatedTracks()[itrk]);
+      newClus->setLinkedMCP(m_HFClusVCol[0]->getLinkedMCP());
+      newClus->setType(m_HFClusVCol[0]->getType());
+      m_bkCol.map_HalfCluster["bkHalfCluster"].push_back(newClus);
+
+      //Match the splitted HalfClusterU and HalfClusterV[icl]
+      std::shared_ptr<PandoraPlus::Calo3DCluster> tmp_clus = std::make_shared<PandoraPlus::Calo3DCluster>();
+      XYClusterMatchingL0(m_HFClusUCol[icl], newClus.get(), tmp_clus);
+      tmp_clusters.push_back(tmp_clus);
+
+    }
+  }
+
+  else{
+    vector<vector<double>> Chi2Matrix(NclusV, vector<double>(NclusU, 0.));
+    std::vector< std::pair<int, int> > Chi2MatrixOrder; Chi2MatrixOrder.clear();
+    //Calculate the chi2 matrix with energy
+    Chi2MatrixCalculation(m_HFClusUCol, m_HFClusVCol, Chi2Matrix, Chi2MatrixOrder);
+
+//cout<<"Print chi2 matrix "<<endl;
+//for (const auto& row : Chi2Matrix) {
+//    for (const double& val : row) {
+//        std::cout << val << " ";
+//    }
+//    std::cout << std::endl;
+//}
+
+    //Calculate the initial pattern matrix with track + neighbor
+    vector<vector<int>> PatternMatrix(NclusV, vector<int>(NclusU, 0));
+    PatternMatrixCalculation(m_HFClusUCol, m_HFClusVCol, PatternMatrix);
+
+    //If the pattern is empty: use chi2 map to determine the first element.
+    bool isEmpty = true;
+    for(int i=0; i<NclusV && isEmpty; i++){
+      for(int j=0; j<NclusU; j++){
+        if(PatternMatrix[i][j]==1) isEmpty = false;
+        break;
+    }}
+    if(isEmpty){
+      for(int ipoint=0; ipoint<Chi2MatrixOrder.size(); ipoint++){
+        if(PatternMatrix[Chi2MatrixOrder[ipoint].first][Chi2MatrixOrder[ipoint].second]==0 ){
+          PatternMatrix[Chi2MatrixOrder[ipoint].first][Chi2MatrixOrder[ipoint].second] = 1;
+          break;
+        }
+      }
+    }
+
+//cout<<"Print initial pattern matrix"<<endl;
+//for (const auto& row : PatternMatrix) {
+//    for (const int& val : row) {
+//        std::cout << val << " ";
+//    }
+//    std::cout << std::endl;
+//}
+
+    //Initialize
+    double min_residual = 9999;
+    int index = -1;
+    std::vector<double> Eij(NclusU*NclusV, 0.);
+    std::vector<std::vector<int>> ParameterMatrix;
+    std::vector<double> En_clusters; En_clusters.clear();
+    std::vector<double> map_residual; map_residual.clear();
+    std::vector<vector<vector<int>> > map_pattern; map_pattern.clear();
+    std::vector<std::vector<double>> map_Eij; map_Eij.clear();
+    for(int icl=0; icl<NclusV; icl++) En_clusters.push_back( m_HFClusVCol[icl]->getEnergy() );
+    for(int icl=0; icl<NclusU; icl++) En_clusters.push_back( m_HFClusUCol[icl]->getEnergy() );
+
+    //Loop up to NclusV+NclusU
+    for(int iter=0; iter<NclusV+NclusU-1; iter++){
+      //Modify the chi2 map with pattern matrix
+      bool modified = false;
+      bool hasEmptyRow = false;
+      bool hasEmptyColumn = false; 
+      for (size_t i = 0; i < PatternMatrix.size(); ++i) {
+          int rowSum = 0;
+          for (size_t j = 0; j < PatternMatrix[i].size(); ++j) {
+              rowSum += PatternMatrix[i][j];
+          }
+          if(rowSum==0){
+            hasEmptyRow = true;
+            break;
+          }
+      }
+      for (size_t j = 0; j < PatternMatrix[0].size(); ++j) {
+          int colSum = 0;
+          for (size_t i = 0; i < PatternMatrix.size(); ++i) {
+              colSum += PatternMatrix[i][j];
+          }
+          if(colSum==0){
+            hasEmptyColumn = true;
+            break;
+          }
+      }
+
+      vector<vector<double>> Chi2Matrix_copy; 
+      if(hasEmptyRow && hasEmptyColumn){
+
+        Chi2Matrix_copy = Chi2Matrix;
+
+        for (size_t i = 0; i < PatternMatrix.size(); ++i) {
+            int rowSum = 0;
+            for (size_t j = 0; j < PatternMatrix[i].size(); ++j) {
+                rowSum += PatternMatrix[i][j];
+            }
+            if (rowSum != 0) {
+                Chi2Matrix_copy[i] = vector<double>(PatternMatrix[0].size(), 9999.);
+                modified = true;
+            }
+        }
+        for (size_t j = 0; j < PatternMatrix[0].size(); ++j) {
+            int colSum = 0;
+            for (size_t i = 0; i < PatternMatrix.size(); ++i) {
+                colSum += PatternMatrix[i][j];
+            }
+            if (colSum != 0) {
+                for (size_t i = 0; i < PatternMatrix.size(); ++i) {
+                    Chi2Matrix_copy[i][j] = 9999.;
+                }
+                modified = true;
+            }
+        }
+      }
+      else if(hasEmptyRow || hasEmptyColumn){
+        Chi2Matrix_copy = vector<vector<double>>(NclusV, vector<double>(NclusU, 9999.));
+        for (size_t i = 0; i < PatternMatrix.size(); ++i) {
+            int rowSum = 0;
+            for (size_t j = 0; j < PatternMatrix[i].size(); ++j) {
+                rowSum += PatternMatrix[i][j];
+            }
+            if (rowSum == 0) {
+                Chi2Matrix_copy[i] = Chi2Matrix[i];
+                modified = true;
+            }
+        }
+
+        for (size_t j = 0; j < PatternMatrix[0].size(); ++j) {
+            int colSum = 0;
+            for (size_t i = 0; i < PatternMatrix.size(); ++i) {
+                colSum += PatternMatrix[i][j];
+            }
+            if (colSum == 0) {
+                for (size_t i = 0; i < PatternMatrix.size(); ++i) {
+                    Chi2Matrix_copy[i][j] = Chi2Matrix[i][j];
+                }
+                modified = true;
+            }
+        }
+      }
+      else Chi2Matrix_copy = Chi2Matrix;
+    
+
+//cout<<"In iteration #"<<iter<<": Print updated chi2 matrix"<<endl;
+//for (const auto& row : Chi2Matrix_copy) {
+//    for (const double& val : row) {
+//        std::cout << val << " ";
+//    }
+//    std::cout << std::endl;
+//}
+
+      std::vector< std::pair<int, int> > Chi2MatrixOrder_copy;
+      if(modified){
+        std::vector<std::pair<double, int>> indices;
+        for(size_t i = 0; i < Chi2Matrix_copy.size(); ++i) {
+          for (size_t j = 0; j < Chi2Matrix_copy[i].size(); ++j) {
+            indices.push_back(std::make_pair(Chi2Matrix_copy[i][j], i * Chi2Matrix_copy[i].size() + j));
+          }
+        }
+        std::sort(indices.begin(), indices.end());
+        for(auto &iter1: indices)
+          Chi2MatrixOrder_copy.push_back( std::make_pair( iter1.second/Chi2Matrix_copy[0].size(), iter1.second%Chi2Matrix_copy[0].size() ) );
+      }
+      else
+        Chi2MatrixOrder_copy = Chi2MatrixOrder;
+
+
+      //Update the pattern matrix with chi2 map
+      bool updateFlag = false;
+      if(iter>0){
+        for(int ipoint=0; ipoint<Chi2MatrixOrder_copy.size(); ipoint++){
+          if(PatternMatrix[Chi2MatrixOrder_copy[ipoint].first][Chi2MatrixOrder_copy[ipoint].second]==0 ){
+            PatternMatrix[Chi2MatrixOrder_copy[ipoint].first][Chi2MatrixOrder_copy[ipoint].second] = 1;
+            updateFlag = true;
+            break;
+          }
+        }
+      }
+      if(iter>0 && !updateFlag){
+        //std::cout<<"ERROR: Already loop for all pattern element. Break. "<<std::endl;
+        break;
+      }
+
+//cout<<"  Print new pattern matrix "<<endl;
+//for (const auto& row : PatternMatrix) {
+//    for (const int& val : row) {
+//        std::cout << val << " ";
+//    }
+//    std::cout << std::endl;
+//}
+
+      //Re-calculate the parameter matrix, simplify it with pattern matrix
+      ParameterMatrix.clear(); Eij.clear();
+      Eij = std::vector<double>(NclusU*NclusV, 0.);
+      ParameterMatrixCalculation(NclusV, NclusU, ParameterMatrix);
+      SimplityMatrix(ParameterMatrix, Eij, PatternMatrix);
+
+      //Recalculate the residual
+      double residual = SolveMatrix(NclusU, NclusV, ParameterMatrix, Eij, En_clusters);
+//cout<<"  In iteration "<<iter<<": residual = "<<residual<<endl;
+
+      if(residual<min_residual){
+        min_residual = residual;
+        index = iter;
+      }
+      map_residual.push_back(residual);
+      map_Eij.push_back(Eij);
+      map_pattern.push_back(PatternMatrix);
+    }
+
+//cout<<"Minimal residual = "<<min_residual<<", index "<<index<<endl;
+//cout<<"Chosen pattern matrix "<<endl;
+//for (const auto& row : map_pattern[index]) {
+//    for (const int& val : row) {
+//        std::cout << val << " ";
+//    }
+//    std::cout << std::endl;
+//}
+
+    //Derive final energy matrix
+    vector<vector<double>> EnergyMatrix(NclusV, vector<double>(NclusU, 0));
+    int icount = 0;
+    for(int icl=0; icl<NclusV; icl++){
+      for(int jcl=0; jcl<NclusU; jcl++){
+        if(map_pattern[index][icl][jcl]==1){
+          EnergyMatrix[icl][jcl] = map_Eij[index][icount];
+          icount++;
+        }
+      }
+    }
+//cout<<"Calculated energy matrix"<<endl;
+//for (const auto& row : EnergyMatrix) {
+//    for (const double& val : row) {
+//        std::cout << val << " ";
+//    }
+//    std::cout << std::endl;
+//}
+
+    //Create Calo3DClusters with the result
+    ClusterBuilding(EnergyMatrix, m_HFClusUCol, m_HFClusVCol, tmp_clusters);
+  }
+
+  //Clean empty Calo3DClusters
+  for(int ic=0; ic<tmp_clusters.size(); ic++){
+    if( !tmp_clusters[ic].get() || tmp_clusters[ic]->getEnergy()==0 || isnan(tmp_clusters[ic]->getShowerCenter().x()) ){
+      tmp_clusters.erase(tmp_clusters.begin()+ic);
+      ic--;
+    }
+  }
+
+  m_clusterCol.insert( m_clusterCol.end(), tmp_clusters.begin(), tmp_clusters.end() );
+  tmp_clusters.clear();
+
+  return StatusCode::SUCCESS;
+}
+
+
+StatusCode EnergyTimeMatchingAlg::PatternMatrixCalculation( std::vector<const PandoraPlus::CaloHalfCluster*> m_HFClusUCol,
+                                                            std::vector<const PandoraPlus::CaloHalfCluster*> m_HFClusVCol,
+                                                            vector<vector<int>>& matrix )
+{
+
+  const int NclusU = m_HFClusUCol.size();
+  const int NclusV = m_HFClusVCol.size();  
+
+  if(NclusU==0 || NclusV==0){
+    std::cout<<"ERROR: empty HalfCluster input. Check! "<<std::endl;
+    matrix.clear();
+    return StatusCode::SUCCESS;
+  }
+  else if(NclusU<=1 || NclusV<=1){
+    for(int icl=0; icl<NclusV; icl++){
+      for(int jcl=0; jcl<NclusU; jcl++) matrix[icl][jcl] = 1;
+    }
+    return StatusCode::SUCCESS;
+  }
+  else{
+    //Fill the matrix with track info
+    std::vector<const PandoraPlus::Track*> m_linkedTrk; m_linkedTrk.clear();
+
+    for(int icl=0; icl<m_HFClusUCol.size(); ++icl){
+      std::vector<const PandoraPlus::Track*> tmp_linkedTrk = m_HFClusUCol[icl]->getAssociatedTracks();
+      if(tmp_linkedTrk.size()==0) continue;
+      m_linkedTrk.insert(m_linkedTrk.end(), tmp_linkedTrk.begin(), tmp_linkedTrk.end() );
+    }
+    for(int icl=0; icl<m_HFClusVCol.size(); ++icl){
+      std::vector<const PandoraPlus::Track*> tmp_linkedTrk = m_HFClusVCol[icl]->getAssociatedTracks();
+      if(tmp_linkedTrk.size()==0) continue;
+      m_linkedTrk.insert(m_linkedTrk.end(), tmp_linkedTrk.begin(), tmp_linkedTrk.end());
+    }
+
+    for(int itrk=0; itrk<m_linkedTrk.size(); itrk++){
+      int indexU = -1;
+      int indexV = -1;
+      for(int icl=0; icl<m_HFClusUCol.size(); ++icl){
+        std::vector<const PandoraPlus::Track*> m_linkedTrkU = m_HFClusUCol[icl]->getAssociatedTracks();
+        if(m_linkedTrkU.size()==0) continue;
+        if(find(m_linkedTrkU.begin(), m_linkedTrkU.end(), m_linkedTrk[itrk])!=m_linkedTrkU.end()){
+          indexU = icl;
+          break;
+        }
+      }
+
+      for(int icl=0; icl<m_HFClusVCol.size(); ++icl){
+        std::vector<const PandoraPlus::Track*> m_linkedTrkV = m_HFClusVCol[icl]->getAssociatedTracks();
+        if(m_linkedTrkV.size()==0) continue;
+        if(find(m_linkedTrkV.begin(), m_linkedTrkV.end(), m_linkedTrk[itrk])!=m_linkedTrkV.end()){
+          indexV = icl;
+          break;
+        }
+      }
+
+      if(indexU>=0 && indexV>=0) matrix[indexV][indexU] = 1;
+    }
+
+    //Fill the matrix with neighbor info
+    for(int icl=0; icl<m_HFClusUCol.size(); ++icl){
+      if(m_HFClusUCol[icl]->getHalfClusterCol("CousinCluster").size()==0) continue;
+      for(int jcl=0; jcl<m_HFClusVCol.size(); ++jcl){
+        if(m_HFClusVCol[jcl]->getHalfClusterCol("CousinCluster").size()==0) continue;
+
+        std::vector<const PandoraPlus::CaloHalfCluster*> m_cousinU = m_HFClusUCol[icl]->getHalfClusterCol("CousinCluster");
+        std::vector<const PandoraPlus::CaloHalfCluster*> m_cousinV = m_HFClusVCol[jcl]->getHalfClusterCol("CousinCluster");
+
+        //Make map: <towerID, (E_clU - E_clV)/mean(E_clU, E_clV)  >
+        std::map<std::vector<int>, pair<float, float>> map_pairE; map_pairE.clear();
+        map_pairE[m_HFClusUCol[icl]->getTowerID()[0]] = make_pair(m_HFClusUCol[icl]->getEnergy(), m_HFClusVCol[jcl]->getEnergy());
+        for(int ics=0; ics<m_cousinU.size(); ++ics){
+        for(int jcs=0; jcs<m_cousinV.size(); ++jcs){
+          if(m_cousinU[ics]->getTowerID()[0] == m_cousinV[jcs]->getTowerID()[0])
+            map_pairE[ m_cousinU[ics]->getTowerID()[0] ] = make_pair(m_cousinU[ics]->getEnergy(), m_cousinV[jcs]->getEnergy());
+
+        }}
+        if(map_pairE.size()<=1) continue; //No common tower cousin clusters.
+
+        bool isLink = true;
+        float totE_U = 0.;
+        float totE_V = 0.;
+        for(auto iter: map_pairE){
+          float deltaE = 2*fabs(iter.second.first - iter.second.second)/(iter.second.first + iter.second.second);
+          totE_U += iter.second.first;
+          totE_V += iter.second.second;
+          //In tower: if too large difference between U/V then do not link.
+          if(deltaE > settings.map_floatPars["th_UVdeltaE"]) { isLink = false; break; }
+          //if(iter.second.first>1 && iter.second.second>1 && deltaE > settings.map_floatPars["th_UVdeltaE1"]){ isLink = false; break; } //High energy: deltaE
+          //else if(iter.second.first/iter.second.second > 10 || iter.second.first/iter.second.second<0.1) { isLink = false; break; }  //Low energy: 0.1<E_u/E_v<10.
+        }
+        if( 2*fabs(totE_U-totE_V)/(totE_U+totE_V)>settings.map_floatPars["th_UVdeltaE"] ) isLink=false;
+
+        if(isLink) matrix[jcl][icl] = 1;
+      }
+    }
+  }
+
+  return StatusCode::SUCCESS;
+}
+
+
+StatusCode EnergyTimeMatchingAlg::Chi2MatrixCalculation( std::vector<const PandoraPlus::CaloHalfCluster*> m_HFClusUCol,
+                                                         std::vector<const PandoraPlus::CaloHalfCluster*> m_HFClusVCol,
+                                                         vector<vector<double>>& matrix,
+                                                         std::vector< std::pair<int, int> >& chi2order)
+{
+
+  const int NclusU = m_HFClusUCol.size();
+  const int NclusV = m_HFClusVCol.size();
+//cout<<"  Chi2MatrixCalculation: input cluster size "<<NclusU<<", "<<NclusV<<endl;
+
+  if(NclusU==0 || NclusV==0){
+    std::cout<<"ERROR: empty HalfCluster input. Check! "<<std::endl;
+    matrix.clear();
+    chi2order.clear();
+    return StatusCode::SUCCESS;
+  }
+  else if(NclusU<=1 || NclusV<=1){
+    for(int icl=0; icl<NclusV; icl++){
+      for(int jcl=0; jcl<NclusU; jcl++) matrix[icl][jcl] = 0.;
+    }
+    chi2order.clear();
+    return StatusCode::SUCCESS;
+  }
+  else{ 
+//cout<<" Print sum chi2 matrix"<<endl;
+//for (const auto& row : matrix) {
+//    for (const double& val : row) {
+//        std::cout << val << " ";
+//    }
+//    std::cout << std::endl;
+//}
+
+    int Nlayer = PandoraPlus::CaloUnit::Nlayer; 
+    for(int il=0; il<Nlayer; il++){
+      std::vector<std::vector<const PandoraPlus::Calo1DCluster*>> tmp_showerU(NclusU);
+      std::vector<std::vector<const PandoraPlus::Calo1DCluster*>> tmp_showerV(NclusV);
+      for(int icl=0; icl<NclusU; icl++){
+        for(int jcl=0; jcl<NclusV; jcl++){
+          tmp_showerU[icl] = m_HFClusUCol[icl]->getClusterInLayer(il+1);
+          tmp_showerV[jcl] = m_HFClusVCol[jcl]->getClusterInLayer(il+1);
+        }
+      }
+//cout<<"  In layer #"<<il<<": cluster size "<<tmp_showerU.size()<<", "<<tmp_showerV.size()<<endl;
+      vector<vector<double>> chi2Map;
+      chi2Map = GetClusterChi2Map(tmp_showerU, tmp_showerV);
+
+//cout<<" Print chi2 matrix"<<endl;
+//for (const auto& row : chi2Map) {
+//    for (const double& val : row) {
+//        std::cout << val << " ";
+//    }
+//    std::cout << std::endl;
+//}
+
+      for(int icl=0; icl<NclusU; icl++){
+        for(int jcl=0; jcl<NclusV; jcl++){
+          matrix[jcl][icl] += chi2Map[jcl][icl];
+          //Use 1/E as weight: 
+          //matrix[jcl][icl] += chi2Map[jcl][icl]*( 1./m_HFClusUCol[icl]->getEnergy() + 1./m_HFClusVCol[jcl]->getEnergy() );
+        }
+      }
+//cout<<" Print sum chi2 matrix"<<endl;
+//for (const auto& row : matrix) {
+//    for (const double& val : row) {
+//        std::cout << val << " ";
+//    }
+//    std::cout << std::endl;
+//}
+    }
+
+    
+    //Get the order of chi2 matrix map
+    std::vector<std::pair<double, int>> indices;
+    for(size_t i = 0; i < matrix.size(); ++i) {
+      for (size_t j = 0; j < matrix[i].size(); ++j) {
+        indices.push_back(std::make_pair(matrix[i][j], i * matrix[i].size() + j));
+      }
+    }
+    std::sort(indices.begin(), indices.end());
+//cout<<"  matrix size: "<<matrix.size()<<", "<<matrix[0].size()<<endl;
+    for(auto &iter: indices)
+      chi2order.push_back( std::make_pair( iter.second/matrix[0].size(), iter.second%matrix[0].size() ) );
+
+  }
+
+  return StatusCode::SUCCESS;
+}
+
+vector<vector<double>> EnergyTimeMatchingAlg::GetClusterChi2Map( std::vector<std::vector<const PandoraPlus::Calo1DCluster*>>& barShowerUCol,
+                                                                 std::vector<std::vector<const PandoraPlus::Calo1DCluster*>>& barShowerVCol )
+{
+
+
+  const int NclusU = barShowerUCol.size();
+  const int NclusV = barShowerVCol.size();
+  vector<vector<double>> chi2map_E(NclusV, vector<double>(NclusU, 0.));
+  vector<vector<double>> chi2map_tx(NclusV, vector<double>(NclusU, 0.));
+  vector<vector<double>> chi2map_ty(NclusV, vector<double>(NclusU, 0.));
+  vector<vector<double>> chi2map(NclusV, vector<double>(NclusU, 0.));
+
+  if(NclusU==0 || NclusV==0) return chi2map;
+
+  double wi_E = settings.map_floatPars["chi2Wi_E"]/(settings.map_floatPars["chi2Wi_E"] + settings.map_floatPars["chi2Wi_T"]);
+  double wi_T = settings.map_floatPars["chi2Wi_T"]/(settings.map_floatPars["chi2Wi_E"] + settings.map_floatPars["chi2Wi_T"]);
+
+  TVector3 m_vec(0,0,0);
+  double rotAngle = -999;
+  TVector3 Ctower(0,0,0);
+  for(int ish=0; ish<barShowerUCol.size(); ish++){
+    if(barShowerUCol[ish].size()==0) continue;
+    rotAngle = -(barShowerUCol[ish][0]->getBars())[0]->getModule()*TMath::TwoPi()/PandoraPlus::CaloUnit::Nmodule;
+    Ctower.SetX( (barShowerUCol[ish][0]->getBars())[0]->getPosition().x() );
+    Ctower.SetY( (barShowerUCol[ish][0]->getBars())[0]->getPosition().y() );
+  }
+  for(int ish=0; ish<barShowerVCol.size(); ish++){
+    if(barShowerVCol[ish].size()==0) continue;
+    Ctower.SetZ( (barShowerVCol[ish][0]->getBars())[0]->getPosition().z() );
+  }
+  Ctower.RotateZ(rotAngle);
+
+  for(int ix=0;ix<NclusU;ix++){
+  for(int iy=0;iy<NclusV;iy++){
+    std::vector<const PandoraPlus::Calo1DCluster*> clusterU = barShowerUCol[ix];
+    std::vector<const PandoraPlus::Calo1DCluster*> clusterV = barShowerVCol[iy];
+
+    if(clusterU.size()==0){
+      double totE_V = 0;
+      for(int icy=0; icy<clusterV.size(); icy++) totE_V += clusterV[icy]->getEnergy();
+      chi2map[iy][ix] = pow(totE_V/settings.map_floatPars["sigmaE"], 2);
+      continue;
+    }
+    if(clusterV.size()==0){
+      double totE_U = 0;
+      for(int icy=0; icy<clusterU.size(); icy++) totE_U += clusterU[icy]->getEnergy();
+      chi2map[iy][ix] = pow(totE_U/settings.map_floatPars["sigmaE"], 2);
+      continue;
+    }
+
+    double min_chi2E = 999;
+    double min_chi2tx = 999;
+    double min_chi2ty = 999;
+
+    for(int icx=0; icx<clusterU.size(); icx++){
+    for(int icy=0; icy<clusterV.size(); icy++){
+      const PandoraPlus::Calo1DCluster* showerX = clusterU[icx];
+      const PandoraPlus::Calo1DCluster* showerY = clusterV[icy];
+
+      double Ex = showerX->getEnergy();
+      double Ey = showerY->getEnergy();
+      double chi2_E = pow(fabs(Ex-Ey)/settings.map_floatPars["sigmaE"], 2);
+      double PosTx = C*(showerY->getT1()-showerY->getT2())/(2*settings.map_floatPars["nMat"]) + showerY->getPos().z();
+      double chi2_tx = pow( fabs(PosTx-showerX->getPos().z())/settings.map_floatPars["sigmaPos"], 2 );
+
+      double PosTy = C*(showerX->getT1()-showerX->getT2())/(2*settings.map_floatPars["nMat"]);
+      m_vec = showerY->getPos();
+      m_vec.RotateZ(rotAngle);
+      double chi2_ty = pow( fabs(PosTy - (m_vec-Ctower).x() )/settings.map_floatPars["sigmaPos"], 2);
+
+      if(chi2_E<min_chi2E) min_chi2E=chi2_E;
+      if(chi2_tx<min_chi2tx) min_chi2tx=chi2_tx;
+      if(chi2_ty<min_chi2ty) min_chi2ty=chi2_ty;
+    }}
+
+    chi2map_E[iy][ix] = min_chi2E;
+    chi2map_tx[iy][ix] = min_chi2tx;
+    chi2map_ty[iy][ix] = min_chi2ty;
+    chi2map[iy][ix] = chi2map_E[iy][ix]*wi_E + (chi2map_tx[iy][ix]+chi2map_ty[iy][ix])*wi_T ;
+//cout<<"  chi2 value "<<min_chi2E<<endl;
+
+  }}
+
+  return chi2map;
+}
+
+
+StatusCode EnergyTimeMatchingAlg::ParameterMatrixCalculation(const int& M, const int& N, vector<vector<int>>& parMatrix){
+
+  parMatrix = vector<vector<int>>(M + N, std::vector<int>(M * N, 0));
+  //Fill first M raws
+  for (int i = 0; i < M; i++) {
+      for (int j = i * N; j < (i + 1) * N; j++) {
+          parMatrix[i][j] = 1;
+      }
+  }
+  //Fill second N raws
+  for(int i=M; i<N+M; i++){
+    for(int j=0; j<M; j++)
+      parMatrix[i][i-M+j*N] = 1;
+  }
+
+  return StatusCode::SUCCESS; 
+}
+
+
+StatusCode EnergyTimeMatchingAlg::SimplityMatrix(vector<vector<int>>& parMatrix, vector<double>& Eij, vector<vector<int>>& pattern){
+  int M = pattern.size();
+  int N = pattern[0].size();
+
+  int count = 0;
+  for(int i=0; i<M; i++){
+    for(int j=0; j<N; j++){
+      int k = i*N+j-count;
+      if(pattern[i][j]==0){
+          for (int a = 0; a < M + N; a++) {
+            parMatrix[a].erase(parMatrix[a].begin() + k);
+          }
+          Eij.erase(Eij.begin()+k);
+          count++;
+      }
+    }
+  }
+
+  return StatusCode::SUCCESS;
+}
+
+
+double EnergyTimeMatchingAlg::SolveMatrix(const int& M, const int& N, vector<vector<int>>& parMatrix, vector<double>& Eij, vector<double>& En_clusters){
+
+
+  leastSquares(parMatrix, En_clusters, Eij);
+  vector<double> residual; residual.clear();
+
+  for (int i = 0; i < parMatrix.size(); i++) {
+      double r = 0;
+      for (int j = 0; j < parMatrix[i].size(); j++) {
+          r += parMatrix[i][j] * Eij[j];
+      }
+      r -= En_clusters[i];
+      residual.push_back(r);
+  }
+
+  double residualSum = 0;
+  for (int i = 0; i < residual.size(); i++) {
+      residualSum += residual[i] * residual[i];
+  }
+
+  return residualSum;
+}
+
+
+StatusCode EnergyTimeMatchingAlg::leastSquares(const vector<vector<int>>& A, const vector<double>& b, vector<double>& x){
+  int m = A.size();
+  int n = A[0].size();
+
+  vector<vector<double>> Q, R;
+  vector<vector<double>> doubleA(m, vector<double>(n));
+  for(int i=0; i<m; i++){
+    for(int j=0; j<n; j++)
+      doubleA[i][j] = (double)A[i][j];
+  }
+
+  //qrDecomposition(doubleA, Q, R);
+  Q = vector<vector<double>>(m, vector<double>(n, 0));
+  R = vector<vector<double>>(n, vector<double>(n, 0));
+  vector<vector<double>> A_copy = doubleA;
+  for (int j = 0; j < n; j++) {
+      double colNorm = 0.0;
+      for (int i = 0; i < m; i++) {
+          colNorm += A_copy[i][j] * A_copy[i][j];
+      }
+      colNorm = sqrt(colNorm);
+
+      R[j][j] = colNorm;
+
+      for (int i = 0; i < m; i++) {
+          Q[i][j] = A_copy[i][j] / colNorm;
+      }
+
+      for (int k = j + 1; k < n; k++) {
+          double dotProduct = 0.0;
+          for (int i = 0; i < m; i++) {
+              dotProduct += Q[i][j] * A_copy[i][k];
+          }
+          R[j][k] = dotProduct;
+          for (int i = 0; i < m; i++) {
+              A_copy[i][k] -= Q[i][j] * dotProduct;
+          }
+      }
+  }
+
+
+  vector<double> Qt_b(n, 0);
+  for (int i = 0; i < m; i++) {
+      for (int j = 0; j < n; j++) {
+          Qt_b[j] += Q[i][j] * b[i];
+      }
+  }
+
+  x = vector<double>(n, 0);
+  for (int i = n - 1; i >= 0; i--) {
+      double sum = 0.0;
+      for (int j = i + 1; j < n; j++) {
+          sum += R[i][j] * x[j];
+      }
+      if(R[i][i]!=0) x[i] = (Qt_b[i] - sum) / R[i][i];
+      if(x[i]<0) x[i] = 0.;
+  }
+
+  return StatusCode::SUCCESS;
+}
+
+StatusCode EnergyTimeMatchingAlg::ClusterBuilding( std::vector<std::vector<double>> Ematrix,
+                                                   std::vector<const PandoraPlus::CaloHalfCluster*>& m_HFClusUCol,
+                                                   std::vector<const PandoraPlus::CaloHalfCluster*>& m_HFClusVCol,
+                                                   std::vector<std::shared_ptr<PandoraPlus::Calo3DCluster>>& m_clusterCol)
+{
+
+  int NclusU = m_HFClusUCol.size();
+  int NclusV = m_HFClusVCol.size();
+  if(NclusU==0 || NclusV==0){
+    std::cout<<"ERROR: Empty readin HalfCluster! "<<NclusU<<", "<<NclusV<<endl;
+  }
+  else if(NclusU==1 && NclusV==1){
+    std::shared_ptr<PandoraPlus::Calo3DCluster> tmp_clus = std::make_shared<PandoraPlus::Calo3DCluster>();
+    XYClusterMatchingL0(m_HFClusUCol[0], m_HFClusVCol[0], tmp_clus);
+    m_clusterCol.push_back(tmp_clus);
+  }
+  else{
+    std::vector<double> sumE_U(NclusU, 0.); //column
+    std::vector<double> sumE_V(NclusV, 0.); //raw
+    for(int icl=0; icl<NclusV; icl++){
+      for(int jcl=0; jcl<NclusU; jcl++){
+        sumE_V[icl] += Ematrix[icl][jcl];
+        sumE_U[jcl] += Ematrix[icl][jcl];
+      }
+    }
+
+    for(int icl=0; icl<NclusV; icl++){
+      for(int jcl=0; jcl<NclusU; jcl++){
+
+        double fracU = Ematrix[icl][jcl]/sumE_U[jcl];
+        if(fracU==0) continue;
+        std::shared_ptr<PandoraPlus::CaloHalfCluster> newClusU = std::make_shared<PandoraPlus::CaloHalfCluster>();
+        if(fracU==1) newClusU = m_HFClusUCol[jcl]->Clone();
+        else{
+          for(int ish=0; ish<m_HFClusUCol[jcl]->getCluster().size(); ish++){
+            const PandoraPlus::Calo1DCluster* p_shower = m_HFClusUCol[jcl]->getCluster()[ish];
+   
+            std::vector<const PandoraPlus::CaloUnit*> Bars; Bars.clear();
+            for(int ibar=0; ibar<p_shower->getCluster().size(); ibar++){
+              auto bar = p_shower->getCluster()[ibar]->Clone();
+              bar->setQ(bar->getQ1()*fracU, bar->getQ2()*fracU );
+              Bars.push_back(bar.get());
+              m_bkCol.map_BarCol["bkBar"].push_back(bar);
+            }
+   
+            std::shared_ptr<PandoraPlus::Calo1DCluster> shower = std::make_shared<PandoraPlus::Calo1DCluster>();
+            shower->setBars(Bars);
+            shower->setSeed();
+            shower->setIDInfo();
+   
+            newClusU->addUnit(shower.get());
+            m_bkCol.map_1DCluster["bk1DCluster"].push_back( shower );
+          }
+          newClusU->setHoughPars(m_HFClusUCol[jcl]->getHoughAlpha(), m_HFClusUCol[jcl]->getHoughRho());
+          newClusU->setIntercept(m_HFClusUCol[jcl]->getHoughIntercept());
+          for(auto iter: m_HFClusUCol[jcl]->getHalfClusterMap()) newClusU->setHalfClusters( iter.first, iter.second );
+          for(int itrk=0; itrk<m_HFClusUCol[jcl]->getAssociatedTracks().size(); itrk++) newClusU->addAssociatedTrack(m_HFClusUCol[jcl]->getAssociatedTracks()[itrk]);
+          newClusU->setLinkedMCP(m_HFClusUCol[jcl]->getLinkedMCP());
+          newClusU->setType(m_HFClusUCol[jcl]->getType());
+        }
+
+        std::shared_ptr<PandoraPlus::CaloHalfCluster> newClusV = std::make_shared<PandoraPlus::CaloHalfCluster>();
+        double fracV = Ematrix[icl][jcl]/sumE_V[icl];
+        if(fracV==0) continue;
+        if(fracV==1) newClusV = m_HFClusVCol[icl]->Clone();
+        else{
+          for(int ish=0; ish<m_HFClusVCol[icl]->getCluster().size(); ish++){
+            const PandoraPlus::Calo1DCluster* p_shower = m_HFClusVCol[icl]->getCluster()[ish];
+   
+            std::vector<const PandoraPlus::CaloUnit*> Bars; Bars.clear();
+            for(int ibar=0; ibar<p_shower->getCluster().size(); ibar++){
+              auto bar = p_shower->getCluster()[ibar]->Clone();
+              bar->setQ(bar->getQ1()*fracV, bar->getQ2()*fracV );
+              Bars.push_back(bar.get());
+              m_bkCol.map_BarCol["bkBar"].push_back(bar);
+            }
+   
+            std::shared_ptr<PandoraPlus::Calo1DCluster> shower = std::make_shared<PandoraPlus::Calo1DCluster>();
+            shower->setBars(Bars);
+            shower->setSeed();
+            shower->setIDInfo();
+   
+            newClusV->addUnit(shower.get());
+            m_bkCol.map_1DCluster["bk1DCluster"].push_back( shower );
+          }
+          newClusV->setHoughPars(m_HFClusVCol[icl]->getHoughAlpha(), m_HFClusVCol[icl]->getHoughRho());
+          newClusV->setIntercept(m_HFClusVCol[icl]->getHoughIntercept());
+          for(auto iter: m_HFClusVCol[icl]->getHalfClusterMap()) newClusV->setHalfClusters( iter.first, iter.second );
+          for(int itrk=0; itrk<m_HFClusVCol[icl]->getAssociatedTracks().size(); itrk++) newClusV->addAssociatedTrack(m_HFClusVCol[icl]->getAssociatedTracks()[itrk]);
+          newClusV->setLinkedMCP(m_HFClusVCol[icl]->getLinkedMCP());
+          newClusV->setType(m_HFClusVCol[icl]->getType());
+        }
+        m_bkCol.map_HalfCluster["bkHalfCluster"].push_back(newClusU);
+        m_bkCol.map_HalfCluster["bkHalfCluster"].push_back(newClusV);
+
+        std::shared_ptr<PandoraPlus::Calo3DCluster> tmp_clus = std::make_shared<PandoraPlus::Calo3DCluster>();
+        XYClusterMatchingL0(newClusU.get(), newClusV.get(), tmp_clus);
+        m_clusterCol.push_back(tmp_clus);
+    }}
+  }
+
+  return StatusCode::SUCCESS;
+}
+
+
+//Longitudinal cluster: 1*1
+StatusCode EnergyTimeMatchingAlg::XYClusterMatchingL0( const PandoraPlus::CaloHalfCluster* m_longiClU,
+                                                       const PandoraPlus::CaloHalfCluster* m_longiClV,
+                                                       std::shared_ptr<PandoraPlus::Calo3DCluster>& m_clus )
+{
+//cout<<"  Cluster matching for case: 1 * 1. Input HalfCluster En: "<<m_longiClU->getEnergy()<<", "<<m_longiClV->getEnergy()<<endl;
+//cout<<"  Print 1DShower En in HalfClusterU: "<<endl;
+//for(int i=0; i<m_longiClU->getCluster().size(); i++)
+//  cout<<m_longiClU->getCluster()[i]->getDlayer()<<'\t'<<m_longiClU->getCluster()[i]->getEnergy()<<endl;
+//cout<<"  Print 1DShower En in HalfClusterV: "<<endl;
+//for(int i=0; i<m_longiClV->getCluster().size(); i++)
+//  cout<<m_longiClV->getCluster()[i]->getDlayer()<<'\t'<<m_longiClV->getCluster()[i]->getEnergy()<<endl;
+
+
+  std::vector<int> layerindex; layerindex.clear();
+  std::map<int, std::vector<const PandoraPlus::Calo1DCluster*> > map_showersUinlayer; map_showersUinlayer.clear();
+  std::map<int, std::vector<const PandoraPlus::Calo1DCluster*> > map_showersVinlayer; map_showersVinlayer.clear();
+
+  for(int is=0; is<m_longiClU->getCluster().size(); is++){
+    int m_layer = m_longiClU->getCluster()[is]->getDlayer();
+    if( find( layerindex.begin(), layerindex.end(), m_layer )==layerindex.end() ) layerindex.push_back(m_layer);
+    map_showersUinlayer[m_layer].push_back(m_longiClU->getCluster()[is]);
+  }
+  for(int is=0; is<m_longiClV->getCluster().size(); is++){
+    int m_layer = m_longiClV->getCluster()[is]->getDlayer();
+    if( find( layerindex.begin(), layerindex.end(), m_layer )==layerindex.end() ) layerindex.push_back(m_layer);
+    map_showersVinlayer[m_layer].push_back(m_longiClV->getCluster()[is]);
+  }
+
+  for(int il=0; il<layerindex.size(); il++){
+    std::vector<const PandoraPlus::Calo1DCluster*> m_showerXcol = map_showersUinlayer[layerindex[il]];
+    std::vector<const PandoraPlus::Calo1DCluster*> m_showerYcol = map_showersVinlayer[layerindex[il]];
+
+//cout<<"  Print 1D showers in layer "<<layerindex[il]<<endl;
+//cout<<"  ShowerU size = "<<m_showerXcol.size()<<endl;
+//for(int a=0; a<m_showerXcol.size(); a++) printf("    #%d shower: En %.3f, address %p \n", a, m_showerXcol[a]->getEnergy(), m_showerXcol[a]);
+//cout<<"  ShowerV size = "<<m_showerYcol.size()<<endl;
+//for(int a=0; a<m_showerYcol.size(); a++) printf("    #%d shower: En %.3f, address %p \n", a, m_showerYcol[a]->getEnergy(), m_showerYcol[a]);
+
+    std::vector<PandoraPlus::Calo2DCluster*> m_showerinlayer; m_showerinlayer.clear();
+
+    if(m_showerXcol.size()==0 || m_showerYcol.size()==0) continue;
+    //else if(m_showerXcol.size()==0){
+    //  std::shared_ptr<PandoraPlus::Calo2DCluster> tmp_shower = std::make_shared<PandoraPlus::Calo2DCluster>();
+    //  GetMatchedShowerFromEmpty(m_showerYcol[0], m_longiClU, tmp_shower.get());
+    //  //m_showerinlayer.push_back(tmp_shower.get());
+    //  //m_bkCol.map_2DCluster["bk2DCluster"].push_back(tmp_shower);
+    //}
+    //else if(m_showerYcol.size()==0){
+    //  std::shared_ptr<PandoraPlus::Calo2DCluster> tmp_shower = std::make_shared<PandoraPlus::Calo2DCluster>();
+    //  GetMatchedShowerFromEmpty(m_showerXcol[0], m_longiClU, tmp_shower.get());
+    //}
+    else if(m_showerXcol.size()==1 && m_showerYcol.size()==1){
+      std::shared_ptr<PandoraPlus::Calo2DCluster> tmp_shower = std::make_shared<PandoraPlus::Calo2DCluster>();
+      GetMatchedShowersL0(m_showerXcol[0], m_showerYcol[0], tmp_shower.get());
+      m_showerinlayer.push_back(tmp_shower.get());
+      m_bkCol.map_2DCluster["bk2DCluster"].push_back(tmp_shower);
+    }
+    else if(m_showerXcol.size()==1) GetMatchedShowersL1(m_showerXcol[0], m_showerYcol, m_showerinlayer );
+    else if(m_showerYcol.size()==1) GetMatchedShowersL1(m_showerYcol[0], m_showerXcol, m_showerinlayer );
+    else{ std::cout<<"CAUSION in XYClusterMatchingL0: HFCluster has ["<<m_showerXcol.size()<<", "<<m_showerYcol.size()<<"] showers in layer "<<layerindex[il]<<std::endl; }
+
+//cout<<"    After matching: shower size = "<<m_showerinlayer.size()<<", Print showers: "<<endl;
+//for(int is=0; is<m_showerinlayer.size(); is++) printf("  Pos+E (%.3f, %.3f, %.3f, %.3f) \t", m_showerinlayer[is]->getPos().x(), m_showerinlayer[is]->getPos().y(), m_showerinlayer[is]->getPos().z(), m_showerinlayer[is]->getEnergy() );
+//cout<<endl;
+
+    for(int is=0; is<m_showerinlayer.size(); is++) m_clus->addUnit(m_showerinlayer[is]);
+  }
+
+  m_clus->setCaloHitsFrom2DCluster();
+  m_clus->addHalfClusterU( "LinkedLongiCluster", m_longiClU );
+  m_clus->addHalfClusterV( "LinkedLongiCluster", m_longiClV );
+  for(auto itrk : m_longiClU->getAssociatedTracks()){
+    for(auto jtrk : m_longiClV->getAssociatedTracks()){
+      if(itrk!=jtrk) continue;
+      if( find(m_clus->getAssociatedTracks().begin(), m_clus->getAssociatedTracks().end(), itrk)==m_clus->getAssociatedTracks().end() )
+        m_clus->addAssociatedTrack(itrk);
+  }}
+  m_clus->getLinkedMCPfromHFCluster("LinkedLongiCluster");
+
+  return StatusCode::SUCCESS;
+}
+
+
+StatusCode EnergyTimeMatchingAlg::GetMatchedShowersL0( const PandoraPlus::Calo1DCluster* barShowerU,
+                                                       const PandoraPlus::Calo1DCluster* barShowerV,
+                                                       PandoraPlus::Calo2DCluster* outsh )
+{
+
+  std::vector<const PandoraPlus::CaloHit*> m_digiCol; m_digiCol.clear();
+  int NbarsX = barShowerU->getBars().size();
+  int NbarsY = barShowerV->getBars().size();
+  if(NbarsX==0 || NbarsY==0){ std::cout<<"WARNING: empty DigiHitsCol returned!"<<std::endl; return StatusCode::SUCCESS; }
+  if(barShowerU->getTowerID().size()==0) { std::cout<<"WARNING:GetMatchedShowersL0  No TowerID in 1DCluster!"<<std::endl; return StatusCode::SUCCESS; }
+  //if(barShowerU->getTowerID().size()==0) { barShowerU->setIDInfo(); }
+
+  int _layer = barShowerU->getDlayer();
+  int _module = barShowerU->getTowerID()[0][0];
+  float rotAngle = -_module*TMath::TwoPi()/PandoraPlus::CaloUnit::Nmodule;
+
+  for(int ibar=0;ibar<NbarsX;ibar++){
+    double En_x = barShowerU->getBars()[ibar]->getEnergy();
+    TVector3 m_vecx = barShowerU->getBars()[ibar]->getPosition();
+    m_vecx.RotateZ(rotAngle);
+
+    for(int jbar=0;jbar<NbarsY;jbar++){
+      double En_y = barShowerV->getBars()[jbar]->getEnergy();
+      TVector3 m_vecy = barShowerV->getBars()[jbar]->getPosition();
+      m_vecy.RotateZ(rotAngle);
+
+      TVector3 p_hit(m_vecy.x(), (m_vecx.y()+m_vecy.y())/2., m_vecx.z() );
+      p_hit.RotateZ(-rotAngle);
+      double m_Ehit = En_x*En_y/barShowerV->getEnergy() + En_x*En_y/barShowerU->getEnergy();
+      //Create new CaloHit
+      std::shared_ptr<PandoraPlus::CaloHit> hit = std::make_shared<PandoraPlus::CaloHit>();
+      hit->setcellID(_module, _layer);
+      hit->setPosition(p_hit);
+      hit->setEnergy(m_Ehit);
+      m_digiCol.push_back(hit.get());
+      m_bkCol.map_CaloHit["bkHit"].push_back( hit );
+    }
+  }
+
+  outsh->addUnit( barShowerU );
+  outsh->addUnit( barShowerV );
+  outsh->setCaloHits( m_digiCol );
+//cout<<"    End output shower"<<endl;
+
+  return StatusCode::SUCCESS;
+}
+
+
+StatusCode EnergyTimeMatchingAlg::GetMatchedShowersL1( const PandoraPlus::Calo1DCluster* shower1,
+                                                       std::vector<const PandoraPlus::Calo1DCluster*>& showerNCol,
+                                                       std::vector<PandoraPlus::Calo2DCluster*>& outshCol )
+{
+//cout<<"  GetMatchedShowersL1: input shower size: 1 * "<<showerNCol.size()<<endl;
+  outshCol.clear();
+
+  int _slayer = shower1->getBars()[0]->getSlayer();
+
+  const int NshY = showerNCol.size();
+  double totE_shY = 0;
+  double EshY[NshY] = {0};
+  for(int is=0;is<NshY;is++){ EshY[is] = showerNCol[is]->getEnergy(); totE_shY += EshY[is]; }
+  for(int is=0;is<NshY;is++){
+    double wi_E = EshY[is]/totE_shY;
+    std::shared_ptr<PandoraPlus::Calo1DCluster> m_splitshower1 = std::make_shared<PandoraPlus::Calo1DCluster>();
+    m_bkCol.map_1DCluster["bk1DCluster"].push_back( m_splitshower1 );
+
+    std::shared_ptr<PandoraPlus::CaloUnit> m_wiseed = nullptr;
+    if(shower1->getSeeds().size()>0) m_wiseed = shower1->getSeeds()[0]->Clone();
+    else{ cout<<"ERROR: Input shower has no seed! Check! Use the most energitic bar as seed. bar size: "<<shower1->getBars().size()<<endl;
+      double m_maxE = -99;
+      int index = -1;
+      for(int ib=0; ib<shower1->getBars().size(); ib++){
+        if(shower1->getBars()[ib]->getEnergy()>m_maxE) { m_maxE=shower1->getBars()[ib]->getEnergy(); index=ib; }
+      }
+      if(index>=0) m_wiseed = shower1->getBars()[index]->Clone();
+    }
+    m_wiseed->setQ( wi_E*m_wiseed->getQ1(), wi_E*m_wiseed->getQ2() );
+    m_bkCol.map_BarCol["bkBar"].push_back( m_wiseed );
+
+    std::vector<const PandoraPlus::CaloUnit*> m_wibars; m_wibars.clear();
+    for(int ib=0;ib<shower1->getBars().size();ib++){
+      std::shared_ptr<PandoraPlus::CaloUnit> m_wibar = shower1->getBars()[ib]->Clone();
+      m_wibar->setQ(wi_E*m_wibar->getQ1(), wi_E*m_wibar->getQ2());
+      m_wibars.push_back(m_wibar.get());
+      m_bkCol.map_BarCol["bkBar"].push_back( m_wibar );
+    }
+    m_splitshower1->setBars( m_wibars );
+    m_splitshower1->addSeed( m_wiseed.get() );
+    m_splitshower1->setIDInfo();
+    std::shared_ptr<PandoraPlus::Calo2DCluster> m_shower = std::make_shared<PandoraPlus::Calo2DCluster>();
+    if(_slayer==0 ) GetMatchedShowersL0( m_splitshower1.get(), showerNCol[is], m_shower.get() );
+    else            GetMatchedShowersL0( showerNCol[is], m_splitshower1.get(), m_shower.get() );
+
+    outshCol.push_back( m_shower.get() );
+    m_bkCol.map_2DCluster["bk2DCluster"].push_back( m_shower );
+  }
+  return StatusCode::SUCCESS;
+}
+
+
+
+
+StatusCode EnergyTimeMatchingAlg::ClusterReconnecting( std::vector<std::shared_ptr<PandoraPlus::Calo3DCluster>>& m_clusterCol ){
+/*
+cout<<"Print 3DClusters"<<endl;
+for(int ic=0; ic<m_clusterCol.size(); ic++){
+  printf("  Cluster #%d pos+En [%.3f, %.3f, %.3f, %.3f], hit size %d \n ", ic, m_clusterCol[ic]->getShowerCenter().x(), 
+                                                          m_clusterCol[ic]->getShowerCenter().y(), 
+                                                          m_clusterCol[ic]->getShowerCenter().z(), 
+                                                          m_clusterCol[ic]->getEnergy(), 
+                                                          m_clusterCol[ic]->getCaloHits().size() );
+  std::vector<const CaloHalfCluster*> m_HFClusUInTower = m_clusterCol[ic]->getHalfClusterUCol("LinkedLongiCluster");
+  std::vector<const CaloHalfCluster*> m_HFClusVInTower = m_clusterCol[ic]->getHalfClusterVCol("LinkedLongiCluster");  
+  cout<<"  Linked half cluster size: "<<m_HFClusUInTower.size()<<", "<<m_HFClusVInTower.size()<<endl;
+  cout<<"    Loop print HalfClusterU: "<<endl;
+  for(int icl=0; icl<m_HFClusUInTower.size(); icl++){
+    cout<<"      In HFClusU #"<<icl<<": shower size = "<<m_HFClusUInTower[icl]->getCluster().size()<<", En = "<<m_HFClusUInTower[icl]->getEnergy()<<", type "<<m_HFClusUInTower[icl]->getType();
+    printf(", Position (%.3f, %.3f, %.3f), address %p ",m_HFClusUInTower[icl]->getPos().x(), m_HFClusUInTower[icl]->getPos().y(), m_HFClusUInTower[icl]->getPos().z(), m_HFClusUInTower[icl]);
+    printf(", cousin size %d, address: ", m_HFClusUInTower[icl]->getHalfClusterCol("CousinCluster").size());
+    for(int ics=0; ics<m_HFClusUInTower[icl]->getHalfClusterCol("CousinCluster").size(); ics++) printf("%p, ", m_HFClusUInTower[icl]->getHalfClusterCol("CousinCluster")[ics]);
+    printf(", track size %d, address: ", m_HFClusUInTower[icl]->getAssociatedTracks().size());
+    for(int itrk=0; itrk<m_HFClusUInTower[icl]->getAssociatedTracks().size(); itrk++) printf("%p, ", m_HFClusUInTower[icl]->getAssociatedTracks()[itrk]);
+    cout<<endl;
+    for(auto ish : m_HFClusUInTower[icl]->getCluster()){
+      printf("          Shower layer %d, Pos+E (%.3f, %.3f, %.3f, %.3f), Nbars %d, NSeed %d, Address %p \n", ish->getDlayer(), ish->getPos().x(), ish->getPos().y(), ish->getPos().z(), ish->getEnergy(), ish->getBars().size(),  ish->getNseeds(), ish );
+    }
+  }
+  cout<<endl;
+  cout<<"    Loop print HalfClusterV: "<<endl;
+  for(int icl=0; icl<m_HFClusVInTower.size(); icl++){
+    cout<<"      In HFClusV #"<<icl<<": shower size = "<<m_HFClusVInTower[icl]->getCluster().size()<<", En = "<<m_HFClusVInTower[icl]->getEnergy()<<", type "<<m_HFClusVInTower[icl]->getType();
+    printf(", Position (%.3f, %.3f, %.3f), address %p ",m_HFClusVInTower[icl]->getPos().x(), m_HFClusVInTower[icl]->getPos().y(), m_HFClusVInTower[icl]->getPos().z(), m_HFClusVInTower[icl]);
+    printf(", cousin size %d, address: ", m_HFClusVInTower[icl]->getHalfClusterCol("CousinCluster").size());
+    for(int ics=0; ics<m_HFClusVInTower[icl]->getHalfClusterCol("CousinCluster").size(); ics++) printf("%p, ", m_HFClusVInTower[icl]->getHalfClusterCol("CousinCluster")[ics]);
+    printf(", track size %d, address: ", m_HFClusVInTower[icl]->getAssociatedTracks().size());
+    for(int itrk=0; itrk<m_HFClusVInTower[icl]->getAssociatedTracks().size(); itrk++) printf("%p, ", m_HFClusVInTower[icl]->getAssociatedTracks()[itrk]);
+    cout<<endl;
+    for(auto ish : m_HFClusVInTower[icl]->getCluster()){
+      printf("          Shower layer %d, Pos+E (%.3f, %.3f, %.3f, %.3f), Nbars %d, NSeed %d, Address %p \n", ish->getDlayer(), ish->getPos().x(), ish->getPos().y(), ish->getPos().z(), ish->getEnergy(), ish->getBars().size(), ish->getNseeds(), ish );
+    }
+  }
+  cout<<endl;
+}
+*/
+  //Remove the same clusters
+  for(int ic=0; ic<m_clusterCol.size() && m_clusterCol.size()>1; ic++){
+    for(int jc=ic+1; jc<m_clusterCol.size(); jc++){
+      if(ic>m_clusterCol.size()) ic--;
+
+      //From pos+E. Can confirm from ChildCluster info.
+      float m_clusE_ic = m_clusterCol[ic].get()->getEnergy();
+      TVector3 m_pos_ic = m_clusterCol[ic].get()->getShowerCenter();
+      float m_clusE_jc = m_clusterCol[jc].get()->getEnergy();
+      TVector3 m_pos_jc = m_clusterCol[jc].get()->getShowerCenter();
+      if( fabs(m_clusE_ic-m_clusE_jc)<0.1 && (m_pos_ic-m_pos_jc).Mag()<10 ){
+        m_clusterCol.erase(m_clusterCol.begin()+jc);
+        jc--;
+        if(ic>jc+1) ic--;
+      }
+    }
+  }
+
+//cout<<"  ClusterReconnecting: Cluster size after double-counting check: "<<m_clusterCol.size()<<endl;
+
+  //  Type2: from longiCluster aspect
+  for(int ic=0; ic<m_clusterCol.size() && m_clusterCol.size()>1; ic++){
+    if( m_clusterCol[ic].get()->getHalfClusterUCol("LinkedLongiCluster")[0]->getHalfClusterCol("ParentCluster").size()==0 ||
+        m_clusterCol[ic].get()->getHalfClusterVCol("LinkedLongiCluster")[0]->getHalfClusterCol("ParentCluster").size()==0 ) continue;
+//cout<<"  Cluster ic "<<ic<<" has parent"<<endl;
+    for(int jc=ic+1; jc<m_clusterCol.size(); jc++){
+      if( m_clusterCol[jc].get()->getHalfClusterUCol("LinkedLongiCluster")[0]->getHalfClusterCol("ParentCluster").size()==0 ||
+          m_clusterCol[jc].get()->getHalfClusterVCol("LinkedLongiCluster")[0]->getHalfClusterCol("ParentCluster").size()==0 ) continue;
+//cout<<"  Cluster jc "<<jc<<" also has parent, start check"<<endl;
+/*
+      std::vector<const PandoraPlus::CaloHalfCluster*> m_cousinU = m_clusterCol[ic].get()->getHalfClusterUCol("LinkedLongiCluster")[0]->getHalfClusterCol("CousinCluster");
+      std::vector<const PandoraPlus::CaloHalfCluster*> m_cousinV = m_clusterCol[ic].get()->getHalfClusterVCol("LinkedLongiCluster")[0]->getHalfClusterCol("CousinCluster");
+      if( find(m_cousinU.begin(), m_cousinU.end(), m_clusterCol[jc].get()->getHalfClusterUCol("LinkedLongiCluster")[0] )!=m_cousinU.end() &&
+          find(m_cousinV.begin(), m_cousinV.end(), m_clusterCol[jc].get()->getHalfClusterVCol("LinkedLongiCluster")[0] )!=m_cousinV.end() &&
+          m_clusterCol[ic].get()->getShowerCenter().Angle( m_clusterCol[jc].get()->getShowerCenter() )<0.05 ){ //3deg, ~10cm in ECAL.
+        m_clusterCol[ic].get()->mergeCluster( m_clusterCol[jc].get() );
+        m_clusterCol.erase(m_clusterCol.begin()+jc);
+        jc--;
+        if(jc<ic) jc=ic;
+      }
+
+      m_cousinU.clear();
+      m_cousinV.clear();
+      m_cousinU = m_clusterCol[jc].get()->getHalfClusterUCol("LinkedLongiCluster")[0]->getHalfClusterCol("CousinCluster");
+      m_cousinV = m_clusterCol[jc].get()->getHalfClusterVCol("LinkedLongiCluster")[0]->getHalfClusterCol("CousinCluster");
+      if( find(m_cousinU.begin(), m_cousinU.end(), m_clusterCol[ic].get()->getHalfClusterUCol("LinkedLongiCluster")[0] )!=m_cousinU.end() &&
+          find(m_cousinV.begin(), m_cousinV.end(), m_clusterCol[ic].get()->getHalfClusterVCol("LinkedLongiCluster")[0] )!=m_cousinV.end() &&
+          m_clusterCol[ic].get()->getShowerCenter().Angle( m_clusterCol[jc].get()->getShowerCenter() )<0.05 ){ //3deg, ~10cm in ECAL.
+        m_clusterCol[ic].get()->mergeCluster( m_clusterCol[jc].get() );
+        m_clusterCol.erase(m_clusterCol.begin()+jc);
+        jc--;
+        if(jc<ic) jc=ic;
+      }
+*/
+      if( m_clusterCol[ic].get()->getHalfClusterUCol("LinkedLongiCluster")[0]->getHalfClusterCol("ParentCluster").size()>1 || 
+          m_clusterCol[ic].get()->getHalfClusterVCol("LinkedLongiCluster")[0]->getHalfClusterCol("ParentCluster").size()>1 ||
+          m_clusterCol[jc].get()->getHalfClusterUCol("LinkedLongiCluster")[0]->getHalfClusterCol("ParentCluster").size()>1 ||
+          m_clusterCol[jc].get()->getHalfClusterVCol("LinkedLongiCluster")[0]->getHalfClusterCol("ParentCluster").size()>1 ){
+
+        cout<<"  ERROR: more than 1 Parent Cluster!"<<endl; 
+        printf("    In #%d cluster: parent cluster size [%d, %d]. In  #%d cluster: parent cluster size [%d, %d] \n", ic, m_clusterCol[ic].get()->getHalfClusterUCol("LinkedLongiCluster")[0]->getHalfClusterCol("ParentCluster").size(), m_clusterCol[ic].get()->getHalfClusterVCol("LinkedLongiCluster")[0]->getHalfClusterCol("ParentCluster").size(), jc, m_clusterCol[jc].get()->getHalfClusterUCol("LinkedLongiCluster")[0]->getHalfClusterCol("ParentCluster").size(), m_clusterCol[jc].get()->getHalfClusterVCol("LinkedLongiCluster")[0]->getHalfClusterCol("ParentCluster").size() );
+        continue;
+      }      
+
+      const PandoraPlus::CaloHalfCluster* m_parentU1 = m_clusterCol[ic].get()->getHalfClusterUCol("LinkedLongiCluster")[0]->getHalfClusterCol("ParentCluster")[0]; 
+      const PandoraPlus::CaloHalfCluster* m_parentV1 = m_clusterCol[ic].get()->getHalfClusterVCol("LinkedLongiCluster")[0]->getHalfClusterCol("ParentCluster")[0]; 
+      const PandoraPlus::CaloHalfCluster* m_parentU2 = m_clusterCol[jc].get()->getHalfClusterUCol("LinkedLongiCluster")[0]->getHalfClusterCol("ParentCluster")[0];
+      const PandoraPlus::CaloHalfCluster* m_parentV2 = m_clusterCol[jc].get()->getHalfClusterVCol("LinkedLongiCluster")[0]->getHalfClusterCol("ParentCluster")[0];
+      if(m_parentU1==m_parentU2 && m_parentV1==m_parentV2 && m_clusterCol[ic].get()->getShowerCenter().Angle( m_clusterCol[jc].get()->getShowerCenter() )<0.05){
+//cout<<"  In pair ("<<ic<<", "<<jc<<"): merge "<<endl;
+        m_clusterCol[ic].get()->mergeCluster( m_clusterCol[jc].get() );
+        m_clusterCol.erase(m_clusterCol.begin()+jc);
+        jc--;
+        if(jc<ic) jc=ic;
+      }
+  }}
+
+//cout<<"  ClusterReconnecting: Cluster size after neighbor check: "<<m_clusterCol.size()<<endl;
+
+  //  Type3: merge clusters linked to the same track.
+  for(int ic=0; ic<m_clusterCol.size() && m_clusterCol.size()>1; ic++){
+    if(m_clusterCol[ic].get()->getAssociatedTracks().size()==0) continue;
+    std::vector<const PandoraPlus::Track*> m_trkCol = m_clusterCol[ic].get()->getAssociatedTracks();
+
+    for(int jc=ic+1; jc<m_clusterCol.size(); jc++){
+      if(m_clusterCol[jc].get()->getAssociatedTracks().size()==0) continue;
+//cout<<"Check pair: ["<<ic<<", "<<jc<<"]. Both have tracks. "<<endl;
+
+      for(int itrk=0; itrk<m_clusterCol[jc].get()->getAssociatedTracks().size(); itrk++){
+        if( find(m_trkCol.begin(), m_trkCol.end(), m_clusterCol[jc].get()->getAssociatedTracks()[itrk])!= m_trkCol.end() ){
+//cout<<"  Merge cluster pair: ["<<ic<<", "<<jc<<"]. "<<endl;
+          m_clusterCol[ic].get()->mergeCluster( m_clusterCol[jc].get() );
+          m_clusterCol.erase(m_clusterCol.begin()+jc);
+          jc--;
+          if(jc<ic) jc=ic;
+        }
+//cout<<"  After merge: ic="<<ic<<", jc="<<jc<<endl;
+        break;
+      }
+    }
+  }
+  for(int ic=0; ic<m_clusterCol.size(); ic++) m_clusterCol[ic].get()->getLinkedMCPfromHFCluster("LinkedLongiCluster");
+
+//cout<<"  ClusterReconnecting: Cluster size after track check: "<<m_clusterCol.size()<<endl;
+
+/*  //Check the clusters with the same ChildHFCluster: re-matching. 
+  //std::multimap<const CaloHalfCluster*, std::shared_ptr<PandoraPlus::Calo3DCluster>> map_linkedClusters; map_linkedClusters.clear();
+  std::vector<std::shared_ptr<PandoraPlus::Calo3DCluster>> Cluster_needMatch; Cluster_needMatch.clear();
+  for(int ic=0; ic<m_clusterCol.size(); ic++){
+    std::vector<const CaloHalfCluster*> m_HFClusU = m_clusterCol[ic].get()->getHalfClusterUCol("LinkedLongiCluster");
+    std::vector<const CaloHalfCluster*> m_HFClusV = m_clusterCol[ic].get()->getHalfClusterVCol("LinkedLongiCluster");
+
+    if(m_HFClusU.size()!=1 || m_HFClusV.size()!=1){
+      std::cout<<"  ERROR: 3Dcluster has more than 1 LinkedLongiCluster: ("<<m_HFClusU.size()<<", "<<m_HFClusV.size()<<")! Check! "<<std::endl;
+      continue;
+    }
+    std::vector<const CaloHalfCluster*> m_ChildClU = m_HFClusU[0]->getHalfClusterCol("ChildCluster");
+    std::vector<const CaloHalfCluster*> m_ChildClV = m_HFClusV[0]->getHalfClusterCol("ChildCluster");
+    if(m_ChildClU.size()==0 && m_ChildClV.size()==0) continue; 
+
+    //for(int icl=0; icl<m_ChildClU.size(); icl++) map_linkedClusters.insert( {m_HFClusU[icl], m_clusterCol[ic]} );
+    //for(int icl=0; icl<m_ChildClV.size(); icl++) map_linkedClusters.insert( {m_HFClusV[icl], m_clusterCol[ic]} );
+    Cluster_needMatch.push_back( m_clusterCol[ic] );
+    m_clusterCol.erase( m_clusterCol.begin()+ic );
+    ic--;
+  }
+cout<<"  ClusterReconnecting: Cluster size after child check: "<<m_clusterCol.size()<<endl;
+cout<<"  Need-match cluster size: "<<Cluster_needMatch.size()<<endl;
+
+  std::vector<const CaloHalfCluster*> m_ChildClU; 
+  std::vector<const CaloHalfCluster*> m_ChildClV; 
+  for(int ic=0; ic<Cluster_needMatch.size(); ic++){
+    std::vector<const CaloHalfCluster*> tmp_ChildCl = Cluster_needMatch[ic].get()->getHalfClusterUCol("LinkedLongiCluster")[0]->getHalfClusterCol("ChildCluster");
+    m_ChildClU.insert( m_ChildClU.end(), tmp_ChildCl.begin(), tmp_ChildCl.end() );
+    tmp_ChildCl.clear();
+    tmp_ChildCl = Cluster_needMatch[ic].get()->getHalfClusterVCol("LinkedLongiCluster")[0]->getHalfClusterCol("ChildCluster");
+    m_ChildClV.insert( m_ChildClV.end(), tmp_ChildCl.begin(), tmp_ChildCl.end() );
+  }
+cout<<"  Total child cluster size: ("<<m_ChildClU.size()<<", "<<m_ChildClV.size()<<endl;
+
+  std::vector<const CaloHalfCluster*> m_mergedClusU; m_mergedClusU.clear();
+  std::vector<const CaloHalfCluster*> m_eraseCl; m_eraseCl.clear(); 
+  do{
+    for(int icl=0; icl<m_ChildClU.size(); icl++){
+      std::vector<const CaloHalfCluster*> tmp_cousin = m_ChildClU[icl]->getHalfClusterCol("CousinCluster");
+      std::shared_ptr<PandoraPlus::CaloHalfCluster> merged_HFClU = std::make_shared<PandoraPlus::CaloHalfCluster>();
+
+      merged_HFClU->mergeHalfCluster(m_ChildClU[icl]);
+      merged_HFClU.get()->addHalfCluster("ChildCluster", m_ChildClU[icl]);
+      for(auto cl_U : tmp_cousin){
+        merged_HFClU.get()->mergeHalfCluster(cl_U);
+        merged_HFClU.get()->addHalfCluster("ChildCluster", cl_U);
+        m_eraseCl.push_back(cl_U);
+      }
+      m_eraseCl.push_back(m_ChildClU[icl]);
+
+      m_bkCol.map_HalfCluster["bkHalfCluster"].push_back(merged_HFClU);
+      m_mergedClusU.push_back(merged_HFClU.get());
+      break;
+    }
+cout<<"    waiting erasing cluster size: "<<m_eraseCl.size()<<endl;
+    for(int icl=0; icl<m_eraseCl.size(); icl++){
+      auto iter_find = find( m_ChildClU.begin(), m_ChildClU.end(), m_eraseCl[icl] );
+      if(iter_find!=m_ChildClU.end()) m_ChildClU.erase( iter_find );
+    }
+    m_eraseCl.clear();
+cout<<"    left child cluster size: "<<m_ChildClU.size()<<endl;
+  }while(m_ChildClU.size()!=0);
+
+  std::vector<const CaloHalfCluster*> m_mergedClusV; m_mergedClusV.clear();
+  m_eraseCl.clear();
+  do{
+    for(int icl=0; icl<m_ChildClV.size(); icl++){
+      std::vector<const CaloHalfCluster*> tmp_cousin = m_ChildClV[icl]->getHalfClusterCol("CousinCluster");
+      std::shared_ptr<PandoraPlus::CaloHalfCluster> merged_HFClV = std::make_shared<PandoraPlus::CaloHalfCluster>();
+      merged_HFClV->mergeHalfCluster(m_ChildClV[icl]);
+      merged_HFClV.get()->addHalfCluster("ChildCluster", m_ChildClV[icl]);
+      for(auto cl_V : tmp_cousin){
+        merged_HFClV.get()->mergeHalfCluster(cl_V);
+        merged_HFClV.get()->addHalfCluster("ChildCluster", cl_V);
+        m_eraseCl.push_back(cl_V);
+      }
+      m_eraseCl.push_back(m_ChildClV[icl]);
+
+      m_bkCol.map_HalfCluster["bkHalfCluster"].push_back(merged_HFClV);
+      m_mergedClusV.push_back(merged_HFClV.get());
+      break;
+    }
+    for(int icl=0; icl<m_eraseCl.size(); icl++){
+      auto iter_find = find( m_ChildClV.begin(), m_ChildClV.end(), m_eraseCl[icl] );
+      if(iter_find!=m_ChildClV.end()) m_ChildClV.erase( iter_find );
+    }
+    m_eraseCl.clear();
+
+  }while(m_ChildClV.size()!=0);
+cout<<"  Merged HFCluster size: ("<<m_mergedClusU.size()<<", "<<m_mergedClusV.size()<<endl;
+
+  std::vector<std::shared_ptr<PandoraPlus::Calo3DCluster>> newClusters; newClusters.clear();
+  if(m_mergedClusU.size()==0 || m_mergedClusV.size()==0) return StatusCode::SUCCESS;
+  else if( m_mergedClusU.size()==1 && m_mergedClusV.size()==1 ){
+    std::shared_ptr<PandoraPlus::Calo3DCluster> tmp_clus = std::make_shared<PandoraPlus::Calo3DCluster>();
+    XYClusterMatchingL0(m_mergedClusU[0], m_mergedClusV[0], tmp_clus);
+    newClusters.push_back(tmp_clus);
+  }
+  else if( m_mergedClusU.size()==1 ){
+    std::vector<std::shared_ptr<PandoraPlus::Calo3DCluster>> emptyCol;
+    XYClusterMatchingL1(m_mergedClusU[0], m_mergedClusV, emptyCol);
+    newClusters.insert( newClusters.end(), emptyCol.begin(), emptyCol.end() );
+  }
+  else if( m_mergedClusV.size()==1 ){
+    std::vector<std::shared_ptr<PandoraPlus::Calo3DCluster>> emptyCol;
+    XYClusterMatchingL1(m_mergedClusV[0], m_mergedClusU, emptyCol);
+    newClusters.insert( newClusters.end(), emptyCol.begin(), emptyCol.end() );
+  }
+  else if( m_mergedClusU.size()==m_mergedClusV.size() ){
+    std::vector<std::shared_ptr<PandoraPlus::Calo3DCluster>> emptyCol;
+    XYClusterMatchingL2(m_mergedClusU, m_mergedClusV, emptyCol);
+    newClusters.insert( newClusters.end(), emptyCol.begin(), emptyCol.end() );
+  }
+  else{
+    std::vector<std::shared_ptr<PandoraPlus::Calo3DCluster>> emptyCol;
+    XYClusterMatchingL3(m_mergedClusU, m_mergedClusV, emptyCol);
+    newClusters.insert( newClusters.end(), emptyCol.begin(), emptyCol.end() );
+  }
+cout<<"  Created new 3DCluster size: "<<newClusters.size()<<endl;
+
+  for(int ic=0; ic<newClusters.size(); ic++){
+    if( !newClusters[ic].get() ){
+      newClusters.erase(newClusters.begin()+ic);
+      ic--;
+  }}
+cout<<"  Created new 3DCluster size: "<<newClusters.size()<<endl;
+
+  m_clusterCol.insert(m_clusterCol.end(), newClusters.begin(), newClusters.end() );
+*/
+  return StatusCode::SUCCESS;
+}
+
+
+
+#endif
diff --git a/Reconstruction/CrystalCaloRec/src/Algorithm/ExampleAlg.cpp b/Reconstruction/CrystalCaloRec/src/Algorithm/ExampleAlg.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..2a3c5b76ac4d6630c7e722d3ae7b7e0ea85f0ddf
--- /dev/null
+++ b/Reconstruction/CrystalCaloRec/src/Algorithm/ExampleAlg.cpp
@@ -0,0 +1,50 @@
+#ifndef _EXAMPLE_ALG_C
+#define _EXAMPLE_ALG_C
+
+#include "Algorithm/ExampleAlg.h"
+
+StatusCode ExampleAlg::ReadSettings(Settings& m_settings){
+  settings = m_settings;
+
+  //Initialize parameters
+  if(settings.map_stringPars.find("Par1")==settings.map_stringPars.end()) settings.map_stringPars["Par1"] = "HERE";
+  if(settings.map_floatPars.find("Par2")==settings.map_floatPars.end()) settings.map_floatPars["Par2"] = 0;
+  if(settings.map_boolPars.find("Par3")==settings.map_boolPars.end()) settings.map_boolPars["Par3"] = 0;
+
+  return StatusCode::SUCCESS;
+};
+
+StatusCode ExampleAlg::Initialize( PandoraPlusDataCol& m_datacol ){
+  std::cout<<"Initialize ExampleAlg"<<std::endl;
+
+  return StatusCode::SUCCESS;
+};
+
+StatusCode ExampleAlg::RunAlgorithm( PandoraPlusDataCol& m_datacol ){
+  std::cout<<"Excute ExampleAlg"<<std::endl;
+  std::cout<<"  Run SelfAlg1"<<std::endl;
+  SelfAlg1();
+
+  return StatusCode::SUCCESS;
+};
+
+StatusCode ExampleAlg::ClearAlgorithm(){
+  std::cout<<"End run ExampleAlg. Clean it."<<std::endl;
+
+  return StatusCode::SUCCESS;
+};
+
+
+StatusCode ExampleAlg::SelfAlg1(){
+  std::cout<<"  Processing SelfAlg1: print Setting parameters"<<std::endl;
+
+  for(auto &iter : settings.map_stringPars) cout<<iter.first<<": "<<iter.second<<endl;
+  for(auto &iter : settings.map_floatPars) cout<<iter.first<<": "<<iter.second<<endl;
+  for(auto &iter : settings.map_boolPars) cout<<iter.first<<": "<<iter.second<<endl;
+
+
+  return StatusCode::SUCCESS;
+};
+
+
+#endif
diff --git a/Reconstruction/CrystalCaloRec/src/Algorithm/GlobalClusteringAlg.cpp b/Reconstruction/CrystalCaloRec/src/Algorithm/GlobalClusteringAlg.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4efcfae2a6e91f953bc52e98993efb3622fbc11d
--- /dev/null
+++ b/Reconstruction/CrystalCaloRec/src/Algorithm/GlobalClusteringAlg.cpp
@@ -0,0 +1,146 @@
+#ifndef GLOBALCLUSTERING_ALG_C
+#define GLOBALCLUSTERING_ALG_C
+
+#include "Algorithm/GlobalClusteringAlg.h"
+using namespace PandoraPlus;
+
+StatusCode GlobalClusteringAlg::ReadSettings(PandoraPlus::Settings& m_settings){
+  settings = m_settings;
+  if(settings.map_floatPars.find("unit_threshold")==settings.map_floatPars.end())  settings.map_floatPars["unit_threshold"] = 0.001;
+  if(settings.map_stringPars.find("InputECALBars")==settings.map_stringPars.end()) settings.map_stringPars["InputECALBars"] = "BarCol";
+  if(settings.map_stringPars.find("OutputECAL1DClusters")==settings.map_stringPars.end()) settings.map_stringPars["OutputECAL1DClusters"] = "Cluster1DCol";
+  if(settings.map_stringPars.find("OutputECALHalfClusters")==settings.map_stringPars.end()) settings.map_stringPars["OutputECALHalfClusters"] = "HalfClusterCol";
+  return StatusCode::SUCCESS;
+};
+
+StatusCode GlobalClusteringAlg::Initialize( PandoraPlusDataCol& m_datacol ){
+  m_bars.clear();
+  m_processbars.clear(); 
+  m_restbars.clear();
+  m_1dclusters.clear();
+  m_halfclusters.clear();
+
+  //Readin data from DataCol: 
+  m_bars = m_datacol.map_BarCol[settings.map_stringPars["InputECALBars"]];
+
+
+  return StatusCode::SUCCESS;
+};
+
+StatusCode GlobalClusteringAlg::RunAlgorithm( PandoraPlusDataCol& m_datacol ){
+  //system("/cefs/higgs/songwz/winter22/CEPCSW/workarea/memory/memory_test.sh cluster_begin");
+  //time_t time_cb;  
+  //time(&time_cb);  
+  //cout<<" When begin clustering: "<<ctime(&time_cb)<<endl;
+
+  //Threshold and scale factor (Todo) for bars. 
+  for(int ibar=0; ibar<m_bars.size(); ibar++)
+  {
+    if(m_bars.at(ibar)->getEnergy()>settings.map_floatPars["unit_threshold"])
+    {
+      m_processbars.push_back(m_bars.at(ibar));
+    }
+    else
+    {
+      m_restbars.push_back(m_bars.at(ibar));
+    }
+  }
+
+  //Clustering
+  Clustering(m_processbars, m_1dclusters);
+  Clustering(m_1dclusters, m_halfclusters);
+
+  //Store created objects to backup col. 
+  m_datacol.map_1DCluster["bk1DCluster"].insert(m_datacol.map_1DCluster["bk1DCluster"].end(), m_1dclusters.begin(), m_1dclusters.end());
+  m_datacol.map_HalfCluster["bkHalfCluster"].insert(m_datacol.map_HalfCluster["bkHalfCluster"].end(), m_halfclusters.begin(), m_halfclusters.end());
+
+
+  std::vector<std::shared_ptr<PandoraPlus::CaloHalfCluster>> m_HalfClusterV; m_HalfClusterV.clear();
+  std::vector<std::shared_ptr<PandoraPlus::CaloHalfCluster>> m_HalfClusterU; m_HalfClusterU.clear();
+  for(int i=0; i<m_halfclusters.size() && m_halfclusters[i]; i++){
+    m_halfclusters[i]->setType(0);
+    if(m_halfclusters[i]->getSlayer()==0 && m_halfclusters[i]->getCluster().size()>0)
+      m_HalfClusterU.push_back(m_halfclusters[i]);
+    else if(m_halfclusters[i]->getSlayer()==1 && m_halfclusters[i]->getCluster().size()>0)
+      m_HalfClusterV.push_back(m_halfclusters[i]);
+  }
+
+//printf("  GlobalClustering: RestBarCol size %d, 1DCluster size %d, HalfCluster size (%d, %d) \n", m_restbars.size(), m_1dclusters.size(), m_HalfClusterU.size(), m_HalfClusterV.size());
+//for(int ic=0; ic<m_HalfClusterU.size(); ic++) cout<<m_HalfClusterU[ic]->getEnergy()<<'\t';
+//cout<<endl;
+//for(int ic=0; ic<m_HalfClusterV.size(); ic++) cout<<m_HalfClusterV[ic]->getEnergy()<<'\t';
+//cout<<endl;
+
+
+  //Write results into DataCol.
+  //m_datacol.map_BarCol["RestBarCol"] = m_restbars;
+  m_datacol.map_1DCluster[settings.map_stringPars["OutputECAL1DClusters"]] = m_1dclusters;
+  m_datacol.map_HalfCluster[settings.map_stringPars["OutputECALHalfClusters"]+"U"] = m_HalfClusterU;
+  m_datacol.map_HalfCluster[settings.map_stringPars["OutputECALHalfClusters"]+"V"] = m_HalfClusterV;
+
+
+  //time_t time_ce;  
+  //time(&time_ce); 
+  //cout<<" When end clustering: "<<ctime(&time_ce)<<endl;
+  //system("/cefs/higgs/songwz/winter22/CEPCSW/workarea/memory/memory_test.sh cluster_end");
+  return StatusCode::SUCCESS;
+};
+
+StatusCode GlobalClusteringAlg::ClearAlgorithm(){
+
+  //Clear local memory
+  m_bars.clear();
+  m_processbars.clear();
+  m_restbars.clear();
+  m_1dclusters.clear();
+  m_halfclusters.clear();
+
+  return StatusCode::SUCCESS;
+};
+
+template<typename T1, typename T2> StatusCode GlobalClusteringAlg::Clustering(std::vector<std::shared_ptr<T1>> &m_input, std::vector<std::shared_ptr<T2>> &m_output) 
+{
+  std::vector<std::shared_ptr<T2>> record;
+  record.clear();
+
+  for(int i=0; i<m_input.size(); i++)
+  {
+    T1* lowlevelcluster = m_input.at(i).get();
+    for(int j=0; j<m_output.size(); j++)
+    {
+      if(m_output.at(j).get()->isNeighbor(lowlevelcluster)) // //m_output.at(j).isNeighbor(lowlevelcluster)
+        record.push_back(m_output.at(j));
+
+    }
+    if(record.size()>0)
+    {
+      record.at(0).get()->addUnit(lowlevelcluster);
+      for(int k=1; k<record.size(); k++)
+      {
+        for(int l=0; l<record.at(k).get()->getCluster().size(); l++)
+        {
+          record.at(0).get()->addUnit(record.at(k).get()->getCluster().at(l));
+        }
+      }
+      for(int m=1; m<record.size(); m++)
+      {
+        m_output.erase(find(m_output.begin(),m_output.end(),record.at(m)));
+        //delete record.at(m); 
+        record.at(m) = nullptr;
+      }
+      record.clear();
+      lowlevelcluster = nullptr;
+      continue;
+    }
+    //T2* highlevelcluster = new T2(); //first new
+    std::shared_ptr<T2> highlevelcluster = std::make_shared<T2>();
+
+    highlevelcluster.get()->addUnit(lowlevelcluster);
+    m_output.push_back(highlevelcluster);
+  }
+	return StatusCode::SUCCESS;
+	//cout<<"how many neighbors: "<<number<<endl;
+}
+
+
+#endif
diff --git a/Reconstruction/CrystalCaloRec/src/Algorithm/HcalClusteringAlg.cpp b/Reconstruction/CrystalCaloRec/src/Algorithm/HcalClusteringAlg.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..31b1bcc5b2b621286e59febfbbfe9e74de7b75b2
--- /dev/null
+++ b/Reconstruction/CrystalCaloRec/src/Algorithm/HcalClusteringAlg.cpp
@@ -0,0 +1,175 @@
+#ifndef HCALCLUSTERING_ALG_C
+#define HCALCLUSTERING_ALG_C
+
+#include "Algorithm/HcalClusteringAlg.h"
+using namespace PandoraPlus;
+
+StatusCode HcalClusteringAlg::ReadSettings(PandoraPlus::Settings& m_settings){
+  settings = m_settings;
+
+  if(settings.map_stringPars.find("InputHCALHits")==settings.map_stringPars.end())         settings.map_stringPars["InputHCALHits"] = "HCALBarrel";
+  if(settings.map_stringPars.find("OutputHCALClusters")==settings.map_stringPars.end())    settings.map_stringPars["OutputHCALClusters"] = "HCALCluster";
+
+  //Set initial values
+  if(settings.map_floatPars.find("th_ConeTheta_l1")==settings.map_floatPars.end())    settings.map_floatPars["th_ConeTheta_l1"] = TMath::Pi()/2.;
+  if(settings.map_floatPars.find("th_ConeR_l1")==settings.map_floatPars.end())        settings.map_floatPars["th_ConeR_l1"] = 70.;
+  if(settings.map_floatPars.find("th_ConeTheta_l2")==settings.map_floatPars.end())    settings.map_floatPars["th_ConeTheta_l2"] = TMath::Pi()/3.;
+  if(settings.map_floatPars.find("th_ConeR_l2")==settings.map_floatPars.end())        settings.map_floatPars["th_ConeR_l2"] = 120.;
+  if(settings.map_floatPars.find("th_ClusChi2")==settings.map_floatPars.end())        settings.map_floatPars["th_ClusChi2"] = 10e17;
+  if(settings.map_floatPars.find("fl_GoodClusLevel")==settings.map_floatPars.end())   settings.map_floatPars["fl_GoodClusLevel"] = 10;
+
+  return StatusCode::SUCCESS;
+};
+
+StatusCode HcalClusteringAlg::Initialize( PandoraPlusDataCol& m_datacol ){
+  return StatusCode::SUCCESS;
+};
+
+StatusCode HcalClusteringAlg::RunAlgorithm( PandoraPlusDataCol& m_datacol ){
+  //Readin data from DataCol: 
+//   std::vector<PandoraPlus::CaloHit*> m_hcalHits;
+//   m_hcalHits.clear();
+//   for(int ih=0; ih<m_datacol.map_CaloHit["HCALBarrel"].size(); ih++)
+//     m_hcalHits.push_back( m_datacol.map_CaloHit["HCALBarrel"][ih].get() );
+//   //ordered hits by layer
+//   std::map<int, std::vector<PandoraPlus::CaloHit*> > m_orderedHit;  
+//   m_orderedHit.clear();
+//   for(int ih=0;ih<m_hcalHits.size();ih++)
+//     m_orderedHit[m_hcalHits[ih]->getLayer()].push_back(m_hcalHits[ih]);
+
+  std::vector<PandoraPlus::CaloHit*> m_hcalHits;
+  m_hcalHits.clear();
+  for(int ih=0; ih<m_datacol.map_CaloHit[settings.map_stringPars["InputHCALHits"]].size(); ih++)
+    m_hcalHits.push_back( m_datacol.map_CaloHit[settings.map_stringPars["InputHCALHits"]][ih].get() );
+
+  std::vector<std::shared_ptr<PandoraPlus::Calo3DCluster>> m_clusterCol;  
+  m_clusterCol.clear();
+
+//   LongiConeLinking( m_orderedHit, m_clusterCol );
+  Clustering(m_hcalHits, m_clusterCol);
+//   m_datacol.bk_Cluster3DCol.insert(  m_datacol.bk_Cluster3DCol.end(), m_clusterCol.begin(), m_clusterCol.end() );
+  // cout<<"  Cluster size: "<<m_clusterCol.size()<<endl;
+  // for(int ic=0; ic<m_clusterCol.size(); ic++)
+  // {
+  //   cout<<"    Cluster "<<ic<<":"<<m_clusterCol[ic]->getCaloHits().size()<<endl;
+  // }
+
+//   m_datacol.map_CaloCluster[settings.map_stringPars["OutputCluster"]] = m_clusterCol;
+  m_datacol.map_CaloCluster[settings.map_stringPars["OutputHCALClusters"]]= m_clusterCol;
+  return StatusCode::SUCCESS;
+};
+
+StatusCode HcalClusteringAlg::ClearAlgorithm(){
+
+  //Clear local memory
+//   m_hcalHits.clear();
+
+  return StatusCode::SUCCESS;
+};
+
+StatusCode HcalClusteringAlg::LongiConeLinking(  const std::map<int, std::vector<PandoraPlus::CaloHit*> >& orderedHit, 
+                                                 std::vector<std::shared_ptr<PandoraPlus::Calo3DCluster> >& ClusterCol)
+{
+
+  if(orderedHit.size()==0) return StatusCode::SUCCESS;
+
+  auto iter = orderedHit.begin();
+  //In first layer: initial clusters. All showers in the first layer are regarded as cluster seed.
+  //cluster initial direction = R.
+  std::vector<PandoraPlus::CaloHit*> HitsinFirstLayer = iter->second;
+  for(int i=0;i<HitsinFirstLayer.size(); i++){
+    std::shared_ptr<PandoraPlus::Calo3DCluster> m_clus = std::make_shared<PandoraPlus::Calo3DCluster>();
+    m_clus->addHit(HitsinFirstLayer[i]);
+    ClusterCol.push_back(m_clus);
+  }
+  iter++;
+
+//cout<<"    LongiConeLinking: Cluster seed in first layer: "<<ClusterCol.size()<<endl;
+
+  //Use different cone angle for 1->2/2->3 and 3->n case
+  //Loop later layers
+  for(iter; iter!=orderedHit.end(); iter++){
+    std::vector<PandoraPlus::CaloHit*> HitsinLayer = iter->second;
+//cout<<"    In Layer: "<<iter->first<<"  Hit size: "<<HitsinLayer.size()<<endl;
+
+    for(int is=0; is<HitsinLayer.size(); is++){
+      PandoraPlus::CaloHit* m_hit = HitsinLayer[is];
+//printf("     New Hit: (%.3f, %.3f, %.3f), Layer %d \n", m_hit->getPosition().x(), m_hit->getPosition().y(), m_hit->getPosition().z(),  m_hit->getLayer() );
+//cout<<"     Cluster size: "<<ClusterCol.size()<<endl;
+
+      for(int ic=0; ic<ClusterCol.size(); ic++ ){
+        int m_Nhits = ClusterCol[ic]->getCaloHits().size();
+        const PandoraPlus::CaloHit* hit_in_clus = ClusterCol[ic]->getCaloHits().back();
+        TVector3 relR_vec = m_hit->getPosition() - hit_in_clus->getPosition();
+//printf("      New hit: (%.2f, %.2f, %.2f), Cluster last: (%.2f, %.2f, %.2f, %d), Cluster axis: (%.2f, %.2f, %.2f) \n",
+//    m_hit->getPosition().x(), m_hit->getPosition().y(),m_hit->getPosition().z(),
+//    hit_in_clus->getPosition().x(), hit_in_clus->getPosition().y(), hit_in_clus->getPosition().z(), hit_in_clus->getLayer(), 
+//    ClusterCol[ic]->getAxis().x(), ClusterCol[ic]->getAxis().y(), ClusterCol[ic]->getAxis().z() );
+
+        if(  (m_Nhits<3 && m_Nhits>0 && relR_vec.Angle(ClusterCol[ic]->getAxis())< settings.map_floatPars["th_ConeTheta_l1"] && relR_vec.Mag()< settings.map_floatPars["th_ConeR_l1"]) ||
+             (m_Nhits>=3             && relR_vec.Angle(ClusterCol[ic]->getAxis())< settings.map_floatPars["th_ConeTheta_l2"] && relR_vec.Mag()< settings.map_floatPars["th_ConeR_l2"])  ){
+
+          ClusterCol[ic]->addHit(m_hit);
+          HitsinLayer.erase(HitsinLayer.begin()+is);
+          is--;
+          break;
+        }
+      }
+    }//end loop showers in layer.
+    if(HitsinLayer.size()>0){
+      for(int i=0;i<HitsinLayer.size(); i++){
+        std::shared_ptr<PandoraPlus::Calo3DCluster> m_clus = std::make_shared<PandoraPlus::Calo3DCluster>();
+        m_clus->addHit(HitsinLayer[i]);
+        ClusterCol.push_back(m_clus);
+    }}//end new cluster
+  }//end loop layers.
+
+
+  return StatusCode::SUCCESS;
+}
+
+template<typename T1, typename T2> StatusCode HcalClusteringAlg::Clustering(std::vector<T1*> &m_input, std::vector<std::shared_ptr<T2>> &m_output) 
+{
+  std::vector<std::shared_ptr<T2>> record;
+  record.clear();
+
+  for(int i=0; i<m_input.size(); i++)
+  {
+    T1* lowlevelcluster = m_input.at(i);
+    for(int j=0; j<m_output.size(); j++)
+    {
+      if(m_output.at(j).get()->isHCALNeighbor(lowlevelcluster)) // //m_output.at(j).isNeighbor(lowlevelcluster)
+        record.push_back(m_output.at(j));
+
+    }
+    if(record.size()>0)
+    {
+      record.at(0).get()->addHit(lowlevelcluster);
+      for(int k=1; k<record.size(); k++)
+      {
+        for(int l=0; l<record.at(k).get()->getCaloHits().size(); l++)
+        {
+          record.at(0).get()->addHit(record.at(k).get()->getCaloHits().at(l));
+        }
+      }
+      for(int m=1; m<record.size(); m++)
+      {
+        m_output.erase(find(m_output.begin(),m_output.end(),record.at(m)));
+        //delete record.at(m); 
+        record.at(m) = nullptr;
+      }
+      record.clear();
+      lowlevelcluster = nullptr;
+      continue;
+    }
+    //T2* highlevelcluster = new T2(); //first new
+    std::shared_ptr<T2> highlevelcluster = std::make_shared<T2>();
+
+    highlevelcluster.get()->addHit(lowlevelcluster);
+    m_output.push_back(highlevelcluster);
+  }
+  return StatusCode::SUCCESS;
+	//cout<<"how many neighbors: "<<number<<endl;
+}
+
+#endif
diff --git a/Reconstruction/CrystalCaloRec/src/Algorithm/HoughClusteringAlg.cpp b/Reconstruction/CrystalCaloRec/src/Algorithm/HoughClusteringAlg.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ba9bbde8e6333a774e14dbf11020bce1b5334519
--- /dev/null
+++ b/Reconstruction/CrystalCaloRec/src/Algorithm/HoughClusteringAlg.cpp
@@ -0,0 +1,812 @@
+#ifndef HOUGHCLUSTERINGALG_C
+#define HOUGHCLUSTERINGALG_C
+
+#include "Algorithm/HoughClusteringAlg.h"
+#include <algorithm>
+#include <cmath>
+#include <set>
+#include "TCanvas.h"
+using namespace std;
+
+StatusCode HoughClusteringAlg::ReadSettings(PandoraPlus::Settings& m_settings){
+  settings = m_settings;
+
+  // ECAL geometry settings
+  //if(settings.map_floatPars.find("cell_size")==settings.map_floatPars.end())
+  //  settings.map_floatPars["cell_size"] = 10; // unit: mm
+  //if(settings.map_floatPars.find("ecal_inner_radius")==settings.map_floatPars.end())
+  //  settings.map_floatPars["ecal_inner_radius"] = 1860; // unit: mm
+
+  // Hough space settings
+  // alpha in V plane (bars parallel to z axis)
+  if(settings.map_floatPars.find("alpha_lowV")==settings.map_floatPars.end())
+    settings.map_floatPars["alpha_lowV"] = -0.1;  
+  if(settings.map_floatPars.find("alpha_highV")==settings.map_floatPars.end())
+    settings.map_floatPars["alpha_highV"] = 2.*TMath::Pi(); 
+  if(settings.map_intPars.find("Nbins_alphaV")==settings.map_intPars.end())
+    settings.map_intPars["Nbins_alphaV"] = 3000; 
+  if(settings.map_floatPars.find("bin_width_alphaV")==settings.map_floatPars.end())
+    settings.map_floatPars["bin_width_alphaV"] = ( settings.map_floatPars["alpha_highV"] - settings.map_floatPars["alpha_lowV"] ) / (double)settings.map_intPars["Nbins_alphaV"];
+  // double bin_width_alphaV = (alpha_highV - alpha_lowV) / (double)Nbins_alphaV;
+  
+  // alpha in U plane (bars perpendicular to z axis)
+  if(settings.map_floatPars.find("alpha_lowU")==settings.map_floatPars.end())
+    settings.map_floatPars["alpha_lowU"] = 0.;
+  if(settings.map_floatPars.find("alpha_highU")==settings.map_floatPars.end())
+    settings.map_floatPars["alpha_highU"] = TMath::Pi();  
+  if(settings.map_intPars.find("Nbins_alphaU")==settings.map_intPars.end())
+    settings.map_intPars["Nbins_alphaU"] = 5000;  
+  if(settings.map_floatPars.find("bin_width_alphaU")==settings.map_floatPars.end())
+    settings.map_floatPars["bin_width_alphaU"] = ( settings.map_floatPars["alpha_highU"] - settings.map_floatPars["alpha_lowU"] ) / (double)settings.map_intPars["Nbins_alphaU"];
+  // double bin_width_alphaU = (alpha_highU - alpha_lowU) / (double)Nbins_alphaU;
+
+  // rho
+  if(settings.map_floatPars.find("rho_low")==settings.map_floatPars.end())
+    settings.map_floatPars["rho_low"] = -50.;
+  if(settings.map_floatPars.find("rho_high")==settings.map_floatPars.end())
+    settings.map_floatPars["rho_high"] = 50.;
+  if(settings.map_intPars.find("Nbins_rho")==settings.map_intPars.end())
+    settings.map_intPars["Nbins_rho"] = 20;   // (rho_high - rho_low)/5
+  if(settings.map_floatPars.find("bin_width_rho")==settings.map_floatPars.end())
+    settings.map_floatPars["bin_width_rho"] = ( settings.map_floatPars["rho_high"] - settings.map_floatPars["rho_low"] ) / (double)settings.map_intPars["Nbins_rho"];
+
+  // Algorithm parameter settings
+  if(settings.map_intPars.find("th_Layers")==settings.map_intPars.end())
+    settings.map_intPars["th_Layers"] = 10;
+  if(settings.map_intPars.find("th_peak")==settings.map_intPars.end())
+    settings.map_intPars["th_peak"] = 3;
+  if(settings.map_intPars.find("th_continueN")==settings.map_intPars.end())
+    settings.map_intPars["th_continueN"] = 3;
+  if(settings.map_floatPars.find("th_AxisE")==settings.map_floatPars.end())
+    settings.map_floatPars["th_AxisE"] = 0.15; // unit: GeV
+  if(settings.map_floatPars.find("th_overlapE")==settings.map_floatPars.end())
+    settings.map_floatPars["th_overlapE"] = 0.5;
+  if(settings.map_floatPars.find("th_dAlpha1")==settings.map_floatPars.end())
+    settings.map_floatPars["th_dAlpha1"] = 0.1;
+  if(settings.map_floatPars.find("th_dAlpha2")==settings.map_floatPars.end())
+    settings.map_floatPars["th_dAlpha2"] = 0.05;
+  if(settings.map_floatPars.find("th_ERatio")==settings.map_floatPars.end())
+    settings.map_floatPars["th_ERatio"] = 0.04;
+
+  if(settings.map_stringPars.find("ReadinLocalMaxName")==settings.map_stringPars.end())  
+    settings.map_stringPars["ReadinLocalMaxName"] = "AllLocalMax";
+  if(settings.map_stringPars.find("LeftLocalMaxName")==settings.map_stringPars.end())    
+    settings.map_stringPars["LeftLocalMaxName"] = "LeftLocalMax";
+  if(settings.map_stringPars.find("OutputLongiClusName")==settings.map_stringPars.end()) 
+    settings.map_stringPars["OutputLongiClusName"] = "HoughAxis"; 
+
+  return StatusCode::SUCCESS;  
+}
+
+StatusCode HoughClusteringAlg::Initialize( PandoraPlusDataCol& m_datacol ){
+  p_HalfClusterU.clear(); 
+  p_HalfClusterV.clear(); 
+
+  for(int ih=0; ih<m_datacol.map_HalfCluster["HalfClusterColU"].size(); ih++)
+    p_HalfClusterU.push_back( m_datacol.map_HalfCluster["HalfClusterColU"][ih].get() );
+  for(int ih=0; ih<m_datacol.map_HalfCluster["HalfClusterColV"].size(); ih++)
+    p_HalfClusterV.push_back( m_datacol.map_HalfCluster["HalfClusterColV"][ih].get() );
+
+  //p_HalfClusterU = m_datacol.map_HalfCluster["HalfClusterColU"];
+  //p_HalfClusterV = m_datacol.map_HalfCluster["HalfClusterColV"];
+
+  return StatusCode::SUCCESS;
+}
+
+StatusCode HoughClusteringAlg::RunAlgorithm( PandoraPlusDataCol& m_datacol ){
+
+  //if( (p_HalfClusterU.size()+p_HalfClusterV.size())<1 ){
+  //  std::cout << "HoughClusteringAlg: No HalfCluster input"<<std::endl;
+  //  return StatusCode::SUCCESS;
+  //}
+
+  
+  if(p_HalfClusterV.size()==0){ std::cout<<"  HoughClusteringAlg: No HalfClusterV in present data collection! "<<std::endl; }
+  if(p_HalfClusterU.size()==0){ std::cout<<"  HoughClusteringAlg: No HalfClusterU in present data collection! "<<std::endl; }
+
+//cout<<"Readin HalfCluster size: "<<p_HalfClusterV.size()<<", "<<p_HalfClusterU.size()<<endl;
+
+  //std::vector<const PandoraPlus::CaloHalfCluster*> m_refHFClusVCol; m_refHFClusVCol.clear();
+  // Processing V(xy) plane
+  for(int it=0; it<p_HalfClusterV.size(); it++){ // process each HalfCluster respectively
+    m_localMaxVCol.clear();
+    std::vector<const PandoraPlus::Calo1DCluster*> tmp_localMaxVCol = p_HalfClusterV[it]->getLocalMaxCol(settings.map_stringPars["ReadinLocalMaxName"]);
+
+    for(int il=0; il<tmp_localMaxVCol.size(); il++){
+      if(tmp_localMaxVCol[il]->getDlayer()<=settings.map_intPars["th_Layers"]) 
+        m_localMaxVCol.push_back(tmp_localMaxVCol[il]);
+    }
+
+    if(m_localMaxVCol.size()<settings.map_intPars["th_peak"]){
+      //std::cout << "    yyy: m_localMaxVCol.size()<th_peak, continue" << std::endl;
+      continue; 
+    } 
+
+//cout<<"  HoughClusteringAlg: Find Hough axis in HalfCluster "<<it<<". Local maximum size V = "<<m_localMaxVCol.size()<<endl;
+    
+    // cout<<"  HoughClusteringAlg: Creating m_HoughObjectsV"<<endl;
+    std::vector<PandoraPlus::HoughObject> m_HoughObjectsV; m_HoughObjectsV.clear(); 
+    for(int il=0; il<m_localMaxVCol.size(); il++){
+      PandoraPlus::HoughObject m_obj(m_localMaxVCol[il], PandoraPlus::CaloUnit::barsize, PandoraPlus::CaloUnit::ecal_innerR);
+      m_HoughObjectsV.push_back(m_obj);
+    }
+//cout<<"  HoughClusteringAlg: HoughObjectV size "<<m_HoughObjectsV.size()<<endl;
+
+    // cout<<"  HoughClusteringAlg: Hough transformation"<<endl;
+    HoughTransformation(m_HoughObjectsV);
+
+    // cout<<"  HoughClusteringAlg: Creating hough_spaceV"<<endl;
+    PandoraPlus::HoughSpace hough_spaceV(settings.map_floatPars["alpha_lowV"], settings.map_floatPars["alpha_highV"], 
+                                         settings.map_floatPars["bin_width_alphaV"], settings.map_intPars["Nbins_alphaV"], 
+                                         settings.map_floatPars["rho_low"], settings.map_floatPars["rho_high"], 
+                                         settings.map_floatPars["bin_width_rho"], settings.map_intPars["Nbins_rho"]);
+
+//cout<<"  HoughClusteringAlg: Filling hough_spaceV"<<endl;
+    FillHoughSpace(m_HoughObjectsV, hough_spaceV);
+
+//cout<<"  HoughClusteringAlg: Finding clusters from Hough space"<<endl;
+
+    //Create output HoughClusters
+    m_longiClusVCol.clear(); 
+    ClusterFinding(m_HoughObjectsV, hough_spaceV, m_longiClusVCol  );
+    CleanClusters(m_longiClusVCol);
+//cout << "  HoughClusteringAlg: final output m_longiClusVCol.size() = " << m_longiClusVCol.size() << endl;
+    m_datacol.map_HalfCluster["bkHalfCluster"].insert( m_datacol.map_HalfCluster["bkHalfCluster"].end(), m_longiClusVCol.begin(), m_longiClusVCol.end() );
+
+    std::vector<const PandoraPlus::CaloHalfCluster*> m_constHoughCluster; m_constHoughCluster.clear();
+    std::vector<const PandoraPlus::Calo1DCluster*> left_localMaxVCol; left_localMaxVCol.clear();
+    std::vector<const PandoraPlus::Calo1DCluster*> m_houghMax; m_houghMax.clear(); 
+    for(int is=0; is<tmp_localMaxVCol.size(); is++){
+      bool fl_incluster = false; 
+      for(int ic=0; ic<m_longiClusVCol.size(); ic++){
+        std::vector<const PandoraPlus::Calo1DCluster*> p_showers = m_longiClusVCol[ic]->getCluster();
+        if( find(p_showers.begin(), p_showers.end(), tmp_localMaxVCol[is])!=p_showers.end() ) { fl_incluster = true; break; }
+      }
+      if(!fl_incluster && find(left_localMaxVCol.begin(), left_localMaxVCol.end(), tmp_localMaxVCol[is])==left_localMaxVCol.end() ) left_localMaxVCol.push_back(tmp_localMaxVCol[is]);
+      m_houghMax.push_back( tmp_localMaxVCol[is] );
+    }
+    for(int ic=0; ic<m_longiClusVCol.size(); ic++)
+      m_constHoughCluster.push_back(m_longiClusVCol[ic].get());
+
+    //m_refHFClusVCol.insert(m_refHFClusVCol.end(), m_constHoughCluster.begin(), m_constHoughCluster.end());
+
+    p_HalfClusterV[it]->setLocalMax("HoughLocalMax", m_houghMax);
+    p_HalfClusterV[it]->setLocalMax(settings.map_stringPars["LeftLocalMaxName"], left_localMaxVCol);
+    p_HalfClusterV[it]->setHalfClusters(settings.map_stringPars["OutputLongiClusName"], m_constHoughCluster);
+    m_houghMax.clear();
+    left_localMaxVCol.clear();
+
+  }  // end of V plane
+//cout<<"Finish Hough in V. Reference HFClusterV size "<<m_refHFClusVCol.size()<<endl;
+
+
+  // Processing U(r-phi) plane
+  for(int it=0; it<p_HalfClusterU.size(); it++){ // process each HalfCluster respectively
+    m_localMaxUCol.clear();
+    std::vector<const PandoraPlus::Calo1DCluster*> tmp_localMaxUCol = p_HalfClusterU[it]->getLocalMaxCol(settings.map_stringPars["ReadinLocalMaxName"]);
+
+    for(int il=0; il<tmp_localMaxUCol.size(); il++){
+      if(tmp_localMaxUCol[il]->getDlayer()<=settings.map_intPars["th_Layers"])
+        m_localMaxUCol.push_back(tmp_localMaxUCol[il]);
+    }
+
+    if(m_localMaxUCol.size()<settings.map_intPars["th_peak"]){
+      //std::cout << "    yyy: m_localMaxUCol.size()<th_peak, continue" << std::endl;
+      continue;
+    }
+
+//cout<<"  HoughClusteringAlg: Find Hough axis in HalfCluster "<<it<<". Local maximum size U = "<<m_localMaxUCol.size()<<endl;
+
+    // cout<<"  HoughClusteringAlg: Creating m_HoughObjectsU"<<endl;
+    std::vector<PandoraPlus::HoughObject> m_HoughObjectsU; m_HoughObjectsU.clear();
+    for(int il=0; il<m_localMaxUCol.size(); il++){
+      PandoraPlus::HoughObject m_obj(m_localMaxUCol[il], PandoraPlus::CaloUnit::barsize, PandoraPlus::CaloUnit::ecal_innerR);
+      m_HoughObjectsU.push_back(m_obj);
+    }
+//cout<<"  HoughClusteringAlg: HoughObjectU size "<<m_HoughObjectsU.size()<<endl;
+
+    // cout<<"  HoughClusteringAlg: Hough transformation"<<endl;
+    HoughTransformation(m_HoughObjectsU);
+
+    // cout<<"  HoughClusteringAlg: Creating hough_spaceU"<<endl;
+    PandoraPlus::HoughSpace hough_spaceU(settings.map_floatPars["alpha_lowU"], settings.map_floatPars["alpha_highU"],
+                                         settings.map_floatPars["bin_width_alphaU"], settings.map_intPars["Nbins_alphaU"],
+                                         settings.map_floatPars["rho_low"], settings.map_floatPars["rho_high"],
+                                         settings.map_floatPars["bin_width_rho"], settings.map_intPars["Nbins_rho"]);
+
+//cout<<"  HoughClusteringAlg: Filling hough_spaceU"<<endl;
+    FillHoughSpace(m_HoughObjectsU, hough_spaceU);
+
+//cout<<"  HoughClusteringAlg: Finding clusters from Hough space"<<endl;
+    //Create output HoughClusters
+    m_longiClusUCol.clear();
+    ClusterFinding(m_HoughObjectsU, hough_spaceU, m_longiClusUCol  );
+    CleanClusters(m_longiClusUCol);
+//cout << "  HoughClusteringAlg: final output m_longiClusUCol.size() = " << m_longiClusUCol.size() << endl;
+    m_datacol.map_HalfCluster["bkHalfCluster"].insert( m_datacol.map_HalfCluster["bkHalfCluster"].end(), m_longiClusUCol.begin(), m_longiClusUCol.end() );
+
+    std::vector<const PandoraPlus::CaloHalfCluster*> m_constHoughCluster; m_constHoughCluster.clear();
+    std::vector<const PandoraPlus::Calo1DCluster*> left_localMaxUCol; left_localMaxUCol.clear();
+    std::vector<const PandoraPlus::Calo1DCluster*> m_houghMax; m_houghMax.clear();
+    for(int is=0; is<tmp_localMaxUCol.size(); is++){
+      bool fl_incluster = false;
+      for(int ic=0; ic<m_longiClusUCol.size(); ic++){
+        std::vector<const PandoraPlus::Calo1DCluster*> p_showers = m_longiClusUCol[ic]->getCluster();
+        if( find(p_showers.begin(), p_showers.end(), tmp_localMaxUCol[is])!=p_showers.end() ) { fl_incluster = true; break; }
+      }
+      if(!fl_incluster && find(left_localMaxUCol.begin(), left_localMaxUCol.end(), tmp_localMaxUCol[is])==left_localMaxUCol.end() ) left_localMaxUCol.push_back(tmp_localMaxUCol[is]);
+      m_houghMax.push_back( tmp_localMaxUCol[is] );
+    }
+    for(int ic=0; ic<m_longiClusUCol.size(); ic++)
+      m_constHoughCluster.push_back(m_longiClusUCol[ic].get());
+
+    p_HalfClusterU[it]->setLocalMax("HoughLocalMax", m_houghMax);
+    p_HalfClusterU[it]->setLocalMax(settings.map_stringPars["LeftLocalMaxName"], left_localMaxUCol);
+    p_HalfClusterU[it]->setHalfClusters(settings.map_stringPars["OutputLongiClusName"], m_constHoughCluster);
+    m_houghMax.clear();
+    left_localMaxUCol.clear();
+
+  }  // end of U plane
+
+
+
+
+
+
+/*
+  for(int it=0; it<p_HalfClusterU.size(); it++){
+    m_localMaxUCol.clear();
+    std::vector<const PandoraPlus::Calo1DCluster*> tmp_localMaxUCol = p_HalfClusterU[it]->getLocalMaxCol(settings.map_stringPars["ReadinLocalMaxName"]);
+
+    for(int il=0; il<tmp_localMaxUCol.size(); il++){
+      if(tmp_localMaxUCol[il]->getDlayer()<=settings.map_intPars["th_Layers"]) 
+        m_localMaxUCol.push_back(tmp_localMaxUCol[il]);
+    }
+
+    if(m_localMaxUCol.size()<settings.map_intPars["th_peak"]){
+      continue; 
+    } 
+//cout<<"  HoughClusteringAlg: Find Hough axis in HalfCluster "<<it<<". Local maximum size U = "<<m_localMaxUCol.size()<<endl;
+
+    std::map<int, std::vector<PandoraPlus::HoughObject> > map_HoughObjectsU_module; map_HoughObjectsU_module.clear();
+    std::map<int, std::vector<PandoraPlus::HoughObject> > map_HoughObjectsU_crack; map_HoughObjectsU_crack.clear();
+  
+    for(int il=0; il<m_localMaxUCol.size(); il++){
+      int module = m_localMaxUCol[il]->getTowerID()[0][0];
+      PandoraPlus::HoughObject m_obj(m_localMaxUCol[il], PandoraPlus::CaloUnit::barsize, PandoraPlus::CaloUnit::ecal_innerR);
+      map_HoughObjectsU_module[module].push_back(m_obj);
+    }
+    for(int iref=0; iref<m_refHFClusVCol.size(); iref++){
+      double tmp_phi = m_refHFClusVCol[iref]->getPos().Phi();  // yyy: tmp_phi ranges from -pi to pi
+//cout<<"    Ref HFCluster phi: "<<tmp_phi<<endl;
+      double intPart, fracPart; 
+      fracPart = modf((tmp_phi+TMath::Pi())/(TMath::Pi()/4.), &intPart);   // yyy: tmp_phi + TMath::Pi() ranges from 0 to 2pi
+//cout<<"    Int part "<<intPart<<", frac part "<<fracPart<<endl;
+      if(fracPart<0.489 || fracPart>0.711) continue;  //Not in crack region. 
+      
+      int iCrack = intPart+2;
+      if(iCrack>=8) iCrack = iCrack-8; 
+//cout<<"  Crack No: "<<iCrack<<endl;
+
+      for(int il=0; il<m_localMaxUCol.size(); il++){
+        if( (m_localMaxUCol[il]->getTowerID()[0][0]==iCrack && m_localMaxUCol[il]->getTowerID()[0][1]==4) ||
+            (iCrack!=7 && m_localMaxUCol[il]->getTowerID()[0][0]==iCrack+1 && m_localMaxUCol[il]->getTowerID()[0][1]==1) || 
+            (iCrack==7 && m_localMaxUCol[il]->getTowerID()[0][0]==0 && m_localMaxUCol[il]->getTowerID()[0][1]==1)){
+          PandoraPlus::HoughObject m_obj(m_localMaxUCol[il], PandoraPlus::CaloUnit::barsize, PandoraPlus::CaloUnit::ecal_innerR, tmp_phi);
+          map_HoughObjectsU_crack[iCrack].push_back(m_obj);
+        }
+      }
+    
+    }
+
+//cout<<"  Module HoughObject: "<<endl;
+//for(auto iter: map_HoughObjectsU_module)
+//printf("    Module #%d: object size %d \n", iter.first, iter.second.size());
+//cout<<"  Crack HoughObject: "<<endl;
+//for(auto iter: map_HoughObjectsU_crack)
+//printf("    Crack #%d: object size %d \n", iter.first, iter.second.size());
+
+
+    //Do hough transformation for HoughObjects 
+    for(auto &imodule: map_HoughObjectsU_module) HoughTransformation(imodule.second);
+    for(auto &icrack: map_HoughObjectsU_crack) HoughTransformation(icrack.second);
+    
+    //Fill Hough space
+    std::map<int, PandoraPlus::HoughSpace> hough_spacesU_module;
+    std::map<int, PandoraPlus::HoughSpace> hough_spacesU_crack;
+    for(auto &imodule: map_HoughObjectsU_module){
+      PandoraPlus::HoughSpace hspaceU(settings.map_floatPars["alpha_lowU"], settings.map_floatPars["alpha_highU"],
+                                      settings.map_floatPars["bin_width_alphaU"], settings.map_intPars["Nbins_alphaU"],
+                                      settings.map_floatPars["rho_low"], settings.map_floatPars["rho_high"],
+                                      settings.map_floatPars["bin_width_rho"], settings.map_intPars["Nbins_rho"]);
+      FillHoughSpace(imodule.second, hspaceU);
+      hough_spacesU_module[imodule.first] = hspaceU;
+    }
+    for(auto &icrack: map_HoughObjectsU_crack){
+      PandoraPlus::HoughSpace hspaceU(settings.map_floatPars["alpha_lowU"], settings.map_floatPars["alpha_highU"],
+                                      settings.map_floatPars["bin_width_alphaU"], settings.map_intPars["Nbins_alphaU"],
+                                      settings.map_floatPars["rho_low"], settings.map_floatPars["rho_high"],
+                                      settings.map_floatPars["bin_width_rho"], settings.map_intPars["Nbins_rho"]);
+      FillHoughSpace(icrack.second, hspaceU);
+      hough_spacesU_crack[icrack.first] = hspaceU;
+    }
+//cout<<"  Module Hough space size: "<<hough_spacesU_module.size()<<endl;
+//cout<<"  Crack Hough space size: "<<hough_spacesU_module.size()<<endl;
+
+    m_longiClusUCol.clear();
+    for(auto &imodule: map_HoughObjectsU_module)
+      ClusterFinding(imodule.second, hough_spacesU_module[imodule.first], m_longiClusUCol );
+    for(auto &icrack: map_HoughObjectsU_crack)
+      ClusterFinding(icrack.second, hough_spacesU_crack[icrack.first], m_longiClusUCol );
+
+//cout<<"  Hough axis size: "<<m_longiClusUCol.size()<<endl;
+    CleanClusters(m_longiClusUCol);
+    m_datacol.map_HalfCluster["bkHalfCluster"].insert( m_datacol.map_HalfCluster["bkHalfCluster"].end(), m_longiClusUCol.begin(), m_longiClusUCol.end() );
+//cout<<"  Hough axis size after cleaning: "<<m_longiClusUCol.size()<<endl;
+//cout<<"  Print axis "<<endl;
+//for(int i=0; i<m_longiClusUCol.size(); i++){
+//  printf("    Axis #%d: hit size %d, type %d, address %p \n", i, m_longiClusUCol[i]->getCluster().size(), m_longiClusUCol[i]->getType(), m_longiClusUCol[i].get() );
+//}
+
+    std::vector<const PandoraPlus::CaloHalfCluster*> m_constHoughCluster; m_constHoughCluster.clear();
+    std::vector<const PandoraPlus::Calo1DCluster*> left_localMaxUCol; left_localMaxUCol.clear();
+    std::vector<const PandoraPlus::Calo1DCluster*> m_houghMax; m_houghMax.clear();
+    for(int is=0; is<tmp_localMaxUCol.size(); is++){
+      bool fl_incluster = false; 
+      for(int ic=0; ic<m_longiClusUCol.size(); ic++){
+        std::vector<const PandoraPlus::Calo1DCluster*> p_showers = m_longiClusUCol[ic]->getCluster();
+        if( find(p_showers.begin(), p_showers.end(), tmp_localMaxUCol[is])!=p_showers.end() ) { fl_incluster = true; break; }
+      }
+      if(!fl_incluster && find(left_localMaxUCol.begin(), left_localMaxUCol.end(), tmp_localMaxUCol[is])==left_localMaxUCol.end() ) left_localMaxUCol.push_back(tmp_localMaxUCol[is]);
+      else m_houghMax.push_back( tmp_localMaxUCol[is] );
+    }
+
+    for(int ic=0; ic<m_longiClusUCol.size(); ic++)
+      m_constHoughCluster.push_back(m_longiClusUCol[ic].get());
+
+    p_HalfClusterU[it]->setLocalMax("HoughLocalMax", m_houghMax);
+    p_HalfClusterU[it]->setLocalMax(settings.map_stringPars["LeftLocalMaxName"], left_localMaxUCol);
+    p_HalfClusterU[it]->setHalfClusters(settings.map_stringPars["OutputLongiClusName"], m_constHoughCluster);
+    m_houghMax.clear();
+    left_localMaxUCol.clear(); 
+
+  }  // end of U plane
+*/
+
+  return StatusCode::SUCCESS;
+}
+
+StatusCode HoughClusteringAlg::ClearAlgorithm(){
+  p_HalfClusterV.clear();
+  p_HalfClusterU.clear(); 
+  m_localMaxVCol.clear();
+  m_localMaxUCol.clear(); 
+  m_longiClusVCol.clear();
+  m_longiClusUCol.clear();
+
+  return StatusCode::SUCCESS;
+};
+
+
+StatusCode HoughClusteringAlg::HoughTransformation(std::vector<PandoraPlus::HoughObject>& Hobjects){
+  if(Hobjects.size()<settings.map_intPars["th_peak"]) return StatusCode::SUCCESS;
+
+  // range of alpha of different lines
+  double range12[2] = {0, 0};
+  double range34[2] = {0, 0};
+
+  for(int iobj=0; iobj<Hobjects.size(); iobj++){
+    int t_slayer = Hobjects[iobj].getSlayer();
+    //SetLineRange(t_module, t_slayer, range12, range34);
+    double point_Phi = Hobjects[iobj].getCenterPoint().Phi();
+    double alpha_min, alpha_max;
+
+    if(t_slayer==0){
+      if(point_Phi<TMath::PiOver2()){
+        alpha_min = TMath::PiOver2();
+        alpha_max = TMath::Pi();
+      }
+      else{   
+        alpha_min = 0;
+        alpha_max = TMath::PiOver2();
+      }
+    }
+    else{
+      if( point_Phi < 5*TMath::Pi()/8. && point_Phi >= TMath::PiOver2() ){
+        alpha_min = -0.1;
+        alpha_max = TMath::PiOver4();
+      }
+      else if(point_Phi>3*TMath::Pi()/8. && point_Phi<TMath::PiOver2()){
+        alpha_min = 7.*TMath::Pi()/4.;
+        alpha_max = 2*TMath::Pi();
+      }
+      else{
+        alpha_min = floor(4*point_Phi/TMath::Pi() - 1.5)*TMath::PiOver4() - TMath::PiOver4();
+        alpha_max = floor(4*point_Phi/TMath::Pi() - 1.5)*TMath::PiOver4() + TMath::PiOver4();
+      }
+   
+      if(alpha_min<=0 && alpha_max<=0){
+        alpha_min += 2*TMath::Pi();
+        alpha_max += 2*TMath::Pi();
+      }
+    }
+
+    TF1 line1("line1", "[0]*cos(x)+[1]*sin(x)", alpha_min, alpha_max);
+    TF1 line2("line2", "[0]*cos(x)+[1]*sin(x)", alpha_min, alpha_max);
+    //TF1 line3("line3", "[0]*cos(x)+[1]*sin(x)", range34[0], range34[1]);
+    //TF1 line4("line4", "[0]*cos(x)+[1]*sin(x)", range34[0], range34[1]);
+
+    if(t_slayer==0){
+      line1.SetParameters( Hobjects[iobj].getUpperPoint().X(), Hobjects[iobj].getUpperPoint().Y() );
+      line2.SetParameters( Hobjects[iobj].getLowerPoint().X(), Hobjects[iobj].getLowerPoint().Y() );
+      //line3.SetParameters( Hobjects[iobj].getPointUL().X(), Hobjects[iobj].getPointUL().Y() );
+      //line4.SetParameters( Hobjects[iobj].getPointDR().X(), Hobjects[iobj].getPointDR().Y() );
+    }
+    else if(t_slayer==1){
+      //if(t_module % 2 == 0){
+        line1.SetParameters( Hobjects[iobj].getUpperPoint().X(), Hobjects[iobj].getUpperPoint().Y() );
+        line2.SetParameters( Hobjects[iobj].getLowerPoint().X(), Hobjects[iobj].getLowerPoint().Y() );
+        //line3.SetParameters( Hobjects[iobj].getPointUL().X(), Hobjects[iobj].getPointUL().Y() );
+        //line4.SetParameters( Hobjects[iobj].getPointDR().X(), Hobjects[iobj].getPointDR().Y() );
+      //}else{
+      //  line1.SetParameters( Hobjects[iobj].getPointU().X(), Hobjects[iobj].getPointU().Y() );
+      //  line2.SetParameters( Hobjects[iobj].getPointD().X(), Hobjects[iobj].getPointD().Y() );
+      //  line3.SetParameters( Hobjects[iobj].getPointL().X(), Hobjects[iobj].getPointL().Y() );
+      //  line4.SetParameters( Hobjects[iobj].getPointR().X(), Hobjects[iobj].getPointR().Y() );
+      //}
+    }
+    
+    Hobjects[iobj].setHoughLine(line1, line2);  
+
+  }
+
+  return StatusCode::SUCCESS;
+}  // HoughTransformation() end
+
+/*
+StatusCode HoughClusteringAlg::SetLineRange(int module, int slayer, double *range12, double* range34){
+  // range12: ur, dl, u, d
+  // range34: ul, dr, l, r
+  if(slayer == 0){
+    range12[0] = 0.;
+    range12[1] = TMath::Pi()/2.;
+    range34[0] = TMath::Pi()/2.;
+    range34[1] = TMath::Pi();
+  }
+  else if(slayer == 1){
+    switch(module){
+      case 0:{
+        range12[0] = -0.1;
+        range12[1] = TMath::Pi()/4.;
+        range34[0] = 7.*TMath::Pi()/4.;
+        range34[1] = range34[0] + TMath::Pi()/4.;
+        break;
+      }
+      case 1:{
+        range12[0] = TMath::Pi()/4.;
+        range12[1] = range12[0] + TMath::Pi()/4.;
+        range34[0] = 0;
+        range34[1] = range34[0] + TMath::Pi()/4.;
+        break;
+      }
+      case 2:{
+        range12[0] = TMath::Pi()/4.;
+        range12[1] = range12[0] + TMath::Pi()/4.;
+        range34[0] = TMath::Pi()/2;
+        range34[1] = range34[0] + TMath::Pi()/4.;
+        break;
+      }
+      case 3:{
+        range12[0] = TMath::Pi()/2.;
+        range12[1] = range12[0] + TMath::Pi()/4.;
+        range34[0] = 3.*TMath::Pi()/4.;
+        range34[1] = range34[0] + TMath::Pi()/4.;
+        break;
+      }
+      case 4:{
+        range12[0] = TMath::Pi();
+        range12[1] = range12[0] + TMath::Pi()/4.;
+        range34[0] = 3.*TMath::Pi()/4.;
+        range34[1] = range34[0] + TMath::Pi()/4.;
+        break;
+      }
+      case 5:{
+        range12[0] = 5.*TMath::Pi()/4.;
+        range12[1] = range12[0] + TMath::Pi()/4.;
+        range34[0] = TMath::Pi();
+        range34[1] = range34[0] + TMath::Pi()/4.;
+        break;
+      }
+      case 6:{
+        range12[0] = 5.*TMath::Pi()/4.;
+        range12[1] = range12[0] + TMath::Pi()/4.;
+        range34[0] = 3.*TMath::Pi()/2.;
+        range34[1] = range34[0] + TMath::Pi()/4.;
+        break;
+      }
+      case 7:{
+        range12[0] = 3.*TMath::Pi()/2.;
+        range12[1] = range12[0] + TMath::Pi()/4.;
+        range34[0] = 7.*TMath::Pi()/4.;
+        range34[1] = range34[0] + TMath::Pi()/4.;
+        break;
+      }
+      default:{
+        cout << "Wrong module: module = " << module << endl;
+      }
+    }
+  }
+
+  return StatusCode::SUCCESS;
+}  // SetLineRange() end
+*/
+
+StatusCode HoughClusteringAlg::FillHoughSpace(vector<PandoraPlus::HoughObject>& Hobjects, PandoraPlus::HoughSpace& Hspace){  
+
+  // Fill Hough space
+  // Loop Hough objects
+  for(int ih=0; ih<Hobjects.size(); ih++){
+    TF1 line1 = Hobjects[ih].getHoughLine1();
+    TF1 line2 = Hobjects[ih].getHoughLine2();
+    //TF1 line3 = Hobjects[ih].getHoughLine3();
+    //TF1 line4 = Hobjects[ih].getHoughLine4();
+
+    // line1 and line2 share the same range in alpha, so does line3 and line4
+    double range_min, range_max;
+    line1.GetRange(range_min, range_max);
+    
+    // Get bin num in alpha axis
+    int bin_min = Hspace.getAlphaBin(range_min);
+    int bin_max = Hspace.getAlphaBin(range_max);
+    //int bin_34_min = Hspace.getAlphaBin(range_34_min);
+    //int bin_34_max = Hspace.getAlphaBin(range_34_max);
+    //if (bin_12_max == bin_34_min) bin_34_min ++;
+    //if (bin_34_max == bin_12_min) bin_12_min ++;
+
+
+    // Loop for alpha bins, line1 and line2
+    for(int ialpha=bin_min; ialpha<=bin_max; ialpha++) {
+      // The lines should be monotone at this range
+      double line1_rho1 = line1.Eval( Hspace.getAlphaBinLowEdge(ialpha) );
+      double line1_rho2 = line1.Eval( Hspace.getAlphaBinUpEdge(ialpha)  );
+      double line2_rho1 = line2.Eval( Hspace.getAlphaBinLowEdge(ialpha) );
+      double line2_rho2 = line2.Eval( Hspace.getAlphaBinUpEdge(ialpha)  );
+
+      double line1_rho_min = TMath::Min(line1_rho1, line1_rho2);
+      double line1_rho_max = TMath::Max(line1_rho1, line1_rho2);
+      double line2_rho_min = TMath::Min(line2_rho1, line2_rho2);
+      double line2_rho_max = TMath::Max(line2_rho1, line2_rho2);
+
+      if(line1_rho_min>line1_rho_max || line2_rho_min>line2_rho_max){
+        cout << "!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!" << endl;
+      }
+
+      double rho_min = TMath::Min( line1_rho_min, line2_rho_min);
+      double rho_max = TMath::Max( line1_rho_max, line2_rho_max);
+
+      if(rho_max<settings.map_floatPars["rho_low"] || rho_min>settings.map_floatPars["rho_high"]) continue;
+
+      int nbin_rho_min = TMath::Max( int(ceil( (rho_min-settings.map_floatPars["rho_low"]) / settings.map_floatPars["bin_width_rho"] )), 1 );
+      int nbin_rho_max = TMath::Min( int(ceil( (rho_max-settings.map_floatPars["rho_low"]) / settings.map_floatPars["bin_width_rho"] )), settings.map_intPars["Nbins_rho"] );
+
+
+      for(int irho=nbin_rho_min; irho<=nbin_rho_max; irho++){
+        Hspace.AddBinHobj(ialpha, irho, ih);
+      }
+    }  // end loop alpha bin, line1 and line2
+/*    // Loop for alpha bins, line3 and line4
+    for(int ialpha=bin_34_min; ialpha<=bin_34_max; ialpha++) {
+      // The lines should be monotone at this range
+      double line3_rho1 = line3.Eval( Hspace.getAlphaBinLowEdge(ialpha) );
+      double line3_rho2 = line3.Eval( Hspace.getAlphaBinUpEdge(ialpha)  );
+      double line4_rho1 = line4.Eval( Hspace.getAlphaBinLowEdge(ialpha) );
+      double line4_rho2 = line4.Eval( Hspace.getAlphaBinUpEdge(ialpha)  );
+
+      double line3_rho_min = TMath::Min(line3_rho1, line3_rho2);
+      double line3_rho_max = TMath::Max(line3_rho1, line3_rho2);;
+      double line4_rho_min = TMath::Min(line4_rho1, line4_rho2);
+      double line4_rho_max = TMath::Max(line4_rho1, line4_rho2);
+
+      if(line3_rho_min>line3_rho_max || line4_rho_min>line4_rho_max){
+        cout << "!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!" << endl;
+      }
+
+      double rho_min = TMath::Min( line3_rho_min, line4_rho_min);
+      double rho_max = TMath::Max( line3_rho_max, line4_rho_max);
+
+      if(rho_max<settings.map_floatPars["rho_low"] || rho_min>settings.map_floatPars["rho_high"]) continue;
+
+      int nbin_rho_min = TMath::Max( int(ceil( (rho_min-settings.map_floatPars["rho_low"]) / settings.map_floatPars["bin_width_rho"] )), 1 );
+      int nbin_rho_max = TMath::Min( int(ceil( (rho_max-settings.map_floatPars["rho_low"]) / settings.map_floatPars["bin_width_rho"] )), settings.map_intPars["Nbins_rho"] );
+
+      for(int irho=nbin_rho_min; irho<=nbin_rho_max; irho++){
+        Hspace.AddBinHobj(ialpha, irho, ih);
+      }
+    }  // end loop alpha bin, line3 and line4
+*/
+  }  // End loop Hough objects
+
+  return StatusCode::SUCCESS;
+}  // FillHoughSpace() end
+
+
+StatusCode HoughClusteringAlg::ClusterFinding(vector<PandoraPlus::HoughObject>& Hobjects, PandoraPlus::HoughSpace& Hspace, 
+                                              std::vector<std::shared_ptr<PandoraPlus::CaloHalfCluster>>& m_longiClusCol ){
+  if(Hobjects.size()==0) return StatusCode::SUCCESS;
+
+  map< pair<int, int>, set<int> > Hough_bins = Hspace.getHoughBins();
+  // transform candidate to longicluster
+  vector<std::shared_ptr<PandoraPlus::CaloHalfCluster>> m_clusCol; m_clusCol.clear();
+  for(auto ihb : Hough_bins){
+    if(ihb.second.size()<settings.map_intPars["th_peak"]) continue;
+    
+    std::shared_ptr<PandoraPlus::CaloHalfCluster> m_clus = std::make_shared<PandoraPlus::CaloHalfCluster>();
+    for(auto it = (ihb.second).begin(); it!=(ihb.second).end(); it++){
+      m_clus->addUnit(Hobjects[*it].getLocalMax());
+    }
+
+    double t_alpha = Hspace.getAlphaBinCenter((ihb.first).first);
+    double t_rho = Hspace.getRhoBinCenter((ihb.first).second);
+    m_clus->setHoughPars(t_alpha, t_rho);
+
+    if( !m_clus->isContinueN(settings.map_intPars["th_continueN"]) ){
+      continue;
+    } 
+    m_clus->setType(100); //EM-type axis.     
+    m_clus->getLinkedMCPfromUnit();
+    m_clusCol.push_back(m_clus);
+  }
+  // Clean cluster
+  //CleanClusters(m_clusCol);
+
+  //bk_HFclus.insert( bk_HFclus.end(), m_clusCol.begin(), m_clusCol.end() );
+  m_longiClusCol.insert( m_longiClusCol.end(), m_clusCol.begin(), m_clusCol.end() );
+
+  return StatusCode::SUCCESS;
+}  // ClusterFinding() end
+
+
+StatusCode HoughClusteringAlg::CleanClusters( std::vector<std::shared_ptr<PandoraPlus::CaloHalfCluster>>& m_longiClusCol){
+
+  if(m_longiClusCol.size()==0)  return StatusCode::SUCCESS;
+
+  // Remove repeated tracks
+  for(int ic=0; ic<m_longiClusCol.size(); ic++){
+  for(int jc=0; jc<m_longiClusCol.size(); jc++){
+    if(ic>=m_longiClusCol.size()) ic--;
+    if(ic==jc) continue;
+
+    if( m_longiClusCol[ic].get()->isSubset(m_longiClusCol[jc].get()) ){  //jc is the subset of ic. remove jc. 
+      //delete m_longiClusCol[jc]; m_longiClusCol[jc] = NULL;
+      m_longiClusCol.erase(m_longiClusCol.begin()+jc );
+      jc--;
+      if(ic>jc+1) ic--;
+    }
+  }}
+
+  //Depart the HoughCluster to 2 sub-clusters if it has blank in middle.
+  for(int ic=0; ic<m_longiClusCol.size(); ic++){
+    int m_nhit = m_longiClusCol[ic].get()->getCluster().size(); 
+    m_longiClusCol[ic].get()->sortBarShowersByLayer();
+
+    for(int ih=0; ih<m_nhit-1; ih++){
+      if(m_longiClusCol[ic].get()->getCluster()[ih+1]->getDlayer() - m_longiClusCol[ic].get()->getCluster()[ih]->getDlayer() > 2){
+        //PandoraPlus::CaloHalfCluster* clus_head = new PandoraPlus::CaloHalfCluster();
+        //PandoraPlus::CaloHalfCluster* clus_tail = new PandoraPlus::CaloHalfCluster();
+        std::shared_ptr<PandoraPlus::CaloHalfCluster> clus_head = std::make_shared<PandoraPlus::CaloHalfCluster>();
+        std::shared_ptr<PandoraPlus::CaloHalfCluster> clus_tail = std::make_shared<PandoraPlus::CaloHalfCluster>();
+
+        for(int jh=0; jh<=ih; jh++) 
+          clus_head->addUnit( m_longiClusCol[ic].get()->getCluster()[jh]);
+        for(int jh=ih+1; jh<m_nhit; jh++) 
+          clus_tail->addUnit( m_longiClusCol[ic].get()->getCluster()[jh]);
+        
+        if( clus_head->isContinueN(settings.map_intPars["th_continueN"]) ) {
+            clus_head->setType(100); 
+            clus_head->setHoughPars(m_longiClusCol[ic].get()->getHoughAlpha(), m_longiClusCol[ic].get()->getHoughRho());
+            clus_head->getLinkedMCPfromUnit();
+            m_longiClusCol.push_back(clus_head);
+        }
+        //else{
+        //  delete clus_head;
+        //}
+        if( clus_tail->isContinueN(settings.map_intPars["th_continueN"]) ) {
+          clus_tail->setHoughPars(m_longiClusCol[ic].get()->getHoughAlpha(), m_longiClusCol[ic].get()->getHoughRho());
+          clus_tail->setType(100);
+          clus_tail->getLinkedMCPfromUnit();
+          m_longiClusCol.push_back(clus_tail);
+        }
+        //else{
+        //  delete clus_tail;
+        //}
+            
+
+        //delete m_longiClusCol[ic]; m_longiClusCol[ic] = NULL;
+        m_longiClusCol.erase(m_longiClusCol.begin()+ic);
+        ic--;
+        break;
+      }
+    }
+  }
+
+  // Remove repeated tracks
+  for(int ic=0; ic<m_longiClusCol.size(); ic++){
+  for(int jc=0; jc<m_longiClusCol.size(); jc++){
+    if(ic>=m_longiClusCol.size()) ic--;
+    if(ic==jc) continue;
+
+    if( m_longiClusCol[ic].get()->isSubset(m_longiClusCol[jc].get()) ){  //jc is the subset of ic. remove jc. 
+      //delete m_longiClusCol[jc]; m_longiClusCol[jc] = NULL;
+      m_longiClusCol.erase(m_longiClusCol.begin()+jc );
+      jc--;
+      if(ic>jc+1) ic--;
+    }
+  }}
+
+  // Cut energy
+  for(int ic=0; ic<m_longiClusCol.size(); ic++){
+    if(m_longiClusCol[ic].get()->getEnergy()<settings.map_floatPars["th_AxisE"]){
+      //delete m_longiClusCol[ic]; m_longiClusCol[ic]=NULL;
+      m_longiClusCol.erase(m_longiClusCol.begin()+ic );
+      ic--;
+    }
+  }
+
+  // Overlap with other clusters: 
+    if(m_longiClusCol.size()>=2){
+      for(int ic=0; ic<m_longiClusCol.size()-1; ic++){
+      for(int jc=ic+1; jc<m_longiClusCol.size(); jc++){
+        if(ic>=m_longiClusCol.size()) ic--;
+
+        double delta_alpha = TMath::Abs(m_longiClusCol[ic].get()->getHoughAlpha() -  m_longiClusCol[jc].get()->getHoughAlpha());
+        if( (delta_alpha > settings.map_floatPars["th_dAlpha1"])
+            && (delta_alpha < 2*TMath::Pi()-settings.map_floatPars["th_dAlpha1"]) ) continue;
+
+        double m_ratio1 = m_longiClusCol[ic].get()->OverlapRatioE(m_longiClusCol[jc].get());
+        double m_ratio2 = m_longiClusCol[jc].get()->OverlapRatioE(m_longiClusCol[ic].get());
+
+        if(m_ratio1>settings.map_floatPars["th_overlapE"] && m_longiClusCol[ic].get()->getEnergy()<m_longiClusCol[jc].get()->getEnergy()){
+          //delete m_longiClusCol[ic]; m_longiClusCol[ic] = NULL;
+          m_longiClusCol.erase( m_longiClusCol.begin()+ic );
+          ic--;
+          break;
+        }
+
+        if(m_ratio2>settings.map_floatPars["th_overlapE"] && m_longiClusCol[jc].get()->getEnergy()<m_longiClusCol[ic].get()->getEnergy()){
+          //delete m_longiClusCol[jc]; m_longiClusCol[jc] = NULL;
+          m_longiClusCol.erase( m_longiClusCol.begin()+jc );
+          jc--;
+        }
+      }}
+  }
+
+  // If two cluster are close to each other, and E_small/E_large < threshold, delete the small ones
+  if(m_longiClusCol.size()>=2){
+    for(int ic=0; ic<m_longiClusCol.size()-1; ic++){
+    for(int jc=ic+1; jc<m_longiClusCol.size(); jc++){
+      if(ic>=m_longiClusCol.size()) ic--;
+
+      double delta_alpha = TMath::Abs(m_longiClusCol[ic].get()->getHoughAlpha() -  m_longiClusCol[jc].get()->getHoughAlpha());
+      if( delta_alpha > settings.map_floatPars["th_dAlpha2"] ) continue;
+
+      double E_ratio1 = m_longiClusCol[ic].get()->getEnergy() / m_longiClusCol[jc].get()->getEnergy();
+      double E_ratio2 = m_longiClusCol[jc].get()->getEnergy() / m_longiClusCol[ic].get()->getEnergy();
+
+      if( E_ratio1 < settings.map_floatPars["th_ERatio"] ){
+        //delete m_longiClusCol[ic]; m_longiClusCol[ic] = NULL;
+        m_longiClusCol.erase( m_longiClusCol.begin()+ic );
+        ic--;
+        break;
+      }
+      else if( E_ratio2 < settings.map_floatPars["th_ERatio"] ){
+        //delete m_longiClusCol[jc]; m_longiClusCol[jc] = NULL;
+        m_longiClusCol.erase( m_longiClusCol.begin()+jc );
+        jc--;
+      }
+    }}
+  }
+
+  return StatusCode::SUCCESS;
+}  // CleanClusters() end
+
+#endif
diff --git a/Reconstruction/CrystalCaloRec/src/Algorithm/LocalMaxFindingAlg.cpp b/Reconstruction/CrystalCaloRec/src/Algorithm/LocalMaxFindingAlg.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..7d261a890277f969024a17fbe9cfb04a50858a0f
--- /dev/null
+++ b/Reconstruction/CrystalCaloRec/src/Algorithm/LocalMaxFindingAlg.cpp
@@ -0,0 +1,150 @@
+#ifndef _LOCALMAXFINDING_ALG_C
+#define _LOCALMAXFINDING_ALG_C
+
+#include "Algorithm/LocalMaxFindingAlg.h"
+using namespace PandoraPlus; 
+
+StatusCode LocalMaxFindingAlg::ReadSettings(PandoraPlus::Settings& m_settings){
+  settings = m_settings;
+
+  if(settings.map_floatPars.find("Eth_localMax")==settings.map_floatPars.end()) settings.map_floatPars["Eth_localMax"] = 0.005;
+  if(settings.map_floatPars.find("Eth_MaxWithNeigh")==settings.map_floatPars.end()) settings.map_floatPars["Eth_MaxWithNeigh"] = 0.;
+  if(settings.map_stringPars.find("OutputLocalMaxName")==settings.map_stringPars.end()) settings.map_stringPars["OutputLocalMaxName"] = "AllLocalMax";
+  return StatusCode::SUCCESS;
+}
+
+StatusCode LocalMaxFindingAlg::Initialize( PandoraPlusDataCol& m_datacol ){
+  p_HalfClusU = NULL;
+  p_HalfClusV = NULL; 
+
+  p_HalfClusU = &(m_datacol.map_HalfCluster["HalfClusterColU"]);
+  p_HalfClusV = &(m_datacol.map_HalfCluster["HalfClusterColV"]);
+
+
+  return StatusCode::SUCCESS;
+}
+
+
+StatusCode LocalMaxFindingAlg::RunAlgorithm( PandoraPlusDataCol& m_datacol){
+
+  if(!p_HalfClusU || !p_HalfClusV) {std::cout<<"ERROR: No HalfClusters in present data collection! "<<std::endl; return StatusCode::FAILURE; }
+
+  for(int iu = 0; iu<p_HalfClusU->size(); iu++){
+    std::vector<const Calo1DCluster*> m_1dClusCol = p_HalfClusU->at(iu).get()->getCluster();
+
+    std::vector<std::shared_ptr<PandoraPlus::Calo1DCluster>> ptr_localMax; ptr_localMax.clear();
+    for(int i1d=0; i1d<m_1dClusCol.size(); i1d++) GetLocalMax(m_1dClusCol[i1d], ptr_localMax);
+
+    std::vector<const PandoraPlus::Calo1DCluster*> tmp_localMax; tmp_localMax.clear();
+    for(auto iter : ptr_localMax){
+      iter->getLinkedMCPfromUnit();
+      tmp_localMax.push_back(iter.get());
+      m_datacol.map_1DCluster["bk1DCluster"].push_back(iter);
+    }
+    p_HalfClusU->at(iu).get()->setLocalMax(settings.map_stringPars["OutputLocalMaxName"], tmp_localMax);
+    m_1dClusCol.clear(); 
+    ptr_localMax.clear();
+  }
+
+  for(int iv=0; iv<p_HalfClusV->size(); iv++){
+    std::vector<const Calo1DCluster*> m_1dClusCol = p_HalfClusV->at(iv).get()->getCluster();
+
+    std::vector<std::shared_ptr<PandoraPlus::Calo1DCluster>> ptr_localMax; ptr_localMax.clear();
+    for(int i1d=0; i1d<m_1dClusCol.size(); i1d++) GetLocalMax(m_1dClusCol[i1d], ptr_localMax);
+
+    std::vector<const PandoraPlus::Calo1DCluster*> tmp_localMax; tmp_localMax.clear();
+    for(auto iter : ptr_localMax){
+      iter->getLinkedMCPfromUnit();
+      tmp_localMax.push_back(iter.get());
+      m_datacol.map_1DCluster["bk1DCluster"].push_back(iter);
+    }
+    p_HalfClusV->at(iv).get()->setLocalMax(settings.map_stringPars["OutputLocalMaxName"], tmp_localMax);
+    m_1dClusCol.clear(); 
+    ptr_localMax.clear();
+  }
+
+  return StatusCode::SUCCESS;
+}
+
+StatusCode LocalMaxFindingAlg::ClearAlgorithm(){
+  p_HalfClusU = nullptr;
+  p_HalfClusV = nullptr;
+
+  return StatusCode::SUCCESS;
+}
+
+StatusCode LocalMaxFindingAlg::GetLocalMax( const PandoraPlus::Calo1DCluster* m_1dClus, 
+                                            std::vector<std::shared_ptr<PandoraPlus::Calo1DCluster>>& m_output){
+
+  if(m_1dClus->getBars().size()==0) return StatusCode::SUCCESS;
+
+  std::vector<const PandoraPlus::CaloUnit*> m_barCol = m_1dClus->getBars();
+
+//cout<<"  LocalMaxFindingAlg::GetLocalMax: Input bar collection size: "<<m_barCol.size()<<endl;
+
+  std::vector<const PandoraPlus::CaloUnit*> localMaxCol; localMaxCol.clear();
+
+  GetLocalMaxBar( m_barCol, localMaxCol );
+
+//cout<<"  LocalMaxFindingAlg::GetLocalMax: Found local max bar size: "<<localMaxCol.size()<<endl;
+//cout<<"  Transfer bar to barShower"<<endl;
+
+  for(int j=0; j<localMaxCol.size(); j++){
+    std::shared_ptr<PandoraPlus::Calo1DCluster> m_shower = std::make_shared<PandoraPlus::Calo1DCluster>();
+    m_shower->addUnit( localMaxCol[j] );
+    m_output.push_back(m_shower);
+  }
+
+//cout<<"  Output bar shower size: "<<m_output.size()<<endl;
+//cout<<endl;
+
+  return StatusCode::SUCCESS;
+}
+
+StatusCode LocalMaxFindingAlg::GetLocalMaxBar( std::vector<const PandoraPlus::CaloUnit*>& barCol, std::vector<const PandoraPlus::CaloUnit*>& localMaxCol ){
+  //std::sort( barCol.begin(), barCol.end(), compBar );
+  for(int ib=0; ib<barCol.size(); ib++){
+    std::vector<const PandoraPlus::CaloUnit*> m_neighbors = getNeighbors( barCol[ib], barCol );
+    if( m_neighbors.size()==0 && barCol[ib]->getEnergy()>settings.map_floatPars["Eth_localMax"] ) { 
+      localMaxCol.push_back( barCol[ib] ); continue; 
+    }
+
+    bool isLocalMax=true;
+    double Eneigh=0;
+    if(barCol[ib]->getEnergy()<settings.map_floatPars["Eth_localMax"]) isLocalMax = false;
+    for(int j=0;j<m_neighbors.size();j++){
+      if(m_neighbors[j]->getEnergy() > barCol[ib]->getEnergy()) isLocalMax=false;
+      Eneigh += m_neighbors[j]->getEnergy();
+    }
+    if( (barCol[ib]->getEnergy()/(barCol[ib]->getEnergy()+Eneigh))<settings.map_floatPars["Eth_MaxWithNeigh"] ) isLocalMax = false;
+    if(isLocalMax) localMaxCol.push_back( barCol[ib] );
+  }
+
+  return StatusCode::SUCCESS;
+}
+
+
+std::vector<const PandoraPlus::CaloUnit*> LocalMaxFindingAlg::getNeighbors( const PandoraPlus::CaloUnit* seed, std::vector<const PandoraPlus::CaloUnit*>& barCol){
+  std::vector<const PandoraPlus::CaloUnit*> m_neighbor; m_neighbor.clear();
+  for(int i=0;i<barCol.size();i++){
+    bool fl_neighbor = false; 
+    if( seed->getModule()==barCol[i]->getModule() && 
+        seed->getStave()==barCol[i]->getStave() && 
+        seed->getDlayer()==barCol[i]->getDlayer() &&
+        seed->getSlayer()==barCol[i]->getSlayer() &&
+        abs( seed->getBar()-barCol[i]->getBar() )==1 ) fl_neighbor=true;
+    else if( seed->getStave()==barCol[i]->getStave() && 
+             ( ( seed->getModule()-barCol[i]->getModule()==1 && seed->isAtLowerEdgePhi() && barCol[i]->isAtUpperEdgePhi() ) ||
+               ( barCol[i]->getModule()-seed->getModule()==1 && seed->isAtUpperEdgePhi() && barCol[i]->isAtLowerEdgePhi() ) ) ) fl_neighbor=true;
+    else if( seed->getModule()==barCol[i]->getModule() && 
+             ( ( seed->getStave()-barCol[i]->getStave()==1 && seed->isAtLowerEdgeZ() && barCol[i]->isAtUpperEdgeZ() ) || 
+               ( barCol[i]->getStave()-seed->getStave()==1 && seed->isAtUpperEdgeZ() && barCol[i]->isAtLowerEdgeZ() ) ) ) fl_neighbor=true;
+
+    if(fl_neighbor) m_neighbor.push_back(barCol[i]);
+  }
+  if(m_neighbor.size()>2) std::cout<<"WARNING: more than 2 hits in neighborCol!!"<<std::endl;
+
+  return m_neighbor;
+}
+
+#endif
diff --git a/Reconstruction/CrystalCaloRec/src/Algorithm/PFOCreatingAlg.cpp b/Reconstruction/CrystalCaloRec/src/Algorithm/PFOCreatingAlg.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..1d044542744bab60294e964020614b672392e9f2
--- /dev/null
+++ b/Reconstruction/CrystalCaloRec/src/Algorithm/PFOCreatingAlg.cpp
@@ -0,0 +1,278 @@
+#ifndef _PFOCREATING_ALG_C
+#define _PFOCREATING_ALG_C
+
+#include "Algorithm/PFOCreatingAlg.h"
+
+StatusCode PFOCreatingAlg::ReadSettings(Settings& m_settings){
+  settings = m_settings;
+
+  if(settings.map_stringPars.find("ReadinECALClusters")==settings.map_stringPars.end()) settings.map_stringPars["ReadinECALClusters"] = "EcalCluster";
+  if(settings.map_stringPars.find("ReadinHCALClusters")==settings.map_stringPars.end()) settings.map_stringPars["ReadinHCALClusters"] = "HCALCluster";
+  if(settings.map_stringPars.find("OutputCombPFO")==settings.map_stringPars.end()) settings.map_stringPars["OutputCombPFO"] = "outputPFO";
+
+  if(settings.map_floatPars.find("delta_phi_cut")==settings.map_floatPars.end())
+    settings.map_floatPars["delta_phi_cut"] = 30./180.*TMath::Pi(); 
+  if(settings.map_floatPars.find("delta_cosTheta_cut")==settings.map_floatPars.end())
+    settings.map_floatPars["delta_cosTheta_cut"] = 0.25; 
+  if(settings.map_floatPars.find("extrPoint_HCALHit_distCut")==settings.map_floatPars.end())
+    settings.map_floatPars["extrPoint_HCALHit_distCut"] = 100; 
+  if(settings.map_floatPars.find("nearby_clus_angleCut")==settings.map_floatPars.end())
+    settings.map_floatPars["nearby_clus_angleCut"] = 0.15; 
+  return StatusCode::SUCCESS;
+};
+
+StatusCode PFOCreatingAlg::Initialize( PandoraPlusDataCol& m_datacol ){
+  m_tracks.clear();
+  m_ecal_clusters.clear();
+  m_hcal_clusters.clear();
+  m_pfobjects.clear();
+
+  for(int it=0; it<m_datacol.TrackCol.size(); it++){
+    m_tracks.push_back( m_datacol.TrackCol[it].get() );
+  }
+  for(int ie=0; ie<m_datacol.map_CaloCluster[settings.map_stringPars["ReadinECALClusters"]].size(); ie++){
+    m_ecal_clusters.push_back( m_datacol.map_CaloCluster[settings.map_stringPars["ReadinECALClusters"]][ie].get() );
+  }
+  for(int ih=0; ih<m_datacol.map_CaloCluster[settings.map_stringPars["ReadinHCALClusters"]].size(); ih++){
+    m_hcal_clusters.push_back( m_datacol.map_CaloCluster[settings.map_stringPars["ReadinHCALClusters"]][ih].get() );
+  }
+
+  return StatusCode::SUCCESS;
+};
+
+StatusCode PFOCreatingAlg::RunAlgorithm( PandoraPlusDataCol& m_datacol ){
+  std::cout << "yyy: Running PFOCreatingAlg" << std::endl;
+
+  if(m_tracks.size()==0 && m_ecal_clusters.size()==0 && m_hcal_clusters.size()==0){
+    std::cout << "  yyy: No tracks, no clusters in ECAL and HCAL. End PFOCreatingAlg" << std::endl;
+    return StatusCode::SUCCESS;
+  }
+
+  // Create PFO with ECAl clusters. If a ECAL cluster is a charged cluster, connect HCAL clusters using extrapolated points
+  for(int ie=0; ie<m_ecal_clusters.size(); ie++){
+    std::vector<const PandoraPlus::Track*> ecal_cls_track = m_ecal_clusters[ie]->getAssociatedTracks();
+    if(ecal_cls_track.size()>1){
+      std::cout << "Error! " << ecal_cls_track.size() << " tracks associated to one ECAL cluster!" << std::endl;
+      continue;
+    }
+
+    // Charged cluster in ECAL (A cluster with a track)
+    if(ecal_cls_track.size()==1){  
+      std::vector<PandoraPlus::Calo3DCluster*> hcal_clus_candidate;
+      hcal_clus_candidate.clear();
+      GetChargedHCALCandidates(ecal_cls_track[0], m_hcal_clusters, hcal_clus_candidate);
+
+      std::shared_ptr<PandoraPlus::PFObject> tmp_pfo = std::make_shared<PandoraPlus::PFObject>();
+      tmp_pfo->addTrack(ecal_cls_track[0]);
+      tmp_pfo->addECALCluster(m_ecal_clusters[ie]);
+      for(int ic=0; ic<hcal_clus_candidate.size(); ic++){
+        tmp_pfo->addHCALCluster(hcal_clus_candidate[ic]);
+      }
+      m_pfobjects.push_back(tmp_pfo);
+
+      CleanUsedElements(hcal_clus_candidate, m_hcal_clusters);
+      CleanUsedElements(ecal_cls_track, m_tracks);
+    }
+
+    // Neutral cluster in ECAL (A cluster without track)
+    else if(ecal_cls_track.size()==0){
+      // Create PFO with only a ECAL cluster
+      std::shared_ptr<PandoraPlus::PFObject> tmp_pfo = std::make_shared<PandoraPlus::PFObject>();
+      tmp_pfo->addECALCluster(m_ecal_clusters[ie]);
+      m_pfobjects.push_back(tmp_pfo);
+      
+      
+    }
+    else{
+      cout << "  yyy: Error: Wrong number of tracks" << endl;
+    }
+  }
+
+  // Create PFO with only tracks
+  for(int it=0; it<m_tracks.size(); it++){
+    std::shared_ptr<PandoraPlus::PFObject> tmp_pfo = std::make_shared<PandoraPlus::PFObject>();
+    tmp_pfo->addTrack(m_tracks[it]);
+    m_pfobjects.push_back(tmp_pfo);
+  }
+
+  
+  // Connect left HCAL clusters to PFO. These PFO are made of ECAL clusters( and tracks) now
+  for(int ih=0; ih<m_hcal_clusters.size(); ih++){
+    TVector3 hcal_clus_pos = m_hcal_clusters[ih]->getHitCenter();
+    double min_angle = 999.0;
+    int pfo_index = -1;
+    
+    for(int ip=0; ip<m_pfobjects.size(); ip++){
+      std::vector<const PandoraPlus::Calo3DCluster*> pf_ecal_clus = m_pfobjects[ip].get()->getECALClusters();
+      for(int ie=0; ie<pf_ecal_clus.size(); ie++){
+        TVector3 ecal_clus_pos = pf_ecal_clus[ie]->getShowerCenter();
+
+        double angle = hcal_clus_pos.Angle(ecal_clus_pos);
+        if(angle<settings.map_floatPars["nearby_clus_angleCut"] && angle<min_angle){
+          min_angle=angle;
+          pfo_index = ip;
+        }
+      } 
+    }
+
+    if(pfo_index>=0){
+      m_pfobjects[pfo_index].get()->addHCALCluster(m_hcal_clusters[ih]);
+    }
+    else{
+      std::shared_ptr<PandoraPlus::PFObject> tmp_pfo = std::make_shared<PandoraPlus::PFObject>();
+      tmp_pfo->addHCALCluster(m_hcal_clusters[ih]);
+      m_pfobjects.push_back(tmp_pfo);
+    }
+
+  }  
+
+  m_datacol.map_PFObjects[settings.map_stringPars["OutputCombPFO"]] = m_pfobjects;
+
+  
+
+  return StatusCode::SUCCESS;
+};
+
+StatusCode PFOCreatingAlg::ClearAlgorithm(){
+  m_tracks.clear();
+  m_ecal_clusters.clear();
+  m_hcal_clusters.clear();
+  m_pfobjects.clear();
+
+  return StatusCode::SUCCESS;
+}
+
+StatusCode PFOCreatingAlg::GetChargedHCALCandidates(const PandoraPlus::Track* _track,
+                                      std::vector<PandoraPlus::Calo3DCluster*>& _hcal_clusters,
+                                      std::vector<PandoraPlus::Calo3DCluster*>& _hcal_clus_candidate)
+{
+  std::vector<TrackState> hcal_trk_states = _track->getTrackStates("Hcal");
+  if(hcal_trk_states.size()==0)
+    return StatusCode::SUCCESS;
+
+
+  std::vector<TVector3> extrpolated_points;
+  for(int it=0; it<hcal_trk_states.size(); it++){
+    extrpolated_points.push_back(hcal_trk_states[it].referencePoint);
+  }
+  
+  for(int ih=0; ih<_hcal_clusters.size(); ih++){
+    TVector3 clus_center = _hcal_clusters[ih]->getHitCenter();
+    TVector3 distance = clus_center - extrpolated_points[0];
+    if(distance.Mag()>1000) continue;  // yyy: harcode for 1000. If the cluster is too far away from the extrpolated points in HCAL, it is obviously not a candidate
+
+    bool is_candidate = false;
+    for(int ihit=0; ihit<_hcal_clusters[ih]->getCaloHits().size(); ihit++){
+      if(is_candidate) break;
+      TVector3 hit_pos = _hcal_clusters[ih]->getCaloHits()[ihit]->getPosition();
+      for(int ie=0; ie<extrpolated_points.size(); ie++){
+        TVector3 dist = hit_pos - extrpolated_points[ie];
+        if(dist.Mag()<settings.map_floatPars["extrPoint_HCALHit_distCut"]){
+          is_candidate = true;
+          break;
+        }
+      }
+    }
+
+    if(is_candidate){
+      _hcal_clus_candidate.push_back(_hcal_clusters[ih]);
+    }
+  }
+} 
+
+StatusCode PFOCreatingAlg::GetNearbyHCALCandidates( PandoraPlus::Calo3DCluster* _ecal_cluster,
+                                  std::vector<PandoraPlus::Calo3DCluster*>& _hcal_clusters,
+                                  std::vector<PandoraPlus::Calo3DCluster*>& _hcal_clus_candidate)
+{
+  TVector3 ecal_pos = _ecal_cluster->getShowerCenter();
+  for(int ih=0; ih<_hcal_clusters.size(); ih++){
+    TVector3 hcal_pos = _hcal_clusters[ih]->getHitCenter();
+
+    double angle = ecal_pos.Angle(hcal_pos);
+    if(angle<settings.map_floatPars["nearby_clus_angleCut"]){
+      _hcal_clus_candidate.push_back(_hcal_clusters[ih]);
+    }
+  }
+
+  return StatusCode::SUCCESS;
+}
+
+bool PFOCreatingAlg::isReachOuterMostECAL(PandoraPlus::Calo3DCluster* _ecal_cluster)
+{
+  // A neutral cluster may deposits energy into HCAL if its cluster in ECAL reach the outermost boundary.
+  //   In this case, it hits the last layer of ECAL or hit the boundary of different modules.
+  const CaloHalfCluster* p_HFClusterU = _ecal_cluster->getHalfClusterUCol("LinkedLongiCluster")[0];
+  const CaloHalfCluster* p_HFClusterV = _ecal_cluster->getHalfClusterVCol("LinkedLongiCluster")[0];
+  int endLayer = max(p_HFClusterU->getEndDlayer(), p_HFClusterV->getEndDlayer());
+  if (endLayer>13){  // yyy: hardcode! number of layer in ECAL may be different from 14 for other design
+    cout << "    yyy: The ECAL cluster reach the outermost ECAL. endLayer=" <<  endLayer << endl;
+    return true;
+  }
+
+  std::vector< std::vector<int> > cluster_towers = _ecal_cluster->getTowerID();
+  set<int> towerID;
+  for(int it=0; it<cluster_towers.size(); it++){
+    towerID.insert(cluster_towers[it][0]);
+  }
+  if(towerID.size()>1){
+    return true;
+
+    cout << "    yyy: The ECAL cluster deposits in over one module. towerID = (";
+    for (int i : towerID) {
+      cout << i << ", ";
+    }cout << ")" << endl;
+  }
+
+  cout << "    yyy: The ECAL cluster does not reach the outermost ECAL. EndLayer=" <<  endLayer 
+        << ", towerID = (";
+  for (int i : towerID) {
+    cout << i << ", ";
+  }cout << ")" << endl;
+
+  return false;
+}
+
+template<typename T1, typename T2>
+StatusCode PFOCreatingAlg::CleanUsedElements(std::vector<T1>& _used_elements,
+                                                    std::vector<T2>& _left_elements)
+{
+  for(int i=0; i<_used_elements.size(); i++){
+    auto it = std::find(_left_elements.begin(), _left_elements.end(), _used_elements[i]);
+    if (it != _left_elements.end()) {
+      _left_elements.erase(it);
+    }
+  }
+
+  return StatusCode::SUCCESS;
+}
+template<typename T1, typename T2> StatusCode CleanUsedElement(T1 _used_element,
+                                                    std::vector<T2>& _left_elements)
+{
+  auto it = std::find(_left_elements.begin(), _left_elements.end(), _used_element);
+
+  if (it != _left_elements.end()) {
+      _left_elements.erase(it);
+  }
+
+  return StatusCode::SUCCESS;
+}
+
+StatusCode PFOCreatingAlg::CreateLeftPFO(std::vector<PandoraPlus::Track*>& _tracks,
+                            std::vector<PandoraPlus::Calo3DCluster*>& _hcal_clusters,
+                            std::vector<std::shared_ptr<PandoraPlus::PFObject>>& _pfobjects)
+{
+  for(int it=0; it<_tracks.size(); it++){
+    std::shared_ptr<PandoraPlus::PFObject> tmp_pfo = std::make_shared<PandoraPlus::PFObject>();
+    tmp_pfo->addTrack(_tracks[it]);
+    _pfobjects.push_back(tmp_pfo);
+  }
+
+  for(int ih=0; ih<_hcal_clusters.size(); ih++){
+    std::shared_ptr<PandoraPlus::PFObject> tmp_pfo = std::make_shared<PandoraPlus::PFObject>();
+    tmp_pfo->addHCALCluster(_hcal_clusters[ih]);
+    _pfobjects.push_back(tmp_pfo);
+  }
+  return StatusCode::SUCCESS;
+} 
+
+#endif
diff --git a/Reconstruction/CrystalCaloRec/src/Algorithm/PFOReclusteringAlg.cpp b/Reconstruction/CrystalCaloRec/src/Algorithm/PFOReclusteringAlg.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..32a50d6131f2be0ed0102e41f18902078dd9c56f
--- /dev/null
+++ b/Reconstruction/CrystalCaloRec/src/Algorithm/PFOReclusteringAlg.cpp
@@ -0,0 +1,579 @@
+#ifndef _PFORECLUSTERING_ALG_C
+#define _PFORECLUSTERING_ALG_C
+
+#include "Algorithm/PFOReclusteringAlg.h"
+
+StatusCode PFOReclusteringAlg::ReadSettings(Settings& m_settings){
+  settings = m_settings;
+
+  //Initialize parameters
+  if(settings.map_stringPars.find("ReadinPFOName")==settings.map_stringPars.end()) settings.map_stringPars["ReadinPFOName"] = "outputPFO";
+  if(settings.map_floatPars.find("ECALCalib")==settings.map_floatPars.end()) settings.map_floatPars["ECALCalib"] = 1.02;
+  if(settings.map_floatPars.find("HCALCalib")==settings.map_floatPars.end()) settings.map_floatPars["HCALCalib"] = 65.;
+  
+  if(settings.map_floatPars.find("EnergyRes")==settings.map_floatPars.end()) settings.map_floatPars["EnergyRes"] = 0.4;
+  if(settings.map_floatPars.find("SplitSigma")==settings.map_floatPars.end()) settings.map_floatPars["SplitSigma"] = 0.;
+  if(settings.map_floatPars.find("NeutralMergeSigma")==settings.map_floatPars.end()) settings.map_floatPars["NeutralMergeSigma"] = 0.;
+  if(settings.map_floatPars.find("VirtualMergeSigma")==settings.map_floatPars.end()) settings.map_floatPars["VirtualMergeSigma"] = 0.6;
+  if(settings.map_floatPars.find("MinAngleForNeuMerge")==settings.map_floatPars.end()) settings.map_floatPars["MinAngleForNeuMerge"] = 0.18;
+  if(settings.map_floatPars.find("MinAngleForVirMerge")==settings.map_floatPars.end()) settings.map_floatPars["MinAngleForVirMerge"] = 0.12;
+
+
+  return StatusCode::SUCCESS;
+};
+
+StatusCode PFOReclusteringAlg::Initialize( PandoraPlusDataCol& m_datacol ){
+  p_PFObjects = nullptr;
+
+  p_PFObjects = &(m_datacol.map_PFObjects[settings.map_stringPars["ReadinPFOName"]]);
+  return StatusCode::SUCCESS;
+};
+
+StatusCode PFOReclusteringAlg::RunAlgorithm( PandoraPlusDataCol& m_datacol ){
+
+  std::vector< std::shared_ptr<PandoraPlus::PFObject> > m_chargedPFOs; 
+  std::vector< std::shared_ptr<PandoraPlus::PFObject> > m_neutralPFOs; 
+
+  for(int ipfo=0; ipfo<p_PFObjects->size(); ipfo++){
+    if(p_PFObjects->at(ipfo)->getTracks().size()==0) m_neutralPFOs.push_back( p_PFObjects->at(ipfo) );
+    else m_chargedPFOs.push_back( p_PFObjects->at(ipfo) );
+  }
+
+  std::sort(m_chargedPFOs.begin(), m_chargedPFOs.end(), compTrkP);
+ double totE_Ecal = 0;
+ double totE_Hcal = 0;
+ cout<<"Readin PFO: "<<p_PFObjects->size()<<", charged "<<m_chargedPFOs.size()<<", neutral "<<m_neutralPFOs.size()<<endl;
+ for(int i=0; i<m_neutralPFOs.size(); i++){
+   cout<<"    PFO #"<<i<<": track size "<<m_neutralPFOs[i]->getTracks().size()<<", leading P "<<m_neutralPFOs[i]->getTrackMomentum();
+   cout<<", ECAL cluster size "<<m_neutralPFOs[i]->getECALClusters().size()<<", totE "<<m_neutralPFOs[i]->getECALClusterEnergy();
+   cout<<", HCAL cluster size "<<m_neutralPFOs[i]->getHCALClusters().size()<<", totE "<<m_neutralPFOs[i]->getHCALClusterEnergy()<<endl;
+   totE_Ecal += m_neutralPFOs[i]->getECALClusterEnergy();
+   totE_Hcal += m_neutralPFOs[i]->getHCALClusterEnergy();
+ }
+ cout<<"-----Neutral cluster Ecal total energy: "<<totE_Ecal<<", Hcal total energy: "<<totE_Hcal<<endl;
+ totE_Ecal = 0;
+ totE_Hcal = 0;
+ for(int i=0; i<m_chargedPFOs.size(); i++){
+   cout<<"    PFO #"<<i<<": track size "<<m_chargedPFOs[i]->getTracks().size()<<", leading P "<<m_chargedPFOs[i]->getTrackMomentum();
+   cout<<", ECAL cluster size "<<m_chargedPFOs[i]->getECALClusters().size()<<", totE "<<m_chargedPFOs[i]->getECALClusterEnergy();
+   cout<<", HCAL cluster size "<<m_chargedPFOs[i]->getHCALClusters().size()<<", totE "<<m_chargedPFOs[i]->getHCALClusterEnergy()<<endl;
+   totE_Ecal += m_chargedPFOs[i]->getECALClusterEnergy();
+   totE_Hcal += m_chargedPFOs[i]->getHCALClusterEnergy();
+ }
+ cout<<"-----Charged cluster Ecal total energy: "<<totE_Ecal<<", Hcal total energy: "<<totE_Hcal<<endl;
+
+
+  //If P_trk < E_cluster, create a virtual neutral PFO. 
+  ReCluster_SplitFromChg(m_chargedPFOs, m_neutralPFOs);
+
+ totE_Ecal = 0;
+ totE_Hcal = 0;
+ cout<<"After split from Ch: charged "<<m_chargedPFOs.size()<<", neutral "<<m_neutralPFOs.size()<<", total "<<p_PFObjects->size()<<endl;
+ for(int i=0; i<m_neutralPFOs.size(); i++){
+   cout<<"    PFO #"<<i<<": track size "<<m_neutralPFOs[i]->getTracks().size()<<", leading P "<<m_neutralPFOs[i]->getTrackMomentum();
+   cout<<", ECAL cluster size "<<m_neutralPFOs[i]->getECALClusters().size()<<", totE "<<m_neutralPFOs[i]->getECALClusterEnergy();
+   cout<<", HCAL cluster size "<<m_neutralPFOs[i]->getHCALClusters().size()<<", totE "<<m_neutralPFOs[i]->getHCALClusterEnergy()<<endl;
+   totE_Ecal += m_neutralPFOs[i]->getECALClusterEnergy();
+   totE_Hcal += m_neutralPFOs[i]->getHCALClusterEnergy();
+ }
+ cout<<"-----Neutral cluster Ecal total energy: "<<totE_Ecal<<", Hcal total energy: "<<totE_Hcal<<endl;
+ totE_Ecal = 0;
+ totE_Hcal = 0;
+ for(int i=0; i<m_chargedPFOs.size(); i++){
+   cout<<"    PFO #"<<i<<": track size "<<m_chargedPFOs[i]->getTracks().size()<<", leading P "<<m_chargedPFOs[i]->getTrackMomentum();
+   cout<<", ECAL cluster size "<<m_chargedPFOs[i]->getECALClusters().size()<<", totE "<<m_chargedPFOs[i]->getECALClusterEnergy();
+   cout<<", HCAL cluster size "<<m_chargedPFOs[i]->getHCALClusters().size()<<", totE "<<m_chargedPFOs[i]->getHCALClusterEnergy()<<endl;
+   totE_Ecal += m_chargedPFOs[i]->getECALClusterEnergy();
+   totE_Hcal += m_chargedPFOs[i]->getHCALClusterEnergy();
+ }
+ cout<<"-----Charged cluster Ecal total energy: "<<totE_Ecal<<", Hcal total energy: "<<totE_Hcal<<endl;
+
+
+  //If P_trk > E_cluster, merge nearby neutral PFO into the charged. 
+  ReCluster_MergeToChg(m_chargedPFOs, m_neutralPFOs);
+
+  m_datacol.map_CaloHit["bkHit"].insert( m_datacol.map_CaloHit["bkHit"].end(), m_bkCol.map_CaloHit["bkHit"].begin(), m_bkCol.map_CaloHit["bkHit"].end() );
+  m_datacol.map_CaloCluster["bk3DCluster"].insert( m_datacol.map_CaloCluster["bk3DCluster"].end(), m_bkCol.map_CaloCluster["bk3DCluster"].begin(), m_bkCol.map_CaloCluster["bk3DCluster"].end() );
+  m_datacol.map_PFObjects["bkPFO"].insert( m_datacol.map_PFObjects["bkPFO"].end(), m_bkCol.map_PFObjects["bkPFO"].begin(), m_bkCol.map_PFObjects["bkPFO"].end() );
+
+ totE_Ecal = 0;
+ totE_Hcal = 0;
+ cout<<"After merge all virtual to Ch: charged "<<m_chargedPFOs.size()<<", neutral "<<m_neutralPFOs.size()<<", total "<<p_PFObjects->size()<<endl;
+ for(int i=0; i<m_neutralPFOs.size(); i++){
+   cout<<"    PFO #"<<i<<": track size "<<m_neutralPFOs[i]->getTracks().size()<<", leading P "<<m_neutralPFOs[i]->getTrackMomentum();
+   cout<<", ECAL cluster size "<<m_neutralPFOs[i]->getECALClusters().size()<<", totE "<<m_neutralPFOs[i]->getECALClusterEnergy();
+   cout<<", HCAL cluster size "<<m_neutralPFOs[i]->getHCALClusters().size()<<", totE "<<m_neutralPFOs[i]->getHCALClusterEnergy()<<endl;
+   totE_Ecal += m_neutralPFOs[i]->getECALClusterEnergy();
+   totE_Hcal += m_neutralPFOs[i]->getHCALClusterEnergy();
+ }
+ cout<<"-----Neutral cluster Ecal total energy: "<<totE_Ecal<<", Hcal total energy: "<<totE_Hcal<<endl;
+ totE_Ecal = 0;
+ totE_Hcal = 0;
+ for(int i=0; i<m_chargedPFOs.size(); i++){
+   cout<<"    PFO #"<<i<<": track size "<<m_chargedPFOs[i]->getTracks().size()<<", leading P "<<m_chargedPFOs[i]->getTrackMomentum();
+   cout<<", ECAL cluster size "<<m_chargedPFOs[i]->getECALClusters().size()<<", totE "<<m_chargedPFOs[i]->getECALClusterEnergy();
+   cout<<", HCAL cluster size "<<m_chargedPFOs[i]->getHCALClusters().size()<<", totE "<<m_chargedPFOs[i]->getHCALClusterEnergy()<<endl;
+   totE_Ecal += m_chargedPFOs[i]->getECALClusterEnergy();
+   totE_Hcal += m_chargedPFOs[i]->getHCALClusterEnergy();
+ }
+ cout<<"-----Charged cluster Ecal total energy: "<<totE_Ecal<<", Hcal total energy: "<<totE_Hcal<<endl;
+
+
+  m_chargedPFOs.clear();
+  m_neutralPFOs.clear();
+  return StatusCode::SUCCESS;
+};
+
+StatusCode PFOReclusteringAlg::ClearAlgorithm(){
+  p_PFObjects = nullptr;
+  m_bkCol.Clear();
+
+  return StatusCode::SUCCESS;
+};
+
+
+StatusCode PFOReclusteringAlg::ReCluster_MergeToChg(std::vector< std::shared_ptr<PandoraPlus::PFObject> >& m_chargedPFOs, 
+                                                    std::vector< std::shared_ptr<PandoraPlus::PFObject> >& m_neutralPFOs ){
+
+  //Merge real neutral PFOs
+  for(int ic=0; ic<m_chargedPFOs.size(); ic++){
+    if(m_chargedPFOs[ic]->getECALClusters().size()==0 && m_chargedPFOs[ic]->getHCALClusters().size()==0) continue;
+
+    double track_energy = m_chargedPFOs[ic]->getTrackMomentum();
+    double ECAL_energy = settings.map_floatPars["ECALCalib"]*m_chargedPFOs[ic]->getECALClusterEnergy();
+    double HCAL_energy = settings.map_floatPars["HCALCalib"]*m_chargedPFOs[ic]->getHCALClusterEnergy();
+    if(track_energy<0 || ECAL_energy<0 || HCAL_energy<0){
+      std::cout<<"ERROR: Charged PFO info break. Ptrk "<<track_energy<<", E_ecal "<<ECAL_energy<<", E_hcal "<<HCAL_energy<<endl;
+      continue;
+    }
+
+    double delta_energy = ECAL_energy + HCAL_energy - track_energy;
+    double sigmaE = settings.map_floatPars["EnergyRes"] * sqrt(ECAL_energy + HCAL_energy);
+
+//cout<<"  ReCluster_MergeToChg: In ChPFO #"<<ic;
+//cout<<": ECAL cluster size "<<m_chargedPFOs[ic]->getECALClusters().size()<<", HCAL cluster size "<<m_chargedPFOs[ic]->getHCALClusters().size();
+//cout<<", Ptrk = "<<track_energy<<", Eecal = "<<ECAL_energy<<", Ehcal = "<<HCAL_energy<<", deltaE = "<<delta_energy<<", sigmaE = "<<sigmaE<<endl;
+
+    if(delta_energy >= settings.map_floatPars["NeutralMergeSigma"]*sigmaE) continue;
+//cout<<"    Do charged PFO merge. "<<endl;
+
+    // All HCAL clusters in the neutral PFO
+    std::vector<const PandoraPlus::Calo3DCluster*> all_neutral_HCAL_clus;
+    for(int ip=0; ip<m_neutralPFOs.size(); ip++){
+      std::vector<const PandoraPlus::Calo3DCluster*> tmp_HCAL_clus = m_neutralPFOs[ip]->getHCALClusters();
+      all_neutral_HCAL_clus.insert(all_neutral_HCAL_clus.end(), tmp_HCAL_clus.begin(), tmp_HCAL_clus.end());
+    }
+//cout<<"      Neutral HCAL cluster size "<<all_neutral_HCAL_clus.size()<<endl;
+
+    TVector3 trackclus_pos(0, 0, 0);
+    if(m_chargedPFOs[ic]->getECALClusters().size()>0){
+      trackclus_pos = m_chargedPFOs[ic]->getECALClusters()[0]->getShowerCenter();
+    }
+    else{
+      for(int jc=0; jc<m_chargedPFOs[ic]->getHCALClusters().size(); jc++)
+        trackclus_pos += m_chargedPFOs[ic]->getHCALClusters()[jc]->getHitCenter() * settings.map_floatPars["HCALCalib"]*m_chargedPFOs[ic]->getHCALClusters()[jc]->getHitsE();
+
+      trackclus_pos = trackclus_pos*(1./HCAL_energy);
+    }
+//printf("    charged cluster trk pos: [%.3f, %.3f, %.3f] \n", trackclus_pos.x(), trackclus_pos.y(), trackclus_pos.z());  
+
+    int loop_count = 0;
+    std::vector<const PandoraPlus::Calo3DCluster*> skip_clus;
+    while(delta_energy < 0){
+      loop_count++;
+      if(loop_count>all_neutral_HCAL_clus.size()+10){
+        break;
+      }
+      double min_angle = 999.0;
+      int clus_index = -1;
+
+      for(int in=0; in<all_neutral_HCAL_clus.size(); in++){
+        if(find(skip_clus.begin(), skip_clus.end(), all_neutral_HCAL_clus[in]) != skip_clus.end()) continue;
+
+        TVector3 neutral_clus_pos = all_neutral_HCAL_clus[in]->getHitCenter();
+        double pfo_angle = trackclus_pos.Angle(neutral_clus_pos);
+        if (pfo_angle<settings.map_floatPars["MinAngleForNeuMerge"] && pfo_angle<min_angle){
+          min_angle=pfo_angle;
+          clus_index = in;
+        }
+      }
+
+//cout<<"    In Loop "<<loop_count<<": current deltaE = "<<delta_energy<<", closest virtual cluster index "<<clus_index<<endl;
+      if(clus_index<0) break;  // No neutral Hcal cluster to be merged
+
+      double tmp_delta_E = delta_energy + settings.map_floatPars["HCALCalib"]*all_neutral_HCAL_clus[clus_index]->getHitsE();
+      if (TMath::Abs(tmp_delta_E) >= TMath::Abs(delta_energy)){
+        skip_clus.push_back(all_neutral_HCAL_clus[clus_index]);
+        continue;  // No need to merge this HCAL cluster
+      }
+
+      // Update delta_energy
+      delta_energy = tmp_delta_E;
+      // Add this HCAL cluster to charged PFO
+      m_chargedPFOs[ic]->addHCALCluster(all_neutral_HCAL_clus[clus_index]);
+
+      // Remove this HCAL cluster from neutral PFO
+      bool is_found = false;
+      for(int in=0; in<m_neutralPFOs.size(); in++){
+        std::vector<const PandoraPlus::Calo3DCluster*> neutral_cluster = m_neutralPFOs[in]->getHCALClusters();
+        int tmp_index=-1;
+        for(int ii=0; ii<neutral_cluster.size(); ii++){
+          if (all_neutral_HCAL_clus[clus_index]==neutral_cluster[ii]){
+            tmp_index = ii;
+            break;
+          }
+        }
+        if (tmp_index==-1) continue;
+//cout<<"  Remove a neutral cluster: En "<<settings.map_floatPars["HCALCalib"]*neutral_cluster[tmp_index]->getHitsE()<<endl;
+
+        neutral_cluster.erase(neutral_cluster.begin()+tmp_index);
+        m_neutralPFOs[in]->setHCALCluster(neutral_cluster);
+
+        if(m_neutralPFOs[in]->getTracks().size() + m_neutralPFOs[in]->getECALClusters().size() + m_neutralPFOs[in]->getHCALClusters().size()==0){
+//cout<<"  Remove a neutral PFO: ECAL En "<<m_neutralPFOs[in]->getECALClusterEnergy()<<", HCAL En "<<m_neutralPFOs[in]->getHCALClusterEnergy()<<endl;
+          auto iter = find(p_PFObjects->begin(), p_PFObjects->end(), m_neutralPFOs[in]);
+          if(iter==p_PFObjects->end()){
+            std::cout<<"ERROR: can not find this neutral PFO in p_PFObjects. "<<std::endl;
+          }
+          else{
+            m_neutralPFOs.erase(m_neutralPFOs.begin()+in);
+            p_PFObjects->erase(iter);
+          }
+        }
+        is_found = true;
+        break;
+      }
+      if(!is_found){
+        cout << "Error! Can not find the HCAL cluster in neutral PFO" << endl;
+      }
+
+      // Remove this HCAL cluster from all_neutral_HCAL_clus
+      all_neutral_HCAL_clus.erase(all_neutral_HCAL_clus.begin()+clus_index);
+    }
+  }
+
+
+ double totE_Ecal = 0;
+ double totE_Hcal = 0;
+ cout<<"After merge real neu to Ch: charged "<<m_chargedPFOs.size()<<", neutral "<<m_neutralPFOs.size()<<", total "<<p_PFObjects->size()<<endl;
+ for(int i=0; i<m_neutralPFOs.size(); i++){
+   cout<<"    PFO #"<<i<<": track size "<<m_neutralPFOs[i]->getTracks().size()<<", leading P "<<m_neutralPFOs[i]->getTrackMomentum();
+   cout<<", ECAL cluster size "<<m_neutralPFOs[i]->getECALClusters().size()<<", totE "<<m_neutralPFOs[i]->getECALClusterEnergy();
+   cout<<", HCAL cluster size "<<m_neutralPFOs[i]->getHCALClusters().size()<<", totE "<<m_neutralPFOs[i]->getHCALClusterEnergy()<<endl;
+   totE_Ecal += m_neutralPFOs[i]->getECALClusterEnergy();
+   totE_Hcal += m_neutralPFOs[i]->getHCALClusterEnergy();
+ }
+ cout<<"-----Neutral cluster Ecal total energy: "<<totE_Ecal<<", Hcal total energy: "<<totE_Hcal<<endl;
+ totE_Ecal = 0;
+ totE_Hcal = 0;
+ for(int i=0; i<m_chargedPFOs.size(); i++){
+   cout<<"    PFO #"<<i<<": track size "<<m_chargedPFOs[i]->getTracks().size()<<", leading P "<<m_chargedPFOs[i]->getTrackMomentum();
+   cout<<", ECAL cluster size "<<m_chargedPFOs[i]->getECALClusters().size()<<", totE "<<m_chargedPFOs[i]->getECALClusterEnergy();
+   cout<<", HCAL cluster size "<<m_chargedPFOs[i]->getHCALClusters().size()<<", totE "<<m_chargedPFOs[i]->getHCALClusterEnergy()<<endl;
+   totE_Ecal += m_chargedPFOs[i]->getECALClusterEnergy();
+   totE_Hcal += m_chargedPFOs[i]->getHCALClusterEnergy();
+ }
+ cout<<"-----Charged cluster Ecal total energy: "<<totE_Ecal<<", Hcal total energy: "<<totE_Hcal<<endl;
+
+
+
+  //Merge virtual neutral PFOs created from splitting. 
+  for(int ic=0; ic<m_chargedPFOs.size(); ic++){
+    if(m_chargedPFOs[ic]->getTracks().size()==0) continue;
+    if( m_chargedPFOs[ic]->getECALClusters().size()==0 && 
+        m_chargedPFOs[ic]->getHCALClusters().size()==0 && 
+        m_chargedPFOs[ic]->getTracks()[0]->getTrackStates("Hcal").size()==0) continue;
+
+    double track_energy = m_chargedPFOs[ic]->getTrackMomentum();
+    double ECAL_energy = settings.map_floatPars["ECALCalib"]*m_chargedPFOs[ic]->getECALClusterEnergy();
+    double HCAL_energy = settings.map_floatPars["HCALCalib"]*m_chargedPFOs[ic]->getHCALClusterEnergy();
+    if(track_energy<0 || ECAL_energy<0 || HCAL_energy<0){
+      std::cout<<"ERROR: Charged PFO info break. Ptrk "<<track_energy<<", E_ecal "<<ECAL_energy<<", E_hcal "<<HCAL_energy<<endl;
+      continue;
+    }
+
+    double sigmaE = settings.map_floatPars["EnergyRes"] * sqrt(track_energy);
+    double delta_energy = ECAL_energy + HCAL_energy - track_energy;
+
+cout<<"  ReCluster_MergeToChg: In ChPFO #"<<ic<<": Ptrk = "<<track_energy<<", Eecal = "<<ECAL_energy<<", Ehcal = "<<HCAL_energy<<", deltaE = "<<delta_energy<<", sigmaE = "<<sigmaE<<endl;
+
+    if(delta_energy > settings.map_floatPars["VirtualMergeSigma"]*sigmaE) continue;
+cout<<"    Do charged PFO merge. "<<endl;
+
+    // Virtual HCAL clusters in the neutral PFO
+    std::vector<const PandoraPlus::Calo3DCluster*> all_neutral_HCAL_clus; all_neutral_HCAL_clus.clear();
+    for(int ip=0; ip<m_neutralPFOs.size(); ip++){
+      std::vector<const PandoraPlus::Calo3DCluster*> tmp_HCAL_clus = m_neutralPFOs[ip]->getHCALClusters();
+      if(tmp_HCAL_clus.size()!=1) continue;
+      if(tmp_HCAL_clus[0]->getType()!=-1) continue;
+      all_neutral_HCAL_clus.push_back(tmp_HCAL_clus[0]);
+    }
+cout<<"    Virtual HCAL cluster size "<<all_neutral_HCAL_clus.size()<<", print them: "<<endl;
+for(int aa=0; aa<all_neutral_HCAL_clus.size(); aa++){
+  printf("      #%d: pos (%.3f, %.3f, %.3f), En %.3f, Nhit %d, type %d \n", aa, all_neutral_HCAL_clus[aa]->getHitCenter().x(), all_neutral_HCAL_clus[aa]->getHitCenter().y(), all_neutral_HCAL_clus[aa]->getHitCenter().z(), all_neutral_HCAL_clus[aa]->getHitsE()*settings.map_floatPars["HCALCalib"], all_neutral_HCAL_clus[aa]->getCaloHits().size(), all_neutral_HCAL_clus[aa]->getType() );
+}
+
+    TVector3 trackclus_pos(0, 0, 0);
+    if(m_chargedPFOs[ic]->getTracks()[0]->getTrackStates("Hcal").size()>0){
+      std::vector<TrackState> m_extTrkStats = m_chargedPFOs[ic]->getTracks()[0]->getTrackStates("Hcal");
+
+      int min_hit_index = -1;
+      double min_distance = 999999;
+      for(int i=0; i<m_extTrkStats.size(); i++){
+        double hit_distance = m_extTrkStats[i].referencePoint.Perp();
+        if(hit_distance<min_distance){
+          min_distance = hit_distance;
+          min_hit_index = i;
+        }
+      }
+      
+      if(min_hit_index>=0) trackclus_pos = m_extTrkStats[min_hit_index].referencePoint;
+    }
+    else if(m_chargedPFOs[ic]->getECALClusters().size()>0){
+      trackclus_pos = m_chargedPFOs[ic]->getECALClusters()[0]->getShowerCenter();
+    }
+    else{
+      for(int jc=0; jc<m_chargedPFOs[ic]->getHCALClusters().size(); jc++)
+        trackclus_pos += m_chargedPFOs[ic]->getHCALClusters()[jc]->getHitCenter() * settings.map_floatPars["HCALCalib"]*m_chargedPFOs[ic]->getHCALClusters()[jc]->getHitsE();
+
+      trackclus_pos = trackclus_pos*(1./HCAL_energy);
+    }
+ printf("    charged cluster trk pos: [%.3f, %.3f, %.3f] \n", trackclus_pos.x(), trackclus_pos.y(), trackclus_pos.z());
+
+    int loop_count = 0;
+    std::vector<const PandoraPlus::Calo3DCluster*> skip_clus;
+    while(delta_energy < sigmaE*settings.map_floatPars["VirtualMergeSigma"]-(1e-6)){
+      loop_count++;
+      if(loop_count>all_neutral_HCAL_clus.size()+10){
+        break;
+      }
+      double min_angle = 999.0;
+      int clus_index = -1;
+
+      for(int in=0; in<all_neutral_HCAL_clus.size(); in++){
+        if(find(skip_clus.begin(), skip_clus.end(), all_neutral_HCAL_clus[in]) != skip_clus.end()) continue;
+
+        TVector3 neutral_clus_pos = all_neutral_HCAL_clus[in]->getHitCenter();
+        double pfo_angle = trackclus_pos.Angle(neutral_clus_pos);
+        if (pfo_angle<settings.map_floatPars["MinAngleForVirMerge"] && pfo_angle<min_angle){
+          min_angle=pfo_angle;
+          clus_index = in;
+        }
+      }
+cout<<"    In Loop "<<loop_count<<": current deltaE = "<<delta_energy<<", closest virtual cluster index "<<clus_index<<endl;
+
+      if(clus_index<0) break;  // No neutral Hcal cluster to be merged
+
+      double tmp_delta_E = delta_energy + settings.map_floatPars["HCALCalib"]*all_neutral_HCAL_clus[clus_index]->getHitsE();
+cout<<"    If include this cluster: new deltaE "<<tmp_delta_E<<", merge = "<<(tmp_delta_E > sigmaE * settings.map_floatPars["VirtualMergeSigma"])<<endl;
+      if(tmp_delta_E > sigmaE * settings.map_floatPars["VirtualMergeSigma"]){
+        double absorbed_energy = sigmaE*settings.map_floatPars["VirtualMergeSigma"] - delta_energy;
+        delta_energy = delta_energy + absorbed_energy;
+
+
+        //Create a new virtual neutral cluster with energy = absorbed_energy. 
+
+        std::shared_ptr<PandoraPlus::CaloHit> m_hit = all_neutral_HCAL_clus[clus_index]->getCaloHits()[0]->Clone();
+        m_hit->setEnergy(absorbed_energy/settings.map_floatPars["HCALCalib"]);
+
+        std::shared_ptr<PandoraPlus::Calo3DCluster> m_clus = std::make_shared<PandoraPlus::Calo3DCluster>();
+        m_clus->addHit(m_hit.get());
+        m_clus->setType(-1);
+
+        m_bkCol.map_CaloHit["bkHit"].push_back( m_hit );
+        m_bkCol.map_CaloCluster["bk3DCluster"].push_back(m_clus);
+
+        m_chargedPFOs[ic]->addHCALCluster( m_clus.get() );
+
+
+        //Re-set neutral virtual cluster energy
+        bool is_found = false;
+        for(int ip=0; ip<m_neutralPFOs.size(); ip++){
+          auto tmp_HCAL_clus = m_neutralPFOs[ip]->getHCALClusters();
+
+          if(tmp_HCAL_clus.size()!=1) continue;
+          if(tmp_HCAL_clus[0]->getType()!=-1) continue;
+
+          if(tmp_HCAL_clus[0]==all_neutral_HCAL_clus[clus_index]){
+            std::shared_ptr<PandoraPlus::CaloHit> m_newhit = all_neutral_HCAL_clus[clus_index]->getCaloHits()[0]->Clone();
+            m_newhit->setEnergy(all_neutral_HCAL_clus[clus_index]->getHitsE() - absorbed_energy/settings.map_floatPars["HCALCalib"] );
+
+            std::shared_ptr<PandoraPlus::Calo3DCluster> m_newclus = std::make_shared<PandoraPlus::Calo3DCluster>();
+            m_newclus->addHit(m_newhit.get());
+            m_newclus->setType(-1);            
+
+            m_bkCol.map_CaloHit["bkHit"].push_back( m_newhit );
+            m_bkCol.map_CaloCluster["bk3DCluster"].push_back(m_newclus);
+
+            std::vector<const Calo3DCluster*> tmp_clusters; tmp_clusters.clear();
+            tmp_clusters.push_back(m_clus.get());
+            m_neutralPFOs[ip]->setHCALCluster( tmp_clusters );
+
+            is_found = true;
+            break;
+          }
+
+        }
+        if(!is_found){
+          cout << "Error! Can not find the HCAL cluster in neutral PFO to delete part of the energy" << endl;
+        }
+        //Reset the energy
+        all_neutral_HCAL_clus.erase(all_neutral_HCAL_clus.begin()+clus_index);
+        all_neutral_HCAL_clus.push_back(m_clus.get());
+        skip_clus.push_back(m_clus.get());
+      }
+      else{
+
+        if (TMath::Abs(tmp_delta_E) >= TMath::Abs(delta_energy)){
+          skip_clus.push_back(all_neutral_HCAL_clus[clus_index]);
+          continue;  // No need to merge this HCAL cluster
+        }
+   
+        // Update delta_energy
+        delta_energy = tmp_delta_E;
+        // Add this HCAL cluster to charged PFO
+        m_chargedPFOs[ic]->addHCALCluster(all_neutral_HCAL_clus[clus_index]);
+   
+        // Remove this HCAL cluster from neutral PFO
+        bool is_found = false;
+        for(int in=0; in<m_neutralPFOs.size(); in++){
+          std::vector<const PandoraPlus::Calo3DCluster*> neutral_cluster = m_neutralPFOs[in]->getHCALClusters();
+          int tmp_index=-1;
+          for(int ii=0; ii<neutral_cluster.size(); ii++){
+            if (all_neutral_HCAL_clus[clus_index]==neutral_cluster[ii]){
+              tmp_index = ii;
+              break;
+            }
+          }
+          if (tmp_index==-1) continue;
+cout<<"  Remove a neutral cluster: En "<<settings.map_floatPars["HCALCalib"]*neutral_cluster[tmp_index]->getHitsE()<<endl;
+
+          neutral_cluster.erase(neutral_cluster.begin()+tmp_index);
+          m_neutralPFOs[in]->setHCALCluster(neutral_cluster);
+   
+          if(m_neutralPFOs[in]->getTracks().size() + m_neutralPFOs[in]->getECALClusters().size() + m_neutralPFOs[in]->getHCALClusters().size()==0){
+cout<<"  Remove a neutral PFO: ECAL En "<<m_neutralPFOs[in]->getECALClusterEnergy()<<", HCAL En "<<m_neutralPFOs[in]->getHCALClusterEnergy()<<endl;
+            auto iter = find(p_PFObjects->begin(), p_PFObjects->end(), m_neutralPFOs[in]);
+            if(iter==p_PFObjects->end()){
+              std::cout<<"ERROR: can not find this neutral PFO in p_PFObjects. "<<std::endl;
+            }
+            else{
+              m_neutralPFOs.erase(m_neutralPFOs.begin()+in);
+              p_PFObjects->erase(iter);
+            }
+          }
+          is_found = true;
+          break;
+        }
+        if(!is_found){
+          cout << "Error! Can not find the HCAL cluster in neutral PFO" << endl;
+        }
+   
+        // Remove this HCAL cluster from all_neutral_HCAL_clus
+        all_neutral_HCAL_clus.erase(all_neutral_HCAL_clus.begin()+clus_index);
+
+      }
+
+    }
+  }
+
+  return StatusCode::SUCCESS;
+};
+
+
+StatusCode PFOReclusteringAlg::ReCluster_SplitFromChg( std::vector< std::shared_ptr<PandoraPlus::PFObject> >& m_chargedPFOs,
+                                                       std::vector< std::shared_ptr<PandoraPlus::PFObject> >& m_neutralPFOs ){
+
+  for(int ipfo=0; ipfo<m_chargedPFOs.size(); ipfo++){
+    if(m_chargedPFOs[ipfo]->getECALClusters().size()==0 && m_chargedPFOs[ipfo]->getHCALClusters().size()==0) continue;
+
+    double track_energy = m_chargedPFOs[ipfo]->getTrackMomentum();
+    double ECAL_energy = settings.map_floatPars["ECALCalib"]*m_chargedPFOs[ipfo]->getECALClusterEnergy();
+    double HCAL_energy = settings.map_floatPars["HCALCalib"]*m_chargedPFOs[ipfo]->getHCALClusterEnergy();
+    if(track_energy<0 || ECAL_energy<0 || HCAL_energy<0){
+      std::cout<<"ERROR: Charged PFO info break. Ptrk "<<track_energy<<", E_ecal "<<ECAL_energy<<", E_hcal "<<HCAL_energy<<endl;
+      continue;
+    }
+
+    double delta_energy = ECAL_energy + HCAL_energy - track_energy;
+    double sigmaE = settings.map_floatPars["EnergyRes"] * sqrt(ECAL_energy + HCAL_energy);
+    
+//cout<<"  ReCluster_MergeToChg: In ChPFO #"<<ipfo<<": Ptrk = "<<track_energy<<", Eecal = "<<ECAL_energy<<", Ehcal = "<<HCAL_energy<<", deltaE = "<<delta_energy<<", sigmaE = "<<sigmaE<<endl;
+
+    if(delta_energy <= settings.map_floatPars["SplitSigma"]*sigmaE) continue;
+//cout<<"    Do charged PFO splitting. "<<endl;
+
+    //Create a new hit and cluster
+    TVector3 tmp_pos(0,0,0); 
+    for(int ic=0; ic<m_chargedPFOs[ipfo]->getECALClusters().size(); ic++)
+      tmp_pos += settings.map_floatPars["ECALCalib"] * m_chargedPFOs[ipfo]->getECALClusters()[ic]->getLongiE() * m_chargedPFOs[ipfo]->getECALClusters()[ic]->getShowerCenter();
+    for(int ic=0; ic<m_chargedPFOs[ipfo]->getHCALClusters().size(); ic++) 
+      tmp_pos += settings.map_floatPars["HCALCalib"] * m_chargedPFOs[ipfo]->getHCALClusters()[ic]->getHitsE() * m_chargedPFOs[ipfo]->getHCALClusters()[ic]->getHitCenter();
+    tmp_pos = tmp_pos*(1./(ECAL_energy+HCAL_energy));
+
+    std::shared_ptr<PandoraPlus::CaloHit> m_hit = std::make_shared<PandoraPlus::CaloHit>();
+    m_hit->setPosition( tmp_pos );
+    m_hit->setEnergy( delta_energy/settings.map_floatPars["HCALCalib"] );
+
+    std::shared_ptr<PandoraPlus::Calo3DCluster> m_clus = std::make_shared<PandoraPlus::Calo3DCluster>();
+    m_clus->addHit(m_hit.get());
+    m_clus->setType(-1);
+
+    std::shared_ptr<PandoraPlus::PFObject> m_pfo = std::make_shared<PandoraPlus::PFObject>();
+    m_pfo->addHCALCluster( m_clus.get() );
+//cout<<"Create a new Neutral PFO: energy "<<m_pfo->getHCALClusterEnergy()<<endl;
+
+    m_neutralPFOs.push_back( m_pfo );   
+    p_PFObjects->push_back( m_pfo );
+
+    //For this charged PFO: reset HCAL energy. (negative HCAL energy is NOT allowed) 
+    if(HCAL_energy>0 && HCAL_energy-delta_energy>0){
+      double m_HcalEnScale = (HCAL_energy-delta_energy)/HCAL_energy;
+//cout<<"[FY debug] In PFO "<<ipfo<<": track P "<<m_chargedPFOs[ipfo]->getTrackMomentum()<<", HCAL cluster size "<<m_chargedPFOs[ipfo]->getHCALClusters().size()<<", first HCAL has Nhit "<<m_chargedPFOs[ipfo]->getHCALClusters()[0]->getCaloHits().size()<<endl;
+//cout<<"[FY debug] Hcal energy scale = "<<m_HcalEnScale<<endl;
+
+      //Create new HCAL cluster
+      std::shared_ptr<PandoraPlus::Calo3DCluster> m_newclus = std::make_shared<PandoraPlus::Calo3DCluster>();
+      for(int ic=0; ic<m_chargedPFOs[ipfo]->getHCALClusters().size(); ic++){
+        std::vector<const PandoraPlus::CaloHit*> tmp_hits = m_chargedPFOs[ipfo]->getHCALClusters()[ic]->getCaloHits();
+        for(int ih=0; ih<tmp_hits.size(); ih++){
+          std::shared_ptr<CaloHit> tmp_newhit = tmp_hits[ih]->Clone();
+          tmp_newhit->setEnergy( tmp_newhit->getEnergy()*m_HcalEnScale );
+          m_newclus->addHit(tmp_newhit.get());
+          m_bkCol.map_CaloHit["bkHit"].push_back( tmp_newhit );
+        }
+      }
+      m_bkCol.map_CaloCluster["bk3DCluster"].push_back(m_newclus);
+
+//cout<<"[FY debug] new HCAL cluster: hit size "<<m_newclus->getCaloHits().size()<<", total energy "<<m_newclus->getHitsE()<<endl;
+
+      //Create a new PFO
+      std::shared_ptr<PandoraPlus::PFObject> m_newpfo = m_chargedPFOs[ipfo]->Clone();
+      std::vector<const PandoraPlus::Calo3DCluster*> tmp_clusvec; tmp_clusvec.clear(); 
+      tmp_clusvec.push_back(m_newclus.get());
+      m_newpfo->setHCALCluster(tmp_clusvec);
+
+      m_bkCol.map_PFObjects["bkPFO"].push_back(m_newpfo);
+
+
+      auto iter = find(p_PFObjects->begin(), p_PFObjects->end(), m_chargedPFOs[ipfo]);
+      if(iter!=p_PFObjects->end())
+        *iter = m_newpfo;
+
+      m_chargedPFOs[ipfo] = m_newpfo;
+//cout<<"[FY debug] The splitted new charged PFO: track size "<<m_newpfo->getTracks().size()<<", leading P "<<m_newpfo->getTrackMomentum();
+//cout<<", ECAL cluster size "<<m_newpfo->getECALClusters().size()<<", totE "<<m_newpfo->getECALClusterEnergy();
+//cout<<", HCAL cluster size "<<m_newpfo->getHCALClusters().size()<<", totE "<<m_newpfo->getHCALClusterEnergy()<<endl;
+    }
+
+    m_bkCol.map_CaloHit["bkHit"].push_back( m_hit );
+    m_bkCol.map_CaloCluster["bk3DCluster"].push_back(m_clus);
+    m_bkCol.map_PFObjects["bkPFO"].push_back(m_pfo);
+  }
+
+
+
+  return StatusCode::SUCCESS;
+};
+
+#endif
diff --git a/Reconstruction/CrystalCaloRec/src/Algorithm/TrackClusterConnectingAlg.cpp b/Reconstruction/CrystalCaloRec/src/Algorithm/TrackClusterConnectingAlg.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..324c9d187594dacd957d4ac231ce399b806e279b
--- /dev/null
+++ b/Reconstruction/CrystalCaloRec/src/Algorithm/TrackClusterConnectingAlg.cpp
@@ -0,0 +1,371 @@
+#ifndef _TRACKCLUSTERCONNECTING_ALG_C
+#define _TRACKCLUSTERCONNECTING_ALG_C
+
+#include "Algorithm/TrackClusterConnectingAlg.h"
+
+StatusCode TrackClusterConnectingAlg::ReadSettings(Settings& m_settings){
+  settings = m_settings;
+
+  //Initialize parameters
+  if(settings.map_stringPars.find("ReadinECALClusterName")==settings.map_stringPars.end()) settings.map_stringPars["ReadinECALClusterName"] = "EcalCluster";
+  if(settings.map_stringPars.find("ReadinHCALClusterName")==settings.map_stringPars.end()) settings.map_stringPars["ReadinHCALClusterName"] = "HcalCluster";
+
+  if(settings.map_floatPars.find("th_ChFragEn")==settings.map_floatPars.end()) settings.map_floatPars["th_ChFragEn"] = 2.;
+  if(settings.map_floatPars.find("th_ChFragDepth")==settings.map_floatPars.end()) settings.map_floatPars["th_ChFragDepth"] = 100.;
+  if(settings.map_floatPars.find("th_ChFragMinR")==settings.map_floatPars.end()) settings.map_floatPars["th_ChFragMinR"] = 200.;
+  if(settings.map_floatPars.find("th_HcalMatchingaR")==settings.map_floatPars.end()) settings.map_floatPars["th_HcalMatchingR"] = 100.;
+
+  if(settings.map_floatPars.find("th_MIPEnergy")==settings.map_floatPars.end()) settings.map_floatPars["th_MIPEnergy"] = 0.5;
+  if(settings.map_floatPars.find("th_AbsorbCone")==settings.map_floatPars.end()) settings.map_floatPars["th_AbsorbCone"] = 0.8;
+
+  if(settings.map_stringPars.find("OutputMergedECALCluster")==settings.map_stringPars.end()) settings.map_stringPars["OutputMergedECALCluster"] = "TrkMergedECAL";
+  if(settings.map_stringPars.find("OutputCombPFO")==settings.map_stringPars.end()) settings.map_stringPars["OutputCombPFO"] = "outputPFO";
+
+
+  return StatusCode::SUCCESS;
+};
+
+StatusCode TrackClusterConnectingAlg::Initialize( PandoraPlusDataCol& m_datacol ){
+  m_EcalClusters.clear();
+  m_HcalClusters.clear();
+  m_tracks.clear();
+  m_absorbedEcal.clear();
+  m_PFObjects.clear();
+  m_bkCol.Clear();
+
+  for(int ic=0; ic<m_datacol.map_CaloCluster[settings.map_stringPars["ReadinECALClusterName"]].size(); ic++){
+    m_EcalClusters.push_back( m_datacol.map_CaloCluster[settings.map_stringPars["ReadinECALClusterName"]][ic].get() );
+  }
+  for(int ic=0; ic<m_datacol.map_CaloCluster[settings.map_stringPars["ReadinHCALClusterName"]].size(); ic++){
+    m_HcalClusters.push_back( m_datacol.map_CaloCluster[settings.map_stringPars["ReadinHCALClusterName"]][ic].get() );
+  }
+  for(int itrk=0; itrk<m_datacol.TrackCol.size(); itrk++){
+    m_tracks.push_back( m_datacol.TrackCol[itrk].get() );
+  }
+
+//cout<<"Readin Track size: "<<m_tracks.size()<<", ECAL cluster size: "<<m_EcalClusters.size()<<", HCAL cluster size "<<m_HcalClusters.size()<<endl;
+//cout<<"Print all ECAL cluster "<<endl;
+//for(int ic=0; ic<m_EcalClusters.size(); ic++){
+//  cout<<"    ECAL Cluster #"<<ic<<": En = "<<m_EcalClusters[ic]->getLongiE()<<", track size "<<m_EcalClusters[ic]->getAssociatedTracks().size();
+//  if(m_EcalClusters[ic]->getAssociatedTracks().size()>0) cout<<", Leading track P = "<<m_EcalClusters[ic]->getAssociatedTracks()[0]->getMomentum()<<endl;
+//  else cout<<endl;
+//}
+
+  return StatusCode::SUCCESS;
+};
+
+StatusCode TrackClusterConnectingAlg::RunAlgorithm( PandoraPlusDataCol& m_datacol ){
+  //Readin: tracks, ECAL clusters and HCAL clusters. 
+  //Output: PFObject
+
+
+  //1. Merge ECAL clusters. 
+  //Possible to add some cluster ID and merge functions. 
+  m_absorbedEcal.clear();
+  EcalChFragAbsorption(m_EcalClusters, m_tracks, m_absorbedEcal);
+//cout<<"  TrackClusterConnectingAlg: After ECAL charged fragment absorption: cluster size "<<m_absorbedEcal.size()<<endl;
+
+  //2. Create PFObject with ECAL cluster and track
+  std::vector<const PandoraPlus::Calo3DCluster*> tmp_constClus; 
+  for(int ic=0; ic<m_absorbedEcal.size(); ic++) tmp_constClus.push_back(m_absorbedEcal[ic].get());
+  PFOCreating(tmp_constClus, m_tracks, m_PFObjects);
+
+//cout<<"  TrackClusterConnectingAlg: created PFO: "<<m_PFObjects.size()<<endl;
+//for(int i=0; i<m_PFObjects.size(); i++){
+//  cout<<"    PFO #"<<i<<": track size "<<m_PFObjects[i]->getTracks().size()<<", leading P "<<m_PFObjects[i]->getTrackMomentum();
+//  cout<<", ECAL cluster size "<<m_PFObjects[i]->getECALClusters().size()<<", totE "<<m_PFObjects[i]->getECALClusterEnergy();
+//  cout<<", HCAL cluster size "<<m_PFObjects[i]->getHCALClusters().size()<<", totE "<<m_PFObjects[i]->getHCALClusterEnergy()<<endl;
+//}
+
+  //3. Add HCAL clusters into the PFObject. 
+  std::sort(m_PFObjects.begin(), m_PFObjects.end(), compTrkP);
+  HcalExtrapolatingMatch(m_HcalClusters, m_PFObjects);
+//cout<<"  TrackClusterConnectingAlg: PFO size after HCAL matching: "<<m_PFObjects.size()<<endl;
+//for(int i=0; i<m_PFObjects.size(); i++){
+//  cout<<"    PFO #"<<i<<": track size "<<m_PFObjects[i]->getTracks().size()<<", leading P "<<m_PFObjects[i]->getTrackMomentum();
+//  cout<<", ECAL cluster size "<<m_PFObjects[i]->getECALClusters().size()<<", totE "<<m_PFObjects[i]->getECALClusterEnergy();
+//  cout<<", HCAL cluster size "<<m_PFObjects[i]->getHCALClusters().size()<<", totE "<<m_PFObjects[i]->getHCALClusterEnergy()<<endl;
+//} 
+
+  m_datacol.map_CaloCluster[ settings.map_stringPars["OutputMergedECALCluster"] ] = m_absorbedEcal;
+  m_datacol.map_PFObjects[settings.map_stringPars["OutputCombPFO"]] = m_PFObjects;
+
+  m_datacol.map_CaloCluster["bk3DCluster"].insert( m_datacol.map_CaloCluster["bk3DCluster"].end(), m_bkCol.map_CaloCluster["bk3DCluster"].begin(), m_bkCol.map_CaloCluster["bk3DCluster"].end() );
+  m_datacol.map_PFObjects["bkPFO"].insert( m_datacol.map_PFObjects["bkPFO"].end(), m_bkCol.map_PFObjects["bkPFO"].begin(), m_bkCol.map_PFObjects["bkPFO"].end() );
+  m_datacol.map_PFObjects["bkPFO"].insert( m_datacol.map_PFObjects["bkPFO"].end(), m_PFObjects.begin(), m_PFObjects.end() );
+
+  return StatusCode::SUCCESS;
+};
+
+StatusCode TrackClusterConnectingAlg::ClearAlgorithm(){
+  m_EcalClusters.clear();
+  m_HcalClusters.clear();
+  m_tracks.clear();
+  m_absorbedEcal.clear();
+  m_PFObjects.clear();
+  m_bkCol.Clear();
+
+  return StatusCode::SUCCESS;
+};
+
+
+
+StatusCode TrackClusterConnectingAlg::PFOCreating( std::vector<const PandoraPlus::Calo3DCluster*>& m_clusters, 
+                                                   std::vector<const PandoraPlus::Track*>& m_trks,
+                                                   std::vector<std::shared_ptr<PandoraPlus::PFObject>>& m_PFOs ){
+
+//cout<<"  PFOCreating: Track size "<<m_trks.size()<<", Cluster size "<<m_clusters.size()<<endl;
+//for(int ic=0; ic<m_clusters.size(); ic++){
+//  cout<<"    ECAL Cluster #"<<ic<<": En = "<<m_clusters[ic]->getLongiE()<<", track size "<<m_clusters[ic]->getAssociatedTracks().size();
+//  if(m_clusters[ic]->getAssociatedTracks().size()>0) cout<<", Leading track P = "<<m_clusters[ic]->getAssociatedTracks()[0]->getMomentum()<<endl;
+//  else cout<<endl;
+//}
+
+  std::vector<const PandoraPlus::Track*> m_leftTrks = m_trks; 
+  for(int ic=0; ic<m_clusters.size(); ic++){
+    std::shared_ptr<PandoraPlus::PFObject> m_newPFO = std::make_shared<PandoraPlus::PFObject>();
+
+    m_newPFO->addECALCluster( m_clusters[ic] );
+
+    std::vector<const PandoraPlus::Track*> m_trkInClus = m_clusters[ic]->getAssociatedTracks();
+    if(m_trkInClus.size()!=0){
+      m_newPFO->addTrack( m_trkInClus[0] );
+      auto iter = find(m_leftTrks.begin(), m_leftTrks.end(), m_trkInClus[0]);
+      if( iter!=m_leftTrks.end() )
+        m_leftTrks.erase(iter);
+    }
+
+    m_PFOs.push_back(m_newPFO);
+    m_bkCol.map_PFObjects["bkPFO"].push_back(m_newPFO);
+  }
+
+  for(int itrk=0; itrk<m_leftTrks.size(); itrk++){
+    std::shared_ptr<PandoraPlus::PFObject> m_newPFO = std::make_shared<PandoraPlus::PFObject>();
+    m_newPFO->addTrack( m_leftTrks[itrk] );
+    m_PFOs.push_back(m_newPFO);
+    m_bkCol.map_PFObjects["bkPFO"].push_back(m_newPFO);
+  }
+
+  return StatusCode::SUCCESS;
+}
+
+
+StatusCode TrackClusterConnectingAlg::EcalChFragAbsorption( std::vector<const PandoraPlus::Calo3DCluster*>& m_clusters, 
+                                                            std::vector<const PandoraPlus::Track*>& m_trks, 
+                                                            std::vector<std::shared_ptr<PandoraPlus::Calo3DCluster>>& m_newclusCol){
+//cout<<"  In EcalChFragAbsorption: Input track size "<<m_trks.size()<<", cluster size "<<m_clusters.size()<<endl;
+//for(int ic=0; ic<m_clusters.size(); ic++){
+//  cout<<"    ECAL Cluster #"<<ic<<": En = "<<m_clusters[ic]->getLongiE()<<", track size "<<m_clusters[ic]->getAssociatedTracks().size()<<endl;
+//}
+
+
+  //1. Absorb neutral clusters to the nearby tracks
+  std::map<const PandoraPlus::Track*, int> m_matchedTrkMap; 
+  for(int ic=0; ic<m_clusters.size(); ic++){
+    for(int itrk=0; itrk<m_clusters[ic]->getAssociatedTracks().size(); itrk++){
+      if( find(m_trks.begin(), m_trks.end(), m_clusters[ic]->getAssociatedTracks()[itrk])!=m_trks.end() )
+        m_matchedTrkMap[m_clusters[ic]->getAssociatedTracks()[itrk]] = ic;
+    }
+  }
+//cout<<"  Matched track size: "<<m_matchedTrkMap.size()<<endl;
+
+  for(int ic=0; ic<m_clusters.size(); ic++){
+    if(m_clusters[ic]->getAssociatedTracks().size()!=0) continue; 
+
+    double clusDepth = m_clusters[ic]->getDepthToECALSurface();
+    double clusEn = m_clusters[ic]->getLongiE();
+
+    double minR2trk = 9999;
+    int index = -1;
+    for(int itrk=0; itrk<m_trks.size(); itrk++){
+      double tmp_minR = GetMinR2Trk( m_clusters[ic], m_trks[itrk]);
+      if(tmp_minR<minR2trk){
+        minR2trk = tmp_minR;
+        index = itrk;
+      }
+    }
+//cout<<"    Clus #"<<ic<<": depth "<<clusDepth<<", En "<<clusEn<<", minR "<<minR2trk<<", index "<<index<<endl;
+    //if(index<0){
+    //  std::cout<<"ERROR: can not find closest track "<<endl;
+    //  continue;
+    //}
+
+    if( clusEn<settings.map_floatPars["th_ChFragEn"] && clusDepth>settings.map_floatPars["th_ChFragDepth"] && minR2trk<settings.map_floatPars["th_ChFragMinR"]){
+      const PandoraPlus::Track* p_selTrk = m_trks[index]; //Closest track to this cluster. 
+
+      if( m_matchedTrkMap.find(p_selTrk)==m_matchedTrkMap.end() ){ //This track does not match to any existing charged cluster
+        std::shared_ptr<PandoraPlus::Calo3DCluster> m_newclus = m_clusters[ic]->Clone();
+        m_newclus->addAssociatedTrack(p_selTrk);
+        m_newclusCol.push_back( m_newclus );
+        m_bkCol.map_CaloCluster["bk3DCluster"].push_back(m_newclus);
+      }
+      else{ 
+        int tmp_index = m_matchedTrkMap[p_selTrk];
+        std::shared_ptr<PandoraPlus::Calo3DCluster> m_newclus = m_clusters[tmp_index]->Clone();
+        m_newclus->mergeCluster(m_clusters[ic]);
+        m_newclusCol.push_back( m_newclus );
+        m_matchedTrkMap.erase(p_selTrk);
+        m_bkCol.map_CaloCluster["bk3DCluster"].push_back(m_newclus);
+      }
+    }
+    else{
+      std::shared_ptr<PandoraPlus::Calo3DCluster> m_newclus = m_clusters[ic]->Clone();
+      m_newclusCol.push_back( m_newclus );
+      m_bkCol.map_CaloCluster["bk3DCluster"].push_back(m_newclus);
+    }
+
+  }
+
+  for(auto iter: m_matchedTrkMap){
+    std::shared_ptr<PandoraPlus::Calo3DCluster> m_newclus = m_clusters[iter.second]->Clone();
+    m_newclusCol.push_back( m_newclus );
+    m_bkCol.map_CaloCluster["bk3DCluster"].push_back(m_newclus);    
+  }
+
+
+  //Merge clusters if linked to the same track
+  for(int ic=0; ic<m_newclusCol.size() && m_newclusCol.size()>1; ic++){
+    if(m_newclusCol[ic].get()->getAssociatedTracks().size()==0) continue;
+    std::vector<const PandoraPlus::Track*> m_trkCol = m_newclusCol[ic].get()->getAssociatedTracks();
+
+    for(int jc=ic+1; jc<m_newclusCol.size(); jc++){
+      if(m_newclusCol[jc].get()->getAssociatedTracks().size()==0) continue;
+
+      for(int itrk=0; itrk<m_newclusCol[jc].get()->getAssociatedTracks().size(); itrk++){
+        if( find(m_trkCol.begin(), m_trkCol.end(), m_newclusCol[jc].get()->getAssociatedTracks()[itrk])!= m_trkCol.end() ){
+          m_newclusCol[ic].get()->mergeCluster( m_newclusCol[jc].get() );
+          m_newclusCol.erase(m_newclusCol.begin()+jc);
+          jc--;
+          if(jc<ic) jc=ic;
+        }
+        break;
+      }
+    }
+  }
+  for(int ic=0; ic<m_newclusCol.size(); ic++) m_newclusCol[ic].get()->getLinkedMCPfromHFCluster("LinkedLongiCluster");
+
+
+
+//cout<<"After nearby absorption: Print ECAL cluster "<<endl;
+//for(int ic=0; ic<m_newclusCol.size(); ic++){
+//  cout<<"    ECAL Cluster #"<<ic<<": En = "<<m_newclusCol[ic]->getLongiE()<<", track size "<<m_newclusCol[ic]->getAssociatedTracks().size();
+//  if(m_newclusCol[ic]->getAssociatedTracks().size()>0) cout<<", Leading track P = "<<m_newclusCol[ic]->getAssociatedTracks()[0]->getMomentum()<<endl;
+//  else cout<<endl;
+//}
+
+  //2. Find the shower vertex, absorb nearby neutral clusters (in a cone) into it. 
+  std::vector<std::shared_ptr<PandoraPlus::Calo3DCluster>> m_newChCluster;
+  std::vector<std::shared_ptr<PandoraPlus::Calo3DCluster>> m_newNeuCluster;
+  for(int icl=0; icl<m_newclusCol.size(); icl++){
+    if(m_newclusCol[icl]->getAssociatedTracks().size()==0 ) m_newNeuCluster.push_back(m_newclusCol[icl]);
+    else m_newChCluster.push_back(m_newclusCol[icl]);
+  }
+
+  for(int icl=0; icl<m_newChCluster.size(); icl++){
+    TVector3 cent = m_newChCluster[icl]->getShowerCenter();
+    double tmp_Ecl = m_newChCluster[icl]->getLongiE();
+
+    //Veto mip clusters and Eclus>Ptrk
+    if(tmp_Ecl<settings.map_floatPars["th_MIPEnergy"]) continue;
+    if(tmp_Ecl>m_newChCluster[icl]->getAssociatedTracks()[0]->getMomentum()) continue;
+
+    //Absorb neutral clusters in a cone angle
+    for(int jcl=0; jcl<m_newNeuCluster.size(); jcl++){
+      //Do not absorb: En, Nhit, start layer, 
+
+
+      TVector3 vec_pNeu = m_newNeuCluster[jcl]->getShowerCenter();
+      if( cent.Angle(vec_pNeu-cent)<settings.map_floatPars["th_AbsorbCone"] ){
+        m_newChCluster[icl]->mergeCluster(m_newNeuCluster[jcl].get());
+        auto iter = find(m_newclusCol.begin(), m_newclusCol.end(), m_newNeuCluster[jcl]);
+        m_newclusCol.erase(iter);
+        m_newNeuCluster.erase(m_newNeuCluster.begin()+jcl);
+        jcl--;
+      }
+    }
+  }
+//for(int ic=0; ic<m_newclusCol.size(); ic++){
+//  cout<<"    ECAL Cluster #"<<ic<<": En = "<<m_newclusCol[ic]->getLongiE()<<", track size "<<m_newclusCol[ic]->getAssociatedTracks().size()<<endl;
+//}
+
+  return StatusCode::SUCCESS;
+};
+
+
+StatusCode TrackClusterConnectingAlg::HcalExtrapolatingMatch(std::vector<const PandoraPlus::Calo3DCluster*>& m_clusters, std::vector<std::shared_ptr<PandoraPlus::PFObject>>& m_PFOs){
+
+  for(int ic=0; ic<m_clusters.size(); ic++){
+    std::vector<const PandoraPlus::CaloHit*> hcal_hits = m_clusters[ic]->getCaloHits();
+//cout<<"HCAL Cluster #"<<ic<<": Nhit "<<hcal_hits.size()<<", En "<<m_clusters[ic]->getHitsE()<<endl;
+
+    bool isInPfo = false; 
+    int index_selPfo = -1;
+    for(int ipfo=0; ipfo<m_PFOs.size(); ipfo++){
+      //Link HCAL cluster to charged PFO
+      if(m_PFOs[ipfo]->getTracks().size()!=0){ 
+        std::vector<TrackState> trk_points = m_PFOs[ipfo]->getTracks()[0]->getAllTrackStates();
+
+
+        bool is_candidate = false;
+        double minDistance = 99999;
+        for(int ihit=0; ihit<hcal_hits.size(); ihit++){
+          if(is_candidate) break;
+          TVector3 hit_pos = hcal_hits[ihit]->getPosition();
+   
+          for(int ipts=0; ipts<trk_points.size(); ipts++){
+            TVector3 hit_distance = hit_pos - trk_points[ipts].referencePoint;;
+            if(minDistance>hit_distance.Mag())  minDistance = hit_distance.Mag();
+            if(hit_distance.Mag()<settings.map_floatPars["th_HcalMatchingR"]){
+              is_candidate = true;
+              break;
+            }
+          }
+        }
+//cout<<"  Min distance "<<minDistance<<", is candidate "<<is_candidate<<endl;
+
+        if(is_candidate){
+//cout<<"  Pfo #"<<ipfo<<": Ntrk "<<m_PFOs[ipfo]->getTracks().size()<<", leading trk P "<<m_PFOs[ipfo]->getTrackMomentum()<<", trk state size "<<trk_points.size()<<endl;
+//cout<<"  Link cluster #"<<ic<<" to pfo #"<<ipfo<<endl;
+          m_PFOs[ipfo]->addHCALCluster( m_clusters[ic] );
+          isInPfo = true;
+          index_selPfo = ipfo;
+          break;
+        }        
+
+      }
+  
+      //Link HCAL cluster to neutral PFO
+
+    }//end loop pfos
+//if(isInPfo) cout<<"  Merged into PFO: Ptrk = "<<m_PFOs[index_selPfo]->getTrackMomentum()<<endl;
+
+    //If HCAL cluster is not linked to any existing PFO: create a new one. 
+    if(!isInPfo){
+//cout<<"  Create a new neutral PFO "<<endl;
+      std::shared_ptr<PandoraPlus::PFObject> m_newPFO = std::make_shared<PandoraPlus::PFObject>();
+      m_newPFO->addHCALCluster( m_clusters[ic] );     
+      m_PFOs.push_back(m_newPFO);
+      m_bkCol.map_PFObjects["bkPFO"].push_back(m_newPFO);
+    }
+  }
+
+  return StatusCode::SUCCESS;
+}
+
+
+double TrackClusterConnectingAlg::GetMinR2Trk( const PandoraPlus::Calo3DCluster* p_clus, const PandoraPlus::Track* m_trk){
+  if(!p_clus || !m_trk) return 99999;
+
+  double minR = 99999;
+  int index = -1;
+  TVector3 clus_position = p_clus->getShowerCenter();
+  std::vector<TrackState> trk_points = m_trk->getAllTrackStates();
+  for(int i=0; i<trk_points.size(); i++){
+    TVector3 hit_distance = clus_position - trk_points[i].referencePoint;
+    if(hit_distance.Mag()<minR) minR = hit_distance.Mag();
+  }  
+
+  return minR;
+}
+#endif
+
diff --git a/Reconstruction/CrystalCaloRec/src/Algorithm/TrackExtrapolatingAlg.cpp b/Reconstruction/CrystalCaloRec/src/Algorithm/TrackExtrapolatingAlg.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5473b9e14995ddb2ef5d0809d6f73dce39e6b869
--- /dev/null
+++ b/Reconstruction/CrystalCaloRec/src/Algorithm/TrackExtrapolatingAlg.cpp
@@ -0,0 +1,450 @@
+#ifndef _TRACKEXTRAPOLATING_ALG_C
+#define _TRACKEXTRAPOLATING_ALG_C
+
+#include "TVector2.h"
+
+#include "Algorithm/TrackExtrapolatingAlg.h"
+#include "Objects/Track.h"
+#include "Objects/TrackState.h"
+
+using namespace TMath;
+using namespace std;
+
+StatusCode TrackExtrapolatingAlg::ReadSettings(Settings& m_settings){
+  settings = m_settings;
+
+  //Initialize parameters
+  // ECAL parameters
+  if(settings.map_floatPars.find("ECAL_innermost_distance")==settings.map_floatPars.end()) 
+    settings.map_floatPars["ECAL_innermost_distance"] = 1830;
+  if(settings.map_floatPars.find("ECAL_outermost_distance")==settings.map_floatPars.end()) 
+    settings.map_floatPars["ECAL_outermost_distance"] = 2130;
+  if(settings.map_intPars.find("ECAL_Nlayers")==settings.map_intPars.end()) 
+    settings.map_intPars["ECAL_Nlayers"] = 30;
+  if(settings.map_floatPars.find("ECAL_layer_width")==settings.map_floatPars.end())
+    settings.map_floatPars["ECAL_layer_width"] = 10;
+  if(settings.map_floatPars.find("ECAL_half_length")==settings.map_floatPars.end())
+    settings.map_floatPars["ECAL_half_length"] = 2900;
+  // HCAL parameters
+  if(settings.map_floatPars.find("HCAL_innermost_distance")==settings.map_floatPars.end()) 
+    settings.map_floatPars["HCAL_innermost_distance"] = 2140;
+  if(settings.map_floatPars.find("HCAL_outermost_distance")==settings.map_floatPars.end()) 
+    settings.map_floatPars["HCAL_outermost_distance"] = 3455;
+  if(settings.map_intPars.find("HCAL_Nlayers")==settings.map_intPars.end()) 
+    settings.map_intPars["HCAL_Nlayers"] = 48;
+  if(settings.map_floatPars.find("HCAL_layer_width")==settings.map_floatPars.end())
+    settings.map_floatPars["HCAL_layer_width"] = 30.5;
+  if(settings.map_floatPars.find("HCAL_sensitive_distance")==settings.map_floatPars.end())
+    settings.map_floatPars["HCAL_sensitive_distance"] = 9.9;  // distance between sensitive material and front face of each layer
+  if(settings.map_floatPars.find("HCAL_half_length")==settings.map_floatPars.end())
+    settings.map_floatPars["HCAL_half_length"] = 3300;
+
+  if(settings.map_intPars.find("Nmodule_ECAL")==settings.map_intPars.end()) 
+    settings.map_intPars["Nmodule_ECAL"] = 32;
+  if(settings.map_intPars.find("Nmodule_HCAL")==settings.map_intPars.end()) 
+    settings.map_intPars["Nmodule_HCAL"] = 16;
+
+  if(settings.map_floatPars.find("B_field")==settings.map_floatPars.end())
+    settings.map_floatPars["B_field"] = 3.0;
+
+  if(settings.map_intPars.find("Input_track")==settings.map_intPars.end())
+    settings.map_intPars["Input_track"] = 0;    // 0: reconstructed tracks.  1: MC particle track
+
+  return StatusCode::SUCCESS;
+};
+
+
+StatusCode TrackExtrapolatingAlg::Initialize( PandoraPlusDataCol& m_datacol ){
+  std::cout<<"Initialize TrackExtrapolatingAlg"<<std::endl;
+
+  return StatusCode::SUCCESS;
+};
+
+
+StatusCode TrackExtrapolatingAlg::RunAlgorithm( PandoraPlusDataCol& m_datacol ){
+//std::cout<<"---oooOO0OOooo--- Excuting TrackExtrapolatingAlg ---oooOO0OOooo---"<<std::endl;
+
+  std::vector<std::shared_ptr<PandoraPlus::Track>>* p_tracks = &(m_datacol.TrackCol);
+//std::cout<<"  Track size: "<<p_tracks->size()<<std::endl;
+
+//std::cout<<"  GetPlaneNormalVector() "<<std::endl;
+  std::vector<TVector2> normal_vectors_Ecal, normal_vectors_Hcal;
+  GetPlaneNormalVector(normal_vectors_Ecal, normal_vectors_Hcal);
+
+//std::cout<<"  GetLayerPoints() "<<std::endl;
+  std::vector<std::vector<TVector2>> ECAL_layer_points;    // 32 modules, 30 layer points in each modules
+  std::vector<std::vector<TVector2>> HCAL_layer_points;    // 16 modules, 48 layer points in each modules
+  GetLayerPoints(normal_vectors_Ecal, normal_vectors_Hcal, ECAL_layer_points, HCAL_layer_points);
+
+  for(int itrk=0; itrk<p_tracks->size(); itrk++){
+    // Only tracks that reach ECAL should be processed.
+    if(!IsReachECAL( p_tracks->at(itrk).get() )) continue;
+
+//std::cout<<"  GetTrackStateAtCalo() "<<std::endl;
+    // get track state at calorimeter
+    PandoraPlus::TrackState CALO_trk_state;
+    GetTrackStateAtCalo(p_tracks->at(itrk).get(), CALO_trk_state);
+    
+//std::cout<<"  ExtrapolateByLayer() "<<std::endl;
+    ExtrapolateByLayer(normal_vectors_Ecal, normal_vectors_Hcal, ECAL_layer_points, HCAL_layer_points, CALO_trk_state, p_tracks->at(itrk).get());
+  } // end loop tracks
+
+
+  p_tracks = nullptr;
+  return StatusCode::SUCCESS;
+}; // RunAlgorithm end
+
+
+StatusCode TrackExtrapolatingAlg::ClearAlgorithm(){
+  std::cout<<"End run TrackExtrapolatingAlg. Clean it."<<std::endl;
+
+  return StatusCode::SUCCESS;
+};
+
+
+// StatusCode TrackExtrapolatingAlg::SelfAlg1(){
+//   std::cout<<"  Processing SelfAlg1: print Par1 = "<<settings.map_floatPars["Par1"]<<std::endl;
+
+//   return StatusCode::SUCCESS;
+// };
+
+
+
+// ...oooOO0OOooo......oooOO0OOooo......oooOO0OOooo...
+StatusCode TrackExtrapolatingAlg::GetPlaneNormalVector(std::vector<TVector2> & normal_vectors_Ecal, std::vector<TVector2> & normal_vectors_Hcal){
+  normal_vectors_Ecal.clear();
+  normal_vectors_Hcal.clear();
+
+  for(int im=0; im<settings.map_intPars["Nmodule_ECAL"]; im++){    
+    TVector2 t_vec(0, 1);
+    t_vec = t_vec.Rotate(1.*im/settings.map_intPars["Nmodule_ECAL"]*2*Pi());
+    normal_vectors_Ecal.push_back(t_vec);
+  }
+
+  for(int im=0; im<settings.map_intPars["Nmodule_HCAL"]; im++){
+    TVector2 t_vec(0, 1);
+    t_vec = t_vec.Rotate(1.*im/settings.map_intPars["Nmodule_HCAL"]*2*Pi());
+    normal_vectors_Hcal.push_back(t_vec);
+  }
+
+  return StatusCode::SUCCESS;
+}
+
+
+// ...oooOO0OOooo......oooOO0OOooo......oooOO0OOooo...
+StatusCode TrackExtrapolatingAlg::GetLayerPoints(const std::vector<TVector2> & normal_vectors_Ecal, const std::vector<TVector2> & normal_vectors_Hcal, 
+                          std::vector<std::vector<TVector2>> & ECAL_layer_points,
+                          std::vector<std::vector<TVector2>> & HCAL_layer_points){
+  // ECAL
+  for(int im=0; im< normal_vectors_Ecal.size(); im++){
+    std::vector<TVector2> t_points;
+    for(int il=0; il<settings.map_intPars["ECAL_Nlayers"]; il++){
+      TVector2 t_p;
+      float dist = settings.map_floatPars["ECAL_innermost_distance"] + 
+                   settings.map_floatPars["ECAL_layer_width"]*il + settings.map_floatPars["ECAL_layer_width"]/2;
+      float xx = (normal_vectors_Ecal[im].X()/normal_vectors_Ecal[im].Mod()) * dist;
+      float yy = (normal_vectors_Ecal[im].Y()/normal_vectors_Ecal[im].Mod()) * dist;
+      t_p.SetX(xx); t_p.SetY(yy);
+      t_points.push_back(t_p);
+    }
+    ECAL_layer_points.push_back(t_points);
+  }
+
+  // HCAL
+  for(int im=0; im< normal_vectors_Hcal.size(); im++){
+    std::vector<TVector2> t_points;
+    for(int il=0; il<settings.map_intPars["HCAL_Nlayers"]; il++){
+      TVector2 t_p;
+      float dist = settings.map_floatPars["HCAL_innermost_distance"] + 
+                   settings.map_floatPars["HCAL_layer_width"]*il + settings.map_floatPars["HCAL_sensitive_distance"];
+      float xx = (normal_vectors_Hcal[im].X()/normal_vectors_Hcal[im].Mod()) * dist;
+      float yy = (normal_vectors_Hcal[im].Y()/normal_vectors_Hcal[im].Mod()) * dist;
+      t_p.SetX(xx); t_p.SetY(yy);
+      t_points.push_back(t_p);
+    }
+    HCAL_layer_points.push_back(t_points);
+  }
+
+  return StatusCode::SUCCESS;
+}
+
+
+// ...oooOO0OOooo......oooOO0OOooo......oooOO0OOooo...
+bool TrackExtrapolatingAlg::IsReachECAL(PandoraPlus::Track * track){
+  if(settings.map_intPars["Input_track"] == 0){
+    // The track is reconstructed in ECAL. If the track reach ECAL, it should have track state at calorimeter
+    std::vector<TrackState> input_trackstates = track->getTrackStates("Input");
+    int count=0;
+    TVector3 t_vec;
+    for(int i=0; i<input_trackstates.size(); i++){
+      if(input_trackstates[i].location==PandoraPlus::TrackState::AtCalorimeter){
+        count++;
+        t_vec = input_trackstates[i].referencePoint;
+        break;
+      }
+    }
+    if(count==0){
+  std::cout<<"      the track has no track state at calorimeter"<<std::endl;
+      return false;
+    } 
+    if( Abs(Abs(t_vec.Z())-settings.map_floatPars["ECAL_half_length"]) < 1e-6 ){
+  std::cout<<"      the track escape from endcap"<<std::endl;
+      return false;
+    } 
+    
+
+    return true;
+  }
+  else if(settings.map_intPars["Input_track"] == 1){
+    // The track is from MC particle as ideal helix. 
+    // The pT should large enough to reach ECAL. The pz should not be so large that it escape from endcap
+    std::vector<TrackState> input_trackstates = track->getTrackStates("Input");
+    if(input_trackstates.size()==0){
+      std::cout << "Error! No track state!" << std::endl;
+      return false;
+    }
+
+    TrackState IP_trk_state;
+    for(int i=0; i<input_trackstates.size(); i++){
+      if(input_trackstates[i].location==PandoraPlus::TrackState::AtIP)
+        IP_trk_state = input_trackstates[i];
+      break;
+    }
+
+    TVector3 ref_point = IP_trk_state.referencePoint;
+    double rho = GetRho(IP_trk_state);
+    double r_max = TMath::Sqrt(ref_point.X()*ref_point.X() + ref_point.Y()*ref_point.Y()) + rho*2;
+
+    if(r_max<settings.map_floatPars["ECAL_innermost_distance"]){ return false; }
+
+    return true;
+  }
+  else{
+    std::cout << "Error, wrong source of input tracks for TrackExtrapolatingAlg!" << std:: endl;
+    return false;
+  }
+  
+}
+
+
+// ...oooOO0OOooo......oooOO0OOooo......oooOO0OOooo...
+StatusCode TrackExtrapolatingAlg::GetTrackStateAtCalo(PandoraPlus::Track * track, 
+                                                PandoraPlus::TrackState & trk_state_at_calo){
+  std::vector<TrackState> input_trackstates = track->getTrackStates("Input");
+
+  if(settings.map_intPars["Input_track"] == 0){
+    for(int its=0; its<input_trackstates.size(); its++){
+      if(input_trackstates[its].location==PandoraPlus::TrackState::AtCalorimeter){
+        trk_state_at_calo=input_trackstates[its];
+        break;
+      }
+    }
+  }
+  else if((settings.map_intPars["Input_track"] == 1)){
+    for(int its=0; its<input_trackstates.size(); its++){
+      if(input_trackstates[its].location==PandoraPlus::TrackState::AtIP){
+        trk_state_at_calo=input_trackstates[its];
+        break;
+      }
+    }
+  }
+  else{
+    std::cout << "Error, wrong source of input tracks for TrackExtrapolatingAlg!" << std:: endl;
+  }
+  
+  return StatusCode::SUCCESS;
+  
+}
+
+
+// ...oooOO0OOooo......oooOO0OOooo......oooOO0OOooo...
+StatusCode TrackExtrapolatingAlg::ExtrapolateByLayer(const std::vector<TVector2> & normal_vectors_Ecal,
+                              const std::vector<TVector2> & normal_vectors_Hcal,
+                              const std::vector<std::vector<TVector2>> & ECAL_layer_points, 
+                              const std::vector<std::vector<TVector2>> & HCAL_layer_points, 
+                              const PandoraPlus::TrackState & CALO_trk_state, 
+                              PandoraPlus::Track* p_track){
+  float rho = GetRho(CALO_trk_state);
+  TVector2 center = GetCenterOfCircle(CALO_trk_state, rho);
+  float alpha0 = GetRefAlpha0(CALO_trk_state, center); 
+  // bool is_rtbk = IsReturn(rho, center);
+  // Evaluate delta_phi
+  std::vector<std::vector<float>> ECAL_delta_phi = GetDeltaPhi(rho, center, alpha0, normal_vectors_Ecal, ECAL_layer_points, CALO_trk_state);
+  std::vector<std::vector<float>> HCAL_delta_phi = GetDeltaPhi(rho, center, alpha0, normal_vectors_Hcal, HCAL_layer_points, CALO_trk_state);
+  
+  // extrpolated track points. Will be stored as reference point in TrackState
+  std::vector<TVector3> ECAL_ext_points = GetExtrapoPoints("ECAL", rho, center, alpha0, CALO_trk_state, ECAL_delta_phi);
+  std::vector<TVector3> HCAL_ext_points = GetExtrapoPoints("HCAL", rho, center, alpha0, CALO_trk_state, HCAL_delta_phi);
+  
+  // Sort Extrapolated points
+  std::vector<TrackState> t_ECAL_states;
+  for(int ip=0; ip<ECAL_ext_points.size(); ip++){
+    TrackState t_state = CALO_trk_state;
+    t_state.location = PandoraPlus::TrackState::AtOther;
+    t_state.referencePoint = ECAL_ext_points[ip];
+    // Note GetExtrapolatedPhi0 is not same as the definition of phi0 in TrackState
+    t_state.phi0 = GetExtrapolatedPhi0(CALO_trk_state.Kappa, CALO_trk_state.phi0, center, ECAL_ext_points[ip]);
+    t_ECAL_states.push_back(t_state);
+  }
+  std::sort(t_ECAL_states.begin(), t_ECAL_states.end(), SortByPhi0);
+  p_track->setTrackStates("Ecal", t_ECAL_states);
+
+  std::vector<TrackState> t_HCAL_states;
+  for(int ip=0; ip<HCAL_ext_points.size(); ip++){
+    TrackState t_state = CALO_trk_state;
+    t_state.location = PandoraPlus::TrackState::AtOther;
+    t_state.referencePoint = HCAL_ext_points[ip];
+    // Note GetExtrapolatedPhi0 is not same as the definition of phi0 in TrackState
+    t_state.phi0 = GetExtrapolatedPhi0(CALO_trk_state.Kappa, CALO_trk_state.phi0, center, HCAL_ext_points[ip]);
+    t_HCAL_states.push_back(t_state);
+  }
+  std::sort(t_HCAL_states.begin(), t_HCAL_states.end(), SortByPhi0);
+  p_track->setTrackStates("Hcal", t_HCAL_states);
+  
+  return StatusCode::SUCCESS;
+}
+
+
+// ...oooOO0OOooo......oooOO0OOooo......oooOO0OOooo...
+float TrackExtrapolatingAlg::GetRho(const PandoraPlus::TrackState & trk_state){
+  float rho = Abs(1000. / (0.3*settings.map_floatPars["B_field"]*trk_state.Kappa));
+  return rho;
+}
+
+
+// ...oooOO0OOooo......oooOO0OOooo......oooOO0OOooo...
+TVector2 TrackExtrapolatingAlg::GetCenterOfCircle(const PandoraPlus::TrackState & trk_state, const float & rho){
+  float phi;
+  if(trk_state.Kappa>=0) phi = trk_state.phi0 - Pi()/2;
+  else phi = trk_state.phi0 + Pi()/2;
+
+  float xc = trk_state.referencePoint.X() + ((rho+trk_state.D0)*Cos(phi));
+  float yc = trk_state.referencePoint.Y() + ((rho+trk_state.D0)*Sin(phi));
+  TVector2 center(xc, yc);
+  return center;
+}
+
+
+// ...oooOO0OOooo......oooOO0OOooo......oooOO0OOooo...
+float TrackExtrapolatingAlg::GetRefAlpha0(const PandoraPlus::TrackState & trk_state, const TVector2 & center){
+  float deltaX = trk_state.referencePoint.X() - center.X();
+  float deltaY = trk_state.referencePoint.Y() - center.Y();
+  float alpha0 = ATan2(deltaY, deltaX);
+  return alpha0;
+}
+
+
+// ...oooOO0OOooo......oooOO0OOooo......oooOO0OOooo...
+std::vector<std::vector<float>> TrackExtrapolatingAlg::GetDeltaPhi(float rho, TVector2 center, float alpha0,
+                              const std::vector<TVector2> & normal_vectors,
+                              const std::vector<std::vector<TVector2>> & layer_points, 
+                              const PandoraPlus::TrackState & CALO_trk_state){
+  std::vector<std::vector<float>> delta_phi;
+  for(int im=0; im<normal_vectors.size(); im++){  // for each module
+    std::vector<float> t_delta_phi;
+    float beta = ATan2(normal_vectors[im].X(), normal_vectors[im].Y());
+    float denominator = rho * Sqrt( (normal_vectors[im].X()*normal_vectors[im].X()) +
+                                    (normal_vectors[im].Y()*normal_vectors[im].Y()) );
+    for(int il=0; il<layer_points[im].size(); il++){  // for each layer
+      float numerator = (layer_points[im][il].X()*normal_vectors[im].X()) -
+                        (center.X()*normal_vectors[im].X()) + 
+                        (layer_points[im][il].Y()*normal_vectors[im].Y()) -
+                        (center.Y()*normal_vectors[im].Y()) ;
+      if(Abs(numerator/denominator)>1) continue;
+      float t_as = ASin(numerator/denominator);
+      float t_ab = ASin(Sin(alpha0+beta));
+
+      float t_dphi1 = t_as - t_ab;
+      // float t_dphi2 = Pi()-t_as - t_ab;
+      if(CALO_trk_state.Kappa < 0){ 
+        t_delta_phi.push_back(t_dphi1); 
+        // if(is_rtbk){
+        //   t_delta_phi.push_back(t_dphi2);
+        // }
+      }
+      else{
+        t_delta_phi.push_back(-t_dphi1); 
+        // if(is_rtbk){
+        //   t_delta_phi.push_back(-t_dphi2);
+        // }
+      }
+    }
+    delta_phi.push_back(t_delta_phi);
+  }
+
+  return delta_phi;
+}
+
+
+// ...oooOO0OOooo......oooOO0OOooo......oooOO0OOooo...
+std::vector<TVector3> TrackExtrapolatingAlg::GetExtrapoPoints(std::string calo_name, 
+                                         float rho, TVector2 center, float alpha0, 
+                                         const PandoraPlus::TrackState & CALO_trk_state,
+                                         const std::vector<std::vector<float>>& delta_phi){
+  std::vector<TVector3> ext_points;
+  for(int im=0; im<delta_phi.size(); im++){
+    for(int ip=0; ip<delta_phi[im].size(); ip++){
+      float x = center.X() + (rho*Cos(alpha0+delta_phi[im][ip]));
+      float y = center.Y() + (rho*Sin(alpha0+delta_phi[im][ip]));
+      float z;
+      if(CALO_trk_state.Kappa > 0){
+        z = CALO_trk_state.referencePoint.Z() + CALO_trk_state.Z0 - 
+                (delta_phi[im][ip]*rho*CALO_trk_state.tanLambda);
+      }else{
+        z = CALO_trk_state.referencePoint.Z() + CALO_trk_state.Z0 + 
+                (delta_phi[im][ip]*rho*CALO_trk_state.tanLambda);
+      }
+
+      TVector3 t_vec3(x,y,z);
+      float rotate_angle = (6-im)*Pi()/4;
+      t_vec3.RotateZ(rotate_angle);
+
+      if(calo_name=="ECAL"){
+        if(Abs(z)>settings.map_floatPars["ECAL_half_length"]) continue;
+        if(Sqrt(x*x+y*y) > settings.map_floatPars["ECAL_outermost_distance"]/Cos(Pi()/settings.map_intPars["Nmodule_ECAL"])+100) continue;
+        if(Sqrt(x*x+y*y) < settings.map_floatPars["ECAL_innermost_distance"]) continue;
+        if(settings.map_floatPars["ECAL_outermost_distance"]*Sqrt(2)-t_vec3.X() < t_vec3.Y()) continue;
+        if(t_vec3.X()-settings.map_floatPars["ECAL_innermost_distance"]*Sqrt(2) > t_vec3.Y()) continue;
+      }
+      else if(calo_name=="HCAL"){
+        if(Abs(z)>settings.map_floatPars["HCAL_half_length"]) continue;
+        if(Sqrt(x*x+y*y) > settings.map_floatPars["HCAL_outermost_distance"]) continue;
+        if(Sqrt(x*x+y*y) < settings.map_floatPars["HCAL_innermost_distance"]) continue;
+        if(t_vec3.X()-settings.map_floatPars["HCAL_innermost_distance"]*Sqrt(2) > t_vec3.Y()) continue;
+      }
+      else continue;
+      
+      TVector3 extp(x,y,z);
+      ext_points.push_back(extp);
+    }
+  }          
+
+  return ext_points;
+}
+
+
+// ...oooOO0OOooo......oooOO0OOooo......oooOO0OOooo...
+bool TrackExtrapolatingAlg::IsReturn(float rho, TVector2 & center){
+  float farest = rho + center.Mod();
+  if (farest < settings.map_floatPars["ECAL_outermost_distance"]/Cos(Pi()/settings.map_intPars["Nmodule_ECAL"])+100) {return true;}
+  else{return false;}
+}
+
+
+// ...oooOO0OOooo......oooOO0OOooo......oooOO0OOooo...
+float TrackExtrapolatingAlg::GetExtrapolatedPhi0(float Kappa, float ECAL_phi0, TVector2 center, TVector3 ext_point){
+  // Note: phi0 of extrapolated points is (phi of velocity at extrapolated point) - (phi of velocity at ECAL front face)
+  TVector2 ext_point_xy(ext_point.X(), ext_point.Y());
+  TVector2 ext2center = center - ext_point_xy;
+  float ext_phi0;
+  if(Kappa>=0) ext_phi0 =  ext2center.Phi() + TMath::Pi()/2.;
+  else ext_phi0 = ext2center.Phi() - TMath::Pi()/2.;
+
+  float phi0 = ext_phi0 - ECAL_phi0;
+  while(phi0 < -Pi())  phi0 = phi0 + 2*Pi();
+  while(phi0 >  Pi())  phi0 = phi0 - 2*Pi();
+  return phi0;
+}
+
+
+#endif
diff --git a/Reconstruction/CrystalCaloRec/src/Algorithm/TrackMatchingAlg.cpp b/Reconstruction/CrystalCaloRec/src/Algorithm/TrackMatchingAlg.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b5c93e61cbf49bf4f2ef3672e175f8a31312c7e0
--- /dev/null
+++ b/Reconstruction/CrystalCaloRec/src/Algorithm/TrackMatchingAlg.cpp
@@ -0,0 +1,604 @@
+#ifndef TRACKMATCHING_C
+#define TRACKMATCHING_C
+
+#include "Algorithm/TrackMatchingAlg.h"
+
+
+StatusCode TrackMatchingAlg::ReadSettings(PandoraPlus::Settings& m_settings){
+  settings = m_settings;
+  // ECAL geometry settings
+  // Note: Bar half length is also geometry parameter, but obtained from the function GetBarHalfLength()
+  if(settings.map_floatPars.find("localmax_area")==settings.map_floatPars.end())
+    settings.map_floatPars["localmax_area"] = 10; // unit: mm
+  if(settings.map_floatPars.find("ConeNearByDistance")==settings.map_floatPars.end())
+    settings.map_floatPars["ConeNearByDistance"] = 100;
+  if(settings.map_floatPars.find("ConeMatchingCut_pT")==settings.map_floatPars.end())
+    settings.map_floatPars["ConeMatchingCut_pT"] = 2.0; // GeV. If pT of a track < ConeMatchingCut_pT, use Cone matching
+  if(settings.map_intPars.find("Max_Seed_Point")==settings.map_intPars.end())
+    settings.map_intPars["Max_Seed_Point"] = 4;
+  if(settings.map_floatPars.find("ConeSeedDistance")==settings.map_floatPars.end())
+    settings.map_floatPars["ConeSeedDistance"] = 20;
+  if(settings.map_floatPars.find("th_ConeTheta")==settings.map_floatPars.end())    
+    settings.map_floatPars["th_ConeTheta"] = TMath::Pi()/4.;
+  if(settings.map_floatPars.find("th_ConeR")==settings.map_floatPars.end())        
+    settings.map_floatPars["th_ConeR"] = 50;
+  if(settings.map_stringPars.find("ReadinLocalMaxName")==settings.map_stringPars.end())
+    settings.map_stringPars["ReadinLocalMaxName"] = "AllLocalMax";
+  if(settings.map_stringPars.find("OutputLongiClusName")==settings.map_stringPars.end()) 
+    settings.map_stringPars["OutputLongiClusName"] = "TrackAxis"; 
+
+  return StatusCode::SUCCESS;
+}
+
+
+StatusCode TrackMatchingAlg::Initialize( PandoraPlusDataCol& m_datacol ){
+  m_TrackCol.clear();
+  p_HalfClusterV = nullptr;
+  p_HalfClusterU = nullptr;
+  // m_trackAxisVCol.clear();
+  // m_trackAxisUCol.clear();
+
+  for(int itrk=0; itrk<m_datacol.TrackCol.size(); itrk++ ) m_TrackCol.push_back(m_datacol.TrackCol[itrk].get());
+  p_HalfClusterU = &(m_datacol.map_HalfCluster["HalfClusterColU"]);
+  p_HalfClusterV = &(m_datacol.map_HalfCluster["HalfClusterColV"]);
+
+  return StatusCode::SUCCESS;
+}
+
+
+StatusCode TrackMatchingAlg::RunAlgorithm( PandoraPlusDataCol& m_datacol ){
+  std::cout << "---oooOO0OOooo---Excuting TrackMatchingAlg---oooOO0OOooo---"<<std::endl;
+  // Associate tracks to HalfClusters.
+  // This association is a many-to-many relationship: 
+  //    One HalCluster may have multiple tracks; 
+  //    One track may pass through multiple HalfClusters.
+//cout<<"track size: "<<m_TrackCol.size()<<", HFClusterU size "<<p_HalfClusterU->size()<<", HFClusterV size "<<p_HalfClusterV->size()<<endl;
+
+  for(int itrk=0; itrk<m_TrackCol.size(); itrk++){  // loop tracks
+//printf("  In track %d: Ptrack %.3f, track state size %d \n", itrk, m_TrackCol[itrk]->getMomentum(), m_TrackCol[itrk]->getTrackStates("Ecal").size());
+    if(m_TrackCol[itrk]->getTrackStates("Ecal").size()==0) continue;
+
+    // Get extrapolated points of the track. These points are sorted by the track
+    std::vector<TVector3> extrapo_points;
+    GetExtrpoECALPoints(m_TrackCol[itrk], extrapo_points);
+//printf("  In track %d: extrapolated point size %d \n", itrk, extrapo_points.size());
+    if(extrapo_points.size()==0) continue;
+
+    double pT = TMath::Abs(1. / m_TrackCol[itrk]->getTrackStates("Ecal")[0].Kappa);
+    if (pT >= settings.map_floatPars["ConeMatchingCut_pT"]){
+//std::cout << "For track " << itrk << ", pT = " << pT <<", match directly" << std::endl;
+
+      for(int ihc=0; ihc<p_HalfClusterV->size(); ihc++){  // loop HalfClusterV
+        // Get local max of the HalfCluster
+        std::vector<const PandoraPlus::Calo1DCluster*> localMaxColV = p_HalfClusterV->at(ihc).get()->getLocalMaxCol(settings.map_stringPars["ReadinLocalMaxName"]);
+//cout<<"    In HalfClusterV #"<<ihc<<": localMax size "<<localMaxColV.size()<<endl;
+        // Track axis candidate.
+        std::shared_ptr<PandoraPlus::CaloHalfCluster> t_track_axis = std::make_shared<PandoraPlus::CaloHalfCluster>();
+        CreateTrackAxis(extrapo_points, localMaxColV, t_track_axis.get());
+
+        // If the track does not match the Halfcluster, the track axis candidate will have no 1DCluster
+        if(t_track_axis->getCluster().size()==0)
+          continue;
+//cout<<"      Created a track axis"<<endl;
+        t_track_axis->addAssociatedTrack(m_TrackCol[itrk]);
+        t_track_axis->setType(10000); //Track-type axis. 
+        m_TrackCol[itrk]->addAssociatedHalfClusterV( p_HalfClusterV->at(ihc).get() );
+        m_datacol.map_HalfCluster["bkHalfCluster"].push_back(t_track_axis);
+        p_HalfClusterV->at(ihc).get()->addHalfCluster(settings.map_stringPars["OutputLongiClusName"], t_track_axis.get());
+      }  // end loop HalfClusterV
+
+      for(int ihc=0; ihc<p_HalfClusterU->size(); ihc++){  // loop HalfClusterU
+        // Get local max of the HalfCluster
+        std::vector<const PandoraPlus::Calo1DCluster*> localMaxColU = p_HalfClusterU->at(ihc).get()->getLocalMaxCol(settings.map_stringPars["ReadinLocalMaxName"]);
+//cout<<"    In HalfClusterU #"<<ihc<<": localMax size "<<localMaxColU.size()<<endl;
+
+        // Track axis candidate.
+        std::shared_ptr<PandoraPlus::CaloHalfCluster> t_track_axis = std::make_shared<PandoraPlus::CaloHalfCluster>();
+        CreateTrackAxis(extrapo_points, localMaxColU, t_track_axis.get());
+
+        // If the track do not match the Halfcluster, the track axis candidate will have no 1DCluster
+        if(t_track_axis->getCluster().size()==0)
+          continue;
+        
+//cout<<"      Created a track axis"<<endl;
+        t_track_axis->addAssociatedTrack(m_TrackCol[itrk]);
+        t_track_axis->setType(10000); //Track-type axis. 
+        m_TrackCol[itrk]->addAssociatedHalfClusterU( p_HalfClusterU->at(ihc).get() );
+        m_datacol.map_HalfCluster["bkHalfCluster"].push_back(t_track_axis);
+        p_HalfClusterU->at(ihc).get()->addHalfCluster(settings.map_stringPars["OutputLongiClusName"], t_track_axis.get());
+      }  // end loop HalfClusterU
+    }
+    else{  // pT < settings.map_floatPars["ConeMatchingCut_pT"]
+//std::cout << "For track " << itrk << ", pT = " << pT <<", using Cone method" << std::endl;
+
+      // Get local max and HalfCluster near the extrapolated points
+      std::vector<PandoraPlus::CaloHalfCluster*> t_nearbyHalfClustersV;  t_nearbyHalfClustersV.clear();
+      std::vector<PandoraPlus::CaloHalfCluster*> t_nearbyHalfClustersU;  t_nearbyHalfClustersU.clear();
+      std::vector<const PandoraPlus::Calo1DCluster*> t_nearbyLocalMaxV;     t_nearbyLocalMaxV.clear();
+      std::vector<const PandoraPlus::Calo1DCluster*> t_nearbyLocalMaxU;     t_nearbyLocalMaxU.clear();
+      GetNearby(p_HalfClusterV, extrapo_points, t_nearbyHalfClustersV, t_nearbyLocalMaxV);
+      GetNearby(p_HalfClusterU, extrapo_points, t_nearbyHalfClustersU, t_nearbyLocalMaxU);
+
+      // V plane
+      std::vector<const PandoraPlus::Calo1DCluster*> t_cone_axisV; t_cone_axisV.clear();
+      LongiConeLinking(extrapo_points, t_nearbyLocalMaxV, t_cone_axisV);
+      CreatConeAxis(m_datacol, m_TrackCol[itrk], t_nearbyHalfClustersV, t_cone_axisV);
+
+      // U plane
+      // Sort local max by their modules
+      //std::map<int, std::vector<const PandoraPlus::Calo1DCluster*> > m_orderedLocalMaxU;  // key: module of the bar
+      //m_orderedLocalMaxU.clear();
+      //for(int is=0; is<t_nearbyLocalMaxU.size(); is++)
+      //  m_orderedLocalMaxU[t_nearbyLocalMaxU[is]->getTowerID()[0][0]].push_back(t_nearbyLocalMaxU[is]);
+      //// linking 
+      //std::vector<const PandoraPlus::Calo1DCluster*> merged_cone_axisU; merged_cone_axisU.clear();
+      //for (auto it = m_orderedLocalMaxU.begin(); it != m_orderedLocalMaxU.end(); ++it){
+      //  std::vector<const PandoraPlus::Calo1DCluster*> moduled_localMaxU = it->second;
+      //  std::vector<const PandoraPlus::Calo1DCluster*> t_cone_axisU; t_cone_axisU.clear();
+      //  LongiConeLinking(extrapo_points, moduled_localMaxU, t_cone_axisU);
+      //  merged_cone_axisU.insert(merged_cone_axisU.end(), t_cone_axisU.begin(), t_cone_axisU.end());
+      //}
+      //CreatConeAxis(m_datacol, m_TrackCol[itrk], t_nearbyHalfClustersU, merged_cone_axisU);
+
+      // U plane
+      std::vector<const PandoraPlus::Calo1DCluster*> t_cone_axisU; t_cone_axisU.clear();
+      LongiConeLinking(extrapo_points, t_nearbyLocalMaxU, t_cone_axisU);
+      CreatConeAxis(m_datacol, m_TrackCol[itrk], t_nearbyHalfClustersU, t_cone_axisU);
+
+    }
+
+  
+
+  }  // end loop tracks
+
+  //Loop track to check the associated cluster: merge clusters if they are associated to the same track.
+  std::vector<PandoraPlus::CaloHalfCluster*> tmp_deleteClus; tmp_deleteClus.clear();
+  for(auto &itrk : m_TrackCol){
+    std::vector<PandoraPlus::CaloHalfCluster*> m_matchedUCol = itrk->getAssociatedHalfClustersU();
+    std::vector<PandoraPlus::CaloHalfCluster*> m_matchedVCol = itrk->getAssociatedHalfClustersV();
+
+    // std::cout << "yyy: Before merge, m_matchedVCol.size() = " << m_matchedVCol.size() << std::endl;
+    for(int imc=0; imc<m_matchedVCol.size(); imc++){
+      // std::cout<<"  yyy: m_matchedVCol["<<imc<<"]->getCluster().size()="<<m_matchedVCol[imc]->getCluster().size()<<std::endl;
+      // std::cout<<"  yyy: m_matchedVCol["<<imc<<"]->getAssociatedTracks().size()=" << m_matchedVCol[imc]->getAssociatedTracks().size() << std::endl;
+      int N_trk_axis = m_matchedVCol[imc]->getHalfClusterMap()[settings.map_stringPars["OutputLongiClusName"]].size() ;
+      // std::cout<<"  yyy: m_matchedVCol["<<imc<<"]->getHalfClusterMap()[TrackAxis].size()="<< N_trk_axis << std::endl;
+      for(int itk=0; itk<N_trk_axis; itk++){
+        // std::cout<<"    yyy: for TrackAxis " << itk 
+        // << ", Nlm = " << m_matchedVCol[imc]->getHalfClusterMap()["TrackAxis"][itk]->getCluster().size()
+        // << ", N trk = " << m_matchedVCol[imc]->getHalfClusterMap()["TrackAxis"][itk]->getAssociatedTracks().size() << std::endl;
+      }
+    }
+
+    if( m_matchedUCol.size()>1 ){
+      for(int i=1; i<m_matchedUCol.size(); i++){ 
+        m_matchedUCol[0]->mergeHalfCluster( m_matchedUCol[i] );
+        tmp_deleteClus.push_back(m_matchedUCol[i]);
+      }
+    }
+    if( m_matchedVCol.size()>1 ){
+      for(int i=1; i<m_matchedVCol.size(); i++){
+        m_matchedVCol[0]->mergeHalfCluster( m_matchedVCol[i] );
+        tmp_deleteClus.push_back(m_matchedVCol[i]);
+      }
+    }
+
+    /////////////////////////////////////////////
+    // if(m_matchedVCol.size()>0){
+    //   // std::cout<<"yyy: After merge, m_matchedVCol[0]->getCluster().size()=" << m_matchedVCol[0]->getCluster().size() << std::endl;
+    //   // std::cout<<"yyy: After merge, m_matchedVCol[0]->getAssociatedTracks().size()=" << m_matchedVCol[0]->getAssociatedTracks().size() << std::endl;
+    //   int N_trk_axis = m_matchedVCol[0]->getHalfClusterMap()["TrackAxis"].size() ;
+    //   // std::cout<<"yyy: After merge, m_matchedVCol[0]->getHalfClusterMap()[TrackAxis].size()="<< N_trk_axis << std::endl;
+    //   for(int nn=0; nn<N_trk_axis; nn++){
+    //     std::cout<<"yyy: for TrackAxis " << nn 
+    //     << ", Nlm = " << m_matchedVCol[0]->getHalfClusterMap()["TrackAxis"][nn]->getCluster().size()
+    //     << ", N trk = " << m_matchedVCol[0]->getHalfClusterMap()["TrackAxis"][nn]->getAssociatedTracks().size() << std::endl;
+
+    //   }
+    // }
+    /////////////////////////////////////////////
+  }
+
+  //Check vector: clean the merged clusters
+  for(int ihc=0; ihc<p_HalfClusterU->size(); ihc++){
+    if( find(tmp_deleteClus.begin(), tmp_deleteClus.end(), p_HalfClusterU->at(ihc).get())!=tmp_deleteClus.end() ){
+      p_HalfClusterU->erase(p_HalfClusterU->begin()+ihc);
+      ihc--;
+    }
+  }
+
+  for(int ihc=0; ihc<p_HalfClusterV->size(); ihc++){
+    if( find(tmp_deleteClus.begin(), tmp_deleteClus.end(), p_HalfClusterV->at(ihc).get())!=tmp_deleteClus.end() ){
+      p_HalfClusterV->erase(p_HalfClusterV->begin()+ihc);
+      ihc--;
+    }
+  }
+
+
+
+  return StatusCode::SUCCESS;
+}
+
+
+StatusCode TrackMatchingAlg::ClearAlgorithm(){
+  m_TrackCol.clear();
+  p_HalfClusterV = nullptr;
+  p_HalfClusterU = nullptr;
+  // m_trackAxisVCol.clear();
+  // m_trackAxisUCol.clear();
+
+  return StatusCode::SUCCESS;
+}
+
+
+StatusCode TrackMatchingAlg::GetExtrpoECALPoints(const PandoraPlus::Track* track, std::vector<TVector3>& extrapo_points){
+  std::vector<PandoraPlus::TrackState> ecal_track_states = track->getTrackStates("Ecal");
+  for(int its=0; its<ecal_track_states.size(); its++){
+    extrapo_points.push_back(ecal_track_states[its].referencePoint);
+  }
+
+  return StatusCode::SUCCESS;
+}
+
+
+StatusCode TrackMatchingAlg::CreateTrackAxis(vector<TVector3>& extrapo_points, std::vector<const PandoraPlus::Calo1DCluster*>& localMaxCol,
+                             PandoraPlus::CaloHalfCluster* t_track_axis){                           
+  if(localMaxCol.size()==0 || extrapo_points.size()==0)
+    return StatusCode::SUCCESS;
+  int t_slayer = localMaxCol[0]->getSlayer();
+  
+  if(t_slayer==1){  // V plane (xy plane)
+    for(int ipt=0; ipt<extrapo_points.size(); ipt++){
+    for(int ilm=0; ilm<localMaxCol.size(); ilm++){
+      // distance from the extrpolated point to the center of the local max bar
+      TVector3 distance = extrapo_points[ipt] - localMaxCol[ilm]->getPos();
+      if( TMath::Abs(distance.Z()) < (localMaxCol[ilm]->getBars()[0]->getBarLength())/2. &&
+          distance.Perp() < PandoraPlus::CaloUnit::barsize ) { 
+        t_track_axis->addUnit(localMaxCol[ilm]);
+      }
+      else { continue; }
+    }}
+  }
+  else{  // U plane (r-phi plane)
+    for(int ipt=0; ipt<extrapo_points.size(); ipt++){
+    for(int ilm=0; ilm<localMaxCol.size(); ilm++){
+      TVector3 lm_pos = localMaxCol[ilm]->getPos();
+      float barLength = localMaxCol[ilm]->getBars()[0]->getBarLength();
+      if( fabs(extrapo_points[ipt].z()-lm_pos.z()) < PandoraPlus::CaloUnit::barsize &&  
+          fabs(extrapo_points[ipt].Phi()-lm_pos.Phi()) < barLength/2./PandoraPlus::CaloUnit::ecal_innerR &&  
+          fabs(extrapo_points[ipt].Perp()-lm_pos.Perp()) < PandoraPlus::CaloUnit::barsize ){
+        t_track_axis->addUnit(localMaxCol[ilm]);
+      }
+      else { continue; }
+    }}
+  }
+
+//std::cout << "end calling CreateTrackAxis()" << std::endl;
+  return StatusCode::SUCCESS;
+}
+
+
+StatusCode TrackMatchingAlg::GetNearby(const std::vector<std::shared_ptr<PandoraPlus::CaloHalfCluster>>* p_HalfCluster, 
+                                       const std::vector<TVector3>& extrapo_points, 
+                                       std::vector<PandoraPlus::CaloHalfCluster*>& t_nearbyHalfClusters, 
+                                       std::vector<const PandoraPlus::Calo1DCluster*>& t_nearbyLocalMax){
+  // std::cout << "calling TrackMatchingAlg::GetNearby()" << std::endl;
+
+  if(p_HalfCluster->size()==0 || extrapo_points.size()==0)  return StatusCode::SUCCESS; 
+
+  std::set<PandoraPlus::CaloHalfCluster*> set_nearbyHalfClusters;
+  int slayer = p_HalfCluster->at(0).get()->getSlayer();
+  if(slayer==1){  // V plane
+    for(int ihc=0; ihc<p_HalfCluster->size(); ihc++){
+      std::vector<const PandoraPlus::Calo1DCluster*> localMaxCol = p_HalfCluster->at(ihc).get()->getLocalMaxCol(settings.map_stringPars["ReadinLocalMaxName"]);
+      for(int ilm=0; ilm<localMaxCol.size(); ilm++){
+      for(int ipt=0; ipt<extrapo_points.size(); ipt++){
+        TVector3 distance(extrapo_points[ipt] - localMaxCol[ilm]->getPos());
+        if(TMath::Abs(distance.Z()) < (localMaxCol[ilm]->getBars()[0]->getBarLength())/2. && 
+            distance.Perp() < settings.map_floatPars["ConeNearByDistance"] ){  
+          t_nearbyLocalMax.push_back(localMaxCol[ilm]);
+          set_nearbyHalfClusters.insert(p_HalfCluster->at(ihc).get());
+          break;
+        }
+      }}
+      
+    }
+  }else{  // U plane
+    for(int ihc=0; ihc<p_HalfCluster->size(); ihc++){
+      std::vector<const PandoraPlus::Calo1DCluster*> localMaxCol = p_HalfCluster->at(ihc).get()->getLocalMaxCol(settings.map_stringPars["ReadinLocalMaxName"]);
+      for(int ilm=0; ilm<localMaxCol.size(); ilm++){
+      for(int ipt=0; ipt<extrapo_points.size(); ipt++){
+        TVector3 lm_pos = localMaxCol[ilm]->getPos();
+        float barLength = localMaxCol[ilm]->getBars()[0]->getBarLength();
+        if( fabs(extrapo_points[ipt].z()-lm_pos.z()) < settings.map_floatPars["ConeNearByDistance"] &&
+            fabs(extrapo_points[ipt].Phi()-lm_pos.Phi()) < barLength/2./PandoraPlus::CaloUnit::ecal_innerR &&
+            fabs(extrapo_points[ipt].Perp()-lm_pos.Perp()) < settings.map_floatPars["ConeNearByDistance"] ){
+          t_nearbyLocalMax.push_back(localMaxCol[ilm]);
+          set_nearbyHalfClusters.insert(p_HalfCluster->at(ihc).get());
+          break;
+        }
+      }}
+    }
+  }
+  t_nearbyHalfClusters.assign(set_nearbyHalfClusters.begin(), set_nearbyHalfClusters.end());
+
+  return StatusCode::SUCCESS; 
+}
+
+
+StatusCode TrackMatchingAlg::LongiConeLinking(const std::vector<TVector3>& extrapo_points,  
+                                              std::vector<const PandoraPlus::Calo1DCluster*>& nearbyLocalMax, 
+                                              std::vector<const PandoraPlus::Calo1DCluster*>& cone_axis){
+  // std::cout<<"yyy: calling longiConeLinking()"<<std::endl;
+  if(nearbyLocalMax.size()==0 || extrapo_points.size()==0) return StatusCode::SUCCESS;
+
+  // Seed finding
+  int slayer = nearbyLocalMax[0]->getSlayer();
+  
+  // int min_point = settings.map_intPars["Max_Seed_Point"];
+  // if (extrapo_points.size()<min_point) min_point = extrapo_points.size();
+  
+  if(slayer==1){  // If V plane
+    // for(int ip=0; ip<min_point; ip++){
+    for(int ip=0; ip<extrapo_points.size(); ip++){
+      double min_distance = 99999;
+      int seed_candidate_index = -1;
+
+      for(int il=0;il<nearbyLocalMax.size(); il++){
+        TVector3 distance = extrapo_points[ip] - nearbyLocalMax[il]->getPos();
+        double distance_2d = distance.Perp();
+        if(TMath::Abs(distance.Z()) < (nearbyLocalMax[il]->getBars()[0]->getBarLength())/2.
+           && distance_2d < settings.map_floatPars["ConeSeedDistance"]
+           && distance_2d < min_distance)
+        {
+          seed_candidate_index = il;
+          min_distance = distance_2d;
+        }
+      }
+      if (seed_candidate_index<0) continue;
+
+      cone_axis.push_back(nearbyLocalMax[seed_candidate_index]);
+      nearbyLocalMax.erase(nearbyLocalMax.begin() + seed_candidate_index);
+      break;
+    }
+  }
+  else{  // If U plane
+    // for(int ip=0; ip<min_point; ip++){
+    for(int ip=0; ip<extrapo_points.size(); ip++){
+      double min_distance = 99999;
+      int seed_candidate_index = -1;
+
+      for(int il=0;il<nearbyLocalMax.size(); il++){
+        TVector3 lm_pos = nearbyLocalMax[il]->getPos();
+        float barLength = nearbyLocalMax[il]->getBars()[0]->getBarLength();
+        float distance_2d = sqrt( pow(extrapo_points[ip].z()-lm_pos.z(), 2) + pow(extrapo_points[ip].Perp()-lm_pos.Perp(), 2) );
+        if( fabs(extrapo_points[ip].Phi()-lm_pos.Phi()) < barLength/2./PandoraPlus::CaloUnit::ecal_innerR && 
+            distance_2d < settings.map_floatPars["ConeSeedDistance"] &&
+            distance_2d < min_distance)
+        {
+          seed_candidate_index = il;
+          min_distance = distance_2d;
+        }
+      }
+      if (seed_candidate_index<0) continue;
+
+      cone_axis.push_back(nearbyLocalMax[seed_candidate_index]);
+      nearbyLocalMax.erase(nearbyLocalMax.begin() + seed_candidate_index);
+      break;
+    }
+  }
+
+  if (cone_axis.size() == 0) return StatusCode::SUCCESS;
+
+  // std::cout<<"  yyy: after seed-finding, cone_axis.size()="<<cone_axis.size()<<endl;
+
+  // Linking
+  while(nearbyLocalMax.size()>0){
+    // std::cout<<"  yyy: nearbyLocalMax.size()="<<nearbyLocalMax.size()<<", linking it!"<<std::endl;
+    const PandoraPlus::Calo1DCluster* shower_in_axis = cone_axis.back();
+    if(!shower_in_axis) break; 
+    if(isStopLinking(extrapo_points, shower_in_axis)) break;
+
+    // std::cout<<"  yyy: looking for a lm to link"<<std::endl;
+
+    double min_delta = 9999;
+    int shower_candidate_index = -1;
+
+    for(int il=0; il<nearbyLocalMax.size(); il++){
+      TVector2 relR = GetProjectedRelR(shower_in_axis, nearbyLocalMax[il]);  //Return vec: 1->2.
+      TVector2 clusaxis = GetProjectedAxis(extrapo_points, shower_in_axis);
+
+      double delta_phi = relR.DeltaPhi(clusaxis);
+      double delta_distance = (relR - (clusaxis*2)).Mod();
+
+      // std::cout<<"    yyy: for nearbyLocalMax["<<il<<"], "<<std::endl
+      //          <<"         shower_in_axis=("<<shower_in_axis->getPos().x()<<", "
+      //          <<shower_in_axis->getPos().y()<<", "
+      //          <<shower_in_axis->getPos().z()<<")"<<std::endl
+      //          <<"         nearbyLocalMax=("<<nearbyLocalMax[il]->getPos().x()<<", "
+      //          <<nearbyLocalMax[il]->getPos().y()<<", "
+      //          <<nearbyLocalMax[il]->getPos().z()<<")"<<std::endl
+      //          <<"         relR = ("<<relR.X()<<", "<<relR.Y()<<")"<<std::endl
+      //          <<"         clusaxis = ("<<clusaxis.X()<<", "<<clusaxis.Y()<<")"<<std::endl
+      //          <<"         delta_phi = "<<delta_phi<<", delta_distance="<<delta_distance<<std::endl;
+      
+      if( delta_phi<settings.map_floatPars["th_ConeTheta"] 
+          && relR.Mod()<settings.map_floatPars["th_ConeR"] 
+          && delta_distance<min_delta)
+      {
+        // std::cout<<"    yyy: nearbyLocalMax["<<il<<"] renewed"<<std::endl;
+        shower_candidate_index = il;
+        min_delta = delta_distance;
+      }
+    }
+    if (shower_candidate_index<0) break;
+
+    cone_axis.push_back(nearbyLocalMax[shower_candidate_index]);
+    nearbyLocalMax.erase(nearbyLocalMax.begin() + shower_candidate_index);
+  }
+  
+  return StatusCode::SUCCESS;
+}
+
+
+bool TrackMatchingAlg::isStopLinking( const std::vector<TVector3>& extrapo_points, 
+                                      const PandoraPlus::Calo1DCluster* final_cone_hit){
+
+  double slayer = final_cone_hit->getSlayer();
+  if(slayer==1){
+    TVector3 f_distance = extrapo_points.back() - final_cone_hit->getPos();
+    double f_distance_2d = f_distance.Perp(); 
+    for(int i=0; i<extrapo_points.size(); i++){
+      TVector3 distance = extrapo_points[i] - final_cone_hit->getPos();
+      double distance_2d = distance.Perp(); 
+      if (distance_2d < f_distance_2d) return false;
+    }
+  }
+  else{
+    
+
+    TVector3 lm_pos = final_cone_hit->getPos();
+    float barLength = final_cone_hit->getBars()[0]->getBarLength();
+    float f_distance_2d = sqrt( pow(extrapo_points.back().z()-lm_pos.z(), 2) + pow(extrapo_points.back().Perp()-lm_pos.Perp(), 2) );
+    for(int i=0; i<extrapo_points.size(); i++){
+      float distance_2d = sqrt( pow(extrapo_points[i].z()-lm_pos.z(), 2) + pow(extrapo_points[i].Perp()-lm_pos.Perp(), 2) );
+      if (distance_2d < f_distance_2d) return false;
+    }
+  }
+  // std::cout<<"  yyy: calling isStopLinking(): stop!"<<std::endl;
+  return true;   
+}
+
+
+TVector2 TrackMatchingAlg::GetProjectedRelR( const PandoraPlus::Calo1DCluster* m_shower1, const PandoraPlus::Calo1DCluster* m_shower2 ){
+  TVector2 paxis1, paxis2;
+  if(m_shower1->getSlayer()==1){ //For V-bars
+    paxis1.Set(m_shower1->getPos().x(), m_shower1->getPos().y());
+    paxis2.Set(m_shower2->getPos().x(), m_shower2->getPos().y());
+    return paxis2 - paxis1;
+  }
+  else{  //For U-bars
+    //if (m_shower1->getTowerID()[0][0] != m_shower2->getTowerID()[0][0])
+    //  std::cout << "warning: In GetProjectedRelR(), modules are different!" << std::endl;
+    TVector3 vec = m_shower2->getPos() - m_shower1->getPos();
+
+    TVector2 vec2d(vec.Perp(), vec.z());
+
+    return vec2d;
+  }
+
+  
+}
+
+
+TVector2 TrackMatchingAlg::GetProjectedAxis(const std::vector<TVector3>& extrapo_points, const PandoraPlus::Calo1DCluster* m_shower){
+  int min_index=0;
+  TVector2 distance(999., 999.);
+  if( m_shower->getSlayer()==1 ){  // V plane
+    for(int i=0; i<extrapo_points.size(); i++){
+      TVector2 t_distance(m_shower->getPos().x()-extrapo_points[i].x(), m_shower->getPos().y()-extrapo_points[i].y());
+      if(t_distance.Mod()<distance.Mod()){
+        distance = t_distance;
+        min_index = i;
+      }
+    }
+
+    if(min_index < extrapo_points.size()-1){
+      TVector2 axis(extrapo_points[min_index+1].x()-extrapo_points[min_index].x(), extrapo_points[min_index+1].y()-extrapo_points[min_index].y());
+      return axis;
+    }else{
+      TVector2 axis(extrapo_points[min_index].x()-extrapo_points[min_index-1].x(), extrapo_points[min_index].y()-extrapo_points[min_index-1].y());
+      return axis;
+    }
+
+  }else{  // U plane
+    for(int i=0; i<extrapo_points.size(); i++){
+      TVector3 dist3d = m_shower->getPos() - extrapo_points[i];
+      //dist3d.RotateZ( TMath::Pi()/4.*(6-m_shower->getTowerID()[0][0]) );
+      TVector2 t_distance(dist3d.Perp(), dist3d.z());
+      if(t_distance.Mod()<distance.Mod()){
+        distance = t_distance;
+        min_index = i;
+      }
+    }
+
+    if(min_index < extrapo_points.size()-1){
+      double dx = extrapo_points[min_index+1].x() - extrapo_points[min_index].x();
+      double dy = extrapo_points[min_index+1].y() - extrapo_points[min_index].y();
+      double dz = extrapo_points[min_index+1].z() - extrapo_points[min_index].z();
+      TVector3 vec(dx, dy, dz);
+      //vec.RotateZ( TMath::Pi()/4.*(6-m_shower->getTowerID()[0][0]) );
+      TVector2 axis(vec.Perp(), vec.z());
+      return axis;
+    }else{
+      double dx = extrapo_points[min_index].x() - extrapo_points[min_index-1].x();
+      double dy = extrapo_points[min_index].y() - extrapo_points[min_index-1].y();
+      double dz = extrapo_points[min_index].z() - extrapo_points[min_index-1].z();
+      TVector3 vec(dx, dy, dz);
+      //vec.RotateZ( TMath::Pi()/4.*(6-m_shower->getTowerID()[0][0]) );
+      TVector2 axis(vec.Perp(), vec.z());
+      return axis;
+    }
+  }
+
+  
+}
+
+
+StatusCode TrackMatchingAlg::CreatConeAxis(PandoraPlusDataCol& m_datacol, PandoraPlus::Track* track, std::vector<PandoraPlus::CaloHalfCluster*>& nearbyHalfClusters, 
+                                           std::vector<const PandoraPlus::Calo1DCluster*>& cone_axis){
+  // std::cout<<"yyy: Calling CreateConeAxis()"<<std::endl;
+  if(nearbyHalfClusters.size()==0 || cone_axis.size()==0) return StatusCode::SUCCESS; 
+
+  for(int ihc=0; ihc<nearbyHalfClusters.size(); ihc++){
+    std::vector<const PandoraPlus::Calo1DCluster*> localMaxCol = nearbyHalfClusters[ihc]->getLocalMaxCol(settings.map_stringPars["ReadinLocalMaxName"]);
+    // std::cout<<"    yyy: for nearbyHalfClusters["<<ihc<<"], localMax are:"
+    // Track axis candidate.
+    // PandoraPlus::CaloHalfCluster* t_track_axis = new PandoraPlus::CaloHalfCluster();
+    std::shared_ptr<PandoraPlus::CaloHalfCluster> t_track_axis = std::make_shared<PandoraPlus::CaloHalfCluster>();
+    for(int ica=0; ica<cone_axis.size(); ica++){
+      if( find(localMaxCol.begin(), localMaxCol.end(), cone_axis[ica]) != localMaxCol.end()){
+        t_track_axis->addUnit(cone_axis[ica]);
+        // std::cout<<"    add Unit from cone_axis to track_axis: " << cone_axis[ica]->getPos().x() << ", "
+        //          << cone_axis[ica]->getPos().y() << ", " << cone_axis[ica]->getPos().z() << std::endl;
+      }
+    }
+
+    // // If the track does not match the Halfcluster, the track axis candidate will have no 1DCluster
+    if(t_track_axis->getCluster().size()==0)
+      continue;
+    
+    t_track_axis->addAssociatedTrack(track);
+    t_track_axis->setType(10000); //Track-type axis. 
+    
+    if(nearbyHalfClusters[ihc]->getSlayer()==1){
+      track->addAssociatedHalfClusterV( nearbyHalfClusters[ihc] );
+      // std::cout<<"    yyy: track->addAssociatedHalfClusterV"<<std::endl;
+    }
+    else{
+      track->addAssociatedHalfClusterU( nearbyHalfClusters[ihc] );
+      // std::cout<<"    yyy: track->addAssociatedHalfClusterU"<<std::endl;
+    }
+    m_datacol.map_HalfCluster["bkHalfCluster"].push_back(t_track_axis);
+    nearbyHalfClusters[ihc]->addHalfCluster(settings.map_stringPars["OutputLongiClusName"], t_track_axis.get());
+    // std::cout<<"    yyy: nearbyHalfClusters["<<ihc<<"]->addHalfCluster, bars in the nearbyHalfClusters:"<<std::endl;
+    // for(int ii=0; ii<nearbyHalfClusters[ihc]->getCluster().size(); ii++){
+    //   std::cout<<"         "<<nearbyHalfClusters[ihc]->getCluster()[ii]->getPos().x()<<", "
+    //                         <<nearbyHalfClusters[ihc]->getCluster()[ii]->getPos().y()<<", "
+    //                         <<nearbyHalfClusters[ihc]->getCluster()[ii]->getPos().z()<<std::endl;
+    // }
+    // std::cout<<"          bars in the t_track_axis:"<<std::endl;
+    // for(int ii=0; ii<t_track_axis->getCluster().size(); ii++){
+    //   std::cout<<"         "<<t_track_axis->getCluster()[ii]->getPos().x()<<", "
+    //                         <<t_track_axis->getCluster()[ii]->getPos().y()<<", "
+    //                         <<t_track_axis->getCluster()[ii]->getPos().z()<<std::endl;
+    // }
+    
+    
+    
+  }
+
+  return StatusCode::SUCCESS; 
+}
+
+#endif
diff --git a/Reconstruction/CrystalCaloRec/src/Algorithm/TruthClusterMergingAlg.cpp b/Reconstruction/CrystalCaloRec/src/Algorithm/TruthClusterMergingAlg.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..47abdaa13dd3016850040fbf1fbc93ce858ade0a
--- /dev/null
+++ b/Reconstruction/CrystalCaloRec/src/Algorithm/TruthClusterMergingAlg.cpp
@@ -0,0 +1,200 @@
+#ifndef _TRUTHCLUSTERMERGING_ALG_C
+#define _TRUTHCLUSTERMERGING_ALG_C
+
+#include "Algorithm/TruthClusterMergingAlg.h"
+
+StatusCode TruthClusterMergingAlg::ReadSettings(Settings& m_settings){
+  settings = m_settings;
+
+  //Initialize parameters
+  if(settings.map_boolPars.find("DoECALMerge")==settings.map_boolPars.end()) settings.map_boolPars["DoECALMerge"] = 1;
+  if(settings.map_boolPars.find("DoHCALMerge")==settings.map_boolPars.end()) settings.map_boolPars["DoHCALMerge"] = 1;
+  if(settings.map_boolPars.find("DoECALHCALConnection")==settings.map_boolPars.end()) settings.map_boolPars["DoECALHCALConnection"] = 1;
+
+  if(settings.map_stringPars.find("ReadinECALClusters")==settings.map_stringPars.end()) settings.map_stringPars["ReadinECALClusters"] = "TruthEcalCluster";
+  if(settings.map_stringPars.find("ReadinHCALClusters")==settings.map_stringPars.end()) settings.map_stringPars["ReadinHCALClusters"] = "TruthHcalCluster";
+  if(settings.map_stringPars.find("OutputECALCluster")==settings.map_stringPars.end()) settings.map_stringPars["OutputECALCluster"] = "TruthMergedEcalCluster";
+  if(settings.map_stringPars.find("OutputHCALCluster")==settings.map_stringPars.end()) settings.map_stringPars["OutputHCALCluster"] = "TruthMergedHcalCluster";
+  if(settings.map_stringPars.find("OutputCombPFO")==settings.map_stringPars.end()) settings.map_stringPars["OutputCombPFO"] = "TruthCombPFO";
+
+  return StatusCode::SUCCESS;
+};
+
+StatusCode TruthClusterMergingAlg::Initialize( PandoraPlusDataCol& m_datacol ){
+
+  for(int icl=0; icl<m_datacol.map_CaloCluster[settings.map_stringPars["ReadinECALClusters"]].size(); icl++)
+    m_EcalClusterCol.push_back(m_datacol.map_CaloCluster[settings.map_stringPars["ReadinECALClusters"]][icl].get()); 
+  for(int icl=0; icl<m_datacol.map_CaloCluster[settings.map_stringPars["ReadinHCALClusters"]].size(); icl++)
+    m_HcalClusterCol.push_back(m_datacol.map_CaloCluster[settings.map_stringPars["ReadinHCALClusters"]][icl].get()); 
+
+
+//cout<<"Print input ECAL cluster"<<endl;
+//for(int ic=0; ic<m_EcalClusterCol.size(); ic++){
+//  printf("  Pos+E (%.3f, %.3f, %.3f, %.3f), linked leading MCP [%d, %.3f], linked track size %d \n", m_EcalClusterCol[ic]->getShowerCenter().x(), m_EcalClusterCol[ic]->getShowerCenter().y(), m_EcalClusterCol[ic]->getShowerCenter().z(), m_EcalClusterCol[ic]->getLongiE(), m_EcalClusterCol[ic]->getLeadingMCP().getPDG(), m_EcalClusterCol[ic]->getLeadingMCPweight(), m_EcalClusterCol[ic]->getAssociatedTracks().size() );
+//  if(m_EcalClusterCol[ic]->getAssociatedTracks().size()>0) printf("    Track MCP: %d \n", m_EcalClusterCol[ic]->getAssociatedTracks()[0]->getLeadingMCP().getPDG());  
+//}
+//cout<<"Print input HCAL cluster"<<endl;
+//for(int ic=0; ic<m_HcalClusterCol.size(); ic++){
+//  printf("  Pos+E (%.3f, %.3f, %.3f, %.3f), linked leading MCP [%d, %.3f], linked track size %d \n", m_HcalClusterCol[ic]->getHitCenter().x(), m_HcalClusterCol[ic]->getHitCenter().y(), m_HcalClusterCol[ic]->getHitCenter().z(), m_HcalClusterCol[ic]->getHitsE(), m_HcalClusterCol[ic]->getLeadingMCP().getPDG(), m_HcalClusterCol[ic]->getLeadingMCPweight(), m_HcalClusterCol[ic]->getAssociatedTracks().size() );
+//  if(m_HcalClusterCol[ic]->getAssociatedTracks().size()>0) printf("    Track MCP: %d \n", m_HcalClusterCol[ic]->getAssociatedTracks()[0]->getLeadingMCP().getPDG());
+//}
+
+
+  return StatusCode::SUCCESS;
+};
+
+StatusCode TruthClusterMergingAlg::RunAlgorithm( PandoraPlusDataCol& m_datacol ){
+
+//cout<<"  TruthClusterMergingAlg: input ECAL cluster size "<<m_EcalClusterCol.size()<<", HCAL cluster size "<<m_HcalClusterCol.size()<<endl;
+//cout<<"  Print ECAL cluster energy: "<<endl;
+//for(int ic=0; ic<m_EcalClusterCol.size(); ic++)
+//  cout<<"    #"<<ic<<" En = "<<m_EcalClusterCol[ic]->getLongiE()<<endl;
+//cout<<"  Print HCAL cluster energy: "<<endl;
+//for(int ic=0; ic<m_HcalClusterCol.size(); ic++)
+//  cout<<"    #"<<ic<<" En = "<<m_HcalClusterCol[ic]->getHitsE()<<endl;
+
+  std::map<edm4hep::MCParticle, std::vector<const PandoraPlus::Calo3DCluster*>> map_ClusterCol_Ecal; 
+  std::map<edm4hep::MCParticle, std::vector<const PandoraPlus::Calo3DCluster*>> map_ClusterCol_Hcal; 
+  for(int ic=0; ic<m_EcalClusterCol.size(); ic++){
+    auto mcp = m_EcalClusterCol[ic]->getLeadingMCP();
+    map_ClusterCol_Ecal[mcp].push_back( m_EcalClusterCol[ic] );
+  }
+  for(int ic=0; ic<m_HcalClusterCol.size(); ic++){
+    auto mcp = m_HcalClusterCol[ic]->getLeadingMCP();
+    map_ClusterCol_Hcal[mcp].push_back( m_HcalClusterCol[ic] );
+  }
+
+  if(settings.map_boolPars["DoECALMerge"]){
+    for(auto& iter: map_ClusterCol_Ecal){
+      if(iter.second.size()==0) continue;
+      auto tmp_newcluster = iter.second[0]->Clone();
+      for(int icl=1; icl<iter.second.size(); icl++){
+        tmp_newcluster->mergeCluster( iter.second[icl] );
+      }
+      //tmp_newcluster->getLinkedMCPfromUnit();
+      merged_EcalClusterCol.push_back(tmp_newcluster);
+      m_datacol.map_CaloCluster["bk3DCluster"].push_back(tmp_newcluster);
+    }
+  }
+
+  if(settings.map_boolPars["DoHCALMerge"]){
+    for(auto& iter: map_ClusterCol_Hcal){
+      if(iter.second.size()==0) continue;
+      auto tmp_newcluster = iter.second[0]->Clone();
+      for(int icl=1; icl<iter.second.size(); icl++){
+        tmp_newcluster->mergeCluster( iter.second[icl] );
+      }
+      //tmp_newcluster->getLinkedMCPfromUnit();
+      merged_HcalClusterCol.push_back(tmp_newcluster);
+      m_datacol.map_CaloCluster["bk3DCluster"].push_back(tmp_newcluster);
+    }
+  }
+
+
+  if(settings.map_boolPars["DoECALHCALConnection"]){
+    map_ClusterCol_Ecal.clear();
+    map_ClusterCol_Hcal.clear();
+
+    if(settings.map_boolPars["DoECALMerge"]){
+      for(int ic=0; ic<merged_EcalClusterCol.size(); ic++){
+        auto mcp = merged_EcalClusterCol[ic]->getLeadingMCP();
+        map_ClusterCol_Ecal[mcp].push_back( merged_EcalClusterCol[ic].get() );
+      }
+    }
+    else{
+      for(int ic=0; ic<m_EcalClusterCol.size(); ic++){
+        auto mcp = m_EcalClusterCol[ic]->getLeadingMCP();
+        map_ClusterCol_Ecal[mcp].push_back( m_EcalClusterCol[ic] );
+      }
+    }
+
+    if(settings.map_boolPars["DoHCALMerge"]){
+      for(int ic=0; ic<merged_HcalClusterCol.size(); ic++){
+        auto mcp = merged_HcalClusterCol[ic]->getLeadingMCP();
+        map_ClusterCol_Hcal[mcp].push_back( merged_HcalClusterCol[ic].get() );
+      }
+    }
+    else{
+      for(int ic=0; ic<m_HcalClusterCol.size(); ic++){
+        auto mcp = m_HcalClusterCol[ic]->getLeadingMCP();
+        map_ClusterCol_Hcal[mcp].push_back( m_HcalClusterCol[ic] );
+      }
+    }
+
+
+    for(auto& iter: map_ClusterCol_Ecal){
+      auto mcp = iter.first;
+      std::vector<const PandoraPlus::Calo3DCluster*> m_linkedEcalClus = iter.second;
+      std::vector<const PandoraPlus::Calo3DCluster*> m_linkedHcalClus = map_ClusterCol_Hcal[mcp];   
+
+      if(m_linkedEcalClus.size()==0 && m_linkedHcalClus.size()==0) continue;
+
+      std::shared_ptr<PandoraPlus::PFObject> tmp_newpfo = std::make_shared<PandoraPlus::PFObject>();
+      double Emax = -99;
+      int index = -1;
+      for(int ic=0; ic<m_linkedEcalClus.size(); ic++){ 
+        tmp_newpfo->addECALCluster(m_linkedEcalClus[ic]);
+        if(m_linkedEcalClus[ic]->getLongiE()>Emax){
+          Emax = m_linkedEcalClus[ic]->getLongiE();
+          index = ic;
+        }
+      }
+      const PandoraPlus::Track* p_EcalLeadingTrk = nullptr;
+      if(index>=0 && m_linkedEcalClus[index]->getAssociatedTracks().size()>0) p_EcalLeadingTrk = m_linkedEcalClus[index]->getAssociatedTracks()[0];
+
+      Emax = -99; index = -1;
+      for(int ic=0; ic<m_linkedHcalClus.size(); ic++){
+        tmp_newpfo->addHCALCluster(m_linkedHcalClus[ic]);
+        if(m_linkedHcalClus[ic]->getHitsE()>Emax){
+          Emax = m_linkedHcalClus[ic]->getHitsE();
+          index = ic;
+        }
+      }
+      const PandoraPlus::Track* p_HcalLeadingTrk = nullptr;
+      if(index>=0 && m_linkedHcalClus[index]->getAssociatedTracks().size()>0) p_HcalLeadingTrk = m_linkedHcalClus[index]->getAssociatedTracks()[0];
+
+      if(!p_EcalLeadingTrk && p_HcalLeadingTrk) tmp_newpfo->addTrack(p_HcalLeadingTrk);
+      else if(p_EcalLeadingTrk) tmp_newpfo->addTrack(p_EcalLeadingTrk);
+
+      merged_CombClusterCol.push_back(tmp_newpfo);
+      m_datacol.map_PFObjects["bkPFO"].push_back(tmp_newpfo);
+    }
+
+  }
+ /* 
+cout<<"Print PFO"<<endl;
+for(int ip=0; ip<merged_CombClusterCol.size(); ip++){
+  printf("  PFO #%d: track size %d, ECAL cluster size %d, HCAL cluster size %d \n", ip, merged_CombClusterCol[ip]->getTracks().size(), merged_CombClusterCol[ip]->getECALClusters().size(), merged_CombClusterCol[ip]->getHCALClusters().size() );
+  cout<<"    Track: "<<endl;
+  for(int itrk=0; itrk<merged_CombClusterCol[ip]->getTracks().size(); itrk++) 
+    printf("      Trk #%d: momentum %.3f, linked MCP %d \n", itrk, merged_CombClusterCol[ip]->getTracks()[itrk]->getMomentum(), merged_CombClusterCol[ip]->getTracks()[itrk]->getLeadingMCP().getPDG() );
+  cout<<"    ECAL cluster: "<<endl;
+  for(int icl=0; icl<merged_CombClusterCol[ip]->getECALClusters().size(); icl++)
+    printf("      ECAL cluster #%d: energy %.3f, linked MCP %d \n", icl, merged_CombClusterCol[ip]->getECALClusters()[icl]->getLongiE(), merged_CombClusterCol[ip]->getECALClusters()[icl]->getLeadingMCP().getPDG() );
+  cout<<"    HCAL cluster: "<<endl;
+  for(int icl=0; icl<merged_CombClusterCol[ip]->getHCALClusters().size(); icl++)
+    printf("      HCAL cluster #%d: energy %.3f, linked MCP %d \n", icl, merged_CombClusterCol[ip]->getHCALClusters()[icl]->getHitsE(), merged_CombClusterCol[ip]->getHCALClusters()[icl]->getLeadingMCP().getPDG() );
+  cout<<endl;
+}
+*/
+
+  m_datacol.map_CaloCluster[settings.map_stringPars["OutputECALCluster"]] = merged_EcalClusterCol;
+  m_datacol.map_CaloCluster[settings.map_stringPars["OutputHCALCluster"]] = merged_HcalClusterCol;
+  m_datacol.map_PFObjects[settings.map_stringPars["OutputCombPFO"]] = merged_CombClusterCol;
+
+  return StatusCode::SUCCESS;
+};
+
+StatusCode TruthClusterMergingAlg::ClearAlgorithm(){
+
+  m_EcalClusterCol.clear();
+  m_HcalClusterCol.clear();
+  merged_EcalClusterCol.clear();
+  merged_HcalClusterCol.clear();
+  merged_CombClusterCol.clear();
+
+  return StatusCode::SUCCESS;
+};
+
+
+#endif
diff --git a/Reconstruction/CrystalCaloRec/src/Algorithm/TruthClusteringAlg.cpp b/Reconstruction/CrystalCaloRec/src/Algorithm/TruthClusteringAlg.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..cff2ea5eabe8fc42deda1206c634cd81d4eeefb0
--- /dev/null
+++ b/Reconstruction/CrystalCaloRec/src/Algorithm/TruthClusteringAlg.cpp
@@ -0,0 +1,496 @@
+#ifndef _TRUTHCLUS_ALG_C
+#define _TRUTHCLUS_ALG_C
+
+#include "Algorithm/TruthClusteringAlg.h"
+
+StatusCode TruthClusteringAlg::ReadSettings(Settings& m_settings){
+  settings = m_settings;
+
+  if(settings.map_boolPars.find("UseSplit")==settings.map_boolPars.end()) 
+    settings.map_boolPars["UseSplit"] = 1;
+  if(settings.map_boolPars.find("DoECALClustering")==settings.map_boolPars.end()) 
+    settings.map_boolPars["DoECALClustering"] = 1;
+  if(settings.map_boolPars.find("DoHCALClustering")==settings.map_boolPars.end()) 
+    settings.map_boolPars["DoHCALClustering"] = 1;
+
+  if(settings.map_stringPars.find("InputECALBars")==settings.map_stringPars.end()) 
+    settings.map_stringPars["InputECALBars"] = "BarCol";
+  if(settings.map_stringPars.find("InputHCALHits")==settings.map_stringPars.end()) 
+    settings.map_stringPars["InputHCALHits"] = "HCALBarrel";
+  if(settings.map_stringPars.find("OutputECAL1DClusters")==settings.map_stringPars.end()) 
+    settings.map_stringPars["OutputECAL1DClusters"] = "TruthCluster1DCol";
+  if(settings.map_stringPars.find("OutputECALHalfClusters")==settings.map_stringPars.end()) 
+    settings.map_stringPars["OutputECALHalfClusters"] = "TruthESCluster";
+  if(settings.map_stringPars.find("OutputECALTower")==settings.map_stringPars.end()) 
+    settings.map_stringPars["OutputECALTower"] = "TruthESTower";
+  if(settings.map_stringPars.find("OutputHCALClusters")==settings.map_stringPars.end()) 
+    settings.map_stringPars["OutputHCALClusters"] = "TruthHcalCluster";
+
+  return StatusCode::SUCCESS;
+};
+
+StatusCode TruthClusteringAlg::Initialize( PandoraPlusDataCol& m_datacol ){
+  m_TrackCol.clear();
+  m_bars.clear();
+  m_1dclusterUCol.clear();
+  m_1dclusterVCol.clear();
+  m_halfclusterU.clear();
+  m_halfclusterV.clear();
+  m_towers.clear();
+
+  m_hits.clear();
+  m_clusters.clear();
+  m_bkCol.Clear();
+
+  m_TrackCol = m_datacol.TrackCol; 
+  m_bars = m_datacol.map_BarCol[settings.map_stringPars["InputECALBars"]];
+  m_hits = m_datacol.map_CaloHit[settings.map_stringPars["InputHCALHits"]];
+
+  return StatusCode::SUCCESS;
+};
+
+StatusCode TruthClusteringAlg::RunAlgorithm( PandoraPlusDataCol& m_datacol ){
+
+  //ECAL clustering
+  if(settings.map_boolPars["DoECALClustering"]){
+//cout<<"Input bar size: "<<m_bars.size()<<endl;
+
+    std::map<edm4hep::MCParticle, std::vector<std::shared_ptr<PandoraPlus::CaloUnit>> > map_barCol; map_barCol.clear();
+    for(int ibar=0; ibar<m_bars.size(); ibar++){
+      if(settings.map_boolPars["UseSplit"]){
+        std::vector< std::pair<edm4hep::MCParticle, float> > m_linkVec = m_bars[ibar]->getLinkedMCP();
+//printf("  Bar #%d: link size %d: ", ibar, m_linkVec.size());
+//for(int il=0; il<m_linkVec.size(); il++) printf("(%d, %.2f), ", m_linkVec[il].first.getPDG(), m_linkVec[il].second);
+//cout<<endl;
+
+        if(m_linkVec.size()==0){
+          std::cout<<"ERROR: No truth info in EcalBar #"<<ibar<<std::endl;
+          continue;
+        }
+        if(m_linkVec.size()==1){
+          edm4hep::MCParticle mcp = m_linkVec[0].first;
+          map_barCol[mcp].push_back(m_bars[ibar]);
+          continue;
+        }
+        for(int ilink=0; ilink<m_linkVec.size(); ilink++){
+          auto tmp_newbar = m_bars[ibar]->Clone();
+          tmp_newbar->setQ(tmp_newbar->getQ1()*m_linkVec[ilink].second, tmp_newbar->getQ2()*m_linkVec[ilink].second);
+          map_barCol[m_linkVec[ilink].first].push_back(tmp_newbar);
+          tmp_newbar->setLinkedMCP( std::vector<std::pair<edm4hep::MCParticle, float>>{std::make_pair(m_linkVec[ilink].first, 1.)} );
+          m_datacol.map_BarCol["bkBars"].push_back(tmp_newbar); 
+        }
+      }
+      else{
+        edm4hep::MCParticle mcp = m_bars[ibar]->getLeadingMCP();
+        map_barCol[mcp].push_back(m_bars[ibar]);
+      }
+    }
+//cout<<"truth map size "<<map_barCol.size()<<endl;
+//for(auto& iter: map_barCol){ 
+//  double totE = 0.;
+//  for(int ibar=0; ibar<iter.second.size(); ibar++)
+//    totE += iter.second[ibar]->getEnergy();
+//  printf("  Truth MCP pdgid %d, bar size %d, totE %.3f \n", iter.first.getPDG(), iter.second.size(), totE);
+//}   
+
+
+    for(auto& iter: map_barCol){
+      std::map<int, std::vector<std::shared_ptr<PandoraPlus::CaloUnit>> > m_orderedBars; m_orderedBars.clear();
+      for(int ibar=0; ibar<iter.second.size(); ibar++)
+        m_orderedBars[iter.second[ibar]->getDlayer()].push_back(iter.second[ibar]);
+//cout<<"  MCP "<<iter.first.getPDG()<<" covers "<<m_orderedBars.size()<<"layers "<<endl;
+
+      std::vector<std::shared_ptr<PandoraPlus::Calo1DCluster>> m_new1DClusUCol; m_new1DClusUCol.clear();
+      std::vector<std::shared_ptr<PandoraPlus::Calo1DCluster>> m_new1DClusVCol; m_new1DClusVCol.clear();
+   
+      for(auto& iter_bar: m_orderedBars){
+        std::shared_ptr<PandoraPlus::Calo1DCluster> tmp_new1dclusterU = std::make_shared<PandoraPlus::Calo1DCluster>();
+        std::shared_ptr<PandoraPlus::Calo1DCluster> tmp_new1dclusterV = std::make_shared<PandoraPlus::Calo1DCluster>();
+        for(int ibar=0; ibar<iter_bar.second.size(); ibar++){ 
+          if(iter_bar.second[ibar]->getSlayer()==0) tmp_new1dclusterU->addUnit( iter_bar.second[ibar].get() );
+          if(iter_bar.second[ibar]->getSlayer()==1) tmp_new1dclusterV->addUnit( iter_bar.second[ibar].get() );
+        }
+        if(tmp_new1dclusterU  && tmp_new1dclusterU->getBars().size()>0){
+          tmp_new1dclusterU->getLinkedMCPfromUnit();
+          tmp_new1dclusterU->setSeed();
+          tmp_new1dclusterU->setIDInfo();
+          m_new1DClusUCol.push_back(tmp_new1dclusterU);
+          m_datacol.map_1DCluster["bk1DCluster"].push_back(tmp_new1dclusterU);
+        }
+        if(tmp_new1dclusterV  && tmp_new1dclusterV->getBars().size()>0){
+          tmp_new1dclusterV->getLinkedMCPfromUnit();
+          tmp_new1dclusterV->setSeed();
+          tmp_new1dclusterV->setIDInfo();
+          m_new1DClusVCol.push_back(tmp_new1dclusterV);
+          m_datacol.map_1DCluster["bk1DCluster"].push_back(tmp_new1dclusterV);
+        }
+      }
+//cout<<"  1D cluster size "<<m_new1DClusUCol.size()<<", "<<m_new1DClusVCol.size()<<endl;
+//for(int icl=0; icl<m_new1DClusUCol.size(); icl++){
+//  printf("    Dlayer %d, Slayer %d, En %.3f, bar size %d, seed size %d, MCP link size %d \n", m_new1DClusUCol[icl]->getDlayer(), m_new1DClusUCol[icl]->getSlayer(), m_new1DClusUCol[icl]->getEnergy(), m_new1DClusUCol[icl]->getBars().size(), m_new1DClusUCol[icl]->getNseeds(), m_new1DClusUCol[icl]->getLinkedMCP().size());
+//}
+//for(int icl=0; icl<m_new1DClusVCol.size(); icl++){
+//  printf("    Dlayer %d, Slayer %d, En %.3f, bar size %d, seed size %d, MCP link size %d \n", m_new1DClusVCol[icl]->getDlayer(), m_new1DClusVCol[icl]->getSlayer(), m_new1DClusVCol[icl]->getEnergy(), m_new1DClusVCol[icl]->getBars().size(), m_new1DClusVCol[icl]->getNseeds(), m_new1DClusVCol[icl]->getLinkedMCP().size());
+//}
+
+      m_1dclusterUCol.insert(m_1dclusterUCol.end(), m_new1DClusUCol.begin(), m_new1DClusUCol.end());
+      m_1dclusterVCol.insert(m_1dclusterVCol.end(), m_new1DClusVCol.begin(), m_new1DClusVCol.end());
+
+      std::shared_ptr<PandoraPlus::CaloHalfCluster> m_newHFClusterU = std::make_shared<PandoraPlus::CaloHalfCluster>();
+      std::shared_ptr<PandoraPlus::CaloHalfCluster> m_newHFClusterV = std::make_shared<PandoraPlus::CaloHalfCluster>();
+      for(int i1d=0; i1d<m_new1DClusUCol.size(); i1d++)
+        m_newHFClusterU->addUnit(m_new1DClusUCol[i1d].get());
+      for(int i1d=0; i1d<m_new1DClusVCol.size(); i1d++)
+        m_newHFClusterV->addUnit(m_new1DClusVCol[i1d].get());
+      
+      m_newHFClusterU->getLinkedMCPfromUnit();
+      m_newHFClusterV->getLinkedMCPfromUnit();
+      m_halfclusterU.push_back(m_newHFClusterU);
+      m_halfclusterV.push_back(m_newHFClusterV);
+      m_datacol.map_HalfCluster["bkHalfCluster"].push_back(m_newHFClusterU);
+      m_datacol.map_HalfCluster["bkHalfCluster"].push_back(m_newHFClusterV);
+    }
+
+    //Track match
+    for(int itrk=0; itrk<m_TrackCol.size(); itrk++){
+      edm4hep::MCParticle mcp_trk = m_TrackCol[itrk]->getLeadingMCP();
+      for(int ihf=0; ihf<m_halfclusterU.size(); ihf++){
+        if(m_halfclusterU[ihf]->getLeadingMCP()==mcp_trk){
+          m_halfclusterU[ihf]->addAssociatedTrack(m_TrackCol[itrk].get());
+          m_TrackCol[itrk]->addAssociatedHalfClusterU(m_halfclusterU[ihf].get());
+        }
+      }
+      for(int ihf=0; ihf<m_halfclusterV.size(); ihf++){
+        if(m_halfclusterV[ihf]->getLeadingMCP()==mcp_trk){
+          m_halfclusterV[ihf]->addAssociatedTrack(m_TrackCol[itrk].get());
+          m_TrackCol[itrk]->addAssociatedHalfClusterV(m_halfclusterV[ihf].get());
+        }
+      }
+    }
+
+/*
+cout<<"Check tower ID: ";
+cout<<endl;
+  printf("    HalfCluster size: (%d, %d) \n", m_halfclusterU.size(), m_halfclusterV.size() );
+  cout<<"    Loop print HalfClusterU: "<<endl;
+  for(int icl=0; icl<m_halfclusterU.size(); icl++){
+    cout<<"      In HFClusU #"<<icl<<": shower size = "<<m_halfclusterU[icl]->getCluster().size()<<", En = "<<m_halfclusterU[icl]->getEnergy();
+    printf(", Position (%.3f, %.3f, %.3f), address %p ",m_halfclusterU[icl]->getPos().x(), m_halfclusterU[icl]->getPos().y(), m_halfclusterU[icl]->getPos().z(), m_halfclusterU[icl]);
+    printf(", cousin size %d, address: ", m_halfclusterU[icl]->getHalfClusterCol("CousinCluster").size());
+    for(int ics=0; ics<m_halfclusterU[icl]->getHalfClusterCol("CousinCluster").size(); ics++) printf("%p, ", m_halfclusterU[icl]->getHalfClusterCol("CousinCluster")[ics]);
+    printf(", track size %d, address: ", m_halfclusterU[icl]->getAssociatedTracks().size());
+    for(int itrk=0; itrk<m_halfclusterU[icl]->getAssociatedTracks().size(); itrk++) printf("%p, ", m_halfclusterU[icl]->getAssociatedTracks()[itrk]);
+    printf(", MCP link size %d, pid, Pz and weight: ", m_halfclusterU[icl]->getLinkedMCP().size());
+    for(int imc=0; imc<m_halfclusterU[icl]->getLinkedMCP().size(); imc++) printf("(%d, %.3f, %.3f), ", m_halfclusterU[icl]->getLinkedMCP()[imc].first.getPDG(), m_halfclusterU[icl]->getLinkedMCP()[imc].first.getMomentum().z, m_halfclusterU[icl]->getLinkedMCP()[imc].second);
+    cout<<endl;
+    for(auto ish : m_halfclusterU[icl]->getCluster()){
+      printf("          Shower layer %d, Pos+E (%.3f, %.3f, %.3f, %.3f), Nbars %d, NSeed %d, Address %p \n", ish->getDlayer(), ish->getPos().x(), ish->getPos().y(), ish->getPos().z(), ish->getEnergy(), ish->getBars().size(),  ish->getNseeds(), ish );
+    }
+  }
+  cout<<endl;
+
+  cout<<"    Loop print HalfClusterV: "<<endl;
+  for(int icl=0; icl<m_halfclusterV.size(); icl++){
+    cout<<"      In HFClusV #"<<icl<<": shower size = "<<m_halfclusterV[icl]->getCluster().size()<<", En = "<<m_halfclusterV[icl]->getEnergy();
+    printf(", Position (%.3f, %.3f, %.3f), address %p ",m_halfclusterV[icl]->getPos().x(), m_halfclusterV[icl]->getPos().y(), m_halfclusterV[icl]->getPos().z(), m_halfclusterV[icl]);
+    printf(", cousin size %d, address: ", m_halfclusterV[icl]->getHalfClusterCol("CousinCluster").size());
+    for(int ics=0; ics<m_halfclusterV[icl]->getHalfClusterCol("CousinCluster").size(); ics++) printf("%p, ", m_halfclusterV[icl]->getHalfClusterCol("CousinCluster")[ics]);
+    printf(", track size %d, address: ", m_halfclusterV[icl]->getAssociatedTracks().size());
+    for(int itrk=0; itrk<m_halfclusterV[icl]->getAssociatedTracks().size(); itrk++) printf("%p, ", m_halfclusterV[icl]->getAssociatedTracks()[itrk]);
+    printf(", MCP link size %d, pid, Pz and weight: ", m_halfclusterV[icl]->getLinkedMCP().size());
+    for(int imc=0; imc<m_halfclusterV[icl]->getLinkedMCP().size(); imc++) printf("(%d, %.3f, %.3f), ", m_halfclusterV[icl]->getLinkedMCP()[imc].first.getPDG(), m_halfclusterV[icl]->getLinkedMCP()[imc].first.getMomentum().z, m_halfclusterV[icl]->getLinkedMCP()[imc].second);
+    cout<<endl;
+    for(auto ish : m_halfclusterV[icl]->getCluster()){
+      printf("          Shower layer %d, Pos+E (%.3f, %.3f, %.3f, %.3f), Nbars %d, NSeed %d, Address %p \n", ish->getDlayer(), ish->getPos().x(), ish->getPos().y(), ish->getPos().z(), ish->getEnergy(), ish->getBars().size(), ish->getNseeds(), ish );
+    }
+  }
+  cout<<endl;
+*/
+
+    //Create tower
+    HalfClusterToTowers(m_halfclusterU, m_halfclusterV, m_towers);
+//cout<<"  Ecal halfcluster size "<<m_halfclusterU.size()<<", "<<m_halfclusterV.size()<<", tower size "<<m_towers.size()<<endl;
+
+    m_datacol.map_1DCluster[settings.map_stringPars["OutputECAL1DClusters"]+"U"] = m_1dclusterUCol;
+    m_datacol.map_1DCluster[settings.map_stringPars["OutputECAL1DClusters"]+"V"] = m_1dclusterVCol;
+    m_datacol.map_HalfCluster[settings.map_stringPars["OutputECALHalfClusters"]+"U"] = m_halfclusterU;
+    m_datacol.map_HalfCluster[settings.map_stringPars["OutputECALHalfClusters"]+"V"] = m_halfclusterV;  
+    m_datacol.map_CaloCluster[settings.map_stringPars["OutputECALTower"]] = m_towers;
+  }
+
+
+
+
+  //HCAL clustering
+  if(settings.map_boolPars["DoHCALClustering"]){
+//cout<<"Input HCAL hit size "<<m_hits.size()<<endl;
+    std::map<edm4hep::MCParticle, std::vector<std::shared_ptr<PandoraPlus::CaloHit>> > map_hitCol; map_hitCol.clear();
+    for(int ihit=0; ihit<m_hits.size(); ihit++){
+      if(settings.map_boolPars["UseSplit"]){
+        std::vector< std::pair<edm4hep::MCParticle, float> > m_linkVec = m_hits[ihit]->getLinkedMCP();
+//printf("  Hit #%d: link size %d: ", ihit, m_linkVec.size());
+//for(int il=0; il<m_linkVec.size(); il++) printf("(%d, %.2f), ", m_linkVec[il].first.getPDG(), m_linkVec[il].second);
+//cout<<endl;
+
+        if(m_linkVec.size()==0){
+          std::cout<<"ERROR: No truth info in hit #"<<ihit<<std::endl;
+          continue;
+        }
+        if(m_linkVec.size()==1){
+          edm4hep::MCParticle mcp = m_linkVec[0].first;
+          map_hitCol[mcp].push_back(m_hits[ihit]);
+          continue;
+        }
+   
+        for(int ilink=0; ilink<m_linkVec.size(); ilink++){
+          auto tmp_newhit = m_hits[ihit]->Clone();
+          tmp_newhit->setEnergy(tmp_newhit->getEnergy()*m_linkVec[ilink].second);
+          tmp_newhit->setLinkedMCP(std::vector<std::pair<edm4hep::MCParticle, float>>{std::make_pair(m_linkVec[ilink].first, 1.)});
+          map_hitCol[m_linkVec[ilink].first].push_back(tmp_newhit);
+          m_datacol.map_CaloHit["bkHit"].push_back(tmp_newhit);
+        }
+      }
+   
+      else{
+        edm4hep::MCParticle mcp = m_hits[ihit]->getLeadingMCP();
+        map_hitCol[mcp].push_back(m_hits[ihit]);
+      }
+   
+    }//End loop hits
+
+//cout<<"truth map size "<<map_hitCol.size()<<endl;
+//for(auto& iter: map_hitCol){
+//  double totE = 0.;
+//  for(int ibar=0; ibar<iter.second.size(); ibar++)
+//    totE += iter.second[ibar]->getEnergy();
+//  printf("  Truth MCP pdgid %d, bar size %d, totE %.3f \n", iter.first.getPDG(), iter.second.size(), totE);
+//} 
+  
+    for(auto& iter: map_hitCol){
+      std::shared_ptr<PandoraPlus::Calo3DCluster> m_newCluster = std::make_shared<PandoraPlus::Calo3DCluster>();
+      for(int ihit=0; ihit<iter.second.size(); ihit++) m_newCluster->addHit( iter.second[ihit].get() );    
+      m_newCluster->getLinkedMCPfromHit();
+      m_clusters.push_back(m_newCluster);
+      m_datacol.map_CaloCluster["bk3DCluster"].push_back(m_newCluster);
+    }
+
+    for(int icl=0; icl<m_clusters.size(); icl++){
+      for(int itrk=0; itrk<m_TrackCol.size(); itrk++){
+        edm4hep::MCParticle mcp_trk = m_TrackCol[itrk]->getLeadingMCP();
+        if(m_clusters[icl]->getLeadingMCP()==mcp_trk){
+          m_clusters[icl]->addAssociatedTrack(m_TrackCol[itrk].get());
+          break;
+        }
+      }
+    }
+
+    m_datacol.map_CaloCluster[settings.map_stringPars["OutputHCALClusters"]] = m_clusters;
+  }
+
+  m_datacol.map_1DCluster["bk1DCluster"].insert( m_datacol.map_1DCluster["bk1DCluster"].end(), m_bkCol.map_1DCluster["bk1DCluster"].begin(), m_bkCol.map_1DCluster["bk1DCluster"].end() );
+  m_datacol.map_2DCluster["bk2DCluster"].insert( m_datacol.map_2DCluster["bk2DCluster"].end(), m_bkCol.map_2DCluster["bk2DCluster"].begin(), m_bkCol.map_2DCluster["bk2DCluster"].end() );
+  m_datacol.map_HalfCluster["bkHalfCluster"].insert( m_datacol.map_HalfCluster["bkHalfCluster"].end(), m_bkCol.map_HalfCluster["bkHalfCluster"].begin(), m_bkCol.map_HalfCluster["bkHalfCluster"].end() );
+
+
+  return StatusCode::SUCCESS;
+};
+
+StatusCode TruthClusteringAlg::ClearAlgorithm(){
+  m_bars.clear();
+  m_1dclusterUCol.clear();
+  m_halfclusterU.clear();
+  m_halfclusterV.clear();
+
+  m_hits.clear();
+  m_clusters.clear();
+  m_towers.clear();
+
+  m_bkCol.Clear();
+  return StatusCode::SUCCESS;
+};
+
+StatusCode TruthClusteringAlg::HalfClusterToTowers( std::vector<std::shared_ptr<PandoraPlus::CaloHalfCluster>>& m_halfClusU,
+                                                    std::vector<std::shared_ptr<PandoraPlus::CaloHalfCluster>>& m_halfClusV,
+                                                    std::vector<std::shared_ptr<PandoraPlus::Calo3DCluster>>& m_towers ){
+  m_towers.clear();
+
+  std::map<std::vector<int>, std::vector<const PandoraPlus::CaloUnit*>> map_barCol; map_barCol.clear(); 
+  for(int il=0; il<m_halfClusU.size(); il++){
+    for(int is=0; is<m_halfClusU[il]->getCluster().size(); is++){
+      const PandoraPlus::Calo1DCluster* p_shower = m_halfClusU[il]->getCluster()[is];
+      for(int ibar=0; ibar<p_shower->getBars().size(); ibar++){
+        std::vector<int> barID(2);
+        barID[0] = p_shower->getBars()[ibar]->getModule();
+        barID[1] = p_shower->getBars()[ibar]->getStave();
+        map_barCol[barID].push_back(p_shower->getBars()[ibar]);
+      }
+    }
+  }
+  for(int il=0; il<m_halfClusV.size(); il++){
+    for(int is=0; is<m_halfClusV[il]->getCluster().size(); is++){
+      const PandoraPlus::Calo1DCluster* p_shower = m_halfClusV[il]->getCluster()[is];
+      for(int ibar=0; ibar<p_shower->getBars().size(); ibar++){
+        std::vector<int> barID(2);
+        barID[0] = p_shower->getBars()[ibar]->getModule();
+        barID[1] = p_shower->getBars()[ibar]->getStave();
+        map_barCol[barID].push_back(p_shower->getBars()[ibar]);
+      }
+    }
+  }
+
+//cout<<"  tower size: "<<map_barCol.size()<<endl;
+//cout<<"  Print tower info "<<endl;
+//for(auto& iter: map_barCol){
+//printf("    TowerID [%d, %d, %d], bar size %d \n", iter.first[0], iter.first[1], iter.first[2], iter.second.size());
+//}
+
+  //Re-build the objects in tower
+  for(auto& itower: map_barCol){
+    std::vector<std::shared_ptr<PandoraPlus::CaloHalfCluster>> m_newHFClusUCol;
+    std::vector<std::shared_ptr<PandoraPlus::CaloHalfCluster>> m_newHFClusVCol;
+    std::vector<std::shared_ptr<PandoraPlus::Calo2DCluster>> m_new2DClusCol;
+
+    //Get map for MCP-bars
+    std::map<edm4hep::MCParticle, std::vector<const PandoraPlus::CaloUnit*> > map_matchBar; 
+    for(int ibar=0; ibar<itower.second.size(); ibar++){
+      edm4hep::MCParticle mcp = itower.second[ibar]->getLeadingMCP();
+      map_matchBar[mcp].push_back(itower.second[ibar]);
+    }
+//printf("    TowerID [%d, %d, %d], MCP size %d, bar size %d \n", itower.first[0], itower.first[1], itower.first[2], map_matchBar.size(), itower.second.size());
+
+    //Build objects for MCP
+    for(auto& iter: map_matchBar){
+//cout<<"    Print bar info"<<endl;
+//for(int ibar=0; ibar<iter.second.size(); ibar++){
+//printf("    cellID (%d, %d, %d, %d, %d, %d), En %.3f \n", iter.second[ibar]->getModule(), iter.second[ibar]->getPart(), iter.second[ibar]->getStave(), iter.second[ibar]->getDlayer(), iter.second[ibar]->getSlayer(), iter.second[ibar]->getBar(), iter.second[ibar]->getEnergy());
+//}
+      std::vector<std::shared_ptr<PandoraPlus::Calo1DCluster>> m_new1DClusUCol;
+      std::vector<std::shared_ptr<PandoraPlus::Calo1DCluster>> m_new1DClusVCol;
+
+      //Ordered bars
+      std::map<int, std::vector<const PandoraPlus::CaloUnit*> > m_orderedBars; m_orderedBars.clear();
+      for(int ibar=0; ibar<iter.second.size(); ibar++)
+        m_orderedBars[iter.second[ibar]->getDlayer()].push_back(iter.second[ibar]);
+
+//cout<<"    Layer size "<<m_orderedBars.size()<<endl;
+
+      //1D&2D cluster
+      for(auto& iter_bar: m_orderedBars){
+//cout<<"    In layer #"<<iter_bar.first<<": bar size "<<iter_bar.second.size()<<endl;
+
+        std::shared_ptr<PandoraPlus::Calo1DCluster> tmp_new1dclusterU = std::make_shared<PandoraPlus::Calo1DCluster>();
+        std::shared_ptr<PandoraPlus::Calo1DCluster> tmp_new1dclusterV = std::make_shared<PandoraPlus::Calo1DCluster>();
+        for(int ibar=0; ibar<iter_bar.second.size(); ibar++){
+          if(iter_bar.second[ibar]->getSlayer()==0) tmp_new1dclusterU->addUnit( iter_bar.second[ibar] );
+          if(iter_bar.second[ibar]->getSlayer()==1) tmp_new1dclusterV->addUnit( iter_bar.second[ibar] );
+        }
+        if(tmp_new1dclusterU && tmp_new1dclusterU->getBars().size()>0){
+          tmp_new1dclusterU->getLinkedMCPfromUnit();
+          tmp_new1dclusterU->setSeed();
+          tmp_new1dclusterU->setIDInfo();
+          m_new1DClusUCol.push_back(tmp_new1dclusterU);
+          m_bkCol.map_1DCluster["bk1DCluster"].push_back(tmp_new1dclusterU);
+        }
+        if(tmp_new1dclusterV && tmp_new1dclusterV->getBars().size()>0){
+          tmp_new1dclusterV->getLinkedMCPfromUnit();
+          tmp_new1dclusterV->setSeed();
+          tmp_new1dclusterV->setIDInfo();
+          m_new1DClusVCol.push_back(tmp_new1dclusterV);
+          m_bkCol.map_1DCluster["bk1DCluster"].push_back(tmp_new1dclusterV);
+        }
+        if(tmp_new1dclusterU && tmp_new1dclusterV && tmp_new1dclusterU->getBars().size()>0 && tmp_new1dclusterV->getBars().size()>0){
+          std::shared_ptr<PandoraPlus::Calo2DCluster> tmp_block = std::make_shared<PandoraPlus::Calo2DCluster>();
+          for(int ibar=0; ibar<iter_bar.second.size(); ibar++) tmp_block->addBar(iter_bar.second[ibar]);
+          tmp_block->addUnit(tmp_new1dclusterU.get());
+          tmp_block->addUnit(tmp_new1dclusterV.get());
+          tmp_block->setTowerID( itower.first );
+          m_new2DClusCol.push_back(tmp_block);
+          m_bkCol.map_2DCluster["bk2DCluster"].push_back(tmp_block);
+        }
+      }
+//cout<<"    1DClusU size "<<m_new1DClusUCol.size()<<", 1DClusV size "<<m_new1DClusVCol.size();
+//cout<<", 2DClus size "<<m_new2DClusCol.size()<<endl;
+
+      //Half cluster
+      std::shared_ptr<PandoraPlus::CaloHalfCluster> m_newHFClusterU = std::make_shared<PandoraPlus::CaloHalfCluster>();
+      std::shared_ptr<PandoraPlus::CaloHalfCluster> m_newHFClusterV = std::make_shared<PandoraPlus::CaloHalfCluster>();
+      for(int i1d=0; i1d<m_new1DClusUCol.size(); i1d++)
+        m_newHFClusterU->addUnit(m_new1DClusUCol[i1d].get());
+      for(int i1d=0; i1d<m_new1DClusVCol.size(); i1d++)
+        m_newHFClusterV->addUnit(m_new1DClusVCol[i1d].get());
+
+      m_newHFClusterU->getLinkedMCPfromUnit();
+      m_newHFClusterV->getLinkedMCPfromUnit();
+      m_newHFClusUCol.push_back(m_newHFClusterU);
+      m_newHFClusVCol.push_back(m_newHFClusterV);
+      m_bkCol.map_HalfCluster["bkHalfCluster"].push_back(m_newHFClusterU);
+      m_bkCol.map_HalfCluster["bkHalfCluster"].push_back(m_newHFClusterV);
+    }
+//printf("    In tower [%d, %d, %d] HFClus size (%d, %d) \n", itower.first[0], itower.first[1], itower.first[2], m_newHFClusUCol.size(), m_newHFClusVCol.size());
+    //Form a tower
+    std::vector<const PandoraPlus::CaloHalfCluster*> const_HFClusU; const_HFClusU.clear();
+    std::vector<const PandoraPlus::CaloHalfCluster*> const_HFClusV; const_HFClusV.clear();
+    for(int ics=0; ics<m_newHFClusUCol.size(); ics++){ const_HFClusU.push_back(m_newHFClusUCol[ics].get()); }
+    for(int ics=0; ics<m_newHFClusVCol.size(); ics++){ const_HFClusV.push_back(m_newHFClusVCol[ics].get()); }
+    
+    std::shared_ptr<PandoraPlus::Calo3DCluster> m_tower = std::make_shared<PandoraPlus::Calo3DCluster>();
+    m_tower->addTowerID(itower.first);
+    for(int i2d=0; i2d<m_new2DClusCol.size(); i2d++) m_tower->addUnit(m_new2DClusCol[i2d].get());
+    m_tower->setHalfClusters( settings.map_stringPars["OutputECALHalfClusters"]+"U", const_HFClusU,
+                              settings.map_stringPars["OutputECALHalfClusters"]+"V", const_HFClusV );
+    m_towers.push_back(m_tower);
+  }
+
+/*
+cout<<"  After splitting: tower size "<<m_towers.size()<<". Print Tower: "<<endl;
+for(auto it : m_towers){
+  std::vector<const CaloHalfCluster*> m_HFClusUInTower = it->getHalfClusterUCol(settings.map_stringPars["OutputECALHalfClusters"]+"U");
+  std::vector<const CaloHalfCluster*> m_HFClusVInTower = it->getHalfClusterVCol(settings.map_stringPars["OutputECALHalfClusters"]+"V");
+
+cout<<"Check tower ID: ";
+for(int i=0; i<it->getTowerID().size(); i++) printf("[%d, %d, %d], ", it->getTowerID()[i][0], it->getTowerID()[i][1], it->getTowerID()[i][2]);
+cout<<endl;
+  printf("    In Tower [%d, %d, %d], ", it->getTowerID()[0][0], it->getTowerID()[0][1], it->getTowerID()[0][2] );
+  printf("    HalfCluster size: (%d, %d) \n", m_HFClusUInTower.size(), m_HFClusVInTower.size() );
+  cout<<"    Loop print HalfClusterU: "<<endl;
+  for(int icl=0; icl<m_HFClusUInTower.size(); icl++){
+    cout<<"      In HFClusU #"<<icl<<": shower size = "<<m_HFClusUInTower[icl]->getCluster().size()<<", En = "<<m_HFClusUInTower[icl]->getEnergy();
+    printf(", Position (%.3f, %.3f, %.3f), address %p ",m_HFClusUInTower[icl]->getPos().x(), m_HFClusUInTower[icl]->getPos().y(), m_HFClusUInTower[icl]->getPos().z(), m_HFClusUInTower[icl]);
+    printf(", cousin size %d, address: ", m_HFClusUInTower[icl]->getHalfClusterCol("CousinCluster").size());
+    for(int ics=0; ics<m_HFClusUInTower[icl]->getHalfClusterCol("CousinCluster").size(); ics++) printf("%p, ", m_HFClusUInTower[icl]->getHalfClusterCol("CousinCluster")[ics]);
+    printf(", track size %d, address: ", m_HFClusUInTower[icl]->getAssociatedTracks().size());
+    for(int itrk=0; itrk<m_HFClusUInTower[icl]->getAssociatedTracks().size(); itrk++) printf("%p, ", m_HFClusUInTower[icl]->getAssociatedTracks()[itrk]);
+    printf(", MCP link size %d, pid, Pz and weight: ", m_HFClusUInTower[icl]->getLinkedMCP().size());
+    for(int imc=0; imc<m_HFClusUInTower[icl]->getLinkedMCP().size(); imc++) printf("(%d, %.3f, %.3f), ", m_HFClusUInTower[icl]->getLinkedMCP()[imc].first.getPDG(), m_HFClusUInTower[icl]->getLinkedMCP()[imc].first.getMomentum().z, m_HFClusUInTower[icl]->getLinkedMCP()[imc].second);
+    cout<<endl;
+    for(auto ish : m_HFClusUInTower[icl]->getCluster()){
+      printf("          Shower layer %d, Pos+E (%.3f, %.3f, %.3f, %.3f), Nbars %d, NSeed %d, Address %p \n", ish->getDlayer(), ish->getPos().x(), ish->getPos().y(), ish->getPos().z(), ish->getEnergy(), ish->getBars().size(),  ish->getNseeds(), ish );
+    }
+  }
+  cout<<endl;
+
+  cout<<"    Loop print HalfClusterV: "<<endl;
+  for(int icl=0; icl<m_HFClusVInTower.size(); icl++){
+    cout<<"      In HFClusV #"<<icl<<": shower size = "<<m_HFClusVInTower[icl]->getCluster().size()<<", En = "<<m_HFClusVInTower[icl]->getEnergy();
+    printf(", Position (%.3f, %.3f, %.3f), address %p ",m_HFClusVInTower[icl]->getPos().x(), m_HFClusVInTower[icl]->getPos().y(), m_HFClusVInTower[icl]->getPos().z(), m_HFClusVInTower[icl]);
+    printf(", cousin size %d, address: ", m_HFClusVInTower[icl]->getHalfClusterCol("CousinCluster").size());
+    for(int ics=0; ics<m_HFClusVInTower[icl]->getHalfClusterCol("CousinCluster").size(); ics++) printf("%p, ", m_HFClusVInTower[icl]->getHalfClusterCol("CousinCluster")[ics]);
+    printf(", track size %d, address: ", m_HFClusVInTower[icl]->getAssociatedTracks().size());
+    for(int itrk=0; itrk<m_HFClusVInTower[icl]->getAssociatedTracks().size(); itrk++) printf("%p, ", m_HFClusVInTower[icl]->getAssociatedTracks()[itrk]);
+    printf(", MCP link size %d, pid, Pz and weight: ", m_HFClusVInTower[icl]->getLinkedMCP().size());
+    for(int imc=0; imc<m_HFClusVInTower[icl]->getLinkedMCP().size(); imc++) printf("(%d, %.3f, %.3f), ", m_HFClusVInTower[icl]->getLinkedMCP()[imc].first.getPDG(), m_HFClusVInTower[icl]->getLinkedMCP()[imc].first.getMomentum().z, m_HFClusVInTower[icl]->getLinkedMCP()[imc].second);
+    cout<<endl;
+    for(auto ish : m_HFClusVInTower[icl]->getCluster()){
+      printf("          Shower layer %d, Pos+E (%.3f, %.3f, %.3f, %.3f), Nbars %d, NSeed %d, Address %p \n", ish->getDlayer(), ish->getPos().x(), ish->getPos().y(), ish->getPos().z(), ish->getEnergy(), ish->getBars().size(), ish->getNseeds(), ish );
+    }
+  }
+  cout<<endl;
+}
+*/
+
+
+  return StatusCode::SUCCESS;
+}
+
+
+#endif
diff --git a/Reconstruction/CrystalCaloRec/src/Algorithm/TruthEnergySplittingAlg.cpp b/Reconstruction/CrystalCaloRec/src/Algorithm/TruthEnergySplittingAlg.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c2f10d5f5662b445956f9550d2a865cd33641ab5
--- /dev/null
+++ b/Reconstruction/CrystalCaloRec/src/Algorithm/TruthEnergySplittingAlg.cpp
@@ -0,0 +1,625 @@
+#ifndef _TRUTHENERGYSPLITTING_ALG_C
+#define _TRUTHENERGYSPLITTING_ALG_C
+
+#include "Algorithm/TruthEnergySplittingAlg.h"
+
+StatusCode TruthEnergySplittingAlg::ReadSettings(Settings& m_settings){
+  settings = m_settings;
+
+  //Initialize parameters
+  if(settings.map_stringPars.find("ReadinAxisName")==settings.map_stringPars.end())  settings.map_stringPars["ReadinAxisName"] = "TruthMergedAxis";
+  if(settings.map_stringPars.find("OutputClusName")==settings.map_stringPars.end())  settings.map_stringPars["OutputClusName"] = "TruthESCluster";
+  if(settings.map_stringPars.find("OutputTowerName")==settings.map_stringPars.end()) settings.map_stringPars["OutputTowerName"] = "TruthESTower";
+
+  if(settings.map_floatPars.find("Eth_HFClus")==settings.map_floatPars.end())        settings.map_floatPars["Eth_HFClus"] = 0.05;
+  if(settings.map_intPars.find("th_Nhit")==settings.map_intPars.end())               settings.map_intPars["th_Nhit"] = 2;
+  if(settings.map_boolPars.find("CompactHFCluster")==settings.map_boolPars.end())    settings.map_boolPars["CompactHFCluster"] = 1;
+
+  return StatusCode::SUCCESS;
+};
+
+StatusCode TruthEnergySplittingAlg::Initialize( PandoraPlusDataCol& m_datacol ){
+
+  p_HalfClusterU.clear();
+  p_HalfClusterV.clear();
+  m_newClusUCol.clear();
+  m_newClusVCol.clear();
+  m_bkCol.Clear();
+
+  for(int ih=0; ih<m_datacol.map_HalfCluster["HalfClusterColU"].size(); ih++)
+    p_HalfClusterU.push_back( m_datacol.map_HalfCluster["HalfClusterColU"][ih].get() );
+  for(int ih=0; ih<m_datacol.map_HalfCluster["HalfClusterColV"].size(); ih++)
+    p_HalfClusterV.push_back( m_datacol.map_HalfCluster["HalfClusterColV"][ih].get() );
+
+  return StatusCode::SUCCESS;
+};
+
+StatusCode TruthEnergySplittingAlg::RunAlgorithm( PandoraPlusDataCol& m_datacol ){
+//cout<<"  TruthEnergySplittingAlg: readin HFCluster size "<<p_HalfClusterU.size()<<", "<<p_HalfClusterV.size()<<endl;
+
+  for(int ih=0; ih<p_HalfClusterU.size(); ih++){
+    std::vector<const PandoraPlus::CaloHalfCluster*> m_axisUCol;
+    if( settings.map_stringPars["ReadinAxisName"] == "AllAxis" ) m_axisUCol = p_HalfClusterU[ih]->getAllHalfClusterCol();
+    else m_axisUCol = p_HalfClusterU[ih]->getHalfClusterCol(settings.map_stringPars["ReadinAxisName"]);    
+
+    std::vector<const PandoraPlus::Calo1DCluster*> m_1dclusCol = p_HalfClusterU[ih]->getCluster();
+//cout<<"    In HFU #"<<ih<<": axis size "<<m_axisUCol.size()<<", 1DCluster size "<<m_1dclusCol.size()<<endl;
+    
+    for(int iax=0; iax<m_axisUCol.size(); iax++){
+      edm4hep::MCParticle truthMCP_axis = m_axisUCol[iax]->getLeadingMCP();
+      std::shared_ptr<PandoraPlus::CaloHalfCluster> m_newClus = std::make_shared<PandoraPlus::CaloHalfCluster>();
+//cout<<"      Axis #"<<iax<<": track size "<<m_axisUCol[iax]->getAssociatedTracks().size()<<", truth MC pid "<<truthMCP_axis.getPDG()<<endl;
+
+      for(int ish=0; ish<m_1dclusCol.size(); ish++){
+        std::shared_ptr<PandoraPlus::Calo1DCluster> m_shower = std::make_shared<PandoraPlus::Calo1DCluster>(); 
+//cout<<"        1D cluster #"<<ish<<": bar size "<<m_1dclusCol[ish]->getBars().size();
+        for(int ibar=0; ibar<m_1dclusCol[ish]->getBars().size(); ibar++){
+          auto truthMap = m_1dclusCol[ish]->getBars()[ibar]->getLinkedMCP();
+          for(auto& iter: truthMap){
+            if(!(iter.first==truthMCP_axis)) continue;
+            if(iter.second<0.05) continue;
+
+            //This bar has contribution from MCP. Split it. 
+            auto m_bar = m_1dclusCol[ish]->getBars()[ibar]->Clone();
+            m_bar->setQ(m_bar->getQ1()*iter.second, m_bar->getQ2()*iter.second);
+            m_bar->setLinkedMCP( std::vector<std::pair<edm4hep::MCParticle, float>>{std::make_pair(iter.first, 1)} );
+
+            m_shower->addUnit(m_bar.get());
+            m_datacol.map_BarCol["bkBars"].push_back(m_bar);
+          }
+        }
+
+        if(m_shower && m_shower->getBars().size()>0){
+//cout<<". Make a new shower. "<<endl;
+          m_shower->getLinkedMCPfromUnit();
+          m_shower->setSeed();
+          m_shower->setIDInfo();
+          m_newClus->addUnit(m_shower.get());
+          m_datacol.map_1DCluster["bk1DCluster"].push_back(m_shower);
+        }
+      }//End loop 1DClusters in HFCluster
+
+      if(m_newClus && m_newClus->getCluster().size()!=0){ 
+        for(int itrk=0; itrk<m_axisUCol[iax]->getAssociatedTracks().size(); itrk++) m_newClus->addAssociatedTrack(m_axisUCol[iax]->getAssociatedTracks()[itrk]);
+        m_newClus->getLinkedMCPfromUnit();
+        m_newClus->fitAxis("");
+        m_newClus->mergeClusterInLayer();
+        m_newClusUCol.push_back(m_newClus);
+        m_datacol.map_HalfCluster["bkHalfCluster"].push_back(m_newClus);
+      }
+    }//End loop axis
+  }//End loop HalfClusters.
+//cout<<"  Splitted HFClusterU size "<<m_newClusUCol.size()<<endl;
+
+  //Merge new HFClusters linked to the same MCP
+  for(int iax=0; iax<m_newClusUCol.size(); iax++){
+    for(int jax=iax+1; jax<m_newClusUCol.size(); jax++){
+      if(m_newClusUCol[iax]->getLeadingMCP() == m_newClusUCol[jax]->getLeadingMCP()){
+        m_newClusUCol[iax]->mergeHalfCluster( m_newClusUCol[jax].get() );
+        m_newClusUCol.erase(m_newClusUCol.begin()+jax);
+        jax--;
+        if(iax>jax+1) iax--;
+      }
+    }
+  }
+//cout<<"  Splitted HFClusterU size after merging "<<m_newClusUCol.size()<<endl;
+
+  for(int ih=0; ih<p_HalfClusterV.size(); ih++){
+    std::vector<const PandoraPlus::CaloHalfCluster*> m_axisVCol;
+    if( settings.map_stringPars["ReadinAxisName"] == "AllAxis" ) m_axisVCol = p_HalfClusterV[ih]->getAllHalfClusterCol();
+    else m_axisVCol = p_HalfClusterV[ih]->getHalfClusterCol(settings.map_stringPars["ReadinAxisName"]);
+
+    std::vector<const PandoraPlus::Calo1DCluster*> m_1dclusCol = p_HalfClusterV[ih]->getCluster();
+//cout<<"    In HFV #"<<ih<<": axis size "<<m_axisVCol.size()<<", 1DCluster size "<<m_1dclusCol.size()<<endl;
+
+    for(int iax=0; iax<m_axisVCol.size(); iax++){
+      edm4hep::MCParticle truthMCP_axis = m_axisVCol[iax]->getLeadingMCP();
+      std::shared_ptr<PandoraPlus::CaloHalfCluster> m_newClus = std::make_shared<PandoraPlus::CaloHalfCluster>();
+//cout<<"      Axis #"<<iax<<": track size "<<m_axisVCol[iax]->getAssociatedTracks().size()<<", truth MC pid "<<truthMCP_axis.getPDG()<<endl;
+
+      for(int ish=0; ish<m_1dclusCol.size(); ish++){
+        std::shared_ptr<PandoraPlus::Calo1DCluster> m_shower = std::make_shared<PandoraPlus::Calo1DCluster>();
+        for(int ibar=0; ibar<m_1dclusCol[ish]->getBars().size(); ibar++){
+          auto truthMap = m_1dclusCol[ish]->getBars()[ibar]->getLinkedMCP();
+          for(auto& iter: truthMap){
+            if(!(iter.first==truthMCP_axis)) continue;
+            if(iter.second<0.05) continue;
+
+            //This bar has contribution from MCP. Split it.
+            auto m_bar = m_1dclusCol[ish]->getBars()[ibar]->Clone();
+            m_bar->setQ(m_bar->getQ1()*iter.second, m_bar->getQ2()*iter.second);
+            m_bar->setLinkedMCP( std::vector<std::pair<edm4hep::MCParticle, float>>{std::make_pair(iter.first, 1)} );
+
+            m_shower->addUnit(m_bar.get());
+            m_datacol.map_BarCol["bkBars"].push_back(m_bar);
+          }
+        }
+        if(m_shower && m_shower->getBars().size()>0){
+          m_shower->getLinkedMCPfromUnit();
+          m_shower->setSeed();
+          m_shower->setIDInfo();
+          m_newClus->addUnit(m_shower.get());
+          m_datacol.map_1DCluster["bk1DCluster"].push_back(m_shower);
+        }
+      }//End loop 1DClusters in HFCluster
+
+      if(m_newClus && m_newClus->getCluster().size()!=0){
+        for(int itrk=0; itrk<m_axisVCol[iax]->getAssociatedTracks().size(); itrk++) m_newClus->addAssociatedTrack(m_axisVCol[iax]->getAssociatedTracks()[itrk]);
+        m_newClus->getLinkedMCPfromUnit();
+        m_newClus->fitAxis("");
+        m_newClus->mergeClusterInLayer();
+        m_newClusVCol.push_back(m_newClus);
+        m_datacol.map_HalfCluster["bkHalfCluster"].push_back(m_newClus);
+      }
+    }//End loop axis
+  }//End loop HalfClusters.
+//cout<<"  Splitted HFClusterV size "<<m_newClusVCol.size()<<endl;
+
+  //Merge new HFClusters linked to the same MCP
+  for(int iax=0; iax<m_newClusVCol.size(); iax++){
+    for(int jax=iax+1; jax<m_newClusVCol.size(); jax++){
+      if(m_newClusVCol[iax]->getLeadingMCP() == m_newClusVCol[jax]->getLeadingMCP()){
+        m_newClusVCol[iax]->mergeHalfCluster( m_newClusVCol[jax].get() );
+        m_newClusVCol.erase(m_newClusVCol.begin()+jax);
+        jax--;
+        if(iax>jax+1) iax--;
+      }
+    }
+  }
+//cout<<"  Splitted HFClusterU size after merging "<<m_newClusVCol.size()<<endl;
+
+  m_datacol.map_HalfCluster[settings.map_stringPars["OutputClusName"]+"U"] = m_newClusUCol;
+  m_datacol.map_HalfCluster[settings.map_stringPars["OutputClusName"]+"V"] = m_newClusVCol;  
+
+  //Make tower
+  m_towerCol.clear();
+  HalfClusterToTowers(m_newClusUCol, m_newClusVCol, m_towerCol);
+  m_datacol.map_CaloCluster[settings.map_stringPars["OutputTowerName"]] = m_towerCol;
+
+/*
+cout<<"  After splitting: tower size "<<m_towerCol.size()<<". Print Tower: "<<endl;
+for(auto it : m_towerCol){
+  std::vector<const CaloHalfCluster*> m_HFClusUInTower = it->getHalfClusterUCol(settings.map_stringPars["OutputClusName"]+"U");
+  std::vector<const CaloHalfCluster*> m_HFClusVInTower = it->getHalfClusterVCol(settings.map_stringPars["OutputClusName"]+"V");
+  //std::vector<const CaloHalfCluster*> m_HFClusUInTower, m_HFClusVInTower; 
+  //for(int i=0; i<m_newClusUCol.size(); i++) m_HFClusUInTower.push_back(m_newClusUCol[i].get());
+  //for(int i=0; i<m_newClusVCol.size(); i++) m_HFClusVInTower.push_back(m_newClusVCol[i].get());
+
+
+cout<<"Check tower ID: ";
+for(int i=0; i<it->getTowerID().size(); i++) printf("[%d, %d, %d], ", it->getTowerID()[i][0], it->getTowerID()[i][1], it->getTowerID()[i][2]);
+cout<<endl;
+  printf("    In Tower [%d, %d, %d], ", it->getTowerID()[0][0], it->getTowerID()[0][1], it->getTowerID()[0][2] );
+  printf("    HalfCluster size: (%d, %d) \n", m_HFClusUInTower.size(), m_HFClusVInTower.size() );
+  cout<<"    Loop print HalfClusterU: "<<endl;
+  for(int icl=0; icl<m_HFClusUInTower.size(); icl++){
+    cout<<"      In HFClusU #"<<icl<<": shower size = "<<m_HFClusUInTower[icl]->getCluster().size()<<", En = "<<m_HFClusUInTower[icl]->getEnergy()<<", linked MC size "<<m_HFClusUInTower[icl]->getLinkedMCP().size();
+    for(int imc=0; imc<m_HFClusUInTower[icl]->getLinkedMCP().size(); imc++) printf("[%d, %.3f], ", m_HFClusUInTower[icl]->getLinkedMCP()[imc].first.getPDG(), m_HFClusUInTower[icl]->getLinkedMCP()[imc].second);
+    printf(", Position (%.3f, %.3f, %.3f), address %p ",m_HFClusUInTower[icl]->getPos().x(), m_HFClusUInTower[icl]->getPos().y(), m_HFClusUInTower[icl]->getPos().z(), m_HFClusUInTower[icl]);
+    printf(", cousin size %d, address: ", m_HFClusUInTower[icl]->getHalfClusterCol("CousinCluster").size());
+    for(int ics=0; ics<m_HFClusUInTower[icl]->getHalfClusterCol("CousinCluster").size(); ics++) printf("%p, ", m_HFClusUInTower[icl]->getHalfClusterCol("CousinCluster")[ics]);
+    printf(", track size %d, address: ", m_HFClusUInTower[icl]->getAssociatedTracks().size());
+    for(int itrk=0; itrk<m_HFClusUInTower[icl]->getAssociatedTracks().size(); itrk++) printf("%p, ", m_HFClusUInTower[icl]->getAssociatedTracks()[itrk]);
+    cout<<endl;
+    for(auto ish : m_HFClusUInTower[icl]->getCluster()){
+      printf("          Shower layer %d, Pos+E (%.3f, %.3f, %.3f, %.3f), leading MCP [%d, %.3f], Nbars %d, NSeed %d, Address %p \n", ish->getDlayer(), ish->getPos().x(), ish->getPos().y(), ish->getPos().z(), ish->getEnergy(), ish->getLeadingMCP().getPDG(), ish->getLeadingMCPweight(),  ish->getBars().size(),  ish->getNseeds(), ish );
+    }
+  }
+  cout<<endl;
+
+  cout<<"    Loop print HalfClusterV: "<<endl;
+  for(int icl=0; icl<m_HFClusVInTower.size(); icl++){
+    cout<<"      In HFClusV #"<<icl<<": shower size = "<<m_HFClusVInTower[icl]->getCluster().size()<<", En = "<<m_HFClusVInTower[icl]->getEnergy()<<", linked MC size "<<m_HFClusVInTower[icl]->getLinkedMCP().size();
+    for(int imc=0; imc<m_HFClusVInTower[icl]->getLinkedMCP().size(); imc++) printf("[%d, %.3f], ", m_HFClusVInTower[icl]->getLinkedMCP()[imc].first.getPDG(), m_HFClusVInTower[icl]->getLinkedMCP()[imc].second);
+    printf(", Position (%.3f, %.3f, %.3f), address %p ",m_HFClusVInTower[icl]->getPos().x(), m_HFClusVInTower[icl]->getPos().y(), m_HFClusVInTower[icl]->getPos().z(), m_HFClusVInTower[icl]);
+    printf(", cousin size %d, address: ", m_HFClusVInTower[icl]->getHalfClusterCol("CousinCluster").size());
+    for(int ics=0; ics<m_HFClusVInTower[icl]->getHalfClusterCol("CousinCluster").size(); ics++) printf("%p, ", m_HFClusVInTower[icl]->getHalfClusterCol("CousinCluster")[ics]);
+    printf(", track size %d, address: ", m_HFClusVInTower[icl]->getAssociatedTracks().size());
+    for(int itrk=0; itrk<m_HFClusVInTower[icl]->getAssociatedTracks().size(); itrk++) printf("%p, ", m_HFClusVInTower[icl]->getAssociatedTracks()[itrk]);
+    cout<<endl;
+    for(auto ish : m_HFClusVInTower[icl]->getCluster()){
+      printf("          Shower layer %d, Pos+E (%.3f, %.3f, %.3f, %.3f), leading MCP [%d, %.3f], Nbars %d, NSeed %d, Address %p \n", ish->getDlayer(), ish->getPos().x(), ish->getPos().y(), ish->getPos().z(), ish->getEnergy(), ish->getLeadingMCP().getPDG(), ish->getLeadingMCPweight(), ish->getBars().size(), ish->getNseeds(), ish );
+    }
+  }
+  cout<<endl;
+}
+*/
+
+  m_datacol.map_1DCluster["bk1DCluster"].insert( m_datacol.map_1DCluster["bk1DCluster"].end(), m_bkCol.map_1DCluster["bk1DCluster"].begin(), m_bkCol.map_1DCluster["bk1DCluster"].end() );
+  m_datacol.map_2DCluster["bk2DCluster"].insert( m_datacol.map_2DCluster["bk2DCluster"].end(), m_bkCol.map_2DCluster["bk2DCluster"].begin(), m_bkCol.map_2DCluster["bk2DCluster"].end() );
+  m_datacol.map_HalfCluster["bkHalfCluster"].insert( m_datacol.map_HalfCluster["bkHalfCluster"].end(), m_bkCol.map_HalfCluster["bkHalfCluster"].begin(), m_bkCol.map_HalfCluster["bkHalfCluster"].end() );
+
+
+  return StatusCode::SUCCESS;
+};
+
+StatusCode TruthEnergySplittingAlg::ClearAlgorithm(){
+  p_HalfClusterU.clear();
+  p_HalfClusterV.clear();
+  m_newClusUCol.clear();
+  m_newClusVCol.clear();
+
+  m_bkCol.Clear();
+  return StatusCode::SUCCESS;
+};
+
+
+
+StatusCode TruthEnergySplittingAlg::HalfClusterToTowers( std::vector<std::shared_ptr<PandoraPlus::CaloHalfCluster>>& m_halfClusU,
+                                                         std::vector<std::shared_ptr<PandoraPlus::CaloHalfCluster>>& m_halfClusV,
+                                                         std::vector<std::shared_ptr<PandoraPlus::Calo3DCluster>>& m_towers )
+{
+
+  m_towers.clear();
+
+  std::map<std::vector<int>, std::vector<const PandoraPlus::Calo2DCluster*> > map_2DCluster;
+  std::map<std::vector<int>, std::vector<PandoraPlus::CaloHalfCluster*> > map_HalfClusterU;
+  std::map<std::vector<int>, std::vector<PandoraPlus::CaloHalfCluster*> > map_HalfClusterV;
+
+  //Split CaloHalfClusterU
+  for(int il=0; il<m_halfClusU.size(); il++){
+    if(m_halfClusU[il]->getCluster().size()==0) {std::cout<<"WARNING: Have an empty CaloHalfCluster! Skip it! "<<std::endl; continue;}
+
+    //HalfCluster does not cover tower:
+    if( m_halfClusU[il]->getTowerID().size()==1 && m_halfClusU[il]->getCluster().size()>=settings.map_intPars["th_Nhit"]){
+      std::vector<int> cl_towerID =  m_halfClusU[il]->getTowerID()[0];
+      if(settings.map_boolPars["CompactHFCluster"]) m_halfClusU[il]->mergeClusterInLayer();
+      map_HalfClusterU[cl_towerID].push_back(m_halfClusU[il].get());
+      continue;
+    }
+
+    //CaloHalfCluster covers towers: Loop check showers.
+    std::map<std::vector<int>, PandoraPlus::CaloHalfCluster* > tmp_LongiClusMaps; tmp_LongiClusMaps.clear();
+    for(int is=0; is<m_halfClusU[il]->getCluster().size(); is++){
+      const PandoraPlus::Calo1DCluster* p_shower = m_halfClusU[il]->getCluster()[is];
+
+      if(p_shower->getSeeds().size()==0){
+        std::cout<<"  HalfClusterToTowers ERROR: No Seed in 1DShower, Check! "<<std::endl;
+        continue;
+      }
+      std::vector<int> seedID(2);
+      seedID[0] = p_shower->getSeeds()[0]->getModule();
+      seedID[1] = p_shower->getSeeds()[0]->getStave();
+
+      if( tmp_LongiClusMaps.find( seedID )!=tmp_LongiClusMaps.end() ){
+        tmp_LongiClusMaps[seedID]->addUnit( p_shower );
+        tmp_LongiClusMaps[seedID]->setTowerID( seedID );
+      }
+      else{
+        std::shared_ptr<PandoraPlus::CaloHalfCluster> tmp_clus = std::make_shared<PandoraPlus::CaloHalfCluster>();
+        tmp_clus->addUnit( p_shower );
+        tmp_clus->setTowerID( seedID );
+        tmp_LongiClusMaps[seedID] = tmp_clus.get();
+        m_bkCol.map_HalfCluster["bkHalfCluster"].push_back( tmp_clus );
+      }
+      p_shower = nullptr;
+
+    }
+
+    //Connect cousins
+    if(tmp_LongiClusMaps.size()>1){
+      for(auto &iter : tmp_LongiClusMaps){
+        for(auto &iter1 : tmp_LongiClusMaps){
+          if(iter!= iter1 &&
+             iter.second->getEnergy()>settings.map_floatPars["Eth_HFClus"] &&
+             iter1.second->getEnergy()>settings.map_floatPars["Eth_HFClus"] &&
+             iter.second->getCluster().size()>=settings.map_intPars["th_Nhit"] &&
+             iter1.second->getCluster().size()>=settings.map_intPars["th_Nhit"] ){ iter.second->addCousinCluster(iter1.second); }
+        }
+      }
+    }
+    for(auto &iter : tmp_LongiClusMaps){
+      if(iter.second->getEnergy()<settings.map_floatPars["Eth_HFClus"]) continue;
+      iter.second->addHalfCluster("ParentCluster", m_halfClusU[il].get());
+      for(int itrk=0; itrk<m_halfClusU[il]->getAssociatedTracks().size(); itrk++)
+        iter.second->addAssociatedTrack( m_halfClusU[il]->getAssociatedTracks()[itrk] );
+      if(iter.second->getCluster().size()>=settings.map_intPars["th_Nhit"]){
+        if(settings.map_boolPars["CompactHFCluster"]){
+          iter.second->mergeClusterInLayer();
+          iter.second->setTowerID(iter.first);
+        }
+        map_HalfClusterU[iter.first].push_back(iter.second);
+      }
+    }
+
+  }
+
+  //Split CaloHalfClusterV
+  for(int il=0; il<m_halfClusV.size(); il++){
+    if(m_halfClusV[il]->getCluster().size()==0) {std::cout<<"WARNING: Have an empty CaloHalfCluster! Skip it! "<<std::endl; continue;}
+
+    //HalfCluster does not cover tower:
+    if( m_halfClusV[il]->getTowerID().size()==1 && m_halfClusV[il]->getCluster().size()>=settings.map_intPars["th_Nhit"]){
+      std::vector<int> cl_towerID =  m_halfClusV[il]->getTowerID()[0];
+      if(settings.map_boolPars["CompactHFCluster"]) m_halfClusV[il]->mergeClusterInLayer();
+      map_HalfClusterV[cl_towerID].push_back(m_halfClusV[il].get());
+      continue;
+    }
+
+    //CaloHalfCluster covers towers: Loop check showers.
+    std::map<std::vector<int>, PandoraPlus::CaloHalfCluster* > tmp_LongiClusMaps; tmp_LongiClusMaps.clear();
+    for(int is=0; is<m_halfClusV[il]->getCluster().size(); is++){
+      const PandoraPlus::Calo1DCluster* p_shower = m_halfClusV[il]->getCluster()[is];
+      if(p_shower->getSeeds().size()==0){
+        std::cout<<"  HalfClusterToTowers ERROR: No Seed in 1DShower, Check! "<<std::endl;
+        continue;
+      }
+      std::vector<int> seedID(2);
+      seedID[0] = p_shower->getSeeds()[0]->getModule();
+      seedID[1] = p_shower->getSeeds()[0]->getStave();
+
+      if( tmp_LongiClusMaps.find( seedID )!=tmp_LongiClusMaps.end() ){
+        tmp_LongiClusMaps[seedID]->addUnit( p_shower );
+        tmp_LongiClusMaps[seedID]->setTowerID( seedID );
+      }
+      else{
+        std::shared_ptr<PandoraPlus::CaloHalfCluster> tmp_clus = std::make_shared<PandoraPlus::CaloHalfCluster>();
+        tmp_clus->addUnit( p_shower );
+        tmp_clus->setTowerID( seedID );
+        tmp_LongiClusMaps[seedID] = tmp_clus.get();
+        m_bkCol.map_HalfCluster["bkHalfCluster"].push_back( tmp_clus );
+      }
+      p_shower = nullptr;
+
+    }
+
+    //Connect cousins
+//cout<<"  LongiClusVMap size: "<<tmp_LongiClusMaps.size()<<endl;
+    if(tmp_LongiClusMaps.size()>1){
+      for(auto &iter : tmp_LongiClusMaps){
+        for(auto &iter1 : tmp_LongiClusMaps){
+          if(iter!= iter1 &&
+             iter.second->getEnergy()>settings.map_floatPars["Eth_HFClus"] &&
+             iter1.second->getEnergy()>settings.map_floatPars["Eth_HFClus"] &&
+             iter.second->getCluster().size()>=settings.map_intPars["th_Nhit"] &&
+             iter1.second->getCluster().size()>=settings.map_intPars["th_Nhit"] ){ iter.second->addCousinCluster(iter1.second); }
+        }
+      }
+    }
+    for(auto &iter : tmp_LongiClusMaps){
+      if(iter.second->getEnergy()<settings.map_floatPars["Eth_HFClus"]) continue;
+      iter.second->addHalfCluster("ParentCluster", m_halfClusV[il].get());
+      for(int itrk=0; itrk<m_halfClusV[il]->getAssociatedTracks().size(); itrk++)
+        iter.second->addAssociatedTrack( m_halfClusV[il]->getAssociatedTracks()[itrk] );
+      if(iter.second->getCluster().size()>=settings.map_intPars["th_Nhit"]){
+        if(settings.map_boolPars["CompactHFCluster"]){
+          iter.second->mergeClusterInLayer();
+          iter.second->setTowerID(iter.first);
+        }
+        map_HalfClusterV[iter.first].push_back(iter.second);
+      }
+    }
+
+  }
+
+  //Build 2DCluster
+  for(auto &iterU : map_HalfClusterU){
+    if( map_HalfClusterV.find(iterU.first)==map_HalfClusterV.end() ){
+      iterU.second.clear();
+      continue;
+    }
+
+    std::vector<PandoraPlus::CaloHalfCluster*> p_halfClusU = iterU.second;
+    std::vector<PandoraPlus::CaloHalfCluster*> p_halfClusV = map_HalfClusterV[iterU.first];
+
+    //Get ordered showers for looping in layers.
+    std::map<int, std::vector<const PandoraPlus::Calo1DCluster*>> m_orderedShowerU; m_orderedShowerU.clear();
+    std::map<int, std::vector<const PandoraPlus::Calo1DCluster*>> m_orderedShowerV; m_orderedShowerV.clear();
+
+    for(int ic=0; ic<p_halfClusU.size(); ic++){
+      for(int is=0; is<p_halfClusU.at(ic)->getCluster().size(); is++)
+        m_orderedShowerU[p_halfClusU.at(ic)->getCluster()[is]->getDlayer()].push_back( p_halfClusU.at(ic)->getCluster()[is] );
+    }
+    for(int ic=0; ic<p_halfClusV.size(); ic++){
+      for(int is=0; is<p_halfClusV.at(ic)->getCluster().size(); is++)
+        m_orderedShowerV[p_halfClusV.at(ic)->getCluster()[is]->getDlayer()].push_back( p_halfClusV.at(ic)->getCluster()[is] );
+    }
+    p_halfClusU.clear(); p_halfClusV.clear();
+
+
+    //Create super-layers (block)
+    std::vector<const PandoraPlus::Calo2DCluster*> m_blocks; m_blocks.clear();
+    for(auto &iter1 : m_orderedShowerU){
+      if( m_orderedShowerV.find( iter1.first )==m_orderedShowerV.end() ) continue;
+      std::shared_ptr<PandoraPlus::Calo2DCluster> tmp_block = std::make_shared<PandoraPlus::Calo2DCluster>();
+      for(int is=0; is<iter1.second.size(); is++) tmp_block->addUnit( iter1.second.at(is) );
+      for(int is=0; is<m_orderedShowerV[iter1.first].size(); is++) tmp_block->addUnit( m_orderedShowerV[iter1.first].at(is) );
+      tmp_block->setTowerID( iterU.first );
+      m_blocks.push_back( tmp_block.get() );
+      m_bkCol.map_2DCluster["bk2DCluster"].push_back( tmp_block );
+    }
+    map_2DCluster[iterU.first] = m_blocks;
+  }
+  for(auto &iterV : map_HalfClusterV){
+    if( map_HalfClusterU.find(iterV.first)==map_HalfClusterU.end() ){
+      iterV.second.clear();
+    }
+  }
+
+  //Form a tower:
+  for(auto &iter : map_2DCluster){
+    std::vector<int> m_towerID = iter.first;
+//printf("  In tower: [%d, %d, %d] \n", m_towerID[0], m_towerID[1], m_towerID[2]);
+    //Check cousin clusters:
+    std::vector<PandoraPlus::CaloHalfCluster*> m_HFClusUInTower = map_HalfClusterU[m_towerID];
+    for(auto &m_HFclus : m_HFClusUInTower){
+      std::vector<const CaloHalfCluster*> tmp_delClus; tmp_delClus.clear();
+//printf("    Check the cousin of HFClus %p: cousin size %d \n",m_HFclus, m_HFclus->getHalfClusterCol("CousinCluster").size());
+      for(int ics=0; ics<m_HFclus->getHalfClusterCol("CousinCluster").size(); ics++){
+        std::vector<int> tmp_towerID = m_HFclus->getHalfClusterCol("CousinCluster")[ics]->getTowerID()[0];
+//printf("      Cousin #%d: address %p, it's in tower [%d, %d, %d]. \n", ics,  m_HFclus->getHalfClusterCol("CousinCluster")[ics],  tmp_towerID[0], tmp_towerID[1], tmp_towerID[2]);
+//if(m_HFclus->getHalfClusterCol("CousinCluster")[ics]->getTowerID().size()!=1) cout<<"ERROR: cousin cluster covers >1 towers. Check here! "<<endl;
+
+        if( map_2DCluster.find( tmp_towerID )==map_2DCluster.end() )
+          tmp_delClus.push_back( m_HFclus->getHalfClusterCol("CousinCluster")[ics] );
+      }
+//cout<<"Need to delete "<<tmp_delClus.size()<<" cousins"<<endl;
+      for(int ics=0; ics<tmp_delClus.size(); ics++) m_HFclus->deleteCousinCluster( tmp_delClus[ics] );
+    }
+
+    std::vector<PandoraPlus::CaloHalfCluster*> m_HFClusVInTower = map_HalfClusterV[m_towerID];
+    for(auto &m_HFclus : m_HFClusVInTower){
+      std::vector<const CaloHalfCluster*> tmp_delClus; tmp_delClus.clear();
+//printf("    Check the cousin of HFClus %p: cousin size %d \n",m_HFclus, m_HFclus->getHalfClusterCol("CousinCluster").size());
+      for(int ics=0; ics<m_HFclus->getHalfClusterCol("CousinCluster").size(); ics++){
+        std::vector<int> tmp_towerID = m_HFclus->getHalfClusterCol("CousinCluster")[ics]->getTowerID()[0];
+//printf("      Cousin #%d: address %p, it's in tower [%d, %d, %d]. \n", ics,  m_HFclus->getHalfClusterCol("CousinCluster")[ics],  tmp_towerID[0], tmp_towerID[1], tmp_towerID[2]);
+//if(m_HFclus->getHalfClusterCol("CousinCluster")[ics]->getTowerID().size()!=1) cout<<"ERROR: cousin cluster covers >1 towers. Check here! "<<endl;
+
+        if( map_2DCluster.find( tmp_towerID )==map_2DCluster.end() )
+          tmp_delClus.push_back( m_HFclus->getHalfClusterCol("CousinCluster")[ics] );
+      }
+//cout<<"Need to delete "<<tmp_delClus.size()<<" cousins"<<endl;
+      for(int ics=0; ics<tmp_delClus.size(); ics++) m_HFclus->deleteCousinCluster( tmp_delClus[ics] );
+    }
+
+    //Convert to const
+    std::vector<const PandoraPlus::CaloHalfCluster*> const_HFClusU; const_HFClusU.clear();
+    std::vector<const PandoraPlus::CaloHalfCluster*> const_HFClusV; const_HFClusV.clear();
+    for(int ics=0; ics<m_HFClusUInTower.size(); ics++){ m_HFClusUInTower[ics]->getLinkedMCPfromUnit(); const_HFClusU.push_back(m_HFClusUInTower[ics]); }
+    for(int ics=0; ics<m_HFClusVInTower.size(); ics++){ m_HFClusVInTower[ics]->getLinkedMCPfromUnit(); const_HFClusV.push_back(m_HFClusVInTower[ics]); }
+
+    std::shared_ptr<PandoraPlus::Calo3DCluster> m_tower = std::make_shared<PandoraPlus::Calo3DCluster>();
+//printf("Creating tower: [%d, %d, %d] \n", m_towerID[0], m_towerID[1], m_towerID[2]);
+    m_tower->addTowerID( m_towerID );
+    for(int i2d=0; i2d<map_2DCluster[m_towerID].size(); i2d++) m_tower->addUnit(map_2DCluster[m_towerID][i2d]);
+    m_tower->setHalfClusters( settings.map_stringPars["OutputClusName"]+"U", const_HFClusU,
+                              settings.map_stringPars["OutputClusName"]+"V", const_HFClusV );
+    m_towers.push_back(m_tower);
+  }
+
+  return StatusCode::SUCCESS;
+}
+
+
+
+
+/*
+StatusCode TruthEnergySplittingAlg::HalfClusterToTowers( std::vector<std::shared_ptr<PandoraPlus::CaloHalfCluster>>& m_halfClusU,
+                                                         std::vector<std::shared_ptr<PandoraPlus::CaloHalfCluster>>& m_halfClusV,
+                                                         std::vector<std::shared_ptr<PandoraPlus::Calo3DCluster>>& m_towers ){
+  m_towers.clear();
+
+  std::map<std::vector<int>, std::vector<const PandoraPlus::CaloUnit*>> map_barCol; map_barCol.clear();
+  for(int il=0; il<m_halfClusU.size(); il++){
+    for(int is=0; is<m_halfClusU[il]->getCluster().size(); is++){
+      const PandoraPlus::Calo1DCluster* p_shower = m_halfClusU[il]->getCluster()[is];
+      for(int ibar=0; ibar<p_shower->getBars().size(); ibar++){
+        std::vector<int> barID(3);
+        barID[0] = p_shower->getBars()[ibar]->getModule();
+        barID[1] = p_shower->getBars()[ibar]->getPart();
+        barID[2] = p_shower->getBars()[ibar]->getStave();
+        map_barCol[barID].push_back(p_shower->getBars()[ibar]);
+      }
+    }
+  }
+  for(int il=0; il<m_halfClusV.size(); il++){
+    for(int is=0; is<m_halfClusV[il]->getCluster().size(); is++){
+      const PandoraPlus::Calo1DCluster* p_shower = m_halfClusV[il]->getCluster()[is];
+      for(int ibar=0; ibar<p_shower->getBars().size(); ibar++){
+        std::vector<int> barID(3);
+        barID[0] = p_shower->getBars()[ibar]->getModule();
+        barID[1] = p_shower->getBars()[ibar]->getPart();
+        barID[2] = p_shower->getBars()[ibar]->getStave();
+        map_barCol[barID].push_back(p_shower->getBars()[ibar]);
+      }
+    }
+  }
+
+  //Re-build the objects in tower
+  for(auto& itower: map_barCol){
+    std::vector<std::shared_ptr<PandoraPlus::CaloHalfCluster>> m_newHFClusUCol;
+    std::vector<std::shared_ptr<PandoraPlus::CaloHalfCluster>> m_newHFClusVCol;
+    std::vector<std::shared_ptr<PandoraPlus::Calo2DCluster>> m_new2DClusCol;
+
+    //Get map for MCP-bars
+    std::map<edm4hep::MCParticle, std::vector<const PandoraPlus::CaloUnit*> > map_matchBar;
+    for(int ibar=0; ibar<itower.second.size(); ibar++){
+      edm4hep::MCParticle mcp = itower.second[ibar]->getLeadingMCP();
+      map_matchBar[mcp].push_back(itower.second[ibar]);
+    }
+
+    //Build objects for MCP
+    for(auto& iter: map_matchBar){
+//cout<<"    Print bar info"<<endl;
+//for(int ibar=0; ibar<iter.second.size(); ibar++){
+//printf("    cellID (%d, %d, %d, %d, %d, %d), En %.3f \n", iter.second[ibar]->getModule(), iter.second[ibar]->getPart(), iter.second[ibar]->getStave(), iter.second[ibar]->getDlayer(), iter.second[ibar]->getSlayer(), iter.second[ibar]->getBar(), iter.second[ibar]->getEnergy());
+//}
+      std::vector<std::shared_ptr<PandoraPlus::Calo1DCluster>> m_new1DClusUCol;
+      std::vector<std::shared_ptr<PandoraPlus::Calo1DCluster>> m_new1DClusVCol;
+
+      //Ordered bars
+      std::map<int, std::vector<const PandoraPlus::CaloUnit*> > m_orderedBars; m_orderedBars.clear();
+      for(int ibar=0; ibar<iter.second.size(); ibar++)
+        m_orderedBars[iter.second[ibar]->getDlayer()].push_back(iter.second[ibar]);
+      //1D&2D cluster
+      for(auto& iter_bar: m_orderedBars){
+//cout<<"    In layer #"<<iter_bar.first<<": bar size "<<iter_bar.second.size()<<endl;
+
+        std::shared_ptr<PandoraPlus::Calo1DCluster> tmp_new1dclusterU = std::make_shared<PandoraPlus::Calo1DCluster>();
+        std::shared_ptr<PandoraPlus::Calo1DCluster> tmp_new1dclusterV = std::make_shared<PandoraPlus::Calo1DCluster>();
+        for(int ibar=0; ibar<iter_bar.second.size(); ibar++){
+          if(iter_bar.second[ibar]->getSlayer()==0) tmp_new1dclusterU->addUnit( iter_bar.second[ibar] );
+          if(iter_bar.second[ibar]->getSlayer()==1) tmp_new1dclusterV->addUnit( iter_bar.second[ibar] );
+        }
+        if(tmp_new1dclusterU && tmp_new1dclusterU->getBars().size()>0){
+          tmp_new1dclusterU->getLinkedMCPfromUnit();
+          tmp_new1dclusterU->setSeed();
+          tmp_new1dclusterU->setIDInfo();
+          m_new1DClusUCol.push_back(tmp_new1dclusterU);
+          m_bkCol.map_1DCluster["bk1DCluster"].push_back(tmp_new1dclusterU);
+        }
+        if(tmp_new1dclusterV && tmp_new1dclusterV->getBars().size()>0){
+          tmp_new1dclusterV->getLinkedMCPfromUnit();
+          tmp_new1dclusterV->setSeed();
+          tmp_new1dclusterV->setIDInfo();
+          m_new1DClusVCol.push_back(tmp_new1dclusterV);
+          m_bkCol.map_1DCluster["bk1DCluster"].push_back(tmp_new1dclusterV);
+        }
+        if(tmp_new1dclusterU && tmp_new1dclusterV && tmp_new1dclusterU->getBars().size()>0 && tmp_new1dclusterV->getBars().size()>0){
+          std::shared_ptr<PandoraPlus::Calo2DCluster> tmp_block = std::make_shared<PandoraPlus::Calo2DCluster>();
+          for(int ibar=0; ibar<iter_bar.second.size(); ibar++) tmp_block->addBar(iter_bar.second[ibar]);
+          tmp_block->addUnit(tmp_new1dclusterU.get());
+          tmp_block->addUnit(tmp_new1dclusterV.get());
+          tmp_block->setTowerID( itower.first );
+          m_new2DClusCol.push_back(tmp_block);
+          m_bkCol.map_2DCluster["bk2DCluster"].push_back(tmp_block);
+        }
+      }
+//cout<<"    1DClusU size "<<m_new1DClusUCol.size()<<", 1DClusV size "<<m_new1DClusVCol.size();
+//cout<<", 2DClus size "<<m_new2DClusCol.size()<<endl;
+      //Half cluster
+      std::shared_ptr<PandoraPlus::CaloHalfCluster> m_newHFClusterU = std::make_shared<PandoraPlus::CaloHalfCluster>();
+      std::shared_ptr<PandoraPlus::CaloHalfCluster> m_newHFClusterV = std::make_shared<PandoraPlus::CaloHalfCluster>();
+      for(int i1d=0; i1d<m_new1DClusUCol.size(); i1d++)
+        m_newHFClusterU->addUnit(m_new1DClusUCol[i1d].get());
+      for(int i1d=0; i1d<m_new1DClusVCol.size(); i1d++)
+        m_newHFClusterV->addUnit(m_new1DClusVCol[i1d].get());
+
+      m_newHFClusterU->getLinkedMCPfromUnit();
+      m_newHFClusterV->getLinkedMCPfromUnit();
+      m_newHFClusUCol.push_back(m_newHFClusterU);
+      m_newHFClusVCol.push_back(m_newHFClusterV);
+      m_bkCol.map_HalfCluster["bkHalfCluster"].push_back(m_newHFClusterU);
+      m_bkCol.map_HalfCluster["bkHalfCluster"].push_back(m_newHFClusterV);
+    }
+
+    //Form a tower
+    std::vector<const PandoraPlus::CaloHalfCluster*> const_HFClusU; const_HFClusU.clear();
+    std::vector<const PandoraPlus::CaloHalfCluster*> const_HFClusV; const_HFClusV.clear();
+    for(int ics=0; ics<m_newHFClusUCol.size(); ics++){ const_HFClusU.push_back(m_newHFClusUCol[ics].get()); }
+    for(int ics=0; ics<m_newHFClusVCol.size(); ics++){ const_HFClusV.push_back(m_newHFClusVCol[ics].get()); }
+
+    std::shared_ptr<PandoraPlus::Calo3DCluster> m_tower = std::make_shared<PandoraPlus::Calo3DCluster>();
+    m_tower->addTowerID(itower.first);
+    for(int i2d=0; i2d<m_new2DClusCol.size(); i2d++) m_tower->addUnit(m_new2DClusCol[i2d].get());
+    m_tower->setHalfClusters( settings.map_stringPars["OutputClusName"]+"U", const_HFClusU,
+                              settings.map_stringPars["OutputClusName"]+"V", const_HFClusV );
+    m_towers.push_back(m_tower);
+  }
+
+  return StatusCode::SUCCESS;
+}
+*/
+
+#endif
diff --git a/Reconstruction/CrystalCaloRec/src/Algorithm/TruthMatchingAlg.cpp b/Reconstruction/CrystalCaloRec/src/Algorithm/TruthMatchingAlg.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ee4f5064a7358204cac5bfc6b4d7d73dac5af873
--- /dev/null
+++ b/Reconstruction/CrystalCaloRec/src/Algorithm/TruthMatchingAlg.cpp
@@ -0,0 +1,444 @@
+#ifndef _TRUTHMATCHING_ALG_C
+#define _TRUTHMATCHING_ALG_C
+
+#include "Algorithm/TruthMatchingAlg.h"
+
+StatusCode TruthMatchingAlg::ReadSettings(Settings& m_settings){
+  settings = m_settings;
+
+  //Initialize parameters
+  if(settings.map_stringPars.find("ReadinHFClusterName")==settings.map_stringPars.end()) settings.map_stringPars["ReadinHFClusterName"] = "TruthESCluster";
+  if(settings.map_stringPars.find("ReadinTowerName")==settings.map_stringPars.end()) settings.map_stringPars["ReadinTowerName"] = "TruthESTower";
+  if(settings.map_stringPars.find("OutputClusterName")==settings.map_stringPars.end()) settings.map_stringPars["OutputClusterName"] = "TruthEcalCluster";
+  
+  return StatusCode::SUCCESS;
+};
+
+StatusCode TruthMatchingAlg::Initialize( PandoraPlusDataCol& m_datacol ){
+  std::cout<<"Initialize TruthMatchingAlg"<<std::endl;
+
+  m_HFClusUCol.clear();
+  m_HFClusVCol.clear();
+  m_clusterCol.clear();
+  m_towerCol.clear();
+  m_bkCol.Clear();
+
+  int ntower = m_datacol.map_CaloCluster[settings.map_stringPars["ReadinTowerName"]].size();
+  for(int it=0; it<ntower; it++)
+    m_towerCol.push_back( m_datacol.map_CaloCluster[settings.map_stringPars["ReadinTowerName"]][it].get() );
+
+  return StatusCode::SUCCESS;
+};
+
+StatusCode TruthMatchingAlg::RunAlgorithm( PandoraPlusDataCol& m_datacol ){
+//cout<<"TruthMatchingAlg: readin tower size "<<m_towerCol.size()<<endl;
+
+  for(int it=0; it<m_towerCol.size(); it++){
+
+    std::vector<std::shared_ptr<PandoraPlus::Calo3DCluster>> tmp_clusters; tmp_clusters.clear();
+    m_HFClusUCol = m_towerCol.at(it)->getHalfClusterUCol(settings.map_stringPars["ReadinHFClusterName"]+"U");
+    m_HFClusVCol = m_towerCol.at(it)->getHalfClusterVCol(settings.map_stringPars["ReadinHFClusterName"]+"V");
+//cout<<"  In tower #"<<it<<": HFCluster size "<<m_HFClusUCol.size()<<", "<<m_HFClusVCol.size()<<endl;
+
+
+    TruthMatching(m_HFClusUCol, m_HFClusVCol, tmp_clusters);
+//cout<<"  After matching: 3DCluster size "<<tmp_clusters.size()<<endl;
+    m_clusterCol.insert(m_clusterCol.end(), tmp_clusters.begin(), tmp_clusters.end());
+  }
+
+  //Merge clusters linked to the same MCP.
+  for(int ic=0; ic<m_clusterCol.size() && m_clusterCol.size()>1; ic++){
+    edm4hep::MCParticle mcp1 = m_clusterCol[ic].get()->getLeadingMCP();
+    for(int jc=ic+1; jc<m_clusterCol.size(); jc++){
+      if(ic>m_clusterCol.size()) ic--;
+      edm4hep::MCParticle mcp2 = m_clusterCol[jc].get()->getLeadingMCP();
+      if(mcp1==mcp2){
+        m_clusterCol[ic].get()->mergeCluster( m_clusterCol[jc].get() );
+        m_clusterCol.erase(m_clusterCol.begin()+jc);
+        jc--;
+        if(jc<ic) jc=ic;
+      }
+    }
+  }
+//cout<<"After cluster merging: size "<<m_clusterCol.size()<<endl;
+
+  m_datacol.map_CaloCluster[settings.map_stringPars["OutputClusterName"]] = m_clusterCol;
+
+  m_datacol.map_BarCol["bkBar"].insert( m_datacol.map_BarCol["bkBar"].end(), m_bkCol.map_BarCol["bkBar"].begin(), m_bkCol.map_BarCol["bkBar"].end() );
+  m_datacol.map_1DCluster["bk1DCluster"].insert( m_datacol.map_1DCluster["bk1DCluster"].end(), m_bkCol.map_1DCluster["bk1DCluster"].begin(), m_bkCol.map_1DCluster["bk1DCluster"].end() );
+  m_datacol.map_2DCluster["bk2DCluster"].insert( m_datacol.map_2DCluster["bk2DCluster"].end(), m_bkCol.map_2DCluster["bk2DCluster"].begin(), m_bkCol.map_2DCluster["bk2DCluster"].end() );
+  m_datacol.map_HalfCluster["bkHalfCluster"].insert( m_datacol.map_HalfCluster["bkHalfCluster"].end(), m_bkCol.map_HalfCluster["bkHalfCluster"].begin(), m_bkCol.map_HalfCluster["bkHalfCluster"].end() );
+
+
+
+  return StatusCode::SUCCESS;
+};
+
+StatusCode TruthMatchingAlg::ClearAlgorithm(){
+  std::cout<<"End run TruthMatchingAlg. Clean it."<<std::endl;
+
+  m_HFClusUCol.clear();
+  m_HFClusVCol.clear();
+  m_clusterCol.clear();
+  m_towerCol.clear();
+  m_bkCol.Clear();
+
+  return StatusCode::SUCCESS;
+};
+
+
+StatusCode TruthMatchingAlg::TruthMatching( std::vector<const PandoraPlus::CaloHalfCluster*>& m_ClUCol,
+                                            std::vector<const PandoraPlus::CaloHalfCluster*>& m_ClVCol,
+                                            std::vector<std::shared_ptr<PandoraPlus::Calo3DCluster>>& m_clusters )
+{
+  if(m_ClUCol.size()==0 || m_ClVCol.size()==0) return StatusCode::SUCCESS;
+  std::vector<std::shared_ptr<PandoraPlus::CaloHalfCluster>> m_truthESClUCol; m_truthESClUCol.clear();
+  std::vector<std::shared_ptr<PandoraPlus::CaloHalfCluster>> m_truthESClVCol; m_truthESClVCol.clear();
+
+//cout<<"  Input HFCluster size "<<m_ClUCol.size()<<", "<<m_ClVCol.size()<<endl;
+
+  //Truth split in HFClusterU
+  for(int icl=0; icl<m_ClUCol.size(); icl++){
+    auto truthMap = m_ClUCol[icl]->getLinkedMCP();
+//cout<<"    In HFClusU #"<<icl<<": energy "<<m_ClUCol[icl]->getEnergy()<<", 1Dcluster size "<<m_ClUCol[icl]->getCluster().size()<<", truth link size "<<truthMap.size()<<endl;
+//for(auto& iter: truthMap)
+//  printf("    MC pid %d, weight %.3f \n", iter.first.getPDG(), iter.second);
+
+    if(truthMap.size()==1){
+      auto newClus = m_ClUCol[icl]->Clone();
+      m_truthESClUCol.push_back(newClus);
+      continue;
+    }
+
+    for(auto& iter: truthMap ){
+      if(iter.second<0.05) continue;
+
+      std::shared_ptr<PandoraPlus::CaloHalfCluster> newClus = std::make_shared<PandoraPlus::CaloHalfCluster>();
+      //Copy and reweight 1D showers in HFCluster
+      for(int ish=0; ish<m_ClUCol[icl]->getCluster().size(); ish++){
+        const PandoraPlus::Calo1DCluster* p_shower = m_ClUCol[icl]->getCluster()[ish];
+
+        std::vector<const PandoraPlus::CaloUnit*> Bars; Bars.clear();
+        for(int ibar=0; ibar<p_shower->getCluster().size(); ibar++){
+          auto bar = p_shower->getCluster()[ibar]->Clone();
+          bar->setQ(bar->getQ1()*iter.second, bar->getQ2()*iter.second );
+
+          bar->setLinkedMCP(std::vector<std::pair<edm4hep::MCParticle, float>>{std::make_pair(iter.first, 1.)});
+          Bars.push_back(bar.get());
+          m_bkCol.map_BarCol["bkBar"].push_back(bar);
+        }
+
+        std::shared_ptr<PandoraPlus::Calo1DCluster> shower = std::make_shared<PandoraPlus::Calo1DCluster>();
+        shower->setBars(Bars);
+        shower->setSeed();
+        shower->setIDInfo();
+        shower->getLinkedMCPfromUnit();
+        newClus->addUnit(shower.get());
+        m_bkCol.map_1DCluster["bk1DCluster"].push_back( shower );
+      }
+      for(int itrk=0; itrk<m_ClUCol[icl]->getAssociatedTracks().size(); itrk++)  newClus->addAssociatedTrack( m_ClUCol[icl]->getAssociatedTracks()[itrk] );
+      newClus->setHoughPars( m_ClUCol[icl]->getHoughAlpha(), m_ClUCol[icl]->getHoughRho() );
+      newClus->setIntercept( m_ClUCol[icl]->getHoughIntercept() );
+      newClus->fitAxis("");
+      newClus->getLinkedMCPfromUnit();
+      newClus->mergeClusterInLayer();
+      m_truthESClUCol.push_back(newClus);
+    }
+  }
+//cout<<"    New HFClusterU size "<<m_truthESClUCol.size()<<endl;
+
+  //Merge HFClusters linked to the same MCP
+  for(int icl=0; icl<m_truthESClUCol.size() && m_truthESClUCol.size()>1; icl++){
+    for(int jcl=icl+1; jcl<m_truthESClUCol.size(); jcl++){
+      if(icl>m_truthESClUCol.size()) icl--;
+
+      if(m_truthESClUCol[icl]->getLeadingMCP()==m_truthESClUCol[jcl]->getLeadingMCP()){
+        m_truthESClUCol[icl].get()->mergeHalfCluster(m_truthESClUCol[jcl].get());
+        m_truthESClUCol.erase(m_truthESClUCol.begin()+jcl);
+        jcl--;
+        if(jcl<icl) jcl=icl;
+      }
+    }
+  }
+//cout<<"    HFClusterU size after merge "<<m_truthESClUCol.size()<<endl;
+  for(int icl=0; icl<m_truthESClUCol.size(); icl++) m_bkCol.map_HalfCluster["bkHalfCluster"].push_back(m_truthESClUCol[icl]);
+
+
+  //Truth split in HFClusterV
+  for(int icl=0; icl<m_ClVCol.size(); icl++){
+    auto truthMap = m_ClVCol[icl]->getLinkedMCP();
+//cout<<"    In HFClusV #"<<icl<<": energy "<<m_ClVCol[icl]->getEnergy()<<", 1Dcluster size "<<m_ClVCol[icl]->getCluster().size()<<", truth link size "<<truthMap.size()<<endl;
+//for(auto& iter: truthMap)
+//  printf("    MC pid %d, weight %.3f \n", iter.first.getPDG(), iter.second);
+
+    if(truthMap.size()==1){
+      auto newClus = m_ClVCol[icl]->Clone();
+      m_truthESClVCol.push_back(newClus);
+      continue;
+    }
+
+    for(auto iter: truthMap ){
+      if(iter.second<0.05) continue;
+
+      std::shared_ptr<PandoraPlus::CaloHalfCluster> newClus = std::make_shared<PandoraPlus::CaloHalfCluster>();
+      //Copy and reweight 1D showers in HFCluster
+      for(int ish=0; ish<m_ClVCol[icl]->getCluster().size(); ish++){
+        const PandoraPlus::Calo1DCluster* p_shower = m_ClVCol[icl]->getCluster()[ish];
+
+        std::vector<const PandoraPlus::CaloUnit*> Bars; Bars.clear();
+        for(int ibar=0; ibar<p_shower->getCluster().size(); ibar++){
+          auto bar = p_shower->getCluster()[ibar]->Clone();
+          bar->setQ(bar->getQ1()*iter.second, bar->getQ2()*iter.second );
+
+          bar->setLinkedMCP(std::vector<std::pair<edm4hep::MCParticle, float>>{std::make_pair(iter.first, 1.)});
+          Bars.push_back(bar.get());
+          m_bkCol.map_BarCol["bkBar"].push_back(bar);
+        }
+        std::shared_ptr<PandoraPlus::Calo1DCluster> shower = std::make_shared<PandoraPlus::Calo1DCluster>();
+        shower->setBars(Bars);
+        shower->setSeed();
+        shower->setIDInfo();
+        shower->getLinkedMCPfromUnit();
+        newClus->addUnit(shower.get());
+        m_bkCol.map_1DCluster["bk1DCluster"].push_back( shower );
+      }
+      for(int itrk=0; itrk<m_ClVCol[icl]->getAssociatedTracks().size(); itrk++)  newClus->addAssociatedTrack( m_ClVCol[icl]->getAssociatedTracks()[itrk] );
+      newClus->setHoughPars( m_ClVCol[icl]->getHoughAlpha(), m_ClVCol[icl]->getHoughRho() );
+      newClus->setIntercept( m_ClVCol[icl]->getHoughIntercept() );
+      newClus->fitAxis("");
+      newClus->getLinkedMCPfromUnit();
+      newClus->mergeClusterInLayer();
+      m_truthESClVCol.push_back(newClus);
+    }
+  }
+//cout<<"    New HFClusterV size "<<m_truthESClVCol.size()<<endl;
+
+  //Merge HFClusters linked to the same MCP
+  for(int icl=0; icl<m_truthESClVCol.size() && m_truthESClVCol.size()>1; icl++){
+    for(int jcl=icl+1; jcl<m_truthESClVCol.size(); jcl++){
+      if(icl>m_truthESClVCol.size()) icl--;
+
+      if(m_truthESClVCol[icl]->getLeadingMCP()==m_truthESClVCol[jcl]->getLeadingMCP()){
+        m_truthESClVCol[icl].get()->mergeHalfCluster(m_truthESClVCol[jcl].get());
+        m_truthESClVCol.erase(m_truthESClVCol.begin()+jcl);
+        jcl--;
+        if(jcl<icl) jcl=icl;
+      }
+    }
+  }
+  for(int icl=0; icl<m_truthESClVCol.size(); icl++) m_bkCol.map_HalfCluster["bkHalfCluster"].push_back(m_truthESClVCol[icl]);
+//cout<<"    HFClusterV size after merge "<<m_truthESClVCol.size()<<endl;
+
+
+  //Doing matching.
+  for(int icl=0; icl<m_truthESClUCol.size(); icl++){
+    for(int jcl=0; jcl<m_truthESClVCol.size(); jcl++){
+      if(m_truthESClUCol[icl]->getLeadingMCP() == m_truthESClVCol[jcl]->getLeadingMCP()){
+        std::shared_ptr<PandoraPlus::Calo3DCluster> tmp_clus = std::make_shared<PandoraPlus::Calo3DCluster>();
+        XYClusterMatchingL0(m_truthESClUCol[icl].get(), m_truthESClVCol[jcl].get(), tmp_clus);
+        m_clusters.push_back(tmp_clus);
+      }
+    }
+  }
+
+  return StatusCode::SUCCESS;
+}
+
+
+//Longitudinal cluster: 1*1
+StatusCode TruthMatchingAlg::XYClusterMatchingL0( const PandoraPlus::CaloHalfCluster* m_longiClU,
+                                                  const PandoraPlus::CaloHalfCluster* m_longiClV,
+                                                  std::shared_ptr<PandoraPlus::Calo3DCluster>& m_clus )
+{
+/*
+cout<<"  XYClusterMatchingL0: print input HFClusterU. ";
+printf("cluster size %d, linked MC pid %d, track size %d, Cover tower %d: ", m_longiClU->getCluster().size(), m_longiClU->getLeadingMCP().getPDG(), m_longiClU->getAssociatedTracks().size(), m_longiClU->getTowerID().size());
+for(int it=0; it<m_longiClU->getTowerID().size(); it++) printf(" [%d, %d, %d] ", m_longiClU->getTowerID()[it][0], m_longiClU->getTowerID()[it][1], m_longiClU->getTowerID()[it][2]);
+cout<<endl;
+for(int ic=0; ic<m_longiClU->getCluster().size(); ic++){
+  printf("    Cluster #%d: towerID [%d, %d, %d], layer %d, pos+E (%.3f, %.3f, %.3f, %.3f), bar size %d, seed size %d, MCP link size %d: ", 
+    ic, m_longiClU->getCluster()[ic]->getTowerID()[0][0], m_longiClU->getCluster()[ic]->getTowerID()[0][1], m_longiClU->getCluster()[ic]->getTowerID()[0][2], m_longiClU->getCluster()[ic]->getDlayer(), 
+    m_longiClU->getCluster()[ic]->getPos().x(), m_longiClU->getCluster()[ic]->getPos().y(), m_longiClU->getCluster()[ic]->getPos().z(),  m_longiClU->getCluster()[ic]->getEnergy(), 
+    m_longiClU->getCluster()[ic]->getBars().size(), m_longiClU->getCluster()[ic]->getNseeds(), m_longiClU->getCluster()[ic]->getLinkedMCP().size() );
+  for(int imc=0; imc<m_longiClU->getCluster()[ic]->getLinkedMCP().size(); imc++) cout<<m_longiClU->getCluster()[ic]->getLinkedMCP()[imc].first.getPDG()<<", ";
+  cout<<endl;
+}
+cout<<"  XYClusterMatchingL0: print input HFClusterV. ";
+printf("cluster size %d, linked MC pid %d, track size %d, Cover tower %d: ", m_longiClV->getCluster().size(), m_longiClV->getLeadingMCP().getPDG(),  m_longiClV->getAssociatedTracks().size(), m_longiClV->getTowerID().size());
+for(int it=0; it<m_longiClV->getTowerID().size(); it++) printf(" [%d, %d, %d] ", m_longiClV->getTowerID()[it][0], m_longiClV->getTowerID()[it][1], m_longiClV->getTowerID()[it][2]);
+cout<<endl;
+for(int ic=0; ic<m_longiClV->getCluster().size(); ic++){
+  printf("    Cluster #%d: towerID [%d, %d, %d], layer %d, pos+E (%.3f, %.3f, %.3f, %.3f), bar size %d, seed size %d, MCP link size %d: ", 
+    ic, m_longiClV->getCluster()[ic]->getTowerID()[0][0], m_longiClV->getCluster()[ic]->getTowerID()[0][1], m_longiClV->getCluster()[ic]->getTowerID()[0][2], m_longiClV->getCluster()[ic]->getDlayer(), 
+    m_longiClV->getCluster()[ic]->getPos().x(), m_longiClV->getCluster()[ic]->getPos().y(), m_longiClV->getCluster()[ic]->getPos().z(), m_longiClV->getCluster()[ic]->getEnergy(), 
+    m_longiClV->getCluster()[ic]->getBars().size(), m_longiClV->getCluster()[ic]->getNseeds(), m_longiClV->getCluster()[ic]->getLinkedMCP().size() );
+  for(int imc=0; imc<m_longiClV->getCluster()[ic]->getLinkedMCP().size(); imc++) cout<<m_longiClV->getCluster()[ic]->getLinkedMCP()[imc].first.getPDG()<<", ";
+  cout<<endl;
+}
+cout<<endl;
+*/
+
+  std::vector<int> layerindex; layerindex.clear();
+  std::map<int, std::vector<const PandoraPlus::Calo1DCluster*> > map_showersUinlayer; map_showersUinlayer.clear();
+  std::map<int, std::vector<const PandoraPlus::Calo1DCluster*> > map_showersVinlayer; map_showersVinlayer.clear();
+
+  for(int is=0; is<m_longiClU->getCluster().size(); is++){
+    int m_layer = m_longiClU->getCluster()[is]->getDlayer();
+    if( find( layerindex.begin(), layerindex.end(), m_layer )==layerindex.end() ) layerindex.push_back(m_layer);
+    map_showersUinlayer[m_layer].push_back(m_longiClU->getCluster()[is]);
+  }
+  for(int is=0; is<m_longiClV->getCluster().size(); is++){
+    int m_layer = m_longiClV->getCluster()[is]->getDlayer();
+    if( find( layerindex.begin(), layerindex.end(), m_layer )==layerindex.end() ) layerindex.push_back(m_layer);
+    map_showersVinlayer[m_layer].push_back(m_longiClV->getCluster()[is]);
+  }
+
+//cout<<"    in XYClusterMatchingL0: "<<layerindex.size()<<" layer need matching"<<endl;
+
+  for(int il=0; il<layerindex.size(); il++){
+    std::vector<const PandoraPlus::Calo1DCluster*> m_showerXcol = map_showersUinlayer[layerindex[il]];
+    std::vector<const PandoraPlus::Calo1DCluster*> m_showerYcol = map_showersVinlayer[layerindex[il]];
+
+    std::vector<PandoraPlus::Calo2DCluster*> m_showerinlayer; m_showerinlayer.clear();
+
+//cout<<"    in layer "<<layerindex[il]<<": 1D shower size ("<<m_showerXcol.size()<<", "<<m_showerYcol.size()<<"). "<<endl;
+    if(m_showerXcol.size()==0 || m_showerYcol.size()==0) continue;
+    //else if(m_showerXcol.size()==0){
+    //  std::shared_ptr<PandoraPlus::Calo2DCluster> tmp_shower = std::make_shared<PandoraPlus::Calo2DCluster>();
+    //  GetMatchedShowerFromEmpty(m_showerYcol[0], m_longiClU, tmp_shower.get());
+    //  //m_showerinlayer.push_back(tmp_shower.get());
+    //  //m_bkCol.map_2DCluster["bk2DCluster"].push_back(tmp_shower);
+    //}
+    //else if(m_showerYcol.size()==0){
+    //  std::shared_ptr<PandoraPlus::Calo2DCluster> tmp_shower = std::make_shared<PandoraPlus::Calo2DCluster>();
+    //  GetMatchedShowerFromEmpty(m_showerXcol[0], m_longiClU, tmp_shower.get());
+    //}
+    else if(m_showerXcol.size()==1 && m_showerYcol.size()==1){
+      std::shared_ptr<PandoraPlus::Calo2DCluster> tmp_shower = std::make_shared<PandoraPlus::Calo2DCluster>();
+      GetMatchedShowersL0(m_showerXcol[0], m_showerYcol[0], tmp_shower.get());
+      m_showerinlayer.push_back(tmp_shower.get());
+      m_bkCol.map_2DCluster["bk2DCluster"].push_back(tmp_shower);
+    }
+    else if(m_showerXcol.size()==1) GetMatchedShowersL1(m_showerXcol[0], m_showerYcol, m_showerinlayer );
+    else if(m_showerYcol.size()==1) GetMatchedShowersL1(m_showerYcol[0], m_showerXcol, m_showerinlayer );
+    else{ std::cout<<"CAUSION in XYClusterMatchingL0: HFCluster has ["<<m_showerXcol.size()<<", "<<m_showerYcol.size()<<"] showers in layer "<<layerindex[il]<<std::endl; }
+
+    for(int is=0; is<m_showerinlayer.size(); is++) m_clus->addUnit(m_showerinlayer[is]);
+//cout<<"    after matching: 2D shower size "<<m_showerinlayer.size()<<endl;
+  }
+
+
+  m_clus->addHalfClusterU( "LinkedLongiCluster", m_longiClU );
+  m_clus->addHalfClusterV( "LinkedLongiCluster", m_longiClV );
+  for(auto itrk : m_longiClU->getAssociatedTracks()){
+    for(auto jtrk : m_longiClV->getAssociatedTracks()){
+      if(itrk!=jtrk) continue;
+      if( find(m_clus->getAssociatedTracks().begin(), m_clus->getAssociatedTracks().end(), itrk)==m_clus->getAssociatedTracks().end() )
+        m_clus->addAssociatedTrack(itrk);
+  }}
+  m_clus->getLinkedMCPfromHFCluster("LinkedLongiCluster");
+
+  return StatusCode::SUCCESS;
+}
+
+
+StatusCode TruthMatchingAlg::GetMatchedShowersL0( const PandoraPlus::Calo1DCluster* barShowerU,
+                                                  const PandoraPlus::Calo1DCluster* barShowerV,
+                                                  PandoraPlus::Calo2DCluster* outsh )
+{
+//cout<<"  In GetMatchedShowersL0"<<endl;
+
+  std::vector<const PandoraPlus::CaloHit*> m_digiCol; m_digiCol.clear();
+  int NbarsX = barShowerU->getBars().size();
+  int NbarsY = barShowerV->getBars().size();
+  if(NbarsX==0 || NbarsY==0){ std::cout<<"WARNING: empty DigiHitsCol returned!"<<std::endl; return StatusCode::SUCCESS; }
+  if(barShowerU->getTowerID().size()==0) { std::cout<<"WARNING:GetMatchedShowersL0  No TowerID in 1DCluster!"<<std::endl; return StatusCode::SUCCESS; }
+  //if(barShowerU->getTowerID().size()==0) { barShowerU->setIDInfo(); }
+
+  int _module = barShowerU->getTowerID()[0][0];
+  float rotAngle = -_module*TMath::TwoPi()/PandoraPlus::CaloUnit::Nmodule;
+
+  for(int ibar=0;ibar<NbarsX;ibar++){
+    double En_x = barShowerU->getBars()[ibar]->getEnergy();
+    TVector3 m_vecx = barShowerU->getBars()[ibar]->getPosition();
+    m_vecx.RotateZ(rotAngle);
+
+    for(int jbar=0;jbar<NbarsY;jbar++){
+      double En_y = barShowerV->getBars()[jbar]->getEnergy();
+      TVector3 m_vecy = barShowerV->getBars()[jbar]->getPosition();
+      m_vecy.RotateZ(rotAngle);
+
+      TVector3 p_hit(m_vecy.x(), (m_vecx.y()+m_vecy.y())/2., m_vecx.z() );
+      p_hit.RotateZ(-rotAngle);
+      double m_Ehit = En_x*En_y/barShowerV->getEnergy() + En_x*En_y/barShowerU->getEnergy();
+      //Create new CaloHit
+      std::shared_ptr<PandoraPlus::CaloHit> hit = std::make_shared<PandoraPlus::CaloHit>();
+      //hit.setCellID(0);
+      hit->setPosition(p_hit);
+      hit->setEnergy(m_Ehit);
+      m_digiCol.push_back(hit.get());
+      m_bkCol.map_CaloHit["bkHit"].push_back( hit );
+    }
+  }
+
+  outsh->addUnit( barShowerU );
+  outsh->addUnit( barShowerV );
+  outsh->setCaloHits( m_digiCol );
+//cout<<"    End output shower"<<endl;
+
+  return StatusCode::SUCCESS;
+}
+
+
+StatusCode TruthMatchingAlg::GetMatchedShowersL1( const PandoraPlus::Calo1DCluster* shower1,
+                                                       std::vector<const PandoraPlus::Calo1DCluster*>& showerNCol,
+                                                       std::vector<PandoraPlus::Calo2DCluster*>& outshCol )
+{
+//cout<<"  GetMatchedShowersL1: input shower size: 1 * "<<showerNCol.size()<<endl;
+  outshCol.clear();
+
+  int _slayer = shower1->getBars()[0]->getSlayer();
+
+  const int NshY = showerNCol.size();
+  double totE_shY = 0;
+  double EshY[NshY] = {0};
+  for(int is=0;is<NshY;is++){ EshY[is] = showerNCol[is]->getEnergy(); totE_shY += EshY[is]; }
+  for(int is=0;is<NshY;is++){
+    double wi_E = EshY[is]/totE_shY;
+    std::shared_ptr<PandoraPlus::Calo1DCluster> m_splitshower1 = std::make_shared<PandoraPlus::Calo1DCluster>();
+    m_bkCol.map_1DCluster["bk1DCluster"].push_back( m_splitshower1 );
+
+    std::shared_ptr<PandoraPlus::CaloUnit> m_wiseed = nullptr;
+    if(shower1->getSeeds().size()>0) m_wiseed = shower1->getSeeds()[0]->Clone();
+    else{ cout<<"ERROR: Input shower has no seed! Check! Use the most energitic bar as seed. bar size: "<<shower1->getBars().size()<<endl;
+      double m_maxE = -99;
+      int index = -1;
+      for(int ib=0; ib<shower1->getBars().size(); ib++){
+        if(shower1->getBars()[ib]->getEnergy()>m_maxE) { m_maxE=shower1->getBars()[ib]->getEnergy(); index=ib; }
+      }
+      if(index>=0) m_wiseed = shower1->getBars()[index]->Clone();
+    }
+    m_wiseed->setQ( wi_E*m_wiseed->getQ1(), wi_E*m_wiseed->getQ2() );
+    m_bkCol.map_BarCol["bkBar"].push_back( m_wiseed );
+
+    std::vector<const PandoraPlus::CaloUnit*> m_wibars; m_wibars.clear();
+    for(int ib=0;ib<shower1->getBars().size();ib++){
+      std::shared_ptr<PandoraPlus::CaloUnit> m_wibar = shower1->getBars()[ib]->Clone();
+      m_wibar->setQ(wi_E*m_wibar->getQ1(), wi_E*m_wibar->getQ2());
+      m_wibars.push_back(m_wibar.get());
+      m_bkCol.map_BarCol["bkBar"].push_back( m_wibar );
+    }
+    m_splitshower1->setBars( m_wibars );
+    m_splitshower1->addSeed( m_wiseed.get() );
+    m_splitshower1->setIDInfo();
+    std::shared_ptr<PandoraPlus::Calo2DCluster> m_shower = std::make_shared<PandoraPlus::Calo2DCluster>();
+    if(_slayer==0 ) GetMatchedShowersL0( m_splitshower1.get(), showerNCol[is], m_shower.get() );
+    else            GetMatchedShowersL0( showerNCol[is], m_splitshower1.get(), m_shower.get() );
+
+    outshCol.push_back( m_shower.get() );
+    m_bkCol.map_2DCluster["bk2DCluster"].push_back( m_shower );
+  }
+  return StatusCode::SUCCESS;
+}
+
+#endif
diff --git a/Reconstruction/CrystalCaloRec/src/Algorithm/TruthPatternRecAlg.cpp b/Reconstruction/CrystalCaloRec/src/Algorithm/TruthPatternRecAlg.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6e13132cccf11d8e92cda0a2d4f7eb5f0f36a8bf
--- /dev/null
+++ b/Reconstruction/CrystalCaloRec/src/Algorithm/TruthPatternRecAlg.cpp
@@ -0,0 +1,213 @@
+#ifndef _TRUTHPATTERNREC_ALG_C
+#define _TRUTHPATTERNREC_ALG_C
+
+#include "Algorithm/TruthPatternRecAlg.h"
+
+StatusCode TruthPatternRecAlg::ReadSettings(Settings& m_settings){
+  settings = m_settings;
+
+  //Initialize parameters
+  if(settings.map_stringPars.find("ReadinLocalMaxName")==settings.map_stringPars.end())
+    settings.map_stringPars["ReadinLocalMaxName"] = "AllLocalMax";
+  if(settings.map_stringPars.find("OutputLongiClusName")==settings.map_stringPars.end())
+    settings.map_stringPars["OutputLongiClusName"] = "TruthAxis";    
+
+  if(settings.map_boolPars.find("DoAxisMerging")==settings.map_boolPars.end())
+    settings.map_boolPars["DoAxisMerging"] = 1;
+  if(settings.map_stringPars.find("ReadinAxisName")==settings.map_stringPars.end())
+    settings.map_stringPars["ReadinAxisName"] = "TruthTrackAxis";
+  if(settings.map_stringPars.find("OutputMergedAxisName")==settings.map_stringPars.end())
+    settings.map_stringPars["OutputMergedAxisName"] = "TruthMergedAxis";
+  if(settings.map_floatPars.find("th_overlap")==settings.map_floatPars.end()) 
+    settings.map_floatPars["th_overlap"] = 0.8;
+
+  return StatusCode::SUCCESS;
+};
+
+StatusCode TruthPatternRecAlg::Initialize( PandoraPlusDataCol& m_datacol ){
+  p_HalfClusterU.clear();
+  p_HalfClusterV.clear();
+
+  for(int ih=0; ih<m_datacol.map_HalfCluster["HalfClusterColU"].size(); ih++)
+    p_HalfClusterU.push_back( m_datacol.map_HalfCluster["HalfClusterColU"][ih].get() );
+  for(int ih=0; ih<m_datacol.map_HalfCluster["HalfClusterColV"].size(); ih++)
+    p_HalfClusterV.push_back( m_datacol.map_HalfCluster["HalfClusterColV"][ih].get() );
+
+  return StatusCode::SUCCESS;
+};
+
+
+StatusCode TruthPatternRecAlg::RunAlgorithm( PandoraPlusDataCol& m_datacol ){
+//cout<<"  TruthPatternRecAlg: Input HFCluster size ("<<p_HalfClusterU.size()<<", "<<p_HalfClusterV.size()<<") "<<endl;
+
+  for(int ihc=0; ihc<p_HalfClusterU.size(); ihc++){
+    std::map<edm4hep::MCParticle, std::vector<const Calo1DCluster*>> TruthAxesMap; 
+
+    std::vector<const PandoraPlus::Calo1DCluster*> tmp_localMaxUCol = p_HalfClusterU[ihc]->getLocalMaxCol(settings.map_stringPars["ReadinLocalMaxName"]);
+    for(int ilm=0; ilm<tmp_localMaxUCol.size(); ilm++){
+      edm4hep::MCParticle mcp_lm = tmp_localMaxUCol[ilm]->getLeadingMCP();
+      TruthAxesMap[mcp_lm].push_back(tmp_localMaxUCol[ilm]);
+    }
+
+//cout<<"    In HFClusterU #"<<ihc<<": localMax size "<<tmp_localMaxUCol.size()<<", print truth axes map"<<endl;
+//for(auto iter : TruthAxesMap)
+//  printf("      MCP id %d: localMax size %d \n", iter.first.getPDG(), iter.second.size());
+
+    //Create axes from MCPMap. 
+    std::vector<std::shared_ptr<PandoraPlus::CaloHalfCluster>> axisCol; axisCol.clear(); 
+    for(auto& iter : TruthAxesMap){
+      if(iter.second.size()==0) continue;
+  
+      std::shared_ptr<PandoraPlus::CaloHalfCluster> t_axis = std::make_shared<PandoraPlus::CaloHalfCluster>();
+      for(int icl=0; icl<iter.second.size(); icl++) t_axis->addUnit(iter.second[icl]);
+      t_axis->setType(100);
+      t_axis->getLinkedMCPfromUnit();
+      axisCol.push_back(t_axis);
+    }
+//cout<<"    Axes size: "<<axisCol.size()<<endl;
+
+    for(int iax=0; iax<axisCol.size(); iax++) p_HalfClusterU[ihc]->addHalfCluster(settings.map_stringPars["OutputLongiClusName"], axisCol[iax].get());
+    m_datacol.map_HalfCluster["bkHalfCluster"].insert(m_datacol.map_HalfCluster["bkHalfCluster"].end(), axisCol.begin(), axisCol.end() );
+  }
+
+
+  //Get the linked axes in V
+  for(int ihc=0; ihc<p_HalfClusterV.size(); ihc++){
+    std::map<edm4hep::MCParticle, std::vector<const Calo1DCluster*>> TruthAxesMap; 
+
+    std::vector<const PandoraPlus::Calo1DCluster*> tmp_localMaxVCol = p_HalfClusterV[ihc]->getLocalMaxCol(settings.map_stringPars["ReadinLocalMaxName"]);
+    for(int ilm=0; ilm<tmp_localMaxVCol.size(); ilm++){
+      edm4hep::MCParticle mcp_lm = tmp_localMaxVCol[ilm]->getLeadingMCP();
+      TruthAxesMap[mcp_lm].push_back(tmp_localMaxVCol[ilm]);
+    }
+
+//cout<<"    In HFClusterV #"<<ihc<<": localMax size "<<tmp_localMaxVCol.size()<<", print truth axes map"<<endl;
+//for(auto iter : TruthAxesMap)
+//  printf("      MCP id %d: localMax size %d \n", iter.first.getPDG(), iter.second.size());
+
+    //Create axes from MCPMap.
+    std::vector<std::shared_ptr<PandoraPlus::CaloHalfCluster>> axisCol; axisCol.clear(); 
+    for(auto& iter : TruthAxesMap){
+      if(iter.second.size()==0) continue;
+   
+      std::shared_ptr<PandoraPlus::CaloHalfCluster> t_axis = std::make_shared<PandoraPlus::CaloHalfCluster>();
+      for(int icl=0; icl<iter.second.size(); icl++) t_axis->addUnit(iter.second[icl]);
+      t_axis->setType(100);
+      t_axis->getLinkedMCPfromUnit();
+      axisCol.push_back(t_axis);
+    }
+
+//cout<<"    Axes size: "<<axisCol.size()<<endl;
+
+    for(int iax=0; iax<axisCol.size(); iax++) p_HalfClusterV[ihc]->addHalfCluster(settings.map_stringPars["OutputLongiClusName"], axisCol[iax].get());
+    m_datacol.map_HalfCluster["bkHalfCluster"].insert(m_datacol.map_HalfCluster["bkHalfCluster"].end(), axisCol.begin(), axisCol.end() );
+  }
+
+  if(settings.map_boolPars["DoAxisMerging"]){
+
+    for(int ihc=0; ihc<p_HalfClusterU.size(); ihc++){
+      std::vector<std::shared_ptr<PandoraPlus::CaloHalfCluster>> m_newAxisCol; m_newAxisCol.clear();
+
+      std::vector<const CaloHalfCluster*> m_recAxis = p_HalfClusterU[ihc]->getHalfClusterCol(settings.map_stringPars["OutputLongiClusName"]);
+      std::vector<const CaloHalfCluster*> m_trkAxis = p_HalfClusterU[ihc]->getHalfClusterCol(settings.map_stringPars["ReadinAxisName"]);
+
+      for(int iax=0; iax<m_recAxis.size(); iax++) m_newAxisCol.push_back(m_recAxis[iax]->Clone());
+      for(int iax=0; iax<m_trkAxis.size(); iax++) m_newAxisCol.push_back(m_trkAxis[iax]->Clone());
+
+//printf("  In HFClusterU #%d: rec axis %d, track axis %d. Print \n", ihc, m_recAxis.size(), m_trkAxis.size());
+//for(int i=0; i<m_recAxis.size(); i++)
+//  printf("    Rec axis %d: localMax size %d, track size %d, linked MC pid %d \n", i, m_recAxis[i]->getCluster().size(), m_recAxis[i]->getAssociatedTracks().size(), m_recAxis[i]->getLeadingMCP().getPDG());
+//for(int i=0; i<m_trkAxis.size(); i++)
+//  printf("    Trk axis %d: localMax size %d, track size %d, linked MC pid %d \n", i, m_trkAxis[i]->getCluster().size(), m_trkAxis[i]->getAssociatedTracks().size(), m_trkAxis[i]->getLeadingMCP().getPDG());
+
+      if(m_newAxisCol.size()>1) OverlapMerging(m_newAxisCol);
+//cout<<"  After merge: axis size "<<m_newAxisCol.size()<<endl;
+//for(int i=0; i<m_newAxisCol.size(); i++)
+//  printf("    Trk axis %d: localMax size %d, track size %d, linked MC pid %d \n", i, m_newAxisCol[i]->getCluster().size(), m_newAxisCol[i]->getAssociatedTracks().size(), m_newAxisCol[i]->getLeadingMCP().getPDG());
+
+      for(int iax=0; iax<m_newAxisCol.size(); iax++) p_HalfClusterU[ihc]->addHalfCluster(settings.map_stringPars["OutputMergedAxisName"], m_newAxisCol[iax].get());
+      m_datacol.map_HalfCluster["bkHalfCluster"].insert(m_datacol.map_HalfCluster["bkHalfCluster"].end(), m_newAxisCol.begin(), m_newAxisCol.end());
+    }
+
+    for(int ihc=0; ihc<p_HalfClusterV.size(); ihc++){
+      std::vector<std::shared_ptr<PandoraPlus::CaloHalfCluster>> m_newAxisCol; m_newAxisCol.clear();
+
+      std::vector<const CaloHalfCluster*> m_recAxis = p_HalfClusterV[ihc]->getHalfClusterCol(settings.map_stringPars["OutputLongiClusName"]);
+      std::vector<const CaloHalfCluster*> m_trkAxis = p_HalfClusterV[ihc]->getHalfClusterCol(settings.map_stringPars["ReadinAxisName"]);
+
+      for(int iax=0; iax<m_recAxis.size(); iax++) m_newAxisCol.push_back(m_recAxis[iax]->Clone());
+      for(int iax=0; iax<m_trkAxis.size(); iax++) m_newAxisCol.push_back(m_trkAxis[iax]->Clone());
+
+
+//printf("  In HFClusterV #%d: rec axis %d, track axis %d. Print \n", ihc, m_recAxis.size(), m_trkAxis.size());
+//for(int i=0; i<m_recAxis.size(); i++)
+//  printf("    Rec axis %d: localMax size %d, track size %d, linked MC pid %d \n", i, m_recAxis[i]->getCluster().size(), m_recAxis[i]->getAssociatedTracks().size(), m_recAxis[i]->getLeadingMCP().getPDG());
+//for(int i=0; i<m_trkAxis.size(); i++)
+//  printf("    Trk axis %d: localMax size %d, track size %d, linked MC pid %d \n", i, m_trkAxis[i]->getCluster().size(), m_trkAxis[i]->getAssociatedTracks().size(), m_trkAxis[i]->getLeadingMCP().getPDG());
+
+      if(m_newAxisCol.size()>1) OverlapMerging(m_newAxisCol);
+
+
+//cout<<"  After merge: axis size "<<m_newAxisCol.size()<<endl;
+//for(int i=0; i<m_newAxisCol.size(); i++)
+//  printf("    Trk axis %d: localMax size %d, track size %d, linked MC pid %d \n", i, m_newAxisCol[i]->getCluster().size(), m_newAxisCol[i]->getAssociatedTracks().size(), m_newAxisCol[i]->getLeadingMCP().getPDG());
+      for(int iax=0; iax<m_newAxisCol.size(); iax++) p_HalfClusterV[ihc]->addHalfCluster(settings.map_stringPars["OutputMergedAxisName"], m_newAxisCol[iax].get());
+      m_datacol.map_HalfCluster["bkHalfCluster"].insert(m_datacol.map_HalfCluster["bkHalfCluster"].end(), m_newAxisCol.begin(), m_newAxisCol.end());
+    }
+
+  }
+
+
+  return StatusCode::SUCCESS;
+};
+
+
+StatusCode TruthPatternRecAlg::ClearAlgorithm(){
+  p_HalfClusterV.clear();
+  p_HalfClusterU.clear();  
+
+  return StatusCode::SUCCESS;
+};
+
+
+StatusCode TruthPatternRecAlg::OverlapMerging( std::vector<std::shared_ptr<PandoraPlus::CaloHalfCluster>>& m_axisCol ){
+  if(m_axisCol.size()<2) return StatusCode::SUCCESS;
+
+  for(int iax=0; iax<m_axisCol.size(); iax++){
+    const PandoraPlus::CaloHalfCluster* m_axis = m_axisCol[iax].get();
+    for(int jax=iax+1; jax<m_axisCol.size(); jax++){
+      const PandoraPlus::CaloHalfCluster* p_axis = m_axisCol[jax].get();
+
+      std::vector<const Calo1DCluster*> tmp_localMax = p_axis->getCluster();
+
+      int nsharedHits = 0;
+      for(int ihit=0; ihit<m_axis->getCluster().size(); ihit++)
+        if( find(tmp_localMax.begin(), tmp_localMax.end(), m_axis->getCluster()[ihit])!=tmp_localMax.end() ) nsharedHits++;
+
+
+      if( (m_axis->getCluster().size()<=p_axis->getCluster().size() && (float)nsharedHits/m_axis->getCluster().size()>settings.map_floatPars["th_overlap"] ) ||
+          (p_axis->getCluster().size()<m_axis->getCluster().size() && (float)nsharedHits/p_axis->getCluster().size()>settings.map_floatPars["th_overlap"] ) ){
+
+        m_axisCol[iax]->mergeHalfCluster( m_axisCol[jax].get() );
+
+        // track axis + neutral axis = track axis
+        int axis_type = m_axisCol[iax]->getType() + m_axisCol[jax]->getType();
+        m_axisCol[iax]->setType(axis_type);
+
+        //delete m_axisCol[jax]; m_axisCol[jax]=nullptr;
+        m_axisCol.erase(m_axisCol.begin()+jax);
+        jax--;
+        if(iax>jax+1) iax--;
+
+      }
+
+      p_axis=nullptr;
+    }
+    m_axis=nullptr;
+  }
+
+
+  return StatusCode::SUCCESS;
+}
+
+#endif
diff --git a/Reconstruction/CrystalCaloRec/src/Algorithm/TruthTrackMatchingAlg.cpp b/Reconstruction/CrystalCaloRec/src/Algorithm/TruthTrackMatchingAlg.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..09a7d046aa1cfa4fa8bbd91eb1820c471cf59445
--- /dev/null
+++ b/Reconstruction/CrystalCaloRec/src/Algorithm/TruthTrackMatchingAlg.cpp
@@ -0,0 +1,271 @@
+#ifndef _TRUTHTRKMATCHING_ALG_C
+#define _TRUTHTRKMATCHING_ALG_C
+
+#include "Algorithm/TruthTrackMatchingAlg.h"
+
+StatusCode TruthTrackMatchingAlg::ReadSettings(Settings& m_settings){
+  settings = m_settings;
+
+  //Initialize parameters
+  if(settings.map_stringPars.find("MatchType")==settings.map_stringPars.end())
+    settings.map_stringPars["MatchType"] = "LocalMax";  //LocalMax or Bar. TODO: implatement this option. 
+  if(settings.map_stringPars.find("ReadinLocalMaxName")==settings.map_stringPars.end())
+    settings.map_stringPars["ReadinLocalMaxName"] = "AllLocalMax";
+  if(settings.map_stringPars.find("OutputLongiClusName")==settings.map_stringPars.end())
+    settings.map_stringPars["OutputLongiClusName"] = "TruthTrackAxis";
+
+  return StatusCode::SUCCESS;
+};
+
+StatusCode TruthTrackMatchingAlg::Initialize( PandoraPlusDataCol& m_datacol ){
+  m_TrackCol.clear();
+  p_HalfClusterV = nullptr;
+  p_HalfClusterU = nullptr;
+
+  for(int itrk=0; itrk<m_datacol.TrackCol.size(); itrk++ ) m_TrackCol.push_back(m_datacol.TrackCol[itrk].get());
+  p_HalfClusterU = &(m_datacol.map_HalfCluster["HalfClusterColU"]);
+  p_HalfClusterV = &(m_datacol.map_HalfCluster["HalfClusterColV"]);
+
+cout<<"TruthTrackMatchingAlg: Input track size "<<m_TrackCol.size()<<", HFClusterU size "<<p_HalfClusterU->size()<<", HFClusterV size "<<p_HalfClusterV->size()<<endl;
+
+  return StatusCode::SUCCESS;
+};
+
+StatusCode TruthTrackMatchingAlg::RunAlgorithm( PandoraPlusDataCol& m_datacol ){
+  if( m_TrackCol.size()==0) return StatusCode::SUCCESS;
+
+  for(int itrk=0; itrk<m_TrackCol.size(); itrk++){
+
+    //Get MCP linked to the track
+    edm4hep::MCParticle mcp_trk = m_TrackCol[itrk]->getLeadingMCP();
+//cout<<"  Track #"<<itrk<<": MC pid "<<mcp_trk.getPDG()<<endl;
+ 
+    //Loop in HFCluster U
+    for(int ihf=0; ihf<p_HalfClusterU->size(); ihf++){
+      //Get Local max of the HalfCluster
+      std::vector<const PandoraPlus::Calo1DCluster*> localMaxColU = p_HalfClusterU->at(ihf).get()->getLocalMaxCol(settings.map_stringPars["ReadinLocalMaxName"]);
+
+      //Loop for the localMax: 
+      std::shared_ptr<PandoraPlus::CaloHalfCluster> t_axis = std::make_shared<PandoraPlus::CaloHalfCluster>();
+      for(int ilm=0; ilm<localMaxColU.size(); ilm++){
+        edm4hep::MCParticle mcp_lm = localMaxColU[ilm]->getLeadingMCP(); 
+        if(mcp_lm==mcp_trk)
+          t_axis->addUnit(localMaxColU[ilm]);
+        
+      }
+
+      // If the track does not match the Halfcluster, the track axis candidate will have no 1DCluster
+      if(!t_axis || t_axis->getCluster().size()==0)
+        continue;
+
+//cout<<"    HFCluster #"<<ihf<<" match with track. New axis localMax size "<<t_axis->getCluster().size()<<endl;
+      t_axis->getLinkedMCPfromUnit();
+      t_axis->addAssociatedTrack(m_TrackCol[itrk]);
+      t_axis->setType(10000); //Track-type axis.
+      m_TrackCol[itrk]->addAssociatedHalfClusterU( p_HalfClusterU->at(ihf).get() );
+      m_datacol.map_HalfCluster["bkHalfCluster"].push_back(t_axis);
+      p_HalfClusterU->at(ihf).get()->addHalfCluster(settings.map_stringPars["OutputLongiClusName"], t_axis.get());
+    }
+
+    //Loop in HFCluster V
+    for(int ihf=0; ihf<p_HalfClusterV->size(); ihf++){
+      //Get Local max of the HalfCluster
+      std::vector<const PandoraPlus::Calo1DCluster*> localMaxColV = p_HalfClusterV->at(ihf).get()->getLocalMaxCol(settings.map_stringPars["ReadinLocalMaxName"]);
+
+      //Loop for the localMax:
+      std::shared_ptr<PandoraPlus::CaloHalfCluster> t_axis = std::make_shared<PandoraPlus::CaloHalfCluster>();
+      for(int ilm=0; ilm<localMaxColV.size(); ilm++){
+        edm4hep::MCParticle mcp_lm = localMaxColV[ilm]->getLeadingMCP();
+        if(mcp_lm==mcp_trk)
+          t_axis->addUnit(localMaxColV[ilm]);
+        
+      }
+
+      // If the track does not match the Halfcluster, the track axis candidate will have no 1DCluster
+      if(!t_axis || t_axis->getCluster().size()==0)
+        continue;
+
+//cout<<"    HFCluster #"<<ihf<<" match with track. New axis localMax size "<<t_axis->getCluster().size()<<endl;
+      t_axis->getLinkedMCPfromUnit();
+      t_axis->addAssociatedTrack(m_TrackCol[itrk]);
+      t_axis->setType(10000); //Track-type axis.
+      m_TrackCol[itrk]->addAssociatedHalfClusterV( p_HalfClusterV->at(ihf).get() );
+      m_datacol.map_HalfCluster["bkHalfCluster"].push_back(t_axis);
+      p_HalfClusterV->at(ihf).get()->addHalfCluster(settings.map_stringPars["OutputLongiClusName"], t_axis.get());
+    }
+
+  }//End loop track
+
+//cout<<"Check HFClusters linked to the same track"<<endl;
+  //Loop track to check the associated cluster: merge clusters if they are associated to the same track.
+  std::vector<PandoraPlus::CaloHalfCluster*> tmp_deleteClus; tmp_deleteClus.clear();
+  for(auto &itrk : m_TrackCol){
+    std::vector<PandoraPlus::CaloHalfCluster*> m_matchedUCol = itrk->getAssociatedHalfClustersU();
+    std::vector<PandoraPlus::CaloHalfCluster*> m_matchedVCol = itrk->getAssociatedHalfClustersV();
+//cout<<"  In track "<<itrk<<": linked HFU size "<<m_matchedUCol.size()<<", linked HFV size "<<m_matchedVCol.size()<<endl;
+
+    if( m_matchedUCol.size()>1 ){
+      for(int i=1; i<m_matchedUCol.size(); i++){
+        m_matchedUCol[0]->mergeHalfCluster( m_matchedUCol[i] );
+        tmp_deleteClus.push_back(m_matchedUCol[i]);
+      }
+    }
+    if( m_matchedVCol.size()>1 ){
+      for(int i=1; i<m_matchedVCol.size(); i++){
+        m_matchedVCol[0]->mergeHalfCluster( m_matchedVCol[i] );
+        tmp_deleteClus.push_back(m_matchedVCol[i]);
+      }
+    }
+  }//End loop track
+
+
+  //Check vector: clean the merged clusters
+  for(int ihc=0; ihc<p_HalfClusterU->size(); ihc++){
+    if( find(tmp_deleteClus.begin(), tmp_deleteClus.end(), p_HalfClusterU->at(ihc).get())!=tmp_deleteClus.end() ){
+      p_HalfClusterU->erase(p_HalfClusterU->begin()+ihc);
+      ihc--;
+    }
+  }
+
+  for(int ihc=0; ihc<p_HalfClusterV->size(); ihc++){
+    if( find(tmp_deleteClus.begin(), tmp_deleteClus.end(), p_HalfClusterV->at(ihc).get())!=tmp_deleteClus.end() ){
+      p_HalfClusterV->erase(p_HalfClusterV->begin()+ihc);
+      ihc--;
+    }
+  }
+
+  //Clean the duplicated axes
+  for(int ihc=0; ihc<p_HalfClusterU->size(); ihc++){
+    std::vector<const PandoraPlus::CaloHalfCluster*> m_axis = p_HalfClusterU->at(ihc)->getHalfClusterCol(settings.map_stringPars["OutputLongiClusName"]);
+    std::vector<const PandoraPlus::CaloHalfCluster*> m_mergedAxis; m_mergedAxis.clear();
+
+//cout<<"  In HFCluster #"<<ihc<<": track axis size "<<m_axis.size();
+    if(m_axis.size()<=1){ 
+//cout<<endl;
+      continue;
+    }
+
+    std::map<const PandoraPlus::Track*, std::vector<const PandoraPlus::CaloHalfCluster*>> map_trk; map_trk.clear();
+    for(int iax=0; iax<m_axis.size(); iax++){
+      if(m_axis[iax]->getAssociatedTracks().size()==0){ 
+        m_mergedAxis.push_back(m_axis[iax]);
+        continue;
+      }
+      if(m_axis[iax]->getAssociatedTracks().size()>1)
+        std::cout<<"Warning: track axis has "<<m_axis[iax]->getAssociatedTracks().size()<<" matched tracks! Need to check! "<<endl;
+
+      map_trk[m_axis[iax]->getAssociatedTracks()[0]].push_back(m_axis[iax]);
+    }
+//cout<<"  map size "<<map_trk.size()<<", neutral axis size "<<m_mergedAxis.size()<<endl;;
+
+    for(auto &itrk: map_trk){
+//cout<<"  In track: axis size "<<itrk.second.size()<<endl;
+      if(itrk.second.size()==0) continue;
+      if(itrk.second.size()==1){ 
+        m_mergedAxis.push_back(itrk.second[0]);
+        continue;
+      }
+      PandoraPlus::CaloHalfCluster* p_axis = const_cast<PandoraPlus::CaloHalfCluster*>(itrk.second[0]);
+      for(int iax=1; iax<itrk.second.size(); iax++) p_axis->mergeHalfCluster(itrk.second[iax]);
+      m_mergedAxis.push_back(p_axis);
+    }
+    
+//cout<<". After merge: axis size "<<m_mergedAxis.size()<<endl;
+     p_HalfClusterU->at(ihc)->setHalfClusters(settings.map_stringPars["OutputLongiClusName"], m_mergedAxis);
+  }
+
+  for(int ihc=0; ihc<p_HalfClusterV->size(); ihc++){
+    std::vector<const PandoraPlus::CaloHalfCluster*> m_axis = p_HalfClusterV->at(ihc)->getHalfClusterCol(settings.map_stringPars["OutputLongiClusName"]);
+    std::vector<const PandoraPlus::CaloHalfCluster*> m_mergedAxis; m_mergedAxis.clear();
+
+//cout<<"  In HFCluster #"<<ihc<<": track axis size "<<m_axis.size();
+    if(m_axis.size()<=1){
+//cout<<endl;
+      continue;
+    }
+
+    std::map<const PandoraPlus::Track*, std::vector<const PandoraPlus::CaloHalfCluster*>> map_trk; map_trk.clear();
+    for(int iax=0; iax<m_axis.size(); iax++){
+      if(m_axis[iax]->getAssociatedTracks().size()==0){
+        m_mergedAxis.push_back(m_axis[iax]);
+        continue;
+      }
+      if(m_axis[iax]->getAssociatedTracks().size()>1)
+        std::cout<<"Warning: track axis has "<<m_axis[iax]->getAssociatedTracks().size()<<" matched tracks! Need to check! "<<endl;
+
+      map_trk[m_axis[iax]->getAssociatedTracks()[0]].push_back(m_axis[iax]);
+    }
+//cout<<"  map size "<<map_trk.size()<<", neutral axis size "<<m_mergedAxis.size()<<endl;;
+
+    for(auto &itrk: map_trk){
+//cout<<"  In track: axis size "<<itrk.second.size()<<endl;
+      if(itrk.second.size()==0) continue;
+      if(itrk.second.size()==1){
+        m_mergedAxis.push_back(itrk.second[0]);
+        continue;
+      }
+      PandoraPlus::CaloHalfCluster* p_axis = const_cast<PandoraPlus::CaloHalfCluster*>(itrk.second[0]);
+      for(int iax=1; iax<itrk.second.size(); iax++) p_axis->mergeHalfCluster(itrk.second[iax]);
+      m_mergedAxis.push_back(p_axis);
+    }
+
+//cout<<". After merge: axis size "<<m_mergedAxis.size()<<endl;
+     p_HalfClusterV->at(ihc)->setHalfClusters(settings.map_stringPars["OutputLongiClusName"], m_mergedAxis);
+  }
+
+
+
+/*
+cout<<"Track size: "<<m_TrackCol.size()<<endl;
+cout<<"Print HalfClusters and axis"<<endl;
+cout<<"  HalfClusterU size: "<<p_HalfClusterU->size()<<endl;
+for(int i=0; i<p_HalfClusterU->size(); i++){
+  std::vector<const PandoraPlus::Calo1DCluster*> m_1dcluster = p_HalfClusterU->at(i)->getCluster();
+  std::vector<const PandoraPlus::Calo1DCluster*> m_localMax = p_HalfClusterU->at(i).get()->getLocalMaxCol(settings.map_stringPars["ReadinLocalMaxName"]);
+  std::vector<const PandoraPlus::CaloHalfCluster*> m_axis = p_HalfClusterU->at(i)->getHalfClusterCol(settings.map_stringPars["OutputLongiClusName"]);
+  printf("    In HalfCluster #%d: 1D cluster size %d, localMax size %d, axis size %d \n", i, m_1dcluster.size(), m_localMax.size(), m_axis.size());
+
+  for(int iax=0; iax<m_axis.size(); iax++){
+    printf("      In axis #%d: localMax size %d \n", iax, m_axis[iax]->getCluster().size() );
+    for(int ilm=0; ilm<m_axis[iax]->getCluster().size(); ilm++){
+      printf("        LocalMax #%d: layer %d, towersize %d, towerID [%d, %d, %d], En %.4f, leadingMC pid %d, address %p \n", 
+        ilm, m_axis[iax]->getCluster()[ilm]->getDlayer(), m_axis[iax]->getCluster()[ilm]->getTowerID().size(), 
+        m_axis[iax]->getCluster()[ilm]->getTowerID()[0][0], m_axis[iax]->getCluster()[ilm]->getTowerID()[0][1],m_axis[iax]->getCluster()[ilm]->getTowerID()[0][2],
+        m_axis[iax]->getCluster()[ilm]->getEnergy(), m_axis[iax]->getCluster()[ilm]->getLeadingMCP().getPDG(), m_axis[iax]->getCluster()[ilm] );
+    }
+  }
+}
+
+cout<<"  HalfClusterV size: "<<p_HalfClusterV->size()<<endl;
+for(int i=0; i<p_HalfClusterV->size(); i++){
+  std::vector<const PandoraPlus::Calo1DCluster*> m_1dcluster = p_HalfClusterV->at(i)->getCluster();
+  std::vector<const PandoraPlus::Calo1DCluster*> m_localMax = p_HalfClusterV->at(i).get()->getLocalMaxCol(settings.map_stringPars["ReadinLocalMaxName"]);
+  std::vector<const PandoraPlus::CaloHalfCluster*> m_axis = p_HalfClusterV->at(i)->getHalfClusterCol(settings.map_stringPars["OutputLongiClusName"]);
+  printf("    In HalfCluster #%d: 1D cluster size %d, localMax size %d, axis size %d \n", i, m_1dcluster.size(), m_localMax.size(), m_axis.size());
+
+  for(int iax=0; iax<m_axis.size(); iax++){
+    printf("      In axis #%d: localMax size %d \n", iax, m_axis[iax]->getCluster().size() );
+    for(int ilm=0; ilm<m_axis[iax]->getCluster().size(); ilm++){
+      printf("        LocalMax #%d: layer %d, towersize %d, towerID [%d, %d, %d], En %.4f, leadingMC pid %d, address %p \n",
+        ilm, m_axis[iax]->getCluster()[ilm]->getDlayer(), m_axis[iax]->getCluster()[ilm]->getTowerID().size(),
+        m_axis[iax]->getCluster()[ilm]->getTowerID()[0][0], m_axis[iax]->getCluster()[ilm]->getTowerID()[0][1],m_axis[iax]->getCluster()[ilm]->getTowerID()[0][2],
+        m_axis[iax]->getCluster()[ilm]->getEnergy(), m_axis[iax]->getCluster()[ilm]->getLeadingMCP().getPDG(), m_axis[iax]->getCluster()[ilm] );
+    }
+  }
+}
+*/
+
+  return StatusCode::SUCCESS;
+};
+
+StatusCode TruthTrackMatchingAlg::ClearAlgorithm(){
+  m_TrackCol.clear();
+  p_HalfClusterV = nullptr;
+  p_HalfClusterU = nullptr;
+
+  return StatusCode::SUCCESS;
+};
+
+
+
+#endif
diff --git a/Reconstruction/CrystalCaloRec/src/Objects/Calo1DCluster.cc b/Reconstruction/CrystalCaloRec/src/Objects/Calo1DCluster.cc
new file mode 100644
index 0000000000000000000000000000000000000000..5ca151ca269d17125c04a44df20d84ebee423918
--- /dev/null
+++ b/Reconstruction/CrystalCaloRec/src/Objects/Calo1DCluster.cc
@@ -0,0 +1,249 @@
+#ifndef CALO_1DCLUSTER_C
+#define CALO_1DCLUSTER_C
+
+#include "Objects/CaloUnit.h"
+#include "Objects/Calo1DCluster.h"
+using namespace std; 
+namespace PandoraPlus{
+
+  void Calo1DCluster::Clear(){
+    Bars.clear();
+    Seeds.clear();
+    Energy=0.;
+    pos.SetXYZ(0.,0.,0.);
+    towerID.clear();
+    //m_modules.clear();
+    //m_parts.clear();
+    //m_staves.clear();
+  }
+
+  void Calo1DCluster::Clean() {
+    //for(int i=0; i<Bars.size(); i++) { delete Bars[i]; Bars[i]=NULL; }
+    //for(int i=0; i<Seeds.size(); i++) { delete Seeds[i]; Seeds[i]=NULL; }
+    //std::vector<int>().swap(m_modules);
+    //std::vector<int>().swap(m_parts);
+    //std::vector<int>().swap(m_staves);
+    Clear();
+  }
+
+  void Calo1DCluster::Check() {
+    for(int i=0; i<Bars.size(); i++)
+      if(!Bars[i]) { Bars.erase(Bars.begin()+i); i--; }
+    for(int i=0; i<Seeds.size(); i++)
+      if(!Seeds[i]) { Seeds.erase(Seeds.begin()+i); i--; }
+  }
+
+  std::shared_ptr<PandoraPlus::Calo1DCluster> Calo1DCluster::Clone() const{
+    std::shared_ptr<PandoraPlus::Calo1DCluster> p_cluster = std::make_shared<PandoraPlus::Calo1DCluster>();
+    p_cluster->setBars( Bars );
+    p_cluster->setSeeds( Seeds );
+    p_cluster->setIDInfo();
+    p_cluster->setLinkedMCP( MCParticleWeight );
+    for(int i=0; i<CousinClusters.size(); i++) p_cluster->addCousinCluster( CousinClusters[i] );
+    for(int i=0; i<ChildClusters.size(); i++) p_cluster->addChildCluster( ChildClusters[i] );
+   
+    return p_cluster;
+  }
+
+
+  bool Calo1DCluster::isNeighbor(const PandoraPlus::CaloUnit* m_bar) const
+  {
+    for(int i1d = 0; i1d<Bars.size(); i1d++){
+      if(Bars[i1d]->isNeighbor(m_bar)){ /*cout<<" isNeighbor! "<<endl;*/ return true;}
+    }
+    return false;
+  }
+
+  bool Calo1DCluster::inCluster(const PandoraPlus::CaloUnit* iBar) const{
+    return (find(Bars.begin(), Bars.end(), iBar)!=Bars.end() );
+  }
+
+  double Calo1DCluster::getEnergy() const{
+    double E=0;
+    for(int i=0;i<Bars.size();i++) E+=Bars[i]->getEnergy();
+    return E;
+  }
+
+  TVector3 Calo1DCluster::getPos() const{
+    TVector3 pos(0,0,0);
+    double Etot=getEnergy();
+    for(int i=0;i<Bars.size();i++) pos += Bars[i]->getPosition() * (Bars[i]->getEnergy()/Etot);
+    return pos;
+  }
+
+  double Calo1DCluster::getT1() const{
+    double T1=0;
+    double Etot = getEnergy();
+    for(int i=0;i<Bars.size();i++) T1 += (Bars[i]->getT1() * Bars[i]->getEnergy())/Etot;
+    return T1;
+  }
+
+  double Calo1DCluster::getT2() const{
+    double T2=0;
+    double Etot = getEnergy();
+    for(int i=0;i<Bars.size();i++) T2 += (Bars[i]->getT2() * Bars[i]->getEnergy())/Etot;
+    return T2;
+  }
+
+  double Calo1DCluster::getWidth() const{
+    TVector3 centPos = getPos();
+    double Etot = getEnergy();
+    double sigmax=0;
+    double sigmay=0;
+    double sigmaz=0;
+    for(int i=0; i<Bars.size(); i++){
+      double wi = Bars[i]->getEnergy()/Etot;
+      sigmax += wi*(Bars[i]->getPosition().x()-centPos.x())*(Bars[i]->getPosition().x()-centPos.x());
+      sigmay += wi*(Bars[i]->getPosition().y()-centPos.y())*(Bars[i]->getPosition().y()-centPos.y());
+      sigmaz += wi*(Bars[i]->getPosition().z()-centPos.z())*(Bars[i]->getPosition().z()-centPos.z());
+    }
+
+    if(sigmaz!=0) return sigmaz;  //sLayer=1, bars along z-axis.
+    else if(sigmax==0 && sigmaz==0) return sigmay; //Module 2, 6
+    else if(sigmay==0 && sigmaz==0) return sigmax; //Module 0, 4
+    else if(sigmax!=0 && sigmay!=0 && sigmaz==0) return sqrt(sigmax*sigmax+sigmay*sigmay); //Module 1, 3, 5, 7;
+    else return 0.;
+  }
+
+  double Calo1DCluster::getScndMoment() const{
+    if(Bars.size()<=1) return 0.;
+    TVector3 pos = getPos();
+    double Etot = getEnergy();
+    double scndM = 0;
+    for(int i=0;i<Bars.size();i++) scndM += (Bars[i]->getEnergy() * (pos-Bars[i]->getPosition()).Mag2()) / Etot;
+    return scndM;
+  }
+
+  bool Calo1DCluster::getGlobalRange( double& xmin,  double& ymin, double& zmin, double& xmax, double& ymax, double& zmax ) const{
+    if(Bars.size()==0) return false; 
+
+    std::vector<double> posx; posx.clear();
+    std::vector<double> posy; posx.clear();
+    std::vector<double> posz; posx.clear();
+    for(int i=0; i<Bars.size(); i++){  
+      posx.push_back(Bars[i]->getPosition().x());  
+      posy.push_back(Bars[i]->getPosition().y());
+      posz.push_back(Bars[i]->getPosition().z());
+    }
+    auto xminptr = std::min_element( std::begin(posx), std::end(posx) );
+    auto xmaxptr = std::max_element( std::begin(posx), std::end(posx) );
+    auto yminptr = std::min_element( std::begin(posy), std::end(posy) );
+    auto ymaxptr = std::max_element( std::begin(posy), std::end(posy) );
+    auto zminptr = std::min_element( std::begin(posz), std::end(posz) );
+    auto zmaxptr = std::max_element( std::begin(posz), std::end(posz) );
+
+    int slayer = Bars[0]->getSlayer(); 
+    if(slayer==0){
+      xmin = -9999.;    xmax = 9999.; 
+      ymin = -9999.;    ymax = 9999.; 
+      zmin = *zminptr-6.;  zmax = *zmaxptr+6.;
+    }
+    else if(slayer==1){
+      xmin = *xminptr-6;  xmax = *xmaxptr+6;
+      ymin = *yminptr-6;  ymax = *ymaxptr+6;
+      zmin = -9999;     zmax = 9999; 
+    }
+    else return false; 
+
+    return true;
+  }
+
+  int Calo1DCluster::getLeftEdge(){
+    std::sort(Bars.begin(), Bars.end());
+    if(Bars.size()==0) return -99;
+    int edge = -99;
+    if( Bars[0]->getSlayer()==0 ) edge = Bars[0]->getBar() + Bars[0]->getStave()*PandoraPlus::CaloUnit::NbarZ; 
+    if( Bars[0]->getSlayer()==1 ){ 
+      if(Bars[0]->getModule()%2==0) edge = Bars[0]->getBar() + Bars[0]->getModule()*(CaloUnit::NbarPhi_even[Bars[0]->getDlayer()]);
+      else edge = Bars[0]->getBar() + Bars[0]->getModule()*(CaloUnit::NbarPhi_odd[Bars[0]->getDlayer()]);
+    }
+    return edge;
+  }
+
+  int Calo1DCluster::getRightEdge(){
+    std::sort(Bars.begin(), Bars.end());
+    if(Bars.size()==0) return -99;
+    int edge = -99;
+    if( Bars[Bars.size()-1]->getSlayer()==0 ) edge = Bars[Bars.size()-1]->getBar() + Bars[Bars.size()-1]->getStave()*CaloUnit::NbarZ;
+    if( Bars[Bars.size()-1]->getSlayer()==1 ){ 
+      if(Bars[Bars.size()-1]->getModule()%2==0) edge = Bars[Bars.size()-1]->getBar() + Bars[Bars.size()-1]->getModule()*(CaloUnit::NbarPhi_even[Bars[Bars.size()-1]->getDlayer()]);
+      else edge = Bars[Bars.size()-1]->getBar() + Bars[Bars.size()-1]->getModule()*(CaloUnit::NbarPhi_odd[Bars[Bars.size()-1]->getDlayer()]);
+    }
+    return edge;
+  }
+
+  void Calo1DCluster::addUnit(const PandoraPlus::CaloUnit* _bar ) 
+  {
+    Bars.push_back(_bar);
+    std::vector<int> id(2);
+    id[0] = _bar->getModule();
+    id[1] = _bar->getStave();
+    if(find(towerID.begin(), towerID.end(), id)==towerID.end()) towerID.push_back(id);
+  }
+
+  void Calo1DCluster::deleteCousinCluster( const PandoraPlus::Calo1DCluster* _cl ){
+    auto iter = find( CousinClusters.begin(), CousinClusters.end(), _cl );
+    if(iter!=CousinClusters.end()) CousinClusters.erase( iter );
+  }
+
+
+  void Calo1DCluster::setIDInfo() {
+    for(int i=0; i<Bars.size(); i++){
+      std::vector<int> id(2);
+      id[0] = Bars[i]->getModule();
+      id[1] = Bars[i]->getStave();
+      if(find(towerID.begin(), towerID.end(), id)==towerID.end()) towerID.push_back(id);  
+    }
+  }
+
+  void Calo1DCluster::setSeed(){
+    double maxE = -1; 
+    int index = -1; 
+    for(int i=0; i<Bars.size(); ++i){
+      if(Bars[i]->getEnergy()>maxE) { maxE = Bars[i]->getEnergy(); index = i; }
+    }
+    //if(Seeds.size()==0 && index>=0 && maxE>0.005 ) Seeds.push_back( Bars[index] );
+    if(Seeds.size()==0 && index>=0 ) Seeds.push_back( Bars[index] );
+  }
+
+
+  std::vector< std::pair<edm4hep::MCParticle, float> > Calo1DCluster::getLinkedMCPfromUnit(){
+    MCParticleWeight.clear();
+
+    std::map<edm4hep::MCParticle, float> map_truthP_totE; map_truthP_totE.clear();
+    for(auto ibar : Bars ){
+      for(auto ipair : ibar->getLinkedMCP()) map_truthP_totE[ipair.first] += ibar->getEnergy()*ipair.second;
+    }
+
+    for(auto imcp: map_truthP_totE){
+      MCParticleWeight.push_back( std::make_pair(imcp.first, imcp.second/getEnergy()) );
+    }
+
+    return MCParticleWeight;
+  }
+
+  edm4hep::MCParticle Calo1DCluster::getLeadingMCP() const{
+    float maxWeight = -1.;
+    edm4hep::MCParticle mcp;
+    for(auto& iter: MCParticleWeight){
+      if(iter.second>maxWeight){
+        mcp = iter.first;
+        maxWeight = iter.second;
+      }
+    }
+
+    return mcp;
+  }
+
+  float Calo1DCluster::getLeadingMCPweight() const{
+    float maxWeight = -1.;
+    for(auto& iter: MCParticleWeight){
+      if(iter.second>maxWeight){
+        maxWeight = iter.second;
+      }
+    }
+    return maxWeight;
+  }
+
+};
+#endif
diff --git a/Reconstruction/CrystalCaloRec/src/Objects/Calo2DCluster.cc b/Reconstruction/CrystalCaloRec/src/Objects/Calo2DCluster.cc
new file mode 100644
index 0000000000000000000000000000000000000000..adf232408b5644a665931e4076890b62dab9598b
--- /dev/null
+++ b/Reconstruction/CrystalCaloRec/src/Objects/Calo2DCluster.cc
@@ -0,0 +1,122 @@
+#ifndef CALO_2DCLUSTER_C
+#define CALO_2DCLUSTER_C
+
+#include "Objects/Calo2DCluster.h"
+#include <cmath>
+using namespace std;
+namespace PandoraPlus{
+
+  void Calo2DCluster::Clear() {
+    towerID.clear();
+    barUCol.clear();
+    barVCol.clear();
+    barShowerUCol.clear(); 
+    barShowerVCol.clear();  
+  }
+
+  void Calo2DCluster::ClearShower() {
+    barShowerUCol.clear(); 
+    barShowerVCol.clear();
+  }
+
+  void Calo2DCluster::Check(){
+    for(int i=0; i<barUCol.size(); i++)
+      if(!barUCol[i]) { barUCol.erase(barUCol.begin()+i); i--; }	  
+    for(int i=0; i<barVCol.size(); i++)
+      if(!barVCol[i]) { barVCol.erase(barVCol.begin()+i); i--; }
+    for(int i=0; i<barShowerUCol.size(); i++)
+      if(!barShowerUCol[i]) { barShowerUCol.erase(barShowerUCol.begin()+i); i--; }
+    for(int i=0; i<barShowerVCol.size(); i++)
+      if(!barShowerVCol[i]) { barShowerVCol.erase(barShowerVCol.begin()+i); i--; }
+  	}
+
+  void Calo2DCluster::Clean(){
+    //for(int i=0; i<barUCol.size(); i++) { delete barUCol[i]; barUCol[i]=NULL; }
+    //for(int i=0; i<barVCol.size(); i++) { delete barVCol[i]; barVCol[i]=NULL; }
+    //for(int i=0; i<barShowerUCol.size(); i++) { delete barShowerUCol[i]; barShowerUCol[i]=NULL; }
+    //for(int i=0; i<barShowerVCol.size(); i++) { delete barShowerVCol[i]; barShowerVCol[i]=NULL; }
+    //std::vector<int>().swap(m_modules);
+    //std::vector<int>().swap(m_parts);
+    //std::vector<int>().swap(m_staves);
+    Clear();
+  	}
+
+  bool Calo2DCluster::isNeighbor(const PandoraPlus::Calo1DCluster* m_1dcluster) const{
+    assert(m_1dcluster->getBars().size() > 0 && getCluster().at(0)->getBars().size()>0 );
+    if(m_1dcluster->getDlayer() != getDlayer()  ) return false; 
+
+    for(int i=0; i<m_1dcluster->getTowerID().size(); i++){
+		for(int j=0; j<towerID.size(); j++){
+      if( m_1dcluster->getTowerID()[i]==towerID[j] ) return true;
+    }}
+    
+    return false;
+  }
+
+ 
+  void Calo2DCluster::addUnit(const Calo1DCluster* _1dcluster)
+  {
+    if(_1dcluster->getSlayer()==0) barShowerUCol.push_back(_1dcluster);
+    if(_1dcluster->getSlayer()==1) barShowerVCol.push_back(_1dcluster); 
+    for(int ib=0; ib<_1dcluster->getBars().size(); ib++) addBar(_1dcluster->getBars()[ib]);
+
+    std::vector< std::vector<int> > id = _1dcluster->getTowerID();
+    for(int ii=0; ii<id.size(); ii++) 
+      if( find(towerID.begin(), towerID.end(), id[ii])==towerID.end() ) towerID.push_back(id[ii]);
+
+  }
+
+  std::vector<const Calo1DCluster*> Calo2DCluster::getCluster() const{ 
+    std::vector<const Calo1DCluster*> m_1dclusters; 
+    m_1dclusters.clear();
+    m_1dclusters.insert(m_1dclusters.end(),barShowerUCol.begin(),barShowerUCol.end());
+    return  m_1dclusters;
+  }
+	
+  std::vector<const CaloUnit*> Calo2DCluster::getBars() const
+  {
+	std::vector<const CaloUnit*> results;
+	results.clear();
+	std::vector<const Calo1DCluster*> m_1dclusters; 
+	m_1dclusters.clear();
+	m_1dclusters.insert(m_1dclusters.end(),barShowerUCol.begin(),barShowerUCol.end());
+	for(int i=0; i<m_1dclusters.size(); i++)
+	{
+		for(int j=0; j<m_1dclusters.at(i)->getBars().size(); j++)
+		{
+			results.push_back(m_1dclusters.at(i)->getBars().at(j));
+		}
+	}
+	return results;
+  }
+	
+  double Calo2DCluster::getEnergy() const {
+    double sumE = 0;
+
+    for(int m=0; m<barShowerUCol.size(); m++) sumE += barShowerUCol[m]->getEnergy();
+    for(int m=0; m<barShowerVCol.size(); m++) sumE += barShowerVCol[m]->getEnergy();
+    return sumE;
+  }
+
+
+  TVector3 Calo2DCluster::getPos() const{
+    TVector3 m_pos(0, 0, 0); 
+    if(towerID.size()==0) return m_pos; 
+
+    float rotAngle = -towerID[0][0]*TMath::TwoPi()/PandoraPlus::CaloUnit::Nmodule;
+    TVector3 m_vecX(0., 0., 0.);  
+    TVector3 m_vecY(0., 0., 0.);
+    for(int m=0; m<barShowerUCol.size(); m++) m_vecX += barShowerUCol[m]->getPos();
+    m_vecX *= (1./barShowerUCol.size());
+    for(int m=0; m<barShowerVCol.size(); m++) m_vecY += barShowerVCol[m]->getPos();
+    m_vecY *= (1./barShowerVCol.size());
+    m_vecX.RotateZ(rotAngle);
+    m_vecY.RotateZ(rotAngle);
+    m_pos.SetXYZ( m_vecY.x(), (m_vecX.y()+m_vecY.y())/2 , m_vecX.z() );
+    m_pos.RotateZ(-rotAngle);
+    return m_pos;
+  }
+
+
+};
+#endif
diff --git a/Reconstruction/CrystalCaloRec/src/Objects/Calo3DCluster.cc b/Reconstruction/CrystalCaloRec/src/Objects/Calo3DCluster.cc
new file mode 100644
index 0000000000000000000000000000000000000000..adc8dbc3c53473e50662f532c539c9e94611ec68
--- /dev/null
+++ b/Reconstruction/CrystalCaloRec/src/Objects/Calo3DCluster.cc
@@ -0,0 +1,436 @@
+#ifndef CALO_3DCLUSTER_C
+#define CALO_3DCLUSTER_C
+
+#include "Objects/Calo3DCluster.h"
+#include <cmath>
+using namespace std;
+namespace PandoraPlus{
+
+  void Calo3DCluster::Clear() 
+  {
+	  m_2dclusters.clear();
+	  m_towers.clear();
+    towerID.clear(); 
+	  //m_modules.clear();
+	  //m_parts.clear();
+	  //m_staves.clear();
+  }
+
+  void Calo3DCluster::Clean(){
+    //for(int i=0; i<m_2dclusters.size(); i++) { delete m_2dclusters[i]; m_2dclusters[i]=NULL; }
+    //for(int i=0; i<m_towers.size(); i++) { delete m_towers[i]; m_towers[i]=NULL; }
+    //std::vector<int>().swap(m_modules);
+    //std::vector<int>().swap(m_parts);
+    //std::vector<int>().swap(m_staves);
+    Clear();
+  }
+
+  void Calo3DCluster::Check()
+  {
+    for(int i=0; i<m_2dclusters.size(); i++)
+    if(!m_2dclusters[i]) { m_2dclusters.erase(m_2dclusters.begin()+i); i--; }
+    //for(int i=0; i<m_towers.size(); i++)
+    //if(!m_towers[i]) { m_towers.erase(m_towers.begin()+i); i--; }
+  }
+
+
+  std::shared_ptr<PandoraPlus::Calo3DCluster> Calo3DCluster::Clone() const{
+    std::shared_ptr<PandoraPlus::Calo3DCluster> m_clus = std::make_shared<PandoraPlus::Calo3DCluster>();
+    m_clus->setCaloHits(hits);
+    m_clus->setClusters(m_2dclusters);
+    m_clus->setTowers(m_towers);
+    for(auto iter:towerID)
+      m_clus->addTowerID(iter);
+    for(auto iter:map_localMaxU) 
+      for(int ilm=0; ilm<iter.second.size(); ilm++) m_clus->addLocalMaxU(iter.first, iter.second[ilm]);
+    for(auto iter:map_localMaxV)
+      for(int ilm=0; ilm<iter.second.size(); ilm++) m_clus->addLocalMaxV(iter.first, iter.second[ilm]);
+    for(auto iter:map_halfClusUCol)
+      for(int ihf=0; ihf<iter.second.size(); ihf++) m_clus->addHalfClusterU(iter.first, iter.second[ihf]);
+    for(auto iter:map_halfClusVCol)
+      for(int ihf=0; ihf<iter.second.size(); ihf++) m_clus->addHalfClusterV(iter.first, iter.second[ihf]);
+    for(auto iter:m_TrackCol)
+      m_clus->addAssociatedTrack(iter);
+    m_clus->setLinkedMCP(MCParticleWeight);
+    m_clus->FitAxis();
+
+    return m_clus;
+  }
+
+
+  //TODO: This function is sooooooo time consumeing now!!
+/*  bool Calo3DCluster::isNeighbor(const PandoraPlus::Calo2DCluster* m_2dcluster) const
+  {
+
+    //Inner module
+    for(int i=0; i<m_2dcluster->getTowerID().size(); i++){
+    for(int j=0; j<towerID.size(); j++){
+      if( m_2dcluster->getTowerID()[i]==towerID[j] ) return true;
+    }}
+
+    //for(int i=0; i<m_2dcluster->getModules().size(); i++){
+    //  for(int j=0; j<m_modules.size(); j++){
+    //    if(m_2dcluster->getModules().at(i)==m_modules.at(j) && m_2dcluster->getParts().at(i)==m_parts.at(j) && m_2dcluster->getStaves().at(i)==m_staves.at(j)){
+    //      return true;
+    //  }}
+    //}
+
+    //Inner module but in adjacent Dlayer
+    for(int i=0; i<m_2dclusters.size(); i++){
+      //If not in the same module:
+      bool inSameModule = false; 
+      std::vector<int> ID_module; ID_module.clear();
+      for(int it=0; it<towerID.size(); it++) ID_module.push_back(towerID[it][0]); 
+      for(int it=0; it<m_2dcluster->getTowerID().size(); it++){
+        if( find(ID_module.begin(), ID_module.end(), m_2dcluster->getTowerID()[it][0])!=ID_module.end() ) { inSameModule=true; break; }
+      }
+      if(!inSameModule) continue;
+
+      //If not the adjacent Dlayer: 
+      if(fabs(m_2dcluster->getDlayer()-m_2dclusters[i]->getDlayer())>1) continue; 
+
+      std::vector<const PandoraPlus::CaloUnit*> m_EdgeBarU_clus1; m_EdgeBarU_clus1.clear(); //Income 2DCluster
+      std::vector<const PandoraPlus::CaloUnit*> m_EdgeBarV_clus1; m_EdgeBarV_clus1.clear();
+      std::vector<const PandoraPlus::CaloUnit*> m_EdgeBarU_clus2; m_EdgeBarU_clus2.clear(); //2DCluster in present 3DCluster. 
+      std::vector<const PandoraPlus::CaloUnit*> m_EdgeBarV_clus2; m_EdgeBarV_clus2.clear();
+      
+      for(int ib=0; ib<m_2dcluster->getBarUCol().size(); ib++){
+        if(m_2dcluster->getBarUCol()[ib]->isAtLowerEdgeZ() || m_2dcluster->getBarUCol()[ib]->isAtUpperEdgeZ()) m_EdgeBarU_clus1.push_back(m_2dcluster->getBarUCol()[ib]);
+      }
+      for(int ib=0; ib<m_2dcluster->getBarVCol().size(); ib++){
+        if(m_2dcluster->getBarVCol()[ib]->isAtLowerEdgePhi() || m_2dcluster->getBarVCol()[ib]->isAtUpperEdgePhi()) m_EdgeBarV_clus1.push_back(m_2dcluster->getBarVCol()[ib]);
+      }
+      for(int ib=0; ib<m_2dclusters[i]->getBarUCol().size(); ib++){
+        if(m_2dclusters[i]->getBarUCol()[ib]->isAtLowerEdgeZ() || m_2dclusters[i]->getBarUCol()[ib]->isAtUpperEdgeZ()) m_EdgeBarU_clus2.push_back(m_2dclusters[i]->getBarUCol()[ib]);
+      }
+      for(int ib=0; ib<m_2dclusters[i]->getBarVCol().size(); ib++){
+        if(m_2dclusters[i]->getBarVCol()[ib]->isAtLowerEdgePhi() || m_2dclusters[i]->getBarVCol()[ib]->isAtUpperEdgePhi()) m_EdgeBarV_clus2.push_back(m_2dclusters[i]->getBarVCol()[ib]);
+      }
+
+
+      for(int ib1=0; ib1<m_EdgeBarU_clus1.size(); ib1++){
+        for(int ib2=0; ib2<m_2dclusters[i]->getBarVCol().size(); ib2++){
+          if( m_EdgeBarU_clus1[ib1]->getPart()==m_2dclusters[i]->getBarVCol()[ib2]->getPart() && abs(m_EdgeBarU_clus1[ib1]->getStave()-m_2dclusters[i]->getBarVCol()[ib2]->getStave())==1 ) 
+            return true; 
+      }}
+      for(int ib1=0; ib1<m_EdgeBarV_clus1.size(); ib1++){
+        for(int ib2=0; ib2<m_2dclusters[i]->getBarUCol().size(); ib2++){
+          if( m_EdgeBarV_clus1[ib1]->getStave()==m_2dclusters[i]->getBarUCol()[ib2]->getStave() && abs(m_EdgeBarV_clus1[ib1]->getPart()-m_2dclusters[i]->getBarUCol()[ib2]->getPart())==1 )
+            return true;
+      }}
+      for(int ib1=0; ib1<m_EdgeBarU_clus2.size(); ib1++){
+        for(int ib2=0; ib2<m_2dcluster->getBarVCol().size(); ib2++){
+          if( m_EdgeBarU_clus2[ib1]->getPart()==m_2dcluster->getBarVCol()[ib2]->getPart() && abs(m_EdgeBarU_clus2[ib1]->getStave()-m_2dcluster->getBarVCol()[ib2]->getStave())==1 )
+            return true;
+      }}
+      for(int ib1=0; ib1<m_EdgeBarV_clus2.size(); ib1++){
+        for(int ib2=0; ib2<m_2dcluster->getBarUCol().size(); ib2++){
+          if( m_EdgeBarV_clus2[ib1]->getStave()==m_2dcluster->getBarUCol()[ib2]->getStave() && abs(m_EdgeBarV_clus2[ib1]->getPart()-m_2dcluster->getBarUCol()[ib2]->getPart())==1 )
+            return true;
+      }}
+
+	  }
+
+    //Over modules
+    std::vector<const PandoraPlus::CaloUnit*> bars_2d = m_2dcluster->getBars();
+    for(int ib2d=0; ib2d<bars_2d.size(); ib2d++){
+      for(int ic=0; ic<m_2dclusters.size(); ic++){
+        for(int ib3d=0; ib3d<m_2dclusters[ic]->getBars().size(); ib3d++){
+          if(bars_2d[ib2d]->isModuleAdjacent(m_2dclusters[ic]->getBars()[ib3d])) return true;
+        }
+      }
+    }
+	  return false;
+  }
+*/
+
+  void Calo3DCluster::addUnit(const Calo2DCluster* _2dcluster){
+
+    m_2dclusters.push_back(_2dcluster);
+    std::vector< std::vector<int> > id = _2dcluster->getTowerID();
+    for(int ii=0; ii<id.size(); ii++)
+      if( find(towerID.begin(), towerID.end(), id[ii])==towerID.end() ) towerID.push_back(id[ii]);
+  }
+
+
+  void Calo3DCluster::mergeCluster( const PandoraPlus::Calo3DCluster* _clus ){
+    for(int i=0; i<_clus->getCluster().size(); i++)
+      addUnit( _clus->getCluster()[i] );
+
+    for(int i=0; i<_clus->getTowers().size(); i++)
+      addTower( _clus->getTowers()[i] );
+
+    for(int itrk=0; itrk<_clus->getAssociatedTracks().size(); itrk++){
+      if( find(m_TrackCol.begin(), m_TrackCol.end(), _clus->getAssociatedTracks()[itrk])==m_TrackCol.end() ) 
+        m_TrackCol.push_back( _clus->getAssociatedTracks()[itrk] );
+    }
+    
+    for(auto iter:_clus->getLocalMaxUMap() ){
+      if(map_localMaxU.find(iter.first)==map_localMaxU.end()) map_localMaxU[iter.first] = iter.second;
+      else{
+        for(int il=0; il<iter.second.size(); il++)
+          if( find(map_localMaxU[iter.first].begin(), map_localMaxU[iter.first].end(), iter.second[il])==map_localMaxU[iter.first].end() )
+            map_localMaxU[iter.first].push_back( iter.second[il] );
+      }
+    }
+    for(auto iter:_clus->getLocalMaxVMap() ){
+      if(map_localMaxV.find(iter.first)==map_localMaxV.end()) map_localMaxV[iter.first] = iter.second;
+      else{
+        for(int il=0; il<iter.second.size(); il++)
+          if( find(map_localMaxV[iter.first].begin(), map_localMaxV[iter.first].end(), iter.second[il])==map_localMaxV[iter.first].end() )
+            map_localMaxV[iter.first].push_back( iter.second[il] );
+      }
+    }
+
+    for(auto iter:_clus->getHalfClusterUMap() ){
+      if(map_halfClusUCol.find(iter.first)==map_halfClusUCol.end()) map_halfClusUCol[iter.first] = iter.second;
+      else{
+        for(int il=0; il<iter.second.size(); il++)
+          if( find(map_halfClusUCol[iter.first].begin(), map_halfClusUCol[iter.first].end(), iter.second[il])==map_halfClusUCol[iter.first].end() )
+            map_halfClusUCol[iter.first].push_back( iter.second[il] );
+      }
+    }
+    for(auto iter:_clus->getHalfClusterVMap() ){
+      if(map_halfClusVCol.find(iter.first)==map_halfClusVCol.end()) map_halfClusVCol[iter.first] = iter.second;
+      else{
+        for(int il=0; il<iter.second.size(); il++)
+          if( find(map_halfClusVCol[iter.first].begin(), map_halfClusVCol[iter.first].end(), iter.second[il])==map_halfClusVCol[iter.first].end() )
+            map_halfClusVCol[iter.first].push_back( iter.second[il] );
+      }
+    }
+
+  }
+
+
+  std::vector<const PandoraPlus::CaloUnit*> Calo3DCluster::getBars() const{
+    std::vector<const PandoraPlus::CaloUnit*> results; results.clear();
+    for(int i=0; i<m_2dclusters.size(); i++){
+      for(int j=0; j<m_2dclusters.at(i)->getBars().size(); j++){
+        results.push_back(m_2dclusters.at(i)->getBars().at(j));
+      }
+    }
+    return results;
+  }
+
+  double Calo3DCluster::getHitsE() const{
+    double en=0;
+    for(int i=0;i<hits.size(); i++) en+=hits[i]->getEnergy();
+    return en;
+  }
+
+  double Calo3DCluster::getEnergy() const{
+    double result = 0;
+    for(int m=0; m<m_2dclusters.size(); m++)
+      result += m_2dclusters[m]->getEnergy();
+    return result;
+  }
+
+  double Calo3DCluster::getLongiE() const{
+    double en=0;
+    for(auto iter: map_halfClusUCol){
+      if(iter.first!="LinkedLongiCluster") continue;
+      for(auto iclus: iter.second)
+        en += iclus->getEnergy();
+    }
+    for(auto iter: map_halfClusVCol){
+      if(iter.first!="LinkedLongiCluster") continue;
+      for(auto iclus: iter.second)
+        en += iclus->getEnergy();
+    }
+    return en;
+  }
+
+  TVector3 Calo3DCluster::getHitCenter() const{
+    TVector3 vec(0,0,0);
+    double totE = getHitsE();
+    for(int i=0;i<hits.size(); i++){
+       TVector3 v_cent = hits[i]->getPosition();
+       vec += v_cent * (hits[i]->getEnergy()/totE);
+    }
+    return vec;
+  }
+
+  TVector3 Calo3DCluster::getShowerCenter() const{
+    TVector3 spos(0,0,0);
+    double totE = 0.;
+    for(int i=0;i<m_2dclusters.size(); i++){ spos += m_2dclusters[i]->getPos()*m_2dclusters[i]->getEnergy(); totE += m_2dclusters[i]->getEnergy(); }
+    spos = spos*(1./totE);
+    return spos;
+  }
+
+  int Calo3DCluster::getBeginningDlayer() const{
+    int re_dlayer = -99;
+    std::vector<int> dlayers; dlayers.clear();
+    if(hits.size()!=0)  for(int ih=0; ih<hits.size(); ih++) dlayers.push_back(hits[ih]->getLayer());
+    else                for(int ish=0; ish<m_2dclusters.size(); ish++) dlayers.push_back(m_2dclusters[ish]->getDlayer());
+    re_dlayer = *std::min_element(dlayers.begin(), dlayers.end());
+
+    return re_dlayer;
+  }
+
+  int Calo3DCluster::getEndDlayer() const{
+    int re_dlayer = -99;
+    std::vector<int> dlayers; dlayers.clear();
+    if(hits.size()!=0)  for(int ih=0; ih<hits.size(); ih++) dlayers.push_back(hits[ih]->getLayer());
+    else                for(int ish=0; ish<m_2dclusters.size(); ish++) dlayers.push_back(m_2dclusters[ish]->getDlayer());
+    re_dlayer = *std::max_element(dlayers.begin(), dlayers.end());
+
+    return re_dlayer;
+  }
+
+  double Calo3DCluster::getDepthToECALSurface() const{
+    TVector3 pos = getShowerCenter();
+    return pos.Perp() - PandoraPlus::CaloUnit::ecal_innerR;
+  }
+
+
+  std::vector<const PandoraPlus::CaloHalfCluster*> Calo3DCluster::getHalfClusterUCol(std::string name) const {
+    std::vector<const CaloHalfCluster*> emptyCol; emptyCol.clear(); 
+    if(map_halfClusUCol.find(name)!=map_halfClusUCol.end()) emptyCol = map_halfClusUCol.at(name);
+    return emptyCol;
+  }
+
+  std::vector<const PandoraPlus::CaloHalfCluster*> Calo3DCluster::getHalfClusterVCol(std::string name) const {
+    std::vector<const CaloHalfCluster*> emptyCol; emptyCol.clear(); 
+    if(map_halfClusVCol.find(name)!=map_halfClusVCol.end()) emptyCol = map_halfClusVCol.at(name);
+    return emptyCol;
+  }
+
+  std::vector<const PandoraPlus::Calo1DCluster*> Calo3DCluster::getLocalMaxUCol(std::string name) const{
+    std::vector<const Calo1DCluster*> emptyCol; emptyCol.clear(); 
+    if(map_localMaxU.find(name)!=map_localMaxU.end()) emptyCol = map_localMaxU.at(name);
+    return emptyCol;
+  }
+
+  std::vector<const PandoraPlus::Calo1DCluster*> Calo3DCluster::getLocalMaxVCol(std::string name) const{
+    std::vector<const Calo1DCluster*> emptyCol; emptyCol.clear(); 
+    if(map_localMaxV.find(name)!=map_localMaxV.end()) emptyCol = map_localMaxV.at(name);
+    return emptyCol; 
+  }
+
+  std::vector< std::pair<edm4hep::MCParticle, float> > Calo3DCluster::getLinkedMCPfromHFCluster(std::string name){
+    MCParticleWeight.clear();
+
+    std::map<edm4hep::MCParticle, float> map_truthP_totE; map_truthP_totE.clear();
+    for(auto icl: map_halfClusUCol[name]){
+      for(auto ipair: icl->getLinkedMCP()) map_truthP_totE[ipair.first] += icl->getEnergy()*ipair.second;
+    }
+    for(auto icl: map_halfClusVCol[name]){
+      for(auto ipair: icl->getLinkedMCP()) map_truthP_totE[ipair.first] += icl->getEnergy()*ipair.second;
+    }
+
+    for(auto imcp: map_truthP_totE){
+      MCParticleWeight.push_back( std::make_pair(imcp.first, imcp.second/getLongiE()) );
+    }
+
+    return MCParticleWeight;
+  }
+
+  std::vector< std::pair<edm4hep::MCParticle, float> > Calo3DCluster::getLinkedMCPfromHit(){
+    MCParticleWeight.clear();
+
+    std::map<edm4hep::MCParticle, float> map_truthP_totE; map_truthP_totE.clear();
+    for(auto ihit: hits){
+      for(auto ipair: ihit->getLinkedMCP()) map_truthP_totE[ipair.first] += ihit->getEnergy()*ipair.second;
+    }
+
+    for(auto imcp: map_truthP_totE){
+      MCParticleWeight.push_back( std::make_pair(imcp.first, imcp.second/getHitsE()) );
+    }
+
+    return MCParticleWeight;
+  }  
+
+  edm4hep::MCParticle Calo3DCluster::getLeadingMCP() const{
+    float maxWeight = -1.;
+    edm4hep::MCParticle mcp;
+    for(auto& iter: MCParticleWeight){
+      if(iter.second>maxWeight){
+        mcp = iter.first;
+        maxWeight = iter.second;
+      }
+    }
+
+    return mcp;
+  }
+
+  float Calo3DCluster::getLeadingMCPweight() const{
+    float maxWeight = -1.;
+    for(auto& iter: MCParticleWeight){
+      if(iter.second>maxWeight){
+        maxWeight = iter.second;
+      }
+    }
+    return maxWeight;
+  }
+
+
+  void Calo3DCluster::FitAxis(){
+    if(m_2dclusters.size()==0) axis.SetXYZ(0,0,0);
+
+    else if(m_2dclusters.size()==1){
+      axis = m_2dclusters[0]->getPos();
+      axis *= 1./axis.Mag();
+    }
+
+    else if( m_2dclusters.size()==2 ){
+      TVector3 pos1 = m_2dclusters[0]->getPos();
+      TVector3 pos2 = m_2dclusters[1]->getPos();
+
+      axis = ( pos1.Mag()>pos2.Mag() ? pos1-pos2 : pos2-pos1 );
+      axis *= 1./axis.Mag();
+    }
+
+    else{
+      trackFitter.clear();
+      //track->setImpactParameter(0., 0.); //fix dr=0, dz=0.
+
+      double barAngle = (towerID[0][0]+2)*TMath::Pi()/4.;
+      double posErr = 10./sqrt(12);
+      if(barAngle>=TMath::TwoPi()) barAngle = barAngle-TMath::TwoPi();
+      trackFitter.setBarAngle(barAngle);
+      for(int is=0;is<m_2dclusters.size();is++){
+        TVector3 pos_barsX = m_2dclusters[is]->getShowerUCol()[0]->getPos(); //U
+        TVector3 pos_barsY = m_2dclusters[is]->getShowerVCol()[0]->getPos(); //Z
+//printf("\t DEBUG: input pointX (%.3f, %.3f, %.3f) \n", barsX.getPos().x(), barsX.getPos().y(), barsX.getPos().z());
+//printf("\t DEBUG: input pointY (%.3f, %.3f, %.3f) \n", barsY.getPos().x(), barsY.getPos().y(), barsY.getPos().z());
+        trackFitter.setGlobalPoint(1, pos_barsX.x(), posErr, pos_barsX.y(), posErr, pos_barsX.z(), posErr);
+        trackFitter.setGlobalPoint(0, pos_barsY.x(), posErr, pos_barsY.y(), posErr, pos_barsY.z(), posErr);
+      }
+      trackFitter.fitTrack();
+      double fitPhi =   trackFitter.getTrkPar(2);
+      double fitTheta = trackFitter.getTrkPar(3);
+//printf("\t DEBUG: fitted phi and theta: %.3f \t %.3f \n", fitPhi, fitTheta);
+
+      axis.SetPhi(fitPhi);
+      axis.SetTheta(fitTheta);
+      axis.SetMag(1.);
+    }
+  }
+
+  //void Calo3DCluster::FitAxisHit(){
+  //}
+
+  //void Calo3DCluster::FitProfile(){
+  //}
+
+  bool Calo3DCluster::isHCALNeighbor(const PandoraPlus::CaloHit* m_hit) const
+  {
+    for(int i=0; i<hits.size(); i++)
+    {
+      if( sqrt( pow(m_hit->getPosition().x()-hits[i]->getPosition().x(),2) + pow(m_hit->getPosition().y()-hits[i]->getPosition().y(),2) + pow(m_hit->getPosition().z()-hits[i]->getPosition().z(),2) ) <= sqrt(pow(40,2)*3) )
+        return true;
+    }
+    return false;
+  }
+
+  void Calo3DCluster::setCaloHitsFrom2DCluster(){
+    hits.clear();
+    for(int i=0; i<m_2dclusters.size(); i++){
+      std::vector<const CaloHit*> tmp_hits = m_2dclusters[i]->getCaloHits();
+      hits.insert(hits.end(), tmp_hits.begin(), tmp_hits.end());
+    }
+  }
+
+};
+#endif
diff --git a/Reconstruction/CrystalCaloRec/src/Objects/CaloHalfCluster.cc b/Reconstruction/CrystalCaloRec/src/Objects/CaloHalfCluster.cc
new file mode 100644
index 0000000000000000000000000000000000000000..e5a463958f26d90e98ca0bff146e3b509f35f94b
--- /dev/null
+++ b/Reconstruction/CrystalCaloRec/src/Objects/CaloHalfCluster.cc
@@ -0,0 +1,453 @@
+#ifndef CALO_HALFCLUSTER_C
+#define CALO_HALFCLUSTER_C
+
+#include "Objects/CaloHalfCluster.h"
+
+namespace PandoraPlus{
+
+  void CaloHalfCluster::Clear() {
+    type = -1; 
+    slayer=-99;
+    towerID.clear(); 
+    m_1dclusters.clear(); 
+    map_localMax.clear();
+    map_halfClusCol.clear();
+  }
+
+  void CaloHalfCluster::Check(){
+    for(int i=0; i<m_1dclusters.size(); i++)
+      if(!m_1dclusters[i]) { m_1dclusters.erase(m_1dclusters.begin()+i); i--; }
+  }
+
+  void CaloHalfCluster::Clean(){
+    //for(int i=0; i<m_1dclusters.size(); i++){ delete m_1dclusters[i]; m_1dclusters[i]=NULL; }
+    for(auto it: map_localMax) { 
+      //for(auto iter: it.second){ delete it.second[iter]; it.second[iter]=NULL; }
+      it.second.clear();
+    }
+    for(auto it: map_halfClusCol) {
+      //for(auto iter: it.second){ delete it.second[iter]; it.second[iter]=NULL; }
+      it.second.clear();
+    }
+    Clear();
+  }
+
+  std::shared_ptr<PandoraPlus::CaloHalfCluster> CaloHalfCluster::Clone() const{
+    std::shared_ptr<PandoraPlus::CaloHalfCluster> m_clus = std::make_shared<PandoraPlus::CaloHalfCluster>();
+    for(int i1d=0; i1d<m_1dclusters.size(); i1d++)  m_clus->addUnit(m_1dclusters[i1d]);
+    for(int itrk=0; itrk<m_TrackCol.size(); itrk++) m_clus->addAssociatedTrack(m_TrackCol[itrk]);
+    for(auto iter:map_localMax)     m_clus->setLocalMax( iter.first, iter.second );
+    for(auto iter:map_halfClusCol)  m_clus->setHalfClusters( iter.first, iter.second );
+    m_clus->setLinkedMCP( MCParticleWeight );
+    m_clus->setHoughPars( Hough_alpha, Hough_rho );
+    m_clus->setIntercept( Hough_intercept );
+    m_clus->setType( type );
+
+    return m_clus;
+  }
+
+  bool CaloHalfCluster::isNeighbor(const PandoraPlus::Calo1DCluster* m_1dcluster) const{
+    assert(m_1dcluster->getBars().size() > 0 && getCluster().at(0)->getBars().size()>0 );
+    if(m_1dcluster->getSlayer() != getSlayer()  ) return false; 
+
+    for(int i1d=0; i1d<m_1dclusters.size(); i1d++)
+    {
+      for(int ibar=0; ibar<m_1dcluster->getBars().size(); ibar++)
+      {
+        for(int jbar=0; jbar<m_1dclusters.at(i1d)->getBars().size(); jbar++)
+        {
+          if( m_1dcluster->getBars().at(ibar)->isLongiNeighbor(m_1dclusters.at(i1d)->getBars().at(jbar)) ) return true;
+          //if( m_1dcluster->getBars().at(ibar)->isLongiModuleAdjacent(m_1dclusters.at(i1d)->getBars().at(jbar)) ) return true;
+        }
+      }
+    }
+    
+    return false;
+  }
+
+ 
+  void CaloHalfCluster::addUnit(const Calo1DCluster* _1dcluster)
+  {
+    if( find( m_1dclusters.begin(), m_1dclusters.end(), _1dcluster)!=m_1dclusters.end() )
+      //std::cout<<"ERROR: attempt to add an existing 1DCluster into HalfCluster! Skip it "<<std::endl;
+      return; 
+    else{
+      if(_1dcluster->getSlayer()==0) slayer=0;
+      if(_1dcluster->getSlayer()==1) slayer=1;
+      m_1dclusters.push_back(_1dcluster);
+   
+      std::vector< std::vector<int> > id = _1dcluster->getTowerID();
+      for(int ii=0; ii<id.size(); ii++)
+        if( find(towerID.begin(), towerID.end(), id[ii])==towerID.end() ) towerID.push_back(id[ii]);    
+   
+      fitAxis("");
+    }
+  }
+  
+
+  void CaloHalfCluster::deleteUnit(const Calo1DCluster* _1dcluster)
+  {
+    auto iter = find( m_1dclusters.begin(), m_1dclusters.end(), _1dcluster); 
+    if( iter != m_1dclusters.end() ){
+      m_1dclusters.erase(iter);
+      fitAxis("");
+    }
+  }
+
+
+  std::vector<const CaloUnit*> CaloHalfCluster::getBars() const
+  {
+    std::vector<const CaloUnit*> results;
+    results.clear();
+    for(int i=0; i<m_1dclusters.size(); i++)
+    {
+      for(int j=0; j<m_1dclusters.at(i)->getBars().size(); j++)
+      {
+        results.push_back(m_1dclusters.at(i)->getBars().at(j));
+      }
+    }
+    return results;
+  }
+
+
+  double CaloHalfCluster::getEnergy() const {
+    double sumE = 0;
+    for(int i=0; i<m_1dclusters.size(); i++)
+    {
+      sumE = sumE + m_1dclusters.at(i)->getEnergy();
+    }
+    return sumE;
+  }
+
+  TVector3 CaloHalfCluster::getPos() const{
+    TVector3 pos(0, 0, 0);
+    double Etot = getEnergy();
+    for(int i=0; i<m_1dclusters.size(); i++){
+      TVector3 m_pos(m_1dclusters[i]->getPos().x(), m_1dclusters[i]->getPos().y(), m_1dclusters[i]->getPos().z());
+      pos += m_pos * (m_1dclusters[i]->getEnergy()/Etot);
+    }
+    return pos;
+  }
+
+  TVector3 CaloHalfCluster::getEnergyCenter() const{
+    TVector3 pos = getPos();
+    double maxEn = -99;
+    for(int i=0; i<m_1dclusters.size(); i++){
+      if(m_1dclusters[i]->getEnergy()>maxEn){
+        maxEn = m_1dclusters[i]->getEnergy();
+        pos = m_1dclusters[i]->getPos();
+      }
+    }
+    return pos;
+  }
+
+  std::vector<int> CaloHalfCluster::getEnergyCenterTower() const{
+    std::vector<int> tower = getTowerID()[0];
+    double maxEn = -99;
+    for(int i=0; i<m_1dclusters.size(); i++){
+      if(m_1dclusters[i]->getEnergy()>maxEn){
+        maxEn = m_1dclusters[i]->getEnergy();
+        tower = m_1dclusters[i]->getTowerID()[0];
+      }
+    }
+    return tower;
+  }
+
+  std::vector<const PandoraPlus::Calo1DCluster*> CaloHalfCluster::getLocalMaxCol(std::string name) const{
+    std::vector<const PandoraPlus::Calo1DCluster*> emptyCol; emptyCol.clear(); 
+    if(map_localMax.find(name)!=map_localMax.end()) emptyCol = map_localMax.at(name);
+    return emptyCol;
+  }
+
+  std::vector<const Calo1DCluster*> CaloHalfCluster::getAllLocalMaxCol() const{
+    std::vector<const PandoraPlus::Calo1DCluster*> emptyCol; emptyCol.clear();
+    for(auto &iter: map_localMax) emptyCol.insert(emptyCol.end(), iter.second.begin(), iter.second.end());
+    return emptyCol;
+  }
+
+  std::vector<const PandoraPlus::CaloHalfCluster*> CaloHalfCluster::getHalfClusterCol(std::string name) const{
+    std::vector<const PandoraPlus::CaloHalfCluster*> emptyCol; emptyCol.clear(); 
+    if(map_halfClusCol.find(name)!=map_halfClusCol.end()) emptyCol = map_halfClusCol.at(name);
+    return emptyCol;
+  }
+
+  std::vector<const PandoraPlus::CaloHalfCluster*> CaloHalfCluster::getAllHalfClusterCol() const{
+    std::vector<const PandoraPlus::CaloHalfCluster*> emptyCol; emptyCol.clear();
+    for(auto &iter: map_halfClusCol) emptyCol.insert(emptyCol.end(), iter.second.begin(), iter.second.end());
+    return emptyCol;
+  }
+
+  std::vector<const PandoraPlus::Calo1DCluster*> CaloHalfCluster::getClusterInLayer(int _layer) const{
+    std::vector<const PandoraPlus::Calo1DCluster*> outShowers; outShowers.clear();
+    for(int i=0; i<m_1dclusters.size(); i++)
+      if(m_1dclusters[i]->getDlayer()==_layer) outShowers.push_back(m_1dclusters[i]);
+    return outShowers;
+  }
+
+
+  int CaloHalfCluster::getBeginningDlayer() const{
+    int Lstart = 99;
+    for(int i=0; i<m_1dclusters.size(); i++)
+      if(m_1dclusters[i]->getDlayer()<Lstart) Lstart = m_1dclusters[i]->getDlayer();
+    if(Lstart==99) return -99;
+    else return Lstart;
+  }
+
+
+  int CaloHalfCluster::getEndDlayer() const{
+    int Lend = -99;
+    for(int i=0; i<m_1dclusters.size(); i++)
+      if(m_1dclusters[i]->getDlayer()>Lend) Lend = m_1dclusters[i]->getDlayer();
+    return Lend;
+  }  
+
+  bool CaloHalfCluster::isContinue() const{
+    int ly_max = -1;
+    int ly_min = 99;
+    std::vector<int> vec_layers; vec_layers.clear();
+    for(int il=0; il<m_1dclusters.size(); il++){
+      vec_layers.push_back(m_1dclusters[il]->getDlayer());
+      if(m_1dclusters[il]->getDlayer()>ly_max) ly_max = m_1dclusters[il]->getDlayer();
+      if(m_1dclusters[il]->getDlayer()<ly_min) ly_min = m_1dclusters[il]->getDlayer();
+    }
+
+    bool flag = true;
+    std::sort(vec_layers.begin(), vec_layers.end());
+    for(int il=0; il<vec_layers.size() && vec_layers[il]!=ly_max; il++)
+      if( find(vec_layers.begin(), vec_layers.end(), vec_layers[il]+1) == vec_layers.end() ){ flag=false; break; }
+
+    return flag;
+  }
+
+  bool CaloHalfCluster::isContinueN(int n) const{
+    if(n<=0) return true;
+
+    int ly_max = -1;
+    int ly_min = 99;
+    std::vector<int> vec_layers; vec_layers.clear();
+    for(int il=0; il<m_1dclusters.size(); il++){
+      vec_layers.push_back(m_1dclusters[il]->getDlayer());
+      if(m_1dclusters[il]->getDlayer()>ly_max) ly_max = m_1dclusters[il]->getDlayer();
+      if(m_1dclusters[il]->getDlayer()<ly_min) ly_min = m_1dclusters[il]->getDlayer();
+    }
+    if(n>(ly_max-ly_min+1)) return false;
+
+    bool flag = false;
+    std::sort(vec_layers.begin(), vec_layers.end());
+    for(int il=0; il<vec_layers.size(); il++){
+      bool fl_continueN = true;
+      for(int in=1; in<n; in++)
+        if( find(vec_layers.begin(), vec_layers.end(), vec_layers[il]+in)==vec_layers.end() ) { fl_continueN=false; break; }
+
+      if(fl_continueN) {flag = true; break;}
+    }
+    return flag;
+  }
+
+
+  bool CaloHalfCluster::isSubset(const CaloHalfCluster* clus) const{
+
+    for(int is=0; is<clus->getCluster().size(); is++)
+      if( find(m_1dclusters.begin(), m_1dclusters.end(), clus->getCluster()[is])==m_1dclusters.end() ) {return false; }
+
+    if(m_1dclusters.size() > clus->getCluster().size())
+      return true;
+    else{
+      if( TMath::Abs(Hough_rho) <= TMath::Abs(clus->getHoughRho()) ) { return true; }
+      else { return false; }
+    }
+  }
+
+  double CaloHalfCluster::OverlapRatioE( const CaloHalfCluster* clus) const{
+    double Eshare = 0.;
+    for(int is=0; is<clus->getCluster().size(); is++)
+      if( find(m_1dclusters.begin(), m_1dclusters.end(), clus->getCluster()[is])!=m_1dclusters.end() ) Eshare += clus->getCluster()[is]->getEnergy();
+
+    return Eshare / getEnergy() ;
+  }
+
+
+  void CaloHalfCluster::fitAxis( std::string name ){
+    std::vector<const PandoraPlus::Calo1DCluster*> barShowerCol; barShowerCol.clear();
+    if(!name.empty() && map_localMax.find(name)!=map_localMax.end() ) barShowerCol = map_localMax.at(name);
+    else barShowerCol = m_1dclusters; 
+
+    if(barShowerCol.size()==0){ axis.SetXYZ(0,0,0); return; }
+    else if(barShowerCol.size()==1){
+      axis.SetXYZ(barShowerCol[0]->getPos().x(), barShowerCol[0]->getPos().y(), barShowerCol[0]->getPos().z());
+      axis = axis.Unit();
+      return;
+    }
+    else if(barShowerCol.size()==2){
+      TVector3 rpos = barShowerCol.back()->getPos() - barShowerCol.front()->getPos();
+      axis.SetXYZ( rpos.x(), rpos.y(), rpos.z() );
+      axis = axis.Unit();
+      return;
+    }
+    else{
+      track->clear();
+      double barAngle = (barShowerCol[0]->getTowerID()[0][0]+PandoraPlus::CaloUnit::Nmodule/4.)*2*TMath::Pi()/PandoraPlus::CaloUnit::Nmodule;
+      double posErr = PandoraPlus::CaloUnit::barsize/sqrt(12);
+      if(barAngle>=TMath::TwoPi()) barAngle = barAngle-TMath::TwoPi();
+      track->setBarAngle(barAngle);
+      for(int is=0; is<barShowerCol.size(); is++){
+        TVector3 b_pos = barShowerCol[is]->getPos();
+        track->setGlobalPoint(0, b_pos.x(), posErr, b_pos.y(), posErr, b_pos.z(), posErr);
+        track->setGlobalPoint(1, b_pos.x(), posErr, b_pos.y(), posErr, b_pos.z(), posErr);
+      }
+
+      track->fitTrack();
+      double fitPhi = track->getPhi();
+      double fitTheta = track->getTheta();
+
+      trk_dr = track->getDr();
+      trk_dz = track->getDz();
+      axis.SetPhi(fitPhi);
+      axis.SetTheta(fitTheta);
+      axis.SetMag(1.);
+    }
+  }
+
+  void CaloHalfCluster::mergeHalfCluster(const CaloHalfCluster* clus ){
+    for(int is=0; is<clus->getCluster().size(); is++){
+      if( find(m_1dclusters.begin(), m_1dclusters.end(), clus->getCluster()[is])==m_1dclusters.end() )
+        addUnit( clus->getCluster()[is] );
+    }
+
+    for(int itrk=0; itrk<clus->getAssociatedTracks().size(); itrk++){
+      if( find(m_TrackCol.begin(), m_TrackCol.end(), clus->getAssociatedTracks()[itrk])==m_TrackCol.end() )
+        m_TrackCol.push_back( clus->getAssociatedTracks()[itrk] );
+    }
+
+    for(auto iter:clus->getLocalMaxMap() ){
+      if(map_localMax.find(iter.first)==map_localMax.end()) map_localMax[iter.first] = iter.second;
+      else{
+        for(int il=0; il<iter.second.size(); il++)
+          if( find(map_localMax[iter.first].begin(), map_localMax[iter.first].end(), iter.second[il])==map_localMax[iter.first].end() ) 
+            map_localMax[iter.first].push_back( iter.second[il] );
+      }
+    }
+
+    for(auto iter:clus->getHalfClusterMap() ){
+      if(map_halfClusCol.find(iter.first)==map_halfClusCol.end()) map_halfClusCol[iter.first] = iter.second;
+      else{
+        for(int il=0; il<iter.second.size(); il++)
+          if( find(map_halfClusCol[iter.first].begin(), map_halfClusCol[iter.first].end(), iter.second[il])==map_halfClusCol[iter.first].end() )
+            map_halfClusCol[iter.first].push_back( iter.second[il] );
+      }
+    }
+
+    fitAxis("");
+  }
+
+
+  void CaloHalfCluster::mergeClusterInLayer(){
+    //std::map<std::vector<int>, std::vector<const Calo1DCluster*>> map_showerinTowers; map_showerinTowers.clear();
+    //for(int is=0; is<m_1dclusters.size(); is++){
+    //  if(m_1dclusters[is]->getNseeds()==0) continue;
+    //  std::vector<int> m_seedID(3);
+    //  m_seedID[0] = m_1dclusters[is]->getSeeds()[0]->getModule();
+    //  m_seedID[1] = m_1dclusters[is]->getSeeds()[0]->getPart();
+    //  m_seedID[2] = m_1dclusters[is]->getSeeds()[0]->getStave();
+    //  map_showerinTowers[m_seedID].push_back( m_1dclusters[is] );
+    //}
+
+    //m_1dclusters.clear();    
+    //for(auto itower : map_showerinTowers){
+
+      std::map<int, std::vector<const Calo1DCluster*>> showersinlayer; showersinlayer.clear();
+      //for(int is=0; is<itower.second.size(); is++)
+        //showersinlayer[itower.second[is]->getDlayer()].push_back( itower.second[is] );
+      for(int is=0; is<m_1dclusters.size(); is++)
+        showersinlayer[m_1dclusters[is]->getDlayer()].push_back( m_1dclusters[is] );
+
+      m_1dclusters.clear();    
+      for(auto &iter : showersinlayer){
+        std::vector<const Calo1DCluster*> tmp_shower = iter.second; 
+        if(tmp_shower.size()>1){
+          //Merge following showers into the first: 
+   
+          PandoraPlus::CaloUnit* p_seed = nullptr;
+          float maxEseed = -99;
+          for(int is=0; is<tmp_shower[0]->getNseeds(); is++){
+            if(tmp_shower[0]->getSeeds()[is]->getEnergy()>maxEseed){
+              p_seed = const_cast<PandoraPlus::CaloUnit*>(tmp_shower[0]->getSeeds()[is]);
+              maxEseed=tmp_shower[0]->getSeeds()[is]->getEnergy();
+            }
+          }
+          
+          for(int is=1; is<tmp_shower.size(); is++){
+            //Find maxE as seed
+            for(int iseed=0; iseed<tmp_shower[is]->getNseeds(); iseed++){
+              if(tmp_shower[is]->getSeeds()[iseed]->getEnergy()>maxEseed){ 
+                p_seed = const_cast<PandoraPlus::CaloUnit*>(tmp_shower[is]->getSeeds()[iseed]); 
+                maxEseed=tmp_shower[is]->getSeeds()[iseed]->getEnergy(); 
+              }
+            }
+            //Bars
+            for(int icl=0; icl<tmp_shower[is]->getBars().size(); icl++){
+              const_cast<PandoraPlus::Calo1DCluster*>(tmp_shower[0])->addUnit(tmp_shower[is]->getBars()[icl]);
+            }
+            tmp_shower.erase(tmp_shower.begin()+is);
+            is--;
+          }
+          if(p_seed){
+            std::vector<const PandoraPlus::CaloUnit*> tmp_seed; tmp_seed.clear();
+            tmp_seed.push_back(p_seed);
+            const_cast<PandoraPlus::Calo1DCluster*>(tmp_shower[0])->setSeeds( tmp_seed );
+          }
+        }
+   
+        addUnit(tmp_shower[0]);
+      }
+    //}
+  }
+
+
+  void CaloHalfCluster::deleteCousinCluster( const PandoraPlus::CaloHalfCluster* _cl ){
+    auto iter = find( map_halfClusCol["CousinCluster"].begin(), map_halfClusCol["CousinCluster"].end(), _cl );
+    if(iter!=map_halfClusCol["CousinCluster"].end()) map_halfClusCol["CousinCluster"].erase( iter );
+  }
+
+
+  std::vector< std::pair<edm4hep::MCParticle, float> > CaloHalfCluster::getLinkedMCPfromUnit(){
+    MCParticleWeight.clear();
+
+    std::map<edm4hep::MCParticle, float> map_truthP_totE; map_truthP_totE.clear();
+    for(auto ish : m_1dclusters ){
+      for(auto ibar : ish->getBars()){
+        for(auto ipair : ibar->getLinkedMCP()) map_truthP_totE[ipair.first] += ibar->getEnergy()*ipair.second;
+      }
+    }
+
+    for(auto imcp: map_truthP_totE){
+      MCParticleWeight.push_back( std::make_pair(imcp.first, imcp.second/getEnergy()) );
+    }
+
+    return MCParticleWeight;
+  }
+
+  edm4hep::MCParticle CaloHalfCluster::getLeadingMCP() const{
+    float maxWeight = -1.;
+    edm4hep::MCParticle mcp; 
+    for(auto& iter: MCParticleWeight){
+      if(iter.second>maxWeight){
+        mcp = iter.first;
+        maxWeight = iter.second;
+      }
+    }
+
+    return mcp;
+  }
+
+  float CaloHalfCluster::getLeadingMCPweight() const{
+    float maxWeight = -1.;
+    for(auto& iter: MCParticleWeight){
+      if(iter.second>maxWeight){
+        maxWeight = iter.second;
+      }
+    }
+    return maxWeight;
+  }
+};
+#endif
diff --git a/Reconstruction/CrystalCaloRec/src/Objects/CaloHit.cc b/Reconstruction/CrystalCaloRec/src/Objects/CaloHit.cc
new file mode 100644
index 0000000000000000000000000000000000000000..baac05bbe2f35acee53ba09e63ec1b130c2866f6
--- /dev/null
+++ b/Reconstruction/CrystalCaloRec/src/Objects/CaloHit.cc
@@ -0,0 +1,46 @@
+#ifndef CALO_HIT_C
+#define CALO_HIT_C
+
+#include "Objects/CaloHit.h"
+
+namespace PandoraPlus{
+
+
+  std::shared_ptr<CaloHit> CaloHit::Clone() const{
+    std::shared_ptr<CaloHit> _newhit = std::make_shared<CaloHit>();
+    _newhit->setcellID(cellID);
+    _newhit->setLayer(layer);
+    _newhit->setPosition(position);
+    _newhit->setEnergy(energy);
+    _newhit->setParentShower(ParentShower);
+    _newhit->setLinkedMCP(MCParticleWeight);
+    edm4hep::CalorimeterHit originhit = getOriginHit();
+    _newhit->setOriginHit( originhit );
+    return _newhit;
+  }
+
+  edm4hep::MCParticle CaloHit::getLeadingMCP() const{
+    float maxWeight = -1.;
+    edm4hep::MCParticle mcp;
+    for(auto& iter: MCParticleWeight){
+      if(iter.second>maxWeight){
+        mcp = iter.first;
+        maxWeight = iter.second;
+      }
+    }
+
+    return mcp;
+  }
+
+  float CaloHit::getLeadingMCPweight() const{
+    float maxWeight = -1.;
+    for(auto& iter: MCParticleWeight){
+      if(iter.second>maxWeight){
+        maxWeight = iter.second;
+      }
+    }
+    return maxWeight;
+  }
+
+};
+#endif
diff --git a/Reconstruction/CrystalCaloRec/src/Objects/CaloUnit.cc b/Reconstruction/CrystalCaloRec/src/Objects/CaloUnit.cc
new file mode 100644
index 0000000000000000000000000000000000000000..694ecc714dd01a9598f23ec74c31b848a134503d
--- /dev/null
+++ b/Reconstruction/CrystalCaloRec/src/Objects/CaloUnit.cc
@@ -0,0 +1,188 @@
+#ifndef CALOBAR_C
+#define CALOBAR_C
+
+#include "Objects/CaloUnit.h"
+#include <cmath>
+
+namespace PandoraPlus{
+
+  bool CaloUnit::isNeighbor(const CaloUnit* x) const {
+    if( cellID==x->getcellID() ) return false;
+    if( system!=x->getSystem() || dlayer!=x->getDlayer() || slayer!=x->getSlayer() ) return false; 
+
+    if( module==x->getModule() && stave==x->getStave() && fabs(bar - x->getBar())==1 ) return true;
+    if( slayer==0 && stave==x->getStave() && (fabs(module-x->getModule())<=1 || fabs(module-x->getModule())==Nmodule-1 ) && fabs(bar - x->getBar())<=1 ) return true;
+    if( slayer==1 && module==x->getModule() && fabs(stave-x->getStave())<=1 && fabs(bar - x->getBar())<=1 ) return true;
+
+    if( isAtLowerEdgeZ()   && x->isAtUpperEdgeZ()   && x->getStave()==stave-1 && (fabs(x->getModule()-module)<=1 || fabs(x->getModule()-module)==Nmodule-1 ) ) return true;
+    if( isAtUpperEdgeZ()   && x->isAtLowerEdgeZ()   && x->getStave()==stave+1 && (fabs(x->getModule()-module)<=1 || fabs(x->getModule()-module)==Nmodule-1 ) ) return true;
+    if( isAtLowerEdgePhi() && x->isAtUpperEdgePhi() && (x->getModule()==module-1 || (x->getModule() - module)==Nmodule-1 ) && fabs(x->getStave()-stave)<=1 ) return true;
+    if( isAtUpperEdgePhi() && x->isAtLowerEdgePhi() && (x->getModule()==module+1 || (module - x->getModule())==Nmodule-1 ) && fabs(x->getStave()-stave)<=1 ) return true;
+
+    return false;
+  }
+
+  bool CaloUnit::isLongiNeighbor(const CaloUnit* x) const {
+    if( cellID==x->getcellID() ) return false;
+    if( system!=x->getSystem() || slayer!=x->getSlayer() ) return false; 
+    if( abs(dlayer-x->getDlayer())!=1) return false;
+    if( slayer==0 )
+    {
+      if( stave==x->getStave() && (fabs(module-x->getModule())<=1 || fabs(module-x->getModule())==Nmodule-1 ) && fabs(bar - x->getBar())<=1 ) return true;
+      if( isAtLowerEdgeZ()   && x->isAtUpperEdgeZ()   && x->getStave()==stave-1 && (fabs(x->getModule()-module)<=1 || fabs(x->getModule()-module)==Nmodule-1 ) ) return true;
+      if( isAtUpperEdgeZ()   && x->isAtLowerEdgeZ()   && x->getStave()==stave+1 && (fabs(x->getModule()-module)<=1 || fabs(x->getModule()-module)==Nmodule-1 ) ) return true;
+    }
+    if( slayer==1 )
+    {
+      if(module==x->getModule() && fabs(stave-x->getStave())<=1 && fabs(bar - x->getBar())<=1 ) return true;
+      if( isAtLowerEdgePhi() && x->isAtUpperEdgePhi() && (x->getModule()==module-1 || (x->getModule() - module)==Nmodule-1 ) && fabs(x->getStave()-stave)<=1 ) return true;
+      if( isAtUpperEdgePhi() && x->isAtLowerEdgePhi() && (x->getModule()==module+1 || (module - x->getModule())==Nmodule-1 ) && fabs(x->getStave()-stave)<=1 ) return true;
+    }
+    
+    return false;
+  }
+
+  bool CaloUnit::isAtLowerEdgePhi() const{
+    return ( slayer==1 && bar==0 ); 
+  }
+
+
+  bool CaloUnit::isAtUpperEdgePhi() const{
+    bool isEdge = false; 
+    if(module%2==0){
+      if(slayer==1 && bar==NbarPhi_even[dlayer]-1) isEdge = true;
+    }
+    else{
+      if(slayer==1 && bar==NbarPhi_odd[dlayer]-1) isEdge = true;
+    }
+    return isEdge; 
+  }
+
+
+  bool CaloUnit::isAtLowerEdgeZ() const{
+    return ( slayer==0 && bar==0 );
+  }
+
+
+  bool CaloUnit::isAtUpperEdgeZ() const{
+    return ( slayer==0 && bar==NbarZ-1 );
+  }
+
+/*
+  bool CaloUnit::isModuleAdjacent( const CaloUnit* x ) const{
+
+    if(module==x->getModule()) return false;
+    if( fabs(module-x->getModule())>1 && abs(module-x->getModule())!=Nmodule-1 ) return false; 
+
+    int dlayer_lo, slayer_lo, part_lo, stave_lo, bar_lo;
+    int dlayer_hi, slayer_hi, part_hi, stave_hi, bar_hi;
+    if( module - x->getModule()==1 || x->getModule()-module==Nmodule-1 ) {
+      dlayer_lo=x->getDlayer(); slayer_lo=x->getSlayer(); part_lo=x->getPart(); stave_lo=x->getStave(); bar_lo=x->getBar(); 
+      dlayer_hi=dlayer; slayer_hi=slayer; part_hi=part; stave_hi=stave; bar_hi=bar; 
+    }
+    else{
+      dlayer_lo=dlayer; slayer_lo=slayer; part_lo=part; stave_lo=stave; bar_lo=bar;
+      dlayer_hi=x->getDlayer(); slayer_hi=x->getSlayer(); part_hi=x->getPart(); stave_hi=x->getStave(); bar_hi=x->getBar();
+    }
+
+
+    if( dlayer_lo!=1 || slayer_lo!=0 || part_lo!=Npart || part_hi!=1 ) return false;
+    if( slayer_hi==0 ){
+      if( (stave_lo==stave_hi && abs(bar_lo-bar_hi)<=1) || 
+          (abs(stave_lo-stave_hi)==1 && isAtLowerEdgeZ() && x->isAtUpperEdgeZ() ) ||
+          (abs(stave_lo-stave_hi)==1 && isAtUpperEdgeZ() && x->isAtLowerEdgeZ() )  ) return true; 
+    }
+    else if( slayer_hi==1 && stave_lo==stave_hi && bar_hi==1 ) return true;
+
+    return false; 
+  }
+*/
+
+  std::shared_ptr<CaloUnit> CaloUnit::Clone() const{
+    std::shared_ptr<CaloUnit> m_bar = std::make_shared<CaloUnit>();
+    m_bar->setcellID(cellID);
+    m_bar->setcellID( system, module, stave, dlayer, slayer, bar );
+    m_bar->setPosition(position);
+    m_bar->setQ(Q1, Q2);
+    m_bar->setT(T1, T2);
+    for(auto ilink : MCParticleWeight) m_bar->addLinkedMCP(ilink);
+    return m_bar;
+  }
+
+  edm4hep::MCParticle CaloUnit::getLeadingMCP() const{
+    float maxWeight = -1.;
+    edm4hep::MCParticle mcp;
+    for(auto& iter: MCParticleWeight){
+      if(iter.second>maxWeight){
+        mcp = iter.first;
+        maxWeight = iter.second;
+      }
+    }
+
+    return mcp;
+  }
+
+  float CaloUnit::getLeadingMCPweight() const{
+    float maxWeight = -1.;
+    for(auto& iter: MCParticleWeight){
+      if(iter.second>maxWeight){
+        maxWeight = iter.second;
+      }
+    }
+    return maxWeight;
+  }
+
+/*  bool CaloUnit::isLongiModuleAdjacent( const CaloUnit* x ) const{
+
+    if(module==x->getModule()) return false;
+    if( fabs(module-x->getModule())>1 && abs(module-x->getModule())!=Nmodule-1 ) return false; 
+
+    int dlayer_lo, slayer_lo, part_lo, stave_lo, bar_lo;
+    int dlayer_hi, slayer_hi, part_hi, stave_hi, bar_hi;
+
+    if( module - x->getModule()==1 || x->getModule()-module==Nmodule-1 ) {
+      dlayer_lo=x->getDlayer(); slayer_lo=x->getSlayer(); part_lo=x->getPart(); stave_lo=x->getStave(); bar_lo=x->getBar(); 
+      dlayer_hi=dlayer; slayer_hi=slayer; part_hi=part; stave_hi=stave; bar_hi=bar; 
+    }
+    else{
+      dlayer_lo=dlayer; slayer_lo=slayer; part_lo=part; stave_lo=stave; bar_lo=bar;
+      dlayer_hi=x->getDlayer(); slayer_hi=x->getSlayer(); part_hi=x->getPart(); stave_hi=x->getStave(); bar_hi=x->getBar();
+    }
+
+    // if( dlayer_lo!=1 || slayer_lo!=0 || part_lo!=Npart || part_hi!=1 ) return false;
+    // if( slayer_hi==0 ){
+    //   if( (stave_lo==stave_hi && abs(bar_lo-bar_hi)<=1) || 
+    //       (abs(stave_lo-stave_hi)==1 && isAtLowerEdgeZ() && x->isAtUpperEdgeZ() ) ||
+    //       (abs(stave_lo-stave_hi)==1 && isAtUpperEdgeZ() && x->isAtLowerEdgeZ() )  ) return true; 
+    // }
+    // else if( slayer_hi==1 && stave_lo==stave_hi && bar_hi==1 ) return true;
+    
+    if(slayer_lo==0)
+    {
+      if(part_lo==Npart && part_hi==1 && dlayer_lo==1)
+      {
+        if( (stave_lo==stave_hi && abs(bar_lo-bar_hi)<=1) || 
+          (abs(stave_lo-stave_hi)==1 && isAtLowerEdgeZ() && x->isAtUpperEdgeZ() ) ||
+          (abs(stave_lo-stave_hi)==1 && isAtUpperEdgeZ() && x->isAtLowerEdgeZ() )  ) return true; 
+      } 
+    }
+    else
+    {
+      if(part_lo==Npart && part_hi==1 && dlayer_lo==1)
+      {
+        if(bar_hi==1 && abs(stave_lo-stave_hi)<=1)
+        {
+          if(abs(bar_lo-over_module[(dlayer_hi-1)*2])<=over_module_set || abs(bar_lo-over_module[(dlayer_hi-1)*2+1])<=over_module_set)
+          {
+            return true;
+          }
+        } 
+        //method1: abs(dlayer_hi*2*sqrt(2)-(bar_lo-14+1))<=3
+        //method2: abs(bar_lo - (14+(dlayer_hi-1)*3))<=3
+      }
+    }
+    return false; 
+  }
+*/  
+};
+#endif
diff --git a/Reconstruction/CrystalCaloRec/src/Objects/HoughObject.cc b/Reconstruction/CrystalCaloRec/src/Objects/HoughObject.cc
new file mode 100644
index 0000000000000000000000000000000000000000..4225bede9eb978d5a78c8b9c5e6356882be526a3
--- /dev/null
+++ b/Reconstruction/CrystalCaloRec/src/Objects/HoughObject.cc
@@ -0,0 +1,72 @@
+#ifndef CALOHOUGHOBJECT_C
+#define CALOHOUGHOBJECT_C
+
+#include "Objects/HoughObject.h"
+
+namespace PandoraPlus{
+
+  HoughObject::HoughObject( const PandoraPlus::Calo1DCluster* _localmax, double _cellSize, double _ecal_inner_radius, double _phi){
+    m_local_max = _localmax;
+
+    setCellSize(_cellSize);
+    setCenterPoint(_ecal_inner_radius, _phi);
+  }
+
+
+  void HoughObject::setCenterPoint(double& _ecal_inner_radius, double _phi){
+    if(m_local_max->getSlayer()==0){
+      TVector3 tmp_vec = m_local_max->getPos();
+      m_center_point.Set(tmp_vec.Perp(), tmp_vec.z());
+
+/*      if(_phi==0)
+        m_center_point.Set( (m_local_max->getDlayer()-1)*20. + _ecal_inner_radius + m_cell_size*0.5, m_local_max->getPos().z() );
+      else{
+        double intPart, fracPart;
+        fracPart = modf((_phi+TMath::Pi())/(TMath::Pi()/4.), &intPart);  // yyy: _phi + TMath::Pi() ranges from 0 to 2pi
+        if(fracPart<0.489 || fracPart>0.711)  //Not in crack region.
+          m_center_point.Set( (m_local_max->getDlayer()-1)*20. + _ecal_inner_radius + m_cell_size*0.5, m_local_max->getPos().z() );
+        else{
+          int iCrack = intPart+2;
+          if(iCrack>=8) iCrack = iCrack-8;
+
+          double tmp_phi = _phi;
+          while(tmp_phi<0.) tmp_phi += TMath::Pi() / 4.;  
+          while(tmp_phi>=TMath::Pi() / 4.) tmp_phi -= TMath::Pi() / 4.;
+
+          int imodule = m_local_max->getTowerID()[0][0];
+          if(imodule==iCrack){
+            double Rref = _ecal_inner_radius/cos( tmp_phi );
+            m_center_point.Set( (m_local_max->getDlayer()-1)*20. + Rref + m_cell_size*0.5 ,
+                          m_local_max->getPos().z());
+          }
+          else{
+            double Rref = _ecal_inner_radius/cos( TMath::Pi() / 4. - tmp_phi );
+            m_center_point.Set( (m_local_max->getDlayer()-1)*20. + Rref + m_cell_size*0.5 ,
+                          m_local_max->getPos().z());
+          }
+
+        }
+      }
+*/
+    }
+    else if(m_local_max->getSlayer()==1){
+      m_center_point.Set(m_local_max->getPos().x(), m_local_max->getPos().y());
+    }
+    else{
+      std::cout<<"Error: Slayer="<<m_local_max->getSlayer()<<", do not use setCenterPoint()!"<<std::endl;
+    }
+    
+  }
+
+
+  void HoughObject::setHoughLine(TF1& _line1, TF1& _line2){
+    m_Hough_line_1 = _line1;  
+    m_Hough_line_2 = _line2;
+    //m_Hough_line_3 = _line3;  
+    //m_Hough_line_4 = _line4;
+  }
+
+
+
+};
+#endif
diff --git a/Reconstruction/CrystalCaloRec/src/Objects/HoughSpace.cc b/Reconstruction/CrystalCaloRec/src/Objects/HoughSpace.cc
new file mode 100644
index 0000000000000000000000000000000000000000..996fa56a0dad94b0ab226d466a26f7a3e5624053
--- /dev/null
+++ b/Reconstruction/CrystalCaloRec/src/Objects/HoughSpace.cc
@@ -0,0 +1,128 @@
+#ifndef HOUGHSPACE_C
+#define HOUGHSPACE_C
+
+#include "Objects/HoughSpace.h"
+#include <iostream>
+namespace PandoraPlus{
+
+  int HoughSpace::getAlphaBin(double alpha) const{ 
+    if(alpha<alpha_low){
+      // cout << "int HoughSpace::getAlphaBin(double alpha): wrong alpha input: " << alpha << ", return 1" << endl;
+      return 1;
+    }
+    if(alpha>alpha_high){
+      // cout << "int HoughSpace::getAlphaBin(double alpha): wrong alpha input: " << alpha << ", return Nbins_alpha" << endl;
+      return Nbins_alpha;
+    }
+
+    int alpha_bin = ceil((alpha-alpha_low)/bin_width_alpha);
+    return alpha_bin;
+  }
+
+  double HoughSpace::getAlphaBinCenter(int alpha_bin) const{ 
+    if(alpha_bin<1){
+      // cout << "double HoughSpace::getAlphaBinCenter(int alpha_bin): wrong alpha_bin input: " << alpha_bin << ", return bincenter when alpha_bin=1" << endl;
+      return alpha_low + (bin_width_alpha * 0.5);
+    }
+    if(alpha_bin>Nbins_alpha){
+      // cout << "double HoughSpace::getAlphaBinCenter(int alpha_bin): wrong alpha_bin input: " << alpha_bin << ", return bincenter when alpha_bin=Nbins_alpha" << endl;
+      return alpha_high - (bin_width_alpha * 0.5);
+    }
+
+    double alpha_bin_center = alpha_low + (alpha_bin*1.*bin_width_alpha) - (bin_width_alpha*0.5) ;
+    return alpha_bin_center;
+  }
+
+  double HoughSpace::getAlphaBinLowEdge(int alpha_bin) const{ 
+    if(alpha_bin<1){
+      // cout << "double HoughSpace::getAlphaBinLowEdge(int alpha_bin): wrong alpha_bin input: " << alpha_bin << ", return bin low edge when alpha_bin=1" << endl;
+      return alpha_low;
+    }
+    if(alpha_bin>Nbins_alpha){
+      // cout << "double HoughSpace::getAlphaBinLowEdge(int alpha_bin): wrong alpha_bin input: " << alpha_bin << ", return bin low edge when alpha_bin=Nbins_alpha" << endl;
+      return alpha_high - bin_width_alpha;
+    }
+
+    double alpha_bin_low_edge = alpha_low + (alpha_bin*1.*bin_width_alpha) - bin_width_alpha;
+    return alpha_bin_low_edge;
+  }
+
+  double HoughSpace::getAlphaBinUpEdge(int alpha_bin) const{
+    if(alpha_bin<1){
+      // cout << "double HoughSpace::getAlphaBinUpEdge(int alpha_bin): wrong alpha_bin input: " << alpha_bin << ", return bin up edge when alpha_bin=1" << endl;
+      return alpha_low + bin_width_alpha;
+    }
+    if(alpha_bin>Nbins_alpha){
+      // cout << "double HoughSpace::getAlphaBinUpEdge(int alpha_bin): wrong alpha_bin input: " << alpha_bin << ", return bin up edge when alpha_bin=Nbins_alpha" << endl;
+      return alpha_high;
+    }
+    
+    double alpha_bin_up_edge = alpha_low + (alpha_bin*1.*bin_width_alpha);
+    return alpha_bin_up_edge;
+  }
+
+  int HoughSpace::getRhoBin(double rho) const{
+    if(rho<rho_low){
+      // cout << "int HoughSpace::getRhoBin(double rho): wrong rho input: " << rho << ", return 1" << endl;
+      return 1;
+    }
+    if(rho>rho_high){
+      // cout << "int HoughSpace::getRhoBin(double rho): wrong rho input: " << rho << ", return Nbins_rho" << endl;
+      return Nbins_rho;
+    }
+
+    int rho_bin = ceil((rho-rho_low)/bin_width_rho);
+    return rho_bin;
+  }
+
+  double HoughSpace::getRhoBinCenter(int rho_bin) const{
+    if(rho_bin<1){
+      // cout << "double HoughSpace::getRhoBinCenter(int rho_bin): wrong rho_bin input: " << rho_bin << ", return bincenter when rho_bin=1" << endl;
+      return rho_low + (bin_width_rho * 0.5);
+    }
+    if(rho_bin>Nbins_rho){
+      // cout << "double HoughSpace::getRhoBinCenter(int rho_bin): wrong rho_bin input: " << rho_bin << ", return bincenter when rho_bin=Nbins_rho" << endl;
+      return rho_high - (bin_width_rho * 0.5);
+    }
+
+    double rho_bin_center = rho_low + (rho_bin*1.*bin_width_rho) - (bin_width_rho*0.5) ;
+    return rho_bin_center;
+  }
+
+  double HoughSpace::getRhoBinLowEdge(int rho_bin) const{
+    if(rho_bin<1){
+      // cout << "double HoughSpace::getRhoBinLowEdge(int rho_bin): wrong rho_bin input: " << rho_bin << ", return bin low edge when rho_bin=1" << endl;
+      return rho_low;
+    }
+    if(rho_bin>Nbins_rho){
+      // cout << "double HoughSpace::getRhoBinLowEdge(int rho_bin): wrong rho_bin input: " << rho_bin << ", return bin low edge when rho_bin=Nbins_rho" << endl;
+      return rho_high - bin_width_rho;
+    }
+
+    double rho_bin_low_edge = rho_low + (rho_bin*1.*bin_width_rho) - bin_width_rho;
+    return rho_bin_low_edge;
+  }
+
+  double HoughSpace::getRhoBinUpEdge(int rho_bin) const{
+    if(rho_bin<1){
+      // cout << "double HoughSpace::getRhoBinUpEdge(int rho_bin): wrong rho_bin input: " << rho_bin << ", return bin up edge when rho_bin=1" << endl;
+      return rho_low + bin_width_rho;
+    }
+    if(rho_bin>Nbins_rho){
+      // cout << "double HoughSpace::getRhoBinUpEdge(int rho_bin): wrong rho_bin input: " << rho_bin << ", return bin up edge when rho_bin=Nbins_rho" << endl;
+      return rho_high;
+    }
+    
+    double rho_bin_up_edge = rho_low + (rho_bin*1.*bin_width_rho);
+    return rho_bin_up_edge;
+  }
+
+  void HoughSpace::AddBinHobj(int bin_alpha, int bin_rho, int index_Hobj){
+    pair<int, int> bin(bin_alpha, bin_rho);
+    Hough_bins[bin].insert(index_Hobj);
+  }
+ 
+
+
+};
+#endif
diff --git a/Reconstruction/CrystalCaloRec/src/Objects/PFObject.cc b/Reconstruction/CrystalCaloRec/src/Objects/PFObject.cc
new file mode 100644
index 0000000000000000000000000000000000000000..3205ff560fe060b0a27ff69688137678c405348f
--- /dev/null
+++ b/Reconstruction/CrystalCaloRec/src/Objects/PFObject.cc
@@ -0,0 +1,81 @@
+#ifndef PFOBJECT_C
+#define PFOBJECT_C
+
+#include "Objects/PFObject.h"
+namespace PandoraPlus{
+
+  void PFObject::Clear()
+  {
+    m_pid = 0;
+    m_tracks.clear();
+    m_ecal_clusters.clear();
+    m_hcal_clusters.clear();
+  }
+
+  std::shared_ptr<PandoraPlus::PFObject> PFObject::Clone() const{
+    std::shared_ptr<PandoraPlus::PFObject> m_newpfo = std::make_shared<PandoraPlus::PFObject>();
+    m_newpfo->setTrack(m_tracks);
+    m_newpfo->setECALCluster(m_ecal_clusters);
+    m_newpfo->setHCALCluster(m_hcal_clusters);
+    m_newpfo->setPID(m_pid);
+
+    return m_newpfo;
+  }
+
+  void PFObject::addTrack(const Track* _track){
+    if( find( m_tracks.begin(), m_tracks.end(), _track)!=m_tracks.end() ){
+      std::cout<<"ERROR: attempt to add an existing track into PFO! Skip it "<<std::endl;
+    }
+    else{
+      m_tracks.push_back(_track);
+    }
+  }
+
+  void PFObject::addECALCluster(const Calo3DCluster* _ecal_cluster){
+    if( find( m_ecal_clusters.begin(), m_ecal_clusters.end(), _ecal_cluster)!=m_ecal_clusters.end() ){
+      std::cout<<"ERROR: attempt to add an existing ECAL cluster into PFO! Skip it "<<std::endl;
+    }
+    else{
+      m_ecal_clusters.push_back(_ecal_cluster);
+    }
+  }
+
+  void PFObject::addHCALCluster(const Calo3DCluster* _hcal_cluster){
+    if( find( m_hcal_clusters.begin(), m_hcal_clusters.end(), _hcal_cluster)!=m_hcal_clusters.end() ){
+      std::cout<<"ERROR: attempt to add an existing HCAL cluster into PFO! Skip it "<<std::endl;
+    }
+    else{
+      m_hcal_clusters.push_back(_hcal_cluster);
+    }
+  }
+
+  double PFObject::getECALClusterEnergy() const{
+    if(m_ecal_clusters.size()==0) return 0.;
+
+    double sumEn = 0;
+    for(int i=0; i<m_ecal_clusters.size(); i++) sumEn += m_ecal_clusters[i]->getLongiE();
+    return sumEn;
+
+  }
+
+  double PFObject::getHCALClusterEnergy() const{
+    if(m_hcal_clusters.size()==0) return 0.;
+
+    double sumEn = 0;
+    for(int i=0; i<m_hcal_clusters.size(); i++) sumEn += m_hcal_clusters[i]->getHitsE();
+    return sumEn;
+  }
+
+  double PFObject::getTrackMomentum() const{
+    if(m_tracks.size()==0) return -99;
+
+    double maxP = -1;
+    for(int i=0; i<m_tracks.size(); i++){
+      if(m_tracks[i]->getMomentum()>maxP) maxP = m_tracks[i]->getMomentum();
+    }
+    return maxP;
+  }
+
+
+};
+#endif
diff --git a/Reconstruction/CrystalCaloRec/src/Objects/Track.cc b/Reconstruction/CrystalCaloRec/src/Objects/Track.cc
new file mode 100644
index 0000000000000000000000000000000000000000..b74fbf402a971750337212c990f8ed51dbec2fb2
--- /dev/null
+++ b/Reconstruction/CrystalCaloRec/src/Objects/Track.cc
@@ -0,0 +1,113 @@
+#ifndef TRACK_C
+#define TRACK_C
+
+#include "Objects/Track.h"
+#include <cmath>
+#include "TGraph.h"
+
+namespace PandoraPlus{
+
+  const double Track::B = 3.;
+
+  int Track::trackStates_size(std::string name) const{
+    std::vector<TrackState> emptyCol; emptyCol.clear(); 
+    if(m_trackStates.find(name)!=m_trackStates.end()) emptyCol = m_trackStates.at(name);
+    return emptyCol.size();
+  }
+
+  int Track::trackStates_size() const{
+    std::vector<TrackState> emptyCol; emptyCol.clear();
+    for(auto iter: m_trackStates) emptyCol.insert(emptyCol.end(), iter.second.begin(), iter.second.end());
+    return emptyCol.size();
+  }
+
+  std::vector<TrackState> Track::getTrackStates(std::string name) const{
+    std::vector<TrackState> emptyCol; emptyCol.clear(); 
+    if(m_trackStates.find(name)!=m_trackStates.end()) emptyCol = m_trackStates.at(name);
+    return emptyCol;
+  }
+
+  std::vector<TrackState> Track::getAllTrackStates() const{
+    std::vector<TrackState> emptyCol; emptyCol.clear();
+    for(auto iter: m_trackStates) emptyCol.insert(emptyCol.end(), iter.second.begin(), iter.second.end());
+    return emptyCol;
+  }
+
+  float Track::getPt() const{
+    std::vector<TrackState> trkStates = getTrackStates("Input");
+    float pt = -99.;
+    for(auto it: trkStates){
+      if(it.location==4 || it.location==1){  //Calorimeter(for real track) or IP (for truth track)
+        pt = 1./it.Kappa;
+      }
+    }
+
+    return pt;
+  }
+
+  float Track::getPz() const{
+    std::vector<TrackState> trkStates = getTrackStates("Input");
+    float pz = -99.;
+    for(auto it: trkStates){
+      if(it.location==4 || it.location==1){ //Calorimeter(for real track) or IP (for truth track)
+        pz = it.tanLambda/it.Kappa;
+      }
+    }
+
+    return pz;
+  }
+
+  TVector3 Track::getP3() const{
+    std::vector<TrackState> trkStates = getTrackStates("Input");
+    float phi = -99;
+    float pt = -99.;
+    float pz = -99.;
+    for(auto it: trkStates){
+      if(it.location==4 || it.location==1){  //Calorimeter(for real track) or IP (for truth track)
+        pt = 1./it.Kappa;
+        phi = it.phi0;
+        pz = it.tanLambda/it.Kappa;
+      }
+    }
+  
+    TVector3 p3(pt*cos(phi), pt*sin(phi), pz);
+    return p3; 
+  }
+
+  float Track::getCharge() const{
+    std::vector<TrackState> trkStates = getTrackStates("Input");
+    float omega = -99.;
+    for(auto it: trkStates){
+      if(it.location==4 || it.location==1){ //Calorimeter(for real track) or IP (for truth track)
+        omega = it.Omega;
+      }
+    }
+
+    return omega/fabs(omega);
+  }
+
+  edm4hep::MCParticle Track::getLeadingMCP() const{
+    float maxWeight = -1.;
+    edm4hep::MCParticle mcp;
+    for(auto& iter: MCParticleWeight){
+      if(iter.second>maxWeight){
+        mcp = iter.first;
+        maxWeight = iter.second;
+      }
+    }
+
+    return mcp;
+  }
+
+  float Track::getLeadingMCPweight() const{
+    float maxWeight = -1.;
+    for(auto& iter: MCParticleWeight){
+      if(iter.second>maxWeight){
+        maxWeight = iter.second;
+      }
+    }  
+    return maxWeight;
+  }
+
+};
+#endif
diff --git a/Reconstruction/CrystalCaloRec/src/PandoraPlusDataCol.cpp b/Reconstruction/CrystalCaloRec/src/PandoraPlusDataCol.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5ee658b9e9b0a5c586951a543d8b5b77f52e13ee
--- /dev/null
+++ b/Reconstruction/CrystalCaloRec/src/PandoraPlusDataCol.cpp
@@ -0,0 +1,27 @@
+#ifndef _PANDORAPLUS_DATA_C
+#define _PANDORAPLUS_DATA_C
+
+#include "PandoraPlusDataCol.h"
+
+StatusCode PandoraPlusDataCol::Clear(){
+  collectionMap_MC.clear();
+  collectionMap_CaloHit.clear();
+  collectionMap_Vertex.clear();
+  collectionMap_Track.clear();
+  collectionMap_CaloRel.clear();
+  collectionMap_TrkRel.clear();
+
+  TrackCol.clear();
+  map_CaloHit.clear();
+
+  map_BarCol.clear();
+  map_1DCluster.clear();
+  map_HalfCluster.clear();
+  map_2DCluster.clear();
+  map_CaloCluster.clear();
+  map_PFObjects.clear();
+  
+  return StatusCode::SUCCESS; 
+};
+
+#endif
diff --git a/Reconstruction/CrystalCaloRec/src/PandoraPlusPFAlg.cpp b/Reconstruction/CrystalCaloRec/src/PandoraPlusPFAlg.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..de6314aeae6c2cc401cc74193cfe0a63eb94e33f
--- /dev/null
+++ b/Reconstruction/CrystalCaloRec/src/PandoraPlusPFAlg.cpp
@@ -0,0 +1,2158 @@
+#ifndef PANDORAPLUS_ALG_C
+#define PANDORAPLUS_ALG_C
+
+#include "PandoraPlusPFAlg.h"
+
+// #include <fstream>
+// #include <ctime>
+
+using namespace std;
+using namespace dd4hep;
+
+int PandoraPlus::CaloUnit::Nmodule = 32;
+int PandoraPlus::CaloUnit::Nstave = 15;
+int PandoraPlus::CaloUnit::Nlayer = 14;
+int PandoraPlus::CaloUnit::NbarPhi_odd[14] = {39, 39, 39, 39, 37, 37, 37, 37, 37, 35, 35, 35, 35, 33};
+int PandoraPlus::CaloUnit::NbarPhi_even[14] = {29, 29, 31, 31, 33, 35, 35, 37, 37, 39, 41, 41, 43, 43};
+int PandoraPlus::CaloUnit::NbarZ = 36;
+//int PandoraPlus::CaloUnit::over_module[28] = {13,15,16,18,19,21,22,24,25,26,28,29,30,32,33,35,36,38,39,41,42,43,45,46};
+//int PandoraPlus::CaloUnit::over_module_set = 2;
+float PandoraPlus::CaloUnit::barsize = 10.; //mm
+float PandoraPlus::CaloUnit::ecal_innerR = 1830;  //mm
+
+DECLARE_COMPONENT( PandoraPlusPFAlg )
+
+PandoraPlusPFAlg::PandoraPlusPFAlg(const std::string& name, ISvcLocator* svcLoc)
+  : GaudiAlgorithm(name, svcLoc),
+    _nEvt(0)
+{
+ 
+  // Output collections
+  declareProperty("RecCaloHitCollection", w_RecEcalCol, "Handle of Reconstructed CaloHit collection");
+  declareProperty("RecCaloCoreCollection", w_RecCoreCol, "Handle of Reconstructed ECAL Core collection");
+  declareProperty("RecHCALHitCollection",  w_RecHcalCol, "Handle of Reconstructed HCAL CaloHit collection");
+  declareProperty("RecTrackCollection", w_RecTrkCol, "Handle of Reconstructed Tracks linked to PFO"); 
+  declareProperty("RecoPFOCollection", w_ReconstructedParticleCollection, "Handle of Reconstructed PFO collection");   
+  // declareProperty("RecoVtxCollection", w_VertexCollection, "Handle of Reconstructed vertex collection");   
+  // declareProperty("MCRecoPFOAssociationCollection", w_MCRecoParticleAssociationCollection, "Handle of MC-RecoPFO association collection");   
+
+}
+
+StatusCode PandoraPlusPFAlg::initialize()
+{
+  //Initialize global settings
+  m_GlobalSettings.map_floatPars["BField"] = m_BField;
+  m_GlobalSettings.map_floatPars["Seed"] = m_seed;
+  m_GlobalSettings.map_intPars["Debug"] = m_Debug;
+  m_GlobalSettings.map_intPars["SkipEvt"] = m_Nskip;
+
+
+  //Initialize Creator settings
+  m_pMCParticleCreatorSettings.map_stringPars["MCParticleCollections"] = name_MCParticleCol.value();
+
+  m_pTrackCreatorSettings.map_stringVecPars["trackCollections"] = name_TrackCol.value();
+  m_pTrackCreatorSettings.map_floatPars["BField"] = m_BField; 
+
+  std::vector<std::string> name_CaloHits = name_EcalHits; 
+  std::vector<std::string> name_CaloReadout = name_EcalReadout;
+  name_CaloHits.insert( name_CaloHits.end(), name_HcalHits.begin(), name_HcalHits.end() );
+  name_CaloReadout.insert(name_CaloReadout.end(), name_HcalReadout.begin(), name_HcalReadout.end());
+
+  m_CaloHitsCreatorSettings.map_stringVecPars["CaloHitCollections"] = name_CaloHits;
+  m_CaloHitsCreatorSettings.map_stringPars["EcalType"] = m_EcalType.value();
+
+  m_OutputCreatorSettings.map_stringPars["OutputPFO"] = name_PFObject.value();
+  m_OutputCreatorSettings.map_boolPars["UseTruthTrk"] = m_useTruthTrk.value();
+  m_OutputCreatorSettings.map_floatPars["BField"] = m_BField.value();
+  m_OutputCreatorSettings.map_floatPars["ECALCalib"] = m_EcalCalib.value();
+  m_OutputCreatorSettings.map_floatPars["HCALCalib"] = m_HcalCalib.value();
+
+  //Initialize Creators
+  m_pMCParticleCreator = new MCParticleCreator( m_pMCParticleCreatorSettings );
+  m_pTrackCreator      = new TrackCreator( m_pTrackCreatorSettings );
+  m_pCaloHitsCreator   = new CaloHitsCreator( m_CaloHitsCreatorSettings );
+  m_pOutputCreator     = new OutputCreator( m_OutputCreatorSettings );
+
+  //Readin collections
+
+  //---MC particle---
+  if(!name_MCParticleCol.empty()) r_MCParticleCol = new DataHandle<edm4hep::MCParticleCollection> (name_MCParticleCol, Gaudi::DataHandle::Reader, this);
+
+  //---Tracks---
+  for(auto& _trk : name_TrackCol) if(!_trk.empty()) r_TrackCols.push_back( new TrackType(_trk, Gaudi::DataHandle::Reader, this) );
+
+  //---Calo Hits---
+  for(auto& _ecal : name_EcalHits){
+    if(!_ecal.empty()){ 
+      //r_ECalHitCols.push_back( new CaloType(_ecal, Gaudi::DataHandle::Reader, this) );
+      r_CaloHitCols.push_back( new CaloType(_ecal, Gaudi::DataHandle::Reader, this) );
+  }}
+  for(auto& _hcal : name_HcalHits){ 
+    if(!_hcal.empty()){
+      //r_HCalHitCols.push_back( new CaloType(_hcal, Gaudi::DataHandle::Reader, this) );
+      r_CaloHitCols.push_back( new CaloType(_hcal, Gaudi::DataHandle::Reader, this) );
+  }}
+
+  //---MCParticle CaloHit Association
+  if(!name_MCPTrkAssoCol.empty())      r_MCPTrkAssoCol = new DataHandle<edm4hep::MCRecoTrackParticleAssociationCollection> (name_MCPTrkAssoCol, Gaudi::DataHandle::Reader, this);
+
+  std::vector<std::string> name_CaloAssoCol = name_EcalMCPAssociation; 
+  name_CaloAssoCol.insert(name_CaloAssoCol.end(), name_HcalMCPAssociation.begin(), name_HcalMCPAssociation.end());
+  if(name_CaloAssoCol.size()==name_CaloHits.size()){
+    for(int iCol=0; iCol<name_CaloAssoCol.size(); iCol++){
+      map_CaloMCPAssoCols[name_CaloHits[iCol]] = new CaloParticleAssoType(name_CaloAssoCol[iCol], Gaudi::DataHandle::Reader, this);
+    }
+  }
+
+
+  //Register Algorithms
+  //--- Initialize algorithm maps ---
+  m_algorithmManager.RegisterAlgorithmFactory("ExampleAlg",             new ExampleAlg::Factory);
+  m_algorithmManager.RegisterAlgorithmFactory("GlobalClusteringAlg",    new GlobalClusteringAlg::Factory);
+  m_algorithmManager.RegisterAlgorithmFactory("HcalClusteringAlg",      new HcalClusteringAlg::Factory);
+  m_algorithmManager.RegisterAlgorithmFactory("LocalMaxFindingAlg",     new LocalMaxFindingAlg::Factory);
+  m_algorithmManager.RegisterAlgorithmFactory("HoughClusteringAlg",     new HoughClusteringAlg::Factory);
+  m_algorithmManager.RegisterAlgorithmFactory("TrackMatchingAlg",       new TrackMatchingAlg::Factory);
+  m_algorithmManager.RegisterAlgorithmFactory("ConeClustering2DAlg",    new ConeClustering2DAlg::Factory);
+  m_algorithmManager.RegisterAlgorithmFactory("AxisMergingAlg",         new AxisMergingAlg::Factory);
+  m_algorithmManager.RegisterAlgorithmFactory("EnergySplittingAlg",     new EnergySplittingAlg::Factory);
+  m_algorithmManager.RegisterAlgorithmFactory("EnergyTimeMatchingAlg",  new EnergyTimeMatchingAlg::Factory);
+  m_algorithmManager.RegisterAlgorithmFactory("PFOCreatingAlg",         new PFOCreatingAlg::Factory);
+  m_algorithmManager.RegisterAlgorithmFactory("ConeClusteringAlg",      new ConeClusteringAlg::Factory);
+  m_algorithmManager.RegisterAlgorithmFactory("TrackClusterConnectingAlg",   new TrackClusterConnectingAlg::Factory);
+  m_algorithmManager.RegisterAlgorithmFactory("PFOReclusteringAlg",          new PFOReclusteringAlg::Factory);
+  //m_algorithmManager.RegisterAlgorithmFactory("ConeClusteringAlgHCAL",  new ConeClusteringAlg::Factory);
+
+  m_algorithmManager.RegisterAlgorithmFactory("TruthTrackMatchingAlg",       new TruthTrackMatchingAlg::Factory);
+  m_algorithmManager.RegisterAlgorithmFactory("TruthPatternRecAlg",          new TruthPatternRecAlg::Factory);
+  m_algorithmManager.RegisterAlgorithmFactory("TruthEnergySplittingAlg",     new TruthEnergySplittingAlg::Factory);
+  m_algorithmManager.RegisterAlgorithmFactory("TruthMatchingAlg",            new TruthMatchingAlg::Factory);
+  m_algorithmManager.RegisterAlgorithmFactory("TruthClusteringAlg",          new TruthClusteringAlg::Factory);
+  m_algorithmManager.RegisterAlgorithmFactory("TruthClusterMergingAlg",      new TruthClusterMergingAlg::Factory);
+
+  //--- Create algorithm from readin settings ---
+  for(int ialg=0; ialg<name_Algs.value().size(); ialg++){
+    Settings m_settings; 
+    for(int ipar=0; ipar<name_AlgPars.value()[ialg].size(); ipar++){
+      if(type_AlgPars.value()[ialg].at(ipar)=="int")    m_settings.map_intPars[name_AlgPars.value()[ialg].at(ipar)] = std::stoi( (string)value_AlgPars.value()[ialg].at(ipar) );
+      if(type_AlgPars.value()[ialg].at(ipar)=="double") m_settings.map_floatPars[name_AlgPars.value()[ialg].at(ipar)] = std::stod( (string)value_AlgPars.value()[ialg].at(ipar) );
+      if(type_AlgPars.value()[ialg].at(ipar)=="string") m_settings.map_stringPars[name_AlgPars.value()[ialg].at(ipar)] = value_AlgPars.value()[ialg].at(ipar) ;
+      if(type_AlgPars.value()[ialg].at(ipar)=="bool")   m_settings.map_boolPars[name_AlgPars.value()[ialg].at(ipar)] = (bool)std::stoi( (string)value_AlgPars.value()[ialg].at(ipar) );
+    }
+
+    m_algorithmManager.RegisterAlgorithm( name_Algs.value()[ialg], m_settings );
+  }
+
+
+  //Initialize services
+  m_geosvc = service<IGeomSvc>("GeomSvc");
+  if ( !m_geosvc )  throw "PandoraPlusPFAlg :Failed to find GeomSvc ...";
+
+  m_energycorsvc = service<ICrystalEcalSvc>("CrystalEcalEnergyCorrectionSvc");
+  if ( !m_energycorsvc )  throw "PandoraPlusPFAlg :Failed to find CrystalEcalEnergyCorrectionSvc ...";
+  //m_energycorsvc->initialize();
+
+  for(unsigned int i=0; i<name_CaloReadout.size(); i++){
+    if(name_CaloReadout[i].empty()) continue;
+    dd4hep::DDSegmentation::BitFieldCoder* tmp_decoder = m_geosvc->getDecoder(name_CaloReadout[i]);
+    if (!tmp_decoder) {
+      error() << "Failed to get the decoder for: " << name_CaloReadout[i] << endmsg;
+      return StatusCode::FAILURE;
+    }
+    map_readout_decoder[name_CaloHits[i]] = tmp_decoder;
+  }
+
+  rndm.SetSeed(m_seed);
+  std::cout<<"PandoraPlusPFAlg::initialize"<<std::endl;
+
+
+  //Output collections
+  w_ClusterCollection["EcalCluster"] = new DataHandle<edm4hep::ClusterCollection>("EcalCluster", Gaudi::DataHandle::Writer, this);
+  w_ClusterCollection["EcalCore"]    = new DataHandle<edm4hep::ClusterCollection>("EcalCore", Gaudi::DataHandle::Writer, this);
+  w_ClusterCollection["HcalCluster"] = new DataHandle<edm4hep::ClusterCollection>("HcalCluster", Gaudi::DataHandle::Writer, this);
+
+  //Output ntuple for analysis.
+  if(m_WriteAna){
+    std::string s_outfile = m_filename;
+    m_wfile = new TFile(s_outfile.c_str(), "recreate");
+    t_MCParticle = new TTree("MCParticle", "MCParticle");
+    t_SimBar = new TTree("SimBarHit", "SimBarHit");
+    t_LocalMax = new TTree("LocalMax", "LocalMax");
+    t_Layers = new TTree("RecLayers","RecLayers");
+    t_Hough = new TTree("Hough", "Hough");
+    t_Cone = new TTree("Cone", "Cone");
+    t_TrackAxis = new TTree("TrackAxis", "TrackAxis");
+    t_Axis = new TTree("Axis", "Axis");
+    t_HalfCluster = new TTree("HalfCluster","HalfCluster");
+    t_Tower = new TTree("Tower", "Tower");
+    t_Cluster = new TTree("RecClusters", "RecClusters");
+    t_Track = new TTree("Track", "Track");
+    t_PFO = new TTree("PFO", "PFO");
+
+    //MC particle 
+    t_MCParticle->Branch("mcPdgid",     &m_mcPdgid);
+    t_MCParticle->Branch("mcStatus",    &m_mcStatus);
+    t_MCParticle->Branch("mcPx", &m_mcPx);
+    t_MCParticle->Branch("mcPy", &m_mcPy);
+    t_MCParticle->Branch("mcPz", &m_mcPz);
+    t_MCParticle->Branch("mcEn", &m_mcEn);
+    t_MCParticle->Branch("mcMass", &m_mcMass);
+    t_MCParticle->Branch("mcCharge", &m_mcCharge);
+    t_MCParticle->Branch("mcEPx", &m_mcEPx);
+    t_MCParticle->Branch("mcEPy", &m_mcEPy);
+    t_MCParticle->Branch("mcEPz", &m_mcEPz);
+    t_MCParticle->Branch("mcdepEn_ecal", &m_depEn_ecal);
+    t_MCParticle->Branch("mcdepEn_hcal", &m_depEn_hcal);
+
+    //Bar
+    t_SimBar->Branch("totE_EcalSim", &m_totE_EcalSim);
+    t_SimBar->Branch("simBar_x", &m_simBar_x);
+    t_SimBar->Branch("simBar_y", &m_simBar_y);
+    t_SimBar->Branch("simBar_z", &m_simBar_z);
+    t_SimBar->Branch("simBar_T1", &m_simBar_T1);
+    t_SimBar->Branch("simBar_T2", &m_simBar_T2);
+    t_SimBar->Branch("simBar_Q1", &m_simBar_Q1);
+    t_SimBar->Branch("simBar_Q2", &m_simBar_Q2);
+    t_SimBar->Branch("simBar_module", &m_simBar_module);
+    t_SimBar->Branch("simBar_dlayer", &m_simBar_dlayer);
+    t_SimBar->Branch("simBar_stave", &m_simBar_stave);
+    t_SimBar->Branch("simBar_slayer", &m_simBar_slayer);
+    t_SimBar->Branch("simBar_bar", &m_simBar_bar);
+    t_SimBar->Branch("simBar_truthMC_tag", &m_simBar_truthMC_tag);
+    t_SimBar->Branch("simBar_truthMC_pid", &m_simBar_truthMC_pid);
+    t_SimBar->Branch("simBar_truthMC_px", &m_simBar_truthMC_px);
+    t_SimBar->Branch("simBar_truthMC_py", &m_simBar_truthMC_py);
+    t_SimBar->Branch("simBar_truthMC_pz", &m_simBar_truthMC_pz);
+    t_SimBar->Branch("simBar_truthMC_E", &m_simBar_truthMC_E);
+    t_SimBar->Branch("simBar_truthMC_EPx", &m_simBar_truthMC_EPx);
+    t_SimBar->Branch("simBar_truthMC_EPy", &m_simBar_truthMC_EPy);
+    t_SimBar->Branch("simBar_truthMC_EPz", &m_simBar_truthMC_EPz);
+    t_SimBar->Branch("simBar_truthMC_weight", &m_simBar_truthMC_weight);
+    t_SimBar->Branch("totE_HcalSim", &m_totE_HcalSim);
+    t_SimBar->Branch("HcalHit_x", &m_HcalHit_x); 
+    t_SimBar->Branch("HcalHit_y", &m_HcalHit_y); 
+    t_SimBar->Branch("HcalHit_z", &m_HcalHit_z); 
+    t_SimBar->Branch("HcalHit_E", &m_HcalHit_E); 
+    t_SimBar->Branch("HcalHit_layer", &m_HcalHit_layer); 
+    t_SimBar->Branch("HcalHit_truthMC_tag", &m_HcalHit_truthMC_tag);
+    t_SimBar->Branch("HcalHit_truthMC_pid", &m_HcalHit_truthMC_pid);
+    t_SimBar->Branch("HcalHit_truthMC_px", &m_HcalHit_truthMC_px);
+    t_SimBar->Branch("HcalHit_truthMC_py", &m_HcalHit_truthMC_py);
+    t_SimBar->Branch("HcalHit_truthMC_pz", &m_HcalHit_truthMC_pz);
+    t_SimBar->Branch("HcalHit_truthMC_E", &m_HcalHit_truthMC_E);
+    t_SimBar->Branch("HcalHit_truthMC_EPx", &m_HcalHit_truthMC_EPx);
+    t_SimBar->Branch("HcalHit_truthMC_EPy", &m_HcalHit_truthMC_EPy);
+    t_SimBar->Branch("HcalHit_truthMC_EPz", &m_HcalHit_truthMC_EPz);
+    t_SimBar->Branch("HcalHit_truthMC_weight", &m_HcalHit_truthMC_weight);    
+
+    //ECAL local max
+    t_LocalMax->Branch("NlocalMaxU", &m_NlmU);
+    t_LocalMax->Branch("NlocalMaxV", &m_NlmV);
+    t_LocalMax->Branch("localMaxU_tag", &m_localMaxU_tag);
+    t_LocalMax->Branch("localMaxU_x", &m_localMaxU_x);
+    t_LocalMax->Branch("localMaxU_y", &m_localMaxU_y);
+    t_LocalMax->Branch("localMaxU_z", &m_localMaxU_z);
+    t_LocalMax->Branch("localMaxU_E", &m_localMaxU_E);
+    t_LocalMax->Branch("localMaxU_mc_tag", &m_localMaxU_mc_tag);
+    t_LocalMax->Branch("localMaxU_mc_pdg", &m_localMaxU_mc_pdg);
+    t_LocalMax->Branch("localMaxU_mc_px", &m_localMaxU_mc_px);
+    t_LocalMax->Branch("localMaxU_mc_py", &m_localMaxU_mc_py);
+    t_LocalMax->Branch("localMaxU_mc_pz", &m_localMaxU_mc_pz);
+    t_LocalMax->Branch("localMaxU_mc_weight", &m_localMaxU_mc_weight);
+    t_LocalMax->Branch("localMaxV_tag", &m_localMaxV_tag);
+    t_LocalMax->Branch("localMaxV_x", &m_localMaxV_x);
+    t_LocalMax->Branch("localMaxV_y", &m_localMaxV_y);
+    t_LocalMax->Branch("localMaxV_z", &m_localMaxV_z);
+    t_LocalMax->Branch("localMaxV_E", &m_localMaxV_E);
+    t_LocalMax->Branch("localMaxV_mc_tag", &m_localMaxV_mc_tag);
+    t_LocalMax->Branch("localMaxV_mc_pdg", &m_localMaxV_mc_pdg);
+    t_LocalMax->Branch("localMaxV_mc_px", &m_localMaxV_mc_px);
+    t_LocalMax->Branch("localMaxV_mc_py", &m_localMaxV_mc_py);
+    t_LocalMax->Branch("localMaxV_mc_pz", &m_localMaxV_mc_pz);
+    t_LocalMax->Branch("localMaxV_mc_weight", &m_localMaxV_mc_weight);
+
+    //1D Showers
+    t_Layers->Branch("NshowerU", &m_NshowerU);
+    t_Layers->Branch("NshowerV", &m_NshowerV);
+    t_Layers->Branch("barShowerU_tag", &m_barShowerU_tag);
+    t_Layers->Branch("barShowerU_x", &m_barShowerU_x);
+    t_Layers->Branch("barShowerU_y", &m_barShowerU_y);
+    t_Layers->Branch("barShowerU_z", &m_barShowerU_z);
+    t_Layers->Branch("barShowerU_E", &m_barShowerU_E);
+    t_Layers->Branch("barShowerU_mc_tag", &m_barShowerU_mc_tag);
+    t_Layers->Branch("barShowerU_mc_pdg", &m_barShowerU_mc_pdg);
+    t_Layers->Branch("barShowerU_mc_px", &m_barShowerU_mc_px);
+    t_Layers->Branch("barShowerU_mc_py", &m_barShowerU_mc_py);
+    t_Layers->Branch("barShowerU_mc_pz", &m_barShowerU_mc_pz);
+    t_Layers->Branch("barShowerU_mc_weight", &m_barShowerU_mc_weight);
+    t_Layers->Branch("barShowerV_tag", &m_barShowerV_tag);
+    t_Layers->Branch("barShowerV_x", &m_barShowerV_x);
+    t_Layers->Branch("barShowerV_y", &m_barShowerV_y);
+    t_Layers->Branch("barShowerV_z", &m_barShowerV_z);
+    t_Layers->Branch("barShowerV_E", &m_barShowerV_E);
+    t_Layers->Branch("barShowerV_mc_tag", &m_barShowerV_mc_tag);
+    t_Layers->Branch("barShowerV_mc_pdg", &m_barShowerV_mc_pdg);
+    t_Layers->Branch("barShowerV_mc_px", &m_barShowerV_mc_px);
+    t_Layers->Branch("barShowerV_mc_py", &m_barShowerV_mc_py);
+    t_Layers->Branch("barShowerV_mc_pz", &m_barShowerV_mc_pz);
+    t_Layers->Branch("barShowerV_mc_weight", &m_barShowerV_mc_weight);
+
+    // Hough
+    t_Hough->Branch("houghU_tag", &m_houghU_tag);
+    t_Hough->Branch("houghU_type", &m_houghU_type);
+    t_Hough->Branch("houghU_x", &m_houghU_x);
+    t_Hough->Branch("houghU_y", &m_houghU_y);
+    t_Hough->Branch("houghU_z", &m_houghU_z);
+    t_Hough->Branch("houghU_E", &m_houghU_E);
+    t_Hough->Branch("houghU_truth_tag", &m_houghU_truth_tag);
+    t_Hough->Branch("houghU_truth_MC_px", &m_houghU_truth_MC_px);
+    t_Hough->Branch("houghU_truth_MC_py", &m_houghU_truth_MC_py);
+    t_Hough->Branch("houghU_truth_MC_pz", &m_houghU_truth_MC_pz);
+    t_Hough->Branch("houghU_truth_MC_E", &m_houghU_truth_MC_E);
+    t_Hough->Branch("houghU_truth_MC_weight", &m_houghU_truth_MC_weight);    
+    t_Hough->Branch("houghU_hit_tag", &m_houghU_hit_tag);
+    t_Hough->Branch("houghU_hit_x", &m_houghU_hit_x);
+    t_Hough->Branch("houghU_hit_y", &m_houghU_hit_y);
+    t_Hough->Branch("houghU_hit_z", &m_houghU_hit_z);
+    t_Hough->Branch("houghU_hit_E", &m_houghU_hit_E);
+    t_Hough->Branch("houghV_tag", &m_houghV_tag);
+    t_Hough->Branch("houghV_type", &m_houghV_type);
+    t_Hough->Branch("houghV_x", &m_houghV_x);
+    t_Hough->Branch("houghV_y", &m_houghV_y);
+    t_Hough->Branch("houghV_z", &m_houghV_z);
+    t_Hough->Branch("houghV_E", &m_houghV_E);
+    t_Hough->Branch("houghV_alpha", &m_houghV_alpha);
+    t_Hough->Branch("houghV_rho", &m_houghV_rho);
+    t_Hough->Branch("houghV_truth_tag", &m_houghV_truth_tag);
+    t_Hough->Branch("houghV_truth_MC_px", &m_houghV_truth_MC_px);
+    t_Hough->Branch("houghV_truth_MC_py", &m_houghV_truth_MC_py);
+    t_Hough->Branch("houghV_truth_MC_pz", &m_houghV_truth_MC_pz);
+    t_Hough->Branch("houghV_truth_MC_E", &m_houghV_truth_MC_E);
+    t_Hough->Branch("houghV_truth_MC_weight", &m_houghV_truth_MC_weight);   
+    t_Hough->Branch("houghV_hit_tag", &m_houghV_hit_tag);
+    t_Hough->Branch("houghV_hit_x", &m_houghV_hit_x);
+    t_Hough->Branch("houghV_hit_y", &m_houghV_hit_y);
+    t_Hough->Branch("houghV_hit_z", &m_houghV_hit_z);
+    t_Hough->Branch("houghV_hit_E", &m_houghV_hit_E);
+    // Cone
+    t_Cone->Branch("coneU_tag", &m_coneU_tag);
+    t_Cone->Branch("coneU_type", &m_coneU_type);
+    t_Cone->Branch("coneU_x", &m_coneU_x);
+    t_Cone->Branch("coneU_y", &m_coneU_y);
+    t_Cone->Branch("coneU_z", &m_coneU_z);
+    t_Cone->Branch("coneU_E", &m_coneU_E);
+    t_Cone->Branch("coneU_truth_tag", &m_coneU_truth_tag);
+    t_Cone->Branch("coneU_truth_MC_px", &m_coneU_truth_MC_px);
+    t_Cone->Branch("coneU_truth_MC_py", &m_coneU_truth_MC_py);
+    t_Cone->Branch("coneU_truth_MC_pz", &m_coneU_truth_MC_pz);
+    t_Cone->Branch("coneU_truth_MC_E", &m_coneU_truth_MC_E);
+    t_Cone->Branch("coneU_truth_MC_weight", &m_coneU_truth_MC_weight);    
+    t_Cone->Branch("coneU_hit_tag", &m_coneU_hit_tag);
+    t_Cone->Branch("coneU_hit_x", &m_coneU_hit_x);
+    t_Cone->Branch("coneU_hit_y", &m_coneU_hit_y);
+    t_Cone->Branch("coneU_hit_z", &m_coneU_hit_z);
+    t_Cone->Branch("coneU_hit_E", &m_coneU_hit_E);
+    t_Cone->Branch("coneV_tag", &m_coneV_tag);
+    t_Cone->Branch("coneV_type", &m_coneV_type);
+    t_Cone->Branch("coneV_x", &m_coneV_x);
+    t_Cone->Branch("coneV_y", &m_coneV_y);
+    t_Cone->Branch("coneV_z", &m_coneV_z);
+    t_Cone->Branch("coneV_E", &m_coneV_E);
+    t_Cone->Branch("coneV_truth_tag", &m_coneV_truth_tag);
+    t_Cone->Branch("coneV_truth_MC_px", &m_coneV_truth_MC_px);
+    t_Cone->Branch("coneV_truth_MC_py", &m_coneV_truth_MC_py);
+    t_Cone->Branch("coneV_truth_MC_pz", &m_coneV_truth_MC_pz);
+    t_Cone->Branch("coneV_truth_MC_E", &m_coneV_truth_MC_E);
+    t_Cone->Branch("coneV_truth_MC_weight", &m_coneV_truth_MC_weight);   
+    t_Cone->Branch("coneV_hit_tag", &m_coneV_hit_tag);
+    t_Cone->Branch("coneV_hit_x", &m_coneV_hit_x);
+    t_Cone->Branch("coneV_hit_y", &m_coneV_hit_y);
+    t_Cone->Branch("coneV_hit_z", &m_coneV_hit_z);
+    t_Cone->Branch("coneV_hit_E", &m_coneV_hit_E);
+    // Track Axis
+    t_TrackAxis->Branch("trackU_tag", &m_trackU_tag);
+    t_TrackAxis->Branch("trackU_type", &m_trackU_type);
+    t_TrackAxis->Branch("trackU_x", &m_trackU_x);
+    t_TrackAxis->Branch("trackU_y", &m_trackU_y);
+    t_TrackAxis->Branch("trackU_z", &m_trackU_z);
+    t_TrackAxis->Branch("trackU_E", &m_trackU_E);
+    t_TrackAxis->Branch("trackU_truth_tag", &m_trackU_truth_tag);
+    t_TrackAxis->Branch("trackU_truth_MC_px", &m_trackU_truth_MC_px);
+    t_TrackAxis->Branch("trackU_truth_MC_py", &m_trackU_truth_MC_py);
+    t_TrackAxis->Branch("trackU_truth_MC_pz", &m_trackU_truth_MC_pz);
+    t_TrackAxis->Branch("trackU_truth_MC_E", &m_trackU_truth_MC_E);
+    t_TrackAxis->Branch("trackU_truth_MC_weight", &m_trackU_truth_MC_weight);    
+    t_TrackAxis->Branch("trackU_hit_tag", &m_trackU_hit_tag);
+    t_TrackAxis->Branch("trackU_hit_x", &m_trackU_hit_x);
+    t_TrackAxis->Branch("trackU_hit_y", &m_trackU_hit_y);
+    t_TrackAxis->Branch("trackU_hit_z", &m_trackU_hit_z);
+    t_TrackAxis->Branch("trackU_hit_E", &m_trackU_hit_E);
+    t_TrackAxis->Branch("trackV_tag", &m_trackV_tag);
+    t_TrackAxis->Branch("trackV_type", &m_trackV_type);
+    t_TrackAxis->Branch("trackV_x", &m_trackV_x);
+    t_TrackAxis->Branch("trackV_y", &m_trackV_y);
+    t_TrackAxis->Branch("trackV_z", &m_trackV_z);
+    t_TrackAxis->Branch("trackV_E", &m_trackV_E);
+    t_TrackAxis->Branch("trackV_truth_tag", &m_trackV_truth_tag);
+    t_TrackAxis->Branch("trackV_truth_MC_px", &m_trackV_truth_MC_px);
+    t_TrackAxis->Branch("trackV_truth_MC_py", &m_trackV_truth_MC_py);
+    t_TrackAxis->Branch("trackV_truth_MC_pz", &m_trackV_truth_MC_pz);
+    t_TrackAxis->Branch("trackV_truth_MC_E", &m_trackV_truth_MC_E);
+    t_TrackAxis->Branch("trackV_truth_MC_weight", &m_trackV_truth_MC_weight);   
+    t_TrackAxis->Branch("trackV_hit_tag", &m_trackV_hit_tag);
+    t_TrackAxis->Branch("trackV_hit_x", &m_trackV_hit_x);
+    t_TrackAxis->Branch("trackV_hit_y", &m_trackV_hit_y);
+    t_TrackAxis->Branch("trackV_hit_z", &m_trackV_hit_z);
+    t_TrackAxis->Branch("trackV_hit_E", &m_trackV_hit_E);
+    //Axis
+    t_Axis->Branch("axisU_tag", &m_axisU_tag);
+    t_Axis->Branch("axisU_type", &m_axisU_type);
+    t_Axis->Branch("axisU_x", &m_axisU_x);
+    t_Axis->Branch("axisU_y", &m_axisU_y);
+    t_Axis->Branch("axisU_z", &m_axisU_z);
+    t_Axis->Branch("axisU_E", &m_axisU_E);
+    t_Axis->Branch("axisU_truth_tag", &m_axisU_truth_tag);
+    t_Axis->Branch("axisU_truth_MC_px", &m_axisU_truth_MC_px);
+    t_Axis->Branch("axisU_truth_MC_py", &m_axisU_truth_MC_py);
+    t_Axis->Branch("axisU_truth_MC_pz", &m_axisU_truth_MC_pz);
+    t_Axis->Branch("axisU_truth_MC_E", &m_axisU_truth_MC_E);
+    t_Axis->Branch("axisU_truth_MC_weight", &m_axisU_truth_MC_weight);
+    t_Axis->Branch("axisU_hit_tag", &m_axisU_hit_tag);
+    t_Axis->Branch("axisU_hit_x", &m_axisU_hit_x);
+    t_Axis->Branch("axisU_hit_y", &m_axisU_hit_y);
+    t_Axis->Branch("axisU_hit_z", &m_axisU_hit_z);
+    t_Axis->Branch("axisU_hit_E", &m_axisU_hit_E);
+    t_Axis->Branch("axisV_tag", &m_axisV_tag);
+    t_Axis->Branch("axisV_type", &m_axisV_type);
+    t_Axis->Branch("axisV_x", &m_axisV_x);
+    t_Axis->Branch("axisV_y", &m_axisV_y);
+    t_Axis->Branch("axisV_z", &m_axisV_z);
+    t_Axis->Branch("axisV_E", &m_axisV_E);
+    t_Axis->Branch("axisV_truth_tag", &m_axisV_truth_tag);
+    t_Axis->Branch("axisV_truth_MC_px", &m_axisV_truth_MC_px);
+    t_Axis->Branch("axisV_truth_MC_py", &m_axisV_truth_MC_py);
+    t_Axis->Branch("axisV_truth_MC_pz", &m_axisV_truth_MC_pz);
+    t_Axis->Branch("axisV_truth_MC_E", &m_axisV_truth_MC_E);
+    t_Axis->Branch("axisV_truth_MC_weight", &m_axisV_truth_MC_weight);
+    t_Axis->Branch("axisV_hit_tag", &m_axisV_hit_tag);
+    t_Axis->Branch("axisV_hit_x", &m_axisV_hit_x);
+    t_Axis->Branch("axisV_hit_y", &m_axisV_hit_y);
+    t_Axis->Branch("axisV_hit_z", &m_axisV_hit_z);
+    t_Axis->Branch("axisV_hit_E", &m_axisV_hit_E);    
+
+    t_Axis->Branch("emptyAxisU_tag", &m_emptyAxisU_tag);
+    t_Axis->Branch("emptyAxisU_x", &m_emptyAxisU_x);
+    t_Axis->Branch("emptyAxisU_y", &m_emptyAxisU_y);
+    t_Axis->Branch("emptyAxisU_z", &m_emptyAxisU_z);
+    t_Axis->Branch("emptyAxisU_E", &m_emptyAxisU_E);
+    t_Axis->Branch("emptyAxisV_tag", &m_emptyAxisV_tag);
+    t_Axis->Branch("emptyAxisV_x", &m_emptyAxisV_x);
+    t_Axis->Branch("emptyAxisV_y", &m_emptyAxisV_y);
+    t_Axis->Branch("emptyAxisV_z", &m_emptyAxisV_z);
+    t_Axis->Branch("emptyAxisV_E", &m_emptyAxisV_E);
+
+    //Half clusters
+    t_HalfCluster->Branch("totE_HFClusV", &m_totE_HFClusV);
+    t_HalfCluster->Branch("HalfClusterV_x", &m_HalfClusterV_x);
+    t_HalfCluster->Branch("HalfClusterV_y", &m_HalfClusterV_y);
+    t_HalfCluster->Branch("HalfClusterV_z", &m_HalfClusterV_z);
+    t_HalfCluster->Branch("HalfClusterV_E", &m_HalfClusterV_E);
+    t_HalfCluster->Branch("HalfClusterV_tag", &m_HalfClusterV_tag);
+    t_HalfCluster->Branch("HalfClusterV_type", &m_HalfClusterV_type);
+    t_HalfCluster->Branch("HalfClusterV_hit_x", &m_HalfClusterV_hit_x);
+    t_HalfCluster->Branch("HalfClusterV_hit_y", &m_HalfClusterV_hit_y);
+    t_HalfCluster->Branch("HalfClusterV_hit_z", &m_HalfClusterV_hit_z);
+    t_HalfCluster->Branch("HalfClusterV_hit_E", &m_HalfClusterV_hit_E);
+    t_HalfCluster->Branch("HalfClusterV_hit_tag", &m_HalfClusterV_hit_tag);
+    t_HalfCluster->Branch("HalfClusterV_truth_tag", &m_HalfClusterV_truth_tag);
+    t_HalfCluster->Branch("HalfClusterV_truthMC_px", &m_HalfClusterV_truthMC_px);
+    t_HalfCluster->Branch("HalfClusterV_truthMC_py", &m_HalfClusterV_truthMC_py);
+    t_HalfCluster->Branch("HalfClusterV_truthMC_pz", &m_HalfClusterV_truthMC_pz);
+    t_HalfCluster->Branch("HalfClusterV_truthMC_E", &m_HalfClusterV_truthMC_E);
+    t_HalfCluster->Branch("HalfClusterV_truthMC_weight", &m_HalfClusterV_truthMC_weight);
+
+    t_HalfCluster->Branch("totE_HFClusU", &m_totE_HFClusU);
+    t_HalfCluster->Branch("HalfClusterU_x", &m_HalfClusterU_x);
+    t_HalfCluster->Branch("HalfClusterU_y", &m_HalfClusterU_y);
+    t_HalfCluster->Branch("HalfClusterU_z", &m_HalfClusterU_z);
+    t_HalfCluster->Branch("HalfClusterU_E", &m_HalfClusterU_E);  
+    t_HalfCluster->Branch("HalfClusterU_tag", &m_HalfClusterU_tag);  
+    t_HalfCluster->Branch("HalfClusterU_type", &m_HalfClusterU_type);  
+    t_HalfCluster->Branch("HalfClusterU_hit_x", &m_HalfClusterU_hit_x);
+    t_HalfCluster->Branch("HalfClusterU_hit_y", &m_HalfClusterU_hit_y);
+    t_HalfCluster->Branch("HalfClusterU_hit_z", &m_HalfClusterU_hit_z);
+    t_HalfCluster->Branch("HalfClusterU_hit_E", &m_HalfClusterU_hit_E);
+    t_HalfCluster->Branch("HalfClusterU_hit_tag", &m_HalfClusterU_hit_tag);
+    t_HalfCluster->Branch("HalfClusterU_truth_tag", &m_HalfClusterU_truth_tag);
+    t_HalfCluster->Branch("HalfClusterU_truthMC_px", &m_HalfClusterU_truthMC_px);
+    t_HalfCluster->Branch("HalfClusterU_truthMC_py", &m_HalfClusterU_truthMC_py);
+    t_HalfCluster->Branch("HalfClusterU_truthMC_pz", &m_HalfClusterU_truthMC_pz);
+    t_HalfCluster->Branch("HalfClusterU_truthMC_E", &m_HalfClusterU_truthMC_E);
+    t_HalfCluster->Branch("HalfClusterU_truthMC_weight", &m_HalfClusterU_truthMC_weight);
+
+
+    //Tower
+    t_Tower->Branch("towerID", towerID, "towerID[3]/I");
+    t_Tower->Branch("NclusU", &m_NclusU);
+    t_Tower->Branch("NclusV", &m_NclusV);
+    t_Tower->Branch("totEn", &m_totEn);
+    t_Tower->Branch("totEn_U", &m_totEn_U);
+    t_Tower->Branch("totEn_V", &m_totEn_V);
+    t_Tower->Branch("HalfClusterU_x", &m_HalfClusterU_x);
+    t_Tower->Branch("HalfClusterU_y", &m_HalfClusterU_y);
+    t_Tower->Branch("HalfClusterU_z", &m_HalfClusterU_z);
+    t_Tower->Branch("HalfClusterU_E", &m_HalfClusterU_E);
+    t_Tower->Branch("HalfClusterU_tag", &m_HalfClusterU_tag);
+    t_Tower->Branch("HalfClusterU_type", &m_HalfClusterU_type);
+    t_Tower->Branch("HalfClusterU_nTrk", &m_HalfClusterU_nTrk);
+    t_Tower->Branch("HalfClusterV_x", &m_HalfClusterV_x);
+    t_Tower->Branch("HalfClusterV_y", &m_HalfClusterV_y);
+    t_Tower->Branch("HalfClusterV_z", &m_HalfClusterV_z);
+    t_Tower->Branch("HalfClusterV_E", &m_HalfClusterV_E);
+    t_Tower->Branch("HalfClusterV_tag", &m_HalfClusterV_tag);
+    t_Tower->Branch("HalfClusterV_type", &m_HalfClusterV_type);
+    t_Tower->Branch("HalfClusterV_nTrk", &m_HalfClusterV_nTrk);
+
+    //Clusters
+    t_Cluster->Branch("totE_Ecal", &m_totE_Ecal);
+    t_Cluster->Branch("totE_Hcal", &m_totE_Hcal);
+    t_Cluster->Branch("Nclus_Ecal", &m_Nclus_Ecal);
+    t_Cluster->Branch("Nclus_Hcal", &m_Nclus_Hcal);
+    t_Cluster->Branch("EcalClus_x", &m_EcalClus_x);
+    t_Cluster->Branch("EcalClus_y", &m_EcalClus_y);
+    t_Cluster->Branch("EcalClus_z", &m_EcalClus_z);
+    t_Cluster->Branch("EcalClus_E", &m_EcalClus_E);
+    t_Cluster->Branch("EcalClus_Escale", &m_EcalClus_Escale);
+    t_Cluster->Branch("EcalClus_nTrk", &m_EcalClus_nTrk);
+    t_Cluster->Branch("EcalClus_ptrk", &m_EcalClus_pTrk);
+    t_Cluster->Branch("EcalClus_typeU", &m_EcalClus_typeU);
+    t_Cluster->Branch("EcalClus_typeV", &m_EcalClus_typeV);
+    t_Cluster->Branch("EcalClus_hitU_x", &m_EcalClus_hitU_x);
+    t_Cluster->Branch("EcalClus_hitU_y", &m_EcalClus_hitU_y);
+    t_Cluster->Branch("EcalClus_hitU_z", &m_EcalClus_hitU_z);
+    t_Cluster->Branch("EcalClus_hitU_E", &m_EcalClus_hitU_E);
+    t_Cluster->Branch("EcalClus_hitU_tag", &m_EcalClus_hitU_tag);
+    t_Cluster->Branch("EcalClus_hitV_x", &m_EcalClus_hitV_x);
+    t_Cluster->Branch("EcalClus_hitV_y", &m_EcalClus_hitV_y);
+    t_Cluster->Branch("EcalClus_hitV_z", &m_EcalClus_hitV_z);
+    t_Cluster->Branch("EcalClus_hitV_E", &m_EcalClus_hitV_E);
+    t_Cluster->Branch("EcalClus_hitV_tag", &m_EcalClus_hitV_tag);
+    t_Cluster->Branch("EcalClus_trk_location", &m_EcalClus_trk_location);
+    t_Cluster->Branch("EcalClus_trk_tag", &m_EcalClus_trk_tag);
+    t_Cluster->Branch("EcalClus_trk_d0", &m_EcalClus_trk_d0);
+    t_Cluster->Branch("EcalClus_trk_z0", &m_EcalClus_trk_z0);
+    t_Cluster->Branch("EcalClus_trk_phi", &m_EcalClus_trk_phi);
+    t_Cluster->Branch("EcalClus_trk_tanL", &m_EcalClus_trk_tanL);
+    t_Cluster->Branch("EcalClus_trk_omega", &m_EcalClus_trk_omega);
+    t_Cluster->Branch("EcalClus_trk_kappa", &m_EcalClus_trk_kappa);
+    t_Cluster->Branch("EcalClus_truthMC_tag", &m_EcalClus_truthMC_tag);
+    t_Cluster->Branch("EcalClus_truthMC_pid", &m_EcalClus_truthMC_pid);
+    t_Cluster->Branch("EcalClus_truthMC_px", &m_EcalClus_truthMC_px);
+    t_Cluster->Branch("EcalClus_truthMC_py", &m_EcalClus_truthMC_py);
+    t_Cluster->Branch("EcalClus_truthMC_pz", &m_EcalClus_truthMC_pz);
+    t_Cluster->Branch("EcalClus_truthMC_E", &m_EcalClus_truthMC_E);
+    t_Cluster->Branch("EcalClus_truthMC_EPx", &m_EcalClus_truthMC_EPx);
+    t_Cluster->Branch("EcalClus_truthMC_EPy", &m_EcalClus_truthMC_EPy);
+    t_Cluster->Branch("EcalClus_truthMC_EPz", &m_EcalClus_truthMC_EPz);
+    t_Cluster->Branch("EcalClus_truthMC_weight", &m_EcalClus_truthMC_weight);
+    t_Cluster->Branch("HcalClus_x", &m_HcalClus_x);
+    t_Cluster->Branch("HcalClus_y", &m_HcalClus_y);
+    t_Cluster->Branch("HcalClus_z", &m_HcalClus_z);
+    t_Cluster->Branch("HcalClus_E", &m_HcalClus_E);
+    t_Cluster->Branch("HcalClus_nTrk", &m_HcalClus_nTrk);
+    t_Cluster->Branch("HcalClus_ptrk", &m_HcalClus_pTrk);
+    t_Cluster->Branch("HcalClus_hit_x", &m_HcalClus_hit_x);
+    t_Cluster->Branch("HcalClus_hit_y", &m_HcalClus_hit_y);
+    t_Cluster->Branch("HcalClus_hit_z", &m_HcalClus_hit_z);
+    t_Cluster->Branch("HcalClus_hit_E", &m_HcalClus_hit_E);
+    t_Cluster->Branch("HcalClus_hit_tag", &m_HcalClus_hit_tag);
+    t_Cluster->Branch("HcalClus_truthMC_tag", &m_HcalClus_truthMC_tag);
+    t_Cluster->Branch("HcalClus_truthMC_pid", &m_HcalClus_truthMC_pid);
+    t_Cluster->Branch("HcalClus_truthMC_px", &m_HcalClus_truthMC_px);
+    t_Cluster->Branch("HcalClus_truthMC_py", &m_HcalClus_truthMC_py);
+    t_Cluster->Branch("HcalClus_truthMC_pz", &m_HcalClus_truthMC_pz);
+    t_Cluster->Branch("HcalClus_truthMC_E", &m_HcalClus_truthMC_E);
+    t_Cluster->Branch("HcalClus_truthMC_EPx", &m_HcalClus_truthMC_EPx);
+    t_Cluster->Branch("HcalClus_truthMC_EPy", &m_HcalClus_truthMC_EPy);
+    t_Cluster->Branch("HcalClus_truthMC_EPz", &m_HcalClus_truthMC_EPz);
+    t_Cluster->Branch("HcalClus_truthMC_weight", &m_HcalClus_truthMC_weight);
+    t_Cluster->Branch("SimpleHcalClus_x", &m_SimpleHcalClus_x);
+    t_Cluster->Branch("SimpleHcalClus_y", &m_SimpleHcalClus_y);
+    t_Cluster->Branch("SimpleHcalClus_z", &m_SimpleHcalClus_z);
+    t_Cluster->Branch("SimpleHcalClus_E", &m_SimpleHcalClus_E);
+    t_Cluster->Branch("SimpleHcalClus_nTrk", &m_SimpleHcalClus_nTrk);
+    t_Cluster->Branch("SimpleHcalClus_ptrk", &m_SimpleHcalClus_pTrk);
+    t_Cluster->Branch("SimpleHcalClus_hit_x", &m_SimpleHcalClus_hit_x);
+    t_Cluster->Branch("SimpleHcalClus_hit_y", &m_SimpleHcalClus_hit_y);
+    t_Cluster->Branch("SimpleHcalClus_hit_z", &m_SimpleHcalClus_hit_z);
+    t_Cluster->Branch("SimpleHcalClus_hit_E", &m_SimpleHcalClus_hit_E);
+    t_Cluster->Branch("SimpleHcalClus_hit_tag", &m_SimpleHcalClus_hit_tag);
+    t_Cluster->Branch("SimpleHcalClus_truthMC_tag", &m_SimpleHcalClus_truthMC_tag);
+    t_Cluster->Branch("SimpleHcalClus_truthMC_pid", &m_SimpleHcalClus_truthMC_pid);
+    t_Cluster->Branch("SimpleHcalClus_truthMC_px", &m_SimpleHcalClus_truthMC_px);
+    t_Cluster->Branch("SimpleHcalClus_truthMC_py", &m_SimpleHcalClus_truthMC_py);
+    t_Cluster->Branch("SimpleHcalClus_truthMC_pz", &m_SimpleHcalClus_truthMC_pz);
+    t_Cluster->Branch("SimpleHcalClus_truthMC_E", &m_SimpleHcalClus_truthMC_E);
+    t_Cluster->Branch("SimpleHcalClus_truthMC_EPx", &m_SimpleHcalClus_truthMC_EPx);
+    t_Cluster->Branch("SimpleHcalClus_truthMC_EPy", &m_SimpleHcalClus_truthMC_EPy);
+    t_Cluster->Branch("SimpleHcalClus_truthMC_EPz", &m_SimpleHcalClus_truthMC_EPz);
+    t_Cluster->Branch("SimpleHcalClus_truthMC_weight", &m_SimpleHcalClus_truthMC_weight);
+
+    // Tracks
+    t_Track->Branch("m_Ntrk", &m_Ntrk);
+    t_Track->Branch("m_type", &m_type);
+    t_Track->Branch("m_trkstate_d0", &m_trkstate_d0);
+    t_Track->Branch("m_trkstate_z0", &m_trkstate_z0);
+    t_Track->Branch("m_trkstate_phi", &m_trkstate_phi);
+    t_Track->Branch("m_trkstate_tanL", &m_trkstate_tanL);
+    t_Track->Branch("m_trkstate_kappa", &m_trkstate_kappa);
+    t_Track->Branch("m_trkstate_omega", &m_trkstate_omega);
+    t_Track->Branch("m_trkstate_refx", &m_trkstate_refx);
+    t_Track->Branch("m_trkstate_refy", &m_trkstate_refy);
+    t_Track->Branch("m_trkstate_refz", &m_trkstate_refz);
+    t_Track->Branch("m_trkstate_location", &m_trkstate_location);
+    t_Track->Branch("m_trkstate_tag", &m_trkstate_tag);
+
+    //PFOs
+    t_PFO->Branch("pfo_tag", &pfo_tag);
+    t_PFO->Branch("pfo_n_track", &pfo_n_track);
+    t_PFO->Branch("pfo_n_ecal_clus", &pfo_n_ecal_clus);
+    t_PFO->Branch("pfo_n_hcal_clus", &pfo_n_hcal_clus);
+    t_PFO->Branch("pfo_ecal_tag", &pfo_ecal_tag);
+    t_PFO->Branch("pfo_hcal_tag", &pfo_hcal_tag);
+    t_PFO->Branch("pfo_ecal_clus_x", &pfo_ecal_clus_x);
+    t_PFO->Branch("pfo_ecal_clus_y", &pfo_ecal_clus_y);
+    t_PFO->Branch("pfo_ecal_clus_z", &pfo_ecal_clus_z);
+    t_PFO->Branch("pfo_ecal_clus_E", &pfo_ecal_clus_E);
+    t_PFO->Branch("pfo_ecal_clus_Escale", &pfo_ecal_clus_Escale);
+    t_PFO->Branch("pfo_hcal_clus_x", &pfo_hcal_clus_x);
+    t_PFO->Branch("pfo_hcal_clus_y", &pfo_hcal_clus_y);
+    t_PFO->Branch("pfo_hcal_clus_z", &pfo_hcal_clus_z);
+    t_PFO->Branch("pfo_hcal_clus_E", &pfo_hcal_clus_E);
+    t_PFO->Branch("pfo_trk_tag", &pfo_trk_tag);
+    t_PFO->Branch("pfo_trk_location", &pfo_trk_location);
+    t_PFO->Branch("pfo_trk_d0", &pfo_trk_d0);
+    t_PFO->Branch("pfo_trk_z0", &pfo_trk_z0);
+    t_PFO->Branch("pfo_trk_phi", &pfo_trk_phi);
+    t_PFO->Branch("pfo_trk_tanL", &pfo_trk_tanL);
+    t_PFO->Branch("pfo_trk_omega", &pfo_trk_omega);
+    t_PFO->Branch("pfo_trk_kappa", &pfo_trk_kappa);    
+
+  }
+
+  return GaudiAlgorithm::initialize();
+}
+
+StatusCode PandoraPlusPFAlg::execute()
+{
+// clock_t yyy_start, yyy_endrec, yyy_endfill;
+// yyy_start = clock(); // 记录开始时间
+
+  if(_nEvt==0) std::cout<<"PandoraPlusPFAlg::execute Start"<<std::endl;
+  std::cout<<"Processing event: "<<_nEvt<<std::endl;
+
+  if(_nEvt<m_Nskip){ _nEvt++;  return GaudiAlgorithm::initialize(); }
+
+  //InitializeForNewEvent(); 
+  PandoraPlusDataCol     m_DataCol;
+  m_DataCol.Clear();
+  m_DataCol.EnergyCorrSvc = m_energycorsvc; 
+
+
+  //Readin collections 
+  m_pMCParticleCreator->CreateMCParticle( m_DataCol, *r_MCParticleCol );
+
+  if(m_useTruthTrk) m_pTrackCreator->CreateTracksFromMCParticle(m_DataCol, *r_MCParticleCol);
+  else m_pTrackCreator->CreateTracks( m_DataCol, r_TrackCols, r_MCPTrkAssoCol );
+
+  m_pCaloHitsCreator->CreateCaloHits( m_DataCol, r_CaloHitCols, map_readout_decoder, map_CaloMCPAssoCols );
+
+  //Perform PFA algorithm
+  m_algorithmManager.RunAlgorithm( m_DataCol );
+
+  m_pOutputCreator->CreateOutputCollections( m_DataCol, 
+                                             w_RecEcalCol, 
+                                             w_RecCoreCol, 
+                                             w_RecHcalCol,
+                                             w_RecTrkCol, 
+                                             w_ClusterCollection, 
+                                             w_ReconstructedParticleCollection);
+
+// yyy_endrec = clock();  // 重建结束的时间
+
+cout<<"Write tuples"<<endl;
+  //---------------------Write Ana tuples-------------------------
+  // MC particles  
+  ClearMCParticle();
+  std::vector<edm4hep::MCParticle> m_MCPCol = m_DataCol.collectionMap_MC[name_MCParticleCol.value()];
+  for(int imc=0; imc<m_MCPCol.size(); imc++){
+    m_mcPdgid.push_back( m_MCPCol[imc].getPDG() );
+    m_mcStatus.push_back( m_MCPCol[imc].getGeneratorStatus() );
+    m_mcPx.push_back( m_MCPCol[imc].getMomentum()[0] );
+    m_mcPy.push_back( m_MCPCol[imc].getMomentum()[1] );
+    m_mcPz.push_back( m_MCPCol[imc].getMomentum()[2] );
+    m_mcEn.push_back( m_MCPCol[imc].getEnergy() );
+    m_mcMass.push_back( m_MCPCol[imc].getMass() );
+    m_mcCharge.push_back( m_MCPCol[imc].getCharge() );
+    m_mcEPx.push_back( m_MCPCol[imc].getEndpoint()[0] );
+    m_mcEPy.push_back( m_MCPCol[imc].getEndpoint()[1] );
+    m_mcEPz.push_back( m_MCPCol[imc].getEndpoint()[2] );
+    //double tmp_phi = std::atan2(m_MCPCol[imc].getMomentum()[1], m_MCPCol[imc].getMomentum()[0])* 180.0 / M_PI;
+    //if (tmp_phi < 0) tmp_phi += 360.0;
+    //double tmp_theta = std::atan2(m_MCPCol[imc].getMomentum()[2], sqrt(m_MCPCol[imc].getMomentum()[1]*m_MCPCol[imc].getMomentum()[1]+m_MCPCol[imc].getMomentum()[0]*m_MCPCol[imc].getMomentum()[0]))* 180.0 / M_PI + 90; 
+    //cout<<"MCParticle: "<<imc<<" PDG: "<<m_MCPCol[imc].getPDG()<<" Theta: "<<tmp_theta<<" Phi: "<<tmp_phi<<endl;
+
+    double EnDep_ecal = GetParticleDepEnergy(m_MCPCol[imc], m_DataCol.map_BarCol["BarCol"]);
+    double EnDep_hcal = GetParticleDepEnergy(m_MCPCol[imc], m_DataCol.map_CaloHit["HCALBarrel"]);
+    m_depEn_ecal.push_back(EnDep_ecal);
+    m_depEn_hcal.push_back(EnDep_hcal);
+  }
+  t_MCParticle->Fill();
+
+
+  //Save Raw bars information
+  ClearBar();
+  m_totE_EcalSim = 0.;
+  for(int ibar=0;ibar<m_DataCol.map_BarCol["BarCol"].size();ibar++){
+    auto p_hitbar = m_DataCol.map_BarCol["BarCol"][ibar].get();
+    m_simBar_x.push_back(p_hitbar->getPosition().x());
+    m_simBar_y.push_back(p_hitbar->getPosition().y());
+    m_simBar_z.push_back(p_hitbar->getPosition().z());
+    m_simBar_Q1.push_back(p_hitbar->getQ1());
+    m_simBar_Q2.push_back(p_hitbar->getQ2());
+    m_simBar_T1.push_back(p_hitbar->getT1());
+    m_simBar_T2.push_back(p_hitbar->getT2());
+    m_simBar_module.push_back(p_hitbar->getModule());
+    m_simBar_dlayer.push_back(p_hitbar->getDlayer());
+    m_simBar_stave.push_back(p_hitbar->getStave());
+    m_simBar_slayer.push_back(p_hitbar->getSlayer());
+    m_simBar_bar.push_back(p_hitbar->getBar());
+    m_totE_EcalSim += (p_hitbar->getQ1()+p_hitbar->getQ2())/2.; 
+
+    auto truthMap = p_hitbar->getLinkedMCP();
+    for(auto iter: truthMap){
+      m_simBar_truthMC_tag.push_back(ibar);
+      m_simBar_truthMC_pid.push_back(iter.first.getPDG());
+      m_simBar_truthMC_px.push_back(iter.first.getMomentum().x);
+      m_simBar_truthMC_py.push_back(iter.first.getMomentum().y);
+      m_simBar_truthMC_pz.push_back(iter.first.getMomentum().z);
+      m_simBar_truthMC_E.push_back(iter.first.getEnergy());
+      m_simBar_truthMC_EPx.push_back(iter.first.getEndpoint().x);
+      m_simBar_truthMC_EPy.push_back(iter.first.getEndpoint().y);
+      m_simBar_truthMC_EPz.push_back(iter.first.getEndpoint().z);
+      m_simBar_truthMC_weight.push_back(iter.second);
+    }
+  }
+
+  std::vector<PandoraPlus::CaloHit*> m_hcalHitsCol; m_hcalHitsCol.clear();
+  for(int ih=0; ih<m_DataCol.map_CaloHit["HCALBarrel"].size(); ih++)
+    m_hcalHitsCol.push_back( m_DataCol.map_CaloHit["HCALBarrel"][ih].get() );
+
+  m_totE_HcalSim = 0.;
+  for(int ihit=0; ihit<m_hcalHitsCol.size(); ihit++){
+    m_HcalHit_x.push_back( m_hcalHitsCol[ihit]->getPosition().x() );
+    m_HcalHit_y.push_back( m_hcalHitsCol[ihit]->getPosition().y() );
+    m_HcalHit_z.push_back( m_hcalHitsCol[ihit]->getPosition().z() );
+    m_HcalHit_E.push_back( m_hcalHitsCol[ihit]->getEnergy() );
+    m_HcalHit_layer.push_back( m_hcalHitsCol[ihit]->getLayer() );
+    m_totE_HcalSim += m_hcalHitsCol[ihit]->getEnergy(); 
+
+    auto truthMap = m_hcalHitsCol[ihit]->getLinkedMCP();
+    for(auto iter: truthMap){
+      m_HcalHit_truthMC_tag.push_back(ihit);
+      m_HcalHit_truthMC_pid.push_back(iter.first.getPDG());
+      m_HcalHit_truthMC_px.push_back(iter.first.getMomentum().x);
+      m_HcalHit_truthMC_py.push_back(iter.first.getMomentum().y);
+      m_HcalHit_truthMC_pz.push_back(iter.first.getMomentum().z);
+      m_HcalHit_truthMC_E.push_back(iter.first.getEnergy());
+      m_HcalHit_truthMC_EPx.push_back(iter.first.getEndpoint().x);
+      m_HcalHit_truthMC_EPy.push_back(iter.first.getEndpoint().y);
+      m_HcalHit_truthMC_EPz.push_back(iter.first.getEndpoint().z);
+      m_HcalHit_truthMC_weight.push_back(iter.second);
+    }    
+
+  }
+  t_SimBar->Fill();
+
+  //Save localMax
+  ClearLocalMax();
+  std::vector<PandoraPlus::CaloHalfCluster*> m_halfclusters; m_halfclusters.clear();
+  for(int i=0; i<m_DataCol.map_HalfCluster["HalfClusterColU"].size(); i++)
+    m_halfclusters.push_back( m_DataCol.map_HalfCluster["HalfClusterColU"][i].get() );
+
+  std::vector<const Calo1DCluster*> m_local_max; m_local_max.clear();
+  for(int ic=0;ic<m_halfclusters.size(); ic++){
+    std::vector<const Calo1DCluster*> tmp_shower = m_halfclusters[ic]->getLocalMaxCol("AllLocalMax");
+    m_local_max.insert(m_local_max.end(), tmp_shower.begin(), tmp_shower.end());
+  }
+  for(int il=0; il<m_local_max.size(); il++){
+    m_localMaxU_tag.push_back( il );
+    m_localMaxU_x.push_back( m_local_max[il]->getPos().x() );
+    m_localMaxU_y.push_back( m_local_max[il]->getPos().y() );
+    m_localMaxU_z.push_back( m_local_max[il]->getPos().z() );
+    m_localMaxU_E.push_back( m_local_max[il]->getEnergy() );
+
+    auto truthMap = m_local_max[il]->getLinkedMCP();
+    for(auto iter: truthMap){
+      m_localMaxU_mc_tag.push_back(il);
+      m_localMaxU_mc_pdg.push_back(iter.first.getPDG());
+      m_localMaxU_mc_px.push_back(iter.first.getMomentum().x);
+      m_localMaxU_mc_py.push_back(iter.first.getMomentum().y);
+      m_localMaxU_mc_pz.push_back(iter.first.getMomentum().z);
+      m_localMaxU_mc_weight.push_back(iter.second);
+    }
+  }
+  m_halfclusters.clear();
+  m_local_max.clear(); 
+  for(int i=0; i<m_DataCol.map_HalfCluster["HalfClusterColV"].size(); i++)
+    m_halfclusters.push_back( m_DataCol.map_HalfCluster["HalfClusterColV"][i].get() );
+
+  for(int ic=0;ic<m_halfclusters.size(); ic++){
+    std::vector<const Calo1DCluster*> tmp_shower = m_halfclusters[ic]->getLocalMaxCol("AllLocalMax");
+    m_local_max.insert(m_local_max.end(), tmp_shower.begin(), tmp_shower.end());
+  }
+  for(int il=0; il<m_local_max.size(); il++){
+    m_localMaxV_tag.push_back( il );
+    m_localMaxV_x.push_back( m_local_max[il]->getPos().x() );
+    m_localMaxV_y.push_back( m_local_max[il]->getPos().y() );
+    m_localMaxV_z.push_back( m_local_max[il]->getPos().z() );
+    m_localMaxV_E.push_back( m_local_max[il]->getEnergy() );
+
+    auto truthMap = m_local_max[il]->getLinkedMCP();
+    for(auto iter: truthMap){
+      m_localMaxV_mc_tag.push_back(il);
+      m_localMaxV_mc_pdg.push_back(iter.first.getPDG());
+      m_localMaxV_mc_px.push_back(iter.first.getMomentum().x);
+      m_localMaxV_mc_py.push_back(iter.first.getMomentum().y);
+      m_localMaxV_mc_pz.push_back(iter.first.getMomentum().z);
+      m_localMaxV_mc_weight.push_back(iter.second);
+    }
+  }
+  t_LocalMax->Fill();
+
+  //Save 1DCluster
+  ClearLayer();
+  m_halfclusters.clear();
+  for(int i=0; i<m_DataCol.map_HalfCluster["ESHalfClusterU"].size(); i++)
+    m_halfclusters.push_back( m_DataCol.map_HalfCluster["ESHalfClusterU"][i].get() );
+
+  m_local_max.clear();
+  for(int ic=0;ic<m_halfclusters.size(); ic++){
+    //std::vector<const Calo1DCluster*> tmp_shower = m_halfclusters[ic]->getLocalMaxCol("AllLocalMax");
+    std::vector<const CaloHalfCluster*> m_axis = m_halfclusters[ic]->getHalfClusterCol("MergedAxis");
+    if(m_axis.size()>0){
+      std::vector<const Calo1DCluster*> tmp_shower = m_axis[0]->getCluster();
+      m_local_max.insert(m_local_max.end(), tmp_shower.begin(), tmp_shower.end());
+    }
+  }
+  for(int il=0; il<m_local_max.size(); il++){
+    m_barShowerU_tag.push_back( il );
+    m_barShowerU_x.push_back( m_local_max[il]->getPos().x() );
+    m_barShowerU_y.push_back( m_local_max[il]->getPos().y() );
+    m_barShowerU_z.push_back( m_local_max[il]->getPos().z() );
+    m_barShowerU_E.push_back( m_local_max[il]->getEnergy() );  
+
+    auto truthMap = m_local_max[il]->getLinkedMCP();
+    for(auto iter: truthMap){
+      m_barShowerU_mc_tag.push_back(il);
+      m_barShowerU_mc_pdg.push_back(iter.first.getPDG());
+      m_barShowerU_mc_px.push_back(iter.first.getMomentum().x);
+      m_barShowerU_mc_py.push_back(iter.first.getMomentum().y);
+      m_barShowerU_mc_pz.push_back(iter.first.getMomentum().z);
+      m_barShowerU_mc_weight.push_back(iter.second);
+    }
+  }
+  m_halfclusters.clear();
+  m_local_max.clear();
+  for(int i=0; i<m_DataCol.map_HalfCluster["ESHalfClusterV"].size(); i++)
+    m_halfclusters.push_back( m_DataCol.map_HalfCluster["ESHalfClusterV"][i].get() );
+
+  for(int ic=0;ic<m_halfclusters.size(); ic++){
+    //std::vector<const Calo1DCluster*> tmp_shower = m_halfclusters[ic]->getLocalMaxCol("AllLocalMax");
+    std::vector<const CaloHalfCluster*> m_axis = m_halfclusters[ic]->getHalfClusterCol("MergedAxis");
+    if(m_axis.size()>0){
+      std::vector<const Calo1DCluster*> tmp_shower = m_axis[0]->getCluster();
+      m_local_max.insert(m_local_max.end(), tmp_shower.begin(), tmp_shower.end());
+    }
+  }
+  for(int il=0; il<m_local_max.size(); il++){
+    m_barShowerV_tag.push_back( il );
+    m_barShowerV_x.push_back( m_local_max[il]->getPos().x() );
+    m_barShowerV_y.push_back( m_local_max[il]->getPos().y() );
+    m_barShowerV_z.push_back( m_local_max[il]->getPos().z() );
+    m_barShowerV_E.push_back( m_local_max[il]->getEnergy() );
+
+    auto truthMap = m_local_max[il]->getLinkedMCP();
+    for(auto iter: truthMap){
+      m_barShowerV_mc_tag.push_back(il);
+      m_barShowerV_mc_pdg.push_back(iter.first.getPDG());
+      m_barShowerV_mc_px.push_back(iter.first.getMomentum().x);
+      m_barShowerV_mc_py.push_back(iter.first.getMomentum().y);
+      m_barShowerV_mc_pz.push_back(iter.first.getMomentum().z);
+      m_barShowerV_mc_weight.push_back(iter.second);
+    }
+  }
+  t_Layers->Fill();
+
+  std::vector<const PandoraPlus::CaloHalfCluster*> m_halfclusterV; m_halfclusterV.clear();
+  std::vector<const PandoraPlus::CaloHalfCluster*> m_halfclusterU; m_halfclusterU.clear();
+  for(int i=0; i<m_DataCol.map_HalfCluster["HalfClusterColU"].size(); i++){
+    m_halfclusterU.push_back( m_DataCol.map_HalfCluster["HalfClusterColU"][i].get() );
+  }
+  for(int i=0; i<m_DataCol.map_HalfCluster["HalfClusterColV"].size(); i++){
+    m_halfclusterV.push_back( m_DataCol.map_HalfCluster["HalfClusterColV"][i].get() );
+  }
+  // Hough
+  ClearHough();
+  int houghU_index=0;
+  int houghV_index=0;
+  for(int i=0; i<m_halfclusterU.size(); i++){  // loop half cluster U
+    std::vector<const PandoraPlus::CaloHalfCluster*> m_mergedaxisU = m_halfclusterU[i]->getHalfClusterCol("HoughAxis");
+    for(int ita=0; ita<m_mergedaxisU.size(); ita++){ // loop  axis U
+      // General information of the axis
+      m_houghU_tag.push_back(houghU_index);
+      m_houghU_type.push_back(m_mergedaxisU[ita]->getType());
+      m_houghU_x.push_back(m_mergedaxisU[ita]->getPos().x());
+      m_houghU_y.push_back(m_mergedaxisU[ita]->getPos().y());
+      m_houghU_z.push_back(m_mergedaxisU[ita]->getPos().z());
+      m_houghU_E.push_back(m_mergedaxisU[ita]->getEnergy());
+
+      // MC truth information of the Axis
+      auto truthMap = m_mergedaxisU[ita]->getLinkedMCP();
+      for(auto iter: truthMap){
+        m_houghU_truth_tag.push_back(houghU_index);
+        m_houghU_truth_MC_px.push_back(iter.first.getMomentum().x);
+        m_houghU_truth_MC_py.push_back(iter.first.getMomentum().y);
+        m_houghU_truth_MC_pz.push_back(iter.first.getMomentum().z);
+        m_houghU_truth_MC_E.push_back(iter.first.getEnergy());
+        m_houghU_truth_MC_weight.push_back(iter.second);
+      }
+
+      // Hits on axis
+      for(int ilm=0; ilm<m_mergedaxisU[ita]->getCluster().size(); ilm++){ // loop local max
+        m_houghU_hit_tag.push_back(houghU_index);
+        m_houghU_hit_x.push_back( m_mergedaxisU[ita]->getCluster()[ilm]->getPos().x() );
+        m_houghU_hit_y.push_back( m_mergedaxisU[ita]->getCluster()[ilm]->getPos().y() );
+        m_houghU_hit_z.push_back( m_mergedaxisU[ita]->getCluster()[ilm]->getPos().z() );
+        m_houghU_hit_E.push_back( m_mergedaxisU[ita]->getCluster()[ilm]->getEnergy()  );
+      }
+
+      houghU_index++;
+    }
+  }
+  for(int i=0; i<m_halfclusterV.size(); i++){  // loop half cluster V
+    std::vector<const PandoraPlus::CaloHalfCluster*> m_mergedaxisV = m_halfclusterV[i]->getHalfClusterCol("HoughAxis");
+    for(int ita=0; ita<m_mergedaxisV.size(); ita++){ // loop  axis V
+      // General information of the axis
+      m_houghV_tag.push_back(houghV_index);
+      m_houghV_type.push_back(m_mergedaxisV[ita]->getType());
+      m_houghV_x.push_back(m_mergedaxisV[ita]->getPos().x());
+      m_houghV_y.push_back(m_mergedaxisV[ita]->getPos().y());
+      m_houghV_z.push_back(m_mergedaxisV[ita]->getPos().z());
+      m_houghV_E.push_back(m_mergedaxisV[ita]->getEnergy());
+      m_houghV_alpha.push_back(m_mergedaxisV[ita]->getHoughAlpha());
+      m_houghV_rho.push_back(m_mergedaxisV[ita]->getHoughRho());
+
+      // MC truth information of the Axis
+      auto truthMap = m_mergedaxisV[ita]->getLinkedMCP();
+      for(auto iter: truthMap){
+        m_houghV_truth_tag.push_back(houghV_index);
+        m_houghV_truth_MC_px.push_back(iter.first.getMomentum().x);
+        m_houghV_truth_MC_py.push_back(iter.first.getMomentum().y);
+        m_houghV_truth_MC_pz.push_back(iter.first.getMomentum().z);
+        m_houghV_truth_MC_E.push_back(iter.first.getEnergy());
+        m_houghV_truth_MC_weight.push_back(iter.second);
+      }
+
+      // Hits on axis
+      for(int ilm=0; ilm<m_mergedaxisV[ita]->getCluster().size(); ilm++){
+        m_houghV_hit_tag.push_back(houghV_index);
+        m_houghV_hit_x.push_back(m_mergedaxisV[ita]->getCluster()[ilm]->getPos().x());
+        m_houghV_hit_y.push_back(m_mergedaxisV[ita]->getCluster()[ilm]->getPos().y());
+        m_houghV_hit_z.push_back(m_mergedaxisV[ita]->getCluster()[ilm]->getPos().z());
+        m_houghV_hit_E.push_back(m_mergedaxisV[ita]->getCluster()[ilm]->getEnergy());
+      }
+
+      houghV_index++;
+    }
+  }
+  t_Hough->Fill();
+  // Cone
+  ClearCone();
+  int coneU_index=0;
+  int coneV_index=0;
+  for(int i=0; i<m_halfclusterU.size(); i++){  // loop half cluster U
+    std::vector<const PandoraPlus::CaloHalfCluster*> m_mergedaxisU = m_halfclusterU[i]->getHalfClusterCol("ConeAxis");
+    for(int ita=0; ita<m_mergedaxisU.size(); ita++){ // loop  axis U
+      // General information of the axis
+      m_coneU_tag.push_back(coneU_index);
+      m_coneU_type.push_back(m_mergedaxisU[ita]->getType());
+      m_coneU_x.push_back(m_mergedaxisU[ita]->getPos().x());
+      m_coneU_y.push_back(m_mergedaxisU[ita]->getPos().y());
+      m_coneU_z.push_back(m_mergedaxisU[ita]->getPos().z());
+      m_coneU_E.push_back(m_mergedaxisU[ita]->getEnergy());
+
+      // MC truth information of the Axis
+      auto truthMap = m_mergedaxisU[ita]->getLinkedMCP();
+      for(auto iter: truthMap){
+        m_coneU_truth_tag.push_back(coneU_index);
+        m_coneU_truth_MC_px.push_back(iter.first.getMomentum().x);
+        m_coneU_truth_MC_py.push_back(iter.first.getMomentum().y);
+        m_coneU_truth_MC_pz.push_back(iter.first.getMomentum().z);
+        m_coneU_truth_MC_E.push_back(iter.first.getEnergy());
+        m_coneU_truth_MC_weight.push_back(iter.second);
+      }
+
+      // Hits on axis
+      for(int ilm=0; ilm<m_mergedaxisU[ita]->getCluster().size(); ilm++){ // loop local max
+        m_coneU_hit_tag.push_back(coneU_index);
+        m_coneU_hit_x.push_back( m_mergedaxisU[ita]->getCluster()[ilm]->getPos().x() );
+        m_coneU_hit_y.push_back( m_mergedaxisU[ita]->getCluster()[ilm]->getPos().y() );
+        m_coneU_hit_z.push_back( m_mergedaxisU[ita]->getCluster()[ilm]->getPos().z() );
+        m_coneU_hit_E.push_back( m_mergedaxisU[ita]->getCluster()[ilm]->getEnergy()  );
+      }
+
+      coneU_index++;
+    }
+  }
+  for(int i=0; i<m_halfclusterV.size(); i++){  // loop half cluster V
+    std::vector<const PandoraPlus::CaloHalfCluster*> m_mergedaxisV = m_halfclusterV[i]->getHalfClusterCol("ConeAxis");
+    for(int ita=0; ita<m_mergedaxisV.size(); ita++){ // loop  axis V
+      // General information of the axis
+      m_coneV_tag.push_back(coneV_index);
+      m_coneV_type.push_back(m_mergedaxisV[ita]->getType());
+      m_coneV_x.push_back(m_mergedaxisV[ita]->getPos().x());
+      m_coneV_y.push_back(m_mergedaxisV[ita]->getPos().y());
+      m_coneV_z.push_back(m_mergedaxisV[ita]->getPos().z());
+      m_coneV_E.push_back(m_mergedaxisV[ita]->getEnergy());
+
+      // MC truth information of the Axis
+      auto truthMap = m_mergedaxisV[ita]->getLinkedMCP();
+      for(auto iter: truthMap){
+        m_coneV_truth_tag.push_back(coneV_index);
+        m_coneV_truth_MC_px.push_back(iter.first.getMomentum().x);
+        m_coneV_truth_MC_py.push_back(iter.first.getMomentum().y);
+        m_coneV_truth_MC_pz.push_back(iter.first.getMomentum().z);
+        m_coneV_truth_MC_E.push_back(iter.first.getEnergy());
+        m_coneV_truth_MC_weight.push_back(iter.second);
+      }
+
+      // Hits on axis
+      for(int ilm=0; ilm<m_mergedaxisV[ita]->getCluster().size(); ilm++){
+        m_coneV_hit_tag.push_back(coneV_index);
+        m_coneV_hit_x.push_back(m_mergedaxisV[ita]->getCluster()[ilm]->getPos().x());
+        m_coneV_hit_y.push_back(m_mergedaxisV[ita]->getCluster()[ilm]->getPos().y());
+        m_coneV_hit_z.push_back(m_mergedaxisV[ita]->getCluster()[ilm]->getPos().z());
+        m_coneV_hit_E.push_back(m_mergedaxisV[ita]->getCluster()[ilm]->getEnergy());
+      }
+
+      coneV_index++;
+    }
+  }
+  t_Cone->Fill();
+  // Track axis
+  ClearTrackAxis();
+  int trackU_index=0;
+  int trackV_index=0;
+  for(int i=0; i<m_halfclusterU.size(); i++){  // loop half cluster U
+    std::vector<const PandoraPlus::CaloHalfCluster*> m_mergedaxisU = m_halfclusterU[i]->getHalfClusterCol("TrackAxis");
+    for(int ita=0; ita<m_mergedaxisU.size(); ita++){ // loop  axis U
+      // General information of the axis
+      m_trackU_tag.push_back(trackU_index);
+      m_trackU_type.push_back(m_mergedaxisU[ita]->getType());
+      m_trackU_x.push_back(m_mergedaxisU[ita]->getPos().x());
+      m_trackU_y.push_back(m_mergedaxisU[ita]->getPos().y());
+      m_trackU_z.push_back(m_mergedaxisU[ita]->getPos().z());
+      m_trackU_E.push_back(m_mergedaxisU[ita]->getEnergy());
+
+      // MC truth information of the Axis
+      auto truthMap = m_mergedaxisU[ita]->getLinkedMCP();
+      for(auto iter: truthMap){
+        m_trackU_truth_tag.push_back(trackU_index);
+        m_trackU_truth_MC_px.push_back(iter.first.getMomentum().x);
+        m_trackU_truth_MC_py.push_back(iter.first.getMomentum().y);
+        m_trackU_truth_MC_pz.push_back(iter.first.getMomentum().z);
+        m_trackU_truth_MC_E.push_back(iter.first.getEnergy());
+        m_trackU_truth_MC_weight.push_back(iter.second);
+      }
+
+      // Hits on axis
+      for(int ilm=0; ilm<m_mergedaxisU[ita]->getCluster().size(); ilm++){ // loop local max
+        m_trackU_hit_tag.push_back(trackU_index);
+        m_trackU_hit_x.push_back( m_mergedaxisU[ita]->getCluster()[ilm]->getPos().x() );
+        m_trackU_hit_y.push_back( m_mergedaxisU[ita]->getCluster()[ilm]->getPos().y() );
+        m_trackU_hit_z.push_back( m_mergedaxisU[ita]->getCluster()[ilm]->getPos().z() );
+        m_trackU_hit_E.push_back( m_mergedaxisU[ita]->getCluster()[ilm]->getEnergy()  );
+      }
+
+      trackU_index++;
+    }
+  }
+  for(int i=0; i<m_halfclusterV.size(); i++){  // loop half cluster V
+    std::vector<const PandoraPlus::CaloHalfCluster*> m_mergedaxisV = m_halfclusterV[i]->getHalfClusterCol("TrackAxis");
+    for(int ita=0; ita<m_mergedaxisV.size(); ita++){ // loop  axis V
+      // General information of the axis
+      m_trackV_tag.push_back(trackV_index);
+      m_trackV_type.push_back(m_mergedaxisV[ita]->getType());
+      m_trackV_x.push_back(m_mergedaxisV[ita]->getPos().x());
+      m_trackV_y.push_back(m_mergedaxisV[ita]->getPos().y());
+      m_trackV_z.push_back(m_mergedaxisV[ita]->getPos().z());
+      m_trackV_E.push_back(m_mergedaxisV[ita]->getEnergy());
+
+      // MC truth information of the Axis
+      auto truthMap = m_mergedaxisV[ita]->getLinkedMCP();
+      for(auto iter: truthMap){
+        m_trackV_truth_tag.push_back(trackV_index);
+        m_trackV_truth_MC_px.push_back(iter.first.getMomentum().x);
+        m_trackV_truth_MC_py.push_back(iter.first.getMomentum().y);
+        m_trackV_truth_MC_pz.push_back(iter.first.getMomentum().z);
+        m_trackV_truth_MC_E.push_back(iter.first.getEnergy());
+        m_trackV_truth_MC_weight.push_back(iter.second);
+      }
+
+      // Hits on axis
+      for(int ilm=0; ilm<m_mergedaxisV[ita]->getCluster().size(); ilm++){
+        m_trackV_hit_tag.push_back(trackV_index);
+        m_trackV_hit_x.push_back(m_mergedaxisV[ita]->getCluster()[ilm]->getPos().x());
+        m_trackV_hit_y.push_back(m_mergedaxisV[ita]->getCluster()[ilm]->getPos().y());
+        m_trackV_hit_z.push_back(m_mergedaxisV[ita]->getCluster()[ilm]->getPos().z());
+        m_trackV_hit_E.push_back(m_mergedaxisV[ita]->getCluster()[ilm]->getEnergy());
+      }
+
+      trackV_index++;
+    }
+  }
+  t_TrackAxis->Fill();
+  //Axis
+  ClearAxis();
+  int axisU_index=0;
+  int axisV_index=0;
+  for(int i=0; i<m_halfclusterU.size(); i++){  // loop half cluster U
+    std::vector<const PandoraPlus::CaloHalfCluster*> m_mergedaxisU = m_halfclusterU[i]->getHalfClusterCol("MergedAxis");
+    for(int ita=0; ita<m_mergedaxisU.size(); ita++){ // loop  axis U
+      // General information of the axis
+      m_axisU_tag.push_back(axisU_index);
+      m_axisU_type.push_back(m_mergedaxisU[ita]->getType());
+      m_axisU_x.push_back(m_mergedaxisU[ita]->getPos().x());
+      m_axisU_y.push_back(m_mergedaxisU[ita]->getPos().y());
+      m_axisU_z.push_back(m_mergedaxisU[ita]->getPos().z());
+      m_axisU_E.push_back(m_mergedaxisU[ita]->getEnergy());
+
+      // MC truth information of the Axis
+      auto truthMap = m_mergedaxisU[ita]->getLinkedMCP();
+      for(auto iter: truthMap){
+        m_axisU_truth_tag.push_back(axisU_index);
+        m_axisU_truth_MC_px.push_back(iter.first.getMomentum().x);
+        m_axisU_truth_MC_py.push_back(iter.first.getMomentum().y);
+        m_axisU_truth_MC_pz.push_back(iter.first.getMomentum().z);
+        m_axisU_truth_MC_E.push_back(iter.first.getEnergy());
+        m_axisU_truth_MC_weight.push_back(iter.second);
+      }
+      // Hits on axis
+      for(int ilm=0; ilm<m_mergedaxisU[ita]->getCluster().size(); ilm++){ // loop local max
+        m_axisU_hit_tag.push_back(axisU_index);
+        m_axisU_hit_x.push_back( m_mergedaxisU[ita]->getCluster()[ilm]->getPos().x() );
+        m_axisU_hit_y.push_back( m_mergedaxisU[ita]->getCluster()[ilm]->getPos().y() );
+        m_axisU_hit_z.push_back( m_mergedaxisU[ita]->getCluster()[ilm]->getPos().z() );
+        m_axisU_hit_E.push_back( m_mergedaxisU[ita]->getCluster()[ilm]->getEnergy()  );
+      }
+
+      axisU_index++;
+    }
+  }
+  for(int i=0; i<m_halfclusterV.size(); i++){  // loop half cluster V
+    std::vector<const PandoraPlus::CaloHalfCluster*> m_mergedaxisV = m_halfclusterV[i]->getHalfClusterCol("MergedAxis");
+    for(int ita=0; ita<m_mergedaxisV.size(); ita++){ // loop  axis V
+      // General information of the axis
+      m_axisV_tag.push_back(axisV_index);
+      m_axisV_type.push_back(m_mergedaxisV[ita]->getType());
+      m_axisV_x.push_back(m_mergedaxisV[ita]->getPos().x());
+      m_axisV_y.push_back(m_mergedaxisV[ita]->getPos().y());
+      m_axisV_z.push_back(m_mergedaxisV[ita]->getPos().z());
+      m_axisV_E.push_back(m_mergedaxisV[ita]->getEnergy());
+
+      // MC truth information of the Axis
+      auto truthMap = m_mergedaxisV[ita]->getLinkedMCP();
+      for(auto iter: truthMap){
+        m_axisV_truth_tag.push_back(axisV_index);
+        m_axisV_truth_MC_px.push_back(iter.first.getMomentum().x);
+        m_axisV_truth_MC_py.push_back(iter.first.getMomentum().y);
+        m_axisV_truth_MC_pz.push_back(iter.first.getMomentum().z);
+        m_axisV_truth_MC_E.push_back(iter.first.getEnergy());
+        m_axisV_truth_MC_weight.push_back(iter.second);
+      }
+      // Hits on axis
+      for(int ilm=0; ilm<m_mergedaxisV[ita]->getCluster().size(); ilm++){
+        m_axisV_hit_tag.push_back(axisV_index);
+        m_axisV_hit_x.push_back(m_mergedaxisV[ita]->getCluster()[ilm]->getPos().x());
+        m_axisV_hit_y.push_back(m_mergedaxisV[ita]->getCluster()[ilm]->getPos().y());
+        m_axisV_hit_z.push_back(m_mergedaxisV[ita]->getCluster()[ilm]->getPos().z());
+        m_axisV_hit_E.push_back(m_mergedaxisV[ita]->getCluster()[ilm]->getEnergy());
+      }
+
+      axisV_index++;
+    }
+  }
+  m_halfclusterU.clear();
+  m_halfclusterV.clear();
+  for(int i=0; i<m_DataCol.map_HalfCluster["emptyHalfClusterU"].size(); i++){
+    m_halfclusterU.push_back( m_DataCol.map_HalfCluster["emptyHalfClusterU"][i].get() );
+  }
+  for(int i=0; i<m_DataCol.map_HalfCluster["emptyHalfClusterV"].size(); i++){
+    m_halfclusterV.push_back( m_DataCol.map_HalfCluster["emptyHalfClusterV"][i].get() );
+  }
+  for(int i=0; i<m_halfclusterU.size(); i++){
+    m_emptyAxisU_tag.push_back(m_halfclusterU[i]->getType());
+    m_emptyAxisU_x.push_back(m_halfclusterU[i]->getPos().x());
+    m_emptyAxisU_y.push_back(m_halfclusterU[i]->getPos().y());
+    m_emptyAxisU_z.push_back(m_halfclusterU[i]->getPos().z());
+    m_emptyAxisU_E.push_back(m_halfclusterU[i]->getEnergy());
+  }
+  for(int i=0; i<m_halfclusterV.size(); i++){
+    m_emptyAxisV_tag.push_back(m_halfclusterV[i]->getType());
+    m_emptyAxisV_x.push_back(m_halfclusterV[i]->getPos().x());
+    m_emptyAxisV_y.push_back(m_halfclusterV[i]->getPos().y());
+    m_emptyAxisV_z.push_back(m_halfclusterV[i]->getPos().z());
+    m_emptyAxisV_E.push_back(m_halfclusterV[i]->getEnergy());
+  }
+  
+  t_Axis->Fill();
+
+
+  //Half cluster
+  ClearHalfCluster();
+  m_halfclusterV.clear();
+  m_halfclusterU.clear();
+  m_totE_HFClusV = 0;
+  m_totE_HFClusU = 0;
+  //for(int i=0; i<m_DataCol.map_HalfCluster["ESHalfClusterU"].size(); i++){
+  //  m_halfclusterU.push_back( m_DataCol.map_HalfCluster["ESHalfClusterU"][i]->getHalfClusterCol("MergedAxis")[0] );
+  //}
+  //for(int i=0; i<m_DataCol.map_HalfCluster["ESHalfClusterV"].size(); i++){
+  //  m_halfclusterV.push_back( m_DataCol.map_HalfCluster["ESHalfClusterV"][i]->getHalfClusterCol("MergedAxis")[0] );
+  //}
+  for(int i=0; i<m_DataCol.map_HalfCluster["HalfClusterColU"].size(); i++){
+    m_halfclusterU.push_back( m_DataCol.map_HalfCluster["HalfClusterColU"][i].get() );
+  }
+  for(int i=0; i<m_DataCol.map_HalfCluster["HalfClusterColV"].size(); i++){
+    m_halfclusterV.push_back( m_DataCol.map_HalfCluster["HalfClusterColV"][i].get() );
+  }
+  for(int i=0; i<m_halfclusterV.size(); i++){
+    m_HalfClusterV_x.push_back(m_halfclusterV[i]->getPos().x());
+    m_HalfClusterV_y.push_back(m_halfclusterV[i]->getPos().y());
+    m_HalfClusterV_z.push_back(m_halfclusterV[i]->getPos().z());
+    m_HalfClusterV_E.push_back(m_halfclusterV[i]->getEnergy());
+    m_HalfClusterV_tag.push_back(i);
+    m_HalfClusterV_type.push_back(m_halfclusterV[i]->getType());
+    m_totE_HFClusV += m_halfclusterV[i]->getEnergy();    
+
+      // MC truth information of the HFCluster
+      auto truthMap = m_halfclusterV[i]->getLinkedMCP();
+      for(auto iter: truthMap){
+        m_HalfClusterV_truth_tag.push_back(i);
+        m_HalfClusterV_truthMC_px.push_back(iter.first.getMomentum().x);
+        m_HalfClusterV_truthMC_py.push_back(iter.first.getMomentum().y);
+        m_HalfClusterV_truthMC_pz.push_back(iter.first.getMomentum().z);
+        m_HalfClusterV_truthMC_E.push_back(iter.first.getEnergy());
+        m_HalfClusterV_truthMC_weight.push_back(iter.second);
+      }
+
+      // 1DClusters (hits)
+      for(int ilm=0; ilm<m_halfclusterV[i]->getCluster().size(); ilm++){ 
+        m_HalfClusterV_hit_tag.push_back(i);
+        m_HalfClusterV_hit_x.push_back( m_halfclusterV[i]->getCluster()[ilm]->getPos().x() );
+        m_HalfClusterV_hit_y.push_back( m_halfclusterV[i]->getCluster()[ilm]->getPos().y() );
+        m_HalfClusterV_hit_z.push_back( m_halfclusterV[i]->getCluster()[ilm]->getPos().z() );
+        m_HalfClusterV_hit_E.push_back( m_halfclusterV[i]->getCluster()[ilm]->getEnergy()  );
+      }
+  }
+  for(int i=0; i<m_halfclusterU.size(); i++){
+    m_HalfClusterU_x.push_back(m_halfclusterU[i]->getPos().x());
+    m_HalfClusterU_y.push_back(m_halfclusterU[i]->getPos().y());
+    m_HalfClusterU_z.push_back(m_halfclusterU[i]->getPos().z());
+    m_HalfClusterU_E.push_back(m_halfclusterU[i]->getEnergy());
+    m_HalfClusterU_tag.push_back(i);
+    m_HalfClusterU_type.push_back(m_halfclusterU[i]->getType());
+    m_totE_HFClusU += m_halfclusterU[i]->getEnergy();
+
+      // MC truth information of the HFCluster
+      auto truthMap = m_halfclusterU[i]->getLinkedMCP();
+      for(auto iter: truthMap){
+        m_HalfClusterU_truth_tag.push_back(i);
+        m_HalfClusterU_truthMC_px.push_back(iter.first.getMomentum().x);
+        m_HalfClusterU_truthMC_py.push_back(iter.first.getMomentum().y);
+        m_HalfClusterU_truthMC_pz.push_back(iter.first.getMomentum().z);
+        m_HalfClusterU_truthMC_E.push_back(iter.first.getEnergy());
+        m_HalfClusterU_truthMC_weight.push_back(iter.second);
+      }
+
+      // 1DClusters (hits)
+      for(int ilm=0; ilm<m_halfclusterU[i]->getCluster().size(); ilm++){
+        m_HalfClusterU_hit_tag.push_back(i);
+        m_HalfClusterU_hit_x.push_back( m_halfclusterU[i]->getCluster()[ilm]->getPos().x() );
+        m_HalfClusterU_hit_y.push_back( m_halfclusterU[i]->getCluster()[ilm]->getPos().y() );
+        m_HalfClusterU_hit_z.push_back( m_halfclusterU[i]->getCluster()[ilm]->getPos().z() );
+        m_HalfClusterU_hit_E.push_back( m_halfclusterU[i]->getCluster()[ilm]->getEnergy()  );
+      }
+  }
+  t_HalfCluster->Fill();
+
+
+  //Tower
+  ClearTower();
+  std::vector<std::shared_ptr<PandoraPlus::Calo3DCluster>> m_tower = m_DataCol.map_CaloCluster["ESTower"];
+  for(int it=0; it<m_tower.size(); it++){
+    ClearTower();
+    towerID[0] = m_tower[it]->getTowerID()[0][0];
+    towerID[1] = m_tower[it]->getTowerID()[0][1];
+    towerID[2] = m_tower[it]->getTowerID()[0][2];
+
+    std::vector<const CaloHalfCluster*> m_HFClusU = m_tower[it]->getHalfClusterUCol("ESHalfClusterU");
+    std::vector<const CaloHalfCluster*> m_HFClusV = m_tower[it]->getHalfClusterVCol("ESHalfClusterV");
+
+    m_NclusU = m_HFClusU.size();
+    m_NclusV = m_HFClusV.size();
+    m_totEn = m_tower[it]->getEnergy();
+    m_totEn_U = 0.;
+    m_totEn_V = 0.;
+    for(int ic=0; ic<m_NclusU; ic++){
+      m_HalfClusterU_tag.push_back(it);
+      m_HalfClusterU_x.push_back(m_HFClusU[ic]->getPos().x());
+      m_HalfClusterU_y.push_back(m_HFClusU[ic]->getPos().y());
+      m_HalfClusterU_z.push_back(m_HFClusU[ic]->getPos().z());
+      m_HalfClusterU_E.push_back(m_HFClusU[ic]->getEnergy());
+      m_HalfClusterU_type.push_back(m_HFClusU[ic]->getType());
+      m_HalfClusterU_nTrk.push_back(m_HFClusU[ic]->getAssociatedTracks().size());
+      m_totEn_U += m_HFClusU[ic]->getEnergy();
+    }
+
+    for(int ic=0; ic<m_NclusV; ic++){
+      m_HalfClusterV_tag.push_back(it);
+      m_HalfClusterV_x.push_back(m_HFClusV[ic]->getPos().x());
+      m_HalfClusterV_y.push_back(m_HFClusV[ic]->getPos().y());
+      m_HalfClusterV_z.push_back(m_HFClusV[ic]->getPos().z());
+      m_HalfClusterV_E.push_back(m_HFClusV[ic]->getEnergy());
+      m_HalfClusterV_type.push_back(m_HFClusV[ic]->getType());
+      m_HalfClusterV_nTrk.push_back(m_HFClusV[ic]->getAssociatedTracks().size());
+      m_totEn_V += m_HFClusV[ic]->getEnergy();
+    }
+
+    t_Tower->Fill();
+  }
+
+cout<<"  Write 3D cluster"<<endl;
+  //3D cluster
+  ClearCluster();
+  std::vector<std::shared_ptr<PandoraPlus::Calo3DCluster>> m_EcalClusterCol = m_DataCol.map_CaloCluster["TrkMergedECAL"];
+  std::vector<std::shared_ptr<PandoraPlus::Calo3DCluster>> m_HcalClusterCol = m_DataCol.map_CaloCluster["HCALCluster"];
+  std::vector<std::shared_ptr<PandoraPlus::Calo3DCluster>> m_SimpleHcalClusterCol = m_DataCol.map_CaloCluster["SimpleHCALCluster"];
+  m_totE_Ecal = 0.;
+  m_totE_Hcal = 0.;
+  m_Nclus_Ecal = m_EcalClusterCol.size();
+  m_Nclus_Hcal = m_HcalClusterCol.size();
+  for(int icl=0; icl<m_EcalClusterCol.size(); icl++){
+    m_EcalClus_x.push_back(m_EcalClusterCol[icl]->getShowerCenter().x());
+    m_EcalClus_y.push_back(m_EcalClusterCol[icl]->getShowerCenter().y());
+    m_EcalClus_z.push_back(m_EcalClusterCol[icl]->getShowerCenter().z());
+    m_EcalClus_E.push_back(m_EcalClusterCol[icl]->getLongiE());
+    m_EcalClus_nTrk.push_back(m_EcalClusterCol[icl]->getAssociatedTracks().size());
+
+    double tmp_phi = std::atan2(m_EcalClusterCol[icl]->getShowerCenter().y(), m_EcalClusterCol[icl]->getShowerCenter().x())* 180.0 / M_PI;
+    if (tmp_phi < 0) tmp_phi += 360.0;
+    double tmp_theta = std::atan2(m_EcalClusterCol[icl]->getShowerCenter().z(), m_EcalClusterCol[icl]->getShowerCenter().Perp())* 180.0 / M_PI + 90; 
+    //cout<<" Theta: "<<tmp_theta<<" Phi: "<<tmp_phi<<endl;
+    m_EcalClus_Escale.push_back(m_energycorsvc->energyCorrection(m_EcalClusterCol[icl]->getLongiE(), tmp_phi, tmp_theta));
+
+
+    if(m_EcalClusterCol[icl]->getAssociatedTracks().size()==1){
+      const Track* trk = m_EcalClusterCol[icl]->getAssociatedTracks()[0];
+      m_EcalClus_pTrk.push_back(trk->getMomentum());
+
+      std::vector<TrackState> AllTrackStates = trk->getAllTrackStates();
+      for(int istate=0; istate<AllTrackStates.size(); istate++){
+        m_EcalClus_trk_tag.push_back(icl);
+        m_EcalClus_trk_d0.push_back(AllTrackStates[istate].D0);
+        m_EcalClus_trk_z0.push_back(AllTrackStates[istate].Z0);
+        m_EcalClus_trk_phi.push_back(AllTrackStates[istate].phi0);
+        m_EcalClus_trk_tanL.push_back( AllTrackStates[istate].tanLambda );
+        m_EcalClus_trk_kappa.push_back( AllTrackStates[istate].Kappa);
+        m_EcalClus_trk_omega.push_back( AllTrackStates[istate].Omega );
+        m_EcalClus_trk_location.push_back( AllTrackStates[istate].location );
+      }
+
+    }
+    else
+      m_EcalClus_pTrk.push_back(-99);
+
+    m_EcalClus_typeU.push_back(m_EcalClusterCol[icl]->getHalfClusterUCol("LinkedLongiCluster")[0]->getType());
+    m_EcalClus_typeV.push_back(m_EcalClusterCol[icl]->getHalfClusterVCol("LinkedLongiCluster")[0]->getType());
+    for(int ii=0; ii<m_EcalClusterCol[icl]->getHalfClusterUCol("LinkedLongiCluster").size(); ii++){
+      for(int ihit=0; ihit<m_EcalClusterCol[icl]->getHalfClusterUCol("LinkedLongiCluster")[ii]->getBars().size(); ihit++){
+        auto shower = m_EcalClusterCol[icl]->getHalfClusterUCol("LinkedLongiCluster")[ii]->getBars()[ihit];
+        m_EcalClus_hitU_tag.push_back(icl);
+        m_EcalClus_hitU_x.push_back(shower->getPosition().x());
+        m_EcalClus_hitU_y.push_back(shower->getPosition().y());
+        m_EcalClus_hitU_z.push_back(shower->getPosition().z());
+        m_EcalClus_hitU_E.push_back(shower->getEnergy());
+      }
+    }
+    for(int ii=0; ii<m_EcalClusterCol[icl]->getHalfClusterVCol("LinkedLongiCluster").size(); ii++){
+      for(int ihit=0; ihit<m_EcalClusterCol[icl]->getHalfClusterVCol("LinkedLongiCluster")[ii]->getBars().size(); ihit++){
+        auto shower = m_EcalClusterCol[icl]->getHalfClusterVCol("LinkedLongiCluster")[ii]->getBars()[ihit];
+        m_EcalClus_hitV_tag.push_back(icl);
+        m_EcalClus_hitV_x.push_back(shower->getPosition().x());
+        m_EcalClus_hitV_y.push_back(shower->getPosition().y());
+        m_EcalClus_hitV_z.push_back(shower->getPosition().z());
+        m_EcalClus_hitV_E.push_back(shower->getEnergy());
+      }
+    }
+
+    m_totE_Ecal += m_EcalClusterCol[icl]->getLongiE();
+    auto truthMap = m_EcalClusterCol[icl]->getLinkedMCP();
+    for(auto iter: truthMap){
+      m_EcalClus_truthMC_tag.push_back(icl);
+      m_EcalClus_truthMC_pid.push_back(iter.first.getPDG() );
+      m_EcalClus_truthMC_px.push_back(iter.first.getMomentum().x);
+      m_EcalClus_truthMC_py.push_back(iter.first.getMomentum().y);
+      m_EcalClus_truthMC_pz.push_back(iter.first.getMomentum().z);
+      m_EcalClus_truthMC_E.push_back(iter.first.getEnergy());
+      m_EcalClus_truthMC_EPx.push_back(iter.first.getEndpoint().x);
+      m_EcalClus_truthMC_EPy.push_back(iter.first.getEndpoint().y);
+      m_EcalClus_truthMC_EPz.push_back(iter.first.getEndpoint().z);
+      m_EcalClus_truthMC_weight.push_back(iter.second);
+    }
+  }
+
+  for(int icl=0; icl<m_HcalClusterCol.size(); icl++){
+    m_HcalClus_x.push_back(m_HcalClusterCol[icl]->getHitCenter().x());
+    m_HcalClus_y.push_back(m_HcalClusterCol[icl]->getHitCenter().y());
+    m_HcalClus_z.push_back(m_HcalClusterCol[icl]->getHitCenter().z());
+    m_HcalClus_E.push_back(m_HcalClusterCol[icl]->getHitsE());
+    m_HcalClus_nTrk.push_back(m_HcalClusterCol[icl]->getAssociatedTracks().size());
+    if(m_HcalClusterCol[icl]->getAssociatedTracks().size()==1)
+      m_HcalClus_pTrk.push_back(m_HcalClusterCol[icl]->getAssociatedTracks()[0]->getMomentum());
+    else
+      m_HcalClus_pTrk.push_back(-99);
+
+    for(int ih=0; ih<m_HcalClusterCol[icl]->getCaloHits().size(); ih++){
+      m_HcalClus_hit_tag.push_back(icl);
+      m_HcalClus_hit_x.push_back(m_HcalClusterCol[icl]->getCaloHits()[ih]->getPosition().x());
+      m_HcalClus_hit_y.push_back(m_HcalClusterCol[icl]->getCaloHits()[ih]->getPosition().y());
+      m_HcalClus_hit_z.push_back(m_HcalClusterCol[icl]->getCaloHits()[ih]->getPosition().z());
+      m_HcalClus_hit_E.push_back(m_HcalClusterCol[icl]->getCaloHits()[ih]->getEnergy());
+    }
+
+    m_totE_Hcal += m_HcalClusterCol[icl]->getHitsE();
+    auto truthMap = m_HcalClusterCol[icl]->getLinkedMCP();
+    for(auto iter: truthMap){
+      m_HcalClus_truthMC_tag.push_back(icl);
+      m_HcalClus_truthMC_pid.push_back(iter.first.getPDG() );
+      m_HcalClus_truthMC_px.push_back(iter.first.getMomentum().x);
+      m_HcalClus_truthMC_py.push_back(iter.first.getMomentum().y);
+      m_HcalClus_truthMC_pz.push_back(iter.first.getMomentum().z);
+      m_HcalClus_truthMC_E.push_back(iter.first.getEnergy());
+      m_HcalClus_truthMC_EPx.push_back(iter.first.getEndpoint().x);
+      m_HcalClus_truthMC_EPy.push_back(iter.first.getEndpoint().y);
+      m_HcalClus_truthMC_EPz.push_back(iter.first.getEndpoint().z);
+      m_HcalClus_truthMC_weight.push_back(iter.second);
+    }
+  }
+
+  for(int icl=0; icl<m_SimpleHcalClusterCol.size(); icl++){
+    m_SimpleHcalClus_x.push_back(m_SimpleHcalClusterCol[icl]->getHitCenter().x());
+    m_SimpleHcalClus_y.push_back(m_SimpleHcalClusterCol[icl]->getHitCenter().y());
+    m_SimpleHcalClus_z.push_back(m_SimpleHcalClusterCol[icl]->getHitCenter().z());
+    m_SimpleHcalClus_E.push_back(m_SimpleHcalClusterCol[icl]->getHitsE());
+    m_SimpleHcalClus_nTrk.push_back(m_SimpleHcalClusterCol[icl]->getAssociatedTracks().size());
+    if(m_SimpleHcalClusterCol[icl]->getAssociatedTracks().size()==1)
+      m_SimpleHcalClus_pTrk.push_back(m_SimpleHcalClusterCol[icl]->getAssociatedTracks()[0]->getMomentum());
+    else
+      m_SimpleHcalClus_pTrk.push_back(-99);
+
+    for(int ih=0; ih<m_SimpleHcalClusterCol[icl]->getCaloHits().size(); ih++){
+      m_SimpleHcalClus_hit_tag.push_back(icl);
+      m_SimpleHcalClus_hit_x.push_back(m_SimpleHcalClusterCol[icl]->getCaloHits()[ih]->getPosition().x());
+      m_SimpleHcalClus_hit_y.push_back(m_SimpleHcalClusterCol[icl]->getCaloHits()[ih]->getPosition().y());
+      m_SimpleHcalClus_hit_z.push_back(m_SimpleHcalClusterCol[icl]->getCaloHits()[ih]->getPosition().z());
+      m_SimpleHcalClus_hit_E.push_back(m_SimpleHcalClusterCol[icl]->getCaloHits()[ih]->getEnergy());
+    }
+
+    auto truthMap = m_SimpleHcalClusterCol[icl]->getLinkedMCP();
+    for(auto iter: truthMap){
+      m_SimpleHcalClus_truthMC_tag.push_back(icl);
+      m_SimpleHcalClus_truthMC_pid.push_back(iter.first.getPDG() );
+      m_SimpleHcalClus_truthMC_px.push_back(iter.first.getMomentum().x);
+      m_SimpleHcalClus_truthMC_py.push_back(iter.first.getMomentum().y);
+      m_SimpleHcalClus_truthMC_pz.push_back(iter.first.getMomentum().z);
+      m_SimpleHcalClus_truthMC_E.push_back(iter.first.getEnergy());
+      m_SimpleHcalClus_truthMC_EPx.push_back(iter.first.getEndpoint().x);
+      m_SimpleHcalClus_truthMC_EPy.push_back(iter.first.getEndpoint().y);
+      m_SimpleHcalClus_truthMC_EPz.push_back(iter.first.getEndpoint().z);
+      m_SimpleHcalClus_truthMC_weight.push_back(iter.second);
+    }
+  }
+  t_Cluster->Fill();
+
+  // Save Track info
+  ClearTrack();
+  std::vector<PandoraPlus::Track*> m_trkCol; 
+  for(int it=0; it<m_DataCol.TrackCol.size(); it++)
+    m_trkCol.push_back( m_DataCol.TrackCol[it].get() );
+
+  m_Ntrk = m_trkCol.size();
+  for(int itrk=0; itrk<m_Ntrk; itrk++){
+    m_type.push_back(m_trkCol[itrk]->getType());
+    std::vector<TrackState> AllTrackStates = m_trkCol[itrk]->getAllTrackStates();
+    for(int istate=0; istate<AllTrackStates.size(); istate++){
+      m_trkstate_d0.push_back( AllTrackStates[istate].D0 );
+      m_trkstate_z0.push_back( AllTrackStates[istate].Z0 );
+      m_trkstate_phi.push_back( AllTrackStates[istate].phi0 );
+      m_trkstate_tanL.push_back( AllTrackStates[istate].tanLambda );
+      m_trkstate_kappa.push_back( AllTrackStates[istate].Kappa);
+      m_trkstate_omega.push_back( AllTrackStates[istate].Omega );
+      m_trkstate_refx.push_back( AllTrackStates[istate].referencePoint.X() );
+      m_trkstate_refy.push_back( AllTrackStates[istate].referencePoint.Y() );
+      m_trkstate_refz.push_back( AllTrackStates[istate].referencePoint.Z() );
+      m_trkstate_location.push_back( AllTrackStates[istate].location );
+      m_trkstate_tag.push_back(itrk);
+  }}
+  t_Track->Fill();
+
+  // yyy: pfo
+  ClearPFO();
+  std::vector<PandoraPlus::PFObject*> m_pfobjects; m_pfobjects.clear();
+  std::vector<PandoraPlus::PFObject*> m_chargedPFOs, m_neutralPFOs;
+  for(int ip=0; ip<m_DataCol.map_PFObjects["outputPFO"].size(); ip++){
+    m_pfobjects.push_back(m_DataCol.map_PFObjects["outputPFO"][ip].get());
+    if(m_DataCol.map_PFObjects["outputPFO"][ip]->getTracks().size()!=0) m_chargedPFOs.push_back( m_DataCol.map_PFObjects["outputPFO"][ip].get() );
+    else m_neutralPFOs.push_back( m_DataCol.map_PFObjects["outputPFO"][ip].get() );
+  }
+
+ double totE_Ecal = 0;
+ double totE_Hcal = 0;
+ cout<<"After merge all virtual to Ch: charged "<<m_chargedPFOs.size()<<", neutral "<<m_neutralPFOs.size()<<", total "<<m_pfobjects.size()<<endl;
+ for(int i=0; i<m_neutralPFOs.size(); i++){
+   cout<<"    PFO #"<<i<<": track size "<<m_neutralPFOs[i]->getTracks().size()<<", leading P "<<m_neutralPFOs[i]->getTrackMomentum();
+   cout<<", ECAL cluster size "<<m_neutralPFOs[i]->getECALClusters().size()<<", totE "<<m_neutralPFOs[i]->getECALClusterEnergy();
+   cout<<", HCAL cluster size "<<m_neutralPFOs[i]->getHCALClusters().size()<<", totE "<<m_neutralPFOs[i]->getHCALClusterEnergy()<<endl;
+   totE_Ecal += m_neutralPFOs[i]->getECALClusterEnergy();
+   totE_Hcal += m_neutralPFOs[i]->getHCALClusterEnergy();
+ }
+ cout<<"-----Neutral cluster Ecal total energy: "<<totE_Ecal<<", Hcal total energy: "<<totE_Hcal<<endl;
+ totE_Ecal = 0;
+ totE_Hcal = 0;
+ for(int i=0; i<m_chargedPFOs.size(); i++){
+   cout<<"    PFO #"<<i<<": track size "<<m_chargedPFOs[i]->getTracks().size()<<", leading P "<<m_chargedPFOs[i]->getTrackMomentum();
+   cout<<", ECAL cluster size "<<m_chargedPFOs[i]->getECALClusters().size()<<", totE "<<m_chargedPFOs[i]->getECALClusterEnergy();
+   cout<<", HCAL cluster size "<<m_chargedPFOs[i]->getHCALClusters().size()<<", totE "<<m_chargedPFOs[i]->getHCALClusterEnergy()<<endl;
+   totE_Ecal += m_chargedPFOs[i]->getECALClusterEnergy();
+   totE_Hcal += m_chargedPFOs[i]->getHCALClusterEnergy();
+ }
+ cout<<"-----Charged cluster Ecal total energy: "<<totE_Ecal<<", Hcal total energy: "<<totE_Hcal<<endl;
+
+
+
+  for(int ip=0; ip<m_pfobjects.size(); ip++){
+    std::vector<const Track*> t_tracks = m_pfobjects[ip]->getTracks();
+    std::vector<const Calo3DCluster*> t_ecal_clusters = m_pfobjects[ip]->getECALClusters();
+    std::vector<const Calo3DCluster*> t_hcal_clusters =  m_pfobjects[ip]->getHCALClusters();
+
+    pfo_tag.push_back(ip);
+    pfo_n_track.push_back(t_tracks.size());
+    pfo_n_ecal_clus.push_back(t_ecal_clusters.size());
+    pfo_n_hcal_clus.push_back(t_hcal_clusters.size());
+
+    for(int it=0; it<t_tracks.size(); it++){
+      std::vector<TrackState> AllTrackStates = t_tracks[it]->getAllTrackStates();
+      for(int istate=0; istate<AllTrackStates.size(); istate++){
+        pfo_trk_tag.push_back(ip);
+        pfo_trk_d0.push_back( AllTrackStates[istate].D0 );
+        pfo_trk_z0.push_back( AllTrackStates[istate].Z0 );
+        pfo_trk_phi.push_back( AllTrackStates[istate].phi0 );
+        pfo_trk_tanL.push_back( AllTrackStates[istate].tanLambda );
+        pfo_trk_kappa.push_back( AllTrackStates[istate].Kappa);
+        pfo_trk_omega.push_back( AllTrackStates[istate].Omega );
+        pfo_trk_location.push_back( AllTrackStates[istate].location );
+      }
+    }
+
+    for(int ie=0; ie<t_ecal_clusters.size(); ie++){
+      pfo_ecal_tag.push_back(ip);
+      pfo_ecal_clus_x.push_back(t_ecal_clusters[ie]->getShowerCenter().x());
+      pfo_ecal_clus_y.push_back(t_ecal_clusters[ie]->getShowerCenter().y());
+      pfo_ecal_clus_z.push_back(t_ecal_clusters[ie]->getShowerCenter().z());
+      pfo_ecal_clus_E.push_back(t_ecal_clusters[ie]->getLongiE());
+
+      double tmp_phi = std::atan2(t_ecal_clusters[ie]->getShowerCenter().y(), t_ecal_clusters[ie]->getShowerCenter().x())* 180.0 / M_PI;
+      if (tmp_phi < 0) tmp_phi += 360.0;
+      double tmp_theta = std::atan2(t_ecal_clusters[ie]->getShowerCenter().z(), t_ecal_clusters[ie]->getShowerCenter().Perp())* 180.0 / M_PI + 90; 
+      pfo_ecal_clus_Escale.push_back(m_energycorsvc->energyCorrection(t_ecal_clusters[ie]->getLongiE(), tmp_phi, tmp_theta));
+
+    }
+    for(int ih=0; ih<t_hcal_clusters.size(); ih++){
+      pfo_hcal_tag.push_back(ip);
+      pfo_hcal_clus_x.push_back(t_hcal_clusters[ih]->getHitCenter().x());
+      pfo_hcal_clus_y.push_back(t_hcal_clusters[ih]->getHitCenter().y());
+      pfo_hcal_clus_z.push_back(t_hcal_clusters[ih]->getHitCenter().z());
+      pfo_hcal_clus_E.push_back(t_hcal_clusters[ih]->getHitsE());
+    }
+  }
+  t_PFO->Fill();
+
+  //Clean Events
+  //system("/cefs/higgs/songwz/winter22/CEPCSW/workarea/memory/memory_test.sh before_clean");
+  m_DataCol.Clear();
+  //system("/cefs/higgs/songwz/winter22/CEPCSW/workarea/memory/memory_test.sh event_end");
+
+
+  std::cout<<"Event: "<<_nEvt<<" is done"<<std::endl;
+  _nEvt ++ ;
+  return StatusCode::SUCCESS;
+}
+
+StatusCode PandoraPlusPFAlg::finalize()
+{
+  m_wfile->cd();
+  t_MCParticle->Write();
+  t_SimBar->Write();
+  t_LocalMax->Write();
+  t_Layers->Write();
+  t_Hough->Write();
+  t_Cone->Write();
+  t_TrackAxis->Write();
+  t_Axis->Write();
+  t_HalfCluster->Write();
+  t_Tower->Write();
+  t_Cluster->Write();
+  t_Track->Write();
+  t_PFO->Write();
+  m_wfile->Close();
+  delete m_wfile, t_MCParticle, t_SimBar, t_LocalMax, t_Layers, t_Hough, t_Cone, t_TrackAxis, t_Axis, t_HalfCluster, t_Tower, t_Cluster, t_Track, t_PFO; 
+
+  delete m_pMCParticleCreator;
+  delete m_pTrackCreator; 
+  delete m_pCaloHitsCreator;
+  delete m_pOutputCreator;
+
+  delete r_MCParticleCol;
+  for(auto iter : r_TrackCols) delete iter; 
+  //for(auto iter : r_ECalHitCols) delete iter; 
+  //for(auto iter : r_HCalHitCols) delete iter; 
+  for(auto iter : r_CaloHitCols) delete iter; 
+  for(auto iter : map_CaloMCPAssoCols) delete iter.second;
+  r_TrackCols.clear();
+  for(auto iter : w_ClusterCollection) delete iter.second;
+  w_ClusterCollection.clear(); 
+  //r_ECalHitCols.clear();
+  //r_HCalHitCols.clear();
+  r_CaloHitCols.clear();
+  //m_energycorsvc->finalize();
+
+  info() << "Processed " << _nEvt << " events " << endmsg;
+  return GaudiAlgorithm::finalize();
+}
+
+void PandoraPlusPFAlg::ClearMCParticle(){
+  m_mcPdgid.clear();
+  m_mcStatus.clear();
+  m_mcPx.clear();
+  m_mcPy.clear();
+  m_mcPz.clear();
+  m_mcEn.clear();
+  m_mcMass.clear();
+  m_mcCharge.clear();
+  m_mcEPx.clear();
+  m_mcEPy.clear();
+  m_mcEPz.clear();
+  m_depEn_ecal.clear();
+  m_depEn_hcal.clear();
+}
+
+void PandoraPlusPFAlg::ClearBar(){
+  m_totE_EcalSim = -99;
+  m_totE_HcalSim = -99;
+  m_simBar_x.clear();
+  m_simBar_y.clear();
+  m_simBar_z.clear();
+  m_simBar_T1.clear();
+  m_simBar_T2.clear();
+  m_simBar_Q1.clear();
+  m_simBar_Q2.clear();
+  m_simBar_module.clear();
+  m_simBar_dlayer.clear();
+  m_simBar_stave.clear();
+  m_simBar_slayer.clear();
+  m_simBar_bar.clear();
+  m_simBar_truthMC_tag.clear(); 
+  m_simBar_truthMC_pid.clear();
+  m_simBar_truthMC_px.clear();
+  m_simBar_truthMC_py.clear();
+  m_simBar_truthMC_pz.clear();
+  m_simBar_truthMC_E.clear();
+  m_simBar_truthMC_EPx.clear();
+  m_simBar_truthMC_EPy.clear();
+  m_simBar_truthMC_EPz.clear();
+  m_simBar_truthMC_weight.clear();
+
+  m_HcalHit_x.clear();
+  m_HcalHit_y.clear();
+  m_HcalHit_z.clear();
+  m_HcalHit_E.clear();
+  m_HcalHit_layer.clear();
+  m_HcalHit_truthMC_tag.clear();
+  m_HcalHit_truthMC_pid.clear();
+  m_HcalHit_truthMC_px.clear();
+  m_HcalHit_truthMC_py.clear();
+  m_HcalHit_truthMC_pz.clear();
+  m_HcalHit_truthMC_E.clear();
+  m_HcalHit_truthMC_EPx.clear();
+  m_HcalHit_truthMC_EPy.clear();
+  m_HcalHit_truthMC_EPz.clear();
+  m_HcalHit_truthMC_weight.clear();
+}
+
+void PandoraPlusPFAlg::ClearLocalMax(){
+  m_NlmU=-99;
+  m_NlmV=-99;
+  m_localMaxU_tag.clear();
+  m_localMaxU_x.clear();
+  m_localMaxU_y.clear();
+  m_localMaxU_z.clear();
+  m_localMaxU_E.clear();
+  m_localMaxU_mc_tag.clear();
+  m_localMaxU_mc_pdg.clear();
+  m_localMaxU_mc_px.clear();
+  m_localMaxU_mc_py.clear();
+  m_localMaxU_mc_pz.clear();
+  m_localMaxU_mc_weight.clear();
+  m_localMaxV_tag.clear();
+  m_localMaxV_x.clear();
+  m_localMaxV_y.clear();
+  m_localMaxV_z.clear();
+  m_localMaxV_E.clear();
+  m_localMaxV_mc_tag.clear();
+  m_localMaxV_mc_pdg.clear();
+  m_localMaxV_mc_px.clear();
+  m_localMaxV_mc_py.clear();
+  m_localMaxV_mc_pz.clear();
+  m_localMaxV_mc_weight.clear();
+}
+
+void PandoraPlusPFAlg::ClearLayer(){
+  m_NshowerU=-99;
+  m_NshowerV=-99;
+  m_barShowerU_tag.clear();
+  m_barShowerU_x.clear();
+  m_barShowerU_y.clear();
+  m_barShowerU_z.clear();
+  m_barShowerU_E.clear();
+  m_barShowerU_mc_tag.clear();
+  m_barShowerU_mc_pdg.clear();
+  m_barShowerU_mc_px.clear();
+  m_barShowerU_mc_py.clear();
+  m_barShowerU_mc_pz.clear();
+  m_barShowerU_mc_weight.clear();
+  m_barShowerV_tag.clear();
+  m_barShowerV_x.clear();
+  m_barShowerV_y.clear();
+  m_barShowerV_z.clear();
+  m_barShowerV_E.clear();
+  m_barShowerV_mc_tag.clear();
+  m_barShowerV_mc_pdg.clear();
+  m_barShowerV_mc_px.clear();
+  m_barShowerV_mc_py.clear();
+  m_barShowerV_mc_pz.clear();
+  m_barShowerV_mc_weight.clear();
+}
+
+
+void PandoraPlusPFAlg::ClearHough(){
+  m_houghU_tag.clear();
+  m_houghU_type.clear();
+  m_houghU_x.clear();
+  m_houghU_y.clear();
+  m_houghU_z.clear();
+  m_houghU_E.clear();
+  m_houghU_truth_tag.clear();
+  m_houghU_truth_MC_px.clear();
+  m_houghU_truth_MC_py.clear();
+  m_houghU_truth_MC_pz.clear();
+  m_houghU_truth_MC_E.clear();
+  m_houghU_truth_MC_weight.clear();
+  m_houghU_hit_tag.clear();
+  m_houghU_hit_x.clear();
+  m_houghU_hit_y.clear();
+  m_houghU_hit_z.clear();
+  m_houghU_hit_E.clear();
+  m_houghV_tag.clear();
+  m_houghV_type.clear();
+  m_houghV_x.clear();
+  m_houghV_y.clear();
+  m_houghV_z.clear();
+  m_houghV_E.clear();
+  m_houghV_alpha.clear();
+  m_houghV_rho.clear();
+  m_houghV_truth_tag.clear();
+  m_houghV_truth_MC_px.clear();
+  m_houghV_truth_MC_py.clear();
+  m_houghV_truth_MC_pz.clear();
+  m_houghV_truth_MC_E.clear();
+  m_houghV_truth_MC_weight.clear();
+  m_houghV_hit_tag.clear();
+  m_houghV_hit_x.clear();
+  m_houghV_hit_y.clear();
+  m_houghV_hit_z.clear();
+  m_houghV_hit_E.clear();
+}
+
+
+void PandoraPlusPFAlg::ClearCone(){
+  m_coneU_tag.clear();
+  m_coneU_type.clear();
+  m_coneU_x.clear();
+  m_coneU_y.clear();
+  m_coneU_z.clear();
+  m_coneU_E.clear();
+  m_coneU_truth_tag.clear();
+  m_coneU_truth_MC_px.clear();
+  m_coneU_truth_MC_py.clear();
+  m_coneU_truth_MC_pz.clear();
+  m_coneU_truth_MC_E.clear();
+  m_coneU_truth_MC_weight.clear();
+  m_coneU_hit_tag.clear();
+  m_coneU_hit_x.clear();
+  m_coneU_hit_y.clear();
+  m_coneU_hit_z.clear();
+  m_coneU_hit_E.clear();
+  m_coneV_tag.clear();
+  m_coneV_type.clear();
+  m_coneV_x.clear();
+  m_coneV_y.clear();
+  m_coneV_z.clear();
+  m_coneV_E.clear();
+  m_coneV_truth_tag.clear();
+  m_coneV_truth_MC_px.clear();
+  m_coneV_truth_MC_py.clear();
+  m_coneV_truth_MC_pz.clear();
+  m_coneV_truth_MC_E.clear();
+  m_coneV_truth_MC_weight.clear();
+  m_coneV_hit_tag.clear();
+  m_coneV_hit_x.clear();
+  m_coneV_hit_y.clear();
+  m_coneV_hit_z.clear();
+  m_coneV_hit_E.clear();
+}
+
+
+void PandoraPlusPFAlg::ClearTrackAxis(){
+  m_trackU_tag.clear();
+  m_trackU_type.clear();
+  m_trackU_x.clear();
+  m_trackU_y.clear();
+  m_trackU_z.clear();
+  m_trackU_E.clear();
+  m_trackU_truth_tag.clear();
+  m_trackU_truth_MC_px.clear();
+  m_trackU_truth_MC_py.clear();
+  m_trackU_truth_MC_pz.clear();
+  m_trackU_truth_MC_E.clear();
+  m_trackU_truth_MC_weight.clear();
+  m_trackU_hit_tag.clear();
+  m_trackU_hit_x.clear();
+  m_trackU_hit_y.clear();
+  m_trackU_hit_z.clear();
+  m_trackU_hit_E.clear();
+  m_trackV_tag.clear();
+  m_trackV_type.clear();
+  m_trackV_x.clear();
+  m_trackV_y.clear();
+  m_trackV_z.clear();
+  m_trackV_E.clear();
+  m_trackV_truth_tag.clear();
+  m_trackV_truth_MC_px.clear();
+  m_trackV_truth_MC_py.clear();
+  m_trackV_truth_MC_pz.clear();
+  m_trackV_truth_MC_E.clear();
+  m_trackV_truth_MC_weight.clear();
+  m_trackV_hit_tag.clear();
+  m_trackV_hit_x.clear();
+  m_trackV_hit_y.clear();
+  m_trackV_hit_z.clear();
+  m_trackV_hit_E.clear();
+}
+
+
+void PandoraPlusPFAlg::ClearAxis(){
+  m_axisU_tag.clear();
+  m_axisU_type.clear();
+  m_axisU_x.clear();
+  m_axisU_y.clear();
+  m_axisU_z.clear();
+  m_axisU_E.clear();
+  m_axisU_truth_tag.clear();
+  m_axisU_truth_MC_px.clear();
+  m_axisU_truth_MC_py.clear();
+  m_axisU_truth_MC_pz.clear();
+  m_axisU_truth_MC_E.clear();
+  m_axisU_truth_MC_weight.clear();
+  m_axisU_hit_tag.clear();
+  m_axisU_hit_x.clear();
+  m_axisU_hit_y.clear();
+  m_axisU_hit_z.clear();
+  m_axisU_hit_E.clear();
+  m_axisV_tag.clear();
+  m_axisV_type.clear();
+  m_axisV_x.clear();
+  m_axisV_y.clear();
+  m_axisV_z.clear();
+  m_axisV_E.clear();
+  m_axisV_truth_tag.clear();
+  m_axisV_truth_MC_px.clear();
+  m_axisV_truth_MC_py.clear();
+  m_axisV_truth_MC_pz.clear();
+  m_axisV_truth_MC_E.clear();
+  m_axisV_truth_MC_weight.clear();
+  m_axisV_hit_tag.clear();
+  m_axisV_hit_x.clear();
+  m_axisV_hit_y.clear();
+  m_axisV_hit_z.clear();
+  m_axisV_hit_E.clear();
+
+  m_emptyAxisU_tag.clear();
+  m_emptyAxisU_x.clear();
+  m_emptyAxisU_y.clear();
+  m_emptyAxisU_z.clear();
+  m_emptyAxisU_E.clear();
+  m_emptyAxisV_tag.clear();
+  m_emptyAxisV_x.clear();
+  m_emptyAxisV_y.clear();
+  m_emptyAxisV_z.clear();
+  m_emptyAxisV_E.clear();
+}
+
+void PandoraPlusPFAlg::ClearHalfCluster(){
+  m_totE_HFClusU = -99.;
+  m_totE_HFClusV = -99.;
+  m_HalfClusterV_x.clear();
+  m_HalfClusterV_y.clear();
+  m_HalfClusterV_z.clear();
+  m_HalfClusterV_E.clear();
+  m_HalfClusterV_tag.clear();
+  m_HalfClusterV_type.clear();
+  m_HalfClusterV_nTrk.clear();
+  m_HalfClusterV_hit_x.clear();
+  m_HalfClusterV_hit_y.clear();
+  m_HalfClusterV_hit_z.clear();
+  m_HalfClusterV_hit_E.clear();
+  m_HalfClusterV_hit_tag.clear();
+  m_HalfClusterV_truth_tag.clear();
+  m_HalfClusterV_truthMC_px.clear();
+  m_HalfClusterV_truthMC_py.clear();
+  m_HalfClusterV_truthMC_pz.clear();
+  m_HalfClusterV_truthMC_E.clear();
+  m_HalfClusterV_truthMC_weight.clear();
+  m_HalfClusterU_x.clear();
+  m_HalfClusterU_y.clear();
+  m_HalfClusterU_z.clear();
+  m_HalfClusterU_E.clear();
+  m_HalfClusterU_tag.clear();
+  m_HalfClusterU_type.clear();
+  m_HalfClusterU_nTrk.clear();
+  m_HalfClusterU_hit_x.clear();
+  m_HalfClusterU_hit_y.clear();
+  m_HalfClusterU_hit_z.clear();
+  m_HalfClusterU_hit_E.clear();
+  m_HalfClusterU_hit_tag.clear();
+  m_HalfClusterU_truth_tag.clear();
+  m_HalfClusterU_truthMC_px.clear();
+  m_HalfClusterU_truthMC_py.clear();
+  m_HalfClusterU_truthMC_pz.clear();
+  m_HalfClusterU_truthMC_E.clear();
+  m_HalfClusterU_truthMC_weight.clear();
+}
+
+void PandoraPlusPFAlg::ClearTower(){
+  towerID[0] = 0;
+  towerID[1] = 0;
+  towerID[2] = 0;
+  m_NclusU = -99;
+  m_NclusV = -99;
+  m_totEn = -99;
+  m_totEn_U = -99.;
+  m_totEn_V = -99.;
+  m_HalfClusterV_x.clear();
+  m_HalfClusterV_y.clear();
+  m_HalfClusterV_z.clear();
+  m_HalfClusterV_E.clear();
+  m_HalfClusterV_tag.clear();
+  m_HalfClusterV_type.clear();
+  m_HalfClusterV_nTrk.clear();
+  m_HalfClusterU_x.clear();
+  m_HalfClusterU_y.clear();
+  m_HalfClusterU_z.clear();
+  m_HalfClusterU_E.clear();
+  m_HalfClusterU_tag.clear();
+  m_HalfClusterU_type.clear();
+  m_HalfClusterU_nTrk.clear();
+
+}
+
+void PandoraPlusPFAlg::ClearCluster(){
+  m_totE_Ecal = -99.;
+  m_totE_Hcal = -99.;
+  m_Nclus_Ecal = -99;
+  m_Nclus_Hcal = -99;
+  m_EcalClus_x.clear();
+  m_EcalClus_y.clear();
+  m_EcalClus_z.clear();
+  m_EcalClus_E.clear();
+  m_EcalClus_Escale.clear();
+  m_EcalClus_nTrk.clear();
+  m_EcalClus_pTrk.clear();
+  m_EcalClus_typeU.clear();
+  m_EcalClus_typeV.clear();
+  m_EcalClus_hitU_x.clear();
+  m_EcalClus_hitU_y.clear();
+  m_EcalClus_hitU_z.clear();
+  m_EcalClus_hitU_E.clear();
+  m_EcalClus_hitU_tag.clear();
+  m_EcalClus_hitV_x.clear();
+  m_EcalClus_hitV_y.clear();
+  m_EcalClus_hitV_z.clear();
+  m_EcalClus_hitV_E.clear();
+  m_EcalClus_hitV_tag.clear();
+  m_EcalClus_trk_location.clear();
+  m_EcalClus_trk_tag.clear();
+  m_EcalClus_trk_d0.clear();
+  m_EcalClus_trk_z0.clear();
+  m_EcalClus_trk_phi.clear();
+  m_EcalClus_trk_tanL.clear();
+  m_EcalClus_trk_kappa.clear();
+  m_EcalClus_trk_omega.clear();
+  m_EcalClus_truthMC_tag.clear();
+  m_EcalClus_truthMC_pid.clear();
+  m_EcalClus_truthMC_px.clear();
+  m_EcalClus_truthMC_py.clear();
+  m_EcalClus_truthMC_pz.clear();
+  m_EcalClus_truthMC_E.clear();
+  m_EcalClus_truthMC_EPx.clear();
+  m_EcalClus_truthMC_EPy.clear();
+  m_EcalClus_truthMC_EPz.clear();
+  m_EcalClus_truthMC_weight.clear();
+  m_HcalClus_x.clear();
+  m_HcalClus_y.clear();
+  m_HcalClus_z.clear();
+  m_HcalClus_E.clear();
+  m_HcalClus_nTrk.clear();
+  m_HcalClus_pTrk.clear();
+  m_HcalClus_hit_x.clear();
+  m_HcalClus_hit_y.clear();
+  m_HcalClus_hit_z.clear();
+  m_HcalClus_hit_E.clear();
+  m_HcalClus_hit_tag.clear();
+  m_HcalClus_truthMC_tag.clear();
+  m_HcalClus_truthMC_pid.clear();
+  m_HcalClus_truthMC_px.clear();
+  m_HcalClus_truthMC_py.clear();
+  m_HcalClus_truthMC_pz.clear();
+  m_HcalClus_truthMC_E.clear();
+  m_HcalClus_truthMC_EPx.clear();
+  m_HcalClus_truthMC_EPy.clear();
+  m_HcalClus_truthMC_EPz.clear();
+  m_HcalClus_truthMC_weight.clear();
+  m_SimpleHcalClus_x.clear();
+  m_SimpleHcalClus_y.clear();
+  m_SimpleHcalClus_z.clear();
+  m_SimpleHcalClus_E.clear();
+  m_SimpleHcalClus_nTrk.clear();
+  m_SimpleHcalClus_pTrk.clear();
+  m_SimpleHcalClus_hit_x.clear();
+  m_SimpleHcalClus_hit_y.clear();
+  m_SimpleHcalClus_hit_z.clear();
+  m_SimpleHcalClus_hit_E.clear();
+  m_SimpleHcalClus_hit_tag.clear();
+  m_SimpleHcalClus_truthMC_tag.clear();
+  m_SimpleHcalClus_truthMC_pid.clear();
+  m_SimpleHcalClus_truthMC_px.clear();
+  m_SimpleHcalClus_truthMC_py.clear();
+  m_SimpleHcalClus_truthMC_pz.clear();
+  m_SimpleHcalClus_truthMC_E.clear();
+  m_SimpleHcalClus_truthMC_EPx.clear();
+  m_SimpleHcalClus_truthMC_EPy.clear();
+  m_SimpleHcalClus_truthMC_EPz.clear();
+  m_SimpleHcalClus_truthMC_weight.clear();
+}
+
+void PandoraPlusPFAlg::ClearTrack(){
+  m_type.clear();
+  m_trkstate_d0.clear();
+  m_trkstate_z0.clear();
+  m_trkstate_phi.clear();
+  m_trkstate_tanL.clear();
+  m_trkstate_kappa.clear();
+  m_trkstate_omega.clear();
+  m_trkstate_refx.clear();
+  m_trkstate_refy.clear();
+  m_trkstate_refz.clear();
+  m_trkstate_location.clear();
+  m_trkstate_tag.clear();
+}
+
+void PandoraPlusPFAlg::ClearPFO(){
+  pfo_tag.clear();
+  pfo_n_track.clear();
+  pfo_n_ecal_clus.clear();
+  pfo_n_hcal_clus.clear();
+  pfo_trk_tag.clear();
+  pfo_trk_d0.clear();
+  pfo_trk_z0.clear();
+  pfo_trk_phi.clear();
+  pfo_trk_tanL.clear();
+  pfo_trk_kappa.clear();
+  pfo_trk_omega.clear();
+  pfo_trk_location.clear();
+  pfo_ecal_tag.clear();
+  pfo_ecal_clus_x.clear();
+  pfo_ecal_clus_y.clear();
+  pfo_ecal_clus_z.clear();
+  pfo_ecal_clus_E.clear();
+  pfo_ecal_clus_Escale.clear();
+  pfo_hcal_tag.clear();
+  pfo_hcal_clus_x.clear();
+  pfo_hcal_clus_y.clear();
+  pfo_hcal_clus_z.clear();
+  pfo_hcal_clus_E.clear();
+}
+
+
+double PandoraPlusPFAlg::GetParticleDepEnergy(edm4hep::MCParticle& mcp, std::vector<std::shared_ptr<PandoraPlus::CaloUnit>>& barcol){
+
+  double EnDep = 0.;
+  for(int i=0; i<barcol.size(); i++){
+    std::vector< std::pair<edm4hep::MCParticle, float> > mcp_map = barcol[i]->getLinkedMCP();
+    for(int ip=0; ip<mcp_map.size(); ip++){
+      if(mcp_map[ip].first==mcp){
+        EnDep += barcol[i]->getEnergy() * mcp_map[ip].second;
+      }
+    }
+  }
+  return EnDep;
+
+}
+
+double PandoraPlusPFAlg::GetParticleDepEnergy(edm4hep::MCParticle& mcp, std::vector<std::shared_ptr<PandoraPlus::CaloHit>>& hitcol){
+
+  double EnDep = 0.;
+  for(int i=0; i<hitcol.size(); i++){
+    std::vector< std::pair<edm4hep::MCParticle, float> > mcp_map = hitcol[i]->getLinkedMCP();
+    for(int ip=0; ip<mcp_map.size(); ip++){
+      if(mcp_map[ip].first==mcp){
+        EnDep += hitcol[i]->getEnergy() * mcp_map[ip].second;
+      }
+    }
+  }
+  return EnDep;
+
+}
+
+#endif
diff --git a/Reconstruction/CrystalCaloRec/src/Tools/CaloHitsCreator.cpp b/Reconstruction/CrystalCaloRec/src/Tools/CaloHitsCreator.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..873d2f6a98d16eb49077a0e4fa5102891d88b810
--- /dev/null
+++ b/Reconstruction/CrystalCaloRec/src/Tools/CaloHitsCreator.cpp
@@ -0,0 +1,128 @@
+#ifndef TRACK_CREATOR_C
+#define TRACK_CREATOR_C
+
+#include "Tools/CaloHitsCreator.h"
+
+namespace PandoraPlus{
+  CaloHitsCreator::CaloHitsCreator(const Settings& m_settings) : settings( m_settings ){
+
+  }; 
+
+  StatusCode CaloHitsCreator::CreateCaloHits( PandoraPlusDataCol& m_DataCol, 
+                                              std::vector<DataHandle<edm4hep::CalorimeterHitCollection>*>& r_CaloHitCols, 
+                                              std::map<std::string, dd4hep::DDSegmentation::BitFieldCoder*>& map_decoder,
+                                              std::map<std::string, DataHandle<edm4hep::MCRecoCaloParticleAssociationCollection>*>& map_CaloParticleAssoCol )
+  {
+    if(r_CaloHitCols.size()==0 || settings.map_stringVecPars.at("CaloHitCollections").size()==0) StatusCode::SUCCESS;
+
+    //Save readin collections
+    m_DataCol.collectionMap_CaloHit.clear(); 
+    for(unsigned int icol=0; icol<r_CaloHitCols.size(); icol++){
+
+      const edm4hep::CalorimeterHitCollection* const_CaloHitCol = r_CaloHitCols[icol]->get(); 
+
+      std::vector<edm4hep::CalorimeterHit> m_HitCol; m_HitCol.clear(); 
+      for(unsigned int ihit=0; ihit<const_CaloHitCol->size(); ihit++){
+        edm4hep::CalorimeterHit m_hit = const_CaloHitCol->at(ihit);
+        m_HitCol.push_back(m_hit);
+      }
+
+      m_DataCol.collectionMap_CaloHit[settings.map_stringVecPars.at("CaloHitCollections")[icol]] = m_HitCol; 
+    }
+
+    //Convert to local objects: 
+    for(auto iter : m_DataCol.collectionMap_CaloHit){
+      if( settings.map_stringPars.at("EcalType")=="BarEcal" && iter.first == "ECALBarrel") continue; 
+      
+      std::vector<std::shared_ptr<PandoraPlus::CaloHit>> m_hitCol; m_hitCol.clear();
+      const edm4hep::MCRecoCaloParticleAssociationCollection* const_MCPCaloAssoCol;
+      if( map_CaloParticleAssoCol.find(iter.first)!=map_CaloParticleAssoCol.end()) 
+        const_MCPCaloAssoCol = map_CaloParticleAssoCol[iter.first]->get();
+
+      for(int ihit=0; ihit<iter.second.size(); ihit++){
+        //PandoraPlus::CaloHit* m_hit = new PandoraPlus::CaloHit();
+        std::shared_ptr<PandoraPlus::CaloHit> m_hit = std::make_shared<PandoraPlus::CaloHit>();
+
+        m_hit->setOriginHit( iter.second[ihit] );
+        m_hit->setcellID( iter.second[ihit].getCellID() );
+        m_hit->setLayer( map_decoder[iter.first]->get(iter.second[ihit].getCellID(), "layer") );
+
+        TVector3 pos( iter.second[ihit].getPosition().x, iter.second[ihit].getPosition().y, iter.second[ihit].getPosition().z );
+        m_hit->setPosition( pos );
+        m_hit->setEnergy( iter.second[ihit].getEnergy() );
+
+        for(int ilink=0; ilink<const_MCPCaloAssoCol->size(); ilink++){
+          if( iter.second[ihit] == const_MCPCaloAssoCol->at(ilink).getRec() ) m_hit->addLinkedMCP( std::make_pair(const_MCPCaloAssoCol->at(ilink).getSim(), const_MCPCaloAssoCol->at(ilink).getWeight()) );
+        }
+        m_hitCol.push_back( m_hit );
+      }
+      m_DataCol.map_CaloHit[iter.first] = m_hitCol;
+      const_MCPCaloAssoCol = nullptr;
+    }
+
+
+    //Convert to local objects: CalorimeterHit to CaloUnit (For ECALBarrel only)
+    if(settings.map_stringPars.at("EcalType")=="BarEcal"){
+      std::vector<std::shared_ptr<PandoraPlus::CaloUnit>> m_barCol; m_barCol.clear(); 
+
+      const edm4hep::MCRecoCaloParticleAssociationCollection* const_MCPCaloAssoCol = map_CaloParticleAssoCol["ECALBarrel"]->get();   
+      auto CaloHits = m_DataCol.collectionMap_CaloHit["ECALBarrel"]; 
+      std::map<std::uint64_t, std::vector<PandoraPlus::CaloUnit> > map_cellID_hits; map_cellID_hits.clear();
+      for(auto& hit : CaloHits){ 
+        PandoraPlus::CaloUnit m_bar; 
+        m_bar.setcellID(hit.getCellID());
+        m_bar.setPosition( TVector3(hit.getPosition().x, hit.getPosition().y, hit.getPosition().z) );
+        m_bar.setQ(hit.getEnergy()/2., hit.getEnergy()/2.);
+        m_bar.setT(hit.getTime(), hit.getTime());
+        for(int ilink=0; ilink<const_MCPCaloAssoCol->size(); ilink++){
+          if( hit == const_MCPCaloAssoCol->at(ilink).getRec() ) m_bar.addLinkedMCP( std::make_pair(const_MCPCaloAssoCol->at(ilink).getSim(), const_MCPCaloAssoCol->at(ilink).getWeight()) );
+        } 
+
+
+        map_cellID_hits[hit.getCellID()].push_back(m_bar);
+      }
+      for(auto& hit : map_cellID_hits){
+        if(hit.second.size()!=2){ std::cout<<"WARNING: didn't find correct hit pairs! "<<std::endl; continue; }
+   
+        //PandoraPlus::CaloUnit* m_bar = new PandoraPlus::CaloUnit(); 
+        std::shared_ptr<PandoraPlus::CaloUnit> m_bar = std::make_shared<PandoraPlus::CaloUnit>();
+   
+        unsigned long long id = hit.first; 
+        m_bar->setcellID( id );
+        m_bar->setcellID( map_decoder["ECALBarrel"]->get(id, "system"),
+                          map_decoder["ECALBarrel"]->get(id, "module"),
+                          map_decoder["ECALBarrel"]->get(id, "stave"),
+                          map_decoder["ECALBarrel"]->get(id, "dlayer"),
+                          map_decoder["ECALBarrel"]->get(id, "slayer"),
+                          map_decoder["ECALBarrel"]->get(id, "bar"));
+        m_bar->setPosition(hit.second[0].getPosition());
+        m_bar->setQ( hit.second[0].getEnergy(), hit.second[1].getEnergy() );
+        m_bar->setT( hit.second[0].getT1(), hit.second[1].getT1() );
+
+        //---oooOOO000OOOooo---set bar length---oooOOO000OOOooo---
+        //TODO: reading bar length from geosvc. 
+        double t_bar_length;
+        if(m_bar->getSlayer()==1) t_bar_length = 375.133;
+        else{
+          if( m_bar->getModule()%2==0 ) t_bar_length = 295.905 + (m_bar->getDlayer()-1)* 6.13231;
+          else t_bar_length = 416.843 - (m_bar->getDlayer()+1)* 2.25221;
+        }
+        m_bar->setBarLength(t_bar_length);
+        //---oooOOO000OOOooo---set bar length---oooOOO000OOOooo---
+
+        //add MCParticle link
+        for(int ilink=0; ilink<hit.second[0].getLinkedMCP().size(); ilink++) m_bar->addLinkedMCP( hit.second[0].getLinkedMCP()[ilink] );
+
+        m_barCol.push_back(m_bar);  //Save for later use in algorithms
+      }
+
+   
+      m_DataCol.map_BarCol["BarCol"] = m_barCol; 
+      const_MCPCaloAssoCol = nullptr;
+    }
+    return StatusCode::SUCCESS;
+  };
+
+};
+
+#endif
diff --git a/Reconstruction/CrystalCaloRec/src/Tools/MCParticleCreator.cpp b/Reconstruction/CrystalCaloRec/src/Tools/MCParticleCreator.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..561dafc474b847979d20748bf719db220efe2f2c
--- /dev/null
+++ b/Reconstruction/CrystalCaloRec/src/Tools/MCParticleCreator.cpp
@@ -0,0 +1,30 @@
+#ifndef MCPARTICLE_CREATOR_C
+#define MCPARTICLE_CREATOR_C
+
+#include "Tools/MCParticleCreator.h"
+
+namespace PandoraPlus{
+  MCParticleCreator::MCParticleCreator( const Settings& m_settings ): settings(m_settings){
+
+  }
+
+
+  StatusCode MCParticleCreator::CreateMCParticle( PandoraPlusDataCol& m_DataCol, DataHandle<edm4hep::MCParticleCollection>& r_MCParticleCol ){
+    if(settings.map_stringPars.at("MCParticleCollections").empty()) return StatusCode::SUCCESS;
+    m_DataCol.collectionMap_MC.clear();
+
+    const edm4hep::MCParticleCollection* const_MCPCol = r_MCParticleCol.get();
+
+    std::vector<edm4hep::MCParticle> m_MCPvec; m_MCPvec.clear(); 
+    for(int imc=0; imc<const_MCPCol->size(); imc++){
+      edm4hep::MCParticle m_MCp = const_MCPCol->at(imc);
+      m_MCPvec.push_back(m_MCp);
+    }
+
+    m_DataCol.collectionMap_MC[ settings.map_stringPars.at("MCParticleCollections") ] = m_MCPvec; 
+
+    return StatusCode::SUCCESS;
+  };
+
+}
+#endif
diff --git a/Reconstruction/CrystalCaloRec/src/Tools/OutputCreator.cpp b/Reconstruction/CrystalCaloRec/src/Tools/OutputCreator.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..8e95bb3f7d76defa57d2f85368076622d65fef56
--- /dev/null
+++ b/Reconstruction/CrystalCaloRec/src/Tools/OutputCreator.cpp
@@ -0,0 +1,233 @@
+#ifndef OUTPUT_CREATOR_C
+#define OUTPUT_CREATOR_C
+
+#include "Tools/OutputCreator.h"
+
+namespace PandoraPlus{
+
+  OutputCreator::OutputCreator( const Settings& m_settings ): settings(m_settings){
+
+  }
+
+
+  StatusCode OutputCreator::CreateOutputCollections( PandoraPlusDataCol& m_DataCol,
+                                                     DataHandle<edm4hep::CalorimeterHitCollection>& m_outRecHitsHandler,
+                                                     DataHandle<edm4hep::CalorimeterHitCollection>& m_outRecCoreHandler,
+                                                     DataHandle<edm4hep::CalorimeterHitCollection>& m_outRecHcalHitsHandler,
+                                                     DataHandle<edm4hep::TrackCollection>& m_outTrkHandler,
+                                                     std::map<std::string, DataHandle<edm4hep::ClusterCollection>*>& m_outClusterColHandler,
+                                                     DataHandle<edm4hep::ReconstructedParticleCollection>& m_recPFOHandler ){
+
+    //Register the collections
+    edm4hep::CalorimeterHitCollection* m_calohitCol = m_outRecHitsHandler.createAndPut();
+    edm4hep::CalorimeterHitCollection* m_corehitCol = m_outRecCoreHandler.createAndPut();
+    edm4hep::CalorimeterHitCollection* m_hcalhitCol = m_outRecHcalHitsHandler.createAndPut();
+    edm4hep::ReconstructedParticleCollection* m_pfocol = m_recPFOHandler.createAndPut();
+    edm4hep::ClusterCollection* m_Ecalcluster = m_outClusterColHandler["EcalCluster"]->createAndPut();
+    edm4hep::ClusterCollection* m_Ecalcore    = m_outClusterColHandler["EcalCore"]->createAndPut();
+    edm4hep::ClusterCollection* m_Hcalcluster = m_outClusterColHandler["HcalCluster"]->createAndPut();
+
+    edm4hep::TrackCollection* m_trkCol = nullptr;
+    if(settings.map_boolPars.at("UseTruthTrk")) m_trkCol = m_outTrkHandler.createAndPut();
+
+
+    //PFO
+    std::vector<std::shared_ptr<PandoraPlus::PFObject>> p_pfos = m_DataCol.map_PFObjects[settings.map_stringPars.at("OutputPFO")];
+std::cout<<"  Input PFO size: "<<p_pfos.size()<<std::endl;    
+    for(int ip=0; ip<p_pfos.size(); ip++){
+      auto m_pfo = m_pfocol->create();
+      std::vector<const Track*> vec_trks = p_pfos[ip]->getTracks();
+      std::vector<const Calo3DCluster*> vec_Ecalclus = p_pfos[ip]->getECALClusters();
+      std::vector<const Calo3DCluster*> vec_Hcalclus = p_pfos[ip]->getHCALClusters();
+
+      TVector3 vec_Pos(0.,0.,0.);
+
+      //Write ECAL cluster
+      double EcalClusE = 0;
+      for(int ic=0; ic<vec_Ecalclus.size(); ic++){
+        auto p_clus = vec_Ecalclus[ic];
+        auto m_clus = m_Ecalcluster->create();
+
+        //create high granularity ECAL hits
+        for(int ih=0; ih<p_clus->getCaloHits().size(); ih++){
+          const PandoraPlus::CaloHit* p_hit = p_clus->getCaloHits()[ih];
+          edm4hep::Vector3f pos(p_hit->getPosition().x(), p_hit->getPosition().y(), p_hit->getPosition().z());
+          int _module = p_hit->getModule();
+          int _layer = p_hit->getLayer();
+          int _cellID = (_module<<5) + _layer;  //A self-defined cell-ID. 
+   
+          auto _hit = m_calohitCol->create();
+          _hit.setCellID(_cellID);
+          _hit.setEnergy( p_hit->getEnergy()*settings.map_floatPars.at("ECALCalib") );
+          _hit.setPosition( pos );
+          _hit.setType(1); //Ecal barrel
+          m_clus.addToHits(_hit);
+        }
+   
+        //create cluster core from 2D cluster
+        auto m_core = m_Ecalcore->create();
+        double totE = 0.;
+        for(int ih=0; ih<p_clus->getCluster().size(); ih++){
+          const PandoraPlus::Calo2DCluster* p_core = p_clus->getCluster()[ih];
+          edm4hep::Vector3f pos(p_core->getPos().x(), p_core->getPos().y(), p_core->getPos().z());
+          int _module = p_core->getTowerID()[0][0];
+          int _layer = p_core->getDlayer();
+          int _cellID = (_module<<5) + _layer;
+   
+          auto _hit = m_corehitCol->create();
+          _hit.setCellID(_cellID);
+          _hit.setEnergy( p_core->getEnergy()*settings.map_floatPars.at("ECALCalib") );
+          _hit.setPosition( pos );
+          _hit.setType(1); //Ecal barrel
+          m_core.addToHits(_hit);
+          totE += p_core->getEnergy()*settings.map_floatPars.at("ECALCalib");
+        }
+   
+        double tmp_clusE = p_clus->getEnergy()*settings.map_floatPars.at("ECALCalib");
+        TVector3 clus_pos = p_clus->getShowerCenter();
+        edm4hep::Vector3f pos( clus_pos.x(), clus_pos.y(), clus_pos.z() );
+        double tmp_phi = std::atan2(clus_pos.y(), clus_pos.x())* 180.0 / M_PI; //TODO: use TVector3 to calculate
+        if (tmp_phi < 0) tmp_phi += 360.0;
+        double tmp_theta = std::atan2(clus_pos.z(), clus_pos.Perp())* 180.0 / M_PI + 90;
+        tmp_clusE = m_DataCol.EnergyCorrSvc->energyCorrection(tmp_clusE, tmp_phi, tmp_theta);
+        totE = m_DataCol.EnergyCorrSvc->energyCorrection(totE, tmp_phi, tmp_theta);
+
+        m_core.setEnergy(totE);
+        m_core.setPosition( pos );
+        m_clus.setEnergy( tmp_clusE );
+        m_clus.setPosition( pos );
+        m_clus.addToClusters(m_core);
+        m_pfo.addToClusters( m_clus );
+
+        EcalClusE += tmp_clusE;
+        vec_Pos += tmp_clusE*p_clus->getShowerCenter();
+      }
+
+
+      //Write HCAL cluster
+      double HcalClusE = 0;
+      for(int ic=0; ic<vec_Hcalclus.size(); ic++){
+        auto p_clus = vec_Hcalclus[ic];
+        auto m_clus = m_Hcalcluster->create();
+   
+        for(int ih=0; ih<p_clus->getCaloHits().size(); ih++){
+          const PandoraPlus::CaloHit* p_hit = p_clus->getCaloHits()[ih];
+          //auto _hit = m_hcalhitCol->create();
+          auto _hit = p_hit->getOriginHit().clone();
+          m_hcalhitCol->push_back(_hit);
+          m_clus.addToHits(_hit);
+        }
+   
+        double tmp_clusE = p_clus->getEnergy()*settings.map_floatPars.at("HCALCalib");
+        m_clus.setEnergy( tmp_clusE );
+        edm4hep::Vector3f pos( p_clus->getHitCenter().x(), p_clus->getHitCenter().y(), p_clus->getHitCenter().z() );
+        m_clus.setPosition( pos );
+        m_pfo.addToClusters( m_clus );
+
+        HcalClusE += tmp_clusE;
+        vec_Pos += tmp_clusE*p_clus->getHitCenter();
+      }
+
+
+      //Write Track
+      if(vec_trks.size()==0){
+        TVector3 p3vec = vec_Pos*(  (EcalClusE+HcalClusE)/vec_Pos.Mag() );
+        edm4hep::Vector3f p3(p3vec.x(), p3vec.y(), p3vec.z());
+
+        m_pfo.setEnergy( EcalClusE+HcalClusE );
+        m_pfo.setCharge( 0 );
+        m_pfo.setMomentum( p3 );
+      }
+      else{
+        double trk_maxP = -99;
+        int trkIndex = -1;
+        for(int itrk=0; itrk<vec_trks.size(); itrk++){
+          if( trk_maxP<vec_trks[itrk]->getMomentum() ){
+            trk_maxP = vec_trks[itrk]->getMomentum();
+            trkIndex = itrk;
+          }
+        }
+        if(trkIndex>=0){
+          auto m_trk = vec_trks[trkIndex]->getOriginTrack();
+          if( !m_trk.isAvailable() || settings.map_boolPars.at("UseTruthTrk") )
+            m_trk = TruthTrack( vec_trks[trkIndex]->getLeadingMCP(), m_trkCol );
+
+          m_pfo.addToTracks( m_trk );
+          m_pfo.setCharge( vec_trks[trkIndex]->getCharge() );
+
+          TVector3 p3vec = vec_trks[trkIndex]->getP3();
+          edm4hep::Vector3f p3(p3vec.x(), p3vec.y(), p3vec.z());
+          m_pfo.setMomentum(p3);
+          m_pfo.setEnergy( vec_trks[trkIndex]->getMomentum() );
+        }
+        else{
+          TVector3 p3vec = vec_Pos*(  (EcalClusE+HcalClusE)/vec_Pos.Mag() );
+          edm4hep::Vector3f p3(p3vec.x(), p3vec.y(), p3vec.z());
+
+          m_pfo.setEnergy( EcalClusE+HcalClusE );
+          m_pfo.setCharge( 0 );
+          m_pfo.setMomentum( p3 );
+        }
+
+      }
+
+    }
+
+std::cout<<"  Created PFO size: "<<m_pfocol->size()<<std::endl;
+
+    return StatusCode::SUCCESS;
+  }
+
+
+
+  edm4hep::Track OutputCreator::TruthTrack(edm4hep::MCParticle _mcp, edm4hep::TrackCollection* _trkCol ){
+    
+    auto m_track = _trkCol->create();
+    if( _mcp.getGeneratorStatus()!=1 || _mcp.getCharge()==0 ) return m_track;
+
+    edm4hep::Vector3f mcp_vertex(_mcp.getVertex().x, _mcp.getVertex().y, _mcp.getVertex().z);
+    edm4hep::Vector3f mcp_p(_mcp.getMomentum().x, _mcp.getMomentum().y, _mcp.getMomentum().z);
+    double mcp_pT = TMath::Sqrt(mcp_p.x*mcp_p.x + mcp_p.y*mcp_p.y);
+    double charge = _mcp.getCharge();
+
+    //Assign the track state at IP
+    edm4hep::TrackState m_trkst;
+    m_trkst.location = 1;  // At IP
+    m_trkst.D0 = 0;
+    m_trkst.Z0 = 0;
+    m_trkst.phi = TMath::ATan2(mcp_p.x, mcp_p.x);
+    m_trkst.tanLambda = mcp_p.z / mcp_pT;
+    m_trkst.omega = 0.3 * settings.map_floatPars.at("BField") / 1000. / mcp_pT;
+    m_trkst.referencePoint = mcp_vertex;
+    m_track.addToTrackStates(m_trkst);
+
+    ////Use helix to calculate the track endpoint
+    //HelixClassD * TrkInit_Helix = new HelixClassD();
+    //TrkInit_Helix->Initialize_Canonical(m_trkst.phi, m_trkst.D0, m_trkst.Z0, m_trkst.omega, m_trkst.tanLambda, settings.map_floatPars["BField"]);
+    //float intPoint[3];
+    //float refPoint[3] = {mcp_vertex.x, mcp_vertex.y, mcp_vertex.z};
+    //TrkInit_Helix->getPointOnCircle(PandoraPlus::CaloUnit::ecal_innerR, refPoint, intPoint);
+    ////If track reach to the edge of barrel
+    //if(fabs(intPoint[2])>ECALHalfZ ){
+    //  TrkInit_Helix->getPointInZ(ECALHalfZ, refPoint, intPoint);
+    //}
+
+    ////Assign the begin and end tracker hits
+    //auto m_hit_IP = m_trkHitCol->create();
+    //m_hit_IP.setPosition(edm4hep::Vector3d(0.,0.,0.));
+    //m_track.addToTrackerHits(m_hit_IP);
+
+    //auto m_hit_End = m_trkHitCol->create();
+    //m_hit_End.setPosition(edm4hep::Vector3d(intPoint[0], intPoint[1], intPoint[2]));
+    //m_track.addToTrackerHits(m_hit_End);
+
+    //delete TrkInit_Helix;
+    return m_track; 
+  }
+
+}
+
+
+
+
+#endif
diff --git a/Reconstruction/CrystalCaloRec/src/Tools/TrackCreator.cpp b/Reconstruction/CrystalCaloRec/src/Tools/TrackCreator.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..1beddc5bac76debe33f8cbf4016376a57571a7b7
--- /dev/null
+++ b/Reconstruction/CrystalCaloRec/src/Tools/TrackCreator.cpp
@@ -0,0 +1,154 @@
+#ifndef TRACK_CREATOR_C
+#define TRACK_CREATOR_C
+
+#include "Tools/TrackCreator.h"
+
+namespace PandoraPlus{
+
+  TrackCreator::TrackCreator(const Settings& m_settings) : settings( m_settings ){
+
+  };
+
+
+  StatusCode TrackCreator::CreateTracks( PandoraPlusDataCol& m_DataCol, 
+                                         std::vector<DataHandle<edm4hep::TrackCollection>*>& r_TrackCols, 
+                                         DataHandle<edm4hep::MCRecoTrackParticleAssociationCollection>* r_MCParticleTrkCol ){
+
+    if(r_TrackCols.size()==0 || settings.map_stringVecPars.at("trackCollections").size()==0) StatusCode::SUCCESS;
+
+    //Save readin collections
+    m_DataCol.collectionMap_Track.clear(); 
+    for(int icol=0; icol<r_TrackCols.size(); icol++){
+      const edm4hep::TrackCollection* const_TrkCol = r_TrackCols[icol]->get(); 
+
+      std::vector<edm4hep::Track> m_TrkCol; m_TrkCol.clear();
+      for(unsigned int icol=0; icol<const_TrkCol->size(); icol++){
+        edm4hep::Track m_trk = const_TrkCol->at(icol);
+        m_TrkCol.push_back(m_trk);
+      }
+
+      m_DataCol.collectionMap_Track[ settings.map_stringVecPars.at("trackCollections")[icol] ] = m_TrkCol; 
+    }
+
+
+    //Convert to local objects
+    std::vector<std::shared_ptr<PandoraPlus::Track>> m_trkCol; m_trkCol.clear();
+    const edm4hep::MCRecoTrackParticleAssociationCollection* const_MCPTrkAssoCol = r_MCParticleTrkCol->get();
+
+    for(auto iter : m_DataCol.collectionMap_Track){
+      auto const_TrkCol = iter.second; 
+      for(int itrk=0; itrk<const_TrkCol.size(); itrk++){
+        //PandoraPlus::Track* m_trk = new PandoraPlus::Track();
+        std::shared_ptr<PandoraPlus::Track> m_trk = std::make_shared<PandoraPlus::Track>();
+        std::vector<PandoraPlus::TrackState> m_trkstates;
+
+        for(int its=0; its<const_TrkCol[itrk].trackStates_size(); its++){
+          PandoraPlus::TrackState m_trkst;
+          m_trkst.D0 = const_TrkCol[itrk].getTrackStates(its).D0;
+          m_trkst.Z0 = const_TrkCol[itrk].getTrackStates(its).Z0;
+          m_trkst.phi0 = const_TrkCol[itrk].getTrackStates(its).phi;
+          m_trkst.tanLambda = const_TrkCol[itrk].getTrackStates(its).tanLambda;
+          m_trkst.Omega = const_TrkCol[itrk].getTrackStates(its).omega;
+          m_trkst.Kappa = m_trkst.Omega*1000./(0.3*settings.map_floatPars.at("BField"));   
+          m_trkst.location = const_TrkCol[itrk].getTrackStates(its).location;
+          m_trkst.referencePoint.SetXYZ( const_TrkCol[itrk].getTrackStates(its).referencePoint[0],
+                                         const_TrkCol[itrk].getTrackStates(its).referencePoint[1],
+                                         const_TrkCol[itrk].getTrackStates(its).referencePoint[2] );
+
+          m_trkstates.push_back(m_trkst);        
+        }
+        m_trk->setTrackStates("Input", m_trkstates);
+        m_trk->setType(const_TrkCol[itrk].getType());
+        m_trk->setOriginTrack( const_TrkCol[itrk] );
+
+        for(int ilink=0; ilink<const_MCPTrkAssoCol->size(); ilink++){
+          if( const_TrkCol[itrk] == const_MCPTrkAssoCol->at(ilink).getRec() ) {
+            m_trk->addLinkedMCP( std::make_pair(const_MCPTrkAssoCol->at(ilink).getSim(), const_MCPTrkAssoCol->at(ilink).getWeight()) );
+            break;
+          }
+        }
+
+        m_trkCol.push_back(m_trk);
+      }
+    }
+    m_DataCol.TrackCol = m_trkCol;
+
+
+    //Track extrapolation
+    //  Write settings: geometry description
+    //  m_TrkExtraSettings.map_floatPar["Nlayers"] = 28;
+
+    m_TrkExtraAlg = new TrackExtrapolatingAlg();
+    m_TrkExtraAlg->ReadSettings(m_TrkExtraSettings);
+    m_TrkExtraAlg->Initialize( m_DataCol );
+    m_TrkExtraAlg->RunAlgorithm( m_DataCol );
+    m_TrkExtraAlg->ClearAlgorithm();
+    delete m_TrkExtraAlg;
+    m_TrkExtraAlg = nullptr;
+
+    return StatusCode::SUCCESS;
+  }
+
+
+  StatusCode TrackCreator::CreateTracksFromMCParticle(PandoraPlusDataCol& m_DataCol, 
+                                          DataHandle<edm4hep::MCParticleCollection>& r_MCParticleCol){
+
+
+    // Convert MC charged particles to local Track objects.
+    // Assuming the tracks are ideal helixes.
+
+    // Get charged MC particles with generatorStatus==1
+    const edm4hep::MCParticleCollection* const_MCPCol = r_MCParticleCol.get();
+    std::vector<edm4hep::MCParticle> m_MCPvec; m_MCPvec.clear(); 
+    for(int i=0; i<const_MCPCol->size(); i++){
+      edm4hep::MCParticle m_MCp = const_MCPCol->at(i);
+      if(m_MCp.getGeneratorStatus()==1 && m_MCp.getCharge()!=0)
+        m_MCPvec.push_back(m_MCp);
+    }
+
+    // Convert to local Track objects
+    std::vector<std::shared_ptr<PandoraPlus::Track>> m_trkCol; m_trkCol.clear();
+    for(auto mcp : m_MCPvec){
+      std::shared_ptr<PandoraPlus::Track> m_trk = std::make_shared<PandoraPlus::Track>();
+      std::vector<PandoraPlus::TrackState> m_trkstates;
+
+      TVector3 mcp_vertex(mcp.getVertex().x, mcp.getVertex().y, mcp.getVertex().z);
+      TVector3 mcp_p(mcp.getMomentum().x, mcp.getMomentum().y, mcp.getMomentum().z);
+      double mcp_pT = TMath::Sqrt(mcp_p.X()*mcp_p.X() + mcp_p.Y()*mcp_p.Y());
+      double charge = mcp.getCharge();
+
+      // Evaluate track state at vertex
+      PandoraPlus::TrackState m_trkst;  
+      m_trkst.location = 1;  // At IP
+      m_trkst.D0 = 0;
+      m_trkst.Z0 = 0;
+      m_trkst.phi0 = TMath::ATan2(mcp_p.Y(), mcp_p.X());
+      m_trkst.Kappa = 1 / mcp_pT;
+      if(charge<0) m_trkst.Kappa = -m_trkst.Kappa;
+      m_trkst.tanLambda = mcp_p.Z() / mcp_pT;
+      m_trkst.Omega = 0.3 * settings.map_floatPars.at("BField") / 1000. / mcp_pT;
+      m_trkst.referencePoint = mcp_vertex;
+      m_trkstates.push_back(m_trkst);
+
+      m_trk->setTrackStates("Input", m_trkstates);
+      m_trk->setType(0);  // It is a "MC track" and not detected by any tracker system
+      m_trk->addLinkedMCP( std::make_pair(mcp, 1.) );
+      m_trkCol.push_back(m_trk);
+    }
+    m_DataCol.TrackCol = m_trkCol;
+
+    m_TrkExtraSettings.map_intPars["Input_track"] = 1;
+
+    m_TrkExtraAlg = new TrackExtrapolatingAlg();
+    m_TrkExtraAlg->ReadSettings(m_TrkExtraSettings);
+    m_TrkExtraAlg->Initialize( m_DataCol );
+    m_TrkExtraAlg->RunAlgorithm( m_DataCol );
+    m_TrkExtraAlg->ClearAlgorithm();
+    delete m_TrkExtraAlg;
+    m_TrkExtraAlg = nullptr;
+
+    return StatusCode::SUCCESS;
+  }
+
+};
+#endif
diff --git a/Reconstruction/CrystalCaloRec/src/Tools/TrackFitInEcal.cpp b/Reconstruction/CrystalCaloRec/src/Tools/TrackFitInEcal.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6bf5dff0a02c0822123c2d460c98d4b4d857edd6
--- /dev/null
+++ b/Reconstruction/CrystalCaloRec/src/Tools/TrackFitInEcal.cpp
@@ -0,0 +1,221 @@
+#include "Tools/TrackFitInEcal.h"
+
+#include <fstream>
+#include <iomanip>
+#include <cmath>
+
+#include "TMinuit.h"
+#include "TMath.h"
+#include "TGraphErrors.h"
+#include "TF1.h"
+#include "TVector3.h"
+
+using namespace std;
+
+vector<int> TrackFitInEcal::m_flagUZ;
+vector<double> TrackFitInEcal::m_uzPos;
+vector<double> TrackFitInEcal::m_uzPosErr;
+vector<double> TrackFitInEcal::m_depth;
+vector<double> TrackFitInEcal::m_depthErr;
+
+TrackFitInEcal::TrackFitInEcal(double barAngle){
+	 m_barAngle = barAngle;
+    m_IPvar[0]=0; m_IPvar[1]=0; 
+    m_FixIP = false; 
+}
+
+TrackFitInEcal::~TrackFitInEcal(){
+
+}
+
+void TrackFitInEcal::setPoint(int flagUZ, double uzPos, double uzPosErr, double depth, double depthErr){
+	 m_flagUZ.push_back(flagUZ);
+	 m_uzPos.push_back(uzPos);
+	 m_uzPosErr.push_back(uzPosErr);
+	 m_depth.push_back(depth);
+	 m_depthErr.push_back(depthErr);
+}
+
+void TrackFitInEcal::setGlobalPoint(int flagUZ, double x, double xerr, double y, double yerr, double z, double zerr){
+  TVector3 vecp(x, y, z);
+  TVector3 vecerr(xerr, yerr, zerr);
+  vecp.RotateZ(-1.*m_barAngle);
+  vecerr.RotateZ(-1.*m_barAngle);
+
+  if(flagUZ==0) setPoint(flagUZ, vecp.y(), vecerr.y(), vecp.x(), vecerr.x());  //U direction
+  else          setPoint(flagUZ, vecp.z(), vecerr.z(), vecp.x(), vecerr.x());  //Z direction
+}
+
+bool TrackFitInEcal::fitTrack(){
+	 int iXY = 0;
+	 int iWZ = 0;
+	 for(unsigned int i=0; i<m_flagUZ.size(); i++){
+		  int flag = m_flagUZ[i];
+		  if(0==flag) iXY++;
+		  else if(1==flag) iWZ++;
+	 }
+	 if((iXY<2) || (iWZ<2)){
+		  cout << "WARNING: too few hits for track fit" << endl;
+		  return false;
+	 }
+
+	 if( ! fit2D() ){
+		  cout << "ERROR in 2-D fit" << endl;
+		  clear();
+		  return false;
+	 }
+
+	 if( ! mnFit3D() ){
+	 	  clear();
+	 	  return false;
+	 }
+
+	 clear();
+	 return true;
+}
+
+bool TrackFitInEcal::fit2D(){
+	 int iPointXY = 0;
+    // cout<<"Fit IP: "<<m_FixIP<<'\t'<<m_IPvar[0]<<'\t'<<m_IPvar[1]<<endl;
+
+	 TGraphErrors* grXY = new TGraphErrors();
+	 for(unsigned int i=0; i<m_flagUZ.size(); i++){
+		  int flag = m_flagUZ[i];
+		  if(0 != flag) continue;
+		  grXY->SetPoint(iPointXY, m_depth[i], m_uzPos[i]);
+		  grXY->SetPointError(iPointXY, m_depthErr[i], m_uzPosErr[i]);
+		  iPointXY++;
+	 }
+   TF1 *func1 = new TF1("func1", "pol1", 1800, 2100);
+	 grXY->Fit("func1", "Q");
+	 double a0 = func1->GetParameter(0);
+	 double a1 = func1->GetParameter(1);
+    if(m_FixIP){
+      func1->FixParameter(0, m_IPvar[0]/cos(atan(a1))); //fix dr. 
+      grXY->Fit("func1", "Q");
+      a0 = func1->GetParameter(0);
+      a1 = func1->GetParameter(1);
+    }
+
+	 m_trkPar[2] = atan(a1); // phi
+	 m_trkPar[0] = a0 * cos(m_trkPar[2]); // dr
+	 double x0 = m_trkPar[0] * cos(m_trkPar[2] + TMath::PiOver2());
+	 // cout << "fit in x-y: " << setw(15) << a0 << setw(15) << a1 << setw(15) << m_trkPar[0] << setw(15) << m_trkPar[2] << endl;
+
+
+	 int iPointWZ = 0;
+	 TGraphErrors* grWZ = new TGraphErrors();
+	 for(unsigned int i=0; i<m_flagUZ.size(); i++){
+		  int flag = m_flagUZ[i];
+		  if(1 != flag) continue;
+
+		  double w = (m_depth[i] - x0)/cos(m_trkPar[2]);
+		  grWZ->SetPoint(iPointWZ, w, m_uzPos[i]);
+		  grWZ->SetPointError(iPointWZ, m_depthErr[i]/cos(m_trkPar[2]), m_uzPosErr[i]);
+		  // cout << "iPointWZ " << setw(5) << iPointWZ << setw(15) << w << setw(15) << m_uzPos[i] << endl;
+		  iPointWZ++;
+	 }
+   TF1 *func2 = new TF1("func2", "pol1", 1800, 2100);
+   if(m_FixIP) func2->FixParameter(0, m_IPvar[1]); //fix dz.
+	 grWZ->Fit("func2", "Q");
+	 a0 = func2->GetParameter(0);
+	 a1 = func2->GetParameter(1);
+
+	 double lamda = atan(a1);
+	 m_trkPar[3] = TMath::PiOver2() - lamda; // theta
+	 m_trkPar[1] = a0;						 // dz
+	 //cout << "fit in w-z: " << setw(15) << a0 << setw(15) << a1 << setw(15) << m_trkPar[1]
+	 // 	  << setw(15) << lamda << setw(15) << m_trkPar[3] << endl;
+
+	 delete grXY;
+	 delete grWZ;
+   delete func1;
+   delete func2;
+	 return true;
+}
+
+bool TrackFitInEcal::mnFit3D(){
+    Int_t ierflg;
+    Int_t istat;
+    Int_t nvpar;
+    Int_t nparx;
+    Double_t fmin;
+    Double_t edm;
+    Double_t errdef;
+    Double_t arglist[10];
+
+    TMinuit* mnTrk = new TMinuit(NTRKPAR);
+    mnTrk->SetPrintLevel(-1);
+    mnTrk->SetFCN(fcnTrk);
+    mnTrk->SetErrorDef(1.0);
+    mnTrk->mnparm(0, "dr", m_IPvar[0], 0.1, 0, 0, ierflg);
+    mnTrk->mnparm(1, "dz", m_IPvar[1], 0.1, 0, 0, ierflg);
+    mnTrk->mnparm(2, "phi", 0, 0.1, 0, 0, ierflg);
+    mnTrk->mnparm(3, "theta", 0, 0.1, 0, 0, ierflg);
+	  arglist[0] = 0;
+    if(m_FixIP){ mnTrk->FixParameter(0); mnTrk->FixParameter(1); }
+
+    mnTrk->mnexcm("SET NOW", arglist, 0, ierflg);
+    
+    for(int i=0; i<NTRKPAR; i++){
+       arglist[0] = i + 1;
+       arglist[1] = m_trkPar[i];
+       mnTrk->mnexcm("SET PARameter", arglist, 2, ierflg);
+    }
+    
+    arglist[0] = 1000;
+    arglist[1] = 0.1;
+    mnTrk->mnexcm("MIGRAD", arglist, 2, ierflg);
+    mnTrk->mnstat(fmin, edm, errdef, nvpar, nparx, istat);
+    
+    if( (0==ierflg) && (3==istat) ){
+      for(int i=0; i<5; i++)
+        mnTrk->GetParameter(i, m_trkPar[i], m_trkParErr[i]);
+           
+    
+      mnTrk->mnemat(*m_covariance, 4);
+      m_chisq = fmin;
+      delete mnTrk;
+      return true;
+    } 
+    else{
+//	 	  cout << "ERROR in fit with minuit! ";
+//      cout <<"ierflg = "<<ierflg<<" , stat = "<<istat<<endl;
+      delete mnTrk;
+      return false;
+    }
+}
+
+void TrackFitInEcal::clear(){
+	 m_flagUZ.clear();
+	 m_uzPos.clear();
+	 m_uzPosErr.clear();
+	 m_depth.clear();
+	 m_depthErr.clear();
+    m_IPvar[0]=0; m_IPvar[1]=0;
+    m_FixIP = false; 
+}
+
+void TrackFitInEcal::fcnTrk(Int_t &npar, Double_t *gin, Double_t &f, Double_t *par, Int_t iflag){
+     Double_t chisq = 0.0;
+
+	 double fi0 = par[2] + TMath::PiOver2();
+	 double x0 = par[0] * cos(fi0);
+	 double y0 = par[0] * sin(fi0);
+	 for(unsigned int i=0; i<m_flagUZ.size(); i++){
+		  if(0==m_flagUZ[i]){
+			   double uzPosFit = (m_depth[i]-x0)*tan(par[2]) + y0;
+			   Double_t deta = (uzPosFit - m_uzPos[i])/m_uzPosErr[i];
+			   chisq += deta*deta;
+			   // cout << "fcn x-y: " << setw(15) << uzPosFit << setw(15) << m_uzPos[i] << endl;
+		  } else if(1==m_flagUZ[i]){
+			   double lamda = TMath::PiOver2() - par[3];
+			   double uzPosFit = (m_depth[i]-x0)*tan(lamda)/cos(par[2]) + par[1];
+			   Double_t deta = (uzPosFit - m_uzPos[i])/m_uzPosErr[i];
+			   chisq += deta*deta;
+			   // cout << "fcn w-z: " << setw(15) << uzPosFit << setw(15) << m_uzPos[i] << endl;
+		  }
+	 }
+	 f = chisq;
+}
+
diff --git a/Service/CMakeLists.txt b/Service/CMakeLists.txt
index 14c795387ec29c5eaa2ad469db497d2048ff8ee6..8d47862c2d08465b84ebe957a621faf558f7a381 100644
--- a/Service/CMakeLists.txt
+++ b/Service/CMakeLists.txt
@@ -2,3 +2,4 @@ add_subdirectory(EventSeeder)
 add_subdirectory(GearSvc)
 add_subdirectory(TrackSystemSvc)
 add_subdirectory(SimplePIDSvc)
+add_subdirectory(CrystalEcalSvc)
diff --git a/Service/CrystalEcalSvc/CMakeLists.txt b/Service/CrystalEcalSvc/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..70966a293ae253a44d7ac8891465b2eb72b67c48
--- /dev/null
+++ b/Service/CrystalEcalSvc/CMakeLists.txt
@@ -0,0 +1,29 @@
+# gaudi_subdir(CrystalEcalSvc v0r0)
+
+gaudi_add_header_only_library(CrystalEcalSvcLib)
+
+gaudi_add_module(CrystalEcalSvc 
+                 SOURCES src/CrystalEcalEnergyCorrectionSvc.cpp
+                 LINK Gaudi::GaudiKernel
+                      CrystalEcalSvcLib
+                      #DataHelperLib
+                      #CRDEcalRecLib
+                      #GearSvc
+                      #Gaudi::GaudiKernel
+                      #${ROOT_LIBRARIES}
+                      #${CLHEP_LIBRARIES}
+                      #${GEAR_LIBRARIES}
+                      #${LCIO_LIBRARIES}
+                      #EDM4HEP::edm4hep EDM4HEP::edm4hepDict
+)
+
+#target_include_directories(CrystalEcalSvc PUBLIC
+#  $<BUILD_INTERFACE:${CMAKE_CURRENT_LIST_DIR}>/include
+#  $<INSTALL_INTERFACE:${CMAKE_INSTALL_INCLUDEDIR}>)
+
+install(TARGETS  CrystalEcalSvcLib CrystalEcalSvc
+  EXPORT CEPCSWTargets
+  RUNTIME DESTINATION "${CMAKE_INSTALL_BINDIR}" COMPONENT bin
+  LIBRARY DESTINATION "${CMAKE_INSTALL_LIBDIR}" COMPONENT shlib
+  COMPONENT dev)
+
diff --git a/Service/CrystalEcalSvc/include/CrystalEcalSvc/ICrystalEcalSvc.h b/Service/CrystalEcalSvc/include/CrystalEcalSvc/ICrystalEcalSvc.h
new file mode 100644
index 0000000000000000000000000000000000000000..9cf5f75f5ec65077c964bbd8b34cb6c4678fe0bb
--- /dev/null
+++ b/Service/CrystalEcalSvc/include/CrystalEcalSvc/ICrystalEcalSvc.h
@@ -0,0 +1,19 @@
+#ifndef IXTALECALSvc_h
+#define IXTALECALSvc_h
+
+#include "GaudiKernel/IService.h"
+
+class ICrystalEcalSvc: virtual public IInterface {
+public:
+    DeclareInterfaceID(ICrystalEcalSvc, 0, 1); // major/minor version
+    virtual ~ICrystalEcalSvc() = default;
+
+    virtual double energyCorrection(double energy, double phi, double theta) = 0;
+
+    // virtual void ClearSystem() = 0;
+
+private:
+
+};
+
+#endif
diff --git a/Service/CrystalEcalSvc/src/CrystalEcalEnergyCorrectionSvc.cpp b/Service/CrystalEcalSvc/src/CrystalEcalEnergyCorrectionSvc.cpp
new file mode 100755
index 0000000000000000000000000000000000000000..c7fa1e865bdac12a636f11e9516721fb4597ac1f
--- /dev/null
+++ b/Service/CrystalEcalSvc/src/CrystalEcalEnergyCorrectionSvc.cpp
@@ -0,0 +1,110 @@
+#ifndef XTALECALENERGYCORRECTIONSvc_C
+#define XTALECALENERGYCORRECTIONSvc_C
+
+#include "CrystalEcalEnergyCorrectionSvc.h"
+
+DECLARE_COMPONENT(CrystalEcalEnergyCorrectionSvc)
+
+double CrystalEcalEnergyCorrectionSvc::thetaCorrectionAngleStart[7] = {94.5, 105, 115, 123.5, 130.5, 136.5, 141};
+double CrystalEcalEnergyCorrectionSvc::thetaCorrectionAngleEnd[7] = {97.0, 107.5, 117.5, 126, 133, 139.0, 143.5};
+double CrystalEcalEnergyCorrectionSvc::phiCorrectionAngleStart[2] = {4, 15.5};
+double CrystalEcalEnergyCorrectionSvc::phiCorrectionAngleEnd[2] = {7, 18.5};
+double CrystalEcalEnergyCorrectionSvc::mpvCorrectionFactor[5] = {-1524.86, -2098.74, 0.01, 191.79, 0.98};
+int CrystalEcalEnergyCorrectionSvc::angleBinNumber = 100;
+double CrystalEcalEnergyCorrectionSvc::angleRangePhi = 3.0;
+double CrystalEcalEnergyCorrectionSvc::angleRangeTheta = 2.5;
+int CrystalEcalEnergyCorrectionSvc::moduleNumberPhi = 32;
+
+StatusCode CrystalEcalEnergyCorrectionSvc::initialize() {
+    
+    // read correction file
+    std::string m_correctionFile = _correctionFile;
+    file = TFile::Open(m_correctionFile.c_str());
+
+    // phi
+    treePhi = (TTree*)(file->Get("scalePhi")); 
+    treePhi->SetBranchAddress("phi", &_phiAngle);
+    treePhi->SetBranchAddress("scale", &_phiScale); 
+    for(int i = 0; i < treePhi->GetEntries(); i++) {
+        treePhi->GetEntry(i);
+        phiAngle.push_back(_phiAngle);
+        phiScale.push_back(_phiScale);
+    }
+
+    //theta
+    treeTheta = (TTree*)(file->Get("scaleTheta"));
+    treeTheta->SetBranchAddress("theta", &_thetaAngle);
+    treeTheta->SetBranchAddress("scale", &_thetaScale);
+    for(int i = 0; i < treeTheta->GetEntries(); i++) {
+        treeTheta->GetEntry(i);
+        thetaAngle.push_back(_thetaAngle);
+        thetaScale.push_back(_thetaScale);
+    }
+
+    StatusCode sc = Service::initialize();
+    return sc;
+}
+
+StatusCode CrystalEcalEnergyCorrectionSvc::finalize() {
+    
+    file->Close();
+
+    phiAngle.clear();
+    phiScale.clear();
+    thetaAngle.clear();
+    thetaScale.clear();
+    
+    StatusCode sc = Service::finalize();
+    return sc;
+}
+
+double CrystalEcalEnergyCorrectionSvc::energyCorrection(double energy, double phi, double theta) { // energy: GeV, phi: 0-360, theta: 0-180
+
+    //std::cout<<"Before correction: "<<energy<<" phi "<<phi<<" theta "<<theta<<std::endl;
+
+    // #####  mpv deviation #####
+    double energyCor = 0;
+    energyCor = energy / (mpvCorrectionFactor[3]/(mpvCorrectionFactor[0] + mpvCorrectionFactor[1]*energy + mpvCorrectionFactor[2]*energy*energy) + mpvCorrectionFactor[4]);
+    //std::cout<<"After MPV correction: "<<energyCor<<std::endl;
+
+    // #####  energy leakage in cracks #####
+    
+    double tmpPhi = fmod(phi, 360./(moduleNumberPhi/2));
+
+    auto itPhiStart = std::lower_bound(std::begin(phiCorrectionAngleStart), std::end(phiCorrectionAngleStart), tmpPhi);
+    auto itPhiEnd = std::upper_bound(std::begin(phiCorrectionAngleEnd), std::end(phiCorrectionAngleEnd), tmpPhi);
+
+    int iPhiStart = std::distance(std::begin(phiCorrectionAngleStart), itPhiStart) - 1;
+    int jPhiEnd = std::distance(std::begin(phiCorrectionAngleEnd), itPhiEnd);
+
+    if(iPhiStart == jPhiEnd) {
+        
+        int binNumber = jPhiEnd*angleBinNumber + abs((tmpPhi-phiCorrectionAngleStart[iPhiStart])/(angleRangePhi/angleBinNumber));
+        //std::cout<<"   phi correction factor: "<<phiScale.at(binNumber)<<std::endl;
+        energyCor = energyCor/phiScale.at(binNumber);
+    }
+
+    //std::cout<<"After phi correction: "<<energyCor<<std::endl;
+    
+    double tmpTheta = theta;
+    if(tmpTheta<90) tmpTheta = 90 + (90-tmpTheta);
+
+    auto itThetaStart = std::lower_bound(std::begin(thetaCorrectionAngleStart), std::end(thetaCorrectionAngleStart), tmpTheta);
+    auto itThetaEnd = std::upper_bound(std::begin(thetaCorrectionAngleEnd), std::end(thetaCorrectionAngleEnd), tmpTheta);
+
+    int iThetaStart = std::distance(std::begin(thetaCorrectionAngleStart), itThetaStart) - 1;
+    int jThetaEnd = std::distance(std::begin(thetaCorrectionAngleEnd), itThetaEnd);
+
+    if(iThetaStart == jThetaEnd) {
+        
+        int binNumber = jThetaEnd*angleBinNumber + abs((tmpTheta-thetaCorrectionAngleStart[iThetaStart])/(angleRangeTheta/angleBinNumber));
+        //std::cout<<"   theta correction factor: "<<thetaScale.at(binNumber)<<std::endl;
+        energyCor = energyCor/thetaScale.at(binNumber);
+    }
+
+    //std::cout<<"After theta correction: "<<energyCor<<std::endl;
+    
+    //msg() << "corection done!" << endmsg;
+    return energyCor;
+}
+#endif
diff --git a/Service/CrystalEcalSvc/src/CrystalEcalEnergyCorrectionSvc.h b/Service/CrystalEcalSvc/src/CrystalEcalEnergyCorrectionSvc.h
new file mode 100755
index 0000000000000000000000000000000000000000..71aa3922e8860e28fa07842a53d0df99efc35b09
--- /dev/null
+++ b/Service/CrystalEcalSvc/src/CrystalEcalEnergyCorrectionSvc.h
@@ -0,0 +1,44 @@
+#ifndef XTALECALENERGYCORRECTIONSvc_h
+#define XTALECALENERGYCORRECTIONSvc_h
+
+#include <GaudiKernel/Service.h>
+#include "CrystalEcalSvc/ICrystalEcalSvc.h"
+#include "TFile.h"
+#include "TTree.h"
+#include "TBranch.h"
+#include <vector>
+#include <string>
+#include <cfloat>
+#include <cmath>
+
+class CrystalEcalEnergyCorrectionSvc: public extends<Service, ICrystalEcalSvc> {
+public:
+    using extends::extends;
+
+    StatusCode initialize() override;
+    StatusCode finalize() override;
+
+    double energyCorrection(double energy, double phi, double theta) override;
+
+    static double thetaCorrectionAngleStart[7];
+    static double thetaCorrectionAngleEnd[7];
+    static double phiCorrectionAngleStart[2];
+    static double phiCorrectionAngleEnd[2];
+    static double mpvCorrectionFactor[5];
+    static int angleBinNumber;
+    static double angleRangePhi;
+    static double angleRangeTheta;
+    static int moduleNumberPhi;
+
+private:
+    mutable Gaudi::Property<std::string> _correctionFile{this, "CorrectionFile", "/cefs/higgs/songwz/summer24/CEPCSW_301/CEPCSW/workArea/scale.root", "position of energy correction file"};
+    
+    TFile* file;
+    TTree* treePhi;
+    TTree* treeTheta; 
+    double _phiAngle, _phiScale, _thetaAngle, _thetaScale;
+    std::vector<double> phiAngle, phiScale, thetaAngle, thetaScale; 
+      
+};
+
+#endif