diff --git a/algo/CMakeLists.txt b/algo/CMakeLists.txt
index d66378af48aa36385b2c27a201dcaee99be8c3e3..cd2e1c91694315f70c56ce5b53715d4395beb66b 100644
--- a/algo/CMakeLists.txt
+++ b/algo/CMakeLists.txt
@@ -38,6 +38,7 @@ set(SRCS
   detectors/tof/Clusterizer.cxx
   detectors/tof/ReadoutConfig.cxx
   detectors/tof/Unpack.cxx
+  detectors/tof/Hitfind.cxx
   detectors/bmon/ReadoutConfig.cxx
   detectors/bmon/Unpack.cxx
   detectors/trd/ReadoutConfig.cxx
diff --git a/algo/detectors/tof/Clusterizer.cxx b/algo/detectors/tof/Clusterizer.cxx
index 7554e1422ad6b75f46310177132693ec51ca5daf..875738135c880742dcb1494acdf62ad2794179bb 100644
--- a/algo/detectors/tof/Clusterizer.cxx
+++ b/algo/detectors/tof/Clusterizer.cxx
@@ -20,8 +20,7 @@ namespace cbm::algo::tof
   {
     //std::vector<inputType> input = calibrateDigis(digisIn);
     std::vector<inputType> input = chanSortDigis(digisIn);
-    //return buildClusters(input);
-    return buildClustersIter(input);  //Iterator based implementation
+    return buildClusters(input);
   }
 
   std::vector<Clusterizer::inputType> Clusterizer::chanSortDigis(inputType& digisIn)
@@ -117,7 +116,7 @@ namespace cbm::algo::tof
     return result;
   }
 
-
+  //Iterator-based version. Faster than index-based version.
   Clusterizer::resultType Clusterizer::buildClusters(std::vector<inputType>& input)
   {
     // Hit variables
@@ -144,30 +143,20 @@ namespace cbm::algo::tof
       if (fParams.fDeadStrips & (1 << chan)) { continue; }  // skip over dead channels
 
       inputType& storDigi = input[chan];
+      auto digiIt         = storDigi.begin();
+
+      while (1 < std::distance(digiIt, storDigi.end())) {
 
-      while (1 < storDigi.size()) {
-        while (storDigi[0].first->GetSide() == storDigi[1].first->GetSide()) {
-          // Not one Digi of each end!
-          uint8_t offset = 0;
-
-          /* D.Smith 14.8.23: This condition is never needed due to time-sorted input (check with PAL and VF)
-	  //D. Smith 8.9.23: Theoretically, this could matter for two digis with identical time stamp but different TOT. Can this even happen?
-	      // use digi that is closer to the last one
-              if (storDigi.size() > 2 && storDigi[2].first->GetSide() != storDigi[0].first->GetSide()
-                  && storDigi[2].first->GetTime() - storDigi[0].first->GetTime()
-                       <= storDigi[2].first->GetTime() - storDigi[1].first->GetTime()) {
-                offset++;
-              }
-*/
-          storDigi.erase(storDigi.begin() + offset);
-          if (2 > storDigi.size()) { break; }
-        }  // same condition side end
-        if (2 > storDigi.size()) { break; }
+        while (digiIt->first->GetSide() == std::next(digiIt, 1)->first->GetSide()) {  // Not one Digi of each end!
+          digiIt++;
+          if (2 > std::distance(digiIt, storDigi.end())) { break; }
+        }
+        if (2 > std::distance(digiIt, storDigi.end())) { break; }
 
         // 2 Digis = both sides present
         cell               = &fParams.fChanPar[chan].cell;
-        CbmTofDigi* xDigiA = storDigi[0].first;
-        CbmTofDigi* xDigiB = storDigi[1].first;
+        CbmTofDigi* xDigiA = digiIt->first;
+        CbmTofDigi* xDigiB = std::next(digiIt, 1)->first;
 
         // use local coordinates, (0,0,0) is in the center of counter  ?
         ROOT::Math::XYZVector pos(((-(double) numChan / 2. + (double) chan) + 0.5) * cell->sizeX, 0., 0.);
@@ -177,15 +166,12 @@ namespace cbm::algo::tof
         pos.SetY(fParams.fSigVel * timeDif * 0.5);            // A is the top side, B is the bottom side
         if (xDigiA->GetSide() != 1.) { pos.SetY(-pos.Y()); }  // B is the bottom side, A is the top side
 
-        while (storDigi.size() > 2 && std::abs(pos.Y()) > cell->sizeY * fParams.fPosYMaxScal) {
+        while (std::distance(digiIt, storDigi.end()) > 2 && std::abs(pos.Y()) > cell->sizeY * fParams.fPosYMaxScal) {
 
-          CbmTofDigi* xDigiC = storDigi[2].first;
+          CbmTofDigi* xDigiC = std::next(digiIt, 2)->first;
 
-          double timeDifN = 0;
-          if (xDigiC->GetSide() == xDigiA->GetSide()) { timeDifN = xDigiC->GetTime() - xDigiB->GetTime(); }
-          else {
-            timeDifN = xDigiC->GetTime() - xDigiA->GetTime();
-          }
+          const double timeDifN = (xDigiC->GetSide() == xDigiA->GetSide()) ? xDigiC->GetTime() - xDigiB->GetTime()
+                                                                           : xDigiC->GetTime() - xDigiA->GetTime();
 
           double posYN = fParams.fSigVel * timeDifN * 0.5;
           if (xDigiC->GetSide() != 1.) { posYN *= -1.; }
@@ -193,28 +179,15 @@ namespace cbm::algo::tof
           if (std::abs(posYN) >= std::abs(pos.Y())) { break; }
           pos.SetY(posYN);
 
-          if (xDigiC->GetSide() == xDigiA->GetSide()) {
-            xDigiA = xDigiC;
-            storDigi.erase(storDigi.begin());
-          }
+          if (xDigiC->GetSide() == xDigiA->GetSide()) { xDigiA = xDigiC; }
           else {
             xDigiB = xDigiC;
-
-            //D.Smith 14.8.23: Shouldn't we erase the "+1" element here instead?
-            //Doesn't seem to have an effect on the result regardless.
-            //"+1" would be better for an iterator based implementation.
-
-            //D.Smith 25.8.23: Actually, for some strange reason I don't fully understand, this
-            //erase operation doesn't matter at all, even though this branch is sometimes called.
-            //Perhaps this is data dependent? Erasing the zero element also works. This would
-            //be best for iterators.
-            storDigi.erase(storDigi.begin() + 2);
           }
-
-        }  //while loop end
+          digiIt++;
+        }
 
         if (std::abs(pos.Y()) > cell->sizeY * fParams.fPosYMaxScal) {  // remove both digis
-          storDigi.erase(storDigi.begin());
+          digiIt++;
           continue;
         }
         // The "Strip" time is the mean time between each end
@@ -237,9 +210,8 @@ namespace cbm::algo::tof
             cluster.reset();
           }
         }
-        cluster.add(pos, time, totSum, totSum, storDigi[0].second, storDigi[1].second);
-        storDigi.erase(storDigi.begin());
-        storDigi.erase(storDigi.begin());
+        cluster.add(pos, time, totSum, totSum, digiIt->second, std::next(digiIt, 1)->second);
+        digiIt += 2;
 
         lastChan = chan;
         lastPosY = pos.Y();
@@ -259,7 +231,6 @@ namespace cbm::algo::tof
     return clustersOut;
   }
 
-
   bool Clusterizer::AddNextChan(std::vector<inputType>& input, int32_t lastChan, TofCluster& cluster,
                                 std::vector<TofCluster>& clustersOut, std::vector<inputType::iterator>* lastChanPos)
   {
@@ -356,119 +327,4 @@ namespace cbm::algo::tof
     return true;
   }
 
-  //Iterator-based version. Faster than index-based version.
-  Clusterizer::resultType Clusterizer::buildClustersIter(std::vector<inputType>& input)
-  {
-    // Hit variables
-    TofCluster cluster;
-    std::vector<TofCluster> clustersOut;
-
-    // Reference cell of a cluster
-    TofCell* cell = nullptr;
-
-    // Last Channel Temp variables
-    int32_t lastChan = -1;
-    double lastPosY  = 0.0;
-    double lastTime  = 0.0;
-
-    const size_t numChan = fParams.fChanPar.size();
-
-    //Store last position in input channels to avoid unnecessary checks in AddNextChan().
-    std::vector<inputType::iterator> lastChanPos;
-    for (int32_t chan = 0; chan < numChan; chan++) {
-      lastChanPos.push_back(input[chan].begin());
-    }
-
-    for (int32_t chan = 0; chan < numChan; chan++) {
-      if (fParams.fDeadStrips & (1 << chan)) { continue; }  // skip over dead channels
-
-      inputType& storDigi = input[chan];
-      auto digiIt         = storDigi.begin();
-
-      while (1 < std::distance(digiIt, storDigi.end())) {
-
-        while (digiIt->first->GetSide() == std::next(digiIt, 1)->first->GetSide()) {  // Not one Digi of each end!
-          digiIt++;
-          if (2 > std::distance(digiIt, storDigi.end())) { break; }
-        }
-        if (2 > std::distance(digiIt, storDigi.end())) { break; }
-
-        // 2 Digis = both sides present
-        cell               = &fParams.fChanPar[chan].cell;
-        CbmTofDigi* xDigiA = digiIt->first;
-        CbmTofDigi* xDigiB = std::next(digiIt, 1)->first;
-
-        // use local coordinates, (0,0,0) is in the center of counter  ?
-        ROOT::Math::XYZVector pos(((-(double) numChan / 2. + (double) chan) + 0.5) * cell->sizeX, 0., 0.);
-
-        double timeDif = xDigiA->GetTime() - xDigiB->GetTime();
-
-        pos.SetY(fParams.fSigVel * timeDif * 0.5);            // A is the top side, B is the bottom side
-        if (xDigiA->GetSide() != 1.) { pos.SetY(-pos.Y()); }  // B is the bottom side, A is the top side
-
-        while (std::distance(digiIt, storDigi.end()) > 2 && std::abs(pos.Y()) > cell->sizeY * fParams.fPosYMaxScal) {
-
-          CbmTofDigi* xDigiC = std::next(digiIt, 2)->first;
-
-          const double timeDifN = (xDigiC->GetSide() == xDigiA->GetSide()) ? xDigiC->GetTime() - xDigiB->GetTime()
-                                                                           : xDigiC->GetTime() - xDigiA->GetTime();
-
-          double posYN = fParams.fSigVel * timeDifN * 0.5;
-          if (xDigiC->GetSide() != 1.) { posYN *= -1.; }
-
-          if (std::abs(posYN) >= std::abs(pos.Y())) { break; }
-          pos.SetY(posYN);
-
-          if (xDigiC->GetSide() == xDigiA->GetSide()) { xDigiA = xDigiC; }
-          else {
-            xDigiB = xDigiC;
-          }
-          digiIt++;
-        }
-
-        if (std::abs(pos.Y()) > cell->sizeY * fParams.fPosYMaxScal) {  // remove both digis
-          digiIt++;
-          continue;
-        }
-        // The "Strip" time is the mean time between each end
-        double time = 0.5 * (xDigiA->GetTime() + xDigiB->GetTime());
-
-        // Weight is the total charge => sum of both ends ToT
-        double totSum = xDigiA->GetTot() + xDigiB->GetTot();
-
-        // Now check if a hit/cluster is already started
-        if (0 < cluster.numChan()) {
-          // a cluster is already started => check distance in space/time
-          // For simplicity, just check along strip direction for now
-          // and break cluster when a not fired strip is found
-          if (!(std::abs(time - lastTime) < fParams.fdMaxTimeDist && lastChan == chan - 1
-                && std::abs(pos.Y() - lastPosY) < fParams.fdMaxSpaceDist)) {
-
-            cluster.normalize(fParams.fTimeRes);
-            cluster.finalize(*cell, fParams);
-            clustersOut.push_back(cluster);
-            cluster.reset();
-          }
-        }
-        cluster.add(pos, time, totSum, totSum, digiIt->second, std::next(digiIt, 1)->second);
-        digiIt += 2;
-
-        lastChan = chan;
-        lastPosY = pos.Y();
-        lastTime = time;
-        if (AddNextChan(input, lastChan, cluster, clustersOut, &lastChanPos)) { cluster.reset(); }
-      }                  // while( 1 < storDigi.size() )
-      storDigi.clear();  //D.Smith 11.8.23: In rare cases, a single digi remains and is deleted here.
-    }                    // for( int32_t chan = 0; chan < iNbCh; chan++ )
-
-    // Now check if another hit/cluster is started
-    // and save it if it's the case.
-    if (0 < cluster.numChan()) {
-      cluster.normalize(fParams.fTimeRes);
-      cluster.finalize(*cell, fParams);
-      clustersOut.push_back(cluster);
-    }
-    return clustersOut;
-  }
-
 }  // namespace cbm::algo::tof
diff --git a/algo/detectors/tof/Clusterizer.h b/algo/detectors/tof/Clusterizer.h
index 7637a89d083feedc7f0412bc1c4465b1af071675..4ddd32ba1f8e0be9dbaf571ea737f041674b2cd9 100644
--- a/algo/detectors/tof/Clusterizer.h
+++ b/algo/detectors/tof/Clusterizer.h
@@ -177,13 +177,9 @@ namespace cbm::algo::tof
     std::vector<inputType> chanSortDigis(inputType& digisIn);
 
     resultType buildClusters(std::vector<inputType>& input);
-    resultType buildClustersIter(std::vector<inputType>& input);
-
 
     bool AddNextChan(std::vector<inputType>& input, int32_t iLastChan, TofCluster& cluster,
                      std::vector<TofCluster>& clustersOut, std::vector<inputType::iterator>* lastChanPos = nullptr);
-
-    int32_t numSameSide;  // Digis quality
   };
 
 }  // namespace cbm::algo::tof
