router: have probing and visiting as separate states

This feels overengineered, but I need something like this for better
debuginfo.

src/router/astar.rs

@@ -96,7 +96,7 @@ where
     }
 }
 
-pub trait AstarStrategy<G, K, R>
+pub trait AstarStrategy<G, K, PS, PE, R>
 where
     G: GraphBase,
     G::NodeId: Eq + Hash,
@@ -104,11 +104,11 @@ where
     K: Measure + Copy,
 {
     fn is_goal(&mut self, graph: &G, node: G::NodeId, tracker: &PathTracker<G>) -> Option<R>;
-    fn edge_cost<'a>(
+    fn probe<'a>(
         &mut self,
         graph: &'a G,
         edge: <&'a G as IntoEdgeReferences>::EdgeRef,
-    ) -> Option<K>;
+    ) -> Result<(K, PS), PE>;
     fn estimate_cost(&mut self, graph: &G, node: G::NodeId) -> K;
 }
 
@@ -140,14 +140,15 @@ pub enum AstarError {
 }
 
 #[derive(Debug)]
-pub enum AstarStatus<G, K, R>
+pub enum AstarStatus<G, K, PS, PE, R>
 where
     G: GraphBase,
     G::NodeId: Eq + Hash,
     for<'a> &'a G: IntoEdges<NodeId = G::NodeId, EdgeId = G::EdgeId> + MakeEdgeRef,
     K: Measure + Copy,
 {
-    Running,
+    Probed(Result<PS, PE>),
+    Visited,
     Finished(K, Vec<G::NodeId>, R),
 }
 
@@ -158,7 +159,11 @@ where
     for<'a> &'a G: IntoEdges<NodeId = G::NodeId, EdgeId = G::EdgeId> + MakeEdgeRef,
     K: Measure + Copy,
 {
-    pub fn new<R>(graph: G, start: G::NodeId, strategy: &mut impl AstarStrategy<G, K, R>) -> Self {
+    pub fn new<PS, PE, R>(
+        graph: G,
+        start: G::NodeId,
+        strategy: &mut impl AstarStrategy<G, K, PS, PE, R>,
+    ) -> Self {
         let mut this = Self {
             graph,
             visit_next: BinaryHeap::new(),
@@ -178,10 +183,10 @@ where
         this
     }
 
-    pub fn step<R>(
+    pub fn step<PS, PE, R>(
         &mut self,
-        strategy: &mut impl AstarStrategy<G, K, R>,
+        strategy: &mut impl AstarStrategy<G, K, PS, PE, R>,
-    ) -> Result<AstarStatus<G, K, R>, AstarError> {
+    ) -> Result<AstarStatus<G, K, PS, PE, R>, AstarError> {
         if let Some(curr_node) = self.maybe_curr_node {
             if let Some(edge_id) = self.edge_ids.pop_front() {
                 // This lookup can be unwrapped without fear of panic since the node was
@@ -189,7 +194,8 @@ where
                 let node_score = self.scores[&curr_node];
                 let edge = (&self.graph).edge_ref(edge_id);
 
-                if let Some(edge_cost) = strategy.edge_cost(&self.graph, edge) {
+                match strategy.probe(&self.graph, edge) {
+                    Ok((edge_cost, probe_status)) => {
                         let next = edge.target();
                         let next_score = node_score + edge_cost;
 
@@ -198,7 +204,7 @@ where
                                 // No need to add neighbors that we have already reached through a
                                 // shorter path than now.
                                 if *entry.get() <= next_score {
-                                return Ok(AstarStatus::Running);
+                                    return Ok(AstarStatus::Probed(Ok(probe_status)));
                                 }
                                 entry.insert(next_score);
                             }
@@ -211,11 +217,16 @@ where
                         let next_estimate_score =
                             next_score + strategy.estimate_cost(&self.graph, next);
                         self.visit_next.push(MinScored(next_estimate_score, next));
+
+                        return Ok(AstarStatus::Probed(Ok(probe_status)));
+                    }
+                    Err(probe_err) => return Ok(AstarStatus::Probed(Err(probe_err))),
                 }
             } else {
                 self.maybe_curr_node = None;
             }
-        } else {
+        }
+
         let Some(MinScored(estimate_score, node)) = self.visit_next.pop() else {
             return Err(AstarError::NotFound);
         };
@@ -231,7 +242,7 @@ where
                 // If the node has already been visited with an equal or lower score than
                 // now, then we do not need to re-visit it.
                 if *entry.get() <= estimate_score {
-                        return Ok(AstarStatus::Running);
+                    return Ok(AstarStatus::Visited);
                 }
                 entry.insert(estimate_score);
             }
@@ -242,16 +253,15 @@ where
 
         self.maybe_curr_node = Some(node);
         self.edge_ids = self.graph.edges(node).map(|edge| edge.id()).collect();
-        }
 
-        Ok(AstarStatus::Running)
+        Ok(AstarStatus::Visited)
     }
 }
 
-pub fn astar<G, K, R>(
+pub fn astar<G, K, PS, PE, R>(
     graph: G,
     start: G::NodeId,
-    strategy: &mut impl AstarStrategy<G, K, R>,
+    strategy: &mut impl AstarStrategy<G, K, PS, PE, R>,
 ) -> Result<(K, Vec<G::NodeId>, R), AstarError>
 where
     G: GraphBase,

src/router/route.rs

@@ -73,7 +73,7 @@ impl Route {
         let mut strategy = RouterAstarStrategy::new(tracer, &mut self.trace, target);
 
         match self.astar.step(&mut strategy)? {
-            AstarStatus::Running => Ok(RouterStatus::Running),
+            AstarStatus::Probed(..) | AstarStatus::Visited => Ok(RouterStatus::Running),
             AstarStatus::Finished(_cost, _path, band) => Ok(RouterStatus::Finished(band)),
         }
     }

src/router/router.rs

@@ -71,7 +71,7 @@ impl<'a, R: AccessRules> RouterAstarStrategy<'a, R> {
     }
 }
 
-impl<'a, R: AccessRules> AstarStrategy<Navmesh, f64, BandFirstSegIndex>
+impl<'a, R: AccessRules> AstarStrategy<Navmesh, f64, (), (), BandFirstSegIndex>
     for RouterAstarStrategy<'a, R>
 {
     fn is_goal(
@@ -92,9 +92,9 @@ impl<'a, R: AccessRules> AstarStrategy<Navmesh, f64, BandFirstSegIndex>
             .ok()
     }
 
-    fn edge_cost(&mut self, navmesh: &Navmesh, edge: NavmeshEdgeReference) -> Option<f64> {
+    fn probe(&mut self, navmesh: &Navmesh, edge: NavmeshEdgeReference) -> Result<(f64, ()), ()> {
         if edge.target().petgraph_index() == self.target.petgraph_index() {
-            return None;
+            return Err(());
         }
 
         let prev_bihead_length = self.bihead_length();
@@ -108,9 +108,9 @@ impl<'a, R: AccessRules> AstarStrategy<Navmesh, f64, BandFirstSegIndex>
 
         if result.is_ok() {
             self.trace.undo_step(&mut self.tracer);
-            Some(probe_length)
+            Ok((probe_length, ()))
         } else {
-            None
+            Err(())
         }
     }