paths.rs

/*!
# Shortest Paths Algorithms

This module provides a collection of shortest‑paths algorithms for the Graphina library.
It supports single‑source and all‑pairs computations via (classical) algorithms including:

- **Dijkstra’s Algorithm:**
  Computes single‑source shortest paths for graphs with nonnegative weights.

- **Bellman–Ford Algorithm:**
  Computes single‑source shortest paths even with negative weights and detects negative cycles.

- **A\* (A-Star) Algorithm:**
  Finds a shortest path from a source to a target using an admissible heuristic.

- **Floyd–Warshall Algorithm:**
  Computes all‑pairs shortest paths using dynamic programming.

- **Johnson’s Algorithm:**
  Computes all‑pairs shortest paths for sparse graphs (even with negative edge weights) by re-weighting the graph and then running Dijkstra’s algorithm from each node.


## Error Handling

Preconditions for each algorithm are enforced at runtime using `graphina::core::error::GraphinaError`.
For example, algorithms that require nonnegative edge weights will return a `Result` containing a
`GraphinaError::InvalidArgument` if a negative weight is encountered. Users should handle these
`Result` types accordingly.

*/

use crate::core::error::{GraphinaError, Result};
use crate::core::types::{BaseGraph, GraphConstructor, GraphinaGraph, NodeId, NodeMap};
use std::cmp::Reverse;
use std::collections::{BinaryHeap, VecDeque};
use std::fmt::Debug;
use std::ops::{Add, Sub};

use ordered_float::NotNan;

/// Result type for pathfinding algorithms: (distances, predecessors).
pub type PathFindResult = (NodeMap<Option<f64>>, NodeMap<Option<NodeId>>);

/// Returns an iterator over outgoing edges from a given node as `(target, weight)`.
fn outgoing_edges<A, W, Ty>(
    graph: &BaseGraph<A, W, Ty>,
    u: NodeId,
) -> impl Iterator<Item = (NodeId, W)> + '_
where
    W: Copy,
    Ty: GraphConstructor<A, W>,
{
    // Delegate to the graph's own incident-edge iterator, which follows petgraph
    // adjacency: outgoing edges for a directed graph, and all incident edges for
    // an undirected graph. Filtering `graph.edges()` by source would miss the
    // undirected edges stored with `u` as the target.
    graph.outgoing_edges(u).map(|(tgt, w)| (tgt, *w))
}

/// Returns an upper bound on node indices, suitable for sizing a dense `Vec`
/// indexed by `NodeId::index()`.
///
/// `BaseGraph` wraps a `StableGraph`, so node indices are stable but not
/// necessarily contiguous after removals. Sizing by this bound (rather than
/// `node_count`) keeps `vec[id.index()]` in range while still allowing dense,
/// hash-free indexing in the inner loops.
fn index_bound<A, W, Ty>(graph: &BaseGraph<A, W, Ty>) -> usize
where
    Ty: GraphConstructor<A, W>,
{
    graph
        .node_ids()
        .map(|n| n.index())
        .max()
        .map_or(0, |m| m + 1)
}

/// Converts a dense, index-keyed slice of per-node values into the `NodeMap`
/// public return type, inserting one entry per existing node.
fn dense_to_nodemap<A, W, Ty, T>(graph: &BaseGraph<A, W, Ty>, dense: &[T]) -> NodeMap<T>
where
    T: Copy,
    Ty: GraphConstructor<A, W>,
{
    let mut map = NodeMap::with_capacity_and_hasher(graph.node_count(), rustc_hash::FxBuildHasher);
    for u in graph.node_ids() {
        map.insert(u, dense[u.index()]);
    }
    map
}

// ============================
// Dijkstra’s Algorithm
// ============================
//