diff --git a/algo/detectors/tof/Hitfind.cxx b/algo/detectors/tof/Hitfind.cxx
new file mode 100644
index 0000000000000000000000000000000000000000..8fda874b2fcfa7aa55a3fb4430c9e2528a760189
--- /dev/null
+++ b/algo/detectors/tof/Hitfind.cxx
@@ -0,0 +1,216 @@
+/* Copyright (C) 2023 Facility for Antiproton and Ion Research in Europe, Darmstadt
+   SPDX-License-Identifier: GPL-3.0-only
+   Authors: Dominik Smith [committer] */
+
+#include "Hitfind.h"
+
+#include <chrono>
+
+#include <xpu/host.h>
+
+#include "compat/Algorithm.h"
+#include "compat/OpenMP.h"
+#include "log.hpp"
+
+using namespace std;
+using fles::Subsystem;
+
+namespace cbm::algo::tof
+{
+  // -----   Execution   -------------------------------------------------------
+  Hitfind::resultType Hitfind::operator()(std::vector<CbmTofDigi>* digiIn)
+  {
+    xpu::push_timer("Hitfind");
+
+    // --- Output data
+    resultType result                  = {};
+    std::vector<TofCluster>& clusterTs = result.first;
+    HitfindMonitorData& monitor        = result.second;
+
+    // Do calibration globally (optional, should not be used together with RPC-wise calibration)
+    *digiIn = CalibRawDigis(*digiIn);
+
+    // Loop over the digis array and store the Digis in separate vectors for
+    // each RPC modules
+    for (int32_t idigi = 0; idigi < digiIn->size(); idigi++) {
+      CbmTofDigi* pDigi = &(digiIn->at(idigi));
+
+      // These are doubles in the digi class
+      const double SmType = pDigi->GetType();
+      const double Sm     = pDigi->GetSm();
+      const double Rpc    = pDigi->GetRpc();
+      const int NbRpc     = fNbRpc[SmType];
+      fStorDigi[SmType][Sm * NbRpc + Rpc].push_back(std::make_pair(pDigi, idigi));
+    }
+
+    // ---  RPC loop
+    for (uint32_t SmType = 0; SmType < fNbSm.size(); SmType++) {
+      const int NbRpc = fNbRpc[SmType];
+      const int NbSm  = fNbSm[SmType];
+      for (uint32_t Sm = 0; Sm < NbSm; Sm++) {
+        for (uint32_t Rpc = 0; Rpc < NbRpc; Rpc++) {
+
+          // Get digis
+          std::vector<std::pair<CbmTofDigi*, int32_t>>& digiExp = fStorDigi[SmType][Sm * NbRpc + Rpc];
+
+          // Build clusters
+          std::vector<cbm::algo::tof::TofCluster> clusters = fAlgo[SmType][Sm * NbRpc + Rpc](digiExp);
+
+          // Append clusters to output
+          clusterTs.insert(clusterTs.end(), clusters.begin(), clusters.end());
+
+          // Clear digi storage
+          digiExp.clear();
+        }
+      }
+    }
+
+    xpu::timings timings = xpu::pop_timer();
+    monitor.fTimeHitfind = timings.wall();
+    return result;
+  }
+  // ----------------------------------------------------------------------------
+
+
+  // -----   Initialisation   ---------------------------------------------------
+  void Hitfind::Init()
+  {
+    //Number of SMs per super module type
+    fNbSm = fTofConfig.NbSm;
+
+    //Number of RPCs per super module type
+    fNbRpc = fTofConfig.NbRpc;
+
+    //Prepare digi storage
+    fStorDigi.resize(fNbSm.size());
+
+    // Create one algorithm per RPC for TOF and configure it with parametersa
+    for (int32_t SmType = 0; SmType < fNbSm.size(); SmType++) {
+
+      int32_t NbSm  = fNbSm[SmType];
+      int32_t NbRpc = fNbRpc[SmType];
+      fStorDigi[SmType].resize(NbSm * NbRpc);
+
+      for (int32_t Sm = 0; Sm < NbSm; Sm++) {
+        for (int32_t Rpc = 0; Rpc < NbRpc; Rpc++) {
+
+          std::unique_ptr<cbm::algo::tof::ClusterizerRpcPar> par(new cbm::algo::tof::ClusterizerRpcPar());
+
+          HitfindSetup::Rpc rpcPar = fTofConfig.rpcs[SmType][Sm * NbRpc + Rpc];
+          par->fDeadStrips         = rpcPar.deadStrips;
+          par->fPosYMaxScal        = rpcPar.posYMaxScal;
+          par->fdMaxTimeDist       = rpcPar.maxTimeDist;
+          par->fdMaxSpaceDist      = rpcPar.maxSpaceDist;
+          par->fSigVel             = rpcPar.sigVel;
+          par->fCPTOffYBinWidth    = rpcPar.CPTOffYBinWidth;
+          par->fCPTOffY            = rpcPar.CPTOffY;
+          par->fCPTOffYRange       = rpcPar.CPTOffYRange;
+          par->fTimeRes            = rpcPar.timeRes;
+          par->numClWalkBinX       = rpcPar.numClWalkBinX;
+          par->TOTMax              = rpcPar.TOTMax;
+          par->TOTMin              = rpcPar.TOTMin;
+          par->swapChannelSides    = rpcPar.swapChannelSides;
+          par->channelDeadtime     = rpcPar.channelDeadtime;
+
+          int32_t NbChan = rpcPar.chanPar.size();
+          par->fChanPar.resize(NbChan);
+
+          for (int32_t Ch = 0; Ch < NbChan; Ch++) {
+
+            HitfindSetup::Channel chanPar   = rpcPar.chanPar[Ch];
+            const double* tra_ptr           = chanPar.cell.translation.data();
+            const double* rot_ptr           = chanPar.cell.rotation.data();
+            par->fChanPar[Ch].address       = chanPar.address;
+            par->fChanPar[Ch].cell.pos      = ROOT::Math::XYZVector(tra_ptr[0], tra_ptr[1], tra_ptr[2]);
+            par->fChanPar[Ch].cell.rotation = ROOT::Math::Rotation3D(&rot_ptr[0], &rot_ptr[9]);
+            par->fChanPar[Ch].cell.sizeX    = chanPar.cell.sizeX;
+            par->fChanPar[Ch].cell.sizeY    = chanPar.cell.sizeY;
+            par->fChanPar[Ch].fvCPTOff      = chanPar.vCPTOff;
+            par->fChanPar[Ch].fvCPTotGain   = chanPar.vCPTotGain;
+            par->fChanPar[Ch].fvCPTotOff    = chanPar.vCPTotOff;
+            par->fChanPar[Ch].fvCPWalk      = chanPar.vCPWalk;
+          }
+          fAlgo[SmType][Sm * NbRpc + Rpc].SetParams(std::move(par));
+        }
+      }
+    }
+    L_(info) << "--- Configured hitfinder algorithms for TOF.";
+    L_(info) << "==================================================";
+  }
+  // ----------------------------------------------------------------------------
+
+
+  // -------------------------- Calibration -------------------------------------
+  std::vector<CbmTofDigi> Hitfind::CalibRawDigis(const std::vector<CbmTofDigi>& digiVec)
+  {
+    // channel deadtime map
+    std::map<int32_t, double> mChannelDeadTime;
+    std::vector<CbmTofDigi> calDigiVecOut;
+
+    for (int32_t iDigi = 0; iDigi < digiVec.size(); iDigi++) {
+
+      CbmTofDigi pDigi              = digiVec.at(iDigi);
+      const double SmType           = pDigi.GetType();
+      const double Sm               = pDigi.GetSm();
+      const double Rpc              = pDigi.GetRpc();
+      const double Chan             = pDigi.GetChannel();
+      const double Side             = pDigi.GetSide();
+      const int NbRpc               = fNbRpc[SmType];
+      HitfindSetup::Rpc rpcPar      = fTofConfig.rpcs[SmType][Sm * NbRpc + Rpc];
+      HitfindSetup::Channel chanPar = rpcPar.chanPar[Chan];
+
+      if (rpcPar.swapChannelSides && 5 != SmType && 8 != SmType) {
+        pDigi.SetAddress(Sm, Rpc, Chan, (0 == Side) ? 1 : 0, SmType);
+      }
+
+      // Check dead time
+      const int32_t iAddr = pDigi.GetAddress();
+      auto it             = mChannelDeadTime.find(iAddr);
+      if (it != mChannelDeadTime.end() && pDigi.GetTime() <= it->second) {
+        it->second = pDigi.GetTime() + rpcPar.channelDeadtime;
+        continue;
+      }
+      mChannelDeadTime[iAddr] = pDigi.GetTime() + rpcPar.channelDeadtime;
+
+      // Create calibrated digi
+      CbmTofDigi pCalDigi(pDigi);
+
+      // calibrate Digi Time
+      pCalDigi.SetTime(pCalDigi.GetTime() - chanPar.vCPTOff[Side]);
+
+      // subtract Offset
+      double dTot = pCalDigi.GetTot() - chanPar.vCPTotOff[Side];
+      if (dTot < 0.001) dTot = 0.001;
+      // calibrate Digi ToT
+      pCalDigi.SetTot(dTot * chanPar.vCPTotGain[Side]);
+
+      // walk correction
+      std::vector<double>& walk = chanPar.vCPWalk[Side];
+      const double dTotBinSize  = (rpcPar.TOTMax - rpcPar.TOTMin) / rpcPar.numClWalkBinX;
+      int32_t iWx               = (int32_t)((pCalDigi.GetTot() - rpcPar.TOTMin) / dTotBinSize);
+
+      if (0 > iWx) { iWx = 0; }
+      if (iWx >= rpcPar.numClWalkBinX) { iWx = rpcPar.numClWalkBinX - 1; }
+
+      const double dDTot = (pCalDigi.GetTot() - rpcPar.TOTMin) / dTotBinSize - (double) iWx - 0.5;
+      double dWT         = walk[iWx];
+
+      // linear interpolation to next bin
+      if (dDTot > 0) {
+        if (iWx < rpcPar.numClWalkBinX - 1) { dWT += dDTot * (walk[iWx + 1] - walk[iWx]); }
+      }
+      else {
+        if (0 < iWx) { dWT -= dDTot * (walk[iWx - 1] - walk[iWx]); }
+      }
+      pCalDigi.SetTime(pCalDigi.GetTime() - dWT);  // calibrate Digi Time
+      calDigiVecOut.push_back(pCalDigi);
+    }
+
+    /// Sort the buffers of hits due to the time offsets applied
+    std::sort(calDigiVecOut.begin(), calDigiVecOut.end(),
+              [](const CbmTofDigi& a, const CbmTofDigi& b) -> bool { return a.GetTime() < b.GetTime(); });
+
+    return calDigiVecOut;
+  }
+
+}  // namespace cbm::algo::tof
diff --git a/algo/detectors/tof/Hitfind.h b/algo/detectors/tof/Hitfind.h
new file mode 100644
index 0000000000000000000000000000000000000000..802ffcdd3a33b3ffb982c0aa41bd438090b3d2b7
--- /dev/null
+++ b/algo/detectors/tof/Hitfind.h
@@ -0,0 +1,99 @@
+/* Copyright (C) 2023 Facility for Antiproton and Ion Research in Europe, Darmstadt
+   SPDX-License-Identifier: GPL-3.0-only
+   Authors: Dominik Smith [committer] */
+
+#ifndef TOFHITFIND_H
+#define TOFHITFIND_H 1
+
+#include "Timeslice.hpp"
+
+#include "tof/Clusterizer.h"
+#include "tof/HitfindSetup.h"
+
+#include <gsl/span>
+#include <optional>
+#include <sstream>
+#include <vector>
+
+#include "DigiData.h"
+#include "PODVector.h"
+
+namespace cbm::algo::tof
+{
+
+  /** @struct HitfindMonitorData
+   ** @author Dominik Smith <d.smith@gsi.de>
+   ** @since 16 Oct 2023
+   ** @brief Monitoring data for hitfinding
+   **/
+  struct HitfindMonitorData {
+    std::vector<tof::HitfindMonitorData> fTof;  ///< Monitoring data for TOF
+
+    //// TO DO: Add something sensible here.
+    double fTimeHitfind         = 0;
+    size_t fNumMs               = 0;
+    size_t fNumBytes            = 0;
+    size_t fNumBytesInTof       = 0;
+    size_t fNumDigis            = 0;
+    size_t fNumCompUsed         = 0;
+    size_t fNumErrInvalidEqId   = 0;
+    size_t fNumErrInvalidSysVer = 0;
+    std::string print() const
+    {
+      std::stringstream ss;
+      ss << "TS stats: num MS " << fNumMs << ", bytes " << fNumBytes << ", digis " << fNumDigis << ", components "
+         << fNumCompUsed << ", invalidEqIds " << fNumErrInvalidEqId << ", invalidSysVersions " << fNumErrInvalidSysVer
+         << std::endl;
+      return ss.str();
+    }
+  };
+
+  /** @class Hitfind
+ ** @brief Algo class for hitfinding 
+ ** @author Dominik Smith <d.smith@gsi.de>
+ ** @since 16.10.2023
+ **
+ **/
+  class Hitfind {
+
+  public:
+    typedef std::pair<std::vector<TofCluster>, HitfindMonitorData> resultType;
+
+    /** @brief Algorithm execution
+     ** @param fles timeslice to hitfind
+     ** @return pair: digi timeslice, monitoring data
+     **/
+    resultType operator()(std::vector<CbmTofDigi>* digiIn);
+
+    /** @brief Default constructor **/
+    Hitfind() {};
+
+    /** @brief Destructor **/
+    ~Hitfind() {};
+
+    /** @brief Parameters for TOF hitfinders **/
+    tof::HitfindSetup fTofConfig {};
+
+    /** @brief Initialize hitfinder and fill parameters from config object
+     **/
+    void Init();
+
+  private:  // members
+    /** @brief TOF hitfinders **/
+    std::map<uint32_t, std::map<uint32_t, tof::Clusterizer>> fAlgo = {};  //[nbType][nbSm*nbRpc]
+
+    /** @brief Intermediate storage variables (digi, index) **/
+    std::vector<std::vector<std::vector<std::pair<CbmTofDigi*, int32_t>>>> fStorDigi;  //[nbType][nbSm*nbRpc][nDigis]
+
+    /** @brief Number of SMs per super module type **/
+    std::vector<int32_t> fNbSm;
+
+    /** @brief Number of RPCs per super module type **/
+    std::vector<int32_t> fNbRpc;
+
+    /** @brief Applies calibration to input digis **/
+    std::vector<CbmTofDigi> CalibRawDigis(const std::vector<CbmTofDigi>& digiVec);
+  };
+}  // namespace cbm::algo::tof
+
+#endif /* TOFHITFIND_H */
diff --git a/algo/detectors/tof/HitfindSetup.h b/algo/detectors/tof/HitfindSetup.h
new file mode 100644
index 0000000000000000000000000000000000000000..d63a4637386989f4774cbd5c9c049c2466025643
--- /dev/null
+++ b/algo/detectors/tof/HitfindSetup.h
@@ -0,0 +1,102 @@
+/* Copyright (C) 2023 Facility for Antiproton and Ion Research in Europe, Darmstadt
+   SPDX-License-Identifier: GPL-3.0-only
+   Authors: Dominik Smith [committer] */
+#ifndef CBM_ALGO_DETECTOR_TOF_HITFIND_SETUP_H
+#define CBM_ALGO_DETECTOR_TOF_HITFIND_SETUP_H
+
+#include <map>
+#include <string>
+#include <vector>
+
+#include "Definitions.h"
+#include "config/Property.h"
+
+namespace cbm::algo::tof
+{
+
+  /**
+   * @brief Hitfind setup / Hardware cabling for TOF
+   * Used to create the hardware mapping for the TOF hitfinder.
+   */
+  struct HitfindSetup {
+
+    struct Cell {
+      double sizeX;
+      double sizeY;
+      std::array<double, 3> translation;
+      std::array<double, 9> rotation;
+
+      static constexpr auto Properties =
+        std::make_tuple(config::Property(&Cell::sizeX, "sizeX", "size in X direction"),
+                        config::Property(&Cell::sizeY, "sizeY", "size in Y direction"),
+                        config::Property(&Cell::translation, "translation", "Translation vector", YAML::Flow),
+                        config::Property(&Cell::rotation, "rotation", "Rotation matrix", YAML::Flow));
+    };
+
+    struct Channel {
+      std::vector<double> vCPTOff;
+      std::vector<double> vCPTotGain;
+      std::vector<double> vCPTotOff;
+      std::vector<std::vector<double>> vCPWalk;
+      i32 address;
+      Cell cell;
+
+      static constexpr auto Properties =
+        std::make_tuple(config::Property(&Channel::vCPTOff, "vCPTOff", "CPT offset"),
+                        config::Property(&Channel::vCPTotGain, "vCPTotGain", "CP time over threshold gain"),
+                        config::Property(&Channel::vCPTotOff, "vCPTotOff", "CP time over threshold offset"),
+                        config::Property(&Channel::vCPWalk, "vCPWalk", "CP walk correction"),
+                        config::Property(&Channel::address, "address", "unique address"),
+                        config::Property(&Channel::cell, "cell", "cell parameters"));
+    };
+
+    struct Rpc {
+      u32 deadStrips;
+      double posYMaxScal;
+      double maxTimeDist;
+      double maxSpaceDist;
+      double sigVel;
+      double timeRes;
+      i32 numClWalkBinX;
+      double TOTMax;
+      double TOTMin;
+      bool swapChannelSides;
+      double channelDeadtime;
+      double CPTOffYBinWidth;
+      double CPTOffYRange;
+
+      std::vector<double> CPTOffY;
+      std::vector<Channel> chanPar;
+
+      static constexpr auto Properties =
+        std::make_tuple(config::Property(&Rpc::deadStrips, "deadStrips", "bit mask for dead strips"),
+                        config::Property(&Rpc::posYMaxScal, "posYMaxScal", "maximum value of y position"),
+                        config::Property(&Rpc::maxTimeDist, "maxTimeDist", "maximum time distance"),
+                        config::Property(&Rpc::maxSpaceDist, "maxSpaceDist", "maximum space distance"),
+                        config::Property(&Rpc::sigVel, "sigVel", "signal velocity"),
+                        config::Property(&Rpc::timeRes, "timeRes", "time resolution"),
+                        config::Property(&Rpc::numClWalkBinX, "numClWalkBinX", "number of walk correction bins"),
+                        config::Property(&Rpc::TOTMax, "TOTMax", "maximum time over threshold"),
+                        config::Property(&Rpc::TOTMin, "TOTMin", "minimum time over threshold"),
+                        config::Property(&Rpc::swapChannelSides, "swapChannelSides", "flag for swapping channel sides"),
+                        config::Property(&Rpc::channelDeadtime, "channelDeadtime", "channel dead time"),
+                        config::Property(&Rpc::CPTOffYBinWidth, "CPTOffYBinWidth", "CPT Y offset bin width"),
+                        config::Property(&Rpc::CPTOffYRange, "CPTOffYRange", "CPT Y offset range"),
+                        config::Property(&Rpc::CPTOffY, "CPTOffY", "CPT Y offset array"),
+                        config::Property(&Rpc::chanPar, "chanPar", "channel parameters"));
+    };
+
+    std::vector<std::vector<Rpc>> rpcs;
+    std::vector<int32_t> NbSm;
+    std::vector<int32_t> NbRpc;
+
+    static constexpr auto Properties = std::make_tuple(
+      config::Property(&HitfindSetup::rpcs, "rpcs", "Parameters of RPCs"),
+      config::Property(&HitfindSetup::NbSm, "NbSm", "Number of SMs per super module type", {}, YAML::Flow),
+      config::Property(&HitfindSetup::NbRpc, "NbRpc", "Number of RPCs per super module type", {}, YAML::Flow));
+  };
+
+
+}  // namespace cbm::algo::tof
+
+#endif  // CBM_ALGO_DETECTOR_TOF_HITFIND_SETUP_H
diff --git a/reco/tasks/CbmTaskTofClusterizer.cxx b/reco/tasks/CbmTaskTofClusterizer.cxx
index edf3d2271682d9dd09d62ac967b290798a20fdf8..52613a1f710885bec5b2f495f64e5aa6074e5d54 100644
--- a/reco/tasks/CbmTaskTofClusterizer.cxx
+++ b/reco/tasks/CbmTaskTofClusterizer.cxx
@@ -9,17 +9,7 @@
 #include "CbmDigiManager.h"
 #include "CbmEvent.h"
 #include "CbmMatch.h"
