spokes 0.3.0

//! Algorithms for use with networks.

#![allow(dead_code)]

use std::{
    cmp::{Ordering, Reverse},
    collections::{HashMap, HashSet, VecDeque},
    fmt::Debug,
    hash::Hash,
    mem,
    ops::Add,
};

use num::{Unsigned, Zero};
use priority_queue::PriorityQueue;

use crate::{
    ArcInfo, Network, arc_storage::ArcStorage, node_storage::NodeStorage, search::Direction,
};

mod preflowpush;
pub use self::preflowpush::preflow_push;

/// An error value when the underlying [`Network`] is cyclic.
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
pub struct Cyclic;

/// An error value when the underlying [`Network`] has a negative cycle.
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
pub struct NegativeCycle;

/// Determine a [Topological Sorting](https://en.wikipedia.org/wiki/Topological_sorting) of a
/// network.
///
/// # Errors
/// If the network is cyclic, then the is no valid Topological Sorting, so an `Err(Cyclic)` is
/// returned.
///
/// # Example
///
/// The following shows how to generate a topological sorting for a network where one is possible.
///
/// ```rust
/// use spokes::{Network, algorithms::{topological_sorting, valid_topological_sorting}, ArcStorage};
///
/// let mut network: Network<usize, (), ()> = Network::new();
/// network.add_nodes((0..6).map(|i| (i, ())));
/// network.add_arcs([
///     (5, 0),
///     (5, 2),
///     (4, 0),
///     (4, 1),
///     (2, 3),
///     (3, 1),
/// ]);
///
///
/// let sorting = topological_sorting(&network);
/// assert!(sorting.is_ok());
/// assert!(valid_topological_sorting(&network, &(sorting.unwrap())));
/// ```
/// Here is a case where the network is cyclic and, therefore, has no topological sorting.
///
/// ```rust
/// use spokes::{Network, algorithms::{topological_sorting, Cyclic}, ArcStorage};
///
/// let mut network: Network<usize, (), ()> = Network::new();
/// network.add_nodes((0..3).map(|i| (i, ())));
/// network.add_arcs([
///     (0, 1),
///     (1, 2),
///     (2, 1),
/// ]);
///
/// assert_eq!(topological_sorting(&network), Err(Cyclic));
/// ```
///
/// # Panics
/// This will panic if the `ArcStore` or `NodeStore` are broken.
pub fn topological_sorting<I, NA, AA, NodeStore, ArcStore>(
    network: &Network<I, NA, AA, NodeStore, ArcStore>,
) -> Result<Vec<I>, Cyclic>
where
    ArcStore: ArcStorage<I, AA>,
    NodeStore: NodeStorage<I, NA>,
    I: Eq + Hash + Clone,
{
    // determine indegree for all nodes
    let mut indegrees: HashMap<I, usize> = network
        .iter_nodes()
        .filter_map(|(node, _)| {
            let indegree = network.reverse_arcs(node).count();
            if indegree == 0 {
                None
            } else {
                Some((node.clone(), indegree))
            }
        })
        .collect();

    let mut indegree_zero: Vec<I> = network
        .iter_nodes()
        .filter_map(|(node, _)| {
            if indegrees.contains_key(node) {
                None
            } else {
                Some(node)
            }
        })
        .cloned()
        .collect();

    let mut sorting = Vec::with_capacity(network.n_nodes());

    while !indegree_zero.is_empty() {
        let this_degree = mem::take(&mut indegree_zero);

        for node in this_degree {
            for neigh in network.forward_arcs(&node) {
                *indegrees.get_mut(&neigh.head).expect("Should be present") -= 1;
                if *indegrees.get(&neigh.head).expect("Should be present") == 0 {
                    indegree_zero.push(neigh.head.clone());
                    indegrees.remove(&neigh.head);
                }
            }
            sorting.push(node);
        }
    }

    if indegrees.is_empty() {
        Ok(sorting)
    } else {
        Err(Cyclic)
    }
}

/// Check if a given topological sorting is valid for a network.
///
/// # Example
/// ```rust
///
/// use spokes::{Network, algorithms::valid_topological_sorting, ArcStorage};
///
/// let mut network: Network<usize, (), ()> = Network::new();
/// network.add_nodes((0..6).map(|i| (i, ())));
/// network.add_arcs([
///     (5, 0),
///     (5, 2),
///     (4, 0),
///     (4, 1),
///     (2, 3),
///     (3, 1),
/// ]);
///
/// assert!(valid_topological_sorting(&network, &[4, 5, 0, 2, 3, 1]));
/// ```
pub fn valid_topological_sorting<I, NodeAttr, ArcAttr, NodeStore, ArcStore>(
    network: &Network<I, NodeAttr, ArcAttr, NodeStore, ArcStore>,
    ordering: &[I],
) -> bool
where
    ArcStore: ArcStorage<I, ArcAttr>,
    NodeStore: NodeStorage<I, NodeAttr>,
    I: Eq + Hash + PartialEq,
{
    let labels: HashMap<&I, usize> = ordering.iter().enumerate().map(|(i, n)| (n, i)).collect();

    network.arc_iter().all(|arc| {
        let a = labels.get(&arc.tail);
        let b = labels.get(&arc.head);

        a.and_then(|a| b.map(|b| a < b)).unwrap_or(false)
    })
}

