Fix incorrect geometry index after advancing

common/ferrostar/src/deviation_detection.rs

@@ -18,7 +18,7 @@ use crate::models::{Route, RouteStep, UserLocation};
 use alloc::sync::Arc;
 use geo::Point;
 
-#[cfg(feature = "wasm-bindgen")]
+#[cfg(any(feature = "wasm-bindgen", test))]
 use serde::{Deserialize, Serialize};
 #[cfg(feature = "wasm-bindgen")]
 use tsify::Tsify;
@@ -114,7 +114,8 @@ impl RouteDeviationTracking {
 /// For example, we could conceivably add a "wrong way" status in the future.
 #[derive(Debug, Copy, Clone, PartialEq)]
 #[cfg_attr(feature = "uniffi", derive(uniffi::Enum))]
-#[cfg_attr(feature = "wasm-bindgen", derive(Serialize, Deserialize, Tsify))]
+#[cfg_attr(any(feature = "wasm-bindgen", test), derive(Serialize, Deserialize))]
+#[cfg_attr(feature = "wasm-bindgen", derive(Tsify))]
 #[cfg_attr(feature = "wasm-bindgen", tsify(into_wasm_abi, from_wasm_abi))]
 pub enum RouteDeviation {
     /// The user is proceeding on course within the expected tolerances; everything is normal.

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,
@@ -372,3 +384,71 @@ impl JsNavigationController {
             .map_err(|e| JsValue::from_str(&format!("{:?}", e)))
     }
 }
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+    use crate::deviation_detection::RouteDeviationTracking;
+    use crate::navigation_controller::models::{CourseFiltering, StepAdvanceMode};
+    use crate::navigation_controller::test_helpers::get_extended_route;
+    use crate::simulation::{
+        advance_location_simulation, location_simulation_from_route, LocationBias,
+    };
+
+    #[test]
+    fn complex_route_snapshot_test() {
+        let route = get_extended_route();
+        let mut simulation_state =
+            location_simulation_from_route(&route, Some(10.0), LocationBias::None)
+                .expect("Unable to create simulation");
+
+        let controller = NavigationController::new(
+            route,
+            NavigationControllerConfig {
+                // NOTE: We will use an exact location to trigger the update;
+                // this is not testing the thresholds.
+                step_advance: StepAdvanceMode::DistanceToEndOfStep {
+                    distance: 0,
+                    minimum_horizontal_accuracy: 0,
+                },
+                route_deviation_tracking: RouteDeviationTracking::None,
+                snapped_location_course_filtering: CourseFiltering::Raw,
+            },
+        );
+
+        let mut state = controller.get_initial_state(simulation_state.current_location);
+        let mut states = vec![state.clone()];
+        loop {
+            let new_simulation_state = advance_location_simulation(&simulation_state);
+            let new_state =
+                controller.update_user_location(new_simulation_state.current_location, &state);
+            if new_simulation_state == simulation_state {
+                break;
+            }
+
+            match new_state {
+                TripState::Idle => {}
+                TripState::Navigating {
+                    current_step_geometry_index,
+                    ref remaining_steps,
+                    ..
+                } => {
+                    if let Some(index) = current_step_geometry_index {
+                        let geom_length = remaining_steps[0].geometry.len() as u64;
+                        assert!(
+                            index < geom_length,
+                            "index = {index}, geom_length = {geom_length}"
+                        );
+                    }
+                }
+                TripState::Complete => {}
+            }
+
+            simulation_state = new_simulation_state;
+            state = new_state;
+            states.push(state.clone());
+        }
+
+        insta::assert_yaml_snapshot!(states);
+    }
+}

common/ferrostar/src/navigation_controller/models.rs

@@ -5,16 +5,17 @@ use crate::models::{RouteStep, SpokenInstruction, UserLocation, VisualInstructio
 #[cfg(feature = "alloc")]
 use alloc::vec::Vec;
 use geo::LineString;
-#[cfg(feature = "wasm-bindgen")]
+#[cfg(any(feature = "wasm-bindgen", test))]
 use serde::{Deserialize, Serialize};
 #[cfg(feature = "wasm-bindgen")]
 use tsify::Tsify;
 
 /// High-level state describing progress through a route.
 #[derive(Debug, Clone, PartialEq)]
 #[cfg_attr(feature = "uniffi", derive(uniffi::Record))]
-#[cfg_attr(feature = "wasm-bindgen", derive(Serialize, Deserialize, Tsify))]
-#[cfg_attr(feature = "wasm-bindgen", serde(rename_all = "camelCase"))]
+#[cfg_attr(any(feature = "wasm-bindgen", test), derive(Serialize, Deserialize))]
+#[cfg_attr(feature = "wasm-bindgen", derive(Tsify))]
+#[cfg_attr(any(feature = "wasm-bindgen", test), serde(rename_all = "camelCase"))]
 #[cfg_attr(feature = "wasm-bindgen", tsify(into_wasm_abi, from_wasm_abi))]
 pub struct TripProgress {
     /// The distance to the next maneuver, in meters.
@@ -34,7 +35,8 @@ pub struct TripProgress {
 /// and [`update_user_location`](super::NavigationController::update_user_location).
 #[derive(Debug, Clone, PartialEq)]
 #[cfg_attr(feature = "uniffi", derive(uniffi::Enum))]
-#[cfg_attr(feature = "wasm-bindgen", derive(Serialize, Deserialize, Tsify))]
+#[cfg_attr(any(feature = "wasm-bindgen", test), derive(Serialize, Deserialize))]
+#[cfg_attr(feature = "wasm-bindgen", derive(Tsify))]
 #[cfg_attr(feature = "wasm-bindgen", tsify(into_wasm_abi, from_wasm_abi))]
 pub enum TripState {
     /// The navigation controller is idle and there is no active trip.