xgraph 3.0.0

//! Weighted shortest paths, written once against [`GraphView`].
//!
//! Dijkstra's algorithm, reporting either distances alone, distances with the predecessor
//! tree, or one reconstructed route.
//!
//! Weights need only be partially ordered, so floating-point weights work — the homogeneous
//! trait used to demand `Ord`, which excluded `f64` from the library's flagship algorithm
//! while every example in the documentation used it. Preconditions the algorithm actually
//! depends on are checked rather than assumed: a negative weight or a weight that does not
//! compare with itself is reported, instead of yielding a plausible-looking wrong answer.

use crate::core::view::{GraphView, NodeId};
use std::cmp::Ordering;
use std::collections::{BinaryHeap, HashMap, HashSet};
use std::ops::Add;

/// Error type for shortest path computation failures.
#[derive(Debug, Clone, PartialEq, Eq, thiserror::Error)]
pub enum ShortestPathError {
    /// The start node does not exist in the graph.
    #[error("Invalid start node: node ID {0} not found")]
    InvalidStartNode(NodeId),
    /// The target node does not exist in the graph.
    #[error("Invalid target node: node ID {0} not found")]
    InvalidTargetNode(NodeId),
    /// A neighbour referenced during the search does not exist.
    #[error("Invalid neighbor node: node ID {0} not found")]
    InvalidNeighborNode(NodeId),
    /// An edge carries a negative weight, which Dijkstra's algorithm cannot handle.
    ///
    /// Use a Bellman-Ford style algorithm for graphs with negative weights.
    #[error("Edge {from} -> {to} has a negative weight; Dijkstra requires non-negative weights")]
    NegativeWeight {
        /// The node the offending edge leaves.
        from: NodeId,
        /// The node the offending edge enters.
        to: NodeId,
    },
    /// An edge carries a weight that does not compare with itself, such as a NaN.
    #[error("Edge {from} -> {to} has a weight that does not compare with itself (such as NaN)")]
    IncomparableWeight {
        /// The node the offending edge leaves.
        from: NodeId,
        /// The node the offending edge enters.
        to: NodeId,
    },
}

/// A node queued in Dijkstra's algorithm, ordered by the cost of reaching it.
///
/// The ordering is inverted so that Rust's max-heap behaves as the min-heap the algorithm
/// needs. Every cost is checked to compare with itself before it reaches the queue, so the
/// fallback to `Ordering::Equal` below is unreachable and never silently reorders it.
#[derive(Copy, Clone)]
struct State<W> {
    cost: W,
    node: NodeId,
}

impl<W: PartialOrd> PartialEq for State<W> {
    fn eq(&self, other: &Self) -> bool {
        self.cost.partial_cmp(&other.cost) == Some(Ordering::Equal)
    }
}

impl<W: PartialOrd> Eq for State<W> {}

impl<W: PartialOrd> Ord for State<W> {
    fn cmp(&self, other: &Self) -> Ordering {
        other
            .cost
            .partial_cmp(&self.cost)
            .unwrap_or(Ordering::Equal)
    }
}

impl<W: PartialOrd> PartialOrd for State<W> {
    fn partial_cmp(&self, other: &Self) -> Option<Ordering> {
        Some(self.cmp(other))
    }
}

/// Weighted shortest paths, available on every [`GraphView`] whose weights can be added
/// and compared.
pub trait ShortestPath: GraphView {
    /// Shortest-path distances from `start` to every reachable node.
    ///
    /// Unreachable nodes are absent from the map rather than present with an infinite
    /// distance, so the weight type needs no notion of infinity.
    fn dijkstra(&self, start: NodeId) -> Result<HashMap<NodeId, Self::Weight>, ShortestPathError>
    where
        Self::Weight: Add<Output = Self::Weight> + PartialOrd + Default,
    {
        self.dijkstra_with_predecessors(start)
            .map(|(distances, _)| distances)
    }