/// Representation of Finite or Infinite distances in a network
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
pub enum Distance<T> {
    /// A finite distance to the root node.
    Finite(T),
    /// Value for a node who is not connected to the root node.
    Infinite,
}

impl<T> Distance<T> {
    /// Return the finite value if the value is finite
    pub fn finite_value(&self) -> Option<&T> {
        match self {
            Distance::Finite(x) => Some(x),
            Distance::Infinite => None,
        }
    }

    /// Map the interior value of the distance type
    pub fn map<U, F: Fn(T) -> U>(self, f: F) -> Distance<U> {
        match self {
            Distance::Finite(x) => Distance::Finite(f(x)),
            Distance::Infinite => Distance::Infinite,
        }
    }
}

impl<T: Add<Output = T>> Add for Distance<T> {
    type Output = Self;

    fn add(self, rhs: Self) -> Self::Output {
        match (self, rhs) {
            (Distance::Finite(a), Distance::Finite(b)) => Distance::Finite(a + b),
            _ => Distance::Infinite,
        }
    }
}

impl<T: Zero> Zero for Distance<T> {
    fn zero() -> Self {
        Distance::Finite(T::zero())
    }

    fn is_zero(&self) -> bool {
        if let Distance::Finite(x) = self {
            x.is_zero()
        } else {
            false
        }
    }
}

impl<T: Add<T, Output = T>> Add<T> for Distance<T> {
    type Output = Distance<T>;

    fn add(self, rhs: T) -> Self::Output {
        match self {
            Distance::Finite(d) => Distance::Finite(d + rhs),
            Distance::Infinite => Distance::Infinite,
        }
    }
}

impl<T: PartialEq> PartialEq<T> for Distance<T> {
    fn eq(&self, other: &T) -> bool {
        match self {
            Distance::Finite(x) => x == other,
            Distance::Infinite => false,
        }
    }
}

impl<T: PartialOrd> PartialOrd<T> for Distance<T> {
    fn partial_cmp(&self, other: &T) -> Option<std::cmp::Ordering> {
        match self {
            Distance::Finite(x) => x.partial_cmp(other),
            Distance::Infinite => Some(Ordering::Greater),
        }
    }
}

impl<T: PartialOrd> PartialOrd for Distance<T> {
    fn partial_cmp(&self, other: &Self) -> Option<std::cmp::Ordering> {
        match (self, other) {
            (Distance::Finite(a), Distance::Finite(b)) => a.partial_cmp(b),
            (Distance::Finite(_), Distance::Infinite) => Some(std::cmp::Ordering::Less),
            (Distance::Infinite, Distance::Finite(_)) => Some(std::cmp::Ordering::Greater),
            (Distance::Infinite, Distance::Infinite) => Some(std::cmp::Ordering::Equal),
        }
    }
}

impl<T: Ord + Eq> Ord for Distance<T> {
    fn cmp(&self, other: &Self) -> std::cmp::Ordering {
        match (self, other) {
            (Distance::Finite(a), Distance::Finite(b)) => a.cmp(b),
            (Distance::Finite(_), Distance::Infinite) => std::cmp::Ordering::Less,
            (Distance::Infinite, Distance::Finite(_)) => std::cmp::Ordering::Greater,
            (Distance::Infinite, Distance::Infinite) => std::cmp::Ordering::Equal,
        }
    }
}

impl<T: std::fmt::Display> std::fmt::Display for Distance<T> {
    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
        match self {
            Distance::Finite(x) => write!(f, "{x}"),
            Distance::Infinite => write!(f, "INFINITE"),
        }
    }
}

impl<T: std::ops::Add<Output = T> + Copy> std::ops::Add for &Distance<T> {
    type Output = Distance<T>;

    fn add(self, rhs: Self) -> Self::Output {
        if let (Distance::Finite(x), Distance::Finite(y)) = (self, rhs) {
            Distance::Finite(*x + *y)
        } else {
            Distance::Infinite
        }
    }
}

