condor-pathfinding-grid 0.4.0

//! Private candidate: exact separator/portal prepared hierarchy.
//!
//! **Hypothesis:** a static-grid separator hierarchy with exact portal
//! distances can amortize repeated weighted queries without presenting an
//! approximate hierarchical route as the canonical answer.
//!
//! **Deliverable-0 invariants (exact contract):**
//! 1. **Separator:** one vertical cut at `x = width / 2` (left cells `x < mid`,
//!    right cells `x >= mid`). Portals are walkable cells on the cut column.
//! 2. **Portal distances:** preprocess stores an exact Dijkstra distance field
//!    from every portal under the destination-cell cost model (same as
//!    [`super::dijkstra::Dijkstra`]). Portal–portal and portal–cell labels are
//!    never hop-count approximations.
//! 3. **Stitch:** when start and goal lie on opposite sides of the cut, the
//!    candidate answer is `min_p dist(p,start) + dist(p,goal)` over portals `p`
//!    with a contiguous witness reconstructed through the chosen portal.
//! 4. **Missing portal / same-side / incomplete cut:** fall back to exact
//!    one-sided Dijkstra on the immutable snapshot — never return a locally
//!    optimal fragment that is not a global shortest path.
//! 5. **Weighted maps OK:** does **not** inherit SubgoalGraph/HPA
//!    `NonUniformCost` rejection.
//!
//! **Evidence and promotion:** ordinary `grid_preprocessing` while developing;
//! remains private until promotion evidence lands.

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

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

/// Builder for an exact separator/portal prepared hierarchy.
///
/// Implements [`PreprocessedGridBuilder`] for weighted 4-connected grids (no
/// uniform-cost rejection). Private candidate beside the prepared-grid family.
#[derive(Debug, Clone, Copy, Default)]
pub struct SeparatorPortalHierarchyBuilder;

impl SeparatorPortalHierarchyBuilder {
    /// Stable source-local candidate identity.
    pub const CANDIDATE_ID: &str = "static-weighted-grid/exact-separator-portal-hierarchy";

    /// Constructs the default mid-cut separator builder.
    #[must_use]
    pub const fn new() -> Self {
        Self
    }
}

impl PreprocessedGridBuilder for SeparatorPortalHierarchyBuilder {
    type Map = PreparedSeparatorPortalHierarchy;

    fn name(&self) -> &'static str {
        "separator-portal-hierarchy"
    }

    fn preprocess(&self, grid: &Grid) -> Result<Self::Map, PreprocessedGridBuildError> {
        let snapshot = grid.clone();
        let mid = snapshot.width() / 2;
        let mut portals = Vec::new();
        for y in 0..snapshot.height() {
            let point = Point::new(mid, y);
            if snapshot.is_walkable(point)
                && let Some(index) = snapshot.index_of(point)
            {
                portals.push(index);
            }
        }

        // Exact distance field from each portal (destination-cell Dijkstra).
        let portal_fields: Vec<DistanceField> = portals
            .iter()
            .map(|&portal_index| dijkstra_field(&snapshot, portal_index))
            .collect();

        Ok(PreparedSeparatorPortalHierarchy {
            grid: snapshot,
            mid_x: mid,
            portals,
            portal_fields,
            metadata: metadata_for_grid(
                grid,
                "separator-portal-hierarchy",
                "separator-portal-query",
            ),
        })
    }
}

/// Prepared exact separator/portal hierarchy over an immutable grid snapshot.
///
/// Opposite-side queries stitch via portal distance fields; same-side or incomplete
/// cuts fall back to one-sided Dijkstra. Destination-cell cost model.
#[derive(Debug, Clone, PartialEq, Eq)]
pub struct PreparedSeparatorPortalHierarchy {
    grid: Grid,
    mid_x: usize,
    portals: Vec<usize>,
    portal_fields: Vec<DistanceField>,
    metadata: PreprocessedGridMetadata,
}

