diff --git a/algo/detectors/tof/HitFinderTof.cxx b/algo/detectors/tof/HitFinderTof.cxx
index dfa2b493710280223d8d2c0dc1dda9d052ad8674..630cfae883a8d19059091ea508bdf0b6171739ee 100644
--- a/algo/detectors/tof/HitFinderTof.cxx
+++ b/algo/detectors/tof/HitFinderTof.cxx
@@ -35,17 +35,13 @@ namespace cbm::algo
         // These are doubles in the digi class
         const double chan   = pDigi->GetChannel();
         const double side   = pDigi->GetSide();
-        const double charge = pDigi->GetTot();
-        const double time   = pDigi->GetTime();
+        const double charge = pDigi->GetTot() * fParams.fChanPar[chan].fvCPTotGain[side];  // calibrate Digi ToT
+        const double time   = pDigi->GetTime() - fParams.fChanPar[chan].fvCPTOff[side];    // calibrate Digi Time
 
         digisExp[chan].push_back(pDigi);
         digisInd[chan].push_back(digiIndexIn[iDigi]);
 
-        // calibrate Digi Time
-        pDigi->SetTime(time - fParams.fChanPar[chan].fvCPTOff[side]);
-
-        // calibrate Digi ToT
-        pDigi->SetTot(charge * fParams.fChanPar[chan].fvCPTotGain[side]);
+        pDigi->SetTot(charge);
 
         {  // walk correction
           const double totBinSize = (TOTMax - TOTMin) / 2. / numClWalkBinX;
@@ -88,27 +84,28 @@ namespace cbm::algo
     //reset counter
     numSameSide = 0;
 
-    cluster.reset();  // Don't spread clusters over RPCs
-
     for (int32_t chan = 0; chan < fParams.fChanPar.size(); chan++) {
 
       std::vector<CbmTofDigi*>& storDigiExp = digisExp[chan];
       std::vector<int32_t>& storDigiInd     = digisInd[chan];
       HitFinderTofChanPar& chanPar          = fParams.fChanPar[chan];
 
-      while (1 < storDigiExp.size()) {
-        while ((storDigiExp[0])->GetSide() == (storDigiExp[1])->GetSide()) {
+      auto digiExpIt = storDigiExp.begin();
+      auto digiIndIt = storDigiInd.begin();
+
+      while (1 < std::distance(digiExpIt, storDigiExp.end())) {
+        while ((*digiExpIt)->GetSide() == (*std::next(digiExpIt, 1))->GetSide()) {
           // Not one Digi of each end!
           numSameSide++;
-          storDigiExp.erase(storDigiExp.begin());
-          storDigiInd.erase(storDigiInd.begin());
-          if (2 > storDigiExp.size()) break;
+          digiExpIt++;
+          digiIndIt++;
+          if (2 > std::distance(digiExpIt, storDigiExp.end())) break;
         }
-        if (2 > storDigiExp.size()) break;  // 2 Digis = both sides present
+        if (2 > std::distance(digiExpIt, storDigiExp.end())) break;  // 2 Digis = both sides present
 
         TofCell* channelInfo = &chanPar.cell;
-        CbmTofDigi* xDigiA   = storDigiExp[0];
-        CbmTofDigi* xDigiB   = storDigiExp[1];
+        CbmTofDigi* xDigiA   = *digiExpIt;
+        CbmTofDigi* xDigiB   = *std::next(digiExpIt, 1);
 
         // The "Strip" time is the mean time between each end
         const double time = 0.5 * (xDigiA->GetTime() + xDigiB->GetTime());
@@ -137,8 +134,8 @@ namespace cbm::algo
         if (channelInfo->sizeY / 2.0 < pos.Y() || -1 * channelInfo->sizeY / 2.0 > pos.Y()) {
           // if outside of strip limits, the pair is bad => try to remove one end and check the next pair
           // (if another possibility exist)
-          storDigiExp.erase(storDigiExp.begin());
-          storDigiInd.erase(storDigiInd.begin());
+          digiExpIt++;
+          digiIndIt++;
           continue;
         }  // Pair leads to hit oustide of strip limits
 
@@ -160,19 +157,15 @@ namespace cbm::algo
             cluster.reset();
           }
         }
-        cluster.add(pos, time, totSum, totSum, storDigiInd[0], storDigiInd[1]);
-        storDigiExp.erase(storDigiExp.begin());
-        storDigiExp.erase(storDigiExp.begin());
-        storDigiInd.erase(storDigiInd.begin());
-        storDigiInd.erase(storDigiInd.begin());
+        cluster.add(pos, time, totSum, totSum, *digiIndIt, *std::next(digiIndIt, 1));
+        digiExpIt += 2;
+        digiIndIt += 2;
 
         lastChan = chan;
         lastPosY = pos.Y();
         lastTime = time;
       }  // while (1 < storDigiExp.size())
-      storDigiExp.clear();
-      storDigiInd.clear();
-    }  // for( int32_t chan = 0; chan < iNbCh; chan++ )
+    }    // for( int32_t chan = 0; chan < iNbCh; chan++ )
 
     // Save last cluster if it exists
     if (0 < cluster.numChan()) {
diff --git a/reco/tasks/CbmTaskTofHitFinder.cxx b/reco/tasks/CbmTaskTofHitFinder.cxx
index 079c9ae224fe2a3e77738324f3f857bf796e5906..a49f1d23293de268c02d758692b56fc213a871a4 100644
--- a/reco/tasks/CbmTaskTofHitFinder.cxx
+++ b/reco/tasks/CbmTaskTofHitFinder.cxx
@@ -402,9 +402,9 @@ pair<int32_t, int32_t> CbmTaskTofHitFinder::BuildClusters(CbmEvent* event)
     assert(pDigi);
 
     // These are doubles in the digi class
-    const double smType  = pDigi->GetType();
-    const double smId    = pDigi->GetSm();
-    const double rpcId   = pDigi->GetRpc();
+    const int32_t smType = CbmTofAddress::GetSmType(pDigi->GetAddress());
+    const int32_t smId   = CbmTofAddress::GetSmId(pDigi->GetAddress());
+    const int32_t rpcId  = CbmTofAddress::GetRpcId(pDigi->GetAddress());
     const int32_t numRpc = fDigiBdfPar->GetNbRpc(smType);
     fStorDigiExp[smType][smId * numRpc + rpcId].push_back(*pDigi);
     fStorDigiInd[smType][smId * numRpc + rpcId].push_back(digiIndex);
@@ -441,6 +441,8 @@ pair<int32_t, int32_t> CbmTaskTofHitFinder::BuildClusters(CbmEvent* event)
           new ((*fTofDigiMatchColl)[hitIndex]) CbmMatch(*digiMatch);
           delete digiMatch;
         }
+        digiExp.clear();
+        digiInd.clear();
       }
     }
   }