impl<T: std::ops::AddAssign + Copy> std::ops::AddAssign for Distance<T> {
    fn add_assign(&mut self, rhs: Self) {
        match (self, rhs) {
            (Distance::Finite(x), Distance::Finite(y)) => {
                *x += y;
            }
            (s, _) => {
                *s = Distance::Infinite;
            }
        }
    }
}

impl<T: std::ops::SubAssign + Copy> std::ops::SubAssign for Distance<T> {
    fn sub_assign(&mut self, rhs: Self) {
        match (self, rhs) {
            (Distance::Finite(x), Distance::Finite(y)) => {
                *x -= y;
            }
            (_, Distance::Infinite) => panic!("X - INFINITE is not well defined."),
            (s, _) => {
                *s = Distance::Infinite;
            }
        }
    }
}

/// Create a network with arcs pointing to the shortest path to `source` and distances stored in
/// the node's attributes.
///
/// # Example
///
/// Using the example from Wikipedia's [Dijkstra's Algorithm](https://en.wikipedia.org/wiki/Dijkstra%27s_algorithm)
/// the following network's shortest path tree is correctly generated.
///
/// ```raw
///       ┌───┐  9    ┌─────┐
///   ┌── │ 5 │ ───── │  0  │ ─┐
///   │   └───┘       └─────┘  │
///   │     │           │      │
///   │     │           │ 7    │
///   │     │           │      │
///   │     │         ┌─────┐  │
///   │ 2   │    ┌─── │  1  │  │ 14
///   │     │    │    └─────┘  │
///   │     │    │      │      │
///   │     │    │      │ 10   │
///   │     │    │      │      │
///   │     │    │    ┌─────┐  │
///   └─────┼────┼─── │  2  │ ─┘
///         │    │    └─────┘
///         │    │      │
///         │    │ 15   │ 11
///         │    │      │
///         │    │    ┌─────┐
///         │    └─── │  3  │
///         │         └─────┘
///         │           │
///         │           │ 6
///         │           │
///         │   9     ┌─────┐
///         └─────────│  4  │
///                   └─────┘
/// ```
///
/// ```rust
/// use spokes::{Network, algorithms::{dijkstra_shortest_path, Distance}, ArcStorage};
///
/// let mut network: Network<usize, (), u16> = Network::new();
/// network.add_nodes((0..6).map(|i| (i, ())));
/// network.add_arcs([
///     (0, 1, 7), (1, 0, 7),
///     (0, 5, 14), (5, 0, 14),
///     (0, 2, 9), (2, 0, 9),
///     (1, 2, 10), (2, 1, 10),
///     (1, 3, 15), (3, 1, 15),
///     (2, 5, 2), (5, 2, 2),
///     (2, 3, 11), (3, 2, 11),
///     (3, 4, 6), (4, 3, 6),
///     (4, 5, 9), (5, 4, 9),
/// ]);
///
/// let shortest_path_tree = dijkstra_shortest_path(&network, 0);
///
/// assert_eq!(shortest_path_tree.node(&4), Some(&Distance::Finite(20)));
///
/// let mut expected_network: Network<usize, Distance<u16>, ()> = Network::new();
///
/// expected_network.add_nodes([
///     (0, Distance::Finite(0)),
///     (1, Distance::Finite(7)),
///     (2, Distance::Finite(9)),
///     (3, Distance::Finite(20)),
///     (4, Distance::Finite(20)),
///     (5, Distance::Finite(11)),
/// ]);
///
/// expected_network.add_arcs([
///     (1, 0),
///     (2, 0),
///     (3, 2),
///     (5, 2),
///     (4, 5),
/// ]);
///
/// assert_eq!(shortest_path_tree, expected_network);
/// ```
///
/// # Panics
/// This will panic if the `ArcStore` or `NodeStore` are broken.
pub fn dijkstra_shortest_path<I, NodeAttr, T, NodeStore, ArcStore>(
    network: &Network<I, NodeAttr, T, NodeStore, ArcStore>,
    source: I,
) -> Network<I, Distance<T>, ()>
where
    NodeStore: NodeStorage<I, NodeAttr>,
    ArcStore: ArcStorage<I, T>,
    I: Hash + Eq + Copy,
    T: Ord + Zero + Copy + Unsigned,
{
    let mut pred = Network::new();
    pred.add_nodes(network.iter_nodes().map(|(n, _)| (*n, Distance::Infinite)));
    pred.add_node(source, Distance::Finite(T::zero()));

    let mut next_nodes: PriorityQueue<I, Reverse<T>> = PriorityQueue::new();
    next_nodes.push(source, Reverse(T::zero()));

    while let Some((position, Reverse(cost))) = next_nodes.pop() {
        for arc in network.forward_arcs(&position) {
            let alt = arc.attributes + cost;
            let altd = arc.attributes + cost;

            if !pred.contains_node(&arc.head) {
                pred.add_node(arc.head, Distance::Infinite);
            }

            let head_dist: &mut Distance<T> = pred
                .node_mut(&arc.head)
                .expect("Should be present, we just inserted it.");

            if *head_dist > altd {
                *head_dist = Distance::Finite(altd);

                // Replace predecessor arcs
                let arcs_to_remove: Vec<(I, I)> = pred
                    .forward_arcs(&arc.head)
                    .map(|a| (a.tail, a.head))
                    .collect();
                pred.remove_arcs(arcs_to_remove.iter().map(|(a, b)| (a, b)));

                let _ = pred.add_arc(ArcInfo::new(arc.head, arc.tail, ()));

                if next_nodes
                    .change_priority(&arc.head, Reverse(alt))
                    .is_none()
                {
                    next_nodes.push(arc.head, Reverse(alt));
                }
            }
        }
    }

    pred
}

