use super::{bellman_ford::Paths, BoundedMeasure, NegativeCycle};
use crate::prelude::*;
use crate::visit::{IntoEdges, IntoNodeIdentifiers, NodeIndexable};
use alloc::{vec, vec::Vec};
pub fn spfa<G, F, K>(
graph: G,
source: G::NodeId,
mut edge_cost: F,
) -> Result<Paths<G::NodeId, K>, NegativeCycle>
where
G: IntoEdges + IntoNodeIdentifiers + NodeIndexable,
F: FnMut(G::EdgeRef) -> K,
K: BoundedMeasure + Copy,
{
let ix = |i| graph.to_index(i);
let mut predecessors = vec![None; graph.node_bound()];
let mut distances = vec![K::max(); graph.node_bound()];
distances[ix(source)] = K::default();
let mut queue: Vec<G::NodeId> = Vec::with_capacity(graph.node_bound());
let mut in_queue = vec![false; graph.node_bound()];
queue.push(source);
in_queue[ix(source)] = true;
let mut visits = vec![0; graph.node_bound()];
while let Some(i) = queue.pop() {
in_queue[ix(i)] = false;
if visits[ix(i)] >= graph.node_bound() {
return Err(NegativeCycle(()));
}
visits[ix(i)] += 1;
for edge in graph.edges(i) {
let j = edge.target();
let w = edge_cost(edge);
let (dist, overflow) = distances[ix(i)].overflowing_add(w);
if !overflow && dist < distances[ix(j)] {
distances[ix(j)] = dist;
predecessors[ix(j)] = Some(i);
if !in_queue[ix(j)] {
in_queue[ix(j)] = true;
queue.push(j);
}
}
}
}
Ok(Paths {
distances,
predecessors,
})
}