/// Generic, Full implementationof Dijkstra's algorithm for finding shortest paths in a graph
/// with non-negative weights.
///
/// # Arguments
///
/// * `graph`: the target graph.
/// * `source`: the source of path finding.
/// * `cutoff`: the maximum total cost before stopping search.
/// * `eval_cost`: callback to evaluate the cost of possible edges in the graph, returning
///     - `Some(f64)` for cost,
///     - `None` for not passable edge.
///
/// # Returns
///
/// - `Vec<Option<f64>>` in which `None` for unreachable, and `Some(cost)` for the total path cost.
/// - `Vec<Option<NodeID>>` in which `None` for no traceback (i.e. is source or unreachable),
///   and `Some(NodeId)` for the previous node visited in the path.
///
/// # Error
///
/// return error, if encounter negative cost, or encounter `NaN` weight.
///
/// # Example
/// ```rust
/// use graphina::core::types::Digraph;
///
/// use graphina::core::paths::dijkstra_path_impl;
///
/// let mut graph: Digraph<String, (f64, String)> = Digraph::new();
/// //                             ^^^^^^^^^^^^^
/// //                                         L arbitrary type as edge
///
/// let cities = ["ATL", "PEK", "LHR", "HND", "CDG", "FRA", "HKG"];
///
/// let ids = cities
///     .iter()
///     .map(|s| graph.add_node(s.to_string()))
///     .collect::<Vec<_>>();
///
/// let edges = [
///     //
///     ("ATL", "PEK", (900.0, "boeing")),
///     ("ATL", "LHR", (500.0, "airbus")),
///     ("ATL", "HND", (700.0, "airbus")),
///     //
///     ("PEK", "LHR", (800.0, "boeing")),
///     ("PEK", "HND", (100.0, "airbus")),
///     ("PEK", "HKG", (100.0, "airbus")),
///     //
///     ("LHR", "CDG", (100.0, "airbus")),
///     ("LHR", "FRA", (200.0, "boeing")),
///     ("LHR", "HND", (600.0, "airbus")),
///     //
///     ("HND", "ATL", (700.0, "airbus")),
///     ("HND", "FRA", (600.0, "airbus")),
///     ("HND", "HKG", (100.0, "airbus")),
///     //
/// ];
///
/// for (s, d, w) in edges {
///     let depart = cities.iter().position(|city| s == *city).unwrap();
///     let destin = cities.iter().position(|city| d == *city).unwrap();
///     graph.add_edge(ids[depart], ids[destin], (w.0, w.1.to_string()));
/// }
///
/// // function for evaluating possible cost for the edge
/// // Some(f64) for cost
/// // None for impassable
/// let eval_cost = |(price, manufactuer): &(f64, String)| match manufactuer.as_str() {
///     "boeing" => None,  // avoid boeing plane
///     _ => Some(*price), // return price as the cost
/// };
///
/// let (cost, trace) = dijkstra_path_impl(&graph, ids[0], Some(1000.0), eval_cost).unwrap();
/// println!("cost : {:?}", cost);
/// println!("trace: {:?}", trace);
///
/// let expected_cost = [
///     Some(0.0),
///     None,
///     Some(500.0),
///     Some(700.0),
///     Some(600.0),
///     None,
///     Some(800.0),
/// ];
/// let expected_trace = [
///     None,
///     None,
///     Some(ids[0]),
///     Some(ids[0]),
///     Some(ids[2]),
///     None,
///     Some(ids[3]),
/// ];
///
/// for id in ids {
///     assert_eq!(cost[&id], expected_cost[id.index()]);
///     assert_eq!(trace[&id], expected_trace[id.index()]);
/// }
/// ```
pub fn dijkstra_path_impl<A, W, Ty>(
    graph: &BaseGraph<A, W, Ty>,
    source: NodeId,
    cutoff: Option<f64>,
    eval_cost: impl Fn(&W) -> Option<f64>,
) -> Result<PathFindResult>
where
    W: Debug,
    A: Debug,
    Ty: GraphConstructor<A, W>,
    NodeId: Ord,
    BaseGraph<A, W, Ty>: GraphinaGraph<A, W>,
{
    // Dense, index-keyed buffers (see `dijkstra`). A cutoff or impassable-edge
    // search may touch few nodes, but the return contract is a complete map (one
    // entry per node, `None` when unreachable), so we fill from the dense buffers
    // at the end rather than building two full maps up front.
    if graph.node_attr(source).is_none() {
        return Err(GraphinaError::node_not_found(format!(
            "Dijkstra source node {:?} does not exist in the graph",
            source
        )));
    }

    let bound = index_bound(graph);
    let mut dist: Vec<Option<f64>> = vec![None; bound];
    let mut trace: Vec<Option<NodeId>> = vec![None; bound];
    let mut heap = BinaryHeap::new();

    dist[source.index()] = Some(0.0);

    heap.push(Reverse((
        NotNan::new(0.0).unwrap_or_else(|_| NotNan::new(1.0).unwrap_or(NotNan::from(1))),
        source,
    )));

    while let Some(Reverse((d, u))) = heap.pop() {
        if let Some(current) = dist[u.index()] {
            if *d > current {
                continue;
            }
        }
        for (v, edge) in graph.outgoing_edges(u) {
            let Some(w) = eval_cost(edge) else {
                continue;
            };
            if w.is_sign_negative() {
                return Err(GraphinaError::invalid_argument(format!(
                    "Dijkstra requires nonnegative costs, but found cost: {:?}, src: {:?}, dst: {:?}, edge: {:?}",
                    w, u, v, edge
                )));
            }
            let Ok(w) = NotNan::new(w) else {
                return Err(GraphinaError::invalid_argument(format!(
                    "Dijkstra requires not NaN costs, but found cost: {:?}, src: {:?}, dst: {:?}, edge: {:?}",
                    w, u, v, edge
                )));
            };
            let next = d + w;
            if let Some(cutoff) = cutoff {
                if *next > cutoff {
                    continue;
                }
            }
            let vi = v.index();
            if dist[vi].is_none() || Some(*next) < dist[vi] {
                dist[vi] = Some(*next);
                trace[vi] = Some(u);
                heap.push(Reverse((next, v)));
            }
        }
    }
    Ok((
        dense_to_nodemap(graph, &dist),
        dense_to_nodemap(graph, &trace),
    ))
}

/// Full implementation of Dijkstra's algorithm for finding shortest paths in a graph
/// for graph with edge type `f64`
/// with non-negative weights.
///
/// # Arguments
///
/// * `graph`: the target graph.
/// * `source`: the source of path finding.
/// * `cutoff`: the maximum total cost before stopping search.
///
/// # Returns
///
/// - `Vec<Option<f64>>` in which `None` for unreachable, and `Some(cost)` for the total path cost.
/// - `Vec<Option<NodeID>>` in which `None` for no traceback (i.e. is source or unreachable),
///   and `Some(NodeId)` for the previous node visited in the path.
///
/// # Error
///
/// return error, if encounter negative cost, or encounter `NaN` weight.
///
/// # Example
/// ```rust
/// use graphina::core::types::Graph;
///
/// use graphina::core::paths::dijkstra_path_f64;
///
/// let mut graph = Graph::new();
/// let ids = (0..5).map(|i| graph.add_node(i)).collect::<Vec<_>>();
/// let edges = [(0, 1, 1.0), (1, 2, 1.0), (2, 3, 2.0), (3, 4, 1.0)];
/// for (s, d, w) in edges {
///     graph.add_edge(ids[s], ids[d], w);
/// }
///
/// let (cost, trace) = dijkstra_path_f64(&graph, ids[0], None).unwrap();
///
/// println!("cost : {:?}", cost);
/// println!("trace: {:?}", trace);
/// let expected_cost = [Some(0.0), Some(1.0), Some(2.0), Some(4.0), Some(5.0)];
/// let expected_trace = [None, Some(ids[0]), Some(ids[1]), Some(ids[2]), Some(ids[3])];
///
/// for id in ids {
///     assert_eq!(cost[&id], expected_cost[id.index()]);
///     assert_eq!(trace[&id], expected_trace[id.index()]);
/// }
/// ```
pub fn dijkstra_path_f64<A, Ty>(
    graph: &BaseGraph<A, f64, Ty>,
    source: NodeId,
    cutoff: Option<f64>,
) -> Result<PathFindResult>
where
    A: Debug,
    Ty: GraphConstructor<A, f64>,
    BaseGraph<A, f64, Ty>: GraphinaGraph<A, f64>,
{
    dijkstra_path_impl(graph, source, cutoff, |f| Some(*f))
}

