First pass at step advance logic

apple/Sources/UniFFI/ferrostar.swift

@@ -433,6 +433,7 @@ private struct FfiConverterTimestamp: FfiConverterRustBuffer {
 }
 
 public protocol NavigationControllerProtocol {
+    func advanceToNextStep() -> NavigationStateUpdate
     func updateUserLocation(location: UserLocation) -> NavigationStateUpdate
 }
 
@@ -459,6 +460,15 @@ public class NavigationController: NavigationControllerProtocol {
         try! rustCall { uniffi_ferrostar_fn_free_navigationcontroller(pointer, $0) }
     }
 
+    public func advanceToNextStep() -> NavigationStateUpdate {
+        return try! FfiConverterTypeNavigationStateUpdate.lift(
+            try!
+                rustCall {
+                    uniffi_ferrostar_fn_method_navigationcontroller_advance_to_next_step(self.pointer, $0)
+                }
+        )
+    }
+
     public func updateUserLocation(location: UserLocation) -> NavigationStateUpdate {
         return try! FfiConverterTypeNavigationStateUpdate.lift(
             try!
@@ -2049,6 +2059,9 @@ private var initializationResult: InitializationResult {
     if uniffi_ferrostar_checksum_method_routeadapter_parse_response() != 353 {
         return InitializationResult.apiChecksumMismatch
     }
+    if uniffi_ferrostar_checksum_method_navigationcontroller_advance_to_next_step() != 26674 {
+        return InitializationResult.apiChecksumMismatch
+    }
     if uniffi_ferrostar_checksum_method_navigationcontroller_update_user_location() != 55838 {
         return InitializationResult.apiChecksumMismatch
     }

common/ferrostar-core/src/ferrostar.udl

@@ -139,5 +139,6 @@ interface RouteAdapter {
 interface NavigationController {
     constructor(UserLocation last_user_location, Route route);
 
+    NavigationStateUpdate advance_to_next_step();
     NavigationStateUpdate update_user_location(UserLocation location);
 };

common/ferrostar-core/src/navigation_controller/mod.rs

@@ -2,12 +2,18 @@ pub mod models;
 mod utils;
 
 use crate::models::{Route, UserLocation};
-use crate::navigation_controller::utils::has_completed_step;
+use crate::navigation_controller::utils::{do_advance_to_next_step, has_completed_step};
 use geo::Coord;
 use models::*;
 use std::sync::Mutex;
 use utils::snap_to_line;
 
+// This may be improved eventually, but is essentially a sentinel value that we reached the end.
+const ARRIVED_EOT: NavigationStateUpdate = NavigationStateUpdate::Arrived {
+    spoken_instruction: None,
+    visual_instructions: None,
+};
+
 /// Manages the navigation lifecycle of a single trip, requesting the initial route and updating
 /// internal state based on inputs like user location updates.
 ///
@@ -31,6 +37,8 @@ pub struct NavigationController {
     /// very well. Others like [core::cell::RefCell] are not enough as the entire object is required to be both
     /// [Send] and [Sync], and [core::cell::RefCell] is explicitly `!Sync`.
     state: Mutex<TripState>,
+    // TODO: Configuration options
+    // - Strategy for advancing to the next step (simple threshold, manually, custom app logic via interface? ...?)
 }
 
 impl NavigationController {
@@ -47,7 +55,7 @@ impl NavigationController {
         Self {
             state: Mutex::new(TripState::Navigating {
                 last_user_location,
-                snapped_user_location: snap_to_line(last_user_location, &route_line_string),
+                snapped_user_location: snap_to_line(&last_user_location, &route_line_string),
                 route,
                 route_line_string,
                 remaining_waypoints,
@@ -56,6 +64,46 @@ impl NavigationController {
         }
     }
 
+    /// Advances navigation to the next step.
+    ///
+    /// Depending on the advancement strategy, this may be automatic.
+    /// For other cases, it is desirable to advance to the next step manually (ex: walking in an
+    /// urban tunnel). We leave this decision to the app developer.
+    pub fn advance_to_next_step(&self) -> NavigationStateUpdate {
+        match self.state.lock() {
+            Ok(mut guard) => {
+                match *guard {
+                    // TODO: Determine current step + mode of travel
+                    TripState::Navigating {
+                        ref snapped_user_location,
+                        ref remaining_waypoints,
+                        ref mut remaining_steps,
+                        ..
+                    } => {
+                        let update = do_advance_to_next_step(
+                            snapped_user_location,
+                            remaining_waypoints,
+                            remaining_steps,
+                        );
+                        if matches!(update, NavigationStateUpdate::Arrived { .. }) {
+                            *guard = TripState::Complete;
+                        }
+                        update
+                    }
+                    // It's tempting to throw an error here, since the caller should know better, but
+                    // a mistake like this is technically harmless.
+                    TripState::Complete => ARRIVED_EOT,
+                }
+            }
+            Err(_) => {
+                // The only way the mutex can become poisoned is if another caller panicked while
+                // holding the mutex. In which case, there is no point in continuing.
+                unreachable!("Poisoned mutex. This should never happen.");
+            }
+        }
+    }
+
+    /// Updates the user's current location and updates the navigation state accordingly.
     pub fn update_user_location(&self, location: UserLocation) -> NavigationStateUpdate {
         match self.state.lock() {
             Ok(mut guard) => {
@@ -84,34 +132,45 @@ impl NavigationController {
                         //
 
                         // Find the nearest point on the route line
-                        snapped_user_location = snap_to_line(location, &route_line_string);
+                        snapped_user_location = snap_to_line(&location, &route_line_string);
 
                         // TODO: Check if the user's distance is > some configurable threshold, accounting for GPS error, mode of travel, etc.
                         // TODO: If so, flag that the user is off route so higher levels can recalculate if desired
 
                         // TODO: If on track, update the set of remaining waypoints, remaining steps (drop from the list), and update current step.
                         // IIUC these should always appear within the route itself, which simplifies the logic a bit.
                         // TBD: Do we want to support disjoint routes?
+                        let remaining_waypoints = remaining_waypoints.clone();
 
                         let current_step = if has_completed_step(current_step, &last_user_location)
                         {
                             // Advance to the next step
-                            if !remaining_steps.is_empty() {
-                                // NOTE: this would be much more efficient if we used a VecDeque, but
-                                // that isn't bridged by UniFFI. Revisit later.
-                                Some(remaining_steps.remove(0))
-                            } else {
-                                None
+                            let update = do_advance_to_next_step(
+                                &snapped_user_location,
+                                &remaining_waypoints,
+                                remaining_steps,
+                            );
+                            match update {
+                                NavigationStateUpdate::Navigating { current_step, .. } => {
+                                    Some(current_step)
+                                }
+                                NavigationStateUpdate::Arrived { .. } => {
+                                    *guard = TripState::Complete;
+                                    None
+                                }
                             }
                         } else {
                             Some(current_step.clone())
                         };
 
+                        // TODO: Calculate distance to the next step
+                        // Hmm... We don't currently store the LineString for the current step...
+                        // let fraction_along_line = route_line_string.line_locate_point(&point!(x: snapped_user_location.coordinates.lng, y: snapped_user_location.coordinates.lat));
 
                         if let Some(step) = current_step {
                             NavigationStateUpdate::Navigating {
                                 snapped_user_location,
-                                remaining_waypoints: remaining_waypoints.clone(),
+                                remaining_waypoints,
                                 current_step: step,
                                 spoken_instruction: None,
                                 visual_instructions: None,

common/ferrostar-core/src/navigation_controller/models.rs

@@ -15,6 +15,8 @@ pub(super) enum TripState {
         /// the route to the final destination are discarded as they are visited.
         /// TODO: Do these need additional details like a name/label?
         remaining_waypoints: Vec<GeographicCoordinates>,
+        /// The ordered list of steps that remain in the trip.
+        /// The step at the front of the list is always the current step.
         remaining_steps: Vec<RouteStep>,
     },
     Complete,
@@ -27,8 +29,7 @@ pub enum NavigationStateUpdate {
         /// The ordered list of waypoints remaining to visit on this trip. Intermediate waypoints on
         /// the route to the final destination are discarded as they are visited.
         remaining_waypoints: Vec<GeographicCoordinates>,
-        /// The ordered list of steps to complete during the rest of the trip. Steps are discarded
-        /// as they are completed.
+        /// The current/active maneuver. Properties such as the distance will be updated live.
         current_step: RouteStep,
         visual_instructions: Option<VisualInstructions>,
         spoken_instruction: Option<SpokenInstruction>,

common/ferrostar-core/src/navigation_controller/utils.rs

@@ -6,9 +6,11 @@ use proptest::prelude::*;
 
 #[cfg(test)]
 use std::time::SystemTime;
+use super::ARRIVED_EOT;
+use crate::NavigationStateUpdate;
 
 /// Snaps a user location to the closest point on a route line.
-pub fn snap_to_line(location: UserLocation, line: &LineString) -> UserLocation {
+pub fn snap_to_line(location: &UserLocation, line: &LineString) -> UserLocation {
     let original_point = Point::new(location.coordinates.lng, location.coordinates.lat);
 
     match line.haversine_closest_point(&original_point) {
@@ -17,9 +19,9 @@ pub fn snap_to_line(location: UserLocation, line: &LineString) -> UserLocation {
                 lng: snapped.x(),
                 lat: snapped.y(),
             },
-            ..location
+            ..*location
         },
-        Closest::Indeterminate => location,
+        Closest::Indeterminate => *location,
     }
 }
 
@@ -40,6 +42,38 @@ pub fn has_completed_step(route_step: &RouteStep, user_location: &UserLocation)
     return distance_to_end < 5.0;
 }
 
+pub fn do_advance_to_next_step(
+        snapped_user_location: &UserLocation,
+        remaining_waypoints: &Vec<GeographicCoordinates>,
+        remaining_steps: &mut Vec<RouteStep>,
+    ) -> NavigationStateUpdate {
+        if remaining_steps.is_empty() {
+            return ARRIVED_EOT;
+        };
+
+        // Advance to the next step
+        let current_step = if !remaining_steps.is_empty() {
+            // NOTE: this would be much more efficient if we used a VecDeque, but
+            // that isn't bridged by UniFFI. Revisit later.
+            remaining_steps.remove(0);
+            remaining_steps.first()
+        } else {
+            None
+        };
+
+        if let Some(step) = current_step {
+            NavigationStateUpdate::Navigating {
+                snapped_user_location: *snapped_user_location,
+                remaining_waypoints: remaining_waypoints.clone(),
+                current_step: step.clone(),
+                spoken_instruction: None,
+                visual_instructions: None,
+            }
+        } else {
+            ARRIVED_EOT
+        }
+    }
+
 #[cfg(test)]
 proptest! {
     #[test]