-#include "CbmTofAddress.h"          // in cbmdata/tof
-#include "CbmTofCell.h"             // in tof/TofData
-#include "CbmTofCreateDigiPar.h"    // in tof/TofTools
-#include "CbmTofDetectorId_v12b.h"  // in cbmdata/tof
-#include "CbmTofDetectorId_v14a.h"  // in cbmdata/tof
-#include "CbmTofDetectorId_v21a.h"  // in cbmdata/tof
-#include "CbmTofDigi.h"             // in cbmdata/tof
-#include "CbmTofDigiBdfPar.h"       // in tof/TofParam
-#include "CbmTofDigiPar.h"          // in tof/TofParam
-#include "CbmTofGeoHandler.h"       // in tof/TofTools
-#include "CbmTofHit.h"              // in cbmdata/tof
+#include "CbmTofHit.h"  // in cbmdata/tof
 #include "CbmTsEventHeader.h"
 
 // FAIR classes and includes
@@ -31,19 +21,14 @@
 
 // ROOT Classes and includes
 #include "TClonesArray.h"
-#include "TGeoManager.h"
-#include "TGeoPhysicalNode.h"
-#include "TH2.h"
-#include "TProfile.h"
-#include "TROOT.h"
 #include "TStopwatch.h"
-#include "TVector3.h"
 
 // C++ Classes and includes
 #include <iomanip>
 #include <vector>
 
-// Globals
+#include "compat/Filesystem.h"
+#include "config/Yaml.h"
 
 
 /************************************************************************************/
@@ -51,9 +36,6 @@ CbmTaskTofClusterizer::CbmTaskTofClusterizer() : CbmTaskTofClusterizer("Testbeam
 
 CbmTaskTofClusterizer::CbmTaskTofClusterizer(const char* name, int32_t verbose, bool writeDataInOut)
   : FairTask(TString(name), verbose)
-  , fTofId(NULL)
-  , fDigiPar(NULL)
-  , fDigiBdfPar(NULL)
   , fTsHeader(NULL)
   , fDigiMan(nullptr)
   , fEventsColl(nullptr)
@@ -64,31 +46,6 @@ CbmTaskTofClusterizer::CbmTaskTofClusterizer(const char* name, int32_t verbose,
   , fTofHitsCollOut(NULL)
   , fTofDigiMatchCollOut(NULL)
   , fiNbHits(0)
-  , fStorDigi()
-  , fvCPTOff()
-  , fvCPTotGain()
-  , fvCPTotOff()
-  , fvCPWalk()
-  , fvCPTOffY()
-  , fvCPTOffYBinWidth()
-  , fvCPTOffYRange()
-  , fvDeadStrips()
-  , fCalMode(0)
-  , fDutId(-1)
-  , fPosYMaxScal(1.)
-  , fTotMax(100.)
-  , fTotMin(0.)
-  , fTotOff(0.)
-  , fTotMean(0.)
-  , fMaxTimeDist(1.)
-  , fdChannelDeadtime(0.)
-  , fCalParFileName("")
-  , fdTOTMax(50.)
-  , fdTOTMin(0.)
-  , fdTTotMean(2.)
-  , fdMaxTimeDist(0.)
-  , fdMaxSpaceDist(0.)
-  , fdModifySigvel(1.0)
   , fdEvent(0)
   , fbSwapChannelSides(false)
   , fiOutputTreeEntry(0)
@@ -105,23 +62,10 @@ InitStatus CbmTaskTofClusterizer::Init()
   LOG(info) << "CbmTaskTofClusterizer initializing... expect Digis in ns units! ";
   if (false == RegisterInputs()) return kFATAL;
   if (false == RegisterOutputs()) return kFATAL;
-  if (false == InitParameters()) return kFATAL;
-  if (false == InitCalibParameter()) return kFATAL;
   if (false == InitAlgos()) return kFATAL;
   return kSUCCESS;
 }
 
-void CbmTaskTofClusterizer::SetParContainers()
-{
-  LOG(info) << "=> Get the digi parameters for tof";
-  FairRunAna* ana     = FairRunAna::Instance();
-  FairRuntimeDb* rtdb = ana->GetRuntimeDb();
-
-  fDigiPar = (CbmTofDigiPar*) (rtdb->getContainer("CbmTofDigiPar"));
-
-  LOG(info) << "found " << fDigiPar->GetNrOfModules() << " cells ";
-  fDigiBdfPar = (CbmTofDigiBdfPar*) (rtdb->getContainer("CbmTofDigiBdfPar"));
-}
 
 void CbmTaskTofClusterizer::Exec(Option_t* option)
 {
@@ -261,12 +205,6 @@ void CbmTaskTofClusterizer::ExecEvent(Option_t* /*option*/, CbmEvent* tEvent)
   FairRootFileSink* sink = (FairRootFileSink*) FairRootManager::Instance()->GetSink();
   if (sink) { fiOutputTreeEntry = sink->GetOutTree()->GetEntries(); }
 
-  // Check for corruption  (D.Smith: disabled )
-  // if (fTofDigiVec.size() > 20. * fDigiBdfPar->GetNbDet()) { // FIXME constant in code, skip this event
-  //    LOG(warn) << "Skip processing of Tof DigiEvent with " << fTofDigiVec.size() << " digis ";
-  //    return;
-  //  }
-
   fiNbHits = 0;
 
   BuildClusters();
@@ -367,390 +305,15 @@ bool CbmTaskTofClusterizer::RegisterOutputs()
   }
   return true;
 }
