condor-pathfinding-grid 0.4.0

//! Private candidate: index-native Dijkstra hotpath for weighted grids.
//!
//! **Hypothesis:** index-native neighbor traversal and a reusable workspace can
//! reduce exact weighted-search overhead without assuming a single frontier
//! policy fits every workload.
//!
//! **Non-negotiable behavior:** preserve destination-cell traversal costs,
//! checked accumulation, and exact path-cost parity. Reuse must not leak state
//! between searches or silently change no-path and invalid-input behavior.
//!
//! **Evidence and promotion:** ordinary `grid_core` while developing; exact
//! parity through grid conformance. Remains private: no feature, fixture,
//! target, or separate harness route.

#![allow(
    dead_code,
    reason = "private candidate retained for normal-family evaluation"
)]

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

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

/// Online [`Pathfinder`] candidate: index-native Dijkstra with reusable workspace.
///
/// Cost model matches [`super::dijkstra::Dijkstra`]: sum of entered-cell
/// `traversal_cost` on 4-connected edges. Workspace buffers are reset on every
/// search so sequential requests cannot leak parents, costs, or no-path state.
#[derive(Debug, Default)]
pub struct DijkstraIndexHotpath {
    workspace: RefCell<Workspace>,
}

impl DijkstraIndexHotpath {
    /// Stable source-local candidate identity.
    pub const CANDIDATE_ID: &str = "weighted-grid/index-hotpath";
}

#[derive(Debug, Default)]
struct Workspace {
    best_costs: Vec<Option<usize>>,
    parents: Vec<Option<usize>>,
    generation: Vec<u32>,
    stamp: u32,
}

impl Workspace {
    fn prepare(&mut self, cell_count: usize) {
        if self.best_costs.len() != cell_count {
            self.best_costs.clear();
            self.best_costs.resize(cell_count, None);
            self.parents.clear();
            self.parents.resize(cell_count, None);
            self.generation.clear();
            self.generation.resize(cell_count, 0);
            self.stamp = 1;
            return;
        }
        // Bump generation stamp so prior search slots become invisible without
        // zeroing full vectors on every call.
        self.stamp = self.stamp.wrapping_add(1);
        if self.stamp == 0 {
            self.generation.fill(0);
            self.best_costs.fill(None);
            self.parents.fill(None);
            self.stamp = 1;
        }
    }

    fn cost(&self, index: usize) -> Option<usize> {
        if self.generation[index] == self.stamp {
            self.best_costs[index]
        } else {
            None
        }
    }

    fn set_cost_parent(&mut self, index: usize, cost: usize, parent: Option<usize>) {
        self.generation[index] = self.stamp;
        self.best_costs[index] = Some(cost);
        self.parents[index] = parent;
    }