/// Computes single‑source shortest paths for graphs with nonnegative weights.
///
/// # Returns
///
/// A `Result` containing a NodeMap keyed by node IDs, where each value is:
/// - `Some(cost)` if the node is reachable from the source, or
/// - `None` if it is unreachable.
///
/// Returns an `Err(GraphinaException)` if a negative edge weight is found.
///
/// # Complexity
///
/// - Time: O(E log V)
/// - Space: O(V)
pub fn dijkstra<A, W, Ty>(graph: &BaseGraph<A, W, Ty>, source: NodeId) -> Result<NodeMap<Option<W>>>
where
    W: Copy + PartialOrd + Add<Output = W> + Sub<Output = W> + From<u8> + Ord + Debug,
    Ty: GraphConstructor<A, W>,
    NodeId: Ord,
{
    if graph.node_attr(source).is_none() {
        return Err(GraphinaError::node_not_found(format!(
            "Dijkstra source node {:?} does not exist in the graph",
            source
        )));
    }

    // Dense, index-keyed distance buffer: `vec[id.index()]` is hash-free in the
    // inner loop. Converted to the `NodeMap` return type once at the end.
    let mut dist: Vec<Option<W>> = vec![None; index_bound(graph)];
    let mut heap = BinaryHeap::new();

    dist[source.index()] = Some(W::from(0u8));
    heap.push(Reverse((W::from(0u8), source)));

    while let Some(Reverse((d, u))) = heap.pop() {
        if let Some(current) = dist[u.index()] {
            if d > current {
                continue;
            }
        }
        for (v, w) in outgoing_edges(graph, u) {
            if w < W::from(0u8) {
                return Err(GraphinaError::invalid_argument(format!(
                    "Dijkstra requires nonnegative weights, but found weight: {:?}",
                    w
                )));
            }
            let next = d + w;
            let vi = v.index();
            if dist[vi].is_none() || Some(next) < dist[vi] {
                dist[vi] = Some(next);
                heap.push(Reverse((next, v)));
            }
        }
    }
    Ok(dense_to_nodemap(graph, &dist))
}

/// ============================
/// Bellman–Ford Algorithm
/// ============================
///
/// Computes single‑source shortest paths for graphs with negative weights.
/// Returns `Some(distances)` if no negative cycle is detected, or `None` otherwise.
///
/// # Complexity
///
/// - **Time:** O(VE)
/// - **Space:** O(V)
pub fn bellman_ford<A, W, Ty>(
    graph: &BaseGraph<A, W, Ty>,
    source: NodeId,
) -> Option<NodeMap<Option<W>>>
where
    W: Copy + PartialOrd + Add<Output = W> + From<u8>,
    Ty: GraphConstructor<A, W>,
{
    let n = graph.node_count();
    let bound = index_bound(graph);
    // Queue-based Bellman-Ford (SPFA): only nodes whose distance just improved are
    // re-examined, rather than relaxing every edge for a fixed V-1 rounds. On the
    // common shallow graph this behaves like BFS (O(V + E)) instead of O(V * E),
    // while still handling negative weights.
    //
    // Dense, index-keyed buffers (see `dijkstra`). `path_len[v]` is the number of
    // edges on the current shortest path to `v`; a simple path has at most n - 1
    // edges, so reaching n edges proves a negative cycle reachable from the source.
    let mut dist: Vec<Option<W>> = vec![None; bound];
    let mut in_queue = vec![false; bound];
    let mut path_len = vec![0usize; bound];
    let mut queue = VecDeque::new();

    // No error channel here: a missing source cannot panic, so report every
    // live node as unreachable instead.
    if graph.node_attr(source).is_none() {
        return Some(dense_to_nodemap(graph, &dist));
    }

    let si = source.index();
    dist[si] = Some(W::from(0u8));
    in_queue[si] = true;
    queue.push_back(source);

    while let Some(u) = queue.pop_front() {
        let ui = u.index();
        in_queue[ui] = false;
        let Some(du) = dist[ui] else {
            continue;
        };
        for (v, w) in outgoing_edges(graph, u) {
            let candidate = du + w;
            let vi = v.index();
            if dist[vi].is_none() || Some(candidate) < dist[vi] {
                dist[vi] = Some(candidate);
                path_len[vi] = path_len[ui] + 1;
                if path_len[vi] >= n {
                    return None;
                }
                if !in_queue[vi] {
                    in_queue[vi] = true;
                    queue.push_back(v);
                }
            }
        }
    }
    Some(dense_to_nodemap(graph, &dist))
}

