condor-pathfinding-geometry 0.4.0

//! Exact polygonal pathfinder using topological fracture search (TFS).
//!
//! # Surface
//!
//! Online [`crate::continuous::PolygonPathfinder`] alternative to the visibility
//! graph: no all-pairs edge materialization. Expands a best-first frontier of
//! **taut** free-space polylines:
//!
//! 1. Start from the straight start→goal candidate (tautened against free space).
//! 2. When the first obstructed segment collides with an obstacle, **fracture**
//!    that segment around the obstacle boundary by splicing vertex chains.
//! 3. Deduplicate taut path keys, expand cheapest incomplete path first, and
//!    stop when a fully walkable taut path is dequeued.
//!
//! # Cost and behavior
//!
//! **Cost model**: Euclidean length of the taut polyline. **Exactness**: same
//! free-space contract as [`crate::visibility_graph::VisibilityGraph`]. Often
//! cheaper on sparse pillar forests; denser all-pairs visibility graphs can
//! still win on many pack scenes.
//!
//! Use [`TopologicalFractureSearch::inspect`] for expansion counters
//! (keep/revert); plain [`PolygonPathfinder::search`] stays lean.

use std::cmp::Ordering;
use std::collections::{BinaryHeap, HashSet};

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

const EPSILON: f64 = 1e-9;

/// Online exact continuous pathfinder via topological fracture search.
///
/// Stateless solver unit: each `search` / `inspect` owns its frontier. Invalid
/// endpoints use the same typed errors as other polygon pathfinders; no-path
/// after frontier exhaustion means no free-space route under the geometry model.
#[derive(Debug, Clone, Copy, Default)]
pub struct TopologicalFractureSearch;

/// Expansion counters collected only by [`TopologicalFractureSearch::inspect`].
///
/// Use for relative keep/revert comparisons on the same pack; not wall-clock
/// latency. `memory_proxy` is a node-count proxy, not heap byte size:
/// `peak_frontier_len + visited_path_keys`.
#[derive(Debug, Default, Clone, Copy, PartialEq, Eq)]
pub struct TfsDiagnostics {
    /// Nodes dequeued from the best-first frontier (including the solution node).
    pub visited_nodes: usize,
    /// Times an obstructed taut path was fractured against an obstacle.
    pub fractures: usize,
    /// Child candidates produced by fracture (before visited-set filtering).
    pub children_generated: usize,
    /// Children inserted into the frontier (new taut path keys only).
    pub children_admitted: usize,
    /// Peak anchor-chain length observed on any node.
    pub max_anchors: usize,
    /// Peak taut polyline vertex count observed on any node.
    pub max_taut_len: usize,
    /// Peak binary-heap size during search.
    pub peak_frontier_len: usize,
    /// `peak_frontier_len + |visited path keys|` for relative memory ranking.
    pub memory_proxy: usize,
}

/// Instrumented TFS outcome: pathfinder result plus expansion diagnostics.
#[derive(Debug, Clone, PartialEq)]
pub struct TfsInspection {
    /// Same found / no-path / invalid-endpoint shape as [`PolygonPathfinder::search`].
    pub result: PolygonSearchResult,
    /// Counters for the run; zeroed on early invalid-endpoint returns.
    pub diagnostics: TfsDiagnostics,
}

impl TopologicalFractureSearch {
    /// Search with instrumentation for stress-pack keep/revert analysis.
    ///
    /// Invalid endpoints are reported through [`TfsInspection::result`] with the
    /// same errors as [`PolygonPathfinder::search`]. Diagnostics remain default
    /// when search aborts before expansion.
    #[must_use]
    pub fn inspect(&self, scene: &PolygonScene, request: PolygonSearchRequest) -> TfsInspection {
        if !scene.is_walkable(request.start) {
            return TfsInspection {
                result: Err(crate::continuous::PolygonSearchError::InvalidStart {
                    point: request.start,
                }),
                diagnostics: TfsDiagnostics::default(),
            };
        }
        if !scene.is_walkable(request.goal) {
            return TfsInspection {
                result: Err(crate::continuous::PolygonSearchError::InvalidGoal {
                    point: request.goal,
                }),
                diagnostics: TfsDiagnostics::default(),
            };
        }
        let execution = search_impl::<true>(scene, request);
        TfsInspection {
            result: execution.result,
            diagnostics: execution.diagnostics,
        }
    }
}