/// Bellman Ford algorith for shortest paths.
///
/// See [Wikipedia](https://en.wikipedia.org/wiki/Bellman%E2%80%93Ford_algorithm) for details of the algorithm.
/// The algorithm actually being used is the [Shortest Path Faster Algorithm](https://en.wikipedia.org/wiki/Shortest_Path_Faster_Algorithm) as it works on the same networks as the Bellman-Ford algorithm.
///
/// # Errors
/// If the network contains a negative cycle, then a [`NegativeCycle`] error will be returned.
///
/// # Example
/// ```rust
/// use spokes::{Network, algorithms::{bellman_ford_shortest_path, Distance}, ArcStorage};
///
/// let mut network: Network<usize, (), u16> = Network::new();
/// network.add_nodes((0..6).map(|i| (i, ())));
/// network.add_arcs([
///     (0, 1, 7), (1, 0, 7),
///     (0, 5, 14), (5, 0, 14),
///     (0, 2, 9), (2, 0, 9),
///     (1, 2, 10), (2, 1, 10),
///     (1, 3, 15), (3, 1, 15),
///     (2, 5, 2), (5, 2, 2),
///     (2, 3, 11), (3, 2, 11),
///     (3, 4, 6), (4, 3, 6),
///     (4, 5, 9), (5, 4, 9),
/// ]);
///
/// let shortest_path_tree = bellman_ford_shortest_path(&network, 0).unwrap();
///
/// assert_eq!(shortest_path_tree.node(&4), Some(&Distance::Finite(20)));
///
/// let mut expected_network: Network<usize, Distance<u16>, ()> = Network::new();
///
/// expected_network.add_nodes([
///     (0, Distance::Finite(0)),
///     (1, Distance::Finite(7)),
///     (2, Distance::Finite(9)),
///     (3, Distance::Finite(20)),
///     (4, Distance::Finite(20)),
///     (5, Distance::Finite(11)),
/// ]);
///
/// expected_network.add_arcs([
///     (1, 0),
///     (2, 0),
///     (3, 2),
///     (5, 2),
///     (4, 5),
/// ]);
///
/// assert_eq!(shortest_path_tree, expected_network);
/// ```
///
/// # Panics
/// This will panic if the `ArcStore` or `NodeStore` are broken.
pub fn bellman_ford_shortest_path<I, NA, T, NodeStore, AS>(
    network: &Network<I, NA, T, NodeStore, AS>,
    source: I,
) -> Result<Network<I, Distance<T>, ()>, NegativeCycle>
where
    NodeStore: NodeStorage<I, NA>,
    AS: ArcStorage<I, T>,
    I: Hash + Eq + Copy + std::fmt::Debug,
    T: Ord + Zero + Copy + Debug,
{
    let n = network.n_nodes();
    let mut pred: Network<I, Distance<T>, ()> = Network::new();
    pred.add_nodes(network.iter_nodes().map(|(n, _)| (*n, Distance::Infinite)));

    pred.add_node(source, Distance::Finite(T::zero()));

    // Queue for next node to visit
    let mut queue = VecDeque::new();

    // Hashset to track if a node is in the queue
    let mut in_queue = HashSet::new();

    // Visit counts
    let mut visit_counts: HashMap<I, usize> = HashMap::new();

    // Add the source to the
    queue.push_back(source);
    in_queue.insert(source);

    while let Some(u) = queue.pop_front() {
        in_queue.remove(&u);

        if let Some(Distance::Finite(du)) = pred.node(&u).copied() {
            for arc in network.forward_arcs(&u) {
                let alt = Distance::Finite(du + arc.attributes);

                if !pred.contains_node(arc.head()) {
                    pred.add_node(*arc.head(), Distance::Infinite);
                }

                let head_dist = pred
                    .node_mut(&arc.head)
                    .expect("The arc's head should resolved to a node");

                if *head_dist > alt {
                    *head_dist = alt;

                    // Replace predecessor arcs
                    let arcs_to_remove: Vec<(I, I)> = pred
                        .forward_arcs(&arc.head)
                        .map(|a| (a.tail, a.head))
                        .collect();

                    pred.remove_arcs(arcs_to_remove.iter().map(|(a, b)| (a, b)));

                    let _ = pred.add_arc(ArcInfo::new(arc.head, arc.tail, ()));

                    if !in_queue.contains(&arc.head) {
                        queue.push_back(arc.head);
                        in_queue.insert(arc.head);

                        let mut visits = *visit_counts.entry(arc.head).or_default();
                        visits += 1;

                        if visits > n {
                            return Err(NegativeCycle);
                        }
                    }
                }
            }
        }
    }

    Ok(pred)
}

