condor_grid/algorithms/

subgoal_graph.rs

//! Prepared-grid sparse subgoal graph over free-space corners.
//!
//! [`SubgoalGraphBuilder`] selects subgoals on a uniform-cost grid; each
//! [`PreparedSubgoalGraph`] query runs abstract A* with lazy BFS edge costs and
//! returns the standard invalid/found/no-path outcome. Empty, oversized, or failed
//! abstractions fall back to online [`AStar`]. Prefer [`AStar`] for the ordinary
//! one-shot route, or [`super::jps_plus::JpsPlusBuilder`] for a durable jump-table lane.

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

use crate::{
    AStar, Grid, Path, Pathfinder, Point,
    preprocessed_grid::{
        PreparedGridSearch, PreprocessedGridBuildError, PreprocessedGridBuilder,
        PreprocessedGridMetadata, metadata_for_grid,
    },
    search::{SearchRequest, SearchResult},
};

/// [`PreprocessedGridBuilder`] for sparse corner subgoal graphs.
///
/// Uniform hop cost only (`traversal_cost == 1`). Correctness-first foundation with
/// online A* fallback when the subgoal set is empty, large, or abstract search fails.
/// Prefer as a multi-query experiment baseline, not a large-map acceleration claim.
#[derive(Debug, Clone, Copy, Default)]
pub struct SubgoalGraphBuilder;

impl SubgoalGraphBuilder {
    /// Creates a subgoal-graph builder (no configuration knobs in v0).
    #[must_use]
    pub const fn new() -> Self {
        Self
    }
}

impl PreprocessedGridBuilder for SubgoalGraphBuilder {
    type Map = PreparedSubgoalGraph;

    fn name(&self) -> &'static str {
        "subgoal-graph"
    }

    fn preprocess(&self, grid: &Grid) -> Result<Self::Map, PreprocessedGridBuildError> {
        ensure_uniform_traversal_costs(grid)?;
        let subgoals = select_corner_subgoals(grid);
        Ok(PreparedSubgoalGraph {
            grid: grid.clone(),
            metadata: metadata_for_grid(grid, self.name(), "subgoal-graph-query"),
            subgoals,
        })
    }
}

/// Prepared subgoal graph implementing [`PreparedGridSearch`].
///
/// Sparse corner-subgoal abstract search with online A* fallback; correctness-first,
/// not a performance claim for huge maps. Uniform hop cost only.
#[derive(Debug, Clone, PartialEq, Eq)]
pub struct PreparedSubgoalGraph {
    grid: Grid,
    metadata: PreprocessedGridMetadata,
    subgoals: Vec<Point>,
}

impl PreparedSubgoalGraph {
    /// Returns a [`SubgoalGraphBuilder`] for preprocess entry.
    #[must_use]
    pub fn builder() -> SubgoalGraphBuilder {
        SubgoalGraphBuilder
    }

    /// Number of corner subgoals retained after preprocess.
    #[must_use]
    pub fn subgoal_count(&self) -> usize {
        self.subgoals.len()
    }
}

impl PreparedGridSearch for PreparedSubgoalGraph {
    fn name(&self) -> &'static str {
        self.metadata.builder_name
    }

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

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

    fn search(&self, request: SearchRequest) -> SearchResult {
        crate::search::validate_request(&self.grid, request)?;
        // Keep abstract search sparse: too many corners degrades to online A*.
        if request.start == request.goal || self.subgoals.is_empty() || self.subgoals.len() > 64 {
            return AStar.search(&self.grid, request);
        }

        abstract_search(self, request).unwrap_or_else(|| AStar.search(&self.grid, request))
    }
}

#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)]
enum AbstractNode {
    Start,
    Goal,
    Subgoal(usize),
}

#[derive(Debug, Clone, PartialEq, Eq)]
struct HeapEntry {
    cost: usize,
    node: AbstractNode,
}

impl Ord for HeapEntry {
    fn cmp(&self, other: &Self) -> Ordering {
        other
            .cost
            .cmp(&self.cost)
            .then_with(|| node_rank(self.node).cmp(&node_rank(other.node)))
    }
}

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