/// ============================
/// A* (A-Star) Algorithm
/// ============================
///
/// Finds a shortest path from `source` to `target` using an admissible heuristic.
///
/// # Preconditions
///
/// - The heuristic must be admissible (i.e., it never overestimates the true cost).
///
/// # Returns
///
/// A `Result` which is `Ok(Some((total_cost, path)))` if a path is found, `Ok(None)` if no path exists,
/// or an `Err(GraphinaException)` if a negative edge weight is found.
///
/// # Complexity
///
/// - **Time:** Worst-case \(O(E \log V)\)
/// - **Space:** \(O(V)\)
pub fn a_star<A, W, Ty, F>(
    graph: &BaseGraph<A, W, Ty>,
    source: NodeId,
    target: NodeId,
    heuristic: F,
) -> Result<Option<(W, Vec<NodeId>)>>
where
    W: Copy + PartialOrd + Add<Output = W> + Sub<Output = W> + From<u8> + Ord + Debug,
    Ty: GraphConstructor<A, W>,
    F: Fn(NodeId) -> W,
    NodeId: Ord,
{
    if graph.node_attr(source).is_none() {
        return Err(GraphinaError::node_not_found(format!(
            "A* source node {:?} does not exist in the graph",
            source
        )));
    }
    if graph.node_attr(target).is_none() {
        return Err(GraphinaError::node_not_found(format!(
            "A* target node {:?} does not exist in the graph",
            target
        )));
    }

    // Buffers are keyed by `NodeId::index()`, which stays stable across node
    // removal, so they must span the index bound (max live index + 1), not the
    // node count. Sizing by `node_count()` panics once a node has been removed.
    let n = index_bound(graph);
    let mut dist = vec![None; n];
    let mut prev = vec![None; n];
    let mut heap = BinaryHeap::new();

    dist[source.index()] = Some(W::from(0u8));
    heap.push(Reverse((W::from(0u8) + heuristic(source), source)));

    while let Some(Reverse((f, u))) = heap.pop() {
        if u == target {
            break;
        }
        if let Some(current) = dist[u.index()] {
            if f - heuristic(u) > current {
                continue;
            }
        }
        for (v, w) in outgoing_edges(graph, u) {
            if w < W::from(0u8) {
                return Err(GraphinaError::invalid_argument(format!(
                    "A* requires nonnegative weights, but found weight: {:?}",
                    w
                )));
            }
            let Some(u_dist) = dist[u.index()] else {
                continue;
            };
            let tentative = u_dist + w;
            if dist[v.index()].is_none() || Some(tentative) < dist[v.index()] {
                dist[v.index()] = Some(tentative);
                prev[v.index()] = Some(u);
                let priority = tentative + heuristic(v);
                heap.push(Reverse((priority, v)));
            }
        }
    }

    if let Some(goal_cost) = dist[target.index()] {
        let mut path = Vec::new();
        let mut cur = target;
        while cur != source {
            path.push(cur);
            cur = prev[cur.index()].ok_or_else(|| {
                GraphinaError::algorithm_error("Path reconstruction failed unexpectedly.")
            })?;
        }
        path.push(source);
        path.reverse();
        Ok(Some((goal_cost, path)))
    } else {
        Ok(None)
    }
}

/// ============================
/// Floyd–Warshall Algorithm
/// ============================
///
/// Computes all‑pairs shortest paths using dynamic programming.
/// Returns `Some(map)` where `map[u][v]` is:
///     - `Some(cost)` if a path from node `u` to `v` exists, or
///     - `None` if `v` is unreachable from `u`.
/// Returns `None` if a negative cycle is detected.
///
/// # Complexity
///
/// - **Time:** O(V^3)
/// - **Space:** O(V^2)
pub fn floyd_warshall<A, W, Ty>(graph: &BaseGraph<A, W, Ty>) -> Option<NodeMap<NodeMap<Option<W>>>>
where
    W: Copy + PartialOrd + Add<Output = W> + From<u8>,
    Ty: GraphConstructor<A, W>,
{
    let n = graph.node_count();
    let nodes: Vec<NodeId> = graph.node_ids().collect();
    // Map each NodeId to its position in `nodes`. NodeId indices are not guaranteed
    // to be contiguous (a StableGraph never recycles indices), so positions, not
    // raw indices, address the matrix.
    let pos: NodeMap<usize> = nodes.iter().enumerate().map(|(i, u)| (*u, i)).collect();

    let mut dist = vec![vec![None; n]; n];
    for (i, row) in dist.iter_mut().enumerate().take(n) {
        row[i] = Some(W::from(0u8));
    }
    // Populate via outgoing_edges so undirected edges (stored once) are recorded in
    // both directions, matching dijkstra and bellman_ford.
    for (i, &u) in nodes.iter().enumerate() {
        for (v, w) in outgoing_edges(graph, u) {
            let Some(&j) = pos.get(&v) else { continue };
            match dist[i][j] {
                Some(current) if w < current => dist[i][j] = Some(w),
                None => dist[i][j] = Some(w),
                _ => {}
            }
        }
    }
    for k in 0..n {
        for i in 0..n {
            for j in 0..n {
                if let (Some(dik), Some(dkj)) = (dist[i][k], dist[k][j]) {
                    let candidate = dik + dkj;
                    match dist[i][j] {
                        Some(dij) if candidate < dij => dist[i][j] = Some(candidate),
                        None => dist[i][j] = Some(candidate),
                        _ => {}
                    }
                }
            }
        }
    }
    // A negative diagonal entry means some node can reach itself at negative
    // cost, which is exactly a negative cycle; this must be checked before the
    // diagonal is reset to zero below.
    for (i, row) in dist.iter().enumerate().take(n) {
        if let Some(dii) = row[i] {
            if dii < W::from(0u8) {
                return None;
            }
        }
    }
    for (i, row) in dist.iter_mut().enumerate().take(n) {
        row[i] = Some(W::from(0u8));
    }
    // Convert to NodeMap form
    let mut outer: NodeMap<NodeMap<Option<W>>> = NodeMap::default();
    for (i, u) in nodes.iter().enumerate() {
        let mut inner: NodeMap<Option<W>> = NodeMap::default();
        for (j, v) in nodes.iter().enumerate() {
            inner.insert(*v, dist[i][j]);
        }
        outer.insert(*u, inner);
    }
    Some(outer)
}