/// Compute the all-pairs shortest paths for a network.
///
/// # Example
/// ```rust
/// use spokes::{Network, algorithms::{floyd_worshall_all_pairs_minimum_path, Distance}, ArcStorage};
///
/// let mut network: Network<usize, (), u16> = Network::new();
/// network.add_nodes((0..6).map(|i| (i, ())));
/// network.add_arcs([
///     (0, 1, 7), (1, 0, 7),
///     (0, 5, 14), (5, 0, 14),
///     (0, 2, 9), (2, 0, 9),
///     (1, 2, 10), (2, 1, 10),
///     (1, 3, 15), (3, 1, 15),
///     (2, 5, 2), (5, 2, 2),
///     (2, 3, 11), (3, 2, 11),
///     (3, 4, 6), (4, 3, 6),
///     (4, 5, 9), (5, 4, 9),
/// ]);
///
/// let shortest_path_trees = floyd_worshall_all_pairs_minimum_path(&network);
///
/// assert_eq!(shortest_path_trees[&0].node(&4), Some(&Distance::Finite(20)));
///
/// let mut expected_network: Network<usize, Distance<u16>, ()> = Network::new();
///
/// expected_network.add_nodes([
///     (0, Distance::Finite(0)),
///     (1, Distance::Finite(7)),
///     (2, Distance::Finite(9)),
///     (3, Distance::Finite(20)),
///     (4, Distance::Finite(20)),
///     (5, Distance::Finite(11)),
/// ]);
///
/// expected_network.add_arcs([
///     (1, 0),
///     (2, 0),
///     (3, 2),
///     (5, 2),
///     (4, 5),
/// ]);
///
/// assert_eq!(shortest_path_trees[&0], expected_network);
/// ```
///
/// # Panics
/// If the `NodeStore` or `ArcStore` is broken, this may panic.
pub fn floyd_worshall_all_pairs_minimum_path<I, NodeAttr, T, NodeStore, ArcStore>(
    network: &Network<I, NodeAttr, T, NodeStore, ArcStore>,
) -> HashMap<I, Network<I, Distance<T>, ()>>
where
    NodeStore: NodeStorage<I, NodeAttr>,
    ArcStore: ArcStorage<I, T>,
    I: Hash + Eq + Copy + std::fmt::Debug,
    T: Ord + Zero + Copy + Debug,
{
    let mut pred = Network::new();
    pred.add_nodes(network.iter_nodes().map(|(n, _)| (*n, Distance::Infinite)));

    let mut preds: HashMap<I, Network<I, Distance<T>, ()>> = network
        .iter_nodes()
        .map(|(&id, _)| {
            let mut this_pred = pred.clone();
            this_pred.add_node(id, Distance::Finite(T::zero()));
            (id, this_pred)
        })
        .collect();

    for arc in network.arc_iter() {
        *preds
            .get_mut(&arc.tail)
            .expect("The key should map to a network")
            .node_mut(&arc.head)
            .expect("Node should exist") = Distance::Finite(arc.attributes);

        let _ = preds
            .get_mut(&arc.tail)
            .expect("The key should map to a network")
            .add_arc((arc.head, arc.tail));
    }

    for k in network.iter_nodes().map(|(i, _)| i) {
        for i in network.iter_nodes().map(|(i, _)| i) {
            for j in network.iter_nodes().map(|(i, _)| i) {
                let dij = preds[i].node(j).expect("Node should exist");
                let alt = preds[i].node(k).expect("Node should exist")
                    + preds[k].node(j).expect("Node should exist");

                if dij > &alt {
                    *preds
                        .get_mut(i)
                        .expect("Key should map to network")
                        .node_mut(j)
                        .expect("Node should exist") = alt;

                    // remove all existing arcs for predecessor graph for this node
                    let arcs_to_remove: Vec<(I, I)> =
                        preds[i].forward_arcs(j).map(|a| (a.tail, a.head)).collect();

                    preds
                        .get_mut(i)
                        .expect("Key should map to network")
                        .remove_arcs(arcs_to_remove.iter().map(|(a, b)| (a, b)));

                    let arc_to_add = preds[k]
                        .forward_arcs(j)
                        .next()
                        .expect("One arc from j should exist")
                        .clone();

                    let _ = preds
                        .get_mut(i)
                        .expect("Key should map to network")
                        .add_arc(arc_to_add);
                }
            }
        }
    }

    preds
}

