condor_grid/

prepared_any_angle.rs

//! Prepared exact any-angle search for many queries over one static grid.
//!
//! [`PreparedAnyAngleGridBuilder`] snapshots retained visibility vertices and their
//! Euclidean LOS edges into CSR form. Each [`PreparedAnyAngleGrid::search`] overlays
//! endpoints without mutating that snapshot and preserves the any-angle invalid/
//! found/no-path contract. Prefer this lane over [`crate::ThetaStar`] or
//! [`crate::Anya`] only when its explicit node, edge, and retained-byte budgets admit
//! a repeated-query workload.

use crate::{
    Grid,
    algorithms::any_angle_visibility_graph_kernel::{
        DijkstraSearchStats, VisibilityGraphBuildStats, VisibilityGraphCsr, add_undirected_edge,
        build_visibility_csr, elide_collinear_points, node_index, path_endpoints_match_request,
        run_dijkstra_csr_with_overlay,
    },
    any_angle::geometry::{
        approximately_equal, canonicalize_grid_vertex, extract_boundary_edges, is_endpoint_valid,
        retained_visibility_vertices, segment_is_legal, validated_any_angle_path,
    },
    any_angle::{
        AnyAngleSearchError, AnyAngleSearchRequest, AnyAngleSearchResult, AnyAngleSearchStats,
    },
    search::SearchOutcome,
};
use condor_core::Point2;

/// Maximum prepared visibility nodes supported by the v0 repeated-query lane.
///
/// Open 64×64 grids retain 4,225 vertices and remain admitted. Open 128×128
/// retains 16,641 vertices and is rejected before pair enumeration.
pub const PREPARED_ANY_ANGLE_NODE_BUDGET: usize = 10_000;

/// Conservative upper bound on possible directed visibility edges.
///
/// Uses the all-pairs-visible estimate `n·(n−1)` so dense open fields are
/// rejected before quadratic visibility work begins.
pub const PREPARED_ANY_ANGLE_DIRECTED_EDGE_BUDGET: usize = 32_000_000;

/// Conservative upper bound on preprocess-retained bytes (vertices + CSR).
pub const PREPARED_ANY_ANGLE_RETAINED_BYTES_BUDGET: usize = 256_000_000;

/// Maximum CSR neighbor/weight index storable in `u32`.
pub const PREPARED_ANY_ANGLE_CSR_U32_INDEX_LIMIT: u32 = u32::MAX;

/// Maximum retained-node count measured by the default prepared-v2 Criterion lane.
///
/// This is deliberately stricter than [`PREPARED_ANY_ANGLE_NODE_BUDGET`]. A graph
/// can be safe to build for explicit evidence while still being too large for a
/// default Criterion invocation to repeatedly preprocess without consuming an
/// unreasonable amount of memory or time.
pub const PREPARED_ANY_ANGLE_DEFAULT_BENCHMARK_NODE_BUDGET: usize = 2_000;

/// Preprocesses a static grid into an exact any-angle visibility-graph snapshot.
///
/// Fails closed when retained vertices or projected edges exceed the published
/// budgets (no partial CSR is returned).
#[derive(Debug, Clone, Copy, Default)]
pub struct PreparedAnyAngleGridBuilder;

impl PreparedAnyAngleGridBuilder {
    /// Constructs the default prepared any-angle builder.
    #[must_use]
    pub const fn new() -> Self {
        Self
    }

    /// Stable builder identity for reports and benchmarks.
    #[must_use]
    pub const fn name(&self) -> &'static str {
        "prepared-any-angle-grid"
    }

    /// Snapshots `grid` into a prepared visibility graph.
    ///
    /// # Errors
    ///
    /// Returns [`PreparedAnyAngleGridBuildError`] when conservative preflight
    /// limits are exceeded.
    pub fn preprocess(
        &self,
        grid: &Grid,
    ) -> Result<PreparedAnyAngleGrid, PreparedAnyAngleGridBuildError> {
        PreparedAnyAngleGrid::build(grid)
    }

    /// Classifies whether `grid` belongs in the default prepared-v2 benchmark lane.
    ///
    /// This performs only retained-vertex discovery and conservative preflight; it
    /// never enumerates visibility pairs or allocates the prepared CSR.
    #[must_use]
    pub fn default_benchmark_admission(&self, grid: &Grid) -> PreparedAnyAngleBenchmarkAdmission {
        let node_count = retained_visibility_vertices(grid).len();
        match preflight_build_limits(node_count) {
            Err(error) => PreparedAnyAngleBenchmarkAdmission::Unsupported { error },
            Ok(()) if node_count > PREPARED_ANY_ANGLE_DEFAULT_BENCHMARK_NODE_BUDGET => {
                PreparedAnyAngleBenchmarkAdmission::DeferredHeavy { node_count }
            }
            Ok(()) => PreparedAnyAngleBenchmarkAdmission::Measure { node_count },
        }
    }
}