/// ============================
/// Johnson’s Algorithm
/// ============================
///
/// Computes all‑pairs shortest paths for sparse graphs (even with negative edge weights)
/// by reweighting the graph to eliminate negatives and then running Dijkstra’s algorithm from each node.
/// Returns `Some(map)` if no negative cycle is detected, or `None` otherwise.
///
/// # Complexity
///
/// - **Time:** O(VE \log V) (implementation uses a binary heap)
/// - **Space:** O(V^2)
pub fn johnson<A, W, Ty>(graph: &BaseGraph<A, W, Ty>) -> Option<NodeMap<NodeMap<Option<W>>>>
where
    W: Copy + PartialOrd + Add<Output = W> + Sub<Output = W> + From<u8> + Ord,
    Ty: GraphConstructor<A, W>,
{
    // Potentials `h` and per-source distances `d` are keyed by `NodeId::index()`,
    // which is stable across node removal, so they span the index bound rather
    // than the node count. The output matrix `dist[i][j]` stays contiguous, keyed
    // by position in `nodes`. Mixing the two index spaces (as the previous version
    // did) is wrong whenever a node has been removed.
    let bound = index_bound(graph);
    let node_count = graph.node_count();
    let mut h = vec![W::from(0u8); bound];

    // Relax edges for node_count - 1 iterations. Undirected edges are stored
    // once but traversable both ways, so they relax in both directions; a
    // single orientation misses the negative cycle any undirected negative
    // edge forms and feeds negative reweighted edges to the Dijkstra phase.
    let is_directed = graph.is_directed();
    for _ in 0..node_count.saturating_sub(1) {
        let mut updated = false;
        for (u, v, &w) in graph.edges() {
            let ui = u.index();
            let vi = v.index();
            if h[ui] + w < h[vi] {
                h[vi] = h[ui] + w;
                updated = true;
            }
            if !is_directed && h[vi] + w < h[ui] {
                h[ui] = h[vi] + w;
                updated = true;
            }
        }
        if !updated {
            break;
        }
    }
    // Check for negative cycles.
    for (u, v, &w) in graph.edges() {
        let ui = u.index();
        let vi = v.index();
        if h[ui] + w < h[vi] {
            return None;
        }
        if !is_directed && h[vi] + w < h[ui] {
            return None;
        }
    }

    // Contiguous list of nodes; `dist[i][j]` is keyed by position here.
    let nodes: Vec<NodeId> = graph.nodes().map(|(node, _)| node).collect();
    let n = nodes.len();

    let mut dist = vec![vec![None; n]; n];
    for (i, &start) in nodes.iter().enumerate() {
        let mut d = vec![None; bound];
        d[start.index()] = Some(W::from(0u8));
        let mut heap = BinaryHeap::new();
        heap.push(Reverse((W::from(0u8), start)));
        while let Some(Reverse((du, current))) = heap.pop() {
            let ci = current.index();
            if let Some(cur) = d[ci] {
                if du > cur {
                    continue;
                }
            }
            for (v, w) in outgoing_edges(graph, current) {
                let vi = v.index();
                let new_w = w + h[current.index()] - h[vi];
                let nd = du + new_w;
                if d[vi].is_none() || Some(nd) < d[vi] {
                    d[vi] = Some(nd);
                    heap.push(Reverse((nd, v)));
                }
            }
        }
        for (j, &v) in nodes.iter().enumerate() {
            if let Some(dprime) = d[v.index()] {
                dist[i][j] = Some(dprime - h[start.index()] + h[v.index()]);
            }
        }
    }
    // Convert to NodeMap form
    let mut outer: NodeMap<NodeMap<Option<W>>> = NodeMap::default();
    for (i, u) in nodes.iter().enumerate() {
        let mut inner: NodeMap<Option<W>> = NodeMap::default();
        for (j, v) in nodes.iter().enumerate() {
            inner.insert(*v, dist[i][j]);
        }
        outer.insert(*u, inner);
    }
    Some(outer)
}

/// ============================
/// Unweighted All-Pairs Shortest Paths (BFS)
/// ============================
///
/// Computes an unweighted all-pairs distance matrix by breadth-first search from
/// every node, treating each edge as a unit step and ignoring weights. This is the
/// hop-count analogue of [`johnson`] and [`floyd_warshall`]: when the weighted
/// algorithms would run Dijkstra or Bellman-Ford per source, this runs a plain
/// queue-based BFS, which is faster whenever hop counts (rather than weighted
/// distances) are wanted.
///
/// Returns the node ordering and a dense, row-major distance matrix. `nodes[i]` is
/// the source for row `i`, and `matrix[i][j]` is the hop distance from `nodes[i]`
/// to `nodes[j]`, with `None` for an unreachable target and `Some(0)` on the
/// diagonal. A dense matrix (rather than a nested `NodeMap`) avoids materializing
/// V^2 hash entries, which otherwise dominates the cost, and a `u32` cell halves
/// the write bandwidth over the O(V^2) output (hop counts never exceed the node
/// count). Reachability follows the same adjacency as the weighted path algorithms:
/// outgoing edges on a directed graph, both directions on an undirected graph.
///
/// # Complexity
///
/// - **Time:** O(V * (V + E))
/// - **Space:** O(V^2)
pub fn all_pairs_shortest_path_length<A, W, Ty>(
    graph: &BaseGraph<A, W, Ty>,
) -> (Vec<NodeId>, Vec<Vec<Option<u32>>>)
where
    W: Copy,
    Ty: GraphConstructor<A, W>,
{
    // Snapshot the adjacency into a compact CSR in position space [0, n) once. Every
    // per-source BFS then walks contiguous `u32` slices instead of the `StableGraph`
    // adjacency (a linked list kept for NodeId stability), so the O(V^2) hot loop is
    // cache-resident and hash-free. The one-time build is O(V + E).
    let (nodes, offsets, adj) = position_csr(graph);
    let n = nodes.len();

    let mut matrix: Vec<Vec<Option<u32>>> = Vec::with_capacity(n);
    let mut queue: VecDeque<u32> = VecDeque::new();
    for source in 0..n {
        // The freshly allocated row doubles as the distance buffer and the visited
        // set (`None` means undiscovered), so BFS fills the output directly in a
        // single O(V) pass, rather than clearing a separate buffer and projecting it.
        let mut row: Vec<Option<u32>> = vec![None; n];
        row[source] = Some(0);
        queue.clear();
        queue.push_back(source as u32);
        while let Some(current) = queue.pop_front() {
            let du = row[current as usize].unwrap_or(0);
            for &next in &adj[offsets[current as usize]..offsets[current as usize + 1]] {
                if row[next as usize].is_none() {
                    row[next as usize] = Some(du + 1);
                    queue.push_back(next);
                }
            }
        }
        matrix.push(row);
    }
    (nodes, matrix)
}

