condor-pathfinding-geometry 0.4.0

//! Private candidate: indexed lazy reflex-visibility search for exact polygonal scenes.
//!
//! # Hypothesis and contract
//!
//! A conservative visibility index plus lazy reflex-edge generation can reduce
//! online graph construction while preserving [`crate::visibility_graph::VisibilityGraph`]
//! exactness under Condor's floating geometry predicates and the
//! [`crate::continuous::PolygonPathfinder`] endpoint contract.
//!
//! This is not a prepared public surface: it remains an online pair-search
//! candidate, is neither constructed nor re-exported, and may be discarded.
//!
//! # Non-negotiable constraints
//!
//! - Index rejection must be conservative: it must never hide a walkable edge.
//! - Invalid endpoints remain typed errors; valid but disconnected endpoints
//!   remain no-path, never a panic or a fabricated witness.
//! - Fewer visited nodes is not promotion evidence. Historical comparisons show
//!   that node count does not reliably predict runtime across solver families.
//!
//! # Evidence and promotion
//!
//! Implement the candidate beside the ordinary geometry owner route:
//! `just test-fast continuous` and `just clippy-target continuous`. Before a
//! public decision, also require geometry-corpus conformance through
//! `just test-geometry-conformance` and reproducible continuous benchmark
//! evidence through `just bench-continuous-core` and, for stress workload claims,
//! `just bench-continuous-stress`. This candidate must first be integrated into
//! those existing benchmark lanes; it does not create its own. Promotion requires
//! exact-cost/witness parity with the established solver contract and a
//! reproducible runtime win; one-shot captures do not establish either claim.

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

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

const EPSILON: f64 = 1e-9;

/// Online exact pathfinder: lazy per-node visibility edges + Dijkstra.
///
/// Edges are generated only when a node is expanded. A uniform spatial bucket
/// index orders neighbor candidates; it never rejects a pair that might still
/// be walkable (rejection would require a proven blocked segment, which this
/// index does not claim). Exact cost matches
/// [`crate::visibility_graph::VisibilityGraph`] under the same predicates.
#[derive(Debug, Clone, Copy, Default)]
pub struct VisibilityGraphLazyIndexed;

impl VisibilityGraphLazyIndexed {
    /// Stable portfolio id for the exact polygonal-scene family.
    pub const CANDIDATE_ID: &str = "exact-polygonal-scene/indexed-lazy-reflex-vg";
}

impl PolygonPathfinder for VisibilityGraphLazyIndexed {
    fn name(&self) -> &'static str {
        "vg-lazy-indexed"
    }

    fn search(&self, scene: &PolygonScene, request: PolygonSearchRequest) -> PolygonSearchResult {
        if !scene.is_walkable(request.start) {
            return Err(crate::continuous::PolygonSearchError::InvalidStart {
                point: request.start,
            });
        }
        if !scene.is_walkable(request.goal) {
            return Err(crate::continuous::PolygonSearchError::InvalidGoal {
                point: request.goal,
            });
        }
        if scene.validate(request).is_err() {
            return crate::continuous::not_found(0);
        }

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

        let nodes = collect_nodes(scene, request);
        let index = SpatialIndex::build(&nodes);
        let (cost, predecessors, visited_nodes) =
            match lazy_shortest_path(scene, &nodes, &index, 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(&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),
        }
    }
}

/// Enumerate every undirected walkable edge the lazy solver would admit.
///
/// Used by unit tests to prove the index never hides a walkable edge relative
/// to the full pairwise visibility predicate.
fn enumerate_walkable_edges(scene: &PolygonScene, nodes: &[Point2]) -> Vec<(usize, usize)> {
    let index = SpatialIndex::build(nodes);
    let mut edges = Vec::new();
    for left in 0..nodes.len() {
        for right in index.candidate_neighbors(left, nodes.len()) {
            if right <= left {
                continue;
            }
            if scene.segment_is_walkable(nodes[left], nodes[right]) {
                edges.push((left, right));
            }
        }
    }
    edges.sort_unstable();
    edges
}

