condor-pathfinding-geometry 0.4.0

//! Private candidate: taut/reflex reduction with streamed visibility construction.
//!
//! # Current implementation (scaffold)
//!
//! Full visibility graph with **bend-first node ordering**. Non-bend vertices are
//! still retained (no vertex omission). Edges are still eagerly all-pairs before
//! Dijkstra — “streaming” is insertion order, not deferred materialization.
//! Exact cost parity with [`crate::visibility_graph::VisibilityGraph`] holds
//! because the graph is complete, not because reduction is proven.
//!
//! # Invariants
//!
//! - A vertex or edge may be omitted only when it cannot participate in any
//!   shortest legal free-space witness under Condor's walkability predicates.
//! - Streaming order and deduplication must not suppress a required visibility
//!   edge; diagnostics must not depend on incidental hash iteration order.
//! - Exact Euclidean cost, legal polyline witness, typed endpoint errors, and
//!   no-path behavior match the online exact solvers
//!   ([`crate::visibility_graph::VisibilityGraph`],
//!   [`crate::topological_fracture_search::TopologicalFractureSearch`]).
//! - Peak retained graph state and geometry checks both count toward any
//!   claimed improvement; fewer materialized vertices alone is not promotion.
//!
//! # Hypothesis and contract
//!
//! Restricting exact graph construction to provably relevant taut/reflex
//! vertices and streaming edges as needed can reduce peak graph materialization
//! for online polygon searches. Free-space bend vertices of obstacle rings
//! (convex corners of the free-space boundary) are streamed first; non-bend
//! vertices are retained when their eligibility proof is incomplete so that
//! walkable edges are never hidden.
//!
//! # Evidence and promotion
//!
//! Use `just test-fast continuous` and `just clippy-target continuous` while
//! implementing. Promotion requires `just test-geometry-conformance` plus
//! reproducible end-to-end evidence through `just bench-continuous-core`; use
//! `just bench-continuous-stress` for stress claims. This candidate creates no
//! feature, fixture, target, or separate harness route.

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

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

const EPSILON: f64 = 1e-9;

/// Online exact pathfinder: taut/reflex-prioritized streamed visibility + Dijkstra.
///
/// Edges are streamed in deterministic phases (start–goal, endpoints↔vertices,
/// vertex↔vertex) so peak adjacency growth is ordered. Exact cost matches
/// [`crate::visibility_graph::VisibilityGraph`].
#[derive(Debug, Clone, Copy, Default)]
pub struct VisibilityGraphTautReflexStreaming;

impl VisibilityGraphTautReflexStreaming {
    /// Stable portfolio identity for online streamed exact graph construction.
    pub const CANDIDATE_ID: &str = "exact-polygonal-scene/taut-reflex-streaming-graph";
}

impl PolygonPathfinder for VisibilityGraphTautReflexStreaming {
    fn name(&self) -> &'static str {
        "vg-taut-reflex-streaming"
    }

    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_streamed_nodes(scene, request);
        let adjacency = stream_visibility_edges(scene, &nodes);
        let (cost, predecessors, visited_nodes) =
            match shortest_path(&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(&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),
        }
    }
}

/// Collect start, goal, free-space bend vertices first, then remaining vertices.
///
/// Bend vertices (convex free-space boundary corners) are the only intermediate
/// vertices an optimal exterior path can need. Remaining obstacle vertices are
/// still retained: floating-predicate eligibility is incomplete without a full
/// formal proof, so the reduction stays conservative and never hides a node the
/// baseline visibility graph would use.
fn collect_streamed_nodes(scene: &PolygonScene, request: PolygonSearchRequest) -> Vec<Point2> {
    let mut nodes = vec![request.start, request.goal];
    let mut bend = Vec::new();
    let mut retained = Vec::new();

    for obstacle in &scene.obstacles {
        for (vertex_index, &vertex) in obstacle.vertices().iter().enumerate() {
            if nodes.iter().any(|point| points_equal(*point, vertex))
                || bend.iter().any(|point| points_equal(*point, vertex))
                || retained.iter().any(|point| points_equal(*point, vertex))
            {
                continue;
            }
            if is_free_space_bend_vertex(obstacle, vertex_index) {
                bend.push(vertex);
            } else {
                // Conservative retention: incomplete eligibility proof.
                retained.push(vertex);
            }
        }
    }

    // Deterministic order: bends first (taut/reflex priority), then retained.
    nodes.extend(bend);
    nodes.extend(retained);
    nodes
}

