condor_grid/algorithms/

astar.rs

//! Static-grid [`Pathfinder`]: weighted 4-connected A*.
//!
//! Each search is independent and uses the standard invalid/found/no-path contract.
//! Cost sums per-cell [`traversal_cost`](crate::Grid::traversal_cost) with a Manhattan
//! heuristic. Prefer this curated default for one static weighted query; use
//! [`crate::PreparedGridSearch`] for repeated static queries or [`super::dijkstra::Dijkstra`]
//! as the no-heuristic weighted baseline.
//!
//! # Examples
//!
//! ```
//! use condor_grid::{AStar, Cell, Grid, Pathfinder, Point, SearchRequest};
//!
//! let mut grid = Grid::new(3, 3).expect("grid dimensions are valid");
//! grid.set_cell(Point::new(1, 1), Cell::Blocked)
//!     .expect("point is in bounds");
//! let request = SearchRequest::new(Point::new(0, 0), Point::new(2, 2));
//! let result = AStar.search(&grid, request).expect("request is valid");
//! assert!(result.is_found());
//! ```
use std::{
    cmp::Ordering,
    collections::{BTreeMap, BinaryHeap},
};

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

/// Weighted static-grid [`Pathfinder`]: A* with Manhattan heuristic.
///
/// Cost model: sums per-cell `traversal_cost` on 4-connected edges. Prefer as the
/// default online weighted baseline; use BFS family for pure hop count, JPS/JPS+ for
/// open-room cardinal pruning, Dijkstra when no heuristic is wanted.
#[derive(Debug, Default, Clone, Copy)]
pub struct AStar;

/// Open-set and relaxation counters from an instrumented [`AStar::inspect`] run.
///
/// Bakeoff/inspect only; raw event tallies, not normalized rates. Not part of
/// public [`crate::SearchStats`].
#[derive(Debug, Default, Clone, Copy, PartialEq, Eq)]
pub struct AStarDiagnostics {
    pub frontier_pushes: usize,
    pub frontier_pops: usize,
    pub stale_pops_skipped: usize,
    pub stale_pops_before_goal_discovery: usize,
    pub stale_pops_after_goal_discovery: usize,
    pub stale_pops_after_goal_discovery_below_goal_cost: usize,
    pub stale_pops_after_goal_discovery_at_goal_cost: usize,
    pub stale_pops_after_goal_discovery_above_goal_cost: usize,
    pub peak_frontier_len: usize,
    pub relaxation_attempts: usize,
    pub relaxation_attempts_after_goal_discovery: usize,
    pub relaxations_accepted: usize,
    pub relaxations_accepted_after_goal_discovery: usize,
    pub relaxations_accepted_after_goal_discovery_first_touch: usize,
    pub relaxations_accepted_after_goal_discovery_improved: usize,
    pub relaxations_accepted_after_goal_discovery_first_touch_slack_zero: usize,
    pub relaxations_accepted_after_goal_discovery_first_touch_slack_1_to_4: usize,
    pub relaxations_accepted_after_goal_discovery_first_touch_slack_5_to_16: usize,
    pub relaxations_accepted_after_goal_discovery_first_touch_slack_17_plus: usize,
    pub relaxations_accepted_after_goal_discovery_first_touch_slack_sum: usize,
    pub relaxations_accepted_after_goal_discovery_first_touch_max_slack: usize,
    pub distinct_estimated_total_costs_popped: usize,
    pub max_equal_f_pop_run: usize,
    pub goal_cost_plateau_pops: usize,
    pub goal_first_discovery_visited_nodes: Option<usize>,
    pub goal_first_discovery_path_cost: Option<usize>,
    pub goal_first_discovery_frontier_len: Option<usize>,
    pub goal_first_discovery_frontier_below_goal_cost: Option<usize>,
    pub goal_first_discovery_frontier_at_goal_cost: Option<usize>,
    pub goal_first_discovery_frontier_above_goal_cost: Option<usize>,
    pub visited_nodes_before_goal_discovery: usize,
    pub visited_nodes_after_goal_discovery: usize,
    pub visited_nodes_after_goal_discovery_below_goal_cost: usize,
    pub visited_nodes_after_goal_discovery_at_goal_cost: usize,
    pub visited_nodes_after_goal_discovery_above_goal_cost: usize,
    pub heuristic_slack_zero_pops: usize,
    pub heuristic_slack_1_to_4_pops: usize,
    pub heuristic_slack_5_to_16_pops: usize,
    pub heuristic_slack_17_plus_pops: usize,
    pub heuristic_slack_sum: usize,
    pub heuristic_slack_sum_before_goal_discovery: usize,
    pub heuristic_slack_sum_after_goal_discovery: usize,
    pub max_heuristic_slack: usize,
}

