condor-pathfinding-grid 0.4.0

//! Any-angle [`AnyAnglePathfinder`]: Anya-family entrypoint.
//!
//! [`Anya::search`] currently delegates to the exact corner-visibility-graph
//! authority shared with the prepared any-angle lane. The row-interval candidate
//! remains private and powers [`Anya::inspect`] diagnostics only; its successor
//! kernel is not yet complete.
//!
//! Cost is Euclidean polyline length between grid-aligned vertices under the
//! no-corner-cut LOS contract. Prefer Anya for the curated any-angle entrypoint,
//! Theta* / Lazy Theta* for their direct online variants, and prepared any-angle
//! for repeated-query amortization.

mod diagnostics;
mod geometry;
#[allow(
    dead_code,
    reason = "private, not-ready candidate retained beside the ordinary Anya family; it may be discarded after evaluation"
)]
mod row_interval;
mod runs;
mod state;
mod successors;

use std::cell::Cell;
use std::collections::{BinaryHeap, HashMap};

pub use diagnostics::{AnyaDiagnostics, AnyaInspection};
pub use geometry::IntervalKind;

use crate::{
    Grid,
    algorithms::any_angle_visibility_graph::AnyAngleVisibilityGraphOracle,
    any_angle::geometry::{approximately_equal, recompute_path_cost},
    any_angle::{
        AnyAnglePath, AnyAnglePathfinder, AnyAngleSearchRequest, AnyAngleSearchResult, found,
        not_found,
    },
};
use condor_core::Point2;

use diagnostics::AnyaDiagnostics as Diagnostics;
use geometry::{parse_request, segment_legal, validate_path};
use runs::RowRunIndex;
use state::{HeapEntry, IntervalState, StateArena, StateId};
use successors::{GoalConnection, SuccessorContext, expand_state, initial_state, push_interval};

/// Any-angle pathfinder implementing [`AnyAnglePathfinder`].
///
/// Public searches are oracle-supervised (exact corner visibility graph). Use
/// [`Self::inspect`] for experimental interval-candidate diagnostics; do not treat
/// inspect-only counters as product search stats.
#[derive(Debug, Clone, Copy, Default)]
pub struct Anya;

impl Anya {
    /// Search with solver-local diagnostics (not included in public stats).
    ///
    /// This explicitly evaluates the experimental interval candidate before
    /// certifying it with the exact oracle. Use it for bounded diagnostic work,
    /// not ordinary application searches.
    #[must_use]
    pub fn inspect(&self, grid: &Grid, request: AnyAngleSearchRequest) -> AnyaInspection {
        let (result, diagnostics) = search_impl(grid, request, true);
        AnyaInspection {
            result,
            diagnostics,
        }
    }
}

impl AnyAnglePathfinder for Anya {
    fn name(&self) -> &'static str {
        "anya"
    }

    fn search(&self, grid: &Grid, request: AnyAngleSearchRequest) -> AnyAngleSearchResult {
        // The row-interval candidate is retained for `inspect`, but its
        // successor kernel is not yet complete. Public calls have always
        // returned this exact supervisor result; avoiding the candidate work
        // here preserves that contract without its unbounded state growth.
        AnyAngleVisibilityGraphOracle.search(grid, request)
    }
}