/// Determine the distances to nodes for arcs of unit length and annotate a network with those
/// values.
///
/// # Example
/// ```rust
/// use spokes::{Network, algorithms::{distance_unit_arcs, Distance}, ArcStorage};
///
/// let mut network: Network<usize, (), usize> = Network::new();
///
/// network.add_nodes((0..=11).map(|i| (i, ())));
///
/// network.add_arcs([
///     (0, 1, 2),
///     (0, 2, 2),
///     (0, 3, 2),
///     (0, 4, 2),
///     (0, 5, 2),
///     (1, 6, 1),
///     (2, 7, 1),
///     (3, 8, 1),
///     (4, 9, 1),
///     (5, 10, 1),
///     (6, 11, 2),
///     (7, 11, 2),
///     (8, 11, 2),
///     (9, 11, 2),
///     (10, 11, 2),
/// ]);
///
/// let distances: Network<usize, Distance<usize>, usize> = distance_unit_arcs(&network, &11);
///
/// assert_eq!(distances.node(&11), Some(&Distance::Finite(0_usize)));
/// assert_eq!(distances.node(&6), Some(&Distance::Finite(1_usize)));
/// assert_eq!(distances.node(&7), Some(&Distance::Finite(1_usize)));
/// assert_eq!(distances.node(&8), Some(&Distance::Finite(1_usize)));
/// assert_eq!(distances.node(&9), Some(&Distance::Finite(1_usize)));
/// assert_eq!(distances.node(&10), Some(&Distance::Finite(1_usize)));
/// assert_eq!(distances.node(&1), Some(&Distance::Finite(2_usize)));
/// assert_eq!(distances.node(&2), Some(&Distance::Finite(2_usize)));
/// assert_eq!(distances.node(&3), Some(&Distance::Finite(2_usize)));
/// assert_eq!(distances.node(&4), Some(&Distance::Finite(2_usize)));
/// assert_eq!(distances.node(&5), Some(&Distance::Finite(2_usize)));
/// assert_eq!(distances.node(&0), Some(&Distance::Finite(3_usize)));
/// ```
pub fn distance_unit_arcs<I, NA, T, NodeStore, ArcStore, NewNodeStore>(
    network: &Network<I, NA, T, NodeStore, ArcStore>,
    root_id: &I,
) -> Network<I, Distance<usize>, T, NewNodeStore, ArcStore>
where
    NewNodeStore: NodeStorage<I, Distance<usize>> + FromIterator<(I, Distance<usize>)>,
    NodeStore: NodeStorage<I, NA>,
    ArcStore: ArcStorage<I, T> + Clone,
    I: Hash + Eq + Copy + Debug,
{
    let mut distances: HashMap<I, Distance<usize>> = HashMap::with_capacity(network.n_nodes());
    distances.insert(*root_id, Distance::Finite(0));

    // PERF: This is currently done in O(m+n) but could just be O(n) instead if we track the node
    // that precedes each node in the bfs

    for node in network.bfs(root_id, Direction::Reverse).skip(1) {
        let distance_to_node = network
            .forward_arcs(dbg!(node))
            .map(|arc| {
                dbg!(
                    distances
                        .get(dbg!(&arc.head))
                        .unwrap_or(&Distance::Infinite)
                )
            })
            .min()
            .copied()
            .unwrap_or(Distance::Infinite)
            + 1;
        distances.insert(*node, dbg!(distance_to_node));
    }

    dbg!(&distances);

    let nodes = network
        .iter_nodes()
        .map(|(id, _)| {
            (
                *id,
                distances.get(id).copied().unwrap_or(Distance::Infinite),
            )
        })
        .collect();

    Network::from_parts(nodes, network.arc_store().clone())
}