fn node_rank(node: AbstractNode) -> u8 {
    match node {
        AbstractNode::Start => 0,
        AbstractNode::Goal => 1,
        AbstractNode::Subgoal(_) => 2,
    }
}

fn abstract_search(map: &PreparedSubgoalGraph, request: SearchRequest) -> Option<SearchResult> {
    let mut distances_from: HashMap<Point, HashMap<Point, usize>> = HashMap::new();
    let mut best = HashMap::new();
    let mut parent: HashMap<AbstractNode, AbstractNode> = HashMap::new();
    let mut heap = BinaryHeap::new();

    best.insert(AbstractNode::Start, 0usize);
    heap.push(HeapEntry {
        cost: 0,
        node: AbstractNode::Start,
    });

    let mut visited_nodes = 0usize;
    let watch = crate::search::BudgetWatch::start(request.budget);

    while let Some(HeapEntry { cost, node }) = heap.pop() {
        if best.get(&node).is_some_and(|&known| cost > known) {
            continue;
        }
        visited_nodes += 1;

        if matches!(node, AbstractNode::Goal) {
            let points = reconstruct_path(map, request, &parent)?;
            let path = Path::from_steps(points).ok()?;
            return Some(crate::search::found(path, visited_nodes));
        }

        if let Err(reason) = watch.check(visited_nodes) {
            return Some(Err(crate::search::budget_error(reason)));
        }

        for (next, step_cost) in outgoing_edges(map, node, request, &mut distances_from) {
            let next_cost = cost.saturating_add(step_cost);
            if best.get(&next).is_some_and(|&known| next_cost >= known) {
                continue;
            }
            best.insert(next, next_cost);
            parent.insert(next, node);
            heap.push(HeapEntry {
                cost: next_cost,
                node: next,
            });
        }
    }

    None
}

fn outgoing_edges(
    map: &PreparedSubgoalGraph,
    node: AbstractNode,
    request: SearchRequest,
    distances_from: &mut HashMap<Point, HashMap<Point, usize>>,
) -> Vec<(AbstractNode, usize)> {
    let from = match node_point(map, node, request) {
        Some(point) => point,
        None => return Vec::new(),
    };

    let distances = distances_from
        .entry(from)
        .or_insert_with(|| bfs_distances(&map.grid, from));

    let mut targets: Vec<(AbstractNode, Point)> = Vec::new();
    match node {
        AbstractNode::Start => {
            for (index, &subgoal) in map.subgoals.iter().enumerate() {
                targets.push((AbstractNode::Subgoal(index), subgoal));
            }
            targets.push((AbstractNode::Goal, request.goal));
        }
        AbstractNode::Subgoal(index) => {
            for (j, &subgoal) in map.subgoals.iter().enumerate() {
                if j != index {
                    targets.push((AbstractNode::Subgoal(j), subgoal));
                }
            }
            targets.push((AbstractNode::Goal, request.goal));
        }
        AbstractNode::Goal => return Vec::new(),
    }

    let mut out = Vec::new();
    for (next, to) in targets {
        if let Some(cost) = distances.get(&to).copied() {
            out.push((next, cost));
        }
    }
    out
}

fn reconstruct_path(
    map: &PreparedSubgoalGraph,
    request: SearchRequest,
    parent: &HashMap<AbstractNode, AbstractNode>,
) -> Option<Vec<Point>> {
    let mut chain = vec![AbstractNode::Goal];
    let mut current = AbstractNode::Goal;
    while !matches!(current, AbstractNode::Start) {
        current = *parent.get(&current)?;
        chain.push(current);
    }
    chain.reverse();

    let mut points = Vec::new();
    for window in chain.windows(2) {
        let from = node_point(map, window[0], request)?;
        let to = node_point(map, window[1], request)?;
        let segment = bfs_path(&map.grid, from, to)?;
        if points.is_empty() {
            points = segment;
        } else {
            points.extend(segment.into_iter().skip(1));
        }
    }
    Some(points)
}

