diff --git a/linmdtw/DTWGPU.cu b/linmdtw/DTWGPU.cu
index a8b789c..15f0d1f 100644
--- a/linmdtw/DTWGPU.cu
+++ b/linmdtw/DTWGPU.cu
@@ -62,27 +62,27 @@ __global__ void DTW_Diag_Step(float* d0, float* d1, float* d2, float* csm0, floa
                 diag = -1;
                 if (j1 == 0) {
                     if (idx > 0) {
-                        left = d1[idx-1] + csm1[idx-1];
+                        left = d1[idx-1];
                     }
                     if (idx > 0 && thisi > 0) {
-                        diag = d0[idx-1] + csm0[idx-1];
+                        diag = d0[idx-1];
                     }
                     if (thisi > 0) {
-                        up = d1[idx] + csm1[idx];
+                        up = d1[idx];
                     }
                 }
                 else if (i1 == M-1 && j1 == 1) {
-                    left = d1[idx] + csm1[idx];
+                    left = d1[idx];
                     if (thisi > 0) {
-                        diag = d0[idx] + csm0[idx];
-                        up = d1[idx+1] + csm1[idx+1];
+                        diag = d0[idx];
+                        up = d1[idx+1];
                     }
                 }
                 else if (i1 == M-1 && j1 > 1) {
-                    left = d1[idx] + csm1[idx];
+                    left = d1[idx];
                     if (thisi > 0) {
-                        diag = d0[idx+1] + csm0[idx+1];
-                        up = d1[idx+1] + csm1[idx+1];
+                        diag = d0[idx+1];
+                        up = d1[idx+1];
                     }
                 }
                 if (left > -1) {
@@ -102,6 +102,6 @@ __global__ void DTW_Diag_Step(float* d0, float* d1, float* d2, float* csm0, floa
                 }
             }
         }
-        d2[idx] = score;
+        d2[idx] = score + csm2[idx];
     }
 }
diff --git a/linmdtw/dtw.cpp b/linmdtw/dtw.cpp
index a967835..2b5bffd 100644
--- a/linmdtw/dtw.cpp
+++ b/linmdtw/dtw.cpp
@@ -123,7 +123,7 @@ void c_diag_step(float* d0, float* d1, float* d2, float* csm0, float* csm1, floa
                 csm2[idx] += diff*diff;
             }
             csm2[idx] = sqrt(csm2[idx]);
-            
+
 
             // Step 2: Figure out the optimal cost
             if (thisi == 0 && thisj == 0) {
@@ -141,27 +141,27 @@ void c_diag_step(float* d0, float* d1, float* d2, float* csm0, float* csm1, floa
                 diag = -1;
                 if (j1 == 0) {
                     if (idx > 0) {
-                        left = d1[idx-1] + csm1[idx-1];
+                        left = d1[idx-1];
                     }
                     if (idx > 0 && thisi > 0) {
-                        diag = d0[idx-1] + csm0[idx-1];
+                        diag = d0[idx-1];
                     }
                     if (thisi > 0) {
-                        up = d1[idx] + csm1[idx];
+                        up = d1[idx];
                     }
                 }
                 else if (i1 == M-1 && j1 == 1) {
-                    left = d1[idx] + csm1[idx];
+                    left = d1[idx];
                     if (thisi > 0) {
-                        diag = d0[idx] + csm0[idx];
-                        up = d1[idx+1] + csm1[idx+1];
+                        diag = d0[idx];
+                        up = d1[idx+1];
                     }
                 }
                 else if (i1 == M-1 && j1 > 1) {
-                    left = d1[idx] + csm1[idx];
+                    left = d1[idx];
                     if (thisi > 0) {
-                        diag = d0[idx+1] + csm0[idx+1];
-                        up = d1[idx+1] + csm1[idx+1];
+                        diag = d0[idx+1];
+                        up = d1[idx+1];
                     }
                 }
                 if (left > -1) {
@@ -181,6 +181,7 @@ void c_diag_step(float* d0, float* d1, float* d2, float* csm0, float* csm1, floa
                 }
             }
         }
-        d2[idx] = score;
+        d2[idx] = score + csm2[idx];    // optimal accumulated cost + euclidean cost
+
     }
 }