impl PolygonPathfinder for TopologicalFractureSearch {
    fn name(&self) -> &'static str {
        "topological-fracture-search"
    }

    /// Exact free-space path via taut-path fracture expansion (no diagnostics).
    ///
    /// Prefer [`Self::inspect`] when expansion counters are needed for keep/revert.
    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,
            });
        }
        search_impl::<false>(scene, request).result
    }
}

struct SearchExecution {
    result: PolygonSearchResult,
    diagnostics: TfsDiagnostics,
}

fn search_impl<const TRACK: bool>(
    scene: &PolygonScene,
    request: PolygonSearchRequest,
) -> SearchExecution {
    let mut diagnostics = TfsDiagnostics::default();

    if scene.validate(request).is_err() {
        return SearchExecution {
            result: crate::continuous::not_found(0),
            diagnostics,
        };
    }

    if points_equal(request.start, request.goal) {
        let result = crate::continuous::found(
            PolygonPath::from_points(vec![request.start])
                .expect("polygon path contains at least one point"),
            1,
        );
        if TRACK {
            diagnostics.visited_nodes = 1;
            diagnostics.max_anchors = 1;
            diagnostics.max_taut_len = 1;
            diagnostics.peak_frontier_len = 1;
            diagnostics.memory_proxy = 1;
        }
        return SearchExecution {
            result,
            diagnostics,
        };
    }

    let root = FractureNode::new(scene, vec![request.start, request.goal]);
    let mut frontier = BinaryHeap::from([root]);
    let mut visited = HashSet::from([path_key(frontier.peek().expect("root node").taut_path())]);
    let mut visited_nodes = 0usize;
    let watch = condor_core::BudgetWatch::start(request.budget);

    if TRACK {
        diagnostics.peak_frontier_len = frontier.len();
        diagnostics.max_anchors = frontier.peek().map(|node| node.anchors.len()).unwrap_or(0);
        diagnostics.max_taut_len = frontier
            .peek()
            .map(|node| node.taut_path.len())
            .unwrap_or(0);
    }

    while let Some(node) = frontier.pop() {
        visited_nodes += 1;
        if TRACK {
            diagnostics.visited_nodes = visited_nodes;
            diagnostics.max_anchors = diagnostics.max_anchors.max(node.anchors.len());
            diagnostics.max_taut_len = diagnostics.max_taut_len.max(node.taut_path.len());
        }

        let Some((segment_index, obstacle_index)) = first_collision(scene, node.taut_path()) else {
            if TRACK {
                diagnostics.memory_proxy = diagnostics.peak_frontier_len + visited.len();
            }
            return SearchExecution {
                result: crate::continuous::found(
                    PolygonPath::from_points_with_cost(node.taut_path().to_vec(), node.cost)
                        .expect("polygon path contains at least one point"),
                    visited_nodes,
                ),
                diagnostics,
            };
        };

        if let Err(reason) = watch.check(visited_nodes) {
            return SearchExecution {
                result: Err(crate::continuous::budget_error(reason)),
                diagnostics,
            };
        }

        if TRACK {
            diagnostics.fractures += 1;
        }

        let children = fracture_node(scene, &node, segment_index, obstacle_index);
        if TRACK {
            diagnostics.children_generated += children.len();
        }
        for child in children {
            let child_key = path_key(child.taut_path());
            if visited.insert(child_key) {
                if TRACK {
                    diagnostics.children_admitted += 1;
                    diagnostics.max_anchors = diagnostics.max_anchors.max(child.anchors.len());
                    diagnostics.max_taut_len = diagnostics.max_taut_len.max(child.taut_path.len());
                }
                frontier.push(child);
            }
        }
        if TRACK {
            diagnostics.peak_frontier_len = diagnostics.peak_frontier_len.max(frontier.len());
        }
    }

    if TRACK {
        diagnostics.visited_nodes = visited_nodes;
        diagnostics.memory_proxy = diagnostics.peak_frontier_len + visited.len();
    }

    SearchExecution {
        result: crate::continuous::not_found(visited_nodes),
        diagnostics,
    }
}

#[derive(Debug, Clone, PartialEq)]
struct FractureNode {
    anchors: Vec<Point2>,
    taut_path: Vec<Point2>,
    cost: f64,
}