fn node_point(
    map: &PreparedSubgoalGraph,
    node: AbstractNode,
    request: SearchRequest,
) -> Option<Point> {
    match node {
        AbstractNode::Start => Some(request.start),
        AbstractNode::Goal => Some(request.goal),
        AbstractNode::Subgoal(index) => map.subgoals.get(index).copied(),
    }
}

/// Walkable cells that sit at free-space corners relative to obstacles.
fn select_corner_subgoals(grid: &Grid) -> Vec<Point> {
    let mut subgoals = Vec::new();
    let width = grid.width();
    let height = grid.height();

    for y in 0..height {
        for x in 0..width {
            let point = Point::new(x, y);
            if grid.is_walkable(point) && is_free_space_corner(grid, point) {
                subgoals.push(point);
            }
        }
    }

    subgoals
}

fn is_free_space_corner(grid: &Grid, point: Point) -> bool {
    // 8-neighbor corner pattern: two consecutive cardinals free and the
    // intervening diagonal blocked (or out of bounds).
    let dirs = [
        (1isize, 0isize),
        (1, 1),
        (0, 1),
        (-1, 1),
        (-1, 0),
        (-1, -1),
        (0, -1),
        (1, -1),
    ];

    for i in (0..8).step_by(2) {
        let (c1x, c1y) = dirs[i];
        let (dx, dy) = dirs[(i + 1) % 8];
        let (c2x, c2y) = dirs[(i + 2) % 8];
        let cardinal_a = offset_point(point, c1x, c1y);
        let diagonal = offset_point(point, dx, dy);
        let cardinal_b = offset_point(point, c2x, c2y);

        if is_free(grid, cardinal_a)
            && is_free(grid, cardinal_b)
            && is_blocked_or_oob(grid, diagonal)
        {
            return true;
        }
    }

    false
}

fn offset_point(point: Point, dx: isize, dy: isize) -> Option<Point> {
    let x = point.x as isize + dx;
    let y = point.y as isize + dy;
    if x < 0 || y < 0 {
        return None;
    }
    Some(Point::new(x as usize, y as usize))
}

fn is_free(grid: &Grid, point: Option<Point>) -> bool {
    point.is_some_and(|p| grid.index_of(p).is_some() && grid.is_walkable(p))
}

fn is_blocked_or_oob(grid: &Grid, point: Option<Point>) -> bool {
    match point {
        None => true,
        Some(p) => grid.index_of(p).is_none() || !grid.is_walkable(p),
    }
}

fn ensure_uniform_traversal_costs(grid: &Grid) -> Result<(), PreprocessedGridBuildError> {
    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(PreprocessedGridBuildError::NonUniformCost {
                    algorithm: "subgoal-graph",
                    point,
                    cost,
                });
            }
        }
    }
    Ok(())
}

fn bfs_distances(grid: &Grid, start: Point) -> HashMap<Point, usize> {
    let mut distances = HashMap::new();
    if !grid.is_walkable(start) {
        return distances;
    }
    let mut queue = VecDeque::from([start]);
    distances.insert(start, 0usize);

    while let Some(current) = queue.pop_front() {
        let current_cost = distances[&current];
        for neighbor in four_neighbors(current) {
            if grid.index_of(neighbor).is_none() || !grid.is_walkable(neighbor) {
                continue;
            }
            if distances.contains_key(&neighbor) {
                continue;
            }
            distances.insert(neighbor, current_cost + 1);
            queue.push_back(neighbor);
        }
    }

    distances
}