diff --git a/linmdtw/dtw.py b/linmdtw/dtw.py
index c5416b9..34be698 100644
--- a/linmdtw/dtw.py
+++ b/linmdtw/dtw.py
@@ -319,8 +319,17 @@ def linmdtw(X, Y, box=None, min_dim=500, do_gpu=True, metadata=None):
         dleft = res1['d%i'%k]
         dright = res2['d%i'%k]
         csmright = res2['csm%i'%k]
-        diagsum = dleft + dright + csmright
-        idx = np.argmin(diagsum)
+
+        # if first index is -1, it will be removed for calculation
+        # for determining correct index, we need to add 1 again
+        shift = 1 if dleft[0] == -1 else 0
+
+        dleft = np.delete(dleft, np.where(dleft == -1))         # Remove -1 from np.array dleft
+        csmright = np.delete(csmright, np.where(dright == -1))  # Remove item where -1 corresponds to dright
+        dright = np.delete(dright, np.where(dright == -1))      # Remove -1 from np.array dright
+        
+        diagsum = dleft + dright - csmright                     # Use formula (4) from paper
+        idx = np.argmin(diagsum) + shift                        # shift index if necessary
         if diagsum[idx] < min_cost:
             min_cost = diagsum[idx]
             i, j = get_diag_indices(X.shape[0], Y.shape[0], k_save-2+k, box)
@@ -330,7 +339,7 @@ def linmdtw(X, Y, box=None, min_dim=500, do_gpu=True, metadata=None):
     left_path = []
     box_left = [box[0], min_idxs[0], box[2], min_idxs[1]]
     left_path = linmdtw(X, Y, box_left, min_dim, do_gpu, metadata)
-
+    
     # Recursively compute right paths
     right_path = []
     box_right = [min_idxs[0], box[1], min_idxs[1], box[3]]
diff --git a/linmdtw/dtwgpu.py b/linmdtw/dtwgpu.py
index d55fa9f..0bfcd7a 100644
--- a/linmdtw/dtwgpu.py
+++ b/linmdtw/dtwgpu.py
@@ -63,7 +63,7 @@ def init_gpu():
 
                 // Step 2: Figure out the optimal cost
                 if (thisi == 0 && thisj == 0) {
-                    score = 0;
+                    score = 0 + csm2[idx];
                     if (debug == -1) {
                         S[0] = 0;
                         U[0] = -1;
@@ -77,27 +77,27 @@ def init_gpu():
                     diag = -1;
                     if (j1 == 0) {
                         if (idx > 0) {
-                            left = d1[idx-1] + csm1[idx-1];
+                            left = d1[idx-1];
                         }
                         if (idx > 0 && thisi > 0) {
-                            diag = d0[idx-1] + csm0[idx-1];
+                            diag = d0[idx-1];
                         }
                         if (thisi > 0) {
-                            up = d1[idx] + csm1[idx];
+                            up = d1[idx];
                         }
                     }
                     else if (i1 == M-1 && j1 == 1) {
-                        left = d1[idx] + csm1[idx];
+                        left = d1[idx];
                         if (thisi > 0) {
-                            diag = d0[idx] + csm0[idx];
-                            up = d1[idx+1] + csm1[idx+1];
+                            diag = d0[idx];
+                            up = d1[idx+1];
                         }
                     }
                     else if (i1 == M-1 && j1 > 1) {
-                        left = d1[idx] + csm1[idx];
+                        left = d1[idx];
                         if (thisi > 0) {
-                            diag = d0[idx+1] + csm0[idx+1];
-                            up = d1[idx+1] + csm1[idx+1];
+                            diag = d0[idx+1];
+                            up = d1[idx+1];
                         }
                     }
                     if (left > -1) {
@@ -117,7 +117,7 @@ def init_gpu():
                     }
                 }
             }
-            d2[idx] = score;
+            d2[idx] = score + csm2[idx];
         }
     }
     """