/// Immutable prepared visibility graph for repeated exact any-angle queries.
///
/// Holds a grid snapshot, retained vertices, and CSR edges from preprocess. Queries
/// never mutate durable state; start/goal are attached via local overlay edges.
#[derive(Debug, Clone, PartialEq)]
pub struct PreparedAnyAngleGrid {
    grid: Grid,
    nodes: Vec<Point2>,
    csr: VisibilityGraphCsr,
    build_diagnostics: PreparedAnyAngleBuildDiagnostics,
}

impl PreparedAnyAngleGrid {
    /// Returns the matching prepared builder.
    #[must_use]
    pub fn builder() -> PreparedAnyAngleGridBuilder {
        PreparedAnyAngleGridBuilder::new()
    }

    /// Stable prepared-map identity.
    #[must_use]
    pub fn name(&self) -> &'static str {
        PreparedAnyAngleGridBuilder::new().name()
    }

    /// Owned immutable grid snapshot used for legality and endpoint validation.
    #[must_use]
    pub fn grid(&self) -> &Grid {
        &self.grid
    }

    /// Retained visibility-graph vertices in deterministic preprocess order.
    #[must_use]
    pub fn nodes(&self) -> &[Point2] {
        &self.nodes
    }

    /// Build-time diagnostics recorded during preprocess.
    #[must_use]
    pub fn build_diagnostics(&self) -> &PreparedAnyAngleBuildDiagnostics {
        &self.build_diagnostics
    }

    /// Runs one exact prepared search.
    pub fn search(&self, request: AnyAngleSearchRequest) -> AnyAngleSearchResult {
        self.search_with_diagnostics(request).0
    }

    /// Runs one exact prepared search and returns query diagnostics.
    pub fn search_with_diagnostics(
        &self,
        request: AnyAngleSearchRequest,
    ) -> (AnyAngleSearchResult, PreparedAnyAngleQueryDiagnostics) {
        let Some(start) = canonicalize_grid_vertex(request.start) else {
            return (
                Err(AnyAngleSearchError::InvalidStart {
                    point: request.start,
                }),
                PreparedAnyAngleQueryDiagnostics::default(),
            );
        };
        let Some(goal) = canonicalize_grid_vertex(request.goal) else {
            return (
                Err(AnyAngleSearchError::InvalidGoal {
                    point: request.goal,
                }),
                PreparedAnyAngleQueryDiagnostics::default(),
            );
        };
        if !is_endpoint_valid(&self.grid, start) {
            return (
                Err(AnyAngleSearchError::InvalidStart {
                    point: request.start,
                }),
                PreparedAnyAngleQueryDiagnostics::default(),
            );
        }
        if !is_endpoint_valid(&self.grid, goal) {
            return (
                Err(AnyAngleSearchError::InvalidGoal {
                    point: request.goal,
                }),
                PreparedAnyAngleQueryDiagnostics::default(),
            );
        }
        let request = AnyAngleSearchRequest::new(start, goal);

        if approximately_equal(request.start.x, request.goal.x)
            && approximately_equal(request.start.y, request.goal.y)
        {
            let path = validated_any_angle_path(&self.grid, vec![request.start, request.goal])
                .expect("start equals goal path is non-empty");
            return (
                Ok(SearchOutcome::found(
                    path,
                    AnyAngleSearchStats { visited_nodes: 1 },
                )),
                PreparedAnyAngleQueryDiagnostics {
                    path_point_count: 2,
                    ..PreparedAnyAngleQueryDiagnostics::default()
                },
            );
        }

        let mut query_diagnostics = PreparedAnyAngleQueryDiagnostics {
            direct_start_goal_tested: true,
            ..Default::default()
        };
        query_diagnostics.endpoint_visibility_tests += 1;
        if segment_is_legal(&self.grid, request.start, request.goal) {
            query_diagnostics.direct_start_goal_visible = true;
            let path = validated_any_angle_path(&self.grid, vec![request.start, request.goal])
                .expect("direct visible segment should validate");
            return (
                Ok(SearchOutcome::found(
                    path,
                    AnyAngleSearchStats { visited_nodes: 1 },
                )),
                PreparedAnyAngleQueryDiagnostics {
                    path_point_count: 2,
                    ..query_diagnostics
                },
            );
        }

        let (supplemental_nodes, overlay_edges, start_index, goal_index, overlay_stats) =
            build_overlay_search_graph(&self.grid, &self.nodes, request.start, request.goal);
        query_diagnostics.endpoint_visibility_tests += overlay_stats.endpoint_visibility_tests;
        query_diagnostics.endpoint_accepted_edges += overlay_stats.endpoint_accepted_edges;

        let mut search_stats = DijkstraSearchStats::default();
        let watch = crate::search::BudgetWatch::start(request.budget);
        let search = match run_dijkstra_csr_with_overlay(
            &self.csr,
            self.nodes.len(),
            &overlay_edges,
            start_index,
            goal_index,
            &mut search_stats,
            &watch,
        ) {
            Ok(outcome) => outcome,
            Err(reason) => {
                return (
                    Err(crate::any_angle::budget_error(reason)),
                    query_diagnostics,
                );
            }
        };
        query_diagnostics.settled_nodes = search_stats.settled_nodes;
        query_diagnostics.pushes = search_stats.pushes;
        query_diagnostics.stale_pops = search_stats.stale_pops;

        let Some(predecessors) = search else {
            return (
                Ok(SearchOutcome::no_path(AnyAngleSearchStats {
                    visited_nodes: query_diagnostics.settled_nodes,
                })),
                query_diagnostics,
            );
        };

        let mut path_points =
            reconstruct_prepared_path(&self.nodes, &supplemental_nodes, &predecessors, goal_index);
        elide_collinear_points(&self.grid, &mut path_points);
        query_diagnostics.path_point_count = path_points.len();

        let Ok(path) = validated_any_angle_path(&self.grid, path_points) else {
            return (
                Ok(SearchOutcome::no_path(AnyAngleSearchStats {
                    visited_nodes: query_diagnostics.settled_nodes,
                })),
                query_diagnostics,
            );
        };
        if !path_endpoints_match_request(&path, request) {
            return (
                Ok(SearchOutcome::no_path(AnyAngleSearchStats {
                    visited_nodes: query_diagnostics.settled_nodes,
                })),
                query_diagnostics,
            );
        }

        (
            Ok(SearchOutcome::found(
                path,
                AnyAngleSearchStats {
                    visited_nodes: query_diagnostics.settled_nodes,
                },
            )),
            query_diagnostics,
        )
    }

    fn build(grid: &Grid) -> Result<Self, PreparedAnyAngleGridBuildError> {
        let boundary_edges = extract_boundary_edges(grid).len();
        let nodes = retained_visibility_vertices(grid);
        preflight_build_limits(nodes.len())?;

        let mut build_stats = VisibilityGraphBuildStats::default();
        let csr = build_visibility_csr(grid, &nodes, &mut build_stats);
        let node_bytes = nodes.len() * size_of::<Point2>();
        let build_diagnostics = PreparedAnyAngleBuildDiagnostics {
            boundary_edges,
            prepared_nodes: nodes.len(),
            directed_edges: csr.directed_edge_count(),
            visibility_checks: build_stats.visibility_checks,
            accepted_undirected_edges: build_stats.accepted_undirected_edges,
            retained_bytes: node_bytes + csr.retained_bytes(),
        };

        Ok(Self {
            grid: grid.clone(),
            nodes,
            csr,
            build_diagnostics,
        })
    }
}

