From 404fc4b44c351c3b6d35068126dea8ffa4b345ab Mon Sep 17 00:00:00 2001
From: Ahmed El-Helw <aelhelw@hudhud.sa>
Date: Tue, 12 Nov 2024 23:21:01 +0400
Subject: [PATCH] Fix incorrect geometry index after advancing

After advancing the step, the geometry index sometimes incorrectly
refers to an index in the previous RouteStep instead of the current one.
This patch recomputes the geometry index when the RouteSteps change.
---
 .../src/navigation_controller/mod.rs          | 41 ++++++++++++-------
 1 file changed, 27 insertions(+), 14 deletions(-)

diff --git a/common/ferrostar/src/navigation_controller/mod.rs b/common/ferrostar/src/navigation_controller/mod.rs
index cb1a8bec..f6508685 100644
--- a/common/ferrostar/src/navigation_controller/mod.rs
+++ b/common/ferrostar/src/navigation_controller/mod.rs
@@ -120,18 +120,31 @@ impl NavigationController {
                         let mut remaining_steps = remaining_steps.clone();
                         remaining_steps.remove(0);
 
+                        // we need to update the geometry index, since the step has changed
+                        let (updated_current_step_geometry_index, updated_snapped_user_location) =
+                            if let Some(current_route_step) = remaining_steps.first() {
+                                // TODO: We could move this to the Route struct or NavigationController directly to only calculate it once.
+                                let current_step_linestring = current_route_step.get_linestring();
+                                self.snap_user_to_line(
+                                    *snapped_user_location,
+                                    &current_step_linestring,
+                                )
+                            } else {
+                                (*current_step_geometry_index, *snapped_user_location)
+                            };
+
                         // Update remaining waypoints
-                        let should_advance_waypoint = if let Some(waypoint) =
-                            remaining_waypoints.first()
-                        {
-                            let current_location: Point = snapped_user_location.coordinates.into();
-                            let next_waypoint: Point = waypoint.coordinate.into();
-                            // TODO: This is just a hard-coded threshold for the time being.
-                            // More sophisticated behavior will take some time and use cases, so punting on this for now.
-                            Haversine::distance(current_location, next_waypoint) < 100.0
-                        } else {
-                            false
-                        };
+                        let should_advance_waypoint =
+                            if let Some(waypoint) = remaining_waypoints.first() {
+                                let current_location: Point =
+                                    updated_snapped_user_location.coordinates.into();
+                                let next_waypoint: Point = waypoint.coordinate.into();
+                                // TODO: This is just a hard-coded threshold for the time being.
+                                // More sophisticated behavior will take some time and use cases, so punting on this for now.
+                                Haversine::distance(current_location, next_waypoint) < 100.0
+                            } else {
+                                false
+                            };
 
                         let remaining_waypoints = if should_advance_waypoint {
                             let mut remaining_waypoints = remaining_waypoints.clone();
@@ -142,7 +155,7 @@ impl NavigationController {
                         };
 
                         let progress = calculate_trip_progress(
-                            &(*snapped_user_location).into(),
+                            &(updated_snapped_user_location).into(),
                             &linestring,
                             &remaining_steps,
                         );
@@ -157,8 +170,8 @@ impl NavigationController {
                             .and_then(|index| current_step.get_annotation_at_current_index(index));
 
                         TripState::Navigating {
-                            current_step_geometry_index: *current_step_geometry_index,
-                            snapped_user_location: *snapped_user_location,
+                            current_step_geometry_index: updated_current_step_geometry_index,
+                            snapped_user_location: updated_snapped_user_location,
                             remaining_steps,
                             remaining_waypoints,
                             progress,