condor-pathfinding-grid 0.4.0

//! Static-grid [`Pathfinder`]: online cardinal Jump Point Search.
//!
//! Each search scans its jump rays at query time and returns the standard
//! invalid/found/no-path outcome. Jump-edge cost sums per-cell
//! [`traversal_cost`](crate::Grid::traversal_cost) and uses a Manhattan heuristic.
//! Prefer [`super::astar::AStar`] as the general weighted default, or
//! [`super::jps_plus::JpsPlusBuilder`] when a static map justifies preprocessing.

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

use crate::{
    algorithms::jps_cardinal::{
        Direction, PointKind, classify_point_kind, manhattan_distance, reconstruct_jump_path, step,
    },
    grid::Grid,
    path::Path,
    point::Point,
    search::{Pathfinder, SearchRequest, SearchResult},
};

/// Online cardinal [`Pathfinder`]: Jump Point Search (4-way).
///
/// Scans jump rays at query time; weighted jump edges sum `traversal_cost` along the
/// ray. Prefer on corridor-heavy maps; open rooms limit pruning. Prefer JPS+ when many
/// queries share a static grid (precomputed jump table).
#[derive(Debug, Default, Clone, Copy)]
pub struct JumpPointSearch;

/// Frontier and jump-scan counters from an instrumented [`JumpPointSearch::inspect`] run.
///
/// Bakeoff/inspect only; not part of public [`crate::SearchStats`].
#[derive(Debug, Default, Clone, Copy, PartialEq, Eq)]
pub struct JumpPointSearchDiagnostics {
    /// Open-set inserts (including re-pushes after decrease).
    pub frontier_pushes: usize,
    /// Open-set pops (including stale).
    pub frontier_pops: usize,
    /// Pops discarded because a better cost was already known.
    pub stale_pops_skipped: usize,
    /// Peak open-set size.
    pub peak_frontier_len: usize,
    /// Cardinal ray scan starts.
    pub scan_attempts: usize,
    /// Scans that hit a blocked neighbor immediately.
    pub blocked_scans: usize,
    /// Scans that produced at least one jump stop.
    pub successful_scans: usize,
    /// Candidate jump-edge relaxations considered.
    pub relaxation_attempts: usize,
    /// Relaxations that improved the known cost.
    pub relaxations_accepted: usize,
    /// Sum of jump lengths (cells) over successful scans.
    pub total_jump_length: usize,
    /// Longest single jump observed.
    pub max_jump_length: usize,
    /// Stops after a single-cell jump.
    pub jump_length_1_stops: usize,
    /// Stops after a short jump (2..=4 cells).
    pub jump_length_2_to_4_stops: usize,
    /// Stops after a long jump (≥5 cells).
    pub jump_length_5_plus_stops: usize,
    /// Rays that stopped because the goal was reached.
    pub goal_stops: usize,
    /// Rays that stopped at a branch cell.
    pub branch_stops: usize,
    /// Rays that stopped at an elbow turn.
    pub elbow_turn_stops: usize,
    /// Rays that stopped at a dead-end.
    pub dead_end_stops: usize,
}

/// Search outcome paired with jump-pruning diagnostics.
#[derive(Debug, Clone, PartialEq, Eq)]
pub struct JumpPointSearchInspection {
    /// Standard found/no-path or invalid/budget result.
    pub result: SearchResult,
    /// Jump-scan tallies for this instrumented run.
    pub diagnostics: JumpPointSearchDiagnostics,
}

impl JumpPointSearch {
    /// Runs an instrumented search. Invalid endpoints are reported through
    /// [`JumpPointSearchInspection::result`] with the same errors as
    /// [`Pathfinder::search`].
    #[must_use]
    pub fn inspect(&self, grid: &Grid, request: SearchRequest) -> JumpPointSearchInspection {
        if let Err(error) = crate::search::validate_request(grid, request) {
            return JumpPointSearchInspection {
                result: Err(error),
                diagnostics: JumpPointSearchDiagnostics::default(),
            };
        }
        let execution = search_impl::<true>(grid, request);
        JumpPointSearchInspection {
            result: execution.result,
            diagnostics: execution.diagnostics,
        }
    }
}