/// Builds a compact CSR adjacency in position space `[0, n)` from a graph whose
/// stable node indices may be sparse. Returns the node ordering (position `i` is
/// `nodes[i]`), row offsets of length `n + 1`, and the concatenated neighbor
/// positions. Iterating a node's neighbors is then a contiguous slice, which the
/// all-pairs searches rely on to stay hash-free and cache-resident.
fn position_csr<A, W, Ty>(graph: &BaseGraph<A, W, Ty>) -> (Vec<NodeId>, Vec<usize>, Vec<u32>)
where
    W: Copy,
    Ty: GraphConstructor<A, W>,
{
    let bound = index_bound(graph);
    let nodes: Vec<NodeId> = graph.node_ids().collect();
    let n = nodes.len();

    // Map a stable node index onto its compact position.
    let mut pos = vec![0u32; bound];
    for (i, node) in nodes.iter().enumerate() {
        pos[node.index()] = i as u32;
    }

    // Degrees, then a prefix sum, give the row offsets.
    let mut offsets = vec![0usize; n + 1];
    for (i, &node) in nodes.iter().enumerate() {
        offsets[i + 1] = graph.neighbors(node).count();
    }
    for i in 0..n {
        offsets[i + 1] += offsets[i];
    }

    // Scatter neighbor positions into the flat array using a moving write cursor.
    let mut adj = vec![0u32; offsets[n]];
    let mut cursor = offsets.clone();
    for (i, &node) in nodes.iter().enumerate() {
        for v in graph.neighbors(node) {
            adj[cursor[i]] = pos[v.index()];
            cursor[i] += 1;
        }
    }
    (nodes, offsets, adj)
}

#[cfg(test)]
mod tests {

    #[test]
    fn test_floyd_warshall_negative_cycle_returns_none() {
        use crate::core::paths::floyd_warshall;
        use crate::core::types::Digraph;

        let mut g: Digraph<i32, i32> = Digraph::new();
        let a = g.add_node(0);
        let b = g.add_node(1);
        g.add_edge(a, b, -1);
        g.add_edge(b, a, -1);

        assert!(
            floyd_warshall(&g).is_none(),
            "a negative cycle must yield None"
        );
    }

    #[test]
    fn test_johnson_undirected_negative_edge_returns_none() {
        use crate::core::paths::johnson;
        use crate::core::types::Graph;

        // Traversing an undirected negative edge back and forth is a negative
        // cycle, so Johnson must return None rather than distances.
        let mut g: Graph<i32, i32> = Graph::new();
        let a = g.add_node(0);
        let b = g.add_node(1);
        g.add_edge(a, b, -1);

        assert!(johnson(&g).is_none());
    }

    #[test]
    fn test_dijkstra_stale_source_errors() {
        use crate::core::paths::dijkstra;
        use crate::core::types::Graph;

        // The removed node held the highest index, so the dense distance buffer
        // is smaller than its index and an unchecked write panicked out of
        // bounds. A stale source must produce a node_not_found error instead.
        let mut g: Graph<i32, i32> = Graph::new();
        let a = g.add_node(0);
        let b = g.add_node(1);
        let c = g.add_node(2);
        g.add_edge(a, b, 1);
        g.remove_node(c);

        assert!(dijkstra(&g, c).is_err());
    }

    #[test]
    fn test_dijkstra_path_f64_stale_source_errors() {
        use crate::core::paths::dijkstra_path_f64;
        use crate::core::types::Graph;

        let mut g: Graph<i32, f64> = Graph::new();
        let a = g.add_node(0);
        let b = g.add_node(1);
        let c = g.add_node(2);
        g.add_edge(a, b, 1.0);
        g.remove_node(c);

        assert!(dijkstra_path_f64(&g, c, None).is_err());
    }

    #[test]
    fn test_dijkstra_removed_mid_index_source_errors() {
        use crate::core::paths::dijkstra;
        use crate::core::types::Graph;

        // A removed node whose index is still below the bound used to return Ok
        // with a meaningless all-None map; it must error like any missing node.
        let mut g: Graph<i32, i32> = Graph::new();
        let a = g.add_node(0);
        let b = g.add_node(1);
        let c = g.add_node(2);
        g.add_edge(a, c, 1);
        g.remove_node(b);

        assert!(dijkstra(&g, b).is_err());
    }

    #[test]
    fn test_a_star_stale_source_and_target_error() {
        use crate::core::paths::a_star;
        use crate::core::types::Graph;

        let mut g: Graph<i32, i32> = Graph::new();
        let a = g.add_node(0);
        let b = g.add_node(1);
        let c = g.add_node(2);
        g.add_edge(a, b, 1);
        g.remove_node(c);

        assert!(a_star(&g, c, a, |_| 0).is_err());
        assert!(a_star(&g, a, c, |_| 0).is_err());
    }

    #[test]
    fn test_bellman_ford_stale_source_no_panic() {
        use crate::core::paths::bellman_ford;
        use crate::core::types::Graph;

        // bellman_ford has no error channel; a stale source must not panic and
        // must report every live node as unreachable.
        let mut g: Graph<i32, i32> = Graph::new();
        let a = g.add_node(0);
        let b = g.add_node(1);
        let c = g.add_node(2);
        g.add_edge(a, b, 1);
        g.remove_node(c);

        let dist = bellman_ford(&g, c).expect("no negative cycle");
        assert_eq!(dist[&a], None);
        assert_eq!(dist[&b], None);
    }

    #[test]
    fn test_dijkstra_negative_weights() {
        use crate::core::paths::dijkstra_path_f64;
        use crate::core::types::Graph;

        let mut g = Graph::<i32, f64>::new();
        let n1 = g.add_node(1);
        let n2 = g.add_node(2);

        g.add_edge(n1, n2, -5.0);

        let result = dijkstra_path_f64(&g, n1, None);
        assert!(result.is_err());
    }

    // Regression: the generic Dijkstra followed only the stored edge orientation,
    // so on an undirected graph a node reached via an edge stored as (other, node)
    // could not reach back. Here the edges are stored as (0,1) and (1,2); from node
    // 2, Dijkstra must still reach node 0 at distance 2.
    #[test]
    fn test_dijkstra_undirected_follows_both_directions() {
        use crate::core::paths::dijkstra;
        use crate::core::types::Graph;

        let mut g = Graph::<i32, i32>::new();
        let n0 = g.add_node(0);
        let n1 = g.add_node(1);
        let n2 = g.add_node(2);
        g.add_edge(n0, n1, 1);
        g.add_edge(n1, n2, 1);

        let dist = dijkstra(&g, n2).expect("dijkstra should succeed");
        assert_eq!(dist[&n2], Some(0));
        assert_eq!(dist[&n1], Some(1));
        assert_eq!(dist[&n0], Some(2), "node 0 must be reachable from node 2");
    }

