condor-pathfinding-geometry 0.4.0

//! Private candidate: prepared visibility graph with exact endpoint overlay.
//!
//! # Current implementation
//!
//! Prepares **full** undirected walkable edges among obstacle vertices (not a
//! pruned tangent/reflex subset), then overlays start/goal with complete
//! incidence. Name retains “tangent” for portfolio identity; true tangent
//! reduction is future work. Exact cost matches
//! [`crate::visibility_graph::VisibilityGraph`] under the same predicates.
//!
//! # Hypothesis and contract
//!
//! A scene-wide tangent/reflex graph prepared once, followed by exact
//! start/goal overlays, can reduce repeated arbitrary-pair polygon queries
//! without weakening the Euclidean free-space contract.
//!
//! The prepared graph is an immutable validated scene snapshot. Each endpoint
//! overlay must still validate endpoints and include every necessary direct or
//! tangent connection; an incomplete overlay is not permission to report a
//! best-known route as exact.
//!
//! # Non-negotiable constraints
//!
//! - Query cost and witness must match the established exact polygon solver
//!   under Condor's floating walkability predicates.
//! - Changing obstacles, bounds, or endpoint validation rules requires a new
//!   prepare step; no cached edge may survive across scene identity.
//! - Preprocess, overlay, query, and fallback work all count toward a claimed
//!   repeated-query improvement.
//!
//! # Evidence and promotion
//!
//! Use the ordinary continuous owner route while implementing:
//! `just test-fast continuous` and `just clippy-target continuous`. Before a
//! public decision require exact geometry conformance
//! (`just test-geometry-conformance`) and reproducible repeated-query evidence
//! through `just bench-continuous-core`; use `just bench-continuous-stress` for
//! stress workload claims. This candidate integrates into those existing lanes:
//! it creates no feature, fixture, target, or separate harness route.

use std::cmp::Ordering;
use std::collections::BinaryHeap;

use crate::continuous::{PolygonPath, PolygonSearchResult};
use crate::polygonal::{Point2, PolygonScene, PolygonSearchRequest, PolygonValidationError};

const EPSILON: f64 = 1e-9;

/// Prepares an immutable scene-wide tangent graph for exact pair overlays.
///
/// Custom prepare + query API (not a new public trait):
/// - [`Self::prepare`] builds obstacle-vertex visibility once.
/// - [`PreparedTangentOverlay::query`] overlays start/goal and runs exact
///   Dijkstra, matching [`crate::visibility_graph::VisibilityGraph`] costs.
#[derive(Debug, Clone, Copy, Default)]
pub struct VisibilityGraphPreparedTangentOverlayBuilder;

impl VisibilityGraphPreparedTangentOverlayBuilder {
    /// Stable portfolio identity for prepared arbitrary-pair polygon queries.
    pub const CANDIDATE_ID: &str = "repeated-polygonal-pair/prepared-tangent-overlay";

    /// Builder identity token.
    #[must_use]
    pub const fn name(&self) -> &'static str {
        "prepared-tangent-overlay"
    }

    /// Snapshot `scene` and materialize undirected walkable edges among obstacle vertices.
    ///
    /// # Errors
    ///
    /// Returns [`PolygonValidationError`] when static scene geometry fails validation.
    pub fn prepare(
        &self,
        scene: &PolygonScene,
    ) -> Result<PreparedTangentOverlay, PolygonValidationError> {
        scene.validate_static()?;

        let nodes = collect_obstacle_nodes(scene);
        let adjacency = build_visibility_edges(scene, &nodes);

        Ok(PreparedTangentOverlay {
            scene: scene.clone(),
            nodes,
            adjacency,
        })
    }
}

/// Immutable prepared tangent graph + exact start/goal overlay queries.
#[derive(Debug, Clone)]
pub struct PreparedTangentOverlay {
    scene: PolygonScene,
    /// Obstacle vertices only (no free-space endpoints until overlay).
    nodes: Vec<Point2>,
    adjacency: Vec<Vec<(usize, f64)>>,
}