fn full_pairwise_walkable_edges(scene: &PolygonScene, nodes: &[Point2]) -> Vec<(usize, usize)> {
    let mut edges = Vec::new();
    for left in 0..nodes.len() {
        for right in (left + 1)..nodes.len() {
            if scene.segment_is_walkable(nodes[left], nodes[right]) {
                edges.push((left, right));
            }
        }
    }
    edges
}

fn collect_nodes(scene: &PolygonScene, request: PolygonSearchRequest) -> Vec<Point2> {
    let mut nodes = vec![request.start, request.goal];
    for obstacle in &scene.obstacles {
        for &vertex in obstacle.vertices() {
            if !nodes.iter().any(|point| points_equal(*point, vertex)) {
                nodes.push(vertex);
            }
        }
    }
    nodes
}

/// Uniform grid over node positions. Used only to order/group candidates; every
/// other node remains a candidate for a walkability check.
struct SpatialIndex {
    /// For each node, bucket key `(bx, by)`.
    buckets: Vec<(i32, i32)>,
    /// Map from bucket key to node indices in that cell.
    cells: std::collections::HashMap<(i32, i32), Vec<usize>>,
    cell_size: f64,
}

impl SpatialIndex {
    fn build(nodes: &[Point2]) -> Self {
        let cell_size = choose_cell_size(nodes);
        let mut cells: std::collections::HashMap<(i32, i32), Vec<usize>> =
            std::collections::HashMap::new();
        let mut buckets = Vec::with_capacity(nodes.len());
        for (index, point) in nodes.iter().enumerate() {
            let key = bucket_key(*point, cell_size);
            buckets.push(key);
            cells.entry(key).or_default().push(index);
        }
        Self {
            buckets,
            cells,
            cell_size,
        }
    }

    /// Yield every node index other than `from` (conservative: full set).
    ///
    /// Neighbors in nearby buckets are yielded first so lazy expansion prefers
    /// local geometry; remote nodes are still included so no walkable edge is
    /// hidden by the index.
    fn candidate_neighbors(&self, from: usize, node_count: usize) -> Vec<usize> {
        let (bx, by) = self.buckets[from];
        let mut ordered = Vec::with_capacity(node_count.saturating_sub(1));
        let mut seen = vec![false; node_count];
        seen[from] = true;

        // Near buckets first (3×3 neighborhood around the source cell).
        for dy in -1..=1 {
            for dx in -1..=1 {
                if let Some(members) = self.cells.get(&(bx + dx, by + dy)) {
                    for &index in members {
                        if !seen[index] {
                            seen[index] = true;
                            ordered.push(index);
                        }
                    }
                }
            }
        }

        // Remaining nodes (remote buckets) — never omitted.
        for (index, already) in seen.iter().enumerate() {
            if !*already {
                ordered.push(index);
            }
        }
        ordered
    }

    #[allow(dead_code)]
    fn cell_size(&self) -> f64 {
        self.cell_size
    }
}

fn choose_cell_size(nodes: &[Point2]) -> f64 {
    if nodes.len() < 2 {
        return 1.0;
    }
    let mut min_x = f64::INFINITY;
    let mut max_x = f64::NEG_INFINITY;
    let mut min_y = f64::INFINITY;
    let mut max_y = f64::NEG_INFINITY;
    for point in nodes {
        min_x = min_x.min(point.x);
        max_x = max_x.max(point.x);
        min_y = min_y.min(point.y);
        max_y = max_y.max(point.y);
    }
    let span = (max_x - min_x).max(max_y - min_y).max(1.0);
    (span / (nodes.len() as f64).sqrt()).max(EPSILON)
}

fn bucket_key(point: Point2, cell_size: f64) -> (i32, i32) {
    (
        (point.x / cell_size).floor() as i32,
        (point.y / cell_size).floor() as i32,
    )
}

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