    // Regression: bellman_ford relaxed each stored edge in one direction only, so on
    // an undirected graph it left nodes reachable only against the stored edge
    // orientation unreachable, disagreeing with dijkstra. It must follow undirected
    // edges in both directions.
    #[test]
    fn test_bellman_ford_undirected_follows_both_directions() {
        use crate::core::paths::bellman_ford;
        use crate::core::types::Graph;

        let mut g = Graph::<i32, i32>::new();
        let n0 = g.add_node(0);
        let n1 = g.add_node(1);
        let n2 = g.add_node(2);
        g.add_edge(n0, n1, 1);
        g.add_edge(n1, n2, 1);

        let dist = bellman_ford(&g, n2).expect("bellman_ford should succeed");
        assert_eq!(dist[&n2], Some(0));
        assert_eq!(dist[&n1], Some(1));
        assert_eq!(dist[&n0], Some(2), "node 0 must be reachable from node 2");
    }

    // Regression: floyd_warshall initialized its distance matrix by iterating
    // graph.edges() and writing only dist[u][v], never dist[v][u]. On an undirected
    // graph (edges stored once) the all-pairs matrix came out asymmetric and most
    // pairs unreachable, inconsistent with dijkstra and bellman_ford.
    #[test]
    fn test_floyd_warshall_undirected_follows_both_directions() {
        use crate::core::paths::floyd_warshall;
        use crate::core::types::Graph;

        let mut g = Graph::<i32, i32>::new();
        let n0 = g.add_node(0);
        let n1 = g.add_node(1);
        let n2 = g.add_node(2);
        g.add_edge(n0, n1, 1);
        g.add_edge(n1, n2, 1);

        let matrix = floyd_warshall(&g).expect("floyd_warshall should succeed");
        assert_eq!(
            matrix[&n2][&n0],
            Some(2),
            "node 0 must be reachable from node 2"
        );
        assert_eq!(matrix[&n0][&n2], Some(2), "matrix must be symmetric");
        assert_eq!(matrix[&n1][&n0], Some(1));
        assert_eq!(matrix[&n0][&n1], Some(1));
    }

    #[test]
    fn test_a_star_after_node_removal_no_panic() {
        use crate::core::types::Digraph;
        // `a_star` sized its dense buffers by `node_count()` and indexed them by
        // `NodeId::index()`. Because `NodeId`s are stable across removal, removing a
        // node leaves a live index above `node_count()`, so the buffer access panicked
        // out of bounds. It must size by the index bound instead, like `dijkstra`.
        use crate::core::paths::a_star;

        let mut g: Digraph<i32, i32> = Digraph::new();
        let a = g.add_node(0);
        let b = g.add_node(1);
        let c = g.add_node(2);
        let d = g.add_node(3);
        g.remove_node(b); // index 3 (d) is now live while node_count() is 3
        g.add_edge(a, c, 1);
        g.add_edge(c, d, 1);

        let result = a_star(&g, a, d, |_| 0).unwrap();
        assert_eq!(result.map(|(cost, _)| cost), Some(2));
    }

    #[test]
    fn test_johnson_after_node_removal_no_panic() {
        use crate::core::types::Digraph;
        // Same stable-index defect as `a_star`, compounded by `johnson` mixing
        // contiguous `0..n` indices with `NodeId::index()`.
        use crate::core::paths::johnson;

        let mut g: Digraph<i32, i32> = Digraph::new();
        let a = g.add_node(0);
        let b = g.add_node(1);
        let c = g.add_node(2);
        let d = g.add_node(3);
        g.remove_node(b);
        g.add_edge(a, c, 1);
        g.add_edge(c, d, 1);

        let all_pairs = johnson(&g).unwrap();
        assert_eq!(all_pairs[&a][&d], Some(2));
        assert_eq!(all_pairs[&a][&c], Some(1));
        assert_eq!(all_pairs[&d][&a], None);
    }

    #[test]
    fn test_all_pairs_bfs_directed_hop_counts() {
        use crate::core::paths::all_pairs_shortest_path_length;
        use crate::core::types::Digraph;

        let mut g: Digraph<i32, f64> = Digraph::new();
        let a = g.add_node(0);
        let b = g.add_node(1);
        let c = g.add_node(2);
        g.add_edge(a, b, 5.0);
        g.add_edge(b, c, 5.0);

        // Hop counts ignore the edge weights entirely.
        let (nodes, matrix) = all_pairs_shortest_path_length(&g);
        let pos = |node| nodes.iter().position(|&x| x == node).unwrap();
        assert_eq!(matrix[pos(a)][pos(a)], Some(0));
        assert_eq!(matrix[pos(a)][pos(b)], Some(1));
        assert_eq!(matrix[pos(a)][pos(c)], Some(2));
        assert_eq!(
            matrix[pos(c)][pos(a)],
            None,
            "directed: no path back from c"
        );
    }

    #[test]
    fn test_all_pairs_bfs_undirected_and_disconnected() {
        use crate::core::paths::all_pairs_shortest_path_length;
        use crate::core::types::Graph;

        let mut g: Graph<i32, f64> = Graph::new();
        let a = g.add_node(0);
        let b = g.add_node(1);
        let c = g.add_node(2);
        let iso = g.add_node(3);
        g.add_edge(a, b, 1.0);
        g.add_edge(b, c, 1.0);

        let (nodes, matrix) = all_pairs_shortest_path_length(&g);
        let pos = |node| nodes.iter().position(|&x| x == node).unwrap();
        // Undirected: reachable in both directions, including against edge storage.
        assert_eq!(matrix[pos(c)][pos(a)], Some(2));
        assert_eq!(matrix[pos(a)][pos(c)], Some(2));
        // The isolated node is unreachable from everyone but itself.
        assert_eq!(matrix[pos(iso)][pos(iso)], Some(0));
        assert_eq!(matrix[pos(a)][pos(iso)], None);
        assert_eq!(matrix[pos(iso)][pos(a)], None);
    }