fn search_impl(
    grid: &Grid,
    request: AnyAngleSearchRequest,
    collect_diagnostics: bool,
) -> (AnyAngleSearchResult, Diagnostics) {
    let mut diagnostics = Diagnostics::default();

    let (start, goal) = match parse_request(grid, request.start, request.goal) {
        Ok(endpoints) => endpoints,
        Err(error) => return (Err(error), diagnostics),
    };

    if approximately_equal(start.x, goal.x) && approximately_equal(start.y, goal.y) {
        let path = AnyAnglePath::from_points(vec![start, goal]).expect("non-empty path");
        return (found(path, 1), diagnostics);
    }

    if segment_legal(grid, start, goal) {
        let path = AnyAnglePath::from_points(vec![start, goal]).expect("non-empty path");
        diagnostics.path_points = 2;
        return (found(path, 1), diagnostics);
    }

    let runs = RowRunIndex::build(grid);
    diagnostics.run_index_bytes = runs.bytes();

    let Some(start_run) = runs.run_containing(start) else {
        return (not_found(0), diagnostics);
    };

    let mut arena = StateArena::default();
    let mut best_by_interval = HashMap::new();
    let mut heap = BinaryHeap::new();
    let best_goal_cost = Cell::new(f64::INFINITY);
    let goal_connection = Cell::new(None::<GoalConnection>);
    let mut visited_nodes = 0usize;

    let initial = initial_state(start, start_run);
    let initial_enqueued = enqueue_interval(
        grid,
        &runs,
        goal,
        &mut arena,
        &mut best_by_interval,
        &mut diagnostics,
        &best_goal_cost,
        &goal_connection,
        StateId(0),
        initial,
    );
    for entry in initial_enqueued {
        heap.push(entry);
    }
    diagnostics.heap_peak = heap.len();

    while let Some(entry) = heap.pop() {
        diagnostics.popped += 1;
        let state_id = entry.state_id;
        let state = *arena.get(state_id);

        if arena.generation(state_id) != state.generation {
            diagnostics.stale += 1;
            continue;
        }

        visited_nodes += 1;

        // Interval keys are an expansion order only.  A state may contain a
        // legal goal connection through a point not represented by its endpoint
        // lower bound, so do not use the key as a proof that the incumbent is
        // globally final.

        let enqueued = expand_and_enqueue(
            grid,
            &runs,
            goal,
            &mut arena,
            &mut best_by_interval,
            &mut diagnostics,
            &best_goal_cost,
            &goal_connection,
            state_id,
            state,
        );
        for queued in enqueued {
            heap.push(queued);
        }
        diagnostics.heap_peak = diagnostics.heap_peak.max(heap.len());
    }

    let interval_result = if let Some(goal_connection) = goal_connection.get() {
        let points = reconstruct_path(grid, &arena, goal_connection, start, goal);
        diagnostics.path_points = points.len();
        diagnostics.validation_segments = points.len().saturating_sub(1);

        if validate_path(grid, &points) {
            let cost = recompute_path_cost(&points);
            let path = AnyAnglePath::from_points_with_cost(points, cost).expect("non-empty path");
            found(path, visited_nodes)
        } else {
            not_found(visited_nodes)
        }
    } else {
        not_found(visited_nodes)
    };

    if collect_diagnostics {
        diagnostics.state_bytes = arena.bytes();
    }

    // The interval engine is intentionally retained as an independently
    // instrumented candidate, but its successor completeness proof is still
    // open.  Certify every non-trivial public result with the exact v0 oracle
    // rather than returning a potentially longer interval witness.
    diagnostics.exact_supervisor_queries = 1;
    let (exact_result, _) = AnyAngleVisibilityGraphOracle
        .search_with_diagnostics(grid, AnyAngleSearchRequest::new(start, goal));
    if !same_outcome_cost(&interval_result, &exact_result) {
        diagnostics.exact_supervisor_replacements = 1;
    }
    (exact_result, diagnostics)
}

fn same_outcome_cost(left: &AnyAngleSearchResult, right: &AnyAngleSearchResult) -> bool {
    match (left, right) {
        (Ok(left), Ok(right)) => match (left.path(), right.path()) {
            (None, None) => true,
            (Some(left), Some(right)) => approximately_equal(left.cost(), right.cost()),
            _ => false,
        },
        (Err(left), Err(right)) => left == right,
        _ => false,
    }
}

#[allow(clippy::too_many_arguments)]
fn enqueue_interval(
    grid: &Grid,
    runs: &RowRunIndex,
    goal: Point2,
    arena: &mut StateArena,
    best_by_interval: &mut HashMap<successors::DominanceKey, (f64, StateId)>,
    diagnostics: &mut Diagnostics,
    best_goal_cost: &Cell<f64>,
    goal_connection: &Cell<Option<GoalConnection>>,
    predecessor: StateId,
    state: IntervalState,
) -> Vec<HeapEntry> {
    let mut pending = Vec::new();
    let mut ctx = SuccessorContext {
        grid,
        runs,
        goal,
        arena,
        best_by_interval,
        diagnostics,
        best_goal_cost,
        goal_connection,
        pending_heap: &mut pending,
    };
    push_interval(&mut ctx, predecessor, state);
    pending
}

#[allow(clippy::too_many_arguments)]
fn expand_and_enqueue(
    grid: &Grid,
    runs: &RowRunIndex,
    goal: Point2,
    arena: &mut StateArena,
    best_by_interval: &mut HashMap<successors::DominanceKey, (f64, StateId)>,
    diagnostics: &mut Diagnostics,
    best_goal_cost: &Cell<f64>,
    goal_connection: &Cell<Option<GoalConnection>>,
    state_id: StateId,
    state: IntervalState,
) -> Vec<HeapEntry> {
    let mut pending = Vec::new();
    let mut ctx = SuccessorContext {
        grid,
        runs,
        goal,
        arena,
        best_by_interval,
        diagnostics,
        best_goal_cost,
        goal_connection,
        pending_heap: &mut pending,
    };
    expand_state(&mut ctx, state_id, state);
    pending
}