// Determine the maximal flow from the source to sink
//
// # Example
// ```rust, ignore
// use spokes::{Network, algorithms::{shortest_augmenting_path_flow, Distance}, ArcStorage};
//
// let mut network: Network<usize, (), usize> = Network::new();
//
// network.add_nodes((0..=11).map(|i| (i, ())));
//
// network.add_arcs([
//     (0, 1, 2),
//     (0, 2, 2),
//     (0, 3, 2),
//     (0, 4, 2),
//     (0, 5, 2),
//     (1, 6, 1),
//     (2, 7, 1),
//     (3, 8, 1),
//     (4, 9, 1),
//     (5, 10, 1),
//     (6, 11, 2),
//     (7, 11, 2),
//     (8, 11, 2),
//     (9, 11, 2),
//     (10, 11, 2),
// ]);
//
// let max_flow_residual = shortest_augmenting_path_flow(&network, &0, &11);
//
// let max_flow: usize = max_flow_residual.reverse_arcs(&0).map(|a| a.attributes).sum();
// dbg!(max_flow);
// dbg!(max_flow_residual);
//
// panic!();
// ```
// # Panics
// TODO: Shouldn't happen
//
//
// XXX: Currently broken! Don't use.
// fn shortest_augmenting_path_flow<I, NA, T, NodeStore, ArcStore>(
//     network: &Network<I, NA, T, NodeStore, ArcStore>,
//     source_id: &I,
//     sink_id: &I,
// ) -> Network<I, Distance<usize>, T>
// where
//     NodeStore: NodeStorage<I, NA>,
//     ArcStore: ArcStorage<I, T> + Clone + std::fmt::Debug,
//     I: Hash + Eq + Copy + std::fmt::Debug + std::fmt::Display,
//     T: Ord + Unsigned + Zero + Copy + Debug + std::ops::SubAssign + std::ops::AddAssign,
//     NA: Clone,
// {
//     let mut flow: Network<I, Distance<usize>, T, ArcStore> = distance_unit_arcs(network, sink_id);
//     let n = Distance::Finite(flow.n_nodes());
//     let mut cur: I = *source_id;
//     let mut pred: HashMap<I, I> = HashMap::new();
//
//     while flow
//         .node(source_id)
//         .expect("source should be in the network")
//         < &n
//     {
//         if let Some(arc) = admissible_arc(&cur, &flow).cloned() {
//             // Search for an admissible arc from the current position and advance
//             println!("Advancing from {} to {}", cur, arc.head);
//             pred.insert(arc.head, cur);
//             cur = arc.head;
//
//             if &cur == sink_id {
//                 let path = pred_to_path(&pred, sink_id);
//                 eprintln!("Augmenting along {:?}", path);
//                 augment(&path, network, &mut flow);
//             }
//         } else {
//             // Retreat
//             eprintln!("Retreating from {}", cur);
//             let new_distance = flow
//                 .forward_arcs(&cur)
//                 .filter_map(|arc| {
//                     if arc.attributes > T::zero() {
//                         Some(*flow.node(&arc.head).expect("Should be present") + 1)
//                     } else {
//                         None
//                     }
//                 })
//                 .min()
//                 .expect("There should be a minimum arc");
//             *flow.node_entry(cur).or_insert(Distance::Infinite) = new_distance;
//
//             if &cur != source_id {
//                 cur = *pred.get(&cur).expect("previous map should exist");
//             }
//         }
//     }
//
//     flow
// }

#[inline]
fn pred_to_path<'a, I: Hash + Eq>(pred: &'a HashMap<I, I>, start: &'a I) -> Vec<(&'a I, &'a I)> {
    let mut cur = start;
    let mut path = Vec::new();

    while let Some(next) = pred.get(cur) {
        path.push((cur, next));
        cur = next;
    }

    path.reverse();
    path
}

#[inline]
fn augment<I, T, NA, NodeStore, AS, FlowNodeStore>(
    path: &[(&I, &I)],
    network: &Network<I, NA, T, NodeStore, AS>,
    flow: &mut Network<I, Distance<usize>, T, FlowNodeStore, AS>,
) where
    NodeStore: NodeStorage<I, NA>,
    FlowNodeStore: NodeStorage<I, Distance<usize>>,
    AS: ArcStorage<I, T>,
    I: Hash + Eq + Copy,
    T: Ord + Zero + Copy + std::ops::SubAssign + std::ops::AddAssign,
{
    let delta = path
        .iter()
        .map(|(tail, head)| network.arc(**tail, **head).expect("Arc should exist"))
        .min()
        .expect("Minimum should exist");

    for (tail, head) in path {
        let forward_arc = if let Some(arc) = flow.arc_mut(**tail, **head) {
            arc
        } else {
            let _ = flow.add_arc(ArcInfo::new(**tail, **head, T::zero()));
            flow.arc_mut(**tail, **head)
                .expect("Should exist, as it was just inserted")
        };
        *forward_arc -= *delta;

        let reverse_arc = if let Some(arc) = flow.arc_mut(**head, **tail) {
            arc
        } else {
            let _ = flow.add_arc(ArcInfo::new(**head, **tail, T::zero()));
            flow.arc_mut(**head, **tail)
                .expect("Should exist, as it was just inserted")
        };
        *reverse_arc += *delta;
    }
}

