condor_grid/algorithms/

hpa_star.rs

//! Prepared hierarchical grid: HPA* cluster abstraction with local BFS refinement.
//!
//! [`HPAStarBuilder`] builds a durable abstract graph; each query adds temporary
//! endpoints, refines abstract hops, and returns the standard invalid/found/no-path
//! outcome. It implements both [`HierarchicalGridBuilder`] and
//! [`PreprocessedGridBuilder`]. Costs are unit hop
//! counts and preprocessing rejects non-uniform grids. Prefer [`super::jps_plus::JpsPlusBuilder`]
//! for prepared cardinal search without hierarchical abstraction.

use std::cmp::Ordering;
use std::collections::{BTreeMap, BinaryHeap, VecDeque};

use crate::{
    Grid, Path, Point,
    hierarchical::{HierarchicalGridBuildError, HierarchicalGridBuilder, PreparedHierarchicalGrid},
    preprocessed_grid::{
        PreparedGridSearch, PreprocessedGridBuildError, PreprocessedGridBuilder,
        PreprocessedGridMetadata, metadata_for_grid,
    },
    search::{SearchRequest, SearchResult},
};

/// Hierarchical / preprocessed builder for HPA* on uniform-cost grids.
///
/// Implements [`HierarchicalGridBuilder`] and [`PreprocessedGridBuilder`].
/// Preprocess rejects
/// non-unit `traversal_cost`. Prefer for multi-query uniform maps with natural
/// cluster structure; not a weighted-grid solver.
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub struct HPAStarBuilder {
    cluster_size: usize,
}

impl HPAStarBuilder {
    /// Builds with the given cluster side length in cells (`cluster_size >= 1`).
    ///
    /// Returns [`HierarchicalGridBuildError::InvalidClusterSize`] when `cluster_size == 0`.
    pub fn new(cluster_size: usize) -> Result<Self, HierarchicalGridBuildError> {
        if cluster_size == 0 {
            return Err(HierarchicalGridBuildError::InvalidClusterSize);
        }
        Ok(Self { cluster_size })
    }
}

impl Default for HPAStarBuilder {
    fn default() -> Self {
        Self { cluster_size: 10 }
    }
}

impl HierarchicalGridBuilder for HPAStarBuilder {
    type Map = PreparedHPAStar;

