condor-pathfinding-grid 0.4.0

//! Private candidate: bounded-escape A* for the static-grid family.
//!
//! **Hypothesis:** a deterministic expansion budget with a broad-frontier escape
//! can improve bounded work without changing the canonical A* answer.
//!
//! **Non-negotiable behavior:** retain four-way shortest-path and `NoPath`
//! semantics, deterministic behavior where the existing contract requires it,
//! and an exact fallback handoff. The bounded phase must never be reported as a
//! complete result merely because its budget is exhausted.
//!
//! **Evidence and promotion:** ordinary `grid_core` while developing; exact
//! parity through grid conformance; performance evidence must include the
//! escape/fallback work. Remains private: no feature, fixture, target, or
//! separate harness route.

#![allow(
    dead_code,
    reason = "private candidate retained for normal-family evaluation"
)]

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

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

/// Deterministic expansion cap for the bounded phase only.
///
/// Exhausting this cap is **not** a search outcome: control falls through to
/// an exact A* handoff. Only the caller [`crate::SearchBudget`] may produce
/// [`crate::GridSearchError::BudgetExhausted`].
const BOUNDED_PHASE_EXPANSIONS: usize = 8;

/// Online [`Pathfinder`] candidate: expansion-budgeted A* with exact fallback.
///
/// Cost model matches [`super::astar::AStar`]. Bounded-phase exhaustion alone
/// never yields Found/NoPath; the exact handoff completes the proof.
#[derive(Debug, Default, Clone, Copy)]
pub struct AStarBounded;

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

impl Pathfinder for AStarBounded {
    fn name(&self) -> &'static str {
        "bounded-astar"
    }

    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 watch = BudgetWatch::start(request.budget);

        // Phase 1: bounded A*. Local expansion cap is not a terminal outcome.
        match run_astar_phase(
            grid,
            request,
            start_index,
            goal_index,
            Some(BOUNDED_PHASE_EXPANSIONS),
            &watch,
            0,
        )? {
            PhaseOutcome::Found {
                path,
                visited_nodes,
            } => crate::search::found(path, visited_nodes),
            PhaseOutcome::Continue { visited_nodes } => {
                // Phase 2: exact fallback — full A* with caller budget only.
                match run_astar_phase(
                    grid,
                    request,
                    start_index,
                    goal_index,
                    None,
                    &watch,
                    visited_nodes,
                )? {
                    PhaseOutcome::Found {
                        path,
                        visited_nodes,
                    } => crate::search::found(path, visited_nodes),
                    PhaseOutcome::Continue { visited_nodes }
                    | PhaseOutcome::ExhaustedLocal { visited_nodes } => {
                        crate::search::not_found(visited_nodes)
                    }
                }
            }
            PhaseOutcome::ExhaustedLocal { visited_nodes } => {
                // Local phase cap: always hand off to exact A* (never terminal).
                match run_astar_phase(
                    grid,
                    request,
                    start_index,
                    goal_index,
                    None,
                    &watch,
                    visited_nodes,
                )? {
                    PhaseOutcome::Found {
                        path,
                        visited_nodes,
                    } => crate::search::found(path, visited_nodes),
                    PhaseOutcome::Continue { visited_nodes }
                    | PhaseOutcome::ExhaustedLocal { visited_nodes } => {
                        crate::search::not_found(visited_nodes)
                    }
                }
            }
        }
    }
}

enum PhaseOutcome {
    Found {
        path: Path,
        visited_nodes: usize,
    },
    /// Bounded phase hit its local cap without a complete answer.
    ExhaustedLocal {
        visited_nodes: usize,
    },
    /// Open set emptied without finding the goal (exact phase no-path).
    Continue {
        visited_nodes: usize,
    },
}