    use super::*;
    use crate::core::types::{Digraph, NodeId};
    use ordered_float::OrderedFloat;
    use std::collections::HashMap;
    fn build_test_graph_ordered() -> (Digraph<i32, OrderedFloat<f64>>, HashMap<i32, NodeId>) {
        let mut graph: Digraph<i32, OrderedFloat<f64>> = Digraph::default();
        let mut nodes = HashMap::new();
        nodes.insert(0, graph.add_node(0));
        nodes.insert(1, graph.add_node(1));
        nodes.insert(2, graph.add_node(2));
        nodes.insert(3, graph.add_node(3));
        graph.add_edge(nodes[&0], nodes[&1], OrderedFloat(1.0));
        graph.add_edge(nodes[&0], nodes[&2], OrderedFloat(4.0));
        graph.add_edge(nodes[&1], nodes[&2], OrderedFloat(2.0));
        graph.add_edge(nodes[&1], nodes[&3], OrderedFloat(6.0));
        graph.add_edge(nodes[&2], nodes[&3], OrderedFloat(3.0));
        (graph, nodes)
    }
    #[test]
    fn test_dijkstra_directed() {
        let (graph, nodes) = build_test_graph_ordered();
        let n0 = nodes[&0];
        let n3 = nodes[&3];
        let dist = dijkstra(&graph, n0).unwrap();
        assert_eq!(dist[&n3], Some(OrderedFloat(6.0)));
    }
    #[test]
    fn test_bellman_ford_directed() {
        let (graph, nodes) = build_test_graph_ordered();
        let n0 = nodes[&0];
        let n3 = nodes[&3];
        let dist = bellman_ford(&graph, n0).expect("No negative cycle");
        assert_eq!(dist[&n3], Some(OrderedFloat(6.0)));
    }
    #[test]
    fn test_bellman_ford_detects_negative_cycle() {
        // Directed cycle 0 -> 1 -> 2 -> 0 with total weight 1 - 2 - 2 = -3, a
        // negative cycle reachable from the source, so no shortest path exists.
        let mut graph: Digraph<i32, OrderedFloat<f64>> = Digraph::new();
        let n0 = graph.add_node(0);
        let n1 = graph.add_node(1);
        let n2 = graph.add_node(2);
        graph.add_edge(n0, n1, OrderedFloat(1.0));
        graph.add_edge(n1, n2, OrderedFloat(-2.0));
        graph.add_edge(n2, n0, OrderedFloat(-2.0));
        assert!(bellman_ford(&graph, n0).is_none());
    }
    #[test]
    fn test_bellman_ford_undirected() {
        // Path 0-1-2-3 on an undirected graph, with each edge stored in the
        // reverse orientation (higher index as source). Bellman-Ford must follow
        // undirected edges in both directions, matching dijkstra, so every node
        // is reachable from node 0.
        use crate::core::types::Graph;
        let mut graph: Graph<i32, OrderedFloat<f64>> = Graph::default();
        let n0 = graph.add_node(0);
        let n1 = graph.add_node(1);
        let n2 = graph.add_node(2);
        let n3 = graph.add_node(3);
        graph.add_edge(n1, n0, OrderedFloat(1.0));
        graph.add_edge(n2, n1, OrderedFloat(2.0));
        graph.add_edge(n3, n2, OrderedFloat(3.0));
        let dist = bellman_ford(&graph, n0).expect("No negative cycle");
        assert_eq!(dist[&n1], Some(OrderedFloat(1.0)));
        assert_eq!(dist[&n2], Some(OrderedFloat(3.0)));
        assert_eq!(dist[&n3], Some(OrderedFloat(6.0)));
    }
    #[test]
    fn test_a_star_directed() {
        let (graph, nodes) = build_test_graph_ordered();
        let n0 = nodes[&0];
        let n1 = nodes[&1];
        let n2 = nodes[&2];
        let n3 = nodes[&3];
        let result = a_star(&graph, n0, n3, |_| OrderedFloat(0.0));
        assert!(result.is_ok());
        let path_opt = result.unwrap();
        assert!(path_opt.is_some());
        let (cost, path) = path_opt.unwrap();
        assert_eq!(cost, OrderedFloat(6.0));
        assert_eq!(path, vec![n0, n1, n2, n3]);
    }
    #[test]
    fn test_floyd_warshall_directed() {
        let (graph, nodes) = build_test_graph_ordered();
        let n0 = nodes[&0];
        let n3 = nodes[&3];
        let matrix = floyd_warshall(&graph).expect("No negative cycle");
        assert_eq!(matrix[&n0][&n3], Some(OrderedFloat(6.0)));
    }
    #[test]
    fn test_floyd_warshall_undirected() {
        // Path 0-1-2-3 on an undirected graph, with each edge stored in the
        // reverse orientation. Floyd-Warshall must record undirected edges in both
        // directions, so the all-pairs matrix is symmetric and every pair reachable.
        use crate::core::types::Graph;
        let mut graph: Graph<i32, OrderedFloat<f64>> = Graph::default();
        let n0 = graph.add_node(0);
        let n1 = graph.add_node(1);
        let n2 = graph.add_node(2);
        let n3 = graph.add_node(3);
        graph.add_edge(n1, n0, OrderedFloat(1.0));
        graph.add_edge(n2, n1, OrderedFloat(2.0));
        graph.add_edge(n3, n2, OrderedFloat(3.0));
        let matrix = floyd_warshall(&graph).expect("No negative cycle");
        assert_eq!(matrix[&n0][&n3], Some(OrderedFloat(6.0)));
        assert_eq!(matrix[&n3][&n0], Some(OrderedFloat(6.0)));
        assert_eq!(matrix[&n0][&n2], Some(OrderedFloat(3.0)));
        assert_eq!(matrix[&n1][&n3], Some(OrderedFloat(5.0)));
    }
}