    fn name(&self) -> &'static str {
        "hpa-star"
    }

    fn preprocess(&self, grid: &Grid) -> Result<Self::Map, HierarchicalGridBuildError> {
        ensure_uniform_traversal_costs(grid)?;

        let mut abstract_graph = AbstractGraph::new();
        let width = grid.width();
        let height = grid.height();

        for y in (0..height).step_by(self.cluster_size) {
            for x in (0..width).step_by(self.cluster_size) {
                if x + self.cluster_size < width {
                    let x_left = x + self.cluster_size - 1;
                    let x_right = x + self.cluster_size;
                    let y_max = (y + self.cluster_size).min(height);

                    let mut current_run = Vec::new();
                    for yi in y..y_max {
                        let p_left = Point::new(x_left, yi);
                        let p_right = Point::new(x_right, yi);
                        if grid.is_walkable(p_left) && grid.is_walkable(p_right) {
                            current_run.push(yi);
                        } else if !current_run.is_empty() {
                            add_entrances(&mut abstract_graph, x_left, x_right, &current_run, true);
                            current_run.clear();
                        }
                    }
                    if !current_run.is_empty() {
                        add_entrances(&mut abstract_graph, x_left, x_right, &current_run, true);
                    }
                }

                if y + self.cluster_size < height {
                    let y_top = y + self.cluster_size - 1;
                    let y_bottom = y + self.cluster_size;
                    let x_max = (x + self.cluster_size).min(width);

                    let mut current_run = Vec::new();
                    for xi in x..x_max {
                        let p_top = Point::new(xi, y_top);
                        let p_bottom = Point::new(xi, y_bottom);
                        if grid.is_walkable(p_top) && grid.is_walkable(p_bottom) {
                            current_run.push(xi);
                        } else if !current_run.is_empty() {
                            add_entrances(
                                &mut abstract_graph,
                                y_top,
                                y_bottom,
                                &current_run,
                                false,
                            );
                            current_run.clear();
                        }
                    }
                    if !current_run.is_empty() {
                        add_entrances(&mut abstract_graph, y_top, y_bottom, &current_run, false);
                    }
                }
            }
        }

        for cy in 0..=((height - 1) / self.cluster_size) {
            for cx in 0..=((width - 1) / self.cluster_size) {
                let cluster_entrances: Vec<Point> = abstract_graph
                    .nodes
                    .keys()
                    .filter(|p| p.x / self.cluster_size == cx && p.y / self.cluster_size == cy)
                    .copied()
                    .collect();

                for i in 0..cluster_entrances.len() {
                    for j in (i + 1)..cluster_entrances.len() {
                        let start = cluster_entrances[i];
                        let end = cluster_entrances[j];
                        if let Some(path) =
                            intra_cluster_search(grid, start, end, self.cluster_size)
                        {
                            abstract_graph.add_edge(start, end, path.cost(), path.steps().to_vec());
                        }
                    }
                }
            }
        }

        let entrances_by_cluster = index_entrances_by_cluster(&abstract_graph, self.cluster_size);

        Ok(PreparedHPAStar {
            grid: grid.clone(),
            cluster_size: self.cluster_size,
            abstract_graph,
            entrances_by_cluster,
            metadata: metadata_for_grid(
                grid,
                <Self as HierarchicalGridBuilder>::name(self),
                <Self as HierarchicalGridBuilder>::name(self),
            ),
        })
    }
}

impl PreprocessedGridBuilder for HPAStarBuilder {
    type Map = PreparedHPAStar;

    fn name(&self) -> &'static str {
        <Self as HierarchicalGridBuilder>::name(self)
    }

    fn preprocess(&self, grid: &Grid) -> Result<Self::Map, PreprocessedGridBuildError> {
        <Self as HierarchicalGridBuilder>::preprocess(self, grid)
            .map_err(PreprocessedGridBuildError::from)
    }
}

fn ensure_uniform_traversal_costs(grid: &Grid) -> Result<(), HierarchicalGridBuildError> {
    for y in 0..grid.height() {
        for x in 0..grid.width() {
            let point = Point::new(x, y);
            if let Some(cost) = grid.traversal_cost(point)
                && cost != 1
            {
                return Err(HierarchicalGridBuildError::NonUniformCost { point, cost });
            }
        }
    }

    Ok(())
}

fn add_entrances(
    graph: &mut AbstractGraph,
    coord1: usize,
    coord2: usize,
    run: &[usize],
    vertical: bool,
) {
    if run.is_empty() {
        return;
    }

    let points = entrance_sample_indices(run);

    for r in points {
        let (p1, p2) = if vertical {
            (Point::new(coord1, r), Point::new(coord2, r))
        } else {
            (Point::new(r, coord1), Point::new(r, coord2))
        };
        graph.add_edge(p1, p2, 1, vec![p1, p2]);
    }
}

/// Sample entrance positions along an opening run (values already sorted).
///
/// Condor's unit-cost 4-way HPA\* v0 materializes **all** border cells of each
/// contiguous opening. Sparse sampling (ends-only / every-k) caused multi-hop
/// suboptimality on warehouse aisles and open-field-128 story maps.
fn entrance_sample_indices(run: &[usize]) -> Vec<usize> {
    run.to_vec()
}