    /// Shortest-path distances together with the predecessor tree.
    ///
    /// The second map sends each reachable node other than `start` to the node preceding it
    /// on some shortest route, which is what makes a route reconstructable.
    #[allow(clippy::type_complexity)]
    fn dijkstra_with_predecessors(
        &self,
        start: NodeId,
    ) -> Result<(HashMap<NodeId, Self::Weight>, HashMap<NodeId, NodeId>), ShortestPathError>
    where
        Self::Weight: Add<Output = Self::Weight> + PartialOrd + Default,
    {
        if !self.contains_node(start) {
            return Err(ShortestPathError::InvalidStartNode(start));
        }

        let zero = Self::Weight::default();
        let mut distances = HashMap::from([(start, zero)]);
        let mut predecessors = HashMap::new();
        let mut heap = BinaryHeap::from([State {
            cost: zero,
            node: start,
        }]);

        while let Some(State { cost, node }) = heap.pop() {
            // Skip stale queue entries: a shorter route to this node was settled after this
            // entry was pushed.
            match distances.get(&node) {
                Some(best) if cost > *best => continue,
                None => continue,
                _ => {}
            }

            // One link per incident edge, so parallel edges are all relaxed and the
            // cheapest of them wins without needing a separate minimum-weight step.
            for link in self.links(node) {
                if !self.contains_node(link.target) {
                    return Err(ShortestPathError::InvalidNeighborNode(link.target));
                }
                if link.weight < zero {
                    return Err(ShortestPathError::NegativeWeight {
                        from: node,
                        to: link.target,
                    });
                }

                let next_cost = cost + link.weight;
                // A value that does not compare with itself would make the queue's ordering
                // incoherent, so it is rejected before it can enter.
                if next_cost.partial_cmp(&next_cost).is_none() {
                    return Err(ShortestPathError::IncomparableWeight {
                        from: node,
                        to: link.target,
                    });
                }

                let improved = match distances.get(&link.target) {
                    Some(current) => next_cost < *current,
                    None => true,
                };
                if improved {
                    distances.insert(link.target, next_cost);
                    predecessors.insert(link.target, node);
                    heap.push(State {
                        cost: next_cost,
                        node: link.target,
                    });
                }
            }
        }

        Ok((distances, predecessors))
    }

    /// Finds a shortest route to one target, guided by an estimate of the distance
    /// remaining.
    ///
    /// This is A*. Where [`shortest_path_to`](ShortestPath::shortest_path_to) settles every
    /// node closer than the target, this settles only those the estimate cannot rule out,
    /// which on a large graph with a good estimate is a small fraction of them.
    ///
    /// `heuristic` must never overestimate the remaining distance — it must be
    /// *admissible*. An estimate that overshoots makes A* return a route that is not the
    /// shortest. Straight-line distance is the usual admissible choice for a map; a
    /// heuristic that always returns zero reduces this to Dijkstra exactly.
    ///
    /// # Returns
    /// - `Ok(Some((route, cost)))` where `route` starts at `start` and ends at `target`.
    /// - `Ok(None)` if `target` is unreachable from `start`.
    #[allow(clippy::type_complexity)]
    fn astar_path_to<H>(
        &self,
        start: NodeId,
        target: NodeId,
        heuristic: H,
    ) -> Result<Option<(Vec<NodeId>, Self::Weight)>, ShortestPathError>
    where
        Self::Weight: Add<Output = Self::Weight> + PartialOrd + Default,
        H: Fn(NodeId) -> Self::Weight,
    {
        if !self.contains_node(start) {
            return Err(ShortestPathError::InvalidStartNode(start));
        }
        if !self.contains_node(target) {
            return Err(ShortestPathError::InvalidTargetNode(target));
        }

        let zero = Self::Weight::default();
        let mut best: HashMap<NodeId, Self::Weight> = HashMap::from([(start, zero)]);
        let mut predecessors: HashMap<NodeId, NodeId> = HashMap::new();
        let mut settled: HashSet<NodeId> = HashSet::new();
        // Ordered by cost-so-far plus estimate-to-go, which is what makes the search head
        // towards the target rather than spreading evenly.
        let mut queue = BinaryHeap::from([State {
            cost: zero + heuristic(start),
            node: start,
        }]);

        while let Some(State { node, .. }) = queue.pop() {
            if node == target {
                let mut route = vec![target];
                let mut current = target;
                while current != start {
                    current = predecessors[&current];
                    route.push(current);
                }
                route.reverse();
                return Ok(Some((route, best[&target])));
            }
            if !settled.insert(node) {
                continue;
            }

            let here = best[&node];
            for link in self.links(node) {
                if !self.contains_node(link.target) {
                    return Err(ShortestPathError::InvalidNeighborNode(link.target));
                }
                if link.weight < zero {
                    return Err(ShortestPathError::NegativeWeight {
                        from: node,
                        to: link.target,
                    });
                }

                let next = here + link.weight;
                if next.partial_cmp(&next).is_none() {
                    return Err(ShortestPathError::IncomparableWeight {
                        from: node,
                        to: link.target,
                    });
                }

                let improved = match best.get(&link.target) {
                    Some(current) => next < *current,
                    None => true,
                };
                if improved {
                    best.insert(link.target, next);
                    predecessors.insert(link.target, node);
                    queue.push(State {
                        cost: next + heuristic(link.target),
                        node: link.target,
                    });
                }
            }
        }
        Ok(None)
    }

