use num_traits::{Bounded, Zero};
use ordermap::OrderMap;
use ordermap::Entry::{Occupied, Vacant};
use std::collections::VecDeque;
use std::hash::Hash;
use std::mem;
use std::usize;
use super::reverse_path;
pub fn fringe<N, C, FN, IN, FH, FS>(
start: &N,
mut neighbours: FN,
mut heuristic: FH,
mut success: FS,
) -> Option<(Vec<N>, C)>
where
N: Eq + Hash + Clone,
C: Bounded + Zero + Ord + Copy,
FN: FnMut(&N) -> IN,
IN: IntoIterator<Item = (N, C)>,
FH: FnMut(&N) -> C,
FS: FnMut(&N) -> bool,
{
let mut now = VecDeque::new();
let mut later = VecDeque::new();
let mut parents: OrderMap<N, (usize, C)> = OrderMap::new();
let mut flimit = heuristic(start);
now.push_back(0);
parents.insert(start.clone(), (usize::MAX, Zero::zero()));
loop {
if now.is_empty() {
return None;
}
let mut fmin = C::max_value();
while let Some(i) = now.pop_front() {
let (g, neighbours) = {
let (node, &(_, g)) = parents.get_index(i).unwrap();
let f = g + heuristic(node);
if f > flimit {
if f < fmin {
fmin = f;
}
later.push_back(i);
continue;
}
if success(node) {
let path = reverse_path(&parents, |&(p, _)| p, i);
return Some((path, g));
}
(g, neighbours(node))
};
for (neighbour, cost) in neighbours {
let g_neighbour = g + cost;
let n; match parents.entry(neighbour) {
Vacant(e) => {
n = e.index();
e.insert((i, g_neighbour));
}
Occupied(e) => if e.get().1 > g_neighbour {
n = e.index();
e.insert((i, g_neighbour));
} else {
continue;
},
}
if !remove(&mut later, &n) {
remove(&mut now, &n);
}
now.push_front(n);
}
}
mem::swap(&mut now, &mut later);
flimit = fmin;
}
}
fn remove<T: Eq>(v: &mut VecDeque<T>, e: &T) -> bool {
if let Some(index) = v.iter().position(|x| x == e) {
v.remove(index);
true
} else {
false
}
}