/// Prepared HPA* map implementing hierarchical and preprocessed search contracts.
///
/// Abstract entrance graph plus owned uniform-cost grid. Queries attach start/goal via
/// temporary intra-cluster edges without mutating durable preprocess state. Path cost is
/// hop count under unit `traversal_cost` only.
pub struct PreparedHPAStar {
    grid: Grid,
    cluster_size: usize,
    abstract_graph: AbstractGraph,
    /// Cluster key `(cx, cy)` → entrance nodes in that cluster (query attach points).
    entrances_by_cluster: BTreeMap<(usize, usize), Vec<Point>>,
    metadata: PreprocessedGridMetadata,
}

impl PreparedHierarchicalGrid for PreparedHPAStar {
    fn name(&self) -> &'static str {
        "hpa-star"
    }

    fn search(&self, request: SearchRequest) -> SearchResult {
        crate::search::validate_request(&self.grid, request)?;
        if !self.grid.is_walkable(request.start) || !self.grid.is_walkable(request.goal) {
            return crate::search::not_found(0);
        }

        if request.start == request.goal {
            return crate::search::found(
                Path::from_steps(vec![request.start]).expect("path contains at least one point"),
                1,
            );
        }

        // Query attaches start/goal with per-request edges only (no abstract-graph clone).
        let mut temp_edges: BTreeMap<Point, Vec<AbstractEdge>> = BTreeMap::new();

        for &p in &[request.start, request.goal] {
            let cx = p.x / self.cluster_size;
            let cy = p.y / self.cluster_size;
            let cluster_entrances = self
                .entrances_by_cluster
                .get(&(cx, cy))
                .map(Vec::as_slice)
                .unwrap_or(&[]);

            for &entrance in cluster_entrances {
                if let Some(path) = intra_cluster_search(&self.grid, p, entrance, self.cluster_size)
                {
                    add_temp_edge(
                        &mut temp_edges,
                        p,
                        entrance,
                        path.cost(),
                        path.steps().to_vec(),
                    );
                }
            }
        }

        if request.start.x / self.cluster_size == request.goal.x / self.cluster_size
            && request.start.y / self.cluster_size == request.goal.y / self.cluster_size
            && let Some(path) =
                intra_cluster_search(&self.grid, request.start, request.goal, self.cluster_size)
        {
            add_temp_edge(
                &mut temp_edges,
                request.start,
                request.goal,
                path.cost(),
                path.steps().to_vec(),
            );
        }

        let watch = crate::search::BudgetWatch::start(request.budget);
        let (dist, parent_map) = match abstract_search(
            &self.abstract_graph,
            &temp_edges,
            request.start,
            request.goal,
            &watch,
        ) {
            Ok(outcome) => outcome,
            Err(reason) => return Err(crate::search::budget_error(reason)),
        };

        if dist == f64::INFINITY {
            return crate::search::not_found(parent_map.len());
        }

        let visited_nodes = parent_map.len();

        let mut abstract_path = Vec::new();
        let mut curr = request.goal;
        while curr != request.start {
            let prev = parent_map[&curr];
            abstract_path.push((prev, curr));
            curr = prev;
        }
        abstract_path.reverse();

        let mut concrete_steps = Vec::new();
        concrete_steps.push(request.start);
        for (u, v) in abstract_path {
            let edge_path = temp_edge_path(&temp_edges, u, v)
                .or_else(|| self.abstract_graph.get_edge_path(u, v))
                .expect("abstract parent edge must exist in base or temp edges");
            // Each stored edge path includes both endpoints; skip the join vertex.
            let skip = if concrete_steps.last() == Some(&edge_path[0]) {
                1
            } else {
                0
            };
            concrete_steps.extend(edge_path.iter().skip(skip));
        }

        crate::search::found(
            Path::from_steps(concrete_steps).expect("path contains at least one point"),
            visited_nodes,
        )
    }
}

impl PreparedGridSearch for PreparedHPAStar {
    fn name(&self) -> &'static str {
        <Self as PreparedHierarchicalGrid>::name(self)
    }

    fn grid(&self) -> &Grid {
        &self.grid
    }

    fn metadata(&self) -> &PreprocessedGridMetadata {
        &self.metadata
    }

    fn search(&self, request: SearchRequest) -> SearchResult {
        <Self as PreparedHierarchicalGrid>::search(self, request)
    }
}