/// Default-measurement classification for the prepared-v2 Criterion lane.
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub enum PreparedAnyAngleBenchmarkAdmission {
    /// The scenario is within both the safe preprocess and default-measurement budgets.
    Measure { node_count: usize },
    /// The scenario is safe to build but intentionally omitted from default measurements.
    DeferredHeavy { node_count: usize },
    /// The scenario is rejected by the normal prepared-map preflight policy.
    Unsupported {
        error: PreparedAnyAngleGridBuildError,
    },
}

/// Build-time diagnostics for a prepared any-angle visibility graph.
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub struct PreparedAnyAngleBuildDiagnostics {
    pub boundary_edges: usize,
    pub prepared_nodes: usize,
    pub directed_edges: usize,
    pub visibility_checks: usize,
    pub accepted_undirected_edges: usize,
    pub retained_bytes: usize,
}

/// Per-query diagnostics for prepared any-angle search.
#[derive(Debug, Clone, Copy, Default, PartialEq, Eq)]
pub struct PreparedAnyAngleQueryDiagnostics {
    pub endpoint_visibility_tests: usize,
    pub endpoint_accepted_edges: usize,
    pub direct_start_goal_tested: bool,
    pub direct_start_goal_visible: bool,
    pub settled_nodes: usize,
    pub pushes: usize,
    pub stale_pops: usize,
    pub path_point_count: usize,
}