fn bfs_path(grid: &Grid, start: Point, goal: Point) -> Option<Vec<Point>> {
    if start == goal {
        return Some(vec![start]);
    }
    if !grid.is_walkable(start) || !grid.is_walkable(goal) {
        return None;
    }

    let mut parent: HashMap<Point, Point> = HashMap::new();
    let mut queue = VecDeque::from([start]);
    parent.insert(start, start);

    while let Some(current) = queue.pop_front() {
        if current == goal {
            break;
        }
        for neighbor in four_neighbors(current) {
            if grid.index_of(neighbor).is_none() || !grid.is_walkable(neighbor) {
                continue;
            }
            if parent.contains_key(&neighbor) {
                continue;
            }
            parent.insert(neighbor, current);
            queue.push_back(neighbor);
        }
    }

    if !parent.contains_key(&goal) {
        return None;
    }

    let mut path = vec![goal];
    let mut cursor = goal;
    while cursor != start {
        cursor = parent[&cursor];
        path.push(cursor);
    }
    path.reverse();
    Some(path)
}

fn four_neighbors(point: Point) -> [Point; 4] {
    [
        Point::new(point.x.wrapping_sub(1), point.y),
        Point::new(point.x + 1, point.y),
        Point::new(point.x, point.y.wrapping_sub(1)),
        Point::new(point.x, point.y + 1),
    ]
}

#[cfg(test)]
mod tests {
    use super::*;
    use crate::{Cell, SearchRequest};

    #[test]
    fn subgoal_graph_matches_astar_on_simple_gap() {
        let mut grid = Grid::new(7, 5).expect("grid");
        for y in 0..5 {
            if y != 2 {
                grid.set_cell(Point::new(3, y), Cell::Blocked)
                    .expect("valid grid edit");
            }
        }

        let prepared = SubgoalGraphBuilder
            .preprocess(&grid)
            .expect("preprocess should succeed");
        let request = SearchRequest::new(Point::new(0, 2), Point::new(6, 2));
        let subgoal = prepared.search(request);
        let exact = AStar.search(&grid, request);

        assert!(subgoal.as_ref().expect("valid search request").is_found());
        assert_eq!(
            subgoal.as_ref().expect("valid search request").cost(),
            exact.as_ref().expect("valid search request").cost()
        );
    }

    #[test]
    fn subgoal_graph_rejects_weighted_grids() {
        let mut grid = Grid::new(4, 3).expect("grid dimensions are valid");
        grid.set_traversal_cost(Point::new(1, 1), 3)
            .expect("cost should be valid");

        let error = SubgoalGraphBuilder
            .preprocess(&grid)
            .expect_err("weighted grid should fail");
        assert!(error.to_string().contains("uniform"));
    }

    #[test]
    fn subgoal_graph_queries_an_immutable_snapshot() {
        let mut grid = Grid::new(5, 1).expect("grid dimensions are valid");
        let request = SearchRequest::new(Point::new(0, 0), Point::new(4, 0));
        let prepared = SubgoalGraphBuilder
            .preprocess(&grid)
            .expect("preprocess should succeed");
        grid.set_cell(Point::new(2, 0), Cell::Blocked)
            .expect("cell should be in bounds");

        let prepared_result = prepared.search(request).expect("valid search request");
        let changed = AStar.search(&grid, request).expect("valid search request");

        assert!(prepared_result.is_found());
        assert_eq!(prepared_result.path().map(|path| path.cost()), Some(4));
        assert!(!changed.is_found());
    }

    #[test]
    fn subgoal_graph_matches_astar_on_a_uniform_obstacle_course() {
        let mut grid = Grid::new(7, 5).expect("grid dimensions are valid");
        for y in 0..5 {
            if y != 2 {
                grid.set_cell(Point::new(3, y), Cell::Blocked)
                    .expect("fixture point is in bounds");
            }
        }
        let request = SearchRequest::new(Point::new(0, 2), Point::new(6, 2));

        let prepared = SubgoalGraphBuilder
            .preprocess(&grid)
            .expect("subgoal preprocessing succeeds");
        let prepared_result = prepared.search(request).expect("request is valid");
        let exact = AStar.search(&grid, request).expect("request is valid");

        assert_eq!(prepared_result.cost(), exact.cost());
        assert_eq!(prepared_result.is_found(), exact.is_found());
        assert!(
            prepared_result
                .path()
                .is_some_and(|path| prepared.grid().path_is_walkable(path.steps()))
        );
    }
}