condor-pathfinding-grid 0.4.0

//! Private candidate: fixed-goal reverse distance field for weighted grids.
//!
//! **Hypothesis:** one reverse exact-distance preprocess from an immutable goal
//! can amortize many start queries on one static weighted grid.
//!
//! **Non-negotiable behavior:** the prepared snapshot must bind grid costs,
//! blocked cells, and goal identity. Reverse labels must use the established
//! destination-cell cost model, and each query must preserve exact path cost,
//! witness, invalid-input, and no-path behavior. Stale geometry or costs require
//! rebuilding; they may not silently reuse an old field.
//!
//! **Evidence and promotion:** ordinary `grid_preprocessing` while developing;
//! promotion needs conformance and preprocess-plus-query break-even evidence.
//! Remains private: no feature, fixture, target, or separate harness route.
//!
//! # API shape
//!
//! Goal is bound on the builder (not on [`crate::PreprocessedGridBuilder`], which
//! only accepts `preprocess(&Grid)`). This deliberately does **not** reuse
//! [`crate::FlowFieldBuilder`] reverse-BFS/unit-hop semantics.

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

use crate::{
    grid::Grid,
    path::Path,
    point::Point,
    preprocessed_grid::{PreprocessedGridBuildError, PreprocessedGridMetadata, metadata_for_grid},
    search::{SearchRequest, SearchResult},
};

/// Builder that binds a fixed goal and builds a reverse destination-cell field.
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub struct FixedGoalReverseDistanceFieldBuilder {
    goal: Point,
}

impl FixedGoalReverseDistanceFieldBuilder {
    /// Stable source-local candidate identity.
    pub const CANDIDATE_ID: &str = "static-weighted-grid/fixed-goal-reverse-distance-field";

    /// Bind `goal` for reverse-field preprocess.
    #[must_use]
    pub const fn new(goal: Point) -> Self {
        Self { goal }
    }

    /// Builder identity token.
    #[must_use]
    pub const fn name(&self) -> &'static str {
        "fixed-goal-reverse-distance-field"
    }

    /// Snapshot `grid` and compute reverse exact distances into `goal`.
    ///
    /// # Errors
    ///
    /// Returns [`PreprocessedGridBuildError`] only if hierarchical prep fails
    /// (not used here). Invalid goals are accepted at build and surface as
    /// no-path / invalid at query time via ordinary search validation.
    pub fn preprocess(
        &self,
        grid: &Grid,
    ) -> Result<PreparedFixedGoalReverseDistanceField, PreprocessedGridBuildError> {
        let snapshot = grid.clone();
        let (reverse_dist, parents) = build_reverse_field(&snapshot, self.goal);
        Ok(PreparedFixedGoalReverseDistanceField {
            grid: snapshot,
            goal: self.goal,
            reverse_dist,
            parents,
            metadata: metadata_for_grid(
                grid,
                "fixed-goal-reverse-distance-field",
                "fixed-goal-reverse-query",
            ),
        })
    }
}

/// Immutable reverse-field snapshot for repeated start queries to one goal.
#[derive(Debug, Clone, PartialEq, Eq)]
pub struct PreparedFixedGoalReverseDistanceField {
    grid: Grid,
    goal: Point,
    /// Distance from each cell to the bound goal under destination-cell costs.
    reverse_dist: Vec<Option<usize>>,
    /// Parent toward the goal for path reconstruction.
    parents: Vec<Option<usize>>,
    metadata: PreprocessedGridMetadata,
}

impl PreparedFixedGoalReverseDistanceField {
    /// Bound goal used to build this field.
    #[must_use]
    pub const fn goal(&self) -> Point {
        self.goal
    }

    /// Builder identity recorded in metadata.
    #[must_use]
    pub fn name(&self) -> &'static str {
        self.metadata.builder_name
    }

    /// Immutable grid snapshot used for walkability and path reconstruction.
    #[must_use]
    pub fn grid(&self) -> &Grid {
        &self.grid
    }

    /// Build-time shape and cost-model summary.
    #[must_use]
    pub fn metadata(&self) -> &PreprocessedGridMetadata {
        &self.metadata
    }

    /// Query from `request.start` to the bound goal.
    ///
    /// `request.goal` must equal the bound goal; otherwise the caller must
    /// rebuild. Geometry edits on the live map do not affect this snapshot —
    /// rebuild from a fresh grid when costs or blocked cells change.
    pub fn search(&self, request: SearchRequest) -> SearchResult {
        crate::search::validate_request(&self.grid, request)?;
        if request.goal != self.goal {
            // Goal mismatch: treat as requiring rebuild rather than inventing
            // a new Err variant — surface as no-path with zero expansions so
            // callers cannot mistake a stale goal for a found path.
            return crate::search::not_found(0);
        }

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

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

        let Some(total_cost) = self.reverse_dist[start_index] else {
            return crate::search::not_found(0);
        };

        crate::search::found(
            reconstruct_toward_goal(
                &self.grid,
                &self.parents,
                start_index,
                goal_index,
                total_cost,
            ),
            1,
        )
    }
}