#[inline]
fn next_admissible_arc<'a, I, NA, AA, NodeStore, ArcStore, F, T>(
    network: &'a Network<I, NA, AA, NodeStore, ArcStore>,
    f: F,
    node: &'a I,
) -> Option<&'a ArcInfo<I, AA>>
where
    NodeStore: NodeStorage<I, NA>,
    ArcStore: ArcStorage<I, AA>,
    I: Hash + Eq + Copy,
    F: Fn(&NA) -> Distance<T>,
    T: std::ops::Add<Output = T> + num::One + PartialEq + Copy,
{
    let this_distance = f(network.node(node).expect("Should exist"));
    for arc in network.forward_arcs(node) {
        let head_dist = f(network.node(&arc.head).expect("Should exist"));

        if this_distance == head_dist + T::one() {
            return Some(arc);
        }
    }
    None
}

#[inline]
fn admissible_arc<'a, I, T, NodeStore, ArcStore>(
    node: &'a I,
    network: &'a Network<I, Distance<usize>, T, NodeStore, ArcStore>,
) -> Option<&'a ArcInfo<I, T>>
where
    NodeStore: NodeStorage<I, Distance<usize>>,
    ArcStore: ArcStorage<I, T>,
    I: Hash + Eq + Copy,
{
    let this_distance = network.node(node).unwrap_or(&Distance::Infinite);
    for arc in network.forward_arcs(node) {
        let head_dist = network.node(node).unwrap_or(&Distance::Infinite);

        if *this_distance == *head_dist + 1 {
            return Some(arc);
        }
    }
    None
}

#[cfg(test)]
mod tests {
    mod distance {
        use super::super::Distance;

        #[test]
        fn partial_ord() {
            let inf: Distance<usize> = Distance::Infinite;
            assert!(inf > Distance::Finite(1_usize));
        }

        #[test]
        fn add() {
            assert_eq!(Distance::Finite(10) + 1, Distance::Finite(11));
            assert_eq!(
                Distance::Finite(10) + Distance::Finite(1),
                Distance::Finite(11)
            );
            assert_eq!(
                Distance::<usize>::Infinite + Distance::Finite(1),
                Distance::Infinite
            );
        }
    }

    mod distance_unit_arcs {
        use crate::{
            Network,
            algorithms::{Distance, distance_unit_arcs},
        };

        #[test]
        fn cycle() {
            let mut network: Network<usize, (), ()> = Network::new();

            network.add_nodes((0..=3).map(|i| (i, ())));
            network.add_arcs([(0, 1), (1, 2), (2, 3), (3, 0)]).unwrap();
            dbg!(&network);

            let distances: Network<usize, Distance<usize>, ()> = distance_unit_arcs(&network, &0);

            assert_eq!(distances.node(&0), Some(&Distance::Finite(0)));
            assert_eq!(distances.node(&1), Some(&Distance::Finite(3)));
            assert_eq!(distances.node(&2), Some(&Distance::Finite(2)));
            assert_eq!(distances.node(&3), Some(&Distance::Finite(1)));
        }
    }

    //mod shortest_augmenting_path_flow {
    //    use crate::{ArcStorage, Network, algorithms::shortest_augmenting_path_flow};

    //    #[test]
    //    #[ignore]
    //    fn simple() {
    //        let mut network: Network<usize, (), usize> = Network::new();

    //        network.add_nodes((0..=11).map(|i| (i, ())));

    //        network.add_arcs([
    //            (0, 1, 2),
    //            (0, 2, 2),
    //            (0, 3, 2),
    //            (0, 4, 2),
    //            (0, 5, 2),
    //            (1, 6, 1),
    //            (2, 7, 1),
    //            (3, 8, 1),
    //            (4, 9, 1),
    //            (5, 10, 1),
    //            (6, 11, 2),
    //            (7, 11, 2),
    //            (8, 11, 2),
    //            (9, 11, 2),
    //            (10, 11, 2),
    //        ]);

    //        let max_flow_residual = shortest_augmenting_path_flow(&network, &0, &11);

    //        let max_flow: usize = max_flow_residual
    //            .reverse_arcs(&0)
    //            .map(|a| a.attributes)
    //            .sum();
    //        dbg!(max_flow);
    //        let _ = dbg!(max_flow_residual);

    //        panic!();
    //    }
    //}
}