impl PreparedTangentOverlay {
    /// Builder / map identity token.
    #[must_use]
    pub const fn name(&self) -> &'static str {
        "prepared-tangent-overlay"
    }

    /// Exact free-space path for one start–goal pair via endpoint overlay.
    ///
    /// Validates endpoints with the same walkability / sealed-boundary rules as
    /// the online visibility graph, overlays both points onto the prepared
    /// tangent graph, and runs Dijkstra for the exact Euclidean polyline.
    pub fn query(&self, start: Point2, goal: Point2) -> PolygonSearchResult {
        if !self.scene.is_walkable(start) {
            return Err(crate::continuous::PolygonSearchError::InvalidStart { point: start });
        }
        if !self.scene.is_walkable(goal) {
            return Err(crate::continuous::PolygonSearchError::InvalidGoal { point: goal });
        }

        let request = PolygonSearchRequest::new(start, goal);
        if self.scene.validate(request).is_err() {
            return crate::continuous::not_found(0);
        }

        if points_equal(start, goal) {
            return crate::continuous::found(
                PolygonPath::from_points(vec![start])
                    .expect("polygon path contains at least one point"),
                1,
            );
        }

        let (overlay_nodes, overlay_adjacency) = overlay_endpoints(self, start, goal);
        let (cost, predecessors, visited_nodes) =
            match shortest_path(&overlay_adjacency, 0, 1, request.budget) {
                Ok(outcome) => outcome,
                Err(reason) => return Err(crate::continuous::budget_error(reason)),
            };

        match cost {
            Some(goal_cost) => {
                let points = reconstruct_path(&overlay_nodes, &predecessors, 1);
                crate::continuous::found(
                    PolygonPath::from_points_with_cost(points, goal_cost)
                        .expect("polygon path contains at least one point"),
                    visited_nodes,
                )
            }
            None => crate::continuous::not_found(visited_nodes),
        }
    }
}

fn collect_obstacle_nodes(scene: &PolygonScene) -> Vec<Point2> {
    let mut nodes = Vec::new();
    for obstacle in &scene.obstacles {
        for &vertex in obstacle.vertices() {
            if !nodes.iter().any(|point| points_equal(*point, vertex)) {
                nodes.push(vertex);
            }
        }
    }
    nodes
}

fn build_visibility_edges(scene: &PolygonScene, nodes: &[Point2]) -> Vec<Vec<(usize, f64)>> {
    let mut adjacency = vec![Vec::new(); nodes.len()];

    for left_index in 0..nodes.len() {
        for right_index in (left_index + 1)..nodes.len() {
            let start = nodes[left_index];
            let end = nodes[right_index];
            if scene.segment_is_walkable(start, end) {
                let cost = start.distance_to(end);
                adjacency[left_index].push((right_index, cost));
                adjacency[right_index].push((left_index, cost));
            }
        }
    }

    adjacency
}

/// Overlay start (index 0) and goal (index 1) onto the prepared vertex graph.
///
/// Prepared nodes are shifted by +2. Every walkable connection from start/goal
/// to prepared vertices (and the direct start–goal edge) is included so the
/// overlay cannot hide a walkable exact witness.
fn overlay_endpoints(
    prepared: &PreparedTangentOverlay,
    start: Point2,
    goal: Point2,
) -> (Vec<Point2>, Vec<Vec<(usize, f64)>>) {
    let prepared_count = prepared.nodes.len();
    let mut nodes = Vec::with_capacity(prepared_count + 2);
    nodes.push(start);
    nodes.push(goal);
    nodes.extend_from_slice(&prepared.nodes);

    let mut adjacency = vec![Vec::new(); nodes.len()];

    // Shift prepared edges by +2.
    for (old_index, neighbors) in prepared.adjacency.iter().enumerate() {
        let new_index = old_index + 2;
        for &(neighbor, cost) in neighbors {
            adjacency[new_index].push((neighbor + 2, cost));
        }
    }

    // Exact overlay: start and goal to every prepared vertex (and each other).
    for right in 1..nodes.len() {
        if prepared.scene.segment_is_walkable(nodes[0], nodes[right]) {
            let cost = nodes[0].distance_to(nodes[right]);
            adjacency[0].push((right, cost));
            adjacency[right].push((0, cost));
        }
    }
    for right in 2..nodes.len() {
        if prepared.scene.segment_is_walkable(nodes[1], nodes[right]) {
            let cost = nodes[1].distance_to(nodes[right]);
            adjacency[1].push((right, cost));
            adjacency[right].push((1, cost));
        }
    }

    (nodes, adjacency)
}

