condor-pathfinding-grid 0.4.0

//! Private candidate contract: IDA* for static weighted 4-connected grids.
//!
//! **Hypothesis:** iterative-deepening A* with the family Manhattan heuristic
//! reproduces the established A* path cost with O(path length) memory, useful
//! where the open/closed arrays of A* are too heavy.
//!
//! **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 counting cumulative node visits across thresholds.
//!
//! **Evidence and promotion:** ordinary `grid_core` route
//! (`just test-fast grid_core`, `just clippy-target grid_core`); promotion
//! requires cost parity against the A* baseline plus memory evidence. This
//! remains private: no feature, fixture, target, or separate harness route.

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

/// Online [`Pathfinder`] candidate: weighted iterative-deepening A*.
///
/// Cost model: sums per-cell `traversal_cost` on 4-connected edges with a
/// Manhattan heuristic, matching [`super::astar::AStar`]. Optimal, with
/// path-length-bounded memory in exchange for threshold re-expansion.
#[derive(Debug, Default, Clone, Copy)]
pub struct IterativeDeepeningAStar;

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

    fn search(&self, grid: &Grid, request: SearchRequest) -> SearchResult {
        crate::search::validate_request(grid, request)?;

        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);
        let mut visited_nodes = 0usize;
        let mut threshold = manhattan_distance(request.start, request.goal);

        loop {
            let mut on_path = vec![request.start];
            let outcome = bounded_search(
                grid,
                request.goal,
                0,
                threshold,
                &mut on_path,
                &mut visited_nodes,
                &watch,
            )?;

            match outcome {
                BoundedOutcome::Found { cost } => {
                    return crate::search::found(
                        Path::from_steps_with_cost(on_path, cost)
                            .expect("path contains at least one point"),
                        visited_nodes,
                    );
                }
                BoundedOutcome::NextThreshold(next) => threshold = next,
                BoundedOutcome::Exhausted => return crate::search::not_found(visited_nodes),
            }
        }
    }
}

enum BoundedOutcome {
    /// Goal reached at exactly this accumulated cost.
    Found { cost: usize },
    /// No goal within the threshold; retry with this smallest exceeding `f`.
    NextThreshold(usize),
    /// No goal and no finite next threshold: search space exhausted.
    Exhausted,
}

fn bounded_search(
    grid: &Grid,
    goal: Point,
    cost_so_far: usize,
    threshold: usize,
    on_path: &mut Vec<Point>,
    visited_nodes: &mut usize,
    watch: &BudgetWatch,
) -> Result<BoundedOutcome, crate::search::GridSearchError> {
    let current = *on_path.last().expect("path always holds the start");
    *visited_nodes += 1;

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

    let lower_bound = cost_so_far.saturating_add(manhattan_distance(current, goal));
    if lower_bound > threshold {
        return Ok(BoundedOutcome::NextThreshold(lower_bound));
    }

    if current == goal {
        return Ok(BoundedOutcome::Found { cost: cost_so_far });
    }

    let mut next_threshold: Option<usize> = None;
    for neighbor in grid.neighbors4(current) {
        if on_path.contains(&neighbor) {
            continue;
        }

        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;
        };

        on_path.push(neighbor);
        let outcome = bounded_search(
            grid,
            goal,
            next_cost,
            threshold,
            on_path,
            visited_nodes,
            watch,
        )?;
        match outcome {
            BoundedOutcome::Found { cost } => return Ok(BoundedOutcome::Found { cost }),
            BoundedOutcome::NextThreshold(candidate) => {
                on_path.pop();
                next_threshold = Some(next_threshold.map_or(candidate, |best: usize| {
                    if candidate < best { candidate } else { best }
                }));
            }
            BoundedOutcome::Exhausted => {
                on_path.pop();
            }
        }
    }

    Ok(next_threshold.map_or(BoundedOutcome::Exhausted, BoundedOutcome::NextThreshold))
}

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, iterative_deepening_astar::IterativeDeepeningAStar},
        grid::{Cell, Grid},
        point::Point,
        search::{Pathfinder, SearchRequest},
    };

    #[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 idastar = IterativeDeepeningAStar
            .search(&grid, request)
            .expect("endpoints are walkable");
        let astar = AStar
            .search(&grid, request)
            .expect("endpoints are walkable");

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

    #[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 idastar = IterativeDeepeningAStar
            .search(&grid, request)
            .expect("endpoints are walkable");
        let astar = AStar
            .search(&grid, request)
            .expect("endpoints are walkable");

        assert!(idastar.is_found());
        assert_eq!(idastar.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 = IterativeDeepeningAStar
            .search(
                &grid,
                SearchRequest::new(Point::new(0, 0), Point::new(2, 2)),
            )
            .expect("endpoints are walkable");
        assert!(!result.is_found());
    }
}