/// Reverse Dijkstra from `goal`: edge expansion charges destination-cell cost
/// of the forward direction (leave cost of the reverse source).
fn build_reverse_field(grid: &Grid, goal: Point) -> (Vec<Option<usize>>, Vec<Option<usize>>) {
    let cell_count = grid.cell_count();
    let mut reverse_dist = vec![None; cell_count];
    let mut parents = vec![None; cell_count];
    let Some(goal_index) = grid.index_of(goal) else {
        return (reverse_dist, parents);
    };
    if !grid.is_walkable(goal) {
        return (reverse_dist, parents);
    }

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

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

        let current = grid.point_from_index(entry.index);
        let leave_cost = grid
            .traversal_cost(current)
            .expect("walkable reverse source must have a traversal cost");

        for neighbor in grid.neighbors4(current) {
            let neighbor_index = grid
                .index_of(neighbor)
                .expect("walkable neighbors must exist inside the grid");
            // Reverse edge current→neighbor ≡ forward neighbor→current, cost = leave_cost.
            let Some(next_cost) = entry.cost_so_far.checked_add(leave_cost) else {
                continue;
            };
            if reverse_dist[neighbor_index].is_some_and(|best| next_cost >= best) {
                continue;
            }
            reverse_dist[neighbor_index] = Some(next_cost);
            parents[neighbor_index] = Some(entry.index);
            frontier.push(FrontierEntry {
                cost_so_far: next_cost,
                index: neighbor_index,
            });
        }
    }

    (reverse_dist, parents)
}

fn reconstruct_toward_goal(
    grid: &Grid,
    parents: &[Option<usize>],
    start_index: usize,
    goal_index: usize,
    total_cost: usize,
) -> Path {
    let mut current_index = start_index;
    let mut steps = vec![grid.point_from_index(start_index)];

    while current_index != goal_index {
        current_index = parents[current_index]
            .expect("a labeled start must have a complete reverse parent chain to the goal");
        steps.push(grid.point_from_index(current_index));
    }

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

#[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))
    }
}

#[cfg(test)]
mod tests {
    use crate::{
        algorithms::{
            dijkstra::Dijkstra,
            fixed_goal_reverse_distance_field::FixedGoalReverseDistanceFieldBuilder,
        },
        grid::{Cell, Grid},
        point::Point,
        search::{Pathfinder, SearchRequest},
    };

    #[test]
    fn query_cost_matches_dijkstra_from_multiple_starts() {
        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 goal = Point::new(4, 4);
        let prepared = FixedGoalReverseDistanceFieldBuilder::new(goal)
            .preprocess(&grid)
            .expect("preprocess succeeds");

        for start in [Point::new(0, 0), Point::new(0, 4), Point::new(4, 0)] {
            let request = SearchRequest::new(start, goal);
            let candidate = prepared.search(request).expect("endpoints walkable");
            let baseline = Dijkstra.search(&grid, request).expect("endpoints walkable");
            assert!(candidate.is_found(), "start {start:?}");
            assert_eq!(candidate.cost(), baseline.cost(), "start {start:?}");
        }
    }

    #[test]
    fn query_cost_matches_dijkstra_on_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 goal = Point::new(3, 1);
        let prepared = FixedGoalReverseDistanceFieldBuilder::new(goal)
            .preprocess(&grid)
            .expect("preprocess succeeds");

        for start in [Point::new(0, 1), Point::new(0, 0), Point::new(0, 2)] {
            let request = SearchRequest::new(start, goal);
            let candidate = prepared.search(request).expect("endpoints walkable");
            let baseline = Dijkstra.search(&grid, request).expect("endpoints walkable");
            assert!(candidate.is_found(), "start {start:?}");
            assert_eq!(
                candidate.cost(),
                baseline.cost(),
                "weighted reverse field must match Dijkstra for start {start:?}"
            );
        }
    }

    #[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 goal = Point::new(2, 2);
        let prepared = FixedGoalReverseDistanceFieldBuilder::new(goal)
            .preprocess(&grid)
            .expect("preprocess succeeds");
        let result = prepared
            .search(SearchRequest::new(Point::new(0, 0), goal))
            .expect("endpoints walkable");
        assert!(!result.is_found());
    }

    #[test]
    fn stale_geometry_or_costs_require_rebuild() {
        let mut grid = Grid::new(4, 1).expect("grid dimensions are valid");
        let goal = Point::new(3, 0);
        let prepared = FixedGoalReverseDistanceFieldBuilder::new(goal)
            .preprocess(&grid)
            .expect("preprocess succeeds");

        // Mutate the *live* grid after preprocess — prepared snapshot must keep
        // the old answer; a rebuild is required for the new geometry.
        grid.set_cell(Point::new(1, 0), Cell::Blocked)
            .expect("valid grid edit");

        let snapshot_answer = prepared
            .search(SearchRequest::new(Point::new(0, 0), goal))
            .expect("endpoints walkable on snapshot");
        assert!(
            snapshot_answer.is_found(),
            "immutable snapshot still has the old open corridor"
        );
        assert_eq!(snapshot_answer.cost(), Some(3));

        let rebuilt = FixedGoalReverseDistanceFieldBuilder::new(goal)
            .preprocess(&grid)
            .expect("rebuild succeeds");
        let rebuilt_answer = rebuilt
            .search(SearchRequest::new(Point::new(0, 0), goal))
            .expect("endpoints walkable");
        assert!(
            !rebuilt_answer.is_found(),
            "rebuild observes the new blocked cell"
        );

        // Goal mismatch must not return a Found path for a different goal.
        let mismatch = prepared
            .search(SearchRequest::new(Point::new(0, 0), Point::new(2, 0)))
            .expect("validation uses snapshot endpoints");
        assert!(!mismatch.is_found());
    }

    #[test]
    fn retains_candidate_id() {
        assert_eq!(
            FixedGoalReverseDistanceFieldBuilder::CANDIDATE_ID,
            "static-weighted-grid/fixed-goal-reverse-distance-field"
        );
    }
}