type ShortestPathOutcome = (Option<f64>, Vec<Option<usize>>, usize);

fn shortest_path(
    adjacency: &[Vec<(usize, f64)>],
    start_index: usize,
    goal_index: usize,
    budget: condor_core::SearchBudget,
) -> Result<ShortestPathOutcome, condor_core::BudgetExhausted> {
    let mut distances = vec![f64::INFINITY; adjacency.len()];
    let mut predecessors = vec![None; adjacency.len()];
    let mut closed = vec![false; adjacency.len()];
    let mut frontier = BinaryHeap::new();
    let mut visited_nodes = 0usize;
    let watch = condor_core::BudgetWatch::start(budget);

    distances[start_index] = 0.0;
    frontier.push(HeapEntry {
        node_index: start_index,
        cost: 0.0,
    });

    while let Some(entry) = frontier.pop() {
        if closed[entry.node_index] {
            continue;
        }

        closed[entry.node_index] = true;
        visited_nodes += 1;

        if entry.node_index == goal_index {
            return Ok((Some(entry.cost), predecessors, visited_nodes));
        }

        watch.check(visited_nodes)?;

        for &(neighbor_index, edge_cost) in &adjacency[entry.node_index] {
            if closed[neighbor_index] {
                continue;
            }

            let next_cost = entry.cost + edge_cost;
            if next_cost + EPSILON < distances[neighbor_index] {
                distances[neighbor_index] = next_cost;
                predecessors[neighbor_index] = Some(entry.node_index);
                frontier.push(HeapEntry {
                    node_index: neighbor_index,
                    cost: next_cost,
                });
            }
        }
    }

    Ok((None, predecessors, visited_nodes))
}

fn reconstruct_path(
    nodes: &[Point2],
    predecessors: &[Option<usize>],
    goal_index: usize,
) -> Vec<Point2> {
    let mut reversed = Vec::new();
    let mut current = Some(goal_index);

    while let Some(index) = current {
        reversed.push(nodes[index]);
        current = predecessors[index];
    }

    reversed.reverse();
    reversed
}

fn points_equal(left: Point2, right: Point2) -> bool {
    (left.x - right.x).abs() <= EPSILON && (left.y - right.y).abs() <= EPSILON
}

#[derive(Debug, Clone, Copy, PartialEq)]
struct HeapEntry {
    node_index: usize,
    cost: f64,
}

impl Eq for HeapEntry {}

impl Ord for HeapEntry {
    fn cmp(&self, other: &Self) -> Ordering {
        other
            .cost
            .total_cmp(&self.cost)
            .then_with(|| other.node_index.cmp(&self.node_index))
    }
}

impl PartialOrd for HeapEntry {
    fn partial_cmp(&self, other: &Self) -> Option<Ordering> {
        Some(self.cmp(other))
    }
}

#[cfg(test)]
mod tests {
    use super::VisibilityGraphPreparedTangentOverlayBuilder;
    use crate::continuous::PolygonPathfinder;
    use crate::polygonal::{Point2, Polygon, PolygonScene, PolygonSearchRequest, WorldBounds};
    use crate::visibility_graph::VisibilityGraph;