/// Free-space bend candidate: convex corner of the free-space boundary.
///
/// For a CCW obstacle ring (positive area), a left turn at the vertex is a
/// convex obstacle corner and a free-space reflex bend site. CW rings reverse
/// the test. Near-collinear vertices are treated as bend sites (conservative).
fn is_free_space_bend_vertex(obstacle: &Polygon, vertex_index: usize) -> bool {
    let vertices = obstacle.vertices();
    let count = vertices.len();
    if count < 3 {
        return true;
    }

    let prev = vertices[(vertex_index + count - 1) % count];
    let curr = vertices[vertex_index];
    let next = vertices[(vertex_index + 1) % count];
    let cross = ((curr.x - prev.x) * (next.y - curr.y)) - ((curr.y - prev.y) * (next.x - curr.x));
    let area = obstacle.signed_area();

    if area >= 0.0 {
        cross >= -EPSILON
    } else {
        cross <= EPSILON
    }
}

/// Stream undirected walkable edges in fixed phases (deterministic, not hash-order).
fn stream_visibility_edges(scene: &PolygonScene, nodes: &[Point2]) -> Vec<Vec<(usize, f64)>> {
    let mut adjacency = vec![Vec::new(); nodes.len()];
    let mut seen = std::collections::HashSet::new();

    // Phase 0: direct start–goal.
    try_stream_edge(scene, nodes, 0, 1, &mut adjacency, &mut seen);

    // Phase 1: endpoints to every other node (tangents / direct attachments).
    for other in 2..nodes.len() {
        try_stream_edge(scene, nodes, 0, other, &mut adjacency, &mut seen);
        try_stream_edge(scene, nodes, 1, other, &mut adjacency, &mut seen);
    }

    // Phase 2: remaining vertex–vertex pairs in index order.
    for left in 2..nodes.len() {
        for right in (left + 1)..nodes.len() {
            try_stream_edge(scene, nodes, left, right, &mut adjacency, &mut seen);
        }
    }

    adjacency
}

fn try_stream_edge(
    scene: &PolygonScene,
    nodes: &[Point2],
    left: usize,
    right: usize,
    adjacency: &mut [Vec<(usize, f64)>],
    seen: &mut std::collections::HashSet<(usize, usize)>,
) {
    let key = if left < right {
        (left, right)
    } else {
        (right, left)
    };
    if !seen.insert(key) {
        return;
    }
    let start = nodes[left];
    let end = nodes[right];
    if scene.segment_is_walkable(start, end) {
        let cost = start.distance_to(end);
        adjacency[left].push((right, cost));
        adjacency[right].push((left, cost));
    }
}

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

    #[test]
    fn open_space_found_cost_parity_vs_visibility_graph() {
        let scene = PolygonScene {
            world_bounds: WorldBounds::new(Point2::new(0.0, 0.0), Point2::new(10.0, 10.0)),
            obstacles: Vec::new(),
        };
        let request = PolygonSearchRequest::new(Point2::new(1.0, 1.0), Point2::new(9.0, 1.0));

        let candidate = VisibilityGraphTautReflexStreaming
            .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!(
            VisibilityGraphTautReflexStreaming::CANDIDATE_ID,
            "exact-polygonal-scene/taut-reflex-streaming-graph"
        );
        assert_eq!(
            VisibilityGraphTautReflexStreaming.name(),
            "vg-taut-reflex-streaming"
        );
    }

    #[test]
    fn separator_no_path() {
        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 request = PolygonSearchRequest::new(Point2::new(2.0, 5.0), Point2::new(8.0, 5.0));

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

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