#[derive(Debug, Clone, PartialEq, Eq)]
struct DistanceField {
    dist: Vec<Option<usize>>,
    parents: Vec<Option<usize>>,
}

impl PreparedGridSearch for PreparedSeparatorPortalHierarchy {
    fn name(&self) -> &'static str {
        self.metadata.builder_name
    }

    fn grid(&self) -> &Grid {
        &self.grid
    }

    fn metadata(&self) -> &PreprocessedGridMetadata {
        &self.metadata
    }

    fn search(&self, request: SearchRequest) -> SearchResult {
        crate::search::validate_request(&self.grid, request)?;
        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(request.goal) else {
            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 start_side = side_of(request.start.x, self.mid_x);
        let goal_side = side_of(request.goal.x, self.mid_x);

        if start_side != goal_side
            && !self.portals.is_empty()
            && let Some(answer) = self.try_portal_stitch(start_index, goal_index)
        {
            return crate::search::found(answer.path, answer.visited_nodes);
            // Missing portal route falls through to exact Dijkstra (never a fragment).
        }

        // Same-side, empty portals, or stitch miss → exact one-sided Dijkstra.
        dijkstra_search(
            &self.grid,
            start_index,
            goal_index,
            request.start,
            request.goal,
        )
    }
}

struct StitchAnswer {
    path: Path,
    visited_nodes: usize,
}

impl PreparedSeparatorPortalHierarchy {
    fn try_portal_stitch(&self, start_index: usize, goal_index: usize) -> Option<StitchAnswer> {
        // Destination-cell costs are directed: dist(start→portal) ≠ dist(portal→start).
        // Stitch as dist(start→portal) + dist(portal→goal) using a start-side field
        // plus each portal's precomputed field toward the goal.
        let start_field = dijkstra_field(&self.grid, start_index);

        let mut best: Option<(usize, usize, usize)> = None; // cost, portal_index, field_idx
        for (field_idx, portal_index) in self.portals.iter().copied().enumerate() {
            let Some(to_portal) = start_field.dist[portal_index] else {
                continue;
            };
            let Some(to_goal) = self.portal_fields[field_idx].dist[goal_index] else {
                continue;
            };
            let Some(total) = to_portal.checked_add(to_goal) else {
                continue;
            };
            let replace = best.is_none_or(|(best_cost, best_portal, _)| {
                (total, portal_index) < (best_cost, best_portal)
            });
            if replace {
                best = Some((total, portal_index, field_idx));
            }
        }

        let (total_cost, portal_index, field_idx) = best?;
        let portal_field = &self.portal_fields[field_idx];

        // start_field parents point toward start: walk portal→…→start then reverse.
        let mut steps = reconstruct_from_field(
            &self.grid,
            &start_field,
            portal_index,
            start_index,
            start_field.dist[portal_index].expect("start→portal"),
        )
        .steps()
        .to_vec();
        steps.reverse(); // now start → … → portal

        // portal_field parents point toward portal: walk goal→…→portal then reverse.
        let mut tail = reconstruct_from_field(
            &self.grid,
            portal_field,
            goal_index,
            portal_index,
            portal_field.dist[goal_index].expect("portal→goal"),
        )
        .steps()
        .to_vec();
        tail.reverse(); // now portal → … → goal
        if tail.first() == Some(&self.grid.point_from_index(portal_index)) {
            tail.remove(0);
        }
        steps.extend(tail);

        let path = Path::from_steps_with_cost(steps, total_cost)
            .expect("stitched path contains at least one point");
        Some(StitchAnswer {
            path,
            visited_nodes: self.portals.len().saturating_add(1),
        })
    }
}

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

fn side_of(x: usize, mid_x: usize) -> Side {
    if x < mid_x { Side::Left } else { Side::Right }
}

