use num_traits::Zero;
use std::hash::Hash;
pub fn idastar<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: Zero + Ord + Copy,
FN: FnMut(&N) -> IN,
IN: IntoIterator<Item = (N, C)>,
FH: FnMut(&N) -> C,
FS: FnMut(&N) -> bool,
{
let mut bound = heuristic(start);
let mut path = vec![start.clone()];
loop {
match search(
&mut path,
Zero::zero(),
bound,
&mut neighbours,
&mut heuristic,
&mut success,
) {
Path::Found(path, cost) => return Some((path, cost)),
Path::Minimum(min) => {
if bound == min {
return None;
}
bound = min;
}
Path::Impossible => return None,
}
}
}
enum Path<N, C> {
Found(Vec<N>, C),
Minimum(C),
Impossible,
}
fn search<N, C, FN, IN, FH, FS>(
path: &mut Vec<N>,
cost: C,
bound: C,
neighbours: &mut FN,
heuristic: &mut FH,
success: &mut FS,
) -> Path<N, C>
where
N: Eq + Hash + Clone,
C: Zero + Ord + Copy,
FN: FnMut(&N) -> IN,
IN: IntoIterator<Item = (N, C)>,
FH: FnMut(&N) -> C,
FS: FnMut(&N) -> bool,
{
let neighbs = {
let start = &path[path.len() - 1];
let f = cost + heuristic(start);
if f > bound {
return Path::Minimum(f);
}
if success(start) {
return Path::Found(path.clone(), f);
}
let mut neighbs = neighbours(start)
.into_iter()
.filter_map(|(n, c)| {
if path.contains(&n) {
None
} else {
let h = heuristic(&n);
Some((n, c, c + h))
}
})
.collect::<Vec<_>>();
neighbs.sort_by_key(|&(_, _, c)| c);
neighbs
};
let mut min = None;
for (node, extra, _) in neighbs {
path.push(node);
match search(path, cost + extra, bound, neighbours, heuristic, success) {
found @ Path::Found(_, _) => return found,
Path::Minimum(m) => match min {
None => min = Some(m),
Some(n) if m < n => min = Some(m),
Some(_) => (),
},
Path::Impossible => (),
}
path.pop();
}
match min {
Some(m) => Path::Minimum(m),
None => Path::Impossible,
}
}