/// Error returned when prepared any-angle preprocessing fails.
#[derive(Debug, Clone, Copy, PartialEq, Eq, thiserror::Error)]
#[non_exhaustive]
pub enum PreparedAnyAngleGridBuildError {
    /// Retained vertex count exceeds the supported repeated-query budget.
    #[error("prepared any-angle graph exceeds supported node budget ({node_count} > {limit})")]
    NodeBudgetExceeded { node_count: usize, limit: usize },
    /// Possible directed-edge count exceeds the supported repeated-query budget.
    #[error(
        "prepared any-angle graph exceeds supported directed-edge budget ({edge_count} > {limit})"
    )]
    DirectedEdgeBudgetExceeded { edge_count: usize, limit: usize },
    /// Retained graph bytes exceed the supported repeated-query budget.
    #[error("prepared any-angle graph exceeds supported retained-byte budget ({bytes} > {limit})")]
    RetainedBytesBudgetExceeded { bytes: usize, limit: usize },
    /// CSR offset/neighbor indices would overflow `u32`.
    #[error("prepared any-angle graph exceeds CSR u32 index capacity ({index_count} > {limit})")]
    CsrIndexCapacityExceeded { index_count: u64, limit: u32 },
}

fn preflight_build_limits(node_count: usize) -> Result<(), PreparedAnyAngleGridBuildError> {
    if node_count > PREPARED_ANY_ANGLE_NODE_BUDGET {
        return Err(PreparedAnyAngleGridBuildError::NodeBudgetExceeded {
            node_count,
            limit: PREPARED_ANY_ANGLE_NODE_BUDGET,
        });
    }

    let directed_edge_upper_bound = node_count.checked_mul(node_count.saturating_sub(1)).ok_or(
        PreparedAnyAngleGridBuildError::DirectedEdgeBudgetExceeded {
            edge_count: usize::MAX,
            limit: PREPARED_ANY_ANGLE_DIRECTED_EDGE_BUDGET,
        },
    )?;
    if directed_edge_upper_bound > PREPARED_ANY_ANGLE_DIRECTED_EDGE_BUDGET {
        return Err(PreparedAnyAngleGridBuildError::DirectedEdgeBudgetExceeded {
            edge_count: directed_edge_upper_bound,
            limit: PREPARED_ANY_ANGLE_DIRECTED_EDGE_BUDGET,
        });
    }

    let retained_bytes = conservative_retained_bytes_upper_bound(node_count).ok_or(
        PreparedAnyAngleGridBuildError::RetainedBytesBudgetExceeded {
            bytes: usize::MAX,
            limit: PREPARED_ANY_ANGLE_RETAINED_BYTES_BUDGET,
        },
    )?;
    if retained_bytes > PREPARED_ANY_ANGLE_RETAINED_BYTES_BUDGET {
        return Err(
            PreparedAnyAngleGridBuildError::RetainedBytesBudgetExceeded {
                bytes: retained_bytes,
                limit: PREPARED_ANY_ANGLE_RETAINED_BYTES_BUDGET,
            },
        );
    }

    let offset_capacity = node_count.saturating_add(1) as u64;
    let index_capacity = directed_edge_upper_bound as u64;
    if offset_capacity > u64::from(PREPARED_ANY_ANGLE_CSR_U32_INDEX_LIMIT)
        || index_capacity > u64::from(PREPARED_ANY_ANGLE_CSR_U32_INDEX_LIMIT)
    {
        return Err(PreparedAnyAngleGridBuildError::CsrIndexCapacityExceeded {
            index_count: offset_capacity.max(index_capacity),
            limit: PREPARED_ANY_ANGLE_CSR_U32_INDEX_LIMIT,
        });
    }

    Ok(())
}

fn conservative_retained_bytes_upper_bound(node_count: usize) -> Option<usize> {
    let directed_edges = node_count.checked_mul(node_count.saturating_sub(1))?;
    let node_bytes = node_count.checked_mul(size_of::<Point2>())?;
    let offset_bytes = node_count.saturating_add(1).checked_mul(size_of::<u32>())?;
    let neighbor_bytes = directed_edges.checked_mul(size_of::<u32>())?;
    let weight_bytes = directed_edges.checked_mul(size_of::<f64>())?;
    node_bytes
        .checked_add(offset_bytes)?
        .checked_add(neighbor_bytes)?
        .checked_add(weight_bytes)
}