    /// Reconstructs one shortest route between two nodes.
    ///
    /// # Returns
    /// - `Ok(Some((route, cost)))` where `route` starts at `start` and ends at `target`.
    /// - `Ok(None)` if `target` is unreachable from `start`.
    #[allow(clippy::type_complexity)]
    fn shortest_path_to(
        &self,
        start: NodeId,
        target: NodeId,
    ) -> Result<Option<(Vec<NodeId>, Self::Weight)>, ShortestPathError>
    where
        Self::Weight: Add<Output = Self::Weight> + PartialOrd + Default,
    {
        // Validated in argument order, so a call with two bad IDs names the first one.
        if !self.contains_node(start) {
            return Err(ShortestPathError::InvalidStartNode(start));
        }
        if !self.contains_node(target) {
            return Err(ShortestPathError::InvalidTargetNode(target));
        }

        let (distances, predecessors) = self.dijkstra_with_predecessors(start)?;
        let Some(&total) = distances.get(&target) else {
            return Ok(None);
        };

        let mut route = vec![target];
        let mut current = target;
        while current != start {
            current = *predecessors
                .get(&current)
                .expect("every reachable node other than the start has a predecessor");
            route.push(current);
        }
        route.reverse();

        Ok(Some((route, total)))
    }
}

impl<G: GraphView + ?Sized> ShortestPath for G {}

#[cfg(test)]
#[cfg(feature = "graph")]
mod tests {
    use super::*;
    use crate::core::EdgeFiltered;
    use crate::graph::Graph;

    #[test]
    fn test_float_weights_are_supported() {
        let mut graph = Graph::<f64, u32, ()>::new(false);
        let a = graph.add_node(0);
        let b = graph.add_node(1);
        let c = graph.add_node(2);
        graph.add_edge(a, b, 1.5, ()).unwrap();
        graph.add_edge(b, c, 2.25, ()).unwrap();
        graph.add_edge(a, c, 10.0, ()).unwrap();

        assert_eq!(graph.dijkstra(a).unwrap()[&c], 3.75);
    }

    #[test]
    fn test_the_cheapest_of_several_parallel_edges_wins() {
        let mut graph = Graph::<u32, u32, ()>::new(false);
        let a = graph.add_node(0);
        let b = graph.add_node(1);
        graph.add_edge(a, b, 9, ()).unwrap();
        graph.add_edge(a, b, 3, ()).unwrap();
        graph.add_edge(a, b, 7, ()).unwrap();

        assert_eq!(graph.dijkstra(a).unwrap()[&b], 3);
    }

    #[test]
    fn test_route_reconstruction() {
        let mut graph = Graph::<u32, u32, ()>::new(false);
        let a = graph.add_node(0);
        let b = graph.add_node(1);
        let c = graph.add_node(2);
        let d = graph.add_node(3);
        graph.add_edge(a, b, 1, ()).unwrap();
        graph.add_edge(b, d, 1, ()).unwrap();
        graph.add_edge(a, c, 5, ()).unwrap();
        graph.add_edge(c, d, 5, ()).unwrap();

        assert_eq!(
            graph.shortest_path_to(a, d).unwrap(),
            Some((vec![a, b, d], 2))
        );
        assert_eq!(graph.shortest_path_to(a, a).unwrap(), Some((vec![a], 0)));
    }

    #[test]
    fn test_unreachable_is_none_and_unknown_is_an_error() {
        let mut graph = Graph::<u32, u32, ()>::new(true);
        let a = graph.add_node(0);
        let b = graph.add_node(1);

        assert_eq!(graph.shortest_path_to(a, b).unwrap(), None);
        assert_eq!(
            graph.shortest_path_to(a, NodeId::from_index(999)),
            Err(ShortestPathError::InvalidTargetNode(NodeId::from_index(
                999
            )))
        );
        assert_eq!(
            graph.dijkstra(NodeId::from_index(999)),
            Err(ShortestPathError::InvalidStartNode(NodeId::from_index(999)))
        );
    }

