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.

common/ferrostar/src/navigation_controller/mod.rs

@@ -120,18 +120,30 @@ 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() {
+                                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 +154,7 @@ impl NavigationController {
                         };
 
                         let progress = calculate_trip_progress(
-                            &(*snapped_user_location).into(),
+                            &(updated_snapped_user_location).into(),
                             &linestring,
                             &remaining_steps,
                         );
@@ -157,8 +169,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,