condor-pathfinding-grid 0.4.0

//! Static-grid [`Pathfinder`]: weighted 4-connected Dijkstra.
//!
//! Each search is independent and returns the standard invalid/found/no-path outcome.
//! Cost sums per-cell [`traversal_cost`](crate::Grid::traversal_cost) with no heuristic,
//! making this the weighted correctness baseline. Prefer [`super::astar::AStar`] for
//! the normal one-shot weighted entrypoint.
//!
//! # Examples
//!
//! ```
//! use condor_grid::{Dijkstra, Grid, Pathfinder, Point, SearchRequest};
//!
//! let grid = Grid::new(5, 1).expect("grid dimensions are valid");
//! let request = SearchRequest::new(Point::new(0, 0), Point::new(4, 0));
//! let result = Dijkstra.search(&grid, request).expect("endpoints are walkable");
//! assert!(result.is_found());
//! assert_eq!(result.cost(), Some(4));
//! ```
use std::{cmp::Ordering, collections::BinaryHeap};

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

/// Weighted-grid [`Pathfinder`]: uniform-priority Dijkstra.
///
/// Cost model: sums per-cell `traversal_cost` on 4-connected edges; no heuristic.
/// Prefer as the exact weighted baseline against A*/JPS; A* is usually faster with
/// admissible heuristics on the same model.
#[derive(Debug, Default, Clone, Copy)]
pub struct Dijkstra;

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

    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 mut frontier = BinaryHeap::from([FrontierEntry {
            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 = BudgetWatch::start(request.budget);

        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;
            if entry.index == goal_index {
                break;
            }

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

            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_cost| next_cost >= best_cost) {
                    continue;
                }

                best_costs[neighbor_index] = Some(next_cost);
                parents[neighbor_index] = Some(entry.index);
                frontier.push(FrontierEntry {
                    cost_so_far: next_cost,
                    index: neighbor_index,
                });
            }
        }

        let Some(goal_cost) = best_costs[goal_index] else {
            return crate::search::not_found(visited_nodes);
        };

        crate::search::found(
            reconstruct_path(grid, &parents, start_index, goal_index, goal_cost),
            visited_nodes,
        )
    }
}

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

impl Ord for FrontierEntry {
    fn cmp(&self, other: &Self) -> Ordering {
        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")
}

#[cfg(test)]
mod tests {
    use crate::{
        algorithms::dijkstra::Dijkstra,
        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 dijkstra = Dijkstra;
        let result = dijkstra.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 dijkstra = Dijkstra;
        let result = dijkstra.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 dijkstra = Dijkstra;
        let result = dijkstra.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!(
            path.steps().contains(&Point::new(0, 0)) || path.steps().contains(&Point::new(0, 2))
        );
        assert_eq!(path.cost(), 6);
    }

    #[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 = Dijkstra.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
        );
    }
}