diff --git a/features/testbot/matching.feature b/features/testbot/matching.feature
index 29c38bbf47e..ed0c54acd76 100644
--- a/features/testbot/matching.feature
+++ b/features/testbot/matching.feature
@@ -651,3 +651,34 @@ Feature: Basic Map Matching
         When I match I should get
             | trace    | geometry                           | code |
             | defgh    | 1,1,1,0.999461,1.000674,0.999461   | Ok   |
+
+    @match @testbot
+    Scenario: Regression test - waypoints trimming too much geometry
+        Given the profile "testbot"
+        Given a grid size of 10 meters
+        Given the query options
+          | geometries | geojson |
+        Given the node map
+          """
+            bh
+             |
+             |
+             |
+             c
+             g\
+               \k
+                \
+                 \
+                  \
+                 j f
+          """
+        And the ways
+          | nodes |
+          | hc    |
+          | cf    |
+        Given the query options
+          | waypoints | 0;3  |
+          | overview  | full |
+        When I match I should get
+          | trace | geometry                                      | code |
+          | bgkj  | 1.00018,1,1.000225,0.999551,1.000396,0.999038 | Ok   |
diff --git a/unit_tests/engine/collapse_internal_route_result.cpp b/unit_tests/engine/collapse_internal_route_result.cpp
index 6e73cafbf44..fc404935b24 100644
--- a/unit_tests/engine/collapse_internal_route_result.cpp
+++ b/unit_tests/engine/collapse_internal_route_result.cpp
@@ -38,6 +38,9 @@ BOOST_AUTO_TEST_CASE(unchanged_collapse_route_result)
 
 BOOST_AUTO_TEST_CASE(two_legs_to_one_leg)
 {
+    // from_edge_based_node, turn_via_node, name_id, is_segregated, weight_until_turn, weight_of_turn,
+    // duration_until_turn, duration_of_turn, turn_instruction, lane_data, travel_mode, classes,
+    // entry_class, datasource_id, pre_turn_bearing, post_turn_bearing, left_hand
     PathData pathy{0, 2, 17, false, 2, 3, 4, 5, 0, {}, 4, 2, {}, 2, {1.0}, {1.0}, false};
     PathData kathy{0, 1, 16, false, 1, 2, 3, 4, 1, {}, 3, 1, {}, 1, {2.0}, {3.0}, false};
     PathData cathy{0, 3, 16, false, 1, 2, 3, 4, 1, {}, 3, 1, {}, 1, {2.0}, {3.0}, false};
@@ -61,10 +64,11 @@ BOOST_AUTO_TEST_CASE(two_legs_to_one_leg)
     BOOST_CHECK_EQUAL(collapsed.segment_end_coordinates[0].target_phantom.forward_segment_id.id,
                       12);
     BOOST_CHECK_EQUAL(collapsed.segment_end_coordinates[0].source_phantom.forward_segment_id.id, 1);
-    BOOST_CHECK_EQUAL(collapsed.unpacked_path_segments[0].size(), 3);
+    BOOST_CHECK_EQUAL(collapsed.unpacked_path_segments[0].size(), 4);
     BOOST_CHECK_EQUAL(collapsed.unpacked_path_segments[0][0].turn_via_node, 2);
     BOOST_CHECK_EQUAL(collapsed.unpacked_path_segments[0][1].turn_via_node, 1);
-    BOOST_CHECK_EQUAL(collapsed.unpacked_path_segments[0][2].turn_via_node, 3);
+    BOOST_CHECK_EQUAL(collapsed.unpacked_path_segments[0][2].turn_via_node, 1);
+    BOOST_CHECK_EQUAL(collapsed.unpacked_path_segments[0][3].turn_via_node, 3);
 }
 
 BOOST_AUTO_TEST_CASE(three_legs_to_two_legs)
@@ -101,13 +105,14 @@ BOOST_AUTO_TEST_CASE(three_legs_to_two_legs)
     BOOST_CHECK_EQUAL(collapsed.segment_end_coordinates[1].target_phantom.forward_segment_id.id,
                       18);
     BOOST_CHECK_EQUAL(collapsed.unpacked_path_segments[0].size(), 2);
-    BOOST_CHECK_EQUAL(collapsed.unpacked_path_segments[1].size(), 4);
+    BOOST_CHECK_EQUAL(collapsed.unpacked_path_segments[1].size(), 5);
     BOOST_CHECK_EQUAL(collapsed.unpacked_path_segments[0][0].turn_via_node, 2);
     BOOST_CHECK_EQUAL(collapsed.unpacked_path_segments[0][1].turn_via_node, 1);
     BOOST_CHECK_EQUAL(collapsed.unpacked_path_segments[1][0].turn_via_node, 1);
     BOOST_CHECK_EQUAL(collapsed.unpacked_path_segments[1][1].turn_via_node, 5);
     BOOST_CHECK_EQUAL(collapsed.unpacked_path_segments[1][2].turn_via_node, 3);
-    BOOST_CHECK_EQUAL(collapsed.unpacked_path_segments[1][3].turn_via_node, 4);
+    BOOST_CHECK_EQUAL(collapsed.unpacked_path_segments[1][3].turn_via_node, 3);
+    BOOST_CHECK_EQUAL(collapsed.unpacked_path_segments[1][4].turn_via_node, 4);
 }
 
 BOOST_AUTO_TEST_CASE(two_legs_to_two_legs)