#[derive(Clone)]
struct AbstractGraph {
    nodes: BTreeMap<Point, Vec<AbstractEdge>>,
}

#[derive(Clone)]
struct AbstractEdge {
    to: Point,
    cost: usize,
    path: Vec<Point>,
}

impl AbstractGraph {
    fn new() -> Self {
        Self {
            nodes: BTreeMap::new(),
        }
    }

    fn add_edge(&mut self, from: Point, to: Point, cost: usize, path: Vec<Point>) {
        self.nodes.entry(from).or_default().push(AbstractEdge {
            to,
            cost,
            path: path.clone(),
        });
        let mut rev_path = path;
        rev_path.reverse();
        self.nodes.entry(to).or_default().push(AbstractEdge {
            to: from,
            cost,
            path: rev_path,
        });
    }

    fn get_edge_path(&self, from: Point, to: Point) -> Option<&[Point]> {
        self.nodes
            .get(&from)?
            .iter()
            .find(|e| e.to == to)
            .map(|e| e.path.as_slice())
    }
}

fn intra_cluster_search(
    grid: &Grid,
    start: Point,
    goal: Point,
    cluster_size: usize,
) -> Option<Path> {
    let cx = start.x / cluster_size;
    let cy = start.y / cluster_size;

    let mut frontier = VecDeque::from([start]);
    let mut parents = BTreeMap::from([(start, None)]);

    while let Some(curr) = frontier.pop_front() {
        if curr == goal {
            let mut steps = Vec::new();
            let mut c = Some(curr);
            while let Some(p) = c {
                steps.push(p);
                c = parents[&p];
            }
            steps.reverse();
            return Some(Path::from_steps(steps).expect("path contains at least one point"));
        }

        for next in grid.neighbors4(curr) {
            if next.x / cluster_size == cx
                && next.y / cluster_size == cy
                && let std::collections::btree_map::Entry::Vacant(e) = parents.entry(next)
            {
                e.insert(Some(curr));
                frontier.push_back(next);
            }
        }
    }
    None
}

fn index_entrances_by_cluster(
    graph: &AbstractGraph,
    cluster_size: usize,
) -> BTreeMap<(usize, usize), Vec<Point>> {
    let mut by_cluster: BTreeMap<(usize, usize), Vec<Point>> = BTreeMap::new();
    for &p in graph.nodes.keys() {
        by_cluster
            .entry((p.x / cluster_size, p.y / cluster_size))
            .or_default()
            .push(p);
    }
    by_cluster
}

fn add_temp_edge(
    temp_edges: &mut BTreeMap<Point, Vec<AbstractEdge>>,
    from: Point,
    to: Point,
    cost: usize,
    path: Vec<Point>,
) {
    temp_edges.entry(from).or_default().push(AbstractEdge {
        to,
        cost,
        path: path.clone(),
    });
    let mut rev_path = path;
    rev_path.reverse();
    temp_edges.entry(to).or_default().push(AbstractEdge {
        to: from,
        cost,
        path: rev_path,
    });
}

fn temp_edge_path(
    temp_edges: &BTreeMap<Point, Vec<AbstractEdge>>,
    from: Point,
    to: Point,
) -> Option<&[Point]> {
    temp_edges
        .get(&from)?
        .iter()
        .find(|e| e.to == to)
        .map(|e| e.path.as_slice())
}