/// A* search result paired with [`AStarDiagnostics`] from one instrumented run.
#[derive(Debug, Clone, PartialEq, Eq)]
pub struct AStarInspection {
    /// Standard found/no-path or invalid/budget result.
    pub result: SearchResult,
    /// Open-set and relaxation tallies for this inspect run.
    pub diagnostics: AStarDiagnostics,
}

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

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

    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: AStarDiagnostics,
}

fn search_impl<const TRACK_DIAGNOSTICS: bool>(
    grid: &Grid,
    request: SearchRequest,
) -> SearchExecution {
    let mut diagnostics = AStarDiagnostics::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 {
        heuristic_cost: initial_heuristic,
        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 = 0;
    let mut last_popped_estimated_total_cost = None;
    let mut current_equal_f_pop_run = 0usize;
    let mut popped_by_estimated_total_cost = TRACK_DIAGNOSTICS.then(BTreeMap::new);
    let goal_distance_map =
        TRACK_DIAGNOSTICS.then(|| reverse_cost_map_from_goal(grid, request.goal));
    let watch = 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;
                if let Some(goal_cost) = diagnostics.goal_first_discovery_path_cost {
                    diagnostics.stale_pops_after_goal_discovery += 1;
                    increment_goal_cost_band(
                        entry.estimated_total_cost,
                        goal_cost,
                        &mut diagnostics.stale_pops_after_goal_discovery_below_goal_cost,
                        &mut diagnostics.stale_pops_after_goal_discovery_at_goal_cost,
                        &mut diagnostics.stale_pops_after_goal_discovery_above_goal_cost,
                    );
                } else {
                    diagnostics.stale_pops_before_goal_discovery += 1;
                }
            }
            continue;
        }

        let before_goal_discovery =
            TRACK_DIAGNOSTICS && diagnostics.goal_first_discovery_visited_nodes.is_none();

        if TRACK_DIAGNOSTICS {
            if last_popped_estimated_total_cost == Some(entry.estimated_total_cost) {
                current_equal_f_pop_run += 1;
            } else {
                last_popped_estimated_total_cost = Some(entry.estimated_total_cost);
                current_equal_f_pop_run = 1;
                diagnostics.distinct_estimated_total_costs_popped += 1;
            }

            diagnostics.max_equal_f_pop_run =
                diagnostics.max_equal_f_pop_run.max(current_equal_f_pop_run);

            if let Some(histogram) = &mut popped_by_estimated_total_cost {
                *histogram.entry(entry.estimated_total_cost).or_default() += 1;
            }
        }

        let current = grid.point_from_index(entry.index);
        if let Some(goal_distances) = &goal_distance_map
            && let Some(true_remaining_cost) = goal_distances[entry.index]
        {
            let heuristic_slack =
                true_remaining_cost.saturating_sub(manhattan_distance(current, request.goal));
            diagnostics.heuristic_slack_sum += heuristic_slack;
            if before_goal_discovery {
                diagnostics.heuristic_slack_sum_before_goal_discovery += heuristic_slack;
            } else {
                diagnostics.heuristic_slack_sum_after_goal_discovery += heuristic_slack;
            }
            diagnostics.max_heuristic_slack = diagnostics.max_heuristic_slack.max(heuristic_slack);

            match heuristic_slack {
                0 => diagnostics.heuristic_slack_zero_pops += 1,
                1..=4 => diagnostics.heuristic_slack_1_to_4_pops += 1,
                5..=16 => diagnostics.heuristic_slack_5_to_16_pops += 1,
                _ => diagnostics.heuristic_slack_17_plus_pops += 1,
            }
        }

        visited_nodes += 1;
        if TRACK_DIAGNOSTICS {
            if before_goal_discovery {
                diagnostics.visited_nodes_before_goal_discovery += 1;
            } else {
                diagnostics.visited_nodes_after_goal_discovery += 1;
                let goal_cost = diagnostics
                    .goal_first_discovery_path_cost
                    .expect("post-goal counters require a discovered goal cost");
                increment_goal_cost_band(
                    entry.estimated_total_cost,
                    goal_cost,
                    &mut diagnostics.visited_nodes_after_goal_discovery_below_goal_cost,
                    &mut diagnostics.visited_nodes_after_goal_discovery_at_goal_cost,
                    &mut diagnostics.visited_nodes_after_goal_discovery_above_goal_cost,
                );
            }
        }

        if entry.index == goal_index {
            break;
        }

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

        for neighbor in ordered_neighbors4(grid, current, request.goal)
            .into_iter()
            .flatten()
        {
            let after_goal_discovery =
                TRACK_DIAGNOSTICS && diagnostics.goal_first_discovery_path_cost.is_some();

            if TRACK_DIAGNOSTICS {
                diagnostics.relaxation_attempts += 1;
                if after_goal_discovery {
                    diagnostics.relaxation_attempts_after_goal_discovery += 1;
                }
            }

            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;
            };
            let was_unseen = best_costs[neighbor_index].is_none();

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

            let heuristic_cost = manhattan_distance(neighbor, request.goal);
            let estimated_total_cost = next_cost.saturating_add(heuristic_cost);

            best_costs[neighbor_index] = Some(next_cost);
            parents[neighbor_index] = Some(entry.index);
            let first_goal_discovery = TRACK_DIAGNOSTICS
                && neighbor_index == goal_index
                && diagnostics.goal_first_discovery_visited_nodes.is_none();
            frontier.push(FrontierEntry {
                heuristic_cost,
                estimated_total_cost,
                cost_so_far: next_cost,
                index: neighbor_index,
            });
            if first_goal_discovery {
                diagnostics.goal_first_discovery_visited_nodes = Some(visited_nodes);
                diagnostics.goal_first_discovery_path_cost = Some(next_cost);
                diagnostics.goal_first_discovery_frontier_len = Some(frontier.len());
                let (below, at, above) = frontier_goal_cost_band_counts(&frontier, next_cost);
                diagnostics.goal_first_discovery_frontier_below_goal_cost = Some(below);
                diagnostics.goal_first_discovery_frontier_at_goal_cost = Some(at);
                diagnostics.goal_first_discovery_frontier_above_goal_cost = Some(above);
            }

            if TRACK_DIAGNOSTICS {
                diagnostics.relaxations_accepted += 1;
                if after_goal_discovery {
                    diagnostics.relaxations_accepted_after_goal_discovery += 1;
                    if was_unseen {
                        diagnostics.relaxations_accepted_after_goal_discovery_first_touch += 1;
                        if let Some(goal_distances) = &goal_distance_map
                            && let Some(true_remaining_cost) = goal_distances[neighbor_index]
                        {
                            let slack = true_remaining_cost
                                .saturating_sub(manhattan_distance(neighbor, request.goal));
                            diagnostics
                                .relaxations_accepted_after_goal_discovery_first_touch_slack_sum +=
                                slack;
                            diagnostics
                                .relaxations_accepted_after_goal_discovery_first_touch_max_slack =
                                diagnostics
                                    .relaxations_accepted_after_goal_discovery_first_touch_max_slack
                                    .max(slack);
                            match slack {
                                0 => {
                                    diagnostics
                                        .relaxations_accepted_after_goal_discovery_first_touch_slack_zero +=
                                        1
                                }
                                1..=4 => {
                                    diagnostics
                                        .relaxations_accepted_after_goal_discovery_first_touch_slack_1_to_4 +=
                                        1
                                }
                                5..=16 => {
                                    diagnostics
                                        .relaxations_accepted_after_goal_discovery_first_touch_slack_5_to_16 +=
                                        1
                                }
                                _ => {
                                    diagnostics
                                        .relaxations_accepted_after_goal_discovery_first_touch_slack_17_plus +=
                                        1
                                }
                            }
                        }
                    } else {
                        diagnostics.relaxations_accepted_after_goal_discovery_improved += 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] {
        if let Some(histogram) = popped_by_estimated_total_cost {
            diagnostics.goal_cost_plateau_pops = histogram.get(&goal_cost).copied().unwrap_or(0);
        }

        crate::search::found(
            reconstruct_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 FrontierEntry {
    heuristic_cost: usize,
    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.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 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 current_index != start_index {
        current_index =
            parents[current_index].expect("a discovered goal must have a complete parent chain");
        steps.push(grid.point_from_index(current_index));
    }

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

fn reverse_cost_map_from_goal(grid: &Grid, goal: Point) -> Vec<Option<usize>> {
    let mut costs = vec![None; grid.cell_count()];
    let Some(goal_index) = grid.index_of(goal) else {
        return costs;
    };
    if !grid.is_walkable(goal) {
        return costs;
    }

    let mut frontier = BinaryHeap::from([FrontierEntry {
        heuristic_cost: 0,
        estimated_total_cost: 0,
        cost_so_far: 0,
        index: goal_index,
    }]);
    costs[goal_index] = Some(0);

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

        let index = entry.index;
        let point = grid.point_from_index(index);
        let cost_from_point = costs[index].expect("queued nodes must have a cost");
        let reverse_edge_cost = grid
            .traversal_cost(point)
            .expect("walkable reverse frontier nodes must have a traversal cost");

        for neighbor in grid.neighbors4(point) {
            let neighbor_index = grid
                .index_of(neighbor)
                .expect("walkable neighbors must exist inside the grid");
            let Some(next_cost) = cost_from_point.checked_add(reverse_edge_cost) else {
                continue;
            };
            if costs[neighbor_index].is_some_and(|best| next_cost >= best) {
                continue;
            }

            costs[neighbor_index] = Some(next_cost);
            frontier.push(FrontierEntry {
                heuristic_cost: 0,
                estimated_total_cost: next_cost,
                cost_so_far: next_cost,
                index: neighbor_index,
            });
        }
    }

    costs
}

fn frontier_goal_cost_band_counts(
    frontier: &BinaryHeap<FrontierEntry>,
    goal_cost: usize,
) -> (usize, usize, usize) {
    let mut below = 0usize;
    let mut at = 0usize;
    let mut above = 0usize;

    for entry in frontier {
        increment_goal_cost_band(
            entry.estimated_total_cost,
            goal_cost,
            &mut below,
            &mut at,
            &mut above,
        );
    }

    (below, at, above)
}

fn increment_goal_cost_band(
    estimated_total_cost: usize,
    goal_cost: usize,
    below: &mut usize,
    at: &mut usize,
    above: &mut usize,
) {
    match estimated_total_cost.cmp(&goal_cost) {
        Ordering::Less => *below += 1,
        Ordering::Equal => *at += 1,
        Ordering::Greater => *above += 1,
    }
}

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

fn ordered_neighbors4(grid: &Grid, current: Point, goal: Point) -> [Option<Point>; 4] {
    let mut ordered = [None; 4];
    let mut count = 0;

    let mut push_direction = |direction| {
        if let Some(candidate) = step(current, direction)
            && grid.is_walkable(candidate)
            && !ordered[..count]
                .iter()
                .flatten()
                .any(|point| *point == candidate)
        {
            ordered[count] = Some(candidate);
            count += 1;
        }
    };

    match current.x.cmp(&goal.x) {
        Ordering::Less => push_direction(Direction::Right),
        Ordering::Greater => push_direction(Direction::Left),
        Ordering::Equal => {}
    }

    match current.y.cmp(&goal.y) {
        Ordering::Less => push_direction(Direction::Down),
        Ordering::Greater => push_direction(Direction::Up),
        Ordering::Equal => {}
    }

    push_direction(Direction::Right);
    push_direction(Direction::Left);
    push_direction(Direction::Down);
    push_direction(Direction::Up);

    ordered
}

fn step(point: Point, direction: Direction) -> Option<Point> {
    match direction {
        Direction::Left if point.x > 0 => Some(Point::new(point.x - 1, point.y)),
        Direction::Right => Some(Point::new(point.x + 1, point.y)),
        Direction::Up if point.y > 0 => Some(Point::new(point.x, point.y - 1)),
        Direction::Down => Some(Point::new(point.x, point.y + 1)),
        _ => None,
    }
}

#[derive(Debug, Clone, Copy, PartialEq, Eq)]
enum Direction {
    Left,
    Right,
    Up,
    Down,
}

#[cfg(test)]
mod tests {
    use crate::{
        algorithms::astar::AStar,
        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 astar = AStar;
        let result = astar.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(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 astar = AStar;
        let result = astar.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 prefers_a_cheaper_weighted_detour() {
        let mut grid = Grid::new(5, 3).expect("grid dimensions are valid");
        assert_eq!(grid.set_traversal_cost(Point::new(1, 1), 5), Ok(()));
        assert_eq!(grid.set_traversal_cost(Point::new(2, 1), 5), Ok(()));
        assert_eq!(grid.set_traversal_cost(Point::new(3, 1), 5), Ok(()));

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

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

    #[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 = AStar.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 inspection_tracks_frontier_churn_metrics() {
        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 inspection = AStar.inspect(
            &grid,
            SearchRequest::new(Point::new(0, 0), Point::new(4, 4)),
        );

        assert!(
            inspection
                .result
                .as_ref()
                .expect("valid search request")
                .is_found()
        );
        assert_eq!(
            inspection
                .result
                .as_ref()
                .expect("valid search request")
                .cost(),
            Some(8)
        );
        assert!(inspection.diagnostics.frontier_pushes > 0);
        assert!(inspection.diagnostics.frontier_pops > 0);
        assert!(inspection.diagnostics.peak_frontier_len > 0);
        assert!(inspection.diagnostics.distinct_estimated_total_costs_popped > 0);
        assert!(inspection.diagnostics.max_equal_f_pop_run > 0);
        assert!(inspection.diagnostics.goal_cost_plateau_pops > 0);
        assert_eq!(
            inspection.diagnostics.heuristic_slack_zero_pops
                + inspection.diagnostics.heuristic_slack_1_to_4_pops
                + inspection.diagnostics.heuristic_slack_5_to_16_pops
                + inspection.diagnostics.heuristic_slack_17_plus_pops,
            inspection
                .result
                .as_ref()
                .expect("valid search request")
                .stats()
                .visited_nodes
        );
        assert!(
            inspection.diagnostics.frontier_pushes > inspection.diagnostics.relaxations_accepted
        );
        assert!(
            inspection.diagnostics.relaxation_attempts
                >= inspection
                    .diagnostics
                    .relaxation_attempts_after_goal_discovery
        );
        assert!(
            inspection.diagnostics.relaxations_accepted
                >= inspection
                    .diagnostics
                    .relaxations_accepted_after_goal_discovery
        );
        assert_eq!(
            inspection
                .diagnostics
                .relaxations_accepted_after_goal_discovery_first_touch
                + inspection
                    .diagnostics
                    .relaxations_accepted_after_goal_discovery_improved,
            inspection
                .diagnostics
                .relaxations_accepted_after_goal_discovery
        );
        assert_eq!(
            inspection
                .diagnostics
                .relaxations_accepted_after_goal_discovery_first_touch_slack_zero
                + inspection
                    .diagnostics
                    .relaxations_accepted_after_goal_discovery_first_touch_slack_1_to_4
                + inspection
                    .diagnostics
                    .relaxations_accepted_after_goal_discovery_first_touch_slack_5_to_16
                + inspection
                    .diagnostics
                    .relaxations_accepted_after_goal_discovery_first_touch_slack_17_plus,
            inspection
                .diagnostics
                .relaxations_accepted_after_goal_discovery_first_touch
        );
        assert!(
            inspection.diagnostics.frontier_pops
                >= inspection
                    .result
                    .as_ref()
                    .expect("valid search request")
                    .stats()
                    .visited_nodes
                    + inspection.diagnostics.stale_pops_skipped
        );
        assert_eq!(
            inspection.diagnostics.stale_pops_before_goal_discovery
                + inspection.diagnostics.stale_pops_after_goal_discovery,
            inspection.diagnostics.stale_pops_skipped
        );
        assert_eq!(
            inspection.diagnostics.visited_nodes_before_goal_discovery
                + inspection.diagnostics.visited_nodes_after_goal_discovery,
            inspection
                .result
                .as_ref()
                .expect("valid search request")
                .stats()
                .visited_nodes
        );
        assert_eq!(
            inspection
                .diagnostics
                .visited_nodes_after_goal_discovery_below_goal_cost
                + inspection
                    .diagnostics
                    .visited_nodes_after_goal_discovery_at_goal_cost
                + inspection
                    .diagnostics
                    .visited_nodes_after_goal_discovery_above_goal_cost,
            inspection.diagnostics.visited_nodes_after_goal_discovery
        );
        assert_eq!(
            inspection
                .diagnostics
                .stale_pops_after_goal_discovery_below_goal_cost
                + inspection
                    .diagnostics
                    .stale_pops_after_goal_discovery_at_goal_cost
                + inspection
                    .diagnostics
                    .stale_pops_after_goal_discovery_above_goal_cost,
            inspection.diagnostics.stale_pops_after_goal_discovery
        );
    }

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

        let inspection = AStar.inspect(
            &grid,
            SearchRequest::new(Point::new(0, 1), Point::new(4, 1)),
        );

        assert!(
            inspection
                .result
                .as_ref()
                .expect("valid search request")
                .is_found()
        );
        assert_eq!(
            inspection
                .result
                .as_ref()
                .expect("valid search request")
                .cost(),
            Some(6)
        );
        assert_eq!(
            inspection
                .result
                .as_ref()
                .expect("valid search request")
                .path()
                .expect("path should exist")
                .cost(),
            6
        );
        assert!(inspection.diagnostics.max_heuristic_slack > 0);
        assert_eq!(
            inspection.diagnostics.goal_first_discovery_path_cost,
            Some(6)
        );
        assert!(
            inspection
                .diagnostics
                .goal_first_discovery_visited_nodes
                .is_some()
        );
        assert!(
            inspection
                .diagnostics
                .goal_first_discovery_frontier_len
                .is_some()
        );
        assert_eq!(
            inspection
                .diagnostics
                .goal_first_discovery_frontier_below_goal_cost
                .zip(
                    inspection
                        .diagnostics
                        .goal_first_discovery_frontier_at_goal_cost
                )
                .zip(
                    inspection
                        .diagnostics
                        .goal_first_discovery_frontier_above_goal_cost
                )
                .map(|((below, at), above)| below + at + above),
            inspection.diagnostics.goal_first_discovery_frontier_len
        );
        assert_eq!(
            inspection.diagnostics.visited_nodes_before_goal_discovery
                + inspection.diagnostics.visited_nodes_after_goal_discovery,
            inspection
                .result
                .as_ref()
                .expect("valid search request")
                .stats()
                .visited_nodes
        );
        assert!(inspection.diagnostics.visited_nodes_after_goal_discovery > 0);
        assert!(
            inspection
                .diagnostics
                .relaxation_attempts_after_goal_discovery
                >= inspection
                    .diagnostics
                    .relaxations_accepted_after_goal_discovery
        );
        assert_eq!(
            inspection
                .diagnostics
                .relaxations_accepted_after_goal_discovery_first_touch
                + inspection
                    .diagnostics
                    .relaxations_accepted_after_goal_discovery_improved,
            inspection
                .diagnostics
                .relaxations_accepted_after_goal_discovery
        );
        assert_eq!(
            inspection
                .diagnostics
                .relaxations_accepted_after_goal_discovery_first_touch_slack_zero
                + inspection
                    .diagnostics
                    .relaxations_accepted_after_goal_discovery_first_touch_slack_1_to_4
                + inspection
                    .diagnostics
                    .relaxations_accepted_after_goal_discovery_first_touch_slack_5_to_16
                + inspection
                    .diagnostics
                    .relaxations_accepted_after_goal_discovery_first_touch_slack_17_plus,
            inspection
                .diagnostics
                .relaxations_accepted_after_goal_discovery_first_touch
        );
        assert_eq!(
            inspection
                .diagnostics
                .visited_nodes_after_goal_discovery_below_goal_cost
                + inspection
                    .diagnostics
                    .visited_nodes_after_goal_discovery_at_goal_cost
                + inspection
                    .diagnostics
                    .visited_nodes_after_goal_discovery_above_goal_cost,
            inspection.diagnostics.visited_nodes_after_goal_discovery
        );
        assert_eq!(
            inspection
                .diagnostics
                .stale_pops_after_goal_discovery_below_goal_cost
                + inspection
                    .diagnostics
                    .stale_pops_after_goal_discovery_at_goal_cost
                + inspection
                    .diagnostics
                    .stale_pops_after_goal_discovery_above_goal_cost,
            inspection.diagnostics.stale_pops_after_goal_discovery
        );
        assert!(
            inspection
                .diagnostics
                .heuristic_slack_sum_before_goal_discovery
                + inspection
                    .diagnostics
                    .heuristic_slack_sum_after_goal_discovery
                == inspection.diagnostics.heuristic_slack_sum
        );
    }
}