condor-pathfinding-grid 0.4.0

//! Crate-private exact corner-visibility oracles for the any-angle grid lane.
//!
//! [`AnyAngleVisibilityGraphOracle`] is the independent Euclidean optimality
//! authority over retained integer-grid corners, LOS edges, and deterministic
//! Dijkstra. [`crate::Anya`] currently delegates to it. The denser
//! [`AnyAngleSamplingReferenceOracle`] remains a differential-test reference.

use crate::{
    Grid,
    algorithms::any_angle_visibility_graph_kernel::{
        DijkstraSearchStats, VisibilityGraphBuildStats, build_visibility_adjacency, dedup_points,
        elide_collinear_points, elide_collinear_with_predicate, node_index,
        path_endpoints_match_request, reconstruct_path, run_dijkstra,
    },
    any_angle::geometry::{
        approximately_equal, canonicalize_grid_vertex, extract_boundary_edges, is_endpoint_valid,
        retained_visibility_vertices, sampling_segment_is_legal, segment_is_legal,
        validated_any_angle_path,
    },
    any_angle::{
        AnyAngleSearchError, AnyAngleSearchRequest, AnyAngleSearchResult, AnyAngleSearchStats,
    },
    search::SearchOutcome,
};
use condor_core::Point2;

/// Instrumentation counters for oracle build and query phases.
///
/// Bakeoff/inspect only; not part of public [`crate::AnyAngleSearchStats`].
#[derive(Debug, Clone, Copy, Default, PartialEq, Eq)]
pub struct AnyAngleOracleDiagnostics {
    /// Obstacle-boundary edges extracted during vertex retention.
    pub boundary_edges: usize,
    /// Vertices retained for the visibility graph (corners + full vertex set v0).
    pub retained_corners: usize,
    /// Pairwise LOS tests during adjacency build.
    pub visibility_checks: usize,
    /// Undirected edges accepted after legality checks.
    pub accepted_edges: usize,
    /// Dijkstra settled-node count for the query.
    pub settled_nodes: usize,
    /// Successful distance decreases.
    pub relaxations: usize,
    /// Binary-heap pushes during Dijkstra.
    pub pushes: usize,
    /// Discarded stale/dominated heap pops.
    pub stale_pops: usize,
    /// Peak open-set size.
    pub frontier_peak: usize,
    /// Vertices in the reconstructed any-angle path (0 when no path).
    pub path_point_count: usize,
}

impl AnyAngleOracleDiagnostics {
    fn absorb_build(&mut self, stats: VisibilityGraphBuildStats) {
        self.visibility_checks = stats.visibility_checks;
        self.accepted_edges = stats.accepted_undirected_edges;
    }

    fn absorb_search(&mut self, stats: DijkstraSearchStats) {
        self.settled_nodes = stats.settled_nodes;
        self.relaxations = stats.relaxations;
        self.pushes = stats.pushes;
        self.stale_pops = stats.stale_pops;
        self.frontier_peak = stats.frontier_peak;
    }
}

/// Exact corner-visibility oracle (retained vertices, all-pairs LOS, Dijkstra).
///
/// Cost model: Euclidean edge length under authority `segment_is_legal`. One-shot
/// per search (no durable preprocess); see [`crate::PreparedAnyAngleGrid`] for the
/// build-once / query-many CSR form of the same graph.
#[derive(Debug, Clone, Copy, Default)]
pub struct AnyAngleVisibilityGraphOracle;