fn abstract_search(
    graph: &AbstractGraph,
    temp_edges: &BTreeMap<Point, Vec<AbstractEdge>>,
    start: Point,
    goal: Point,
    watch: &crate::search::BudgetWatch,
) -> Result<(f64, BTreeMap<Point, Point>), crate::search::BudgetExhausted> {
    let mut distances = BTreeMap::new();
    let mut parents = BTreeMap::new();
    let mut frontier = BinaryHeap::new();
    let mut expansions = 0usize;

    distances.insert(start, 0.0);
    frontier.push(AbstractHeapEntry {
        point: start,
        cost: 0.0,
    });

    while let Some(entry) = frontier.pop() {
        if entry.point == goal {
            return Ok((entry.cost, parents));
        }

        if entry.cost > *distances.get(&entry.point).unwrap_or(&f64::INFINITY) {
            continue;
        }

        expansions += 1;
        watch.check(expansions)?;

        let base = graph
            .nodes
            .get(&entry.point)
            .map(Vec::as_slice)
            .unwrap_or(&[]);
        let temp = temp_edges
            .get(&entry.point)
            .map(Vec::as_slice)
            .unwrap_or(&[]);
        for edge in base.iter().chain(temp.iter()) {
            let next_dist = entry.cost + edge.cost as f64;
            if next_dist < *distances.get(&edge.to).unwrap_or(&f64::INFINITY) {
                distances.insert(edge.to, next_dist);
                parents.insert(edge.to, entry.point);
                frontier.push(AbstractHeapEntry {
                    point: edge.to,
                    cost: next_dist,
                });
            }
        }
    }

    Ok((f64::INFINITY, parents))
}

#[derive(PartialEq)]
struct AbstractHeapEntry {
    point: Point,
    cost: f64,
}

impl Eq for AbstractHeapEntry {}

impl Ord for AbstractHeapEntry {
    fn cmp(&self, other: &Self) -> Ordering {
        other
            .cost
            .partial_cmp(&self.cost)
            .unwrap_or(Ordering::Equal)
    }
}

impl PartialOrd for AbstractHeapEntry {
    fn partial_cmp(&self, other: &Self) -> Option<Ordering> {
        Some(self.cmp(other))
    }
}

#[cfg(test)]
mod tests {
    use super::*;
    use crate::{AStar, Cell, Pathfinder, PreparedGridSearch, PreprocessedGridBuilder};

    #[test]
    fn neutral_adapter_preprocesses_through_the_preprocessed_contract() {
        let mut grid = Grid::new(6, 4).expect("grid dimensions are valid");
        grid.set_cell(Point::new(1, 1), Cell::Blocked)
            .expect("point is in bounds");
        grid.set_cell(Point::new(4, 2), Cell::Blocked)
            .expect("point is in bounds");

        let builder = HPAStarBuilder::new(3).expect("cluster size is positive");
        let prepared = <HPAStarBuilder as PreprocessedGridBuilder>::preprocess(&builder, &grid)
            .expect("HPA* should preprocess through neutral contract");

        assert_eq!(
            <HPAStarBuilder as PreprocessedGridBuilder>::name(&builder),
            "hpa-star"
        );
        assert_eq!(PreparedGridSearch::name(&prepared), "hpa-star");
        let metadata = *prepared.metadata();
        assert_eq!(metadata.builder_name, "hpa-star");
        assert_eq!(metadata.query_algorithm, "hpa-star");
        assert_eq!(metadata.width, 6);
        assert_eq!(metadata.height, 4);
        assert_eq!(metadata.cell_count, 24);
        assert_eq!(metadata.walkable_cell_count, 22);
        assert_eq!(metadata.blocked_cell_count, 2);
        assert_eq!(metadata.movement_model, "4-way");
        assert_eq!(metadata.cost_model, "uniform");

        let request = SearchRequest::new(Point::new(0, 0), Point::new(5, 3));
        let result = PreparedGridSearch::search(&prepared, request).expect("valid request");
        assert!(result.is_found());
        assert_eq!(*prepared.metadata(), metadata);
        assert!(result.stats().visited_nodes > 0);
        let path = result.path().expect("path should exist");
        assert_eq!(path.start(), request.start);
        assert_eq!(path.goal(), request.goal);
        assert!(prepared.grid().path_is_walkable(path.steps()));
    }

