condor-pathfinding-grid 0.4.0

//! Private candidate: monotone-bucket A* for static unweighted grids.
//!
//! **Hypothesis:** a bounded, monotone `f`-cost bucket frontier can reduce
//! priority-queue overhead on four-connected unit-cost grids without changing
//! canonical A* results.
//!
//! **Non-negotiable behavior:** preserve the established A* path-cost, no-path,
//! invalid-input, and deterministic tie behavior. Bucket rollover and stale
//! entries must not alter when a goal is settled or make a bounded frontier an
//! implicit approximation mode.
//!
//! **Failure memory:** an indexed decrease-key A* previously regressed the
//! fast static scenarios. Do not promote a more elaborate frontier merely by
//! reducing duplicate entries; compare complete end-to-end work against the
//! existing A* baseline.
//!
//! **Evidence and promotion:** ordinary `grid_core` route for development;
//! promotion still needs `just test-grid-conformance` and end-to-end evidence
//! through `just bench-grid-core`. Remains private: no feature, fixture, target,
//! or separate harness route.

use std::collections::VecDeque;

use crate::{
    grid::Grid,
    path::Path,
    point::Point,
    search::{BudgetWatch, Pathfinder, SearchRequest, SearchResult},
};

/// Online [`Pathfinder`] candidate: unit-cost monotone-bucket A*.
///
/// Cost model matches [`super::astar::AStar`]: sum of entered-cell
/// `traversal_cost` on 4-connected edges with Manhattan heuristic. The open
/// set is a rising `f`-bucket list instead of a binary heap.
#[derive(Debug, Default, Clone, Copy)]
pub struct AStarMonotoneBucket;

impl AStarMonotoneBucket {
    /// Stable source-local candidate identity.
    pub const CANDIDATE_ID: &str = "static-unweighted-grid/monotone-bucket-astar";
}

impl Pathfinder for AStarMonotoneBucket {
    fn name(&self) -> &'static str {
        "monotone-bucket"
    }

    fn search(&self, grid: &Grid, request: SearchRequest) -> SearchResult {
        crate::search::validate_request(grid, request)?;
        let Some(start_index) = grid.index_of(request.start) else {
            return crate::search::not_found(0);
        };
        let Some(goal_index) = grid.index_of(request.goal) else {
            return crate::search::not_found(0);
        };

        if !grid.is_walkable(request.start) || !grid.is_walkable(request.goal) {
            return crate::search::not_found(0);
        }

        if !grid.is_reachable(request.start, 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,
            );
        }

        let initial_h = manhattan_distance(request.start, request.goal);
        // Buckets keyed by f = g + h. Monotone extraction walks min_f upward.
        let mut buckets: Vec<VecDeque<(usize, usize)>> = Vec::new();
        let mut min_f = initial_h;
        ensure_bucket(&mut buckets, initial_h);
        buckets[initial_h].push_back((0, start_index));

        let mut best_costs: Vec<Option<usize>> = vec![None; grid.cell_count()];
        let mut parents: Vec<Option<usize>> = vec![None; grid.cell_count()];
        let mut visited_nodes = 0usize;
        let watch = BudgetWatch::start(request.budget);
        best_costs[start_index] = Some(0);

        loop {
            while min_f < buckets.len() && buckets[min_f].is_empty() {
                min_f += 1;
            }
            if min_f >= buckets.len() {
                return crate::search::not_found(visited_nodes);
            }

            let Some((cost_so_far, current_index)) = buckets[min_f].pop_front() else {
                continue;
            };

            // Stale bucket entry after a better path was found.
            if best_costs[current_index] != Some(cost_so_far) {
                continue;
            }

            visited_nodes += 1;
            // Goal settlement precedes budget: BudgetWatch must not reject an
            // accepted goal expansion (matches A*/Dijkstra/Bfs family order).
            if current_index == goal_index {
                return crate::search::found(
                    reconstruct_path(grid, &parents, start_index, goal_index, cost_so_far),
                    visited_nodes,
                );
            }

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

            let current = grid.point_from_index(current_index);
            for neighbor in grid.neighbors4(current) {
                let neighbor_index = grid
                    .index_of(neighbor)
                    .expect("walkable neighbors must exist inside the grid");
                let edge_cost = grid
                    .traversal_cost(neighbor)
                    .expect("walkable neighbors must have a traversal cost");
                let Some(next_cost) = cost_so_far.checked_add(edge_cost) else {
                    continue;
                };

                if best_costs[neighbor_index].is_some_and(|best| next_cost >= best) {
                    continue;
                }

                best_costs[neighbor_index] = Some(next_cost);
                parents[neighbor_index] = Some(current_index);
                let f = next_cost.saturating_add(manhattan_distance(neighbor, request.goal));
                ensure_bucket(&mut buckets, f);
                buckets[f].push_back((next_cost, neighbor_index));
                if f < min_f {
                    min_f = f;
                }
            }
        }
    }
}