fn lazy_shortest_path(
    scene: &PolygonScene,
    nodes: &[Point2],
    index: &SpatialIndex,
    start_index: usize,
    goal_index: usize,
    budget: condor_core::SearchBudget,
) -> Result<ShortestPathOutcome, condor_core::BudgetExhausted> {
    let mut distances = vec![f64::INFINITY; nodes.len()];
    let mut predecessors = vec![None; nodes.len()];
    let mut closed = vec![false; nodes.len()];
    let mut generated = vec![false; nodes.len()];
    let mut adjacency: Vec<Vec<(usize, f64)>> = vec![Vec::new(); nodes.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)?;

        if !generated[entry.node_index] {
            generated[entry.node_index] = true;
            let neighbors = index.candidate_neighbors(entry.node_index, nodes.len());
            for neighbor_index in neighbors {
                if closed[neighbor_index] {
                    continue;
                }
                let start = nodes[entry.node_index];
                let end = nodes[neighbor_index];
                if scene.segment_is_walkable(start, end) {
                    let cost = start.distance_to(end);
                    adjacency[entry.node_index].push((neighbor_index, cost));
                }
            }
        }

        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::{
        VisibilityGraphLazyIndexed, collect_nodes, enumerate_walkable_edges,
        full_pairwise_walkable_edges,
    };
    use crate::continuous::PolygonPathfinder;
    use crate::polygonal::{Point2, Polygon, PolygonScene, PolygonSearchRequest, WorldBounds};
    use crate::visibility_graph::VisibilityGraph;

    fn open_scene() -> PolygonScene {
        PolygonScene {
            world_bounds: WorldBounds::new(Point2::new(0.0, 0.0), Point2::new(10.0, 10.0)),
            obstacles: Vec::new(),
        }
    }

    fn separator_scene() -> PolygonScene {
        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),
            ])],
        }
    }

    #[test]
    fn open_space_found_cost_parity_vs_visibility_graph() {
        let scene = open_scene();
        let request = PolygonSearchRequest::new(Point2::new(1.0, 1.0), Point2::new(9.0, 1.0));

        let candidate = VisibilityGraphLazyIndexed
            .search(&scene, request)
            .expect("valid search request");
        let baseline = VisibilityGraph
            .search(&scene, request)
            .expect("valid search request");

        assert!(candidate.is_found());
        assert!(baseline.is_found());
        let candidate_cost = candidate.cost().expect("found path cost");
        let baseline_cost = baseline.cost().expect("found path cost");
        assert!((candidate_cost - baseline_cost).abs() <= 1e-9);
        assert!((candidate_cost - 8.0).abs() <= 1e-9);
        assert_eq!(
            VisibilityGraphLazyIndexed::CANDIDATE_ID,
            "exact-polygonal-scene/indexed-lazy-reflex-vg"
        );
        assert_eq!(VisibilityGraphLazyIndexed.name(), "vg-lazy-indexed");
    }

    #[test]
    fn separator_no_path() {
        let scene = separator_scene();
        let request = PolygonSearchRequest::new(Point2::new(2.0, 5.0), Point2::new(8.0, 5.0));

        let result = VisibilityGraphLazyIndexed
            .search(&scene, request)
            .expect("valid search request");

        assert!(!result.is_found());
        assert!(result.path().is_none());
        assert_eq!(result.cost(), None);
    }

    #[test]
    fn never_hides_walkable_edge() {
        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(3.0, 3.0),
                    Point2::new(5.0, 3.0),
                    Point2::new(5.0, 6.0),
                    Point2::new(3.0, 6.0),
                ]),
                Polygon::new(vec![
                    Point2::new(7.0, 5.0),
                    Point2::new(9.0, 5.0),
                    Point2::new(9.0, 9.0),
                    Point2::new(7.0, 9.0),
                ]),
            ],
        };
        let request = PolygonSearchRequest::new(Point2::new(1.0, 1.0), Point2::new(11.0, 11.0));
        let nodes = collect_nodes(&scene, request);

        let full = full_pairwise_walkable_edges(&scene, &nodes);
        let lazy = enumerate_walkable_edges(&scene, &nodes);

        assert_eq!(
            lazy, full,
            "lazy index must admit every walkable visibility edge"
        );
        assert!(
            !full.is_empty(),
            "fixture should contain at least one walkable edge"
        );
    }
}