    #[test]
    fn neutral_adapter_matches_the_hierarchical_entrypoint() {
        let grid = Grid::new(7, 3).expect("grid dimensions are valid");
        let request = SearchRequest::new(Point::new(0, 1), Point::new(6, 1));
        let builder = HPAStarBuilder::new(2).expect("cluster size is positive");

        let neutral = <HPAStarBuilder as PreprocessedGridBuilder>::preprocess(&builder, &grid)
            .expect("HPA* should preprocess through neutral contract");
        let hierarchical = <HPAStarBuilder as HierarchicalGridBuilder>::preprocess(&builder, &grid)
            .expect("HPA* should preprocess through hierarchical contract");

        assert_eq!(
            PreparedGridSearch::search(&neutral, request),
            PreparedHierarchicalGrid::search(&hierarchical, request)
        );
    }

    #[test]
    fn neutral_adapter_rejects_weighted_grids_before_reporting_unit_cost_paths() {
        let mut grid = Grid::new(3, 1).expect("grid dimensions are valid");
        grid.set_traversal_cost(Point::new(1, 0), 5)
            .expect("walkable weighted cell should accept positive cost");
        let builder = HPAStarBuilder::new(1).expect("cluster size is positive");

        let neutral_error =
            match <HPAStarBuilder as PreprocessedGridBuilder>::preprocess(&builder, &grid) {
                Ok(_) => panic!("neutral HPA* adapter should reject weighted grids"),
                Err(error) => error,
            };
        assert_eq!(
            neutral_error,
            PreprocessedGridBuildError::Hierarchical(HierarchicalGridBuildError::NonUniformCost {
                point: Point::new(1, 0),
                cost: 5,
            })
        );

        let hierarchical_error =
            match <HPAStarBuilder as HierarchicalGridBuilder>::preprocess(&builder, &grid) {
                Ok(_) => panic!("hierarchical HPA* builder should reject weighted grids"),
                Err(error) => error,
            };
        assert_eq!(
            hierarchical_error,
            HierarchicalGridBuildError::NonUniformCost {
                point: Point::new(1, 0),
                cost: 5,
            }
        );
    }

    #[test]
    fn neutral_adapter_returns_normal_not_found_result() {
        let mut grid = Grid::new(5, 1).expect("grid dimensions are valid");
        grid.set_cell(Point::new(2, 0), Cell::Blocked)
            .expect("point is in bounds");

        let builder = HPAStarBuilder::new(2).expect("cluster size is positive");
        let prepared = <HPAStarBuilder as PreprocessedGridBuilder>::preprocess(&builder, &grid)
            .expect("HPA* should preprocess through neutral contract");
        let result = PreparedGridSearch::search(
            &prepared,
            SearchRequest::new(Point::new(0, 0), Point::new(4, 0)),
        )
        .expect("valid request");

        assert!(!result.is_found());
        assert!(result.path().is_none());
        assert_eq!(result.path().map(|path| path.cost()), None);
    }

    #[test]
    fn neutral_adapter_preserves_the_online_pathfinder_surface() {
        let grid = Grid::new(3, 1).expect("grid dimensions are valid");
        let request = SearchRequest::new(Point::new(0, 0), Point::new(2, 0));
        let online = AStar.search(&grid, request).expect("valid request");
        let prepared = <HPAStarBuilder as PreprocessedGridBuilder>::preprocess(
            &HPAStarBuilder::new(1).expect("cluster size is positive"),
            &grid,
        )
        .expect("HPA* should preprocess through neutral contract");
        let prepared_result =
            PreparedGridSearch::search(&prepared, request).expect("valid request");

        assert!(online.is_found());
        assert!(prepared_result.is_found());
        assert_eq!(
            online.path().map(|path| path.cost()),
            prepared_result.path().map(|path| path.cost())
        );
    }
}