impl Pathfinder for JumpPointSearch {
    fn name(&self) -> &'static str {
        "jump-point-search"
    }

    fn search(&self, grid: &Grid, request: SearchRequest) -> SearchResult {
        crate::search::validate_request(grid, request)?;
        search_impl::<false>(grid, request).result
    }
}

#[derive(Debug, Clone, PartialEq, Eq)]
struct SearchExecution {
    result: SearchResult,
    diagnostics: JumpPointSearchDiagnostics,
}

fn search_impl<const TRACK_DIAGNOSTICS: bool>(
    grid: &Grid,
    request: SearchRequest,
) -> SearchExecution {
    let mut diagnostics = JumpPointSearchDiagnostics::default();

    let Some(start_index) = grid.index_of(request.start) else {
        return SearchExecution {
            result: crate::search::not_found(0),
            diagnostics,
        };
    };
    let Some(goal_index) = grid.index_of(request.goal) else {
        return SearchExecution {
            result: crate::search::not_found(0),
            diagnostics,
        };
    };

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

    if !grid.is_reachable(request.start, request.goal) {
        return SearchExecution {
            result: crate::search::not_found(0),
            diagnostics,
        };
    }

    if request.start == request.goal {
        return SearchExecution {
            result: crate::search::found(
                Path::from_steps(vec![request.start]).expect("path contains at least one point"),
                1,
            ),
            diagnostics,
        };
    }

    let initial_heuristic = manhattan_distance(request.start, request.goal);
    let mut frontier = BinaryHeap::from([FrontierEntry {
        estimated_total_cost: initial_heuristic,
        heuristic_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;
    let watch = crate::search::BudgetWatch::start(request.budget);

    best_costs[start_index] = Some(0);

    if TRACK_DIAGNOSTICS {
        diagnostics.frontier_pushes = 1;
        diagnostics.peak_frontier_len = 1;
    }

    while let Some(entry) = frontier.pop() {
        if TRACK_DIAGNOSTICS {
            diagnostics.frontier_pops += 1;
        }

        if best_costs[entry.index] != Some(entry.cost_so_far) {
            if TRACK_DIAGNOSTICS {
                diagnostics.stale_pops_skipped += 1;
            }
            continue;
        }

        visited_nodes += 1;
        if entry.index == goal_index {
            break;
        }

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

        let current = grid.point_from_index(entry.index);
        for direction in Direction::ALL {
            if TRACK_DIAGNOSTICS {
                diagnostics.scan_attempts += 1;
            }

            let Some(scan_stop) =
                jump_in_direction(grid, current, entry.index, direction, goal_index)
            else {
                if TRACK_DIAGNOSTICS {
                    diagnostics.blocked_scans += 1;
                }
                continue;
            };

            if TRACK_DIAGNOSTICS {
                diagnostics.successful_scans += 1;
                diagnostics.total_jump_length += scan_stop.edge_cost;
                diagnostics.max_jump_length = diagnostics.max_jump_length.max(scan_stop.edge_cost);
                match scan_stop.edge_cost {
                    0 => {}
                    1 => diagnostics.jump_length_1_stops += 1,
                    2..=4 => diagnostics.jump_length_2_to_4_stops += 1,
                    _ => diagnostics.jump_length_5_plus_stops += 1,
                }

                match scan_stop.reason {
                    StopReason::Goal => diagnostics.goal_stops += 1,
                    StopReason::Branch => diagnostics.branch_stops += 1,
                    StopReason::ElbowTurn => diagnostics.elbow_turn_stops += 1,
                    StopReason::DeadEnd => diagnostics.dead_end_stops += 1,
                }
                diagnostics.relaxation_attempts += 1;
            }

            let Some(next_cost) = entry.cost_so_far.checked_add(scan_stop.edge_weight) else {
                continue;
            };
            if best_costs[scan_stop.target_index].is_some_and(|best_cost| next_cost >= best_cost) {
                continue;
            }

            best_costs[scan_stop.target_index] = Some(next_cost);
            parents[scan_stop.target_index] = Some(entry.index);
            let target = grid.point_from_index(scan_stop.target_index);
            let heuristic_cost = manhattan_distance(target, request.goal);
            frontier.push(FrontierEntry {
                estimated_total_cost: next_cost.saturating_add(heuristic_cost),
                heuristic_cost,
                cost_so_far: next_cost,
                index: scan_stop.target_index,
            });

            if TRACK_DIAGNOSTICS {
                diagnostics.relaxations_accepted += 1;
                diagnostics.frontier_pushes += 1;
                diagnostics.peak_frontier_len = diagnostics.peak_frontier_len.max(frontier.len());
            }
        }
    }

    let result = if let Some(goal_cost) = best_costs[goal_index] {
        crate::search::found(
            reconstruct_jump_path(grid, &parents, start_index, goal_index, goal_cost),
            visited_nodes,
        )
    } else {
        crate::search::not_found(visited_nodes)
    };

    SearchExecution {
        result,
        diagnostics,
    }
}

#[derive(Debug, Clone, Copy, PartialEq, Eq)]
struct ScanStop {
    target_index: usize,
    edge_cost: usize,
    edge_weight: usize,
    reason: StopReason,
}

#[derive(Debug, Clone, Copy, PartialEq, Eq)]
enum StopReason {
    Goal,
    Branch,
    ElbowTurn,
    DeadEnd,
}

#[derive(Debug, Clone, Copy, PartialEq, Eq)]
struct FrontierEntry {
    estimated_total_cost: usize,
    heuristic_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.heuristic_cost.cmp(&self.heuristic_cost))
            .then_with(|| self.cost_so_far.cmp(&other.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 jump_in_direction(
    grid: &Grid,
    start: Point,
    start_index: usize,
    direction: Direction,
    goal_index: usize,
) -> Option<ScanStop> {
    let mut point = start;
    let mut index = start_index;
    let mut edge_cost = 0usize;
    let mut edge_weight = 0usize;

    loop {
        let (next_point, next_index) = step(grid, point, index, direction)?;
        point = next_point;
        index = next_index;
        edge_cost = edge_cost.checked_add(1)?;
        edge_weight = edge_weight.checked_add(grid.traversal_cost(point).unwrap_or(1))?;

        if index == goal_index {
            return Some(ScanStop {
                target_index: index,
                edge_cost,
                edge_weight,
                reason: StopReason::Goal,
            });
        }

        let point_kind = classify_point_kind(grid, point);
        if point_kind != PointKind::StraightCorridor {
            return Some(ScanStop {
                target_index: index,
                edge_cost,
                edge_weight,
                reason: match point_kind {
                    PointKind::StraightCorridor => unreachable!("straight corridor handled above"),
                    PointKind::Branch => StopReason::Branch,
                    PointKind::ElbowTurn => StopReason::ElbowTurn,
                    PointKind::DeadEnd => StopReason::DeadEnd,
                },
            });
        }
    }
}

#[cfg(test)]
mod tests {
    use crate::{
        algorithms::jps_cardinal::{PointKind, classify_point_kind},
        algorithms::jump_point_search::JumpPointSearch,
        grid::{Cell, Grid},
        point::Point,
        search::{Pathfinder, SearchRequest},
    };

    #[test]
    fn finds_a_shortest_path_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 jps = JumpPointSearch;
        let result = jps.search(
            &grid,
            SearchRequest::new(Point::new(0, 0), Point::new(4, 4)),
        );

        assert!(result.as_ref().expect("valid search request").is_found());
        assert_eq!(
            result.as_ref().expect("valid search request").cost(),
            Some(8)
        );
        let path = result
            .as_ref()
            .expect("valid search request")
            .path()
            .expect("path should exist");
        assert!(path.steps().contains(&Point::new(2, 2)));
    }

    #[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 jps = JumpPointSearch;
        let result = jps.search(
            &grid,
            SearchRequest::new(Point::new(0, 0), Point::new(2, 2)),
        );

        assert!(!result.as_ref().expect("valid search request").is_found());
        assert_eq!(result.as_ref().expect("valid search request").cost(), None);
        assert!(
            result
                .as_ref()
                .expect("valid search request")
                .stats()
                .visited_nodes
                > 0
        );
    }

    #[test]
    fn skips_a_straight_corridor() {
        let grid = Grid::new(8, 1).expect("grid dimensions are valid");
        let jps = JumpPointSearch;
        let result = jps.search(
            &grid,
            SearchRequest::new(Point::new(0, 0), Point::new(7, 0)),
        );

        assert!(result.as_ref().expect("valid search request").is_found());
        assert_eq!(
            result.as_ref().expect("valid search request").cost(),
            Some(7)
        );
        assert_eq!(
            result
                .as_ref()
                .expect("valid search request")
                .stats()
                .visited_nodes,
            2
        );
        let path = result
            .as_ref()
            .expect("valid search request")
            .path()
            .expect("path should exist");
        assert_eq!(path.len(), 8);
    }

    #[test]
    fn charges_per_cell_traversal_cost_matching_dijkstra() {
        let mut grid = Grid::new(8, 1).expect("grid dimensions are valid");
        assert_eq!(grid.set_traversal_cost(Point::new(4, 0), 5), Ok(()));

        let request = SearchRequest::new(Point::new(0, 0), Point::new(7, 0));
        let jps_cost = JumpPointSearch
            .search(&grid, request)
            .as_ref()
            .expect("valid search request")
            .cost();
        let dijkstra_cost = crate::Dijkstra
            .search(&grid, request)
            .as_ref()
            .expect("valid search request")
            .cost();

        assert_eq!(jps_cost, Some(11));
        assert_eq!(jps_cost, dijkstra_cost);
    }

    #[test]
    fn supports_maximum_single_edge_cost() {
        let mut grid = Grid::new(2, 1).expect("grid dimensions are valid");
        assert_eq!(
            grid.set_traversal_cost(Point::new(1, 0), usize::MAX),
            Ok(())
        );

        let result = JumpPointSearch.search(
            &grid,
            SearchRequest::new(Point::new(0, 0), Point::new(1, 0)),
        );

        assert!(result.as_ref().expect("valid search request").is_found());
        assert_eq!(
            result.as_ref().expect("valid search request").cost(),
            Some(usize::MAX)
        );
        assert_eq!(
            result
                .as_ref()
                .expect("valid search request")
                .path()
                .expect("path should exist")
                .cost(),
            usize::MAX
        );
    }

    #[test]
    fn skips_overflowing_jump_costs_matching_dijkstra() {
        let mut grid = Grid::new(3, 1).expect("grid dimensions are valid");
        assert_eq!(
            grid.set_traversal_cost(Point::new(1, 0), usize::MAX),
            Ok(())
        );

        let request = SearchRequest::new(Point::new(0, 0), Point::new(2, 0));
        let jps = JumpPointSearch.search(&grid, request);
        let dijkstra = crate::Dijkstra.search(&grid, request);

        assert!(!jps.as_ref().expect("valid search request").is_found());
        assert_eq!(jps.as_ref().expect("valid search request").cost(), None);
        assert_eq!(
            jps.as_ref().expect("valid search request").cost(),
            dijkstra.as_ref().expect("valid search request").cost()
        );
    }

    #[test]
    fn stops_at_goal_inside_a_straight_corridor() {
        let grid = Grid::new(8, 1).expect("grid dimensions are valid");
        let jps = JumpPointSearch;
        let result = jps.search(
            &grid,
            SearchRequest::new(Point::new(0, 0), Point::new(4, 0)),
        );

        assert!(result.as_ref().expect("valid search request").is_found());
        assert_eq!(
            result.as_ref().expect("valid search request").cost(),
            Some(4)
        );
        assert_eq!(
            result
                .as_ref()
                .expect("valid search request")
                .stats()
                .visited_nodes,
            2
        );
        let path = result
            .as_ref()
            .expect("valid search request")
            .path()
            .expect("path should exist");
        assert_eq!(path.len(), 5);
        assert_eq!(path.goal(), Point::new(4, 0));
    }

    #[test]
    fn finds_a_shortest_path_inside_an_open_room() {
        let grid = Grid::new(5, 5).expect("grid dimensions are valid");
        let jps = JumpPointSearch;
        let result = jps.search(
            &grid,
            SearchRequest::new(Point::new(0, 0), Point::new(2, 2)),
        );

        assert!(result.as_ref().expect("valid search request").is_found());
        assert_eq!(
            result.as_ref().expect("valid search request").cost(),
            Some(4)
        );
        let path = result
            .as_ref()
            .expect("valid search request")
            .path()
            .expect("path should exist");
        assert_eq!(path.start(), Point::new(0, 0));
        assert_eq!(path.goal(), Point::new(2, 2));
    }

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

        let jps = JumpPointSearch;
        let result = jps.search(
            &grid,
            SearchRequest::new(Point::new(0, 1), Point::new(2, 0)),
        );

        assert!(result.as_ref().expect("valid search request").is_found());
        assert_eq!(
            result.as_ref().expect("valid search request").cost(),
            Some(3)
        );
        let path = result
            .as_ref()
            .expect("valid search request")
            .path()
            .expect("path should exist");
        assert!(path.steps().contains(&Point::new(2, 1)));
        assert_eq!(path.goal(), Point::new(2, 0));
    }

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

        let jps = JumpPointSearch;
        let result = jps.search(
            &grid,
            SearchRequest::new(Point::new(0, 0), Point::new(4, 4)),
        );

        assert!(result.as_ref().expect("valid search request").is_found());
        assert_eq!(
            result.as_ref().expect("valid search request").cost(),
            Some(8)
        );
        let path = result
            .as_ref()
            .expect("valid search request")
            .path()
            .expect("path should exist");
        assert_eq!(path.start(), Point::new(0, 0));
        assert_eq!(path.goal(), Point::new(4, 4));
        assert!(path.steps().contains(&Point::new(0, 4)));
    }

    #[test]
    fn inspection_tracks_branch_and_jump_length_metrics() {
        let grid = Grid::new(8, 1).expect("grid dimensions are valid");

        let inspection = JumpPointSearch.inspect(
            &grid,
            SearchRequest::new(Point::new(0, 0), Point::new(7, 0)),
        );

        assert!(
            inspection
                .result
                .as_ref()
                .expect("valid search request")
                .is_found()
        );
        assert_eq!(
            inspection
                .result
                .as_ref()
                .expect("valid search request")
                .cost(),
            Some(7)
        );
        assert_eq!(inspection.diagnostics.frontier_pushes, 2);
        assert_eq!(inspection.diagnostics.frontier_pops, 2);
        assert_eq!(inspection.diagnostics.successful_scans, 1);
        assert_eq!(inspection.diagnostics.goal_stops, 1);
        assert_eq!(inspection.diagnostics.jump_length_5_plus_stops, 1);
        assert_eq!(inspection.diagnostics.max_jump_length, 7);
    }

    #[test]
    fn classifies_open_room_cells_as_branches() {
        let grid = Grid::new(3, 3).expect("grid dimensions are valid");

        assert_eq!(
            classify_point_kind(&grid, Point::new(1, 1)),
            PointKind::Branch
        );
    }
}