hypergraphx 0.0.5

use super::measures::Measure;
use crate::traits::*;
use hashbrown::{
    HashMap,
    hash_map::Entry::{Occupied, Vacant},
};
use std::{collections::BinaryHeap, marker::PhantomData};

use super::MinScored;

/// \[Generic\] A* shortest path algorithm.
///
/// Computes the shortest path from `start` to `finish`, including the total path cost.
///
/// `finish` is implicitly given via the `is_goal` callback, which should return `true` if the
/// given node is the finish node.
///
/// The function `edge_cost` should return the cost for a particular edge. Edge costs must be
/// non-negative.
///
/// The function `estimate_cost` should return the estimated cost to the finish for a particular
/// node. For the algorithm to find the actual shortest path, it should be admissible, meaning that
/// it should never overestimate the actual cost to get to the nearest goal node. Estimate costs
/// must also be non-negative.
///
/// Returns the total cost + the path of subsequent nodes from start to finish, if one was
/// found.
///
/// ```
/// use hypergraphx::{algo::astar::astar, prelude::*};
/// let mut g: UndirectedHypergraph<(i32, i32), i32> = UndirectedHypergraph::new();
/// let a = g.add_node((0, 0));
/// let b = g.add_node((2, 0));
/// let c = g.add_node((1, 1));
/// let d = g.add_node((0, 2));
/// let e = g.add_node((3, 3));
/// let f = g.add_node((4, 2));
/// g.add_edges(
///     [
///         (2, vec![a, b]),
///         (4, vec![a, d]),
///         (1, vec![b, c]),
///         (7, vec![b, f]),
///         (5, vec![c, e]),
///         (1, vec![e, f]),
///         (1, vec![d, e]),
///     ]
///     .into_iter(),
/// )
/// .unwrap();

/// // Graph represented with the weight of each edge
/// // Edges with '*' are part of the optimal path.
/// //     2       1
/// // a ----- b ----- c
/// // | 4*    | 7     |
/// // d       f       | 5
/// // | 1*    | 1*    |
/// // \------ e ------/

/// let path = astar(
///     &g,
///     a,
///     |finish| finish == f,
///     |e| g.edge_weight_copied_unchecked(e),
///     |_| 0,
/// );
/// assert_eq!(path, Some((6, vec![a, d, e, f])));
/// ```
pub fn astar<'a, G, F, H, IsGoal, K, T: GraphType>(
    graph: &'a G,
    start: usize,
    mut is_goal: IsGoal,
    edge_cost: F,
    mut estimate_cost: H,
) -> Option<(K, Vec<usize>)>
where
    G: CommonProperties<'a, T, NodeIndex = usize, EdgeIndex = usize> + ?Sized,
    IsGoal: FnMut(usize) -> bool,
    F: Fn(usize) -> K,
    H: FnMut(usize) -> K,
    K: Measure + Copy,
{
    let mut visit_next = BinaryHeap::new();
    let mut scores = HashMap::new(); // g-values, cost to reach the node
    let mut estimate_scores = HashMap::new(); // f-values, cost to reach + estimate cost to goal
    let mut path_tracker = PathTracker::<G>::new();

    let zero_score = K::default();
    scores.insert(start, zero_score);
    visit_next.push(MinScored(estimate_cost(start), start));

    while let Some(MinScored(estimate_score, node)) = visit_next.pop() {
        if is_goal(node) {
            let path = path_tracker.reconstruct_path_to(node);
            let cost = scores[&node];
            return Some((cost, path));
        }

        // This lookup can be unwrapped without fear of panic since the node was necessarily scored
        // before adding it to `visit_next`.
        let node_score = scores[&node];

        match estimate_scores.entry(node) {
            Occupied(mut entry) => {
                // If the node has already been visited with an equal or lower score than now, then
                // we do not need to re-visit it.
                if *entry.get() <= estimate_score {
                    continue;
                }
                entry.insert(estimate_score);
            }
            Vacant(entry) => {
                entry.insert(estimate_score);
            }
        }

        for (next_score, nb) in
            graph.costed_neighbours(node, |edge_index| node_score + edge_cost(edge_index))
        {
            for next in nb {
                match scores.entry(next) {
                    Occupied(mut entry) => {
                        // No need to add neighbors that we have already reached through a shorter path
                        // than now.
                        if *entry.get() <= next_score {
                            continue;
                        }
                        entry.insert(next_score);
                    }
                    Vacant(entry) => {
                        entry.insert(next_score);
                    }
                }

                path_tracker.set_predecessor(next, node);
                let next_estimate_score = next_score + estimate_cost(next);
                visit_next.push(MinScored(next_estimate_score, next));
            }
        }
    }

    None
}

struct PathTracker<'a, G>
where
    G: GraphBasics<'a, NodeIndex = usize, EdgeIndex = usize> + ?Sized,
{
    came_from: HashMap<usize, usize>,
    _pd: PhantomData<&'a G>,
}

impl<'a, G> PathTracker<'a, G>
where
    G: GraphBasics<'a, NodeIndex = usize, EdgeIndex = usize> + ?Sized,
{
    fn new() -> PathTracker<'a, G> {
        PathTracker {
            came_from: HashMap::new(),
            _pd: PhantomData,
        }
    }

    fn set_predecessor(&mut self, node: usize, previous: usize) {
        self.came_from.insert(node, previous);
    }

    fn reconstruct_path_to(&self, last: usize) -> Vec<usize> {
        let mut path = vec![last];

        let mut current = last;
        while let Some(&previous) = self.came_from.get(&current) {
            path.push(previous);
            current = previous;
        }

        path.reverse();

        path
    }
}