    #[test]
    fn query_parity_vs_visibility_graph() {
        let scene = PolygonScene {
            world_bounds: WorldBounds::new(Point2::new(0.0, 0.0), Point2::new(12.0, 12.0)),
            obstacles: vec![Polygon::new(vec![
                Point2::new(4.0, 4.0),
                Point2::new(6.0, 4.0),
                Point2::new(6.0, 8.0),
                Point2::new(4.0, 8.0),
            ])],
        };
        let builder = VisibilityGraphPreparedTangentOverlayBuilder;
        let prepared = builder.prepare(&scene).expect("scene should prepare");

        assert_eq!(
            VisibilityGraphPreparedTangentOverlayBuilder::CANDIDATE_ID,
            "repeated-polygonal-pair/prepared-tangent-overlay"
        );
        assert_eq!(builder.name(), "prepared-tangent-overlay");
        assert_eq!(prepared.name(), "prepared-tangent-overlay");

        for (start, goal) in [
            (Point2::new(1.0, 1.0), Point2::new(11.0, 1.0)),
            (Point2::new(1.0, 5.0), Point2::new(11.0, 5.0)),
            (Point2::new(1.0, 1.0), Point2::new(11.0, 11.0)),
            (Point2::new(2.0, 2.0), Point2::new(2.0, 2.0)),
        ] {
            let candidate = prepared.query(start, goal).expect("valid endpoints");
            let baseline = VisibilityGraph
                .search(&scene, PolygonSearchRequest::new(start, goal))
                .expect("valid endpoints");

            assert_eq!(
                candidate.is_found(),
                baseline.is_found(),
                "found parity for {start:?} → {goal:?}"
            );
            match (candidate.cost(), baseline.cost()) {
                (Some(left), Some(right)) => {
                    assert!(
                        (left - right).abs() <= 1e-9,
                        "cost parity for {start:?} → {goal:?}: {left} vs {right}"
                    );
                }
                (None, None) => {}
                other => panic!("cost shape mismatch for {start:?} → {goal:?}: {other:?}"),
            }
        }
    }

    #[test]
    fn no_path_for_separator() {
        let scene = PolygonScene {
            world_bounds: WorldBounds::new(Point2::new(0.0, 0.0), Point2::new(10.0, 10.0)),
            obstacles: vec![Polygon::new(vec![
                Point2::new(4.0, 0.0),
                Point2::new(6.0, 0.0),
                Point2::new(6.0, 10.0),
                Point2::new(4.0, 10.0),
            ])],
        };
        let prepared = VisibilityGraphPreparedTangentOverlayBuilder
            .prepare(&scene)
            .expect("scene should prepare");

        let result = prepared
            .query(Point2::new(2.0, 5.0), Point2::new(8.0, 5.0))
            .expect("valid endpoints");

        assert!(!result.is_found());
        assert!(result.path().is_none());
        assert_eq!(result.cost(), None);
        assert_eq!(
            VisibilityGraphPreparedTangentOverlayBuilder::CANDIDATE_ID,
            "repeated-polygonal-pair/prepared-tangent-overlay"
        );
    }

    #[test]
    fn open_space_found_cost_parity() {
        let scene = PolygonScene {
            world_bounds: WorldBounds::new(Point2::new(0.0, 0.0), Point2::new(10.0, 10.0)),
            obstacles: Vec::new(),
        };
        let prepared = VisibilityGraphPreparedTangentOverlayBuilder
            .prepare(&scene)
            .expect("open scene should prepare");
        let start = Point2::new(1.0, 1.0);
        let goal = Point2::new(9.0, 1.0);

        let candidate = prepared.query(start, goal).expect("valid endpoints");
        let baseline = VisibilityGraph
            .search(&scene, PolygonSearchRequest::new(start, goal))
            .expect("valid endpoints");

        assert!(candidate.is_found());
        assert!(baseline.is_found());
        assert!((candidate.cost().expect("cost") - baseline.cost().expect("cost")).abs() <= 1e-9);
    }
}