fn run_astar_phase(
    grid: &Grid,
    request: SearchRequest,
    start_index: usize,
    goal_index: usize,
    local_expansion_cap: Option<usize>,
    watch: &BudgetWatch,
    prior_visited: usize,
) -> Result<PhaseOutcome, crate::search::GridSearchError> {
    let initial_heuristic = manhattan_distance(request.start, request.goal);
    let mut frontier = BinaryHeap::from([FrontierEntry {
        estimated_total_cost: initial_heuristic,
        cost_so_far: 0,
        index: start_index,
    }]);
    let mut best_costs = vec![None; grid.cell_count()];
    let mut parents = vec![None; grid.cell_count()];
    let mut visited_nodes = 0usize;
    best_costs[start_index] = Some(0);

    while let Some(entry) = frontier.pop() {
        if best_costs[entry.index] != Some(entry.cost_so_far) {
            continue;
        }

        visited_nodes += 1;
        let total_visited = prior_visited.saturating_add(visited_nodes);

        if entry.index == goal_index {
            return Ok(PhaseOutcome::Found {
                path: reconstruct_path(grid, &parents, start_index, goal_index, entry.cost_so_far),
                visited_nodes: total_visited,
            });
        }

        // Caller budget owns BudgetExhausted; check before local phase cap.
        if let Err(reason) = watch.check(total_visited) {
            return Err(crate::search::budget_error(reason));
        }

        if let Some(cap) = local_expansion_cap
            && visited_nodes >= cap
        {
            // Bounded phase exhaust: hand off; never Found/NoPath here.
            return Ok(PhaseOutcome::ExhaustedLocal {
                visited_nodes: total_visited,
            });
        }

        let current = grid.point_from_index(entry.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) = entry.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(entry.index);
            let heuristic = manhattan_distance(neighbor, request.goal);
            frontier.push(FrontierEntry {
                estimated_total_cost: next_cost.saturating_add(heuristic),
                cost_so_far: next_cost,
                index: neighbor_index,
            });
        }
    }

    Ok(PhaseOutcome::Continue {
        visited_nodes: prior_visited.saturating_add(visited_nodes),
    })
}

#[derive(Debug, Clone, Copy, PartialEq, Eq)]
struct FrontierEntry {
    estimated_total_cost: usize,
    cost_so_far: usize,
    index: usize,
}

impl Ord for FrontierEntry {
    fn cmp(&self, other: &Self) -> Ordering {
        other
            .estimated_total_cost
            .cmp(&self.estimated_total_cost)
            .then_with(|| other.cost_so_far.cmp(&self.cost_so_far))
            .then_with(|| other.index.cmp(&self.index))
    }
}

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

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_bounded::AStarBounded},
        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 = AStarBounded
            .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 = AStarBounded
            .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 = AStarBounded
            .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(20, 1).expect("grid dimensions are valid");
        let request = SearchRequest::new(Point::new(0, 0), Point::new(19, 0))
            .with_budget(SearchBudget::max_expansions(2));
        let error = AStarBounded
            .search(&grid, request)
            .expect_err("caller budget should exhaust on a long corridor");
        assert_eq!(
            error,
            GridSearchError::BudgetExhausted(BudgetExhausted::Expansions {
                limit: 2,
                expansions: 2
            })
        );
    }

    #[test]
    fn bounded_phase_budget_exhaustion_is_not_complete_without_fallback() {
        // Long corridor: bounded phase alone cannot finish within its local cap,
        // but exact fallback must still find the A*-optimal path.
        let grid = Grid::new(40, 1).expect("grid dimensions are valid");
        let request = SearchRequest::new(Point::new(0, 0), Point::new(39, 0));

        let candidate = AStarBounded
            .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());
        // Visited nodes include both phases when fallback runs.
        assert!(
            candidate.visited_nodes() > super::BOUNDED_PHASE_EXPANSIONS,
            "fallback handoff should continue expanding past the local phase cap"
        );
    }

    #[test]
    fn local_phase_cap_alone_never_returns_err_or_no_path() {
        // Unlimited caller budget + path needing more than BOUNDED_PHASE_EXPANSIONS
        // expansions: must Found, never BudgetExhausted or NoPath from local cap.
        let grid = Grid::new(40, 1).expect("grid dimensions are valid");
        let request = SearchRequest::new(Point::new(0, 0), Point::new(39, 0));
        let result = AStarBounded
            .search(&grid, request)
            .expect("local cap must hand off, not Err");
        assert!(result.is_found());
        assert_eq!(result.cost(), Some(39));
    }

    #[test]
    fn retains_candidate_id() {
        assert_eq!(
            AStarBounded::CANDIDATE_ID,
            "static-unweighted-grid/bounded-astar-escape"
        );
    }
}