fn reconstruct_path(
    grid: &Grid,
    arena: &StateArena,
    goal_connection: GoalConnection,
    start: Point2,
    goal: Point2,
) -> Vec<Point2> {
    let mut points = vec![goal];
    let goal_id = goal_connection.terminal_state;
    if let Some(probe) = goal_connection.via
        && (!approximately_equal(probe.x, goal.x) || !approximately_equal(probe.y, goal.y))
    {
        points.push(probe);
    }

    let mut current = Some(goal_id);
    while let Some(id) = current {
        let state = arena.get(id);
        if points.last().is_none_or(|last| {
            !approximately_equal(last.x, state.root.x) || !approximately_equal(last.y, state.root.y)
        }) {
            points.push(state.root);
        }
        current = state.predecessor;
    }
    if points.last().is_none_or(|last| {
        !approximately_equal(last.x, start.x) || !approximately_equal(last.y, start.y)
    }) {
        points.push(start);
    }
    points.reverse();
    simplify_collinear(grid, &mut points);
    points
}

fn simplify_collinear(grid: &Grid, points: &mut Vec<Point2>) {
    if points.len() < 3 {
        return;
    }
    let mut simplified = Vec::with_capacity(points.len());
    simplified.push(points[0]);
    for idx in 1..points.len() - 1 {
        let prev = simplified[simplified.len() - 1];
        let current = points[idx];
        let next = points[idx + 1];
        if are_collinear(prev, current, next) && segment_legal(grid, prev, next) {
            continue;
        }
        simplified.push(current);
    }
    simplified.push(*points.last().expect("non-empty"));
    *points = simplified;
}

fn are_collinear(a: Point2, b: Point2, c: Point2) -> bool {
    let abx = b.x - a.x;
    let aby = b.y - a.y;
    let bcx = c.x - b.x;
    let bcy = c.y - b.y;
    (abx * bcy - aby * bcx).abs() <= 1e-12
}

#[cfg(test)]
mod tests {
    use super::*;
    use crate::{grid::Cell, point::Point};

    #[test]
    fn legality_aware_collinear_simplification_retains_checkerboard_corners() {
        let mut points = vec![
            Point2::new(0.0, 0.0),
            Point2::new(1.0, 1.0),
            Point2::new(2.0, 2.0),
            Point2::new(3.0, 3.0),
            Point2::new(4.0, 4.0),
        ];
        let mut grid = Grid::new(5, 5).expect("grid");
        for (x, y) in [
            (1, 0),
            (3, 0),
            (0, 1),
            (2, 1),
            (4, 1),
            (1, 2),
            (3, 2),
            (0, 3),
            (2, 3),
            (4, 3),
            (1, 4),
            (3, 4),
        ] {
            grid.set_cell(Point::new(x, y), Cell::Blocked)
                .expect("block");
        }
        simplify_collinear(&grid, &mut points);
        assert_eq!(
            points.len(),
            5,
            "must retain corner waypoints, got {points:?}"
        );
    }

    #[test]
    fn forbidden_pinch_matches_oracle_cost() {
        use crate::{
            algorithms::any_angle_visibility_graph::AnyAngleVisibilityGraphOracle,
            any_angle::geometry::approximately_equal,
        };

        let mut grid = Grid::new(4, 4).expect("grid");
        for point in [Point::new(1, 1), Point::new(2, 2)] {
            grid.set_cell(point, Cell::Blocked).expect("block");
        }
        let request = AnyAngleSearchRequest::new(Point2::new(0.0, 0.0), Point2::new(3.0, 3.0));
        let oracle = AnyAngleVisibilityGraphOracle
            .search(&grid, request)
            .expect("valid");
        let result = Anya.search(&grid, request).expect("valid");

        assert!(result.is_found());
        let oracle_cost = oracle.path().expect("oracle").cost();
        let anya_cost = result.path().expect("anya").cost();
        assert!(approximately_equal(anya_cost, oracle_cost));
    }

    #[test]
    fn fully_blocked_grid_matches_oracle_reachability() {
        use crate::algorithms::any_angle_visibility_graph::AnyAngleVisibilityGraphOracle;
        let mut grid = Grid::new(2, 2).expect("grid");
        for x in 0..2 {
            for y in 0..2 {
                grid.set_cell(Point::new(x, y), Cell::Blocked)
                    .expect("block");
            }
        }
        let request = AnyAngleSearchRequest::new(Point2::new(0.0, 0.0), Point2::new(2.0, 2.0));
        let oracle = AnyAngleVisibilityGraphOracle
            .search(&grid, request)
            .expect("valid");
        let result = Anya.search(&grid, request).expect("valid");
        assert_eq!(
            result.is_found(),
            oracle.is_found(),
            "anya/oracle reachability mismatch on fully blocked 2x2 (anya path={:?})",
            result.path().map(|p| p.points().to_vec())
        );
    }
}