    fn parent(&self, index: usize) -> Option<usize> {
        if self.generation[index] == self.stamp {
            self.parents[index]
        } else {
            None
        }
    }
}

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

    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 workspace = self.workspace.borrow_mut();
        workspace.prepare(grid.cell_count());
        workspace.set_cost_parent(start_index, 0, None);

        let mut frontier = BinaryHeap::from([FrontierEntry {
            cost_so_far: 0,
            index: start_index,
        }]);
        let mut visited_nodes = 0usize;
        let watch = BudgetWatch::start(request.budget);

        while let Some(entry) = frontier.pop() {
            if workspace.cost(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 workspace
                    .cost(neighbor_index)
                    .is_some_and(|best| next_cost >= best)
                {
                    continue;
                }

                workspace.set_cost_parent(neighbor_index, next_cost, Some(entry.index));
                frontier.push(FrontierEntry {
                    cost_so_far: next_cost,
                    index: neighbor_index,
                });
            }
        }

        let Some(goal_cost) = workspace.cost(goal_index) else {
            return crate::search::not_found(visited_nodes);
        };

        let path = reconstruct_path(grid, &workspace, start_index, goal_index, goal_cost);
        crate::search::found(path, 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,
    workspace: &Workspace,
    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 = workspace
            .parent(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")
}

#[cfg(test)]
mod tests {
    use crate::{
        algorithms::{dijkstra::Dijkstra, dijkstra_index_hotpath::DijkstraIndexHotpath},
        grid::{Cell, Grid},
        point::Point,
        search::{BudgetExhausted, GridSearchError, Pathfinder, SearchBudget, SearchRequest},
    };

    #[test]
    fn matches_dijkstra_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 hotpath = DijkstraIndexHotpath::default();

        let candidate = hotpath
            .search(&grid, request)
            .expect("endpoints are walkable");
        let baseline = Dijkstra
            .search(&grid, request)
            .expect("endpoints are walkable");

        assert!(candidate.is_found());
        assert_eq!(candidate.cost(), baseline.cost());
    }

    #[test]
    fn matches_dijkstra_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 hotpath = DijkstraIndexHotpath::default();

        let candidate = hotpath
            .search(&grid, request)
            .expect("endpoints are walkable");
        let baseline = Dijkstra
            .search(&grid, request)
            .expect("endpoints are walkable");

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

    #[test]
    fn sequential_searches_do_not_leak_workspace_state() {
        let hotpath = DijkstraIndexHotpath::default();

        // First search: no path (horizontal wall).
        let mut blocked = Grid::new(3, 3).expect("grid dimensions are valid");
        for x in 0..3 {
            blocked
                .set_cell(Point::new(x, 1), Cell::Blocked)
                .expect("valid grid edit");
        }
        let no_path = hotpath
            .search(
                &blocked,
                SearchRequest::new(Point::new(0, 0), Point::new(2, 2)),
            )
            .expect("endpoints are walkable");
        assert!(!no_path.is_found());

        // Second search reuses the same solver instance on a clear corridor.
        let open = Grid::new(5, 1).expect("grid dimensions are valid");
        let found = hotpath
            .search(
                &open,
                SearchRequest::new(Point::new(0, 0), Point::new(4, 0)),
            )
            .expect("endpoints are walkable");
        assert!(found.is_found());
        assert_eq!(found.cost(), Some(4));

        // Third search: different weighted map; must not inherit prior parents.
        let mut weighted = Grid::new(4, 1).expect("grid dimensions are valid");
        weighted
            .set_traversal_cost(Point::new(1, 0), 5)
            .expect("valid cost edit");
        weighted
            .set_traversal_cost(Point::new(2, 0), 5)
            .expect("valid cost edit");
        let weighted_result = hotpath
            .search(
                &weighted,
                SearchRequest::new(Point::new(0, 0), Point::new(3, 0)),
            )
            .expect("endpoints are walkable");
        let baseline = Dijkstra
            .search(
                &weighted,
                SearchRequest::new(Point::new(0, 0), Point::new(3, 0)),
            )
            .expect("endpoints are walkable");
        assert_eq!(weighted_result.cost(), baseline.cost());
    }

    #[test]
    fn same_size_sequential_searches_use_stamp_not_resize() {
        // All grids are 5×5 (cell_count identical) so prepare takes the stamp-bump
        // path rather than full vector reallocation.
        let hotpath = DijkstraIndexHotpath::default();
        let size = 5;

        let open = Grid::new(size, size).expect("grid");
        let first = hotpath
            .search(
                &open,
                SearchRequest::new(Point::new(0, 0), Point::new(4, 4)),
            )
            .expect("walkable");
        assert!(first.is_found());
        let first_cost = first.cost();

        // Block the previous open diagonal path with a mid wall; same size.
        let mut cut = Grid::new(size, size).expect("grid");
        for y in 0..size {
            cut.set_cell(Point::new(2, y), Cell::Blocked)
                .expect("valid");
        }
        let second = hotpath
            .search(&cut, SearchRequest::new(Point::new(0, 0), Point::new(4, 4)))
            .expect("walkable endpoints");
        assert!(
            !second.is_found(),
            "must not inherit open-field parents/costs"
        );

        // Weighted detour on same size — cost must match fresh Dijkstra.
        let mut weighted = Grid::new(size, size).expect("grid");
        weighted
            .set_traversal_cost(Point::new(1, 0), 7)
            .expect("valid");
        weighted
            .set_traversal_cost(Point::new(2, 0), 7)
            .expect("valid");
        let request = SearchRequest::new(Point::new(0, 0), Point::new(4, 0));
        let third = hotpath.search(&weighted, request).expect("walkable");
        let baseline = Dijkstra.search(&weighted, request).expect("walkable");
        assert_eq!(third.cost(), baseline.cost());

        // Re-open: still must not leak the blocked-grid no-path.
        let reopen = hotpath
            .search(
                &open,
                SearchRequest::new(Point::new(0, 0), Point::new(4, 4)),
            )
            .expect("walkable");
        assert!(reopen.is_found());
        assert_eq!(reopen.cost(), first_cost);
    }

    #[test]
    fn retains_candidate_id() {
        assert_eq!(
            DijkstraIndexHotpath::CANDIDATE_ID,
            "weighted-grid/index-hotpath"
        );
    }
}