impl AnyAngleVisibilityGraphOracle {
    /// Runs one exact oracle search and returns diagnostics alongside the result.
    pub fn search_with_diagnostics(
        &self,
        grid: &Grid,
        request: AnyAngleSearchRequest,
    ) -> (AnyAngleSearchResult, AnyAngleOracleDiagnostics) {
        let Some(start) = canonicalize_grid_vertex(request.start) else {
            return (
                Err(AnyAngleSearchError::InvalidStart {
                    point: request.start,
                }),
                AnyAngleOracleDiagnostics::default(),
            );
        };
        let Some(goal) = canonicalize_grid_vertex(request.goal) else {
            return (
                Err(AnyAngleSearchError::InvalidGoal {
                    point: request.goal,
                }),
                AnyAngleOracleDiagnostics::default(),
            );
        };
        if !is_endpoint_valid(grid, start) {
            return (
                Err(AnyAngleSearchError::InvalidStart {
                    point: request.start,
                }),
                AnyAngleOracleDiagnostics::default(),
            );
        }
        if !is_endpoint_valid(grid, goal) {
            return (
                Err(AnyAngleSearchError::InvalidGoal {
                    point: request.goal,
                }),
                AnyAngleOracleDiagnostics::default(),
            );
        }
        let budget = request.budget;
        let request = AnyAngleSearchRequest::new(start, goal).with_budget(budget);
        let watch = crate::search::BudgetWatch::start(budget);

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

        let boundary_edges = extract_boundary_edges(grid).len();
        let mut nodes = retained_visibility_vertices(grid);
        let mut diagnostics = AnyAngleOracleDiagnostics {
            boundary_edges,
            retained_corners: nodes.len(),
            ..AnyAngleOracleDiagnostics::default()
        };
        nodes.push(request.start);
        nodes.push(request.goal);
        dedup_points(&mut nodes);

        let mut build_stats = VisibilityGraphBuildStats::default();
        let adjacency =
            build_visibility_adjacency(grid, &nodes, segment_is_legal, &mut build_stats);
        diagnostics.absorb_build(build_stats);

        let start_index = node_index(&nodes, request.start).expect("start node should exist");
        let goal_index = node_index(&nodes, request.goal).expect("goal node should exist");

        let mut search_stats = DijkstraSearchStats::default();
        let search = match run_dijkstra(
            &adjacency,
            start_index,
            goal_index,
            &mut search_stats,
            &watch,
        ) {
            Ok(outcome) => outcome,
            Err(reason) => {
                return (Err(crate::any_angle::budget_error(reason)), diagnostics);
            }
        };
        diagnostics.absorb_search(search_stats);

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

        let mut path_points = reconstruct_path(&nodes, &predecessors, goal_index);
        elide_collinear_points(grid, &mut path_points);
        diagnostics.path_point_count = path_points.len();

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

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

/// All-grid-vertex reference oracle using the independent sampling legality predicate.
#[derive(Debug, Clone, Copy, Default)]
pub struct AnyAngleSamplingReferenceOracle;

impl AnyAngleSamplingReferenceOracle {
    /// Runs one sampling-predicate oracle search (independent of corner-only VG).
    pub fn search(&self, grid: &Grid, request: AnyAngleSearchRequest) -> AnyAngleSearchResult {
        run_vertex_graph_oracle(
            grid,
            request,
            sampling_segment_is_legal,
            validated_sampling_any_angle_path,
            elide_collinear_sampling_points,
        )
    }
}

fn run_vertex_graph_oracle(
    grid: &Grid,
    request: AnyAngleSearchRequest,
    segment_legal: fn(&Grid, Point2, Point2) -> bool,
    build_path: fn(&Grid, Vec<Point2>) -> Result<crate::any_angle::AnyAnglePath, ()>,
    elide_collinear: fn(&Grid, &mut Vec<Point2>),
) -> AnyAngleSearchResult {
    let Some(start) = canonicalize_grid_vertex(request.start) else {
        return Err(AnyAngleSearchError::InvalidStart {
            point: request.start,
        });
    };
    let Some(goal) = canonicalize_grid_vertex(request.goal) else {
        return Err(AnyAngleSearchError::InvalidGoal {
            point: request.goal,
        });
    };
    if !is_endpoint_valid(grid, start) {
        return Err(AnyAngleSearchError::InvalidStart {
            point: request.start,
        });
    }
    if !is_endpoint_valid(grid, goal) {
        return Err(AnyAngleSearchError::InvalidGoal {
            point: request.goal,
        });
    }
    let budget = request.budget;
    let request = AnyAngleSearchRequest::new(start, goal).with_budget(budget);
    let watch = crate::search::BudgetWatch::start(budget);

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

    let mut nodes = Vec::new();
    for vy in 0..=grid.height() {
        for vx in 0..=grid.width() {
            let point = Point2::new(vx as f64, vy as f64);
            if is_endpoint_valid(grid, point) {
                nodes.push(point);
            }
        }
    }

    let mut unused_build_stats = VisibilityGraphBuildStats::default();
    let adjacency =
        build_visibility_adjacency(grid, &nodes, segment_legal, &mut unused_build_stats);

    let start_index = node_index(&nodes, request.start).expect("start node should exist");
    let goal_index = node_index(&nodes, request.goal).expect("goal node should exist");

    let mut search_stats = DijkstraSearchStats::default();
    let search = match run_dijkstra(
        &adjacency,
        start_index,
        goal_index,
        &mut search_stats,
        &watch,
    ) {
        Ok(outcome) => outcome,
        Err(reason) => return Err(crate::any_angle::budget_error(reason)),
    };

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

    let mut path_points = reconstruct_path(&nodes, &predecessors, goal_index);
    elide_collinear(grid, &mut path_points);
    let Ok(path) = build_path(grid, path_points) else {
        return Ok(SearchOutcome::no_path(AnyAngleSearchStats {
            visited_nodes: search_stats.settled_nodes,
        }));
    };
    if !path_endpoints_match_request(&path, request) {
        return Ok(SearchOutcome::no_path(AnyAngleSearchStats {
            visited_nodes: search_stats.settled_nodes,
        }));
    }

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

fn validated_sampling_any_angle_path(
    grid: &Grid,
    points: Vec<Point2>,
) -> Result<crate::any_angle::AnyAnglePath, ()> {
    if points.is_empty() {
        return Err(());
    }
    if !crate::any_angle::geometry::validate_sampling_path(grid, &points) {
        return Err(());
    }
    crate::any_angle::AnyAnglePath::from_points(points).map_err(|_| ())
}

fn elide_collinear_sampling_points(grid: &Grid, points: &mut Vec<Point2>) {
    elide_collinear_with_predicate(grid, points, sampling_segment_is_legal);
}

#[cfg(test)]
mod tests {
    use super::*;
    use crate::Grid;

    #[test]
    fn wall_detour_endpoints_are_preserved() {
        let mut grid = Grid::new(10, 10).expect("grid");
        for x in 0..8 {
            grid.set_cell(crate::Point::new(x, 5), crate::grid::Cell::Blocked)
                .expect("block");
        }
        let request = AnyAngleSearchRequest::new(Point2::new(0.0, 0.0), Point2::new(0.0, 9.0));
        let (result, _) = AnyAngleVisibilityGraphOracle.search_with_diagnostics(&grid, request);
        let result = result.expect("valid request");
        assert!(
            result.is_found(),
            "wall detour should be reachable: {result:?}"
        );
        let path = result.path().expect("path");
        assert_eq!(path.points().first(), Some(&request.start));
        assert_eq!(path.points().last(), Some(&request.goal));
    }

    #[test]
    fn finite_2x2_vertex_pairs_are_oracle_connected() {
        let oracle = AnyAngleVisibilityGraphOracle;
        for mask in 0u16..(1 << 4) {
            let mut grid = Grid::new(2, 2).expect("grid");
            for y in 0..2 {
                for x in 0..2 {
                    if mask & (1 << (y * 2 + x)) != 0 {
                        grid.set_cell(crate::Point::new(x, y), crate::grid::Cell::Blocked)
                            .expect("block");
                    }
                }
            }
            for sy in 0..=2 {
                for sx in 0..=2 {
                    for gy in 0..=2 {
                        for gx in 0..=2 {
                            if sx == gx && sy == gy {
                                continue;
                            }
                            let request = AnyAngleSearchRequest::new(
                                Point2::new(sx as f64, sy as f64),
                                Point2::new(gx as f64, gy as f64),
                            );
                            let (result, _) = oracle.search_with_diagnostics(&grid, request);
                            let result = result.expect("valid request");
                            assert!(
                                result.is_found(),
                                "mask={mask:04b} ({sx},{sy})->({gx},{gy}) should be reachable"
                            );
                        }
                    }
                }
            }
        }
    }

    #[test]
    fn open_field_corner_oracle_finds_diagonal() {
        let grid = Grid::new(10, 10).expect("grid");
        let request = AnyAngleSearchRequest::new(Point2::new(0.0, 0.0), Point2::new(9.0, 9.0));
        let (result, _) = AnyAngleVisibilityGraphOracle.search_with_diagnostics(&grid, request);
        let result = result.expect("valid request");
        assert!(result.is_found(), "open field should be reachable");
        assert!(approximately_equal(
            result.path().expect("path").cost(),
            9.0 * 2.0_f64.sqrt(),
        ));
    }
}