diff --git a/include/engine/api/match_parameters_tidy.hpp b/include/engine/api/match_parameters_tidy.hpp
index 8565fe923..b4e3a2fea 100644
--- a/include/engine/api/match_parameters_tidy.hpp
+++ b/include/engine/api/match_parameters_tidy.hpp
@@ -80,6 +80,8 @@ inline Result keep_all(const MatchParameters &params)
 
 inline Result tidy(const MatchParameters &params, Thresholds cfg = {15., 5})
 {
+    BOOST_ASSERT(!params.coordinates.empty());
+
     Result result;
 
     result.can_be_removed.resize(params.coordinates.size(), false);
@@ -91,10 +93,8 @@ inline Result tidy(const MatchParameters &params, Thresholds cfg = {15., 5})
     Thresholds running{0., 0};
 
     // Walk over adjacent (coord, ts)-pairs, with rhs being the candidate to discard or keep
-    for (std::size_t current = 0; current < params.coordinates.size() - 1; ++current)
+    for (std::size_t current = 0, next = 1; next < params.coordinates.size() - 1; ++current, ++next)
     {
-        const auto next = current + 1;
-
         auto distance_delta = util::coordinate_calculation::haversineDistance(
             params.coordinates[current], params.coordinates[next]);
         running.distance_in_meters += distance_delta;
@@ -130,7 +130,11 @@ inline Result tidy(const MatchParameters &params, Thresholds cfg = {15., 5})
         }
     }
 
-    BOOST_ASSERT(result.can_be_removed.size() == params.coordinates.size());
+    // Always use the last coordinate if more than two original coordinates
+    if (params.coordinates.size() > 1)
+    {
+        result.tidied_to_original.push_back(params.coordinates.size() - 1);
+    }
 
     // We have to filter parallel arrays that may be empty or the exact same size.
     // result.parameters contains an empty MatchParameters at this point: conditionally fill.
diff --git a/unit_tests/engine/tidy.cpp b/unit_tests/engine/tidy.cpp
index 0ce7c8056..45fdd0c78 100644
--- a/unit_tests/engine/tidy.cpp
+++ b/unit_tests/engine/tidy.cpp
@@ -53,10 +53,10 @@ BOOST_AUTO_TEST_CASE(two_item_trace_needs_tidiying_test)
     auto result = tidy::tidy(params, thresholds);
 
     BOOST_CHECK_EQUAL(result.can_be_removed.size(), 2);
-    BOOST_CHECK_EQUAL(result.tidied_to_original.size(), 1);
+    BOOST_CHECK_EQUAL(result.tidied_to_original.size(), 2);
 
     BOOST_CHECK_EQUAL(result.can_be_removed[0], false);
-    BOOST_CHECK_EQUAL(result.can_be_removed[1], true);
+    BOOST_CHECK_EQUAL(result.can_be_removed[1], false);
     BOOST_CHECK_EQUAL(result.tidied_to_original[0], 0);
 }
 
@@ -87,16 +87,18 @@ BOOST_AUTO_TEST_CASE(two_blobs_in_traces_needs_tidiying_test)
     auto result = tidy::tidy(params, thresholds);
 
     BOOST_CHECK_EQUAL(result.can_be_removed.size(), params.coordinates.size());
-    BOOST_CHECK_EQUAL(result.tidied_to_original.size(), 2);
+    BOOST_CHECK_EQUAL(result.tidied_to_original.size(), 3);
 
     BOOST_CHECK_EQUAL(result.tidied_to_original[0], 0);
     BOOST_CHECK_EQUAL(result.tidied_to_original[1], 3);
+    BOOST_CHECK_EQUAL(result.tidied_to_original[2], 5);
 
     const auto redundant = result.can_be_removed.count();
-    BOOST_CHECK_EQUAL(redundant, params.coordinates.size() - 2);
+    BOOST_CHECK_EQUAL(redundant, params.coordinates.size() - 3);
 
     BOOST_CHECK_EQUAL(result.can_be_removed[0], false);
     BOOST_CHECK_EQUAL(result.can_be_removed[3], false);
+    BOOST_CHECK_EQUAL(result.can_be_removed[5], false);
 }
 
 BOOST_AUTO_TEST_CASE(two_blobs_in_traces_needs_tidiying_no_timestamps_test)
@@ -118,15 +120,17 @@ BOOST_AUTO_TEST_CASE(two_blobs_in_traces_needs_tidiying_no_timestamps_test)
     auto result = tidy::tidy(params, thresholds);
 
     BOOST_CHECK_EQUAL(result.can_be_removed.size(), params.coordinates.size());
-    BOOST_CHECK_EQUAL(result.tidied_to_original.size(), 2);
+    BOOST_CHECK_EQUAL(result.tidied_to_original.size(), 3);
     BOOST_CHECK_EQUAL(result.tidied_to_original[0], 0);
     BOOST_CHECK_EQUAL(result.tidied_to_original[1], 3);
+    BOOST_CHECK_EQUAL(result.tidied_to_original[2], 5);
 
     const auto redundant = result.can_be_removed.count();
-    BOOST_CHECK_EQUAL(redundant, params.coordinates.size() - 2);
+    BOOST_CHECK_EQUAL(redundant, params.coordinates.size() - 3);
 
     BOOST_CHECK_EQUAL(result.can_be_removed[0], false);
     BOOST_CHECK_EQUAL(result.can_be_removed[3], false);
+    BOOST_CHECK_EQUAL(result.can_be_removed[5], false);
 }
 
 BOOST_AUTO_TEST_SUITE_END()