use std::collections::hash_map::Entry::{Occupied, Vacant};
use std::collections::{BinaryHeap, HashMap};
use std::hash::Hash;
use super::visit::{EdgeRef, GraphBase, IntoEdges, VisitMap, Visitable};
use crate::scored::MinScored;
use crate::algo::Measure;
pub fn astar<G, F, H, K, IsGoal>(
graph: G,
start: G::NodeId,
mut is_goal: IsGoal,
mut edge_cost: F,
mut estimate_cost: H,
) -> Option<(K, Vec<G::NodeId>)>
where
G: IntoEdges + Visitable,
IsGoal: FnMut(G::NodeId) -> bool,
G::NodeId: Eq + Hash,
F: FnMut(G::EdgeRef) -> K,
H: FnMut(G::NodeId) -> K,
K: Measure + Copy,
{
let mut visited = graph.visit_map();
let mut visit_next = BinaryHeap::new();
let mut scores = HashMap::new();
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(_, node)) = visit_next.pop() {
if is_goal(node) {
let path = path_tracker.reconstruct_path_to(node);
let cost = scores[&node];
return Some((cost, path));
}
if !visited.visit(node) {
continue;
}
let node_score = scores[&node];
for edge in graph.edges(node) {
let next = edge.target();
if visited.is_visited(&next) {
continue;
}
let mut next_score = node_score + edge_cost(edge);
match scores.entry(next) {
Occupied(ent) => {
let old_score = *ent.get();
if next_score < old_score {
*ent.into_mut() = next_score;
path_tracker.set_predecessor(next, node);
} else {
next_score = old_score;
}
}
Vacant(ent) => {
ent.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<G>
where
G: GraphBase,
G::NodeId: Eq + Hash,
{
came_from: HashMap<G::NodeId, G::NodeId>,
}
impl<G> PathTracker<G>
where
G: GraphBase,
G::NodeId: Eq + Hash,
{
fn new() -> PathTracker<G> {
PathTracker {
came_from: HashMap::new(),
}
}
fn set_predecessor(&mut self, node: G::NodeId, previous: G::NodeId) {
self.came_from.insert(node, previous);
}
fn reconstruct_path_to(&self, last: G::NodeId) -> Vec<G::NodeId> {
let mut path = vec![last];
let mut current = last;
while let Some(&previous) = self.came_from.get(¤t) {
path.push(previous);
current = previous;
}
path.reverse();
path
}
}