fn ensure_bucket(buckets: &mut Vec<VecDeque<(usize, usize)>>, f: usize) {
    if buckets.len() <= f {
        buckets.resize_with(f + 1, VecDeque::new);
    }
}

fn reconstruct_path(
    grid: &Grid,
    parents: &[Option<usize>],
    start_index: usize,
    goal_index: usize,
    total_cost: usize,
) -> Path {
    let mut current_index = goal_index;
    let mut steps = vec![grid.point_from_index(goal_index)];

    while let Some(parent_index) = parents[current_index] {
        steps.push(grid.point_from_index(parent_index));
        current_index = parent_index;
    }

    steps.reverse();
    debug_assert_eq!(current_index, start_index);
    Path::from_steps_with_cost(steps, total_cost).expect("path contains at least one point")
}

fn manhattan_distance(from: Point, to: Point) -> usize {
    from.x.abs_diff(to.x) + from.y.abs_diff(to.y)
}

#[cfg(test)]
mod tests {
    use crate::{
        algorithms::{astar::AStar, astar_monotone_bucket::AStarMonotoneBucket},
        grid::{Cell, Grid},
        point::Point,
        search::{BudgetExhausted, GridSearchError, Pathfinder, SearchBudget, SearchRequest},
    };

    #[test]
    fn matches_astar_cost_through_the_only_gap() {
        let mut grid = Grid::new(5, 5).expect("grid dimensions are valid");
        for y in 0..5 {
            if y != 2 {
                grid.set_cell(Point::new(2, y), Cell::Blocked)
                    .expect("valid grid edit");
            }
        }
        let request = SearchRequest::new(Point::new(0, 0), Point::new(4, 4));

        let candidate = AStarMonotoneBucket
            .search(&grid, request)
            .expect("endpoints are walkable");
        let astar = AStar
            .search(&grid, request)
            .expect("endpoints are walkable");

        assert!(candidate.is_found());
        assert_eq!(candidate.cost(), astar.cost());
    }

    #[test]
    fn matches_astar_cost_on_a_weighted_detour() {
        let mut grid = Grid::new(4, 3).expect("grid dimensions are valid");
        grid.set_traversal_cost(Point::new(1, 1), 10)
            .expect("valid cost edit");
        grid.set_traversal_cost(Point::new(2, 1), 10)
            .expect("valid cost edit");
        let request = SearchRequest::new(Point::new(0, 1), Point::new(3, 1));

        let candidate = AStarMonotoneBucket
            .search(&grid, request)
            .expect("endpoints are walkable");
        let astar = AStar
            .search(&grid, request)
            .expect("endpoints are walkable");

        assert!(candidate.is_found());
        assert_eq!(candidate.cost(), astar.cost());
    }

    #[test]
    fn reports_when_no_path_exists() {
        let mut grid = Grid::new(3, 3).expect("grid dimensions are valid");
        for x in 0..3 {
            grid.set_cell(Point::new(x, 1), Cell::Blocked)
                .expect("valid grid edit");
        }

        let result = AStarMonotoneBucket
            .search(
                &grid,
                SearchRequest::new(Point::new(0, 0), Point::new(2, 2)),
            )
            .expect("endpoints are walkable");
        assert!(!result.is_found());
    }

    #[test]
    fn expansion_budget_stops_before_goal() {
        let grid = Grid::new(6, 1).expect("grid dimensions are valid");
        let request = SearchRequest::new(Point::new(0, 0), Point::new(5, 0))
            .with_budget(SearchBudget::max_expansions(2));
        let error = AStarMonotoneBucket
            .search(&grid, request)
            .expect_err("budget should exhaust on a long corridor");
        assert_eq!(
            error,
            GridSearchError::BudgetExhausted(BudgetExhausted::Expansions {
                limit: 2,
                expansions: 2
            })
        );
    }

    #[test]
    fn goal_settlement_on_expansion_limit_is_found_not_budget() {
        // Unit-cost corridor length 3: start, mid, goal. Goal is first settled
        // on expansion N when limit is N — must be Found, not BudgetExhausted.
        let grid = Grid::new(3, 1).expect("grid dimensions are valid");
        let request = SearchRequest::new(Point::new(0, 0), Point::new(2, 0))
            .with_budget(SearchBudget::max_expansions(3));
        let result = AStarMonotoneBucket
            .search(&grid, request)
            .expect("goal expansion must complete within the limit");
        assert!(result.is_found());
        assert_eq!(result.cost(), Some(2));
    }

    #[test]
    fn retains_candidate_id() {
        assert_eq!(
            AStarMonotoneBucket::CANDIDATE_ID,
            "static-unweighted-grid/monotone-bucket-astar"
        );
    }
}