-bool CbmTaskTofClusterizer::InitParameters()
-{
-
-  // Initialize the TOF GeoHandler
-  bool isSimulation = false;
-  LOG(info) << "CbmTaskTofClusterizer::InitParameters - Geometry, Mapping, ...  ??";
-
-  // Get Base Container
-  FairRun* ana        = FairRun::Instance();
-  FairRuntimeDb* rtdb = ana->GetRuntimeDb();
-
-  CbmTofGeoHandler geoHandler;
-  int32_t iGeoVersion = geoHandler.Init(isSimulation);
-  if (k14a > iGeoVersion) {
-    LOG(error) << "CbmTaskTofClusterizer::InitParameters => Only compatible "
-                  "with geometries after v14a !!!";
-    return false;
-  }
-  if (iGeoVersion == k14a) fTofId = new CbmTofDetectorId_v14a();
-  else
-    fTofId = new CbmTofDetectorId_v21a();
-
-  // create digitization parameters from geometry file
-  CbmTofCreateDigiPar* tofDigiPar = new CbmTofCreateDigiPar("TOF Digi Producer", "TOF task");
-  LOG(info) << "Create DigiPar ";
-  tofDigiPar->Init();
-
-  fDigiPar = (CbmTofDigiPar*) (rtdb->getContainer("CbmTofDigiPar"));
-  if (0 == fDigiPar) {
-    LOG(error) << "CbmTaskTofClusterizer::InitParameters => Could not obtain "
-                  "the CbmTofDigiPar ";
-    return false;
-  }
-
-  fDigiBdfPar = (CbmTofDigiBdfPar*) (rtdb->getContainer("CbmTofDigiBdfPar"));
-  if (0 == fDigiBdfPar) {
-    LOG(error) << "CbmTaskTofClusterizer::InitParameters => Could not obtain "
-                  "the CbmTofDigiBdfPar ";
-    return false;
-  }
-
-  rtdb->initContainers(ana->GetRunId());
-
-  LOG(info) << "CbmTaskTofClusterizer::InitParameter: currently " << fDigiPar->GetNrOfModules() << " digi cells ";
-
-  fdMaxTimeDist  = fDigiBdfPar->GetMaxTimeDist();     // in ns
-  fdMaxSpaceDist = fDigiBdfPar->GetMaxDistAlongCh();  // in cm
-
-  if (fMaxTimeDist != fdMaxTimeDist) {
-    fdMaxTimeDist  = fMaxTimeDist;  // modify default
-    fdMaxSpaceDist = fdMaxTimeDist * fDigiBdfPar->GetSignalSpeed()
-                     * 0.5;  // cut consistently on positions (with default signal velocity)
-  }
-
-  LOG(info) << " BuildCluster with MaxTimeDist " << fdMaxTimeDist << ", MaxSpaceDist " << fdMaxSpaceDist;
-
-  fvDeadStrips.resize(fDigiBdfPar->GetNbDet());
-  return true;
-}
-/************************************************************************************/
-bool CbmTaskTofClusterizer::InitCalibParameter()
-{
-  // dimension and initialize calib parameter
-  int32_t iNbSmTypes = fDigiBdfPar->GetNbSmTypes();
-
-  if (fTotMean != 0.) fdTTotMean = fTotMean;  // adjust target mean for TOT
-
-  fvCPTOff.resize(iNbSmTypes);
-  fvCPTotGain.resize(iNbSmTypes);
-  fvCPTotOff.resize(iNbSmTypes);
-  fvCPWalk.resize(iNbSmTypes);
-  fvCPTOffY.resize(iNbSmTypes);
-  fvCPTOffYBinWidth.resize(iNbSmTypes);
-  fvCPTOffYRange.resize(iNbSmTypes);
-  for (int32_t iSmType = 0; iSmType < iNbSmTypes; iSmType++) {
-    int32_t iNbSm  = fDigiBdfPar->GetNbSm(iSmType);
-    int32_t iNbRpc = fDigiBdfPar->GetNbRpc(iSmType);
-    fvCPTOff[iSmType].resize(iNbSm * iNbRpc);
-    fvCPTotGain[iSmType].resize(iNbSm * iNbRpc);
-    fvCPTotOff[iSmType].resize(iNbSm * iNbRpc);
-    fvCPWalk[iSmType].resize(iNbSm * iNbRpc);
-    fvCPTOffY[iSmType].resize(iNbSm * iNbRpc);
-    fvCPTOffYBinWidth[iSmType].resize(iNbSm * iNbRpc);
-    fvCPTOffYRange[iSmType].resize(iNbSm * iNbRpc);
-    for (int32_t iSm = 0; iSm < iNbSm; iSm++) {
-      for (int32_t iRpc = 0; iRpc < iNbRpc; iRpc++) {
-
-        fvCPTOffYBinWidth[iSmType][iSm * iNbRpc + iRpc] = 0.;  // initialize
-        fvCPTOffYRange[iSmType][iSm * iNbRpc + iRpc]    = 0.;  // initialize
-
-        /*	D.Smith 23.8.23: For testing hit time calibration. Please remove when done.
-        fvCPTOffYBinWidth[iSmType][iSm * iNbRpc + iRpc] = 1.;  // initialize
-        fvCPTOffYRange[iSmType][iSm * iNbRpc + iRpc]    = 200.;  // initialize
-        fvCPTOffY[iSmType][iSm * iNbRpc + iRpc] =  std::vector<double>(10000, 1000.);  
-        for( size_t i = 0; i < fvCPTOffY[iSmType][iSm * iNbRpc + iRpc].size(); i++ )
-	{
-		fvCPTOffY[iSmType][iSm * iNbRpc + iRpc][i] = 1000.+500.*i;	
-	}
-*/
-        int32_t iNbChan = fDigiBdfPar->GetNbChan(iSmType, iRpc);
-        fvCPTOff[iSmType][iSm * iNbRpc + iRpc].resize(iNbChan);
-        fvCPTotGain[iSmType][iSm * iNbRpc + iRpc].resize(iNbChan);
-        fvCPTotOff[iSmType][iSm * iNbRpc + iRpc].resize(iNbChan);
-        fvCPWalk[iSmType][iSm * iNbRpc + iRpc].resize(iNbChan);
-        int32_t nbSide = 2 - fDigiBdfPar->GetChanType(iSmType, iRpc);
-        for (int32_t iCh = 0; iCh < iNbChan; iCh++) {
-          fvCPTOff[iSmType][iSm * iNbRpc + iRpc][iCh].resize(nbSide);
-          fvCPTotGain[iSmType][iSm * iNbRpc + iRpc][iCh].resize(nbSide);
-          fvCPTotOff[iSmType][iSm * iNbRpc + iRpc][iCh].resize(nbSide);
-          fvCPWalk[iSmType][iSm * iNbRpc + iRpc][iCh].resize(nbSide);
-          for (int32_t iSide = 0; iSide < nbSide; iSide++) {
-            fvCPTOff[iSmType][iSm * iNbRpc + iRpc][iCh][iSide]    = 0.;  //initialize
-            fvCPTotGain[iSmType][iSm * iNbRpc + iRpc][iCh][iSide] = 1.;  //initialize
-            fvCPTotOff[iSmType][iSm * iNbRpc + iRpc][iCh][iSide]  = 0.;  //initialize
-            fvCPWalk[iSmType][iSm * iNbRpc + iRpc][iCh][iSide].resize(nbClWalkBinX);
-            for (int32_t iWx = 0; iWx < nbClWalkBinX; iWx++) {
-              fvCPWalk[iSmType][iSm * iNbRpc + iRpc][iCh][iSide][iWx] = 0.;
-            }
-          }
-        }
-      }
-    }
-  }
-  LOG(info) << "CbmTaskTofClusterizer::InitCalibParameter: defaults set";
-
-  if (fCalMode <= 0) { return true; }  // Skip calibration from histograms in mode zero
-
-  /// Save old global file and folder pointer to avoid messing with FairRoot
-  // <= To prevent histos from being sucked in by the param file of the TRootManager!
-  TFile* oldFile     = gFile;
-  TDirectory* oldDir = gDirectory;
-
-  LOG(info) << "CbmTaskTofClusterizer::InitCalibParameter: read histos from "
-            << "file " << fCalParFileName;
-
-  // read parameter from histos
-  if (fCalParFileName.IsNull()) return true;
-
-  TFile* calParFile = new TFile(fCalParFileName, "READ");
-  if (NULL == calParFile) {
-    if (fCalMode % 10 == 9) {  //modify reference file name
-      int iCalMode              = fCalParFileName.Index("tofClust") - 3;
-      fCalParFileName(iCalMode) = '3';
-      LOG(info) << "Modified CalFileName = " << fCalParFileName;
-      calParFile = new TFile(fCalParFileName, "update");
-      if (NULL == calParFile)
-        LOG(fatal) << "CbmTaskTofClusterizer::InitCalibParameter: "
-                   << "file " << fCalParFileName << " does not exist!";
-
-      return true;
-    }
-    LOG(fatal) << "Calibration parameter file not existing!";
-  }
-
-  for (int32_t iSmType = 0; iSmType < iNbSmTypes; iSmType++) {
-    int32_t iNbSm   = fDigiBdfPar->GetNbSm(iSmType);
-    int32_t iNbRpc  = fDigiBdfPar->GetNbRpc(iSmType);
-    TProfile* hSvel = (TProfile*) gDirectory->FindObjectAny(Form("cl_SmT%01d_Svel", iSmType));
-    for (int32_t iSm = 0; iSm < iNbSm; iSm++)
-      for (int32_t iRpc = 0; iRpc < iNbRpc; iRpc++) {
-
-        std::vector<std::vector<double>>& vCPTotGain           = fvCPTotGain[iSmType][iSm * iNbRpc + iRpc];
-        std::vector<std::vector<double>>& vCPTOff              = fvCPTOff[iSmType][iSm * iNbRpc + iRpc];
-        std::vector<std::vector<double>>& vCPTotOff            = fvCPTotOff[iSmType][iSm * iNbRpc + iRpc];
-        std::vector<std::vector<std::vector<double>>>& vCPWalk = fvCPWalk[iSmType][iSm * iNbRpc + iRpc];
-
-        std::vector<double>& vCPTOffY = fvCPTOffY[iSmType][iSm * iNbRpc + iRpc];
-        double& vCPTOffYBinWidth      = fvCPTOffYBinWidth[iSmType][iSm * iNbRpc + iRpc];
-        double& vCPTOffYRange         = fvCPTOffYRange[iSmType][iSm * iNbRpc + iRpc];
-
-        // update default parameter
-        if (NULL != hSvel) {
-          double Vscal = hSvel->GetBinContent(iSm * iNbRpc + iRpc + 1);
-          if (Vscal == 0.) Vscal = 1.;
-          Vscal *= fdModifySigvel;  //1.03; // testing the effect of wrong signal velocity, FIXME
-          fDigiBdfPar->SetSigVel(iSmType, iSm, iRpc, fDigiBdfPar->GetSigVel(iSmType, iSm, iRpc) * Vscal);
-          LOG(info) << "Modify " << iSmType << iSm << iRpc << " Svel by " << Vscal << " to "
-                    << fDigiBdfPar->GetSigVel(iSmType, iSm, iRpc);
-        }
-        TH2D* htempPos_pfx =
-          (TH2D*) gDirectory->FindObjectAny(Form("cl_CorSmT%01d_sm%03d_rpc%03d_Pos_pfx", iSmType, iSm, iRpc));
-        TH2D* htempTOff_pfx =
-          (TH2D*) gDirectory->FindObjectAny(Form("cl_CorSmT%01d_sm%03d_rpc%03d_TOff_pfx", iSmType, iSm, iRpc));
-        TH1D* htempTot_Mean =
-          (TH1D*) gDirectory->FindObjectAny(Form("cl_CorSmT%01d_sm%03d_rpc%03d_Tot_Mean", iSmType, iSm, iRpc));
-        TH1D* htempTot_Off =
-          (TH1D*) gDirectory->FindObjectAny(Form("cl_CorSmT%01d_sm%03d_rpc%03d_Tot_Off", iSmType, iSm, iRpc));
-        if (NULL != htempPos_pfx && NULL != htempTOff_pfx && NULL != htempTot_Mean && NULL != htempTot_Off) {
-          int32_t iNbCh = fDigiBdfPar->GetNbChan(iSmType, iRpc);
-          //int32_t iNbinTot = htempTot_Mean->GetNbinsX();//not used any more
-          for (int32_t iCh = 0; iCh < iNbCh; iCh++) {
-            for (int32_t iSide = 0; iSide < 2; iSide++) {
-              double TotMean = htempTot_Mean->GetBinContent(iCh * 2 + 1 + iSide);  //nh +1 empirical(?)
-              if (0.001 < TotMean) { vCPTotGain[iCh][iSide] *= fdTTotMean / TotMean; }
-              vCPTotOff[iCh][iSide] = htempTot_Off->GetBinContent(iCh * 2 + 1 + iSide);
-            }
-            double YMean = ((TProfile*) htempPos_pfx)->GetBinContent(iCh + 1);  //nh +1 empirical(?)
-            double TMean = ((TProfile*) htempTOff_pfx)->GetBinContent(iCh + 1);
-            if (std::isnan(YMean) || std::isnan(TMean)) {
-              LOG(warn) << "Invalid calibration for TSRC " << iSmType << iSm << iRpc << iCh << ", use default!";
-              continue;
-            }
-            double dTYOff = YMean / fDigiBdfPar->GetSigVel(iSmType, iSm, iRpc);
-            if (5 == iSmType || 8 == iSmType) dTYOff = 0.;  // no valid Y positon for pad counters
-            vCPTOff[iCh][0] += -dTYOff + TMean;
-            vCPTOff[iCh][1] += +dTYOff + TMean;
-
-            if (5 == iSmType || 8 == iSmType) {  // for PAD counters
-              vCPTOff[iCh][1]    = vCPTOff[iCh][0];
-              vCPTotGain[iCh][1] = vCPTotGain[iCh][0];
-              vCPTotOff[iCh][1]  = vCPTotOff[iCh][0];
-            }
-            //if(iSmType==0 && iSm==4 && iRpc==2 && iCh==26)
-            if (iSmType == 0 && iSm == 2 && iRpc == 0)
-              //if (iSmType == 9 && iSm == 0 && iRpc == 0 && iCh == 10)  // select specific channel
-              LOG(info) << "InitCalibParameter:"
-                        << " TSRC " << iSmType << iSm << iRpc << iCh
-                        << Form(": YMean %6.3f, TYOff %6.3f, TMean %6.3f", YMean, dTYOff, TMean) << " -> "
-                        << Form(" TOff %f, %f, TotG %f, %f ", vCPTOff[iCh][0], vCPTOff[iCh][1], vCPTotGain[iCh][0],
-                                vCPTotGain[iCh][1]);
-
-            TH1D* htempWalk0 = (TH1D*) gDirectory->FindObjectAny(
-              Form("Cor_SmT%01d_sm%03d_rpc%03d_Ch%03d_S0_Walk_px", iSmType, iSm, iRpc, iCh));
-            TH1D* htempWalk1 = (TH1D*) gDirectory->FindObjectAny(
-              Form("Cor_SmT%01d_sm%03d_rpc%03d_Ch%03d_S1_Walk_px", iSmType, iSm, iRpc, iCh));
-            if (NULL == htempWalk0 && NULL == htempWalk1) {  // regenerate Walk histos
-              int iSide  = 0;
-              htempWalk0 = new TH1D(Form("Cor_SmT%01d_sm%03d_rpc%03d_Ch%03d_S0_Walk_px", iSmType, iSm, iRpc, iCh),
-                                    Form("Walk in SmT%01d_sm%03d_rpc%03d_Ch%03d_S%01d_Walk; Tot [a.u.];  #DeltaT [ns]",
-                                         iSmType, iSm, iRpc, iCh, iSide),
-                                    nbClWalkBinX, fdTOTMin, fdTOTMax);
-              iSide      = 1;
-              htempWalk1 = new TH1D(Form("Cor_SmT%01d_sm%03d_rpc%03d_Ch%03d_S0_Walk_px", iSmType, iSm, iRpc, iCh),
-                                    Form("Walk in SmT%01d_sm%03d_rpc%03d_Ch%03d_S%01d_Walk; Tot [a.u.];  #DeltaT [ns]",
-                                         iSmType, iSm, iRpc, iCh, iSide),
-                                    nbClWalkBinX, fdTOTMin, fdTOTMax);
-            }
-            if (NULL != htempWalk0 && NULL != htempWalk1) {  // reinitialize Walk array
-              LOG(debug) << "Initialize Walk correction for "
-                         << Form(" SmT%01d_sm%03d_rpc%03d_Ch%03d", iSmType, iSm, iRpc, iCh);
-              if (htempWalk0->GetNbinsX() != nbClWalkBinX)
-                LOG(error) << "CbmTaskTofClusterizer::InitCalibParameter: "
-                              "Inconsistent Walk histograms";
-              for (int32_t iBin = 0; iBin < nbClWalkBinX; iBin++) {
-                vCPWalk[iCh][0][iBin] = htempWalk0->GetBinContent(iBin + 1);
-                vCPWalk[iCh][1][iBin] = htempWalk1->GetBinContent(iBin + 1);
-                if (iSmType == 0 && iSm == 0 && iRpc == 2 && iCh == 15)  // debugging
-                  LOG(info) << Form("Read new SmT%01d_sm%03d_rpc%03d_Ch%03d bin %d cen %f walk %f %f", iSmType, iSm,
-                                    iRpc, iCh, iBin, htempWalk0->GetBinCenter(iBin + 1), vCPWalk[iCh][0][iBin],
-                                    vCPWalk[iCh][1][iBin]);
-                if (5 == iSmType || 8 == iSmType) {  // Pad structure, enforce consistency
-                  if (vCPWalk[iCh][1][iBin] != vCPWalk[iCh][0][iBin]) {
-                    LOG(fatal) << "Inconsisten walk values for TSRC " << iSmType << iSm << iRpc << iCh << ", Bin "
-                               << iBin << ": " << vCPWalk[iCh][0][iBin] << ", " << vCPWalk[iCh][1][iBin];
-                  }
-                  vCPWalk[iCh][1][iBin] = vCPWalk[iCh][0][iBin];
-                  htempWalk1->SetBinContent(iBin + 1, vCPWalk[iCh][1][iBin]);
-                }
-              }
-            }
-            else {
-              LOG(info) << "No Walk histograms for TSRC " << iSmType << iSm << iRpc << iCh;
-            }
-          }
-          // look for TcorY corrections
-          LOG(info) << "Check for TCorY in " << gDirectory->GetName();
-          TH1* hTCorY =
-            (TH1*) gDirectory->FindObjectAny(Form("calXY_SmT%d_sm%03d_rpc%03d_TOff_z_all_TcorY", iSmType, iSm, iRpc));
-          if (NULL != hTCorY) {
-            vCPTOffYBinWidth = hTCorY->GetBinWidth(0);
-            vCPTOffYRange    = hTCorY->GetXaxis()->GetXmax();
-            LOG(info) << "Initialize TCorY: TSR " << iSmType << iSm << iRpc << ", Bwid " << vCPTOffYBinWidth
-                      << ", Range " << vCPTOffYRange;
-            vCPTOffY.resize(hTCorY->GetNbinsX());
-            for (int iB = 0; iB < hTCorY->GetNbinsX(); iB++) {
-              vCPTOffY[iB] = hTCorY->GetBinContent(iB + 1);
-            }
-          }
-        }
-        else {
-          LOG(warning) << " Calibration histos " << Form("cl_SmT%01d_sm%03d_rpc%03d_XXX", iSmType, iSm, iRpc)
-                       << " not found. ";
-        }
-      }
-  }
 
-  /// Restore old global file and folder pointer to avoid messing with FairRoot
-  // <= To prevent histos from being sucked in by the param file of the TRootManager!
-  gFile      = oldFile;
-  gDirectory = oldDir;
-  LOG(info) << "CbmTaskTofClusterizer::InitCalibParameter: initialization done";
-  return true;
-}
 
 bool CbmTaskTofClusterizer::InitAlgos()
 {
-  // Needed as external TOT values might be different than ones used for input histo reading (TO DO: FIX)
-  if (fTotMax != 0.) fdTOTMax = fTotMax;
-  if (fTotMin != 0.) fdTOTMin = fTotMin;
-  LOG(info) << "ToT init to Min " << fdTOTMin << " Max " << fdTOTMax;
-
-  /// Go to Top volume of the geometry in the GeoManager to make sure our nodes are found
-  gGeoManager->CdTop();
-
-  int32_t iNbSmTypes = fDigiBdfPar->GetNbSmTypes();
-
-  //Prepare storage vectors
-  fStorDigi.resize(iNbSmTypes);
-  for (int32_t iSmType = 0; iSmType < iNbSmTypes; iSmType++) {
-    int32_t iNbSm  = fDigiBdfPar->GetNbSm(iSmType);
-    int32_t iNbRpc = fDigiBdfPar->GetNbRpc(iSmType);
-    fStorDigi[iSmType].resize(iNbSm * iNbRpc);
-  }
-
-  // Create map with unique detector IDs and fill (needed only for dead strip array)
-  std::map<uint32_t, uint32_t> detIdIndexMap;
-  for (int32_t ind = 0; ind < fDigiBdfPar->GetNbDet(); ind++) {
-    int32_t iUniqueId        = fDigiBdfPar->GetDetUId(ind);
-    detIdIndexMap[iUniqueId] = ind;
-  }
-
-  // Create one algorithm per RPC and configure it with parameters
-  for (int32_t iSmType = 0; iSmType < iNbSmTypes; iSmType++) {
-    int32_t iNbSm  = fDigiBdfPar->GetNbSm(iSmType);
-    int32_t iNbRpc = fDigiBdfPar->GetNbRpc(iSmType);
-    for (int32_t iSm = 0; iSm < iNbSm; iSm++) {
-      for (int32_t iRpc = 0; iRpc < iNbRpc; iRpc++) {
-        std::unique_ptr<cbm::algo::tof::ClusterizerRpcPar> par(new cbm::algo::tof::ClusterizerRpcPar());
-
-        const int32_t iDetId   = CbmTofAddress::GetUniqueAddress(iSm, iRpc, 0, 0, iSmType);
-        const int32_t iDetIndx = detIdIndexMap[iDetId];
-        par->fDeadStrips       = fvDeadStrips[iDetIndx];
-        par->fPosYMaxScal      = fPosYMaxScal;
-        par->fdMaxTimeDist     = fdMaxTimeDist;
-        par->fdMaxSpaceDist    = fdMaxSpaceDist;
-        par->fCPTOffYBinWidth  = fvCPTOffYBinWidth[iSmType][iSm * iNbRpc + iRpc];
-        par->fCPTOffY          = fvCPTOffY[iSmType][iSm * iNbRpc + iRpc];
-        par->fCPTOffYRange     = fvCPTOffYRange[iSmType][iSm * iNbRpc + iRpc];
-        par->fSigVel           = fDigiBdfPar->GetSigVel(iSmType, iSm, iRpc);
-        par->fTimeRes          = 0.08;
-        par->numClWalkBinX     = nbClWalkBinX;
-        par->TOTMax            = fdTOTMax;
-        par->TOTMin            = fdTOTMin;
-        par->swapChannelSides  = fbSwapChannelSides;
-        par->channelDeadtime   = fdChannelDeadtime;
-
-        int32_t iNbChan = fDigiBdfPar->GetNbChan(iSmType, iRpc);
-        par->fChanPar.resize(iNbChan);
-        for (int32_t iCh = 0; iCh < iNbChan; iCh++) {
-
-          const int32_t address   = CbmTofAddress::GetUniqueAddress(iSm, iRpc, iCh, 0, iSmType);
-          CbmTofCell* channelInfo = fDigiPar->GetCell(address);
-          if (channelInfo == nullptr) { continue; }  //D.Smith 17.8.23: Needed as channel info sometimes not defined
-
-          gGeoManager->FindNode(channelInfo->GetX(), channelInfo->GetY(), channelInfo->GetZ());
-          const double* tra_ptr = gGeoManager->MakePhysicalNode()->GetMatrix()->GetTranslation();
-
-          //init Tof cell
-          cbm::algo::tof::TofCell& cell = par->fChanPar[iCh].cell;
-          cell.pos                      = ROOT::Math::XYZVector(tra_ptr[0], tra_ptr[1], tra_ptr[2]);
-
-          /*  why doesn't this variant work?
-          cell.pos.SetX(channelInfo->GetX());
-          cell.pos.SetY(channelInfo->GetY());
-          cell.pos.SetZ(channelInfo->GetZ());
-*/
-          cell.sizeX = channelInfo->GetSizex();
-          cell.sizeY = channelInfo->GetSizey();
-
-          // prepare local->global trafo
-          gGeoManager->FindNode(channelInfo->GetX(), channelInfo->GetY(), channelInfo->GetZ());
-          double* rot_ptr = gGeoManager->GetCurrentMatrix()->GetRotationMatrix();
-          cell.rotation   = ROOT::Math::Rotation3D(&rot_ptr[0], &rot_ptr[9]);
-
-          par->fChanPar[iCh].fvCPTOff    = fvCPTOff[iSmType][iSm * iNbRpc + iRpc][iCh];
-          par->fChanPar[iCh].fvCPTotGain = fvCPTotGain[iSmType][iSm * iNbRpc + iRpc][iCh];
-          par->fChanPar[iCh].fvCPTotOff  = fvCPTotOff[iSmType][iSm * iNbRpc + iRpc][iCh];
-          par->fChanPar[iCh].fvCPWalk    = fvCPWalk[iSmType][iSm * iNbRpc + iRpc][iCh];
-          par->fChanPar[iCh].address     = address;
-        }
-        fAlgo[iSmType][iSm * iNbRpc + iRpc].SetParams(std::move(par));
-      }
-    }
-  }
+  // Read hitfinder parameters and initialize algo
+  cbm::algo::fs::path paramsPath = "TofHitfinderPar.yaml";
+  YAML::Node yaml                = YAML::LoadFile(paramsPath.string());
+  fAlgo.fTofConfig               = cbm::algo::config::Read<cbm::algo::tof::HitfindSetup>(yaml);
+  fAlgo.Init();
   return true;
 }
 