impl FractureNode {
    fn new(scene: &PolygonScene, anchors: Vec<Point2>) -> Self {
        let taut_path = tauten_path(scene, &anchors);
        let cost = polyline_length(&taut_path);
        Self {
            anchors,
            taut_path,
            cost,
        }
    }

    fn taut_path(&self) -> &[Point2] {
        &self.taut_path
    }
}

impl Eq for FractureNode {}

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

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

fn fracture_node(
    scene: &PolygonScene,
    node: &FractureNode,
    segment_index: usize,
    obstacle_index: usize,
) -> Vec<FractureNode> {
    let start = node.taut_path()[segment_index];
    let goal = node.taut_path()[segment_index + 1];
    let obstacle = &scene.obstacles[obstacle_index];
    let vertices = obstacle.vertices();
    let mut children = Vec::new();

    for start_vertex_index in 0..vertices.len() {
        let branch_start = vertices[start_vertex_index];
        if segment_collides_obstacle(scene, start, branch_start, obstacle) {
            continue;
        }

        for goal_vertex_index in 0..vertices.len() {
            let branch_goal = vertices[goal_vertex_index];
            if segment_collides_obstacle(scene, branch_goal, goal, obstacle) {
                continue;
            }

            for chain in boundary_chains(vertices, start_vertex_index, goal_vertex_index) {
                let mut anchors = Vec::new();
                anchors.extend_from_slice(&node.taut_path()[..=segment_index]);
                for point in chain {
                    push_unique_point(&mut anchors, point);
                }
                for &point in &node.taut_path()[(segment_index + 1)..] {
                    push_unique_point(&mut anchors, point);
                }

                children.push(FractureNode::new(scene, anchors));
            }
        }
    }

    children
}

fn tauten_path(scene: &PolygonScene, anchors: &[Point2]) -> Vec<Point2> {
    let anchors = dedup_points(anchors);
    if anchors.len() <= 2 {
        return anchors;
    }

    let mut taut_path = Vec::with_capacity(anchors.len());
    let mut index = 0usize;
    taut_path.push(anchors[index]);

    while index < anchors.len() - 1 {
        let mut next = anchors.len() - 1;
        while next > index + 1 && !scene.segment_is_walkable(anchors[index], anchors[next]) {
            next -= 1;
        }

        taut_path.push(anchors[next]);
        index = next;
    }

    taut_path
}

fn first_collision(scene: &PolygonScene, path: &[Point2]) -> Option<(usize, usize)> {
    for (segment_index, pair) in path.windows(2).enumerate() {
        if scene.segment_is_walkable(pair[0], pair[1]) {
            continue;
        }

        for (obstacle_index, obstacle) in scene.obstacles.iter().enumerate() {
            if segment_collides_obstacle(scene, pair[0], pair[1], obstacle) {
                return Some((segment_index, obstacle_index));
            }
        }
    }

    None
}

fn boundary_chains(
    vertices: &[Point2],
    start_vertex_index: usize,
    goal_vertex_index: usize,
) -> [Vec<Point2>; 2] {
    [
        walk_chain(vertices, start_vertex_index, goal_vertex_index, true),
        walk_chain(vertices, start_vertex_index, goal_vertex_index, false),
    ]
}

fn walk_chain(
    vertices: &[Point2],
    start_vertex_index: usize,
    goal_vertex_index: usize,
    forward: bool,
) -> Vec<Point2> {
    let mut index = start_vertex_index;
    let mut chain = vec![vertices[index]];

    while index != goal_vertex_index {
        index = if forward {
            (index + 1) % vertices.len()
        } else if index == 0 {
            vertices.len() - 1
        } else {
            index - 1
        };
        chain.push(vertices[index]);
    }

    chain
}