    #[test]
    fn test_violated_preconditions_are_reported() {
        let mut negative = Graph::<i32, u32, ()>::new(true);
        let a = negative.add_node(0);
        let b = negative.add_node(1);
        negative.add_edge(a, b, -3, ()).unwrap();
        assert!(matches!(
            negative.dijkstra(a),
            Err(ShortestPathError::NegativeWeight { .. })
        ));

        let mut nan = Graph::<f64, u32, ()>::new(true);
        let a = nan.add_node(0);
        let b = nan.add_node(1);
        nan.add_edge(a, b, f64::NAN, ()).unwrap();
        assert!(matches!(
            nan.dijkstra(a),
            Err(ShortestPathError::IncomparableWeight { .. })
        ));
    }

    #[test]
    fn test_dijkstra_over_a_filtered_view() {
        let mut graph = Graph::<u32, u32, &str>::new(false);
        let a = graph.add_node(0);
        let b = graph.add_node(1);
        let slow = graph.add_edge(a, b, 10, "road").unwrap();
        graph.add_edge(a, b, 2, "rail").unwrap();

        assert_eq!(graph.dijkstra(a).unwrap()[&b], 2);
        let road_only = EdgeFiltered::new(&graph, |edge| edge == slow);
        assert_eq!(road_only.dijkstra(a).unwrap()[&b], 10);
    }

    /// A square grid graph, with each node holding its own (row, column).
    type Grid = (Graph<u32, (usize, usize), ()>, Vec<Vec<NodeId>>);

    /// A grid where the straight-line distance is an admissible estimate.
    fn grid(side: usize) -> Grid {
        let mut graph = Graph::<u32, (usize, usize), ()>::new(false);
        let ids: Vec<Vec<NodeId>> = (0..side)
            .map(|row| (0..side).map(|col| graph.add_node((row, col))).collect())
            .collect();
        for row in 0..side {
            for col in 0..side {
                if row + 1 < side {
                    graph
                        .add_edge(ids[row][col], ids[row + 1][col], 1, ())
                        .unwrap();
                }
                if col + 1 < side {
                    graph
                        .add_edge(ids[row][col], ids[row][col + 1], 1, ())
                        .unwrap();
                }
            }
        }
        (graph, ids)
    }

    /// A* with an admissible estimate must agree with Dijkstra on the cost.
    #[test]
    fn test_astar_agrees_with_dijkstra() {
        let (graph, ids) = grid(8);
        let start = ids[0][0];
        let target = ids[7][7];

        let manhattan = |node: NodeId| {
            let &(row, col) = &graph.node(node).unwrap().data;
            (7 - row + 7 - col) as u32
        };

        let (astar_route, astar_cost) = graph
            .astar_path_to(start, target, manhattan)
            .unwrap()
            .unwrap();
        let (_, dijkstra_cost) = graph.shortest_path_to(start, target).unwrap().unwrap();

        assert_eq!(astar_cost, dijkstra_cost);
        assert_eq!(astar_route.first(), Some(&start));
        assert_eq!(astar_route.last(), Some(&target));
    }

    /// A heuristic that always estimates zero is admissible, so A* must still be correct.
    #[test]
    fn test_a_zero_heuristic_reduces_to_dijkstra() {
        let (graph, ids) = grid(5);
        let start = ids[0][0];
        let target = ids[4][4];

        let (_, with_zero) = graph.astar_path_to(start, target, |_| 0).unwrap().unwrap();
        let (_, dijkstra) = graph.shortest_path_to(start, target).unwrap().unwrap();
        assert_eq!(with_zero, dijkstra);
    }

    #[test]
    fn test_astar_reports_an_unreachable_target() {
        let mut graph = Graph::<u32, u32, ()>::new(true);
        let a = graph.add_node(0);
        let b = graph.add_node(1);

        assert_eq!(graph.astar_path_to(a, b, |_| 0).unwrap(), None);
        assert_eq!(
            graph.astar_path_to(a, NodeId::from_index(999), |_| 0),
            Err(ShortestPathError::InvalidTargetNode(NodeId::from_index(
                999
            )))
        );
    }

    #[test]
    fn test_astar_rejects_a_negative_weight() {
        let mut graph = Graph::<i32, u32, ()>::new(true);
        let a = graph.add_node(0);
        let b = graph.add_node(1);
        graph.add_edge(a, b, -3, ()).unwrap();

        assert!(matches!(
            graph.astar_path_to(a, b, |_| 0),
            Err(ShortestPathError::NegativeWeight { .. })
        ));
    }

    #[test]
    fn test_astar_from_a_node_to_itself() {
        let (graph, ids) = grid(3);
        let start = ids[0][0];

        assert_eq!(
            graph.astar_path_to(start, start, |_| 0).unwrap(),
            Some((vec![start], 0))
        );
    }
}