@@ -758,19 +321,15 @@ bool CbmTaskTofClusterizer::InitAlgos()
 /************************************************************************************/
 bool CbmTaskTofClusterizer::BuildClusters()
 {
-  gGeoManager->CdTop();
-
   if (fTofDigiVec.empty()) {
     LOG(info) << " No RawDigis defined ! Check! ";
     return false;
   }
   LOG(info) << "Build clusters from " << fTofDigiVec.size() << " digis in event " << fdEvent + 1;
 
-  int32_t iNbTofDigi = fTofDigiVec.size();
-
   if (bAddBeamCounterSideDigi) {
     // Duplicate type "5" - digis
-    for (int32_t iDigInd = 0; iDigInd < iNbTofDigi; iDigInd++) {
+    for (int32_t iDigInd = 0; iDigInd < fTofDigiVec.size(); iDigInd++) {
       CbmTofDigi* pDigi = &(fTofDigiVec.at(iDigInd));
       if (pDigi->GetType() == 5) {  // || pDigi->GetType() == 8) {
         if (pDigi->GetSide() == 1) {
@@ -786,164 +345,31 @@ bool CbmTaskTofClusterizer::BuildClusters()
                    << pDigiN->GetAddress();
       }
     }
-    iNbTofDigi = fTofDigiVec.size();
-  }
-
-  //D.Smith 10.8.23: This entire if(true){...} block doesn't seem to actually do anything
-  //for run 2391 data if enabled. Check with PA whether it can be dropped (might be obsolete).
-  //The original purpose was to correct missing hits, which might have been fixed.
-  if (true) {
-    for (int32_t iDigInd = 0; iDigInd < iNbTofDigi; iDigInd++) {
-      CbmTofDigi* pDigi   = &(fTofDigiVec.at(iDigInd));
-      const double SmType = pDigi->GetType();
-      const double Sm     = pDigi->GetSm();
-      const double Rpc    = pDigi->GetRpc();
-      const double Chan   = pDigi->GetChannel();
-      const double Side   = pDigi->GetSide();
-      int32_t iDetIndx    = fDigiBdfPar->GetDetInd(pDigi->GetAddress() & DetMask);
-
-      if (fDigiBdfPar->GetNbDet() - 1 < iDetIndx || iDetIndx < 0) {
-        LOG(debug) << Form(" Wrong DetIndx %d >< %d ", iDetIndx, fDigiBdfPar->GetNbDet());
-        break;
-      }
-      CbmTofDigi* pDigi2Min = NULL;
-      double dTDifMin       = dDoubleMax;
-
-      if (fDutId > -1) {
-        for (int32_t iDigI2 = 0; iDigI2 < iNbTofDigi; iDigI2++) {
-          CbmTofDigi* pDigi2   = &(fTofDigiVec.at(iDigI2));
-          const double SmType2 = pDigi2->GetType();
-          const double Sm2     = pDigi2->GetSm();
-          const double Rpc2    = pDigi2->GetRpc();
-          const double Chan2   = pDigi2->GetChannel();
-          const double Side2   = pDigi2->GetSide();
-
-          if (iDetIndx == fDigiBdfPar->GetDetInd(pDigi2->GetAddress())) {
-            if (Side != Side2) {
-              if (Chan == Chan2) {
-                double dTDif = TMath::Abs(pDigi->GetTime() - pDigi2->GetTime());
-                if (dTDif < dTDifMin) {
-                  dTDifMin  = dTDif;
-                  pDigi2Min = pDigi2;
-                }
-              }
-              else if (TMath::Abs(Chan - Chan2)
-                       == 1) {  // opposite side missing, neighbouring channel has hit on opposite side // FIXME
-                // check that same side digi of neighbouring channel is absent
-                int32_t iDigI3 = 0;
-                for (; iDigI3 < iNbTofDigi; iDigI3++) {
-                  CbmTofDigi* pDigi3 = &(fTofDigiVec.at(iDigI3));
-                  if (pDigi3->GetSide() == Side && Chan2 == pDigi3->GetChannel()) break;
-                }
-                if (iDigI3 == iNbTofDigi)  // same side neighbour did not fire
-                {
-                  int32_t iCorMode = 0;  // Missing hit correction mode
-                  switch (iCorMode) {
-                    case 0:  // no action
-                      break;
-                    case 1:  // shift found hit
-                      if (Side == 0) pDigi2->SetAddress(Sm, Rpc, Chan, 1 - Side, SmType);
-                      else
-                        pDigi->SetAddress(Sm2, Rpc2, Chan2, 1 - Side2, SmType2);
-
-                      break;
-                    case 2:  // insert missing hits
-                      fTofDigiVec.push_back(CbmTofDigi(*pDigi));
-                      CbmTofDigi* pDigiN = &(fTofDigiVec.back());
-                      pDigiN->SetAddress(Sm, Rpc, Chan2, Side, SmType);
-                      pDigiN->SetTot(pDigi2->GetTot());
-
-                      fTofDigiVec.push_back(CbmTofDigi(*pDigi2));
-                      CbmTofDigi* pDigiN2 = &(fTofDigiVec.back());
-                      pDigiN2->SetAddress(Sm2, Rpc2, Chan, Side2, SmType2);
-                      pDigiN2->SetTot(pDigi->GetTot());
-
-                      break;
-                  }
-                }
-              }
-            }
-          }
-        }
-      }
-
-      if (pDigi2Min != NULL) {
-        CbmTofDetectorInfo xDetInfo(ECbmModuleId::kTof, SmType, Sm, Rpc, 0, Chan);
-        int32_t iChId    = fTofId->SetDetectorInfo(xDetInfo);
-        CbmTofCell* cell = fDigiPar->GetCell(iChId);
-        if (NULL == cell) {
-          LOG(warning) << Form("BuildClusters: invalid ChannelInfo for 0x%08x", iChId);
-          continue;
-        }
-        if (fDigiBdfPar->GetSigVel(SmType, Sm, Rpc) * dTDifMin * 0.5 < fPosYMaxScal * cell->GetSizey()) {
-          //check consistency
-          if (8 == SmType || 5 == SmType) {
-            if (pDigi->GetTime() != pDigi2Min->GetTime()) {
-              LOG(warning) << " BuildClusters: Inconsistent duplicated digis in event " << fdEvent + 1
-                           << ", Ind: " << iDigInd;  // << "CTyp: " << pDigi->GetCounterType;
-              LOG(warning) << "   " << pDigi->ToString();
-              LOG(warning) << "   " << pDigi2Min->ToString();
-
-              pDigi2Min->SetTot(pDigi->GetTot());
-              pDigi2Min->SetTime(pDigi->GetTime());
-            }
-          }
-        }
-      }
-    }
-  }  // true end
-
-  // Calibrate RawDigis
-  *fTofCalDigiVec = CalibRawDigis(fTofDigiVec);
-  //  *fTofCalDigiVec = fTofDigiVec;  //disable calibration for now (done in algo). Linked indices will differ!
-
-  // Then loop over the digis array and store the Digis in separate vectors for
-  // each RPC modules
-  for (int32_t iDigInd = 0; iDigInd < fTofCalDigiVec->size(); iDigInd++) {
-    CbmTofDigi* pDigi = &(fTofCalDigiVec->at(iDigInd));
-
-    // These are doubles in the digi class
-    const double SmType = pDigi->GetType();
-    const double Sm     = pDigi->GetSm();
-    const double Rpc    = pDigi->GetRpc();
-    const int NbRpc     = fDigiBdfPar->GetNbRpc(SmType);
-    fStorDigi[SmType][Sm * NbRpc + Rpc].push_back(std::make_pair(pDigi, iDigInd));
   }
-  // inspect digi array (D.Smith: Block removed for cleanup. Could be in monitoring class in principle.)
 
-  for (uint32_t iSmType = 0; iSmType < fDigiBdfPar->GetNbSmTypes(); iSmType++) {
-    const uint32_t iNbSm  = fDigiBdfPar->GetNbSm(iSmType);
-    const uint32_t iNbRpc = fDigiBdfPar->GetNbRpc(iSmType);
-    for (uint32_t iSm = 0; iSm < iNbSm; iSm++) {
-      for (uint32_t iRpc = 0; iRpc < iNbRpc; iRpc++) {
-        //read digis
-        const uint32_t rpcIdx                                 = iSm * iNbRpc + iRpc;
-        std::vector<std::pair<CbmTofDigi*, int32_t>>& digiExp = fStorDigi[iSmType][rpcIdx];
-
-        //call cluster finder
-        std::vector<cbm::algo::tof::TofCluster> clusters = fAlgo[iSmType][rpcIdx](digiExp);
-
-        //Store hits and match
-        for (auto const& cluster : clusters) {
-          const int32_t hitIndex = fTofHitsColl->GetEntriesFast();
-          TVector3 hitpos        = TVector3(cluster.globalPos.X(), cluster.globalPos.Y(), cluster.globalPos.Z());
-          TVector3 hiterr        = TVector3(cluster.globalErr.X(), cluster.globalErr.Y(), cluster.globalErr.Z());
-          new ((*fTofHitsColl)[hitIndex])
-            CbmTofHit(cluster.detId, hitpos, hiterr, fiNbHits, cluster.weightedTime, cluster.weightedTimeErr,
-                      cluster.vDigiIndRef.size(), int32_t(cluster.weightsSum * 10.));
-
-          fiNbHits++;
-          //if (event) event->AddData(ECbmDataType::kTofHit, hitIndex);
-
-          CbmMatch* digiMatch = new ((*fTofDigiMatchColl)[hitIndex]) CbmMatch();
-          for (uint32_t i = 0; i < cluster.vDigiIndRef.size(); i++) {
-            double tot = fTofCalDigiVec->at(cluster.vDigiIndRef.at(i)).GetTot();
-            digiMatch->AddLink(CbmLink(tot, cluster.vDigiIndRef.at(i), fiOutputTreeEntry, fiFileIndex));
-          }
-        }
-        // digiExp.clear();
-        // digiInd.clear();
-      }
+  // Make copy for calibrated digis (calibration done in algo)
+  // Can be left out if calibrated digis not stored separately!
+  *fTofCalDigiVec = fTofDigiVec;
+
+  //call cluster finder
+  std::vector<cbm::algo::tof::TofCluster> clusters = fAlgo(fTofCalDigiVec).first;
+
+  //Store hits and match
+  for (auto const& cluster : clusters) {
+    const int32_t hitIndex = fTofHitsColl->GetEntriesFast();
+    TVector3 hitpos        = TVector3(cluster.globalPos.X(), cluster.globalPos.Y(), cluster.globalPos.Z());
+    TVector3 hiterr        = TVector3(cluster.globalErr.X(), cluster.globalErr.Y(), cluster.globalErr.Z());
+    new ((*fTofHitsColl)[hitIndex])
+      CbmTofHit(cluster.detId, hitpos, hiterr, fiNbHits, cluster.weightedTime, cluster.weightedTimeErr,
+                cluster.vDigiIndRef.size(), int32_t(cluster.weightsSum * 10.));
+
+    fiNbHits++;
+    //if (event) event->AddData(ECbmDataType::kTofHit, hitIndex);
+
+    CbmMatch* digiMatch = new ((*fTofDigiMatchColl)[hitIndex]) CbmMatch();
+    for (uint32_t i = 0; i < cluster.vDigiIndRef.size(); i++) {
+      double tot = fTofCalDigiVec->at(cluster.vDigiIndRef.at(i)).GetTot();
+      digiMatch->AddLink(CbmLink(tot, cluster.vDigiIndRef.at(i), fiOutputTreeEntry, fiFileIndex));
     }
   }
   std::cout << "hits " << fiNbHits << std::endl;
@@ -951,132 +377,3 @@ bool CbmTaskTofClusterizer::BuildClusters()
   fdEvent++;
   return true;
 }
-
-/* No longer used. Keeping for reference / discussion of comments
-void CbmTaskTofClusterizer::StoreHit(TofCluster& cluster, int32_t iSmType, int32_t iChId, int32_t iNbCh, int32_t iSm,
-                                      int32_t iRpc)
-{
-  double* tra_ptr = fCellIdGeoMap[iChId]->GetMatrix()->GetTranslation();
-  double* rot_ptr = fCellIdGeoMap[iChId]->GetMatrix()->GetRotationMatrix();
-
-  tof::TofCell cell;
-  cell.pos      = ROOT::Math::XYZVector(tra_ptr[0], tra_ptr[1], tra_ptr[2]);
-  cell.rotation = ROOT::Math::Rotation3D(&rot_ptr[0], &rot_ptr[9]);
-  cell.sizeX    = fDigiPar->GetCell(iChId)->GetSizex();
-
-  //D.Smith 10.8.23: Replacing this with the lines above seems to work, although the exact
-  //cell might be a different one when called from AddNextChan().
-  //int32_t iChm = floor(cluster.weightedPos.X() / fChannelInfo->GetSizex()) + iNbCh / 2;
-
-  //D.Smith 15.8.23: Can we drop the condition?
-  if (5 != iSmType) { cluster.finalize(cell, iNbCh, tof::ClusterizerRpcPar()); }
-
-  TVector3 hitPos(cluster.globalPos.X(), cluster.globalPos.Y(), cluster.globalPos.Z());
-  TVector3 hitPosErr(cluster.globalErr.X(), cluster.globalErr.Y(), cluster.globalErr.Z());
-
-  const int32_t iDetId = CbmTofAddress::GetUniqueAddress(iSm, iRpc, cluster.channel, 0, iSmType);
-
-  if (cluster.vDigiIndRef.size() < 2) {
-    LOG(warning) << "Digi refs for Hit " << fiNbHits << ":        vDigiIndRef.size()";
-  }
-  CbmTofHit* pHit = new CbmTofHit(iDetId, hitPos,
-                                  hitPosErr,  //local detector coordinates
-                                  fiNbHits,   // this number is used as reference!!
-                                  cluster.weightedTime,
-                                  cluster.vDigiIndRef.size(),          // number of linked digis =  2*CluSize
-                                  int32_t(cluster.weightsSum * 10.));  //channel -> Tot
-  pHit->SetTimeError(dTimeRes);
-
-  // output hit
-  new ((*fTofHitsColl)[fiNbHits]) CbmTofHit(*pHit);
-  pHit->Delete();
-
-  CbmMatch* digiMatch = new ((*fTofDigiMatchColl)[fiNbHits]) CbmMatch();
-
-  for (size_t i = 0; i < cluster.vDigiIndRef.size(); i++) {
-    double dTot = fTofCalDigiVec->at(cluster.vDigiIndRef.at(i)).GetTot();
-    digiMatch->AddLink(CbmLink(dTot, cluster.vDigiIndRef.at(i), fiOutputTreeEntry, fiFileIndex));
-  }
-  fiNbHits++;
-}
-*/
-
-std::vector<CbmTofDigi> CbmTaskTofClusterizer::CalibRawDigis(const std::vector<CbmTofDigi>& digiVec)
-{
-  // channel deadtime map
-  std::map<int32_t, double> mChannelDeadTime;
-  std::vector<CbmTofDigi> calDigiVecOut;
-
-  for (int32_t iDigi = 0; iDigi < digiVec.size(); iDigi++) {
-
-    CbmTofDigi pDigi      = digiVec.at(iDigi);
-    const double SmType   = pDigi.GetType();
-    const double Sm       = pDigi.GetSm();
-    const double Rpc      = pDigi.GetRpc();
-    const double Chan     = pDigi.GetChannel();
-    const double Side     = pDigi.GetSide();
-    const int NbRpc       = fDigiBdfPar->GetNbRpc(SmType);
-    const uint32_t rpcIdx = Sm * NbRpc + Rpc;
-
-    if (fbSwapChannelSides && 5 != SmType && 8 != SmType) {
-      pDigi.SetAddress(Sm, Rpc, Chan, (0 == Side) ? 1 : 0, SmType);
-    }
-
-    // Check dead time
-    const int32_t iAddr = pDigi.GetAddress();
-    auto it             = mChannelDeadTime.find(iAddr);
-    if (it != mChannelDeadTime.end() && pDigi.GetTime() <= it->second) {
-      it->second = pDigi.GetTime() + fdChannelDeadtime;
-      continue;
-    }
-    mChannelDeadTime[iAddr] = pDigi.GetTime() + fdChannelDeadtime;
-
-    // Create calibrated digi
-    CbmTofDigi pCalDigi(pDigi);
-
-    // calibrate Digi Time
-    pCalDigi.SetTime(pCalDigi.GetTime() - fvCPTOff[SmType][rpcIdx][Chan][Side]);
-
-    // subtract Offset
-    double dTot = pCalDigi.GetTot() - fvCPTotOff[SmType][rpcIdx][Chan][Side];
-    if (dTot < 0.001) dTot = 0.001;
-
-    // calibrate Digi ToT
-    pCalDigi.SetTot(dTot * fvCPTotGain[SmType][rpcIdx][Chan][Side]);
-
-    // walk correction
-    std::vector<double>& walk = fvCPWalk[SmType][rpcIdx][Chan][Side];
-    const double dTotBinSize  = (fdTOTMax - fdTOTMin) / nbClWalkBinX;
-    int32_t iWx               = (int32_t)((pCalDigi.GetTot() - fdTOTMin) / dTotBinSize);
-
-    if (0 > iWx) { iWx = 0; }
-    if (iWx >= nbClWalkBinX) { iWx = nbClWalkBinX - 1; }
-
-    const double dDTot = (pCalDigi.GetTot() - fdTOTMin) / dTotBinSize - (double) iWx - 0.5;
-    double dWT         = walk[iWx];
-
-    // linear interpolation to next bin   //memory leak??? (D.Smith 10.8.23: Clarify this comment!)
-    if (dDTot > 0) {
-      if (iWx < nbClWalkBinX - 1) { dWT += dDTot * (walk[iWx + 1] - walk[iWx]); }
-    }
-    else {
-      if (0 < iWx) { dWT -= dDTot * (walk[iWx - 1] - walk[iWx]); }
-    }
-    pCalDigi.SetTime(pCalDigi.GetTime() - dWT);  // calibrate Digi Time
-    calDigiVecOut.push_back(pCalDigi);
-  }
-
-  // D.Smith 10.8.23: Sorting might not be needed. Check with P.A.
-  LOG(debug) << "CbmTaskTofClusterizer::BuildClusters: Sort " << calDigiVecOut.size() << " calibrated digis ";
-  /// Sort the buffers of hits due to the time offsets applied
-  std::sort(calDigiVecOut.begin(), calDigiVecOut.end(),
-            [](const CbmTofDigi& a, const CbmTofDigi& b) -> bool { return a.GetTime() < b.GetTime(); });
-
-  return calDigiVecOut;
-}
-
-void CbmTaskTofClusterizer::SetDeadStrips(int32_t iDet, uint32_t ival)
-{
-  if (fvDeadStrips.size() < static_cast<size_t>(iDet + 1)) fvDeadStrips.resize(iDet + 1);
-  fvDeadStrips[iDet] = ival;
-}
diff --git a/reco/tasks/CbmTaskTofClusterizer.h b/reco/tasks/CbmTaskTofClusterizer.h
index e599f6dd7f4fe1366e17dac06f4bc6f6f4faa522..04a26e1ca3ed14ffc317948630174cd8e21a4803 100644
--- a/reco/tasks/CbmTaskTofClusterizer.h
+++ b/reco/tasks/CbmTaskTofClusterizer.h
@@ -8,11 +8,6 @@
 // TOF Classes and includes
 // Input/Output
 class CbmEvent;
-// Geometry
-class CbmTofDetectorId;
-class CbmTofDigiPar;
-class CbmTofDigiBdfPar;
-class CbmTofCell;
 class CbmDigiManager;
 class CbmTsEventHeader;
 
@@ -20,6 +15,7 @@ class CbmTsEventHeader;
 #include "CbmTofDigi.h"
 
 #include "tof/Clusterizer.h"
+#include "tof/Hitfind.h"
 
 // ROOT Classes and includes
 #include "Math/Rotation3D.h"
@@ -59,7 +55,7 @@ public:
   /**
        ** @brief Inherited from FairTask.
        **/
-  virtual void SetParContainers();
+  virtual void SetParContainers() {};
 
   /**
        ** @brief Inherited from FairTask.
@@ -73,19 +69,19 @@ public:
   virtual void Finish();
   virtual void Finish(double calMode);
 
-  inline void SetCalMode(int32_t iMode) { fCalMode = iMode; }
-  inline void SetDutId(int32_t Id) { fDutId = Id; }
-
-  inline void PosYMaxScal(double val) { fPosYMaxScal = val; }
-  inline void SetTotMax(double val) { fTotMax = val; }
-  inline void SetTotMin(double val) { fTotMin = val; }
-  inline void SetTotMean(double val) { fTotMean = val; }
-  inline void SetMaxTimeDist(double val) { fMaxTimeDist = val; }
-  inline void SetChannelDeadtime(double val) { fdChannelDeadtime = val; }
-  inline void SetCalParFileName(TString CalParFileName) { fCalParFileName = CalParFileName; }
-
+  ///////////// Empty functions for interface compatibility
+  inline void SetCalMode(int32_t iMode) {}
+  inline void SetDutId(int32_t Id) {}
+  inline void PosYMaxScal(double val) {}
+  inline void SetTotMax(double val) {}
+  inline void SetTotMin(double val) {}
+  inline void SetTotMean(double val) {}
+  inline void SetMaxTimeDist(double val) {}
+  inline void SetChannelDeadtime(double val) {}
+  inline void SetCalParFileName(TString CalParFileName) {}
+  inline double GetTotMean() { return 0.0; }
+  ////////////
   inline int GetNbHits() { return fiNbHits; }
-  inline double GetTotMean() { return fTotMean; }
 
   void SwapChannelSides(bool bSwap) { fbSwapChannelSides = bSwap; }
   void SetFileIndex(int32_t iIndex) { fiFileIndex = iIndex; }
@@ -98,10 +94,6 @@ private:
   int32_t iNbTs                = 0;
   int fiHitStart               = 0;
   bool bAddBeamCounterSideDigi = true;
-  const double dDoubleMax      = 1.E300;
-  const int32_t nbClWalkBinX   = 50;          // was 100 (11.10.2018)
-  const int32_t nbClWalkBinY   = 41;          // choose odd number to have central bin symmetric around 0
-  const int32_t DetMask        = 0x001FFFFF;  // geo v21a
 
   std::vector<CbmTofDigi>* fT0DigiVec = nullptr;  //! T0 Digis
 
@@ -124,19 +116,6 @@ private:
        ** @brief Create and register output TClonesArray of Tof Hits.
        **/
   bool RegisterOutputs();
-  /**
-       ** @brief Initialize other parameters not included in parameter classes.
-       **/
-  bool InitParameters();
-  /**
-       ** @brief Initialize other parameters not included in parameter classes.
-       **/
-  bool InitCalibParameter();
-
-  /**
-       ** @brief Apply calibration to digis
-       **/
-  std::vector<CbmTofDigi> CalibRawDigis(const std::vector<CbmTofDigi>& digiVec);
 
   /**
        ** @brief Build clusters out of ToF Digis and store the resulting info in a TofHit.
@@ -149,12 +128,7 @@ private:
   bool InitAlgos();
 
   // Hit finder algo
-  std::map<uint32_t, std::map<uint32_t, cbm::algo::tof::Clusterizer>> fAlgo = {};  //[nbType][nbSm*nbRpc]
-
-  // ToF geometry variables
-  CbmTofDetectorId* fTofId;
-  CbmTofDigiPar* fDigiPar;
-  CbmTofDigiBdfPar* fDigiBdfPar;
+  cbm::algo::tof::Hitfind fAlgo;
 
   const CbmTsEventHeader* fTsHeader;
 
@@ -174,45 +148,7 @@ private:
   TClonesArray* fTofDigiMatchCollOut;                    // TOF Digi Links
   int32_t fiNbHits;                                      // Index of the CbmTofHit TClonesArray
 
-  // Intermediate storage variables (digi, index)
-  std::vector<std::vector<std::vector<std::pair<CbmTofDigi*, int32_t>>>> fStorDigi;  //[nbType][nbSm*nbRpc][nDigis]
-
-  //Calibration parameters
-  std::vector<std::vector<std::vector<std::vector<double>>>> fvCPTOff;     //[nSMT][nRpc][nCh][nbSide]
-  std::vector<std::vector<std::vector<std::vector<double>>>> fvCPTotGain;  //[nSMT][nRpc][nCh][nbSide]
-  std::vector<std::vector<std::vector<std::vector<double>>>> fvCPTotOff;   //[nSMT][nRpc][nCh][nbSide]
-  std::vector<std::vector<std::vector<std::vector<std::vector<double>>>>>
-    fvCPWalk;                                               //[nSMT][nRpc][nCh][nbSide][nbWalkBins]
-  std::vector<std::vector<std::vector<double>>> fvCPTOffY;  //[nSMT][nRpc][nBin]
-  std::vector<std::vector<double>> fvCPTOffYBinWidth;       //[nSMT][nRpc]
-  std::vector<std::vector<double>> fvCPTOffYRange;          //[nSMT][nRpc]
-
-  std::vector<uint32_t> fvDeadStrips;  //[nbDet]
-
-  // Calib
-  int32_t fCalMode;
-  int32_t fDutId;
-
-  double fPosYMaxScal;
-  double fTotMax;
-  double fTotMin;
-  double fTotOff;
-  double fTotMean;
-  double fMaxTimeDist;
-  double fdChannelDeadtime;
-
-  TString fCalParFileName;  // name of the file name with Calibration Parameters
-
-  double fdTOTMax;
-  double fdTOTMin;
-  double fdTTotMean;
-
-  double fdMaxTimeDist;   // Isn't this just a local variable? Why make it global and preset?!?
-  double fdMaxSpaceDist;  // Isn't this just a local variable? Why make it global and preset?!?
-  double fdModifySigvel;
-
   double fdEvent;
-
   double fProcessTime = 0.0;
   uint64_t fuNbDigis  = 0;
   uint64_t fuNbHits   = 0;
diff --git a/reco/tasks/CbmTaskTofClusterizerParWrite.cxx b/reco/tasks/CbmTaskTofClusterizerParWrite.cxx
index cb8d8c202c873eb8923f04196b97659175e1629b..c1beee73fc251867f5cfd5ad40a932bf05a2dd4f 100644
--- a/reco/tasks/CbmTaskTofClusterizerParWrite.cxx
+++ b/reco/tasks/CbmTaskTofClusterizerParWrite.cxx
@@ -5,22 +5,15 @@
 #include "CbmTaskTofClusterizerParWrite.h"
 
 // TOF Classes and includes
-#include "CbmBmonDigi.h"  // in cbmdata/bmon
-#include "CbmDigiManager.h"
-#include "CbmEvent.h"
-#include "CbmMatch.h"
 #include "CbmTofAddress.h"          // in cbmdata/tof
 #include "CbmTofCell.h"             // in tof/TofData
 #include "CbmTofCreateDigiPar.h"    // in tof/TofTools
 #include "CbmTofDetectorId_v12b.h"  // in cbmdata/tof
 #include "CbmTofDetectorId_v14a.h"  // in cbmdata/tof
 #include "CbmTofDetectorId_v21a.h"  // in cbmdata/tof
-#include "CbmTofDigi.h"             // in cbmdata/tof
 #include "CbmTofDigiBdfPar.h"       // in tof/TofParam
 #include "CbmTofDigiPar.h"          // in tof/TofParam
 #include "CbmTofGeoHandler.h"       // in tof/TofTools
-#include "CbmTofHit.h"              // in cbmdata/tof
-#include "CbmTsEventHeader.h"
 
 // FAIR classes and includes
 #include "FairRootFileSink.h"
@@ -29,6 +22,10 @@
 #include "FairRuntimeDb.h"
 #include <Logger.h>
 
+#include "tof/HitfindSetup.h"
+
+#include "config/Yaml.h"
+
 // ROOT Classes and includes
 #include "TClonesArray.h"
 #include "TGeoManager.h"
@@ -43,28 +40,18 @@
 #include <iomanip>
 #include <vector>
 
-// Globals
-
 
 /************************************************************************************/
-CbmTaskTofClusterizerParWrite::CbmTaskTofClusterizerParWrite() : CbmTaskTofClusterizerParWrite("TestbeamClusterizer", 0, 0) {}
+CbmTaskTofClusterizerParWrite::CbmTaskTofClusterizerParWrite()
+  : CbmTaskTofClusterizerParWrite("TestbeamClusterizer", 0, 0)
+{
+}
 
 CbmTaskTofClusterizerParWrite::CbmTaskTofClusterizerParWrite(const char* name, int32_t verbose, bool writeDataInOut)
   : FairTask(TString(name), verbose)
   , fTofId(NULL)
   , fDigiPar(NULL)
   , fDigiBdfPar(NULL)
-  , fTsHeader(NULL)
-  , fDigiMan(nullptr)
-  , fEventsColl(nullptr)
-  , fbWriteHitsInOut(writeDataInOut)
-  , fbWriteDigisInOut(writeDataInOut)
-  , fTofHitsColl(NULL)
-  , fTofDigiMatchColl(NULL)
-  , fTofHitsCollOut(NULL)
-  , fTofDigiMatchCollOut(NULL)
-  , fiNbHits(0)
-  , fStorDigi()
   , fvCPTOff()
   , fvCPTotGain()
   , fvCPTotOff()
@@ -74,7 +61,6 @@ CbmTaskTofClusterizerParWrite::CbmTaskTofClusterizerParWrite(const char* name, i
   , fvCPTOffYRange()
   , fvDeadStrips()
   , fCalMode(0)
-  , fDutId(-1)
   , fPosYMaxScal(1.)
   , fTotMax(100.)
   , fTotMin(0.)
@@ -89,10 +75,7 @@ CbmTaskTofClusterizerParWrite::CbmTaskTofClusterizerParWrite(const char* name, i
   , fdMaxTimeDist(0.)
   , fdMaxSpaceDist(0.)
   , fdModifySigvel(1.0)
-  , fdEvent(0)
   , fbSwapChannelSides(false)
-  , fiOutputTreeEntry(0)
-  , fiFileIndex(0)
 {
 }
 
@@ -121,21 +104,8 @@ void CbmTaskTofClusterizerParWrite::SetParContainers()
   fDigiBdfPar = (CbmTofDigiBdfPar*) (rtdb->getContainer("CbmTofDigiBdfPar"));
 }
 
-void CbmTaskTofClusterizerParWrite::Exec(Option_t* option)
-{
-}
-
 
 /************************************************************************************/
-void CbmTaskTofClusterizerParWrite::Finish()
-{
-}
-
-void CbmTaskTofClusterizerParWrite::Finish(double calMode)
-{
-}
-
-
 bool CbmTaskTofClusterizerParWrite::InitParameters()
 {
 
@@ -179,7 +149,8 @@ bool CbmTaskTofClusterizerParWrite::InitParameters()
 
   rtdb->initContainers(ana->GetRunId());
 
-  LOG(info) << "CbmTaskTofClusterizerParWrite::InitParameter: currently " << fDigiPar->GetNrOfModules() << " digi cells ";
+  LOG(info) << "CbmTaskTofClusterizerParWrite::InitParameter: currently " << fDigiPar->GetNrOfModules()
+            << " digi cells ";
 
   fdMaxTimeDist  = fDigiBdfPar->GetMaxTimeDist();     // in ns
   fdMaxSpaceDist = fDigiBdfPar->GetMaxDistAlongCh();  // in cm
@@ -442,14 +413,6 @@ bool CbmTaskTofClusterizerParWrite::InitAlgos()
 
   int32_t iNbSmTypes = fDigiBdfPar->GetNbSmTypes();
 
-  //Prepare storage vectors
-  fStorDigi.resize(iNbSmTypes);
-  for (int32_t iSmType = 0; iSmType < iNbSmTypes; iSmType++) {
-    int32_t iNbSm  = fDigiBdfPar->GetNbSm(iSmType);
-    int32_t iNbRpc = fDigiBdfPar->GetNbRpc(iSmType);
-    fStorDigi[iSmType].resize(iNbSm * iNbRpc);
-  }
-
   // Create map with unique detector IDs and fill (needed only for dead strip array)
   std::map<uint32_t, uint32_t> detIdIndexMap;
   for (int32_t ind = 0; ind < fDigiBdfPar->GetNbDet(); ind++) {
@@ -457,70 +420,76 @@ bool CbmTaskTofClusterizerParWrite::InitAlgos()
     detIdIndexMap[iUniqueId] = ind;
   }
 
+  cbm::algo::tof::HitfindSetup setup;
+
+  setup.rpcs.resize(iNbSmTypes);
+
   // Create one algorithm per RPC and configure it with parameters
   for (int32_t iSmType = 0; iSmType < iNbSmTypes; iSmType++) {
     int32_t iNbSm  = fDigiBdfPar->GetNbSm(iSmType);
     int32_t iNbRpc = fDigiBdfPar->GetNbRpc(iSmType);
+    setup.NbSm.push_back(iNbSm);
+    setup.NbRpc.push_back(iNbRpc);
+
+    setup.rpcs[iSmType].resize(iNbSm * iNbRpc);
+
     for (int32_t iSm = 0; iSm < iNbSm; iSm++) {
       for (int32_t iRpc = 0; iRpc < iNbRpc; iRpc++) {
-        std::unique_ptr<cbm::algo::tof::ClusterizerRpcPar> par(new cbm::algo::tof::ClusterizerRpcPar());
+
+        cbm::algo::tof::HitfindSetup::Rpc par;
 
         const int32_t iDetId   = CbmTofAddress::GetUniqueAddress(iSm, iRpc, 0, 0, iSmType);
         const int32_t iDetIndx = detIdIndexMap[iDetId];
-        par->fDeadStrips       = fvDeadStrips[iDetIndx];
-        par->fPosYMaxScal      = fPosYMaxScal;
-        par->fdMaxTimeDist     = fdMaxTimeDist;
-        par->fdMaxSpaceDist    = fdMaxSpaceDist;
-        par->fCPTOffYBinWidth  = fvCPTOffYBinWidth[iSmType][iSm * iNbRpc + iRpc];
-        par->fCPTOffY          = fvCPTOffY[iSmType][iSm * iNbRpc + iRpc];
-        par->fCPTOffYRange     = fvCPTOffYRange[iSmType][iSm * iNbRpc + iRpc];
-        par->fSigVel           = fDigiBdfPar->GetSigVel(iSmType, iSm, iRpc);
-        par->fTimeRes          = 0.08;
-        par->numClWalkBinX     = nbClWalkBinX;
-        par->TOTMax            = fdTOTMax;
-        par->TOTMin            = fdTOTMin;
-        par->swapChannelSides  = fbSwapChannelSides;
-        par->channelDeadtime   = fdChannelDeadtime;
+        par.deadStrips         = fvDeadStrips[iDetIndx];
+        par.posYMaxScal        = fPosYMaxScal;
+        par.maxTimeDist        = fdMaxTimeDist;
+        par.maxSpaceDist       = fdMaxSpaceDist;
+        par.CPTOffYBinWidth    = fvCPTOffYBinWidth[iSmType][iSm * iNbRpc + iRpc];
+        par.CPTOffY            = fvCPTOffY[iSmType][iSm * iNbRpc + iRpc];
+        par.CPTOffYRange       = fvCPTOffYRange[iSmType][iSm * iNbRpc + iRpc];
+        par.sigVel             = fDigiBdfPar->GetSigVel(iSmType, iSm, iRpc);
+        par.timeRes            = 0.08;
+        par.numClWalkBinX      = nbClWalkBinX;
+        par.TOTMax             = fdTOTMax;
+        par.TOTMin             = fdTOTMin;
+        par.swapChannelSides   = fbSwapChannelSides;
+        par.channelDeadtime    = fdChannelDeadtime;
 
         int32_t iNbChan = fDigiBdfPar->GetNbChan(iSmType, iRpc);
-        par->fChanPar.resize(iNbChan);
+        par.chanPar.resize(iNbChan);
         for (int32_t iCh = 0; iCh < iNbChan; iCh++) {
 
+          cbm::algo::tof::HitfindSetup::Channel& chan = par.chanPar[iCh];
+
           const int32_t address   = CbmTofAddress::GetUniqueAddress(iSm, iRpc, iCh, 0, iSmType);
           CbmTofCell* channelInfo = fDigiPar->GetCell(address);
-          if (channelInfo == nullptr) { continue; }  //D.Smith 17.8.23: Needed as channel info sometimes not defined
-
-          gGeoManager->FindNode(channelInfo->GetX(), channelInfo->GetY(), channelInfo->GetZ());
-          const double* tra_ptr = gGeoManager->MakePhysicalNode()->GetMatrix()->GetTranslation();
-
-          //init Tof cell
-          cbm::algo::tof::TofCell& cell = par->fChanPar[iCh].cell;
-          cell.pos                      = ROOT::Math::XYZVector(tra_ptr[0], tra_ptr[1], tra_ptr[2]);
-
-          /*  why doesn't this variant work?
-          cell.pos.SetX(channelInfo->GetX());
-          cell.pos.SetY(channelInfo->GetY());
-          cell.pos.SetZ(channelInfo->GetZ());
-*/
-          cell.sizeX = channelInfo->GetSizex();
-          cell.sizeY = channelInfo->GetSizey();
+          if (channelInfo == nullptr) { continue; }
 
           // prepare local->global trafo
           gGeoManager->FindNode(channelInfo->GetX(), channelInfo->GetY(), channelInfo->GetZ());
-          double* rot_ptr = gGeoManager->GetCurrentMatrix()->GetRotationMatrix();
-          cell.rotation   = ROOT::Math::Rotation3D(&rot_ptr[0], &rot_ptr[9]);
-
-          par->fChanPar[iCh].fvCPTOff    = fvCPTOff[iSmType][iSm * iNbRpc + iRpc][iCh];
-          par->fChanPar[iCh].fvCPTotGain = fvCPTotGain[iSmType][iSm * iNbRpc + iRpc][iCh];
-          par->fChanPar[iCh].fvCPTotOff  = fvCPTotOff[iSmType][iSm * iNbRpc + iRpc][iCh];
-          par->fChanPar[iCh].fvCPWalk    = fvCPWalk[iSmType][iSm * iNbRpc + iRpc][iCh];
-          par->fChanPar[iCh].address     = address;
+          const double* tra_ptr = gGeoManager->MakePhysicalNode()->GetMatrix()->GetTranslation();
+          const double* rot_ptr = gGeoManager->GetCurrentMatrix()->GetRotationMatrix();
+
+          memcpy(chan.cell.translation.data(), tra_ptr, 3 * sizeof(double));
+          memcpy(chan.cell.rotation.data(), rot_ptr, 9 * sizeof(double));
+
+          chan.cell.sizeX = channelInfo->GetSizex();
+          chan.cell.sizeY = channelInfo->GetSizey();
+          chan.vCPTOff    = fvCPTOff[iSmType][iSm * iNbRpc + iRpc][iCh];
+          chan.vCPTotGain = fvCPTotGain[iSmType][iSm * iNbRpc + iRpc][iCh];
+          chan.vCPTotOff  = fvCPTotOff[iSmType][iSm * iNbRpc + iRpc][iCh];
+          chan.vCPWalk    = fvCPWalk[iSmType][iSm * iNbRpc + iRpc][iCh];
+          chan.address    = address;
         }
-        fAlgo[iSmType][iSm * iNbRpc + iRpc].SetParams(std::move(par));
+        setup.rpcs[iSmType][iSm * iNbRpc + iRpc] = par;
       }
     }
   }
-  return true;
-}
 
+  cbm::algo::config::Dump dump;
+  std::ofstream fout("TofHitfinderPar.yaml");
+  fout << dump(setup);
+  fout.close();
 
+  return true;
+}
diff --git a/reco/tasks/CbmTaskTofClusterizerParWrite.h b/reco/tasks/CbmTaskTofClusterizerParWrite.h
index 0dee02b3a50edd748b10790cc6ffee30c6b8731b..f57a5a6a5c1f9a14c6a21b6e1dba517553e2dc60 100644
--- a/reco/tasks/CbmTaskTofClusterizerParWrite.h
+++ b/reco/tasks/CbmTaskTofClusterizerParWrite.h
@@ -6,18 +6,11 @@
 #define CBMTASKTOFCLUSTERIZERPARWRITE_H 1
 
 // TOF Classes and includes
-// Input/Output
-class CbmEvent;
 // Geometry
 class CbmTofDetectorId;
 class CbmTofDigiPar;
 class CbmTofDigiBdfPar;
 class CbmTofCell;
-class CbmDigiManager;
-class CbmTsEventHeader;
-
-#include "CbmMatch.h"
-#include "CbmTofDigi.h"
 
 #include "tof/Clusterizer.h"
 
@@ -64,17 +57,14 @@ public:
   /**
        ** @brief Inherited from FairTask.
        **/
-  virtual void Exec(Option_t* option);
+  virtual void Exec(Option_t* option) {};
 
   /**
        ** @brief Inherited from FairTask.
        **/
-  virtual void Finish();
-  virtual void Finish(double calMode);
+  virtual void Finish() {};
 
   inline void SetCalMode(int32_t iMode) { fCalMode = iMode; }
-  inline void SetDutId(int32_t Id) { fDutId = Id; }
-
   inline void PosYMaxScal(double val) { fPosYMaxScal = val; }
   inline void SetTotMax(double val) { fTotMax = val; }
   inline void SetTotMin(double val) { fTotMin = val; }
@@ -83,27 +73,16 @@ public:
   inline void SetChannelDeadtime(double val) { fdChannelDeadtime = val; }
   inline void SetCalParFileName(TString CalParFileName) { fCalParFileName = CalParFileName; }
 
-  inline int GetNbHits() { return fiNbHits; }
   inline double GetTotMean() { return fTotMean; }
 
   void SwapChannelSides(bool bSwap) { fbSwapChannelSides = bSwap; }
-  void SetFileIndex(int32_t iIndex) { fiFileIndex = iIndex; }
-  void SetWriteDigisInOut(bool bDigis) { fbWriteDigisInOut = bDigis; }
-  void SetWriteHitsInOut(bool bHits) { fbWriteHitsInOut = bHits; }
   void SetDeadStrips(int32_t iDet, uint32_t ival);
 
 protected:
 private:
-  int32_t iNbTs                = 0;
-  int fiHitStart               = 0;
   bool bAddBeamCounterSideDigi = true;
-  const double dDoubleMax      = 1.E300;
-  const int32_t nbClWalkBinX   = 50;          // was 100 (11.10.2018)
-  const int32_t nbClWalkBinY   = 41;          // choose odd number to have central bin symmetric around 0
-  const int32_t DetMask        = 0x001FFFFF;  // geo v21a
-
-  std::vector<CbmTofDigi>* fT0DigiVec = nullptr;  //! T0 Digis
-
+  const int32_t nbClWalkBinX   = 50;  // was 100 (11.10.2018)
+  const int32_t nbClWalkBinY   = 41;  // choose odd number to have central bin symmetric around 0
 
   /**
        ** @brief Copy constructor.
@@ -129,35 +108,11 @@ private:
        **/
   bool InitAlgos();
 
-  // Hit finder algo
-  std::map<uint32_t, std::map<uint32_t, cbm::algo::tof::Clusterizer>> fAlgo = {};  //[nbType][nbSm*nbRpc]
-
   // ToF geometry variables
   CbmTofDetectorId* fTofId;
   CbmTofDigiPar* fDigiPar;
   CbmTofDigiBdfPar* fDigiBdfPar;
 
-  const CbmTsEventHeader* fTsHeader;
-
-  // Input variables
-  std::vector<CbmTofDigi> fTofDigiVec {};  //! TOF Digis
-  CbmDigiManager* fDigiMan;                // TOF Input Digis
-  TClonesArray* fEventsColl;               // CBMEvents (time based)
-
-  // Output variables
-  bool fbWriteHitsInOut;
-  bool fbWriteDigisInOut;
-  std::vector<CbmTofDigi>* fTofCalDigiVec = nullptr;     //! // Calibrated TOF Digis
-  TClonesArray* fTofHitsColl;                            // TOF hits
-  TClonesArray* fTofDigiMatchColl;                       // TOF Digi Links
-  std::vector<CbmTofDigi>* fTofCalDigiVecOut = nullptr;  //! // Calibrated TOF Digis
-  TClonesArray* fTofHitsCollOut;                         // TOF hits
-  TClonesArray* fTofDigiMatchCollOut;                    // TOF Digi Links
-  int32_t fiNbHits;                                      // Index of the CbmTofHit TClonesArray
-
-  // Intermediate storage variables (digi, index)
-  std::vector<std::vector<std::vector<std::pair<CbmTofDigi*, int32_t>>>> fStorDigi;  //[nbType][nbSm*nbRpc][nDigis]
-
   //Calibration parameters
   std::vector<std::vector<std::vector<std::vector<double>>>> fvCPTOff;     //[nSMT][nRpc][nCh][nbSide]
   std::vector<std::vector<std::vector<std::vector<double>>>> fvCPTotGain;  //[nSMT][nRpc][nCh][nbSide]
@@ -172,7 +127,6 @@ private:
 
   // Calib
   int32_t fCalMode;
-  int32_t fDutId;
 
   double fPosYMaxScal;
   double fTotMax;
@@ -192,15 +146,7 @@ private:
   double fdMaxSpaceDist;  // Isn't this just a local variable? Why make it global and preset?!?
   double fdModifySigvel;
 
-  double fdEvent;
-
-  double fProcessTime = 0.0;
-  uint64_t fuNbDigis  = 0;
-  uint64_t fuNbHits   = 0;
-
   bool fbSwapChannelSides;
-  int32_t fiOutputTreeEntry;
-  int32_t fiFileIndex;
 
   ClassDef(CbmTaskTofClusterizerParWrite, 1);
 };