fn dijkstra_field(grid: &Grid, source_index: usize) -> DistanceField {
    let mut dist = vec![None; grid.cell_count()];
    let mut parents = vec![None; grid.cell_count()];
    let mut frontier = BinaryHeap::from([FrontierEntry {
        cost_so_far: 0,
        index: source_index,
    }]);
    dist[source_index] = Some(0);

    while let Some(entry) = frontier.pop() {
        if dist[entry.index] != Some(entry.cost_so_far) {
            continue;
        }
        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 dist[neighbor_index].is_some_and(|best| next_cost >= best) {
                continue;
            }
            dist[neighbor_index] = Some(next_cost);
            parents[neighbor_index] = Some(entry.index);
            frontier.push(FrontierEntry {
                cost_so_far: next_cost,
                index: neighbor_index,
            });
        }
    }

    DistanceField { dist, parents }
}

fn dijkstra_search(
    grid: &Grid,
    start_index: usize,
    goal_index: usize,
    start: Point,
    goal: Point,
) -> SearchResult {
    if !grid.is_reachable(start, goal) {
        return crate::search::not_found(0);
    }

    let field = dijkstra_field(grid, start_index);
    let Some(total_cost) = field.dist[goal_index] else {
        return crate::search::not_found(0);
    };
    crate::search::found(
        reconstruct_from_field(grid, &field, goal_index, start_index, total_cost),
        1,
    )
}

/// Reconstruct path from `from_index` to field `source_index` by walking parents
/// toward the source, then reverse so the path reads from→…→source.
fn reconstruct_from_field(
    grid: &Grid,
    field: &DistanceField,
    from_index: usize,
    source_index: usize,
    total_cost: usize,
) -> Path {
    let mut current_index = from_index;
    let mut steps = vec![grid.point_from_index(from_index)];
    while current_index != source_index {
        current_index = field.parents[current_index]
            .expect("labeled node must have a parent chain to the field source");
        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,
            prepared_separator_portal_hierarchy::SeparatorPortalHierarchyBuilder,
        },
        grid::{Cell, Grid},
        point::Point,
        preprocessed_grid::{PreparedGridSearch, PreprocessedGridBuilder},
        search::{Pathfinder, SearchRequest},
    };

    #[test]
    fn exact_stitch_cost_matches_dijkstra() {
        let mut grid = Grid::new(7, 5).expect("grid dimensions are valid");
        for y in 0..5 {
            if y != 2 {
                grid.set_cell(Point::new(3, y), Cell::Blocked)
                    .expect("valid grid edit");
            }
        }
        // Force a weighted detour on the only gap.
        grid.set_traversal_cost(Point::new(3, 2), 3)
            .expect("valid cost edit");

        let prepared = SeparatorPortalHierarchyBuilder::new()
            .preprocess(&grid)
            .expect("preprocess succeeds");
        let request = SearchRequest::new(Point::new(0, 2), Point::new(6, 2));

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

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

    #[test]
    fn missing_portal_falls_back_or_no_path_never_fragment() {
        // Fully blocked mid column: no portals. Opposite-side query must not
        // invent a fragment; fallback Dijkstra reports no-path.
        let mut grid = Grid::new(5, 3).expect("grid dimensions are valid");
        let mid = grid.width() / 2;
        for y in 0..3 {
            grid.set_cell(Point::new(mid, y), Cell::Blocked)
                .expect("valid grid edit");
        }

        let prepared = SeparatorPortalHierarchyBuilder::new()
            .preprocess(&grid)
            .expect("preprocess succeeds");
        let request = SearchRequest::new(Point::new(0, 1), Point::new(4, 1));
        let result = prepared.search(request).expect("endpoints walkable");
        assert!(
            !result.is_found(),
            "missing portals must not yield a local fragment"
        );

        let baseline = Dijkstra.search(&grid, request).expect("endpoints walkable");
        assert!(!baseline.is_found());
    }

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

    #[test]
    fn retains_candidate_id() {
        assert_eq!(
            SeparatorPortalHierarchyBuilder::CANDIDATE_ID,
            "static-weighted-grid/exact-separator-portal-hierarchy"
        );
    }
}