diff --git a/src/transmissionReconstruction/detection/detection.cpp b/src/transmissionReconstruction/detection/detection.cpp
index 1d850bd..6d60ab6 100644
--- a/src/transmissionReconstruction/detection/detection.cpp
+++ b/src/transmissionReconstruction/detection/detection.cpp
@@ -4,6 +4,7 @@
 #include "Function2D.h"
 #include "Function3D.h"
 #include "Matrix.h"
+#include <sys/types.h>
 
 using namespace Aurora;
 namespace Recon {
@@ -94,10 +95,11 @@ namespace Recon {
                     AscanBlockProcessed(j, i) = AscanBlock(j, i);
                 }
                 auto temp = zeros(AscanBlockProcessed.getDimSize(0), 1);
-                size_t end = round(expectedPosWater[i])-floor(windowWidth[i]/2)+gauss.getDataSize()-1;
-                for (size_t k = floor(windowWidth[i]/2); k < end; k++)
+                size_t end = std::round(expectedPosWater[i])-std::round(windowWidth[i]/2)+gauss.getDataSize()-1;
+                size_t gIdx = 0;
+                for (size_t k = std::round(expectedPosWater[i])- std::round(windowWidth[i]/2)-1; k < end; k++)
                 {
-                    temp(round(expectedPosWater[i])-k) = gauss;
+                    temp[k] = gauss[gIdx++];
                 }
                 AscanBlockProcessed($,i) = AscanBlockProcessed($,i).toMatrix() * temp;
             }
@@ -185,10 +187,10 @@ namespace Recon {
             auto x = fft(AscanBlock, m2);
             auto y = fft(AscanRefBlock, m2);
             auto c1 = ifft(x*conj(y));
-            auto c = zeros(mxl+mxl+1,c1.getDimSize(1));
+            auto c = complex(zeros(mxl+mxl+1,c1.getDimSize(1)));
             for (size_t i = 0; i < mxl; i++)
             {
-                c(i,$) = c1(m2-mxl+(1:mxl), $);
+                c(i,$) = c1(m2-mxl+i, $);
                 c(i+mxl,$) = c1(i, $);
             }
             c(mxl+mxl,$) = c1(mxl, $);
diff --git a/src/transmissionReconstruction/detection/detection.h b/src/transmissionReconstruction/detection/detection.h
index 158ef58..4a8ce8d 100644
--- a/src/transmissionReconstruction/detection/detection.h
+++ b/src/transmissionReconstruction/detection/detection.h
@@ -35,12 +35,20 @@ TimeWindowResult applyTimeWindowing(
     double expectedSOSWater, double startOffset, double segmentLenOffset,
     double minSpeedOfSound, double maxSpeedOfSound, bool gaussWindow);
 
-// Aurora::Matrix
-// detectAttVectorized(const Aurora::Matrix &Ascan, const Aurora::Matrix &AscanRef,
-//                     const Aurora::Matrix &distRef,
-//                     const Aurora::Matrix &sosWaterRef,
-//                     const Aurora::Matrix &tof, int aScanReconstructionFrequency,
-//                     double offsetElectronic, int detectionWindowATT);
+Aurora::Matrix detectAttVectorized(const Aurora::Matrix &Ascan, const Aurora::Matrix &AscanRef,
+                        const Aurora::Matrix &distRef,
+                        const Aurora::Matrix &sosWaterRef,
+                        const Aurora::Matrix &tof, int aScanReconstructionFrequency,
+                        double offsetElectronic, int detectionWindowATT);
+
+Aurora::Matrix detectTofVectorized(
+        const Aurora::Matrix &AscanBlock, const Aurora::Matrix &AscanRefBlock,
+        const Aurora::Matrix &distBlock, const Aurora::Matrix &distBlockRef,
+        const Aurora::Matrix &sosWaterBlock,
+        const Aurora::Matrix &sosWaterRefBlock, double expectedSOSWater,
+        int useTimeWindowing, int aScanReconstructionFrequency,
+        double offsetElectronic, int detectionWindowSOS, double minSpeedOfSound,
+        double maxSpeedOfSound, bool gaussWindow) ;
 
 } // namespace Recon