#[derive(Debug, Clone, Copy, Default, PartialEq, Eq)]
struct OverlayBuildStats {
    endpoint_visibility_tests: usize,
    endpoint_accepted_edges: usize,
}

type OverlaySearchGraph = (
    Vec<Point2>,
    Vec<Vec<(usize, f64)>>,
    usize,
    usize,
    OverlayBuildStats,
);

fn build_overlay_search_graph(
    grid: &Grid,
    prepared_nodes: &[Point2],
    start: Point2,
    goal: Point2,
) -> OverlaySearchGraph {
    let prepared_count = prepared_nodes.len();
    let mut supplemental_nodes = Vec::new();
    let mut stats = OverlayBuildStats::default();

    let start_index = resolve_search_node_index(prepared_nodes, &mut supplemental_nodes, start);
    let goal_index = resolve_search_node_index(prepared_nodes, &mut supplemental_nodes, goal);

    let node_count = prepared_count + supplemental_nodes.len();
    let mut overlay_edges = vec![Vec::new(); node_count];
    overlay_endpoint_edges(
        grid,
        prepared_nodes,
        &supplemental_nodes,
        &mut overlay_edges,
        prepared_count,
        start_index,
        goal_index,
        start,
        &mut stats,
    );
    overlay_endpoint_edges(
        grid,
        prepared_nodes,
        &supplemental_nodes,
        &mut overlay_edges,
        prepared_count,
        goal_index,
        start_index,
        goal,
        &mut stats,
    );

    stats.endpoint_visibility_tests += 1;
    if segment_is_legal(grid, start, goal) {
        let weight = start.distance_to(goal);
        add_undirected_edge(&mut overlay_edges, start_index, goal_index, weight);
        stats.endpoint_accepted_edges += 1;
    }

    (
        supplemental_nodes,
        overlay_edges,
        start_index,
        goal_index,
        stats,
    )
}

fn reconstruct_prepared_path(
    prepared_nodes: &[Point2],
    supplemental_nodes: &[Point2],
    predecessors: &[Option<usize>],
    goal_index: usize,
) -> Vec<Point2> {
    let prepared_count = prepared_nodes.len();
    let point_for = |index: usize| {
        if index < prepared_count {
            prepared_nodes[index]
        } else {
            supplemental_nodes[index - prepared_count]
        }
    };

    let mut path = vec![point_for(goal_index)];
    let mut current = goal_index;
    while let Some(previous) = predecessors[current] {
        path.push(point_for(previous));
        if previous == current {
            break;
        }
        current = previous;
    }
    path.reverse();
    path
}

fn resolve_search_node_index(
    prepared_nodes: &[Point2],
    supplemental_nodes: &mut Vec<Point2>,
    point: Point2,
) -> usize {
    match node_index(prepared_nodes, point) {
        Some(index) => index,
        None => {
            let index = prepared_nodes.len() + supplemental_nodes.len();
            supplemental_nodes.push(point);
            index
        }
    }
}

#[allow(clippy::too_many_arguments)]
fn overlay_endpoint_edges(
    grid: &Grid,
    prepared_nodes: &[Point2],
    supplemental_nodes: &[Point2],
    overlay_edges: &mut [Vec<(usize, f64)>],
    prepared_count: usize,
    endpoint_index: usize,
    other_endpoint_index: usize,
    endpoint: Point2,
    stats: &mut OverlayBuildStats,
) {
    if endpoint_index < prepared_count {
        return;
    }

    for (node_index, node) in prepared_nodes.iter().enumerate().take(prepared_count) {
        if node_index == endpoint_index || node_index == other_endpoint_index {
            continue;
        }
        let node = *node;
        stats.endpoint_visibility_tests += 1;
        if !segment_is_legal(grid, endpoint, node) {
            continue;
        }
        let weight = endpoint.distance_to(node);
        add_undirected_edge(overlay_edges, endpoint_index, node_index, weight);
        stats.endpoint_accepted_edges += 1;
    }

    for (local_index, node) in supplemental_nodes.iter().enumerate() {
        let node_index = prepared_count + local_index;
        if node_index == endpoint_index || node_index == other_endpoint_index {
            continue;
        }
        let node = *node;
        stats.endpoint_visibility_tests += 1;
        if !segment_is_legal(grid, endpoint, node) {
            continue;
        }
        let weight = endpoint.distance_to(node);
        add_undirected_edge(overlay_edges, endpoint_index, node_index, weight);
        stats.endpoint_accepted_edges += 1;
    }
}