use std::{
collections::VecDeque,
ops::{Add, Rem},
};
use schemars::JsonSchema;
use serde::{Deserialize, Serialize};
pub type NodeID = u32;
pub trait SearchProblem {
type Step: Copy + Default;
type Cost: Copy
+ Ord
+ Add<Output = Self::Cost>
+ Rem<Output = Self::Cost>
+ From<u8>
+ TryInto<usize>;
fn initial(&mut self) -> NodeID;
fn max_edge_cost(&self) -> Self::Cost;
fn is_goal(&self, node: NodeID) -> bool;
fn expand<F: FnMut(NodeID, bool, Self::Cost, Self::Step)>(
&mut self,
node: NodeID,
via: Option<Self::Step>,
emit: F,
);
}
#[derive(Debug)]
struct Node<S, C> {
distance: C,
parent: NodeID,
step: S,
finished: bool,
}
#[derive(Debug, Clone)]
pub struct SearchResult<S, C = u32> {
pub path: Vec<S>,
pub cost: C,
pub states_visited: usize,
}
#[derive(Debug, Clone, PartialEq, Serialize, Deserialize, JsonSchema)]
pub enum SearchError {
LimitReached,
Unreachable,
MaxEdgeCostTooLarge,
}
#[derive(Debug)]
pub struct SearchState<S, C = u32> {
nodes: Vec<Node<S, C>>,
buckets: Vec<VecDeque<NodeID>>,
}
impl<S, C> Default for SearchState<S, C> {
fn default() -> Self {
Self {
nodes: Vec::new(),
buckets: Vec::new(),
}
}
}
#[inline]
pub fn search<P: SearchProblem>(
problem: &mut P,
state: &mut SearchState<P::Step, P::Cost>,
max_states: Option<usize>,
) -> Result<SearchResult<P::Step, P::Cost>, SearchError> {
let num_buckets_cost = problem.max_edge_cost() + P::Cost::from(1);
let num_buckets: usize = num_buckets_cost
.try_into()
.map_err(|_| SearchError::MaxEdgeCostTooLarge)?;
let start = problem.initial();
let nodes = &mut state.nodes;
let buckets = &mut state.buckets;
nodes.clear();
nodes.push(Node {
distance: P::Cost::from(0),
parent: start,
step: P::Step::default(),
finished: false,
});
buckets.resize_with(num_buckets, VecDeque::new);
buckets.iter_mut().for_each(VecDeque::clear);
buckets[0].push_back(start);
let limit = max_states.unwrap_or(usize::MAX);
let mut queued: usize = 1;
let mut bucket: usize = 0;
let mut states_visited: usize = 0;
while queued > 0 {
while buckets[bucket].is_empty() {
bucket = (bucket + 1) % num_buckets;
}
let node_id = buckets[bucket].pop_front().expect("Bucket not empty");
queued -= 1;
if nodes[node_id as usize].finished {
continue;
}
nodes[node_id as usize].finished = true;
states_visited += 1;
if states_visited > limit {
return Err(SearchError::LimitReached);
}
if problem.is_goal(node_id) {
return Ok(SearchResult {
path: reconstruct(nodes, node_id),
cost: nodes[node_id as usize].distance,
states_visited,
});
}
let distance = nodes[node_id as usize].distance;
let via = if nodes[node_id as usize].parent == node_id {
None
} else {
Some(nodes[node_id as usize].step)
};
problem.expand(node_id, via, |next_id, is_new, cost, step| {
let new_distance = distance + cost;
let idx = (new_distance % num_buckets_cost).try_into().unwrap_or(0);
if is_new {
debug_assert_eq!(next_id as usize, nodes.len(), "no NodeID should be skipped");
nodes.push(Node {
distance: new_distance,
parent: node_id,
step,
finished: false,
});
buckets[idx].push_back(next_id);
queued += 1;
} else {
let node = &mut nodes[next_id as usize];
if !node.finished && new_distance < node.distance {
node.distance = new_distance;
node.parent = node_id;
node.step = step;
buckets[idx].push_back(next_id);
queued += 1;
}
}
});
}
Err(SearchError::Unreachable)
}
fn reconstruct<S: Copy, C>(nodes: &[Node<S, C>], target: NodeID) -> Vec<S> {
let mut path = Vec::new();
let mut current = target;
while nodes[current as usize].parent != current {
let node = &nodes[current as usize];
path.push(node.step);
current = node.parent;
}
path.reverse();
path
}