fn segment_collides_obstacle(
    scene: &PolygonScene,
    start: Point2,
    end: Point2,
    obstacle: &Polygon,
) -> bool {
    let mut parameters = vec![0.0, 1.0];
    for (edge_start, edge_end) in polygon_edges(obstacle.vertices()) {
        parameters.extend(segment_intersection_parameters(
            start, end, edge_start, edge_end,
        ));
    }

    sort_and_dedup_parameters(&mut parameters);

    for parameter in &parameters {
        let point = interpolate_segment(start, end, *parameter);
        if obstacle.contains_point_strict(point)
            || point_on_sealed_obstacle_boundary(scene.world_bounds, obstacle, point)
        {
            return true;
        }
    }

    for interval in parameters.windows(2) {
        if interval[1] - interval[0] <= EPSILON {
            continue;
        }

        let midpoint = interpolate_segment(start, end, (interval[0] + interval[1]) / 2.0);
        if obstacle.contains_point_strict(midpoint)
            || point_on_sealed_obstacle_boundary(scene.world_bounds, obstacle, midpoint)
        {
            return true;
        }
    }

    false
}

fn point_on_sealed_obstacle_boundary(
    world_bounds: WorldBounds,
    obstacle: &Polygon,
    point: Point2,
) -> bool {
    polygon_edges(obstacle.vertices()).any(|(start, end)| {
        point_on_segment(point, start, end) && edge_lies_on_world_boundary(start, end, world_bounds)
    })
}

fn polyline_length(path: &[Point2]) -> f64 {
    path.windows(2)
        .map(|pair| pair[0].distance_to(pair[1]))
        .sum()
}

fn dedup_points(points: &[Point2]) -> Vec<Point2> {
    let mut deduped = Vec::with_capacity(points.len());
    for &point in points {
        push_unique_point(&mut deduped, point);
    }
    deduped
}

fn push_unique_point(points: &mut Vec<Point2>, point: Point2) {
    if points.last().is_none_or(|last| !points_equal(*last, point)) {
        points.push(point);
    }
}

fn path_key(path: &[Point2]) -> Vec<(u64, u64)> {
    path.iter()
        .map(|point| (point.x.to_bits(), point.y.to_bits()))
        .collect()
}

fn polygon_edges(vertices: &[Point2]) -> impl Iterator<Item = (Point2, Point2)> + '_ {
    vertices
        .iter()
        .copied()
        .zip(vertices.iter().copied().cycle().skip(1))
        .take(vertices.len())
}

fn sort_and_dedup_parameters(parameters: &mut Vec<f64>) {
    parameters.sort_by(f64::total_cmp);
    parameters.dedup_by(|left, right| (*left - *right).abs() <= EPSILON);
}

fn interpolate_segment(start: Point2, end: Point2, parameter: f64) -> Point2 {
    Point2::new(
        start.x + ((end.x - start.x) * parameter),
        start.y + ((end.y - start.y) * parameter),
    )
}

fn segment_intersection_parameters(
    a_start: Point2,
    a_end: Point2,
    b_start: Point2,
    b_end: Point2,
) -> Vec<f64> {
    let mut parameters = Vec::with_capacity(2);
    for point in [a_start, a_end, b_start, b_end] {
        if point_on_segment(point, a_start, a_end) && point_on_segment(point, b_start, b_end) {
            parameters.push(segment_parameter(point, a_start, a_end));
        }
    }

    if !parameters.is_empty() {
        sort_and_dedup_parameters(&mut parameters);
        return parameters;
    }

    if let Some(parameter) = proper_intersection_parameter(a_start, a_end, b_start, b_end) {
        parameters.push(parameter);
    }

    parameters
}

fn segment_parameter(point: Point2, start: Point2, end: Point2) -> f64 {
    let dx = end.x - start.x;
    let dy = end.y - start.y;
    if dx.abs() >= dy.abs() && dx.abs() > EPSILON {
        ((point.x - start.x) / dx).clamp(0.0, 1.0)
    } else if dy.abs() > EPSILON {
        ((point.y - start.y) / dy).clamp(0.0, 1.0)
    } else {
        0.0
    }
}

fn proper_intersection_parameter(
    a_start: Point2,
    a_end: Point2,
    b_start: Point2,
    b_end: Point2,
) -> Option<f64> {
    let o1 = orientation(a_start, a_end, b_start);
    let o2 = orientation(a_start, a_end, b_end);
    let o3 = orientation(b_start, b_end, a_start);
    let o4 = orientation(b_start, b_end, a_end);

    let properly_crosses = (o1 > EPSILON && o2 < -EPSILON || o1 < -EPSILON && o2 > EPSILON)
        && (o3 > EPSILON && o4 < -EPSILON || o3 < -EPSILON && o4 > EPSILON);
    if !properly_crosses {
        return None;
    }

    let a_dx = a_end.x - a_start.x;
    let a_dy = a_end.y - a_start.y;
    let b_dx = b_end.x - b_start.x;
    let b_dy = b_end.y - b_start.y;
    let denominator = cross(a_dx, a_dy, b_dx, b_dy);
    if denominator.abs() <= EPSILON {
        return None;
    }

    let offset_x = b_start.x - a_start.x;
    let offset_y = b_start.y - a_start.y;
    Some((cross(offset_x, offset_y, b_dx, b_dy) / denominator).clamp(0.0, 1.0))
}

