condor-pathfinding-grid 0.4.0

//! Private candidate contract: fringe search for static weighted 4-connected grids.
//!
//! **Hypothesis:** the now/later fringe-list iteration replaces the binary-heap
//! frontier of A* with cheap list rotation, which can win on grids where heap
//! churn dominates and the Manhattan bound is tight.
//!
//! **Non-negotiable behavior:** the same weighted cost model as
//! [`super::astar::AStar`] (sum of entered-cell `traversal_cost`, Manhattan
//! heuristic), identical optimal costs, standard invalid/found/no-path outcome,
//! and budget enforcement through [`BudgetWatch`].
//!
//! **Evidence and promotion:** ordinary `grid_core` route
//! (`just test-fast grid_core`, `just clippy-target grid_core`); promotion
//! requires end-to-end evidence against the A* baseline through
//! `just bench-grid-core`. This 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: weighted fringe search.
///
/// Cost model: sums per-cell `traversal_cost` on 4-connected edges with a
/// Manhattan heuristic, matching [`super::astar::AStar`]. Optimal; iterates a
/// fringe list under a rising `f`-limit instead of maintaining a heap.
#[derive(Debug, Default, Clone, Copy)]
pub struct FringeSearch;

impl Pathfinder for FringeSearch {
    fn name(&self) -> &'static str {
        "fringe"
    }

    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_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 mut fringe = VecDeque::from([start_index]);
        let mut in_fringe = vec![false; grid.cell_count()];
        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);
        let mut flimit = manhattan_distance(request.start, request.goal);

        in_fringe[start_index] = true;
        best_costs[start_index] = Some(0);

        loop {
            let mut next_flimit: Option<usize> = None;
            let mut rotations = fringe.len();

            while rotations > 0 {
                rotations -= 1;
                let Some(current_index) = fringe.pop_front() else {
                    break;
                };
                in_fringe[current_index] = false;

                let cost_so_far =
                    best_costs[current_index].expect("fringe nodes always carry a best cost");
                let current = grid.point_from_index(current_index);
                let f_cost = cost_so_far.saturating_add(manhattan_distance(current, request.goal));

                if f_cost > flimit {
                    // Defer past-limit nodes to the next threshold round.
                    next_flimit = Some(next_flimit.map_or(f_cost, |best: usize| best.min(f_cost)));
                    fringe.push_back(current_index);
                    in_fringe[current_index] = true;
                    continue;
                }

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

                if current_index == goal_index {
                    return crate::search::found(
                        reconstruct_path(grid, &parents, start_index, goal_index, cost_so_far),
                        visited_nodes,
                    );
                }

                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);
                    if !in_fringe[neighbor_index] {
                        fringe.push_front(neighbor_index);
                        in_fringe[neighbor_index] = true;
                        rotations += 1;
                    }
                }
            }

            match next_flimit {
                Some(limit) if !fringe.is_empty() => flimit = limit,
                _ => return crate::search::not_found(visited_nodes),
            }
        }
    }
}

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, fringe_search::FringeSearch},
        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 fringe = FringeSearch
            .search(&grid, request)
            .expect("endpoints are walkable");
        let astar = AStar
            .search(&grid, request)
            .expect("endpoints are walkable");

        assert!(fringe.is_found());
        assert_eq!(fringe.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 fringe = FringeSearch
            .search(&grid, request)
            .expect("endpoints are walkable");
        let astar = AStar
            .search(&grid, request)
            .expect("endpoints are walkable");

        assert!(fringe.is_found());
        assert_eq!(fringe.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 = FringeSearch
            .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 = FringeSearch
            .search(&grid, request)
            .expect_err("budget should exhaust on a long corridor");
        assert_eq!(
            error,
            GridSearchError::BudgetExhausted(BudgetExhausted::Expansions {
                limit: 2,
                expansions: 2
            })
        );
    }
}