fn edge_lies_on_world_boundary(start: Point2, end: Point2, bounds: WorldBounds) -> bool {
    ((start.x - bounds.min.x).abs() <= EPSILON && (end.x - bounds.min.x).abs() <= EPSILON)
        || ((start.x - bounds.max.x).abs() <= EPSILON && (end.x - bounds.max.x).abs() <= EPSILON)
        || ((start.y - bounds.min.y).abs() <= EPSILON && (end.y - bounds.min.y).abs() <= EPSILON)
        || ((start.y - bounds.max.y).abs() <= EPSILON && (end.y - bounds.max.y).abs() <= EPSILON)
}

fn point_on_segment(point: Point2, start: Point2, end: Point2) -> bool {
    let cross =
        ((point.y - start.y) * (end.x - start.x)) - ((point.x - start.x) * (end.y - start.y));
    if cross.abs() > EPSILON {
        return false;
    }

    let dot = ((point.x - start.x) * (end.x - start.x)) + ((point.y - start.y) * (end.y - start.y));
    if dot < -EPSILON {
        return false;
    }

    let length_sq =
        ((end.x - start.x) * (end.x - start.x)) + ((end.y - start.y) * (end.y - start.y));
    dot <= length_sq + EPSILON
}

fn orientation(start: Point2, end: Point2, point: Point2) -> f64 {
    ((end.x - start.x) * (point.y - start.y)) - ((end.y - start.y) * (point.x - start.x))
}

fn cross(ax: f64, ay: f64, bx: f64, by: f64) -> f64 {
    (ax * by) - (ay * bx)
}

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

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

    #[test]
    fn tfs_finds_direct_path_in_open_space() {
        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 result = TopologicalFractureSearch.search(&scene, request);

        assert!(result.as_ref().expect("valid search request").is_found());
        let path = result
            .as_ref()
            .expect("valid search request")
            .path()
            .expect("path should be present");
        assert_eq!(path.points(), &[request.start, request.goal]);
        assert!((path.cost() - 8.0).abs() <= 1e-9);
    }

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

        let result = TopologicalFractureSearch.search(&scene, request);

        assert!(!result.as_ref().expect("valid search request").is_found());
        assert!(
            result
                .as_ref()
                .expect("valid search request")
                .path()
                .is_none()
        );
        assert_eq!(result.as_ref().expect("valid search request").cost(), None);
    }

    #[test]
    fn inspect_reports_zero_fractures_on_open_space() {
        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 inspection = TopologicalFractureSearch.inspect(&scene, request);
        assert!(
            inspection
                .result
                .as_ref()
                .expect("valid search request")
                .is_found()
        );
        assert_eq!(inspection.diagnostics.fractures, 0);
        assert_eq!(inspection.diagnostics.children_generated, 0);
        assert_eq!(inspection.diagnostics.visited_nodes, 1);
    }

    #[test]
    fn inspect_reports_fractures_on_blocked_rectangle() {
        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, 3.0),
                Point2::new(6.0, 3.0),
                Point2::new(6.0, 7.0),
                Point2::new(4.0, 7.0),
            ])],
        };
        let request = PolygonSearchRequest::new(Point2::new(1.0, 5.0), Point2::new(9.0, 5.0));
        let inspection = TopologicalFractureSearch.inspect(&scene, request);
        assert!(
            inspection
                .result
                .as_ref()
                .expect("valid search request")
                .is_found()
        );
        assert!(inspection.diagnostics.fractures > 0);
        assert!(inspection.diagnostics.children_generated > 0);
        assert!(inspection.diagnostics.children_admitted > 0);
        assert!(inspection.diagnostics.memory_proxy > 0);
    }
}