condor-pathfinding-grid 0.4.0

//! Shared visibility-graph storage and deterministic Dijkstra for any-angle oracles.
//!
//! CSR adjacency ([`VisibilityGraphCsr`]), all-pairs build helpers, collinear elision,
//! and overlay-capable Dijkstra used by the one-shot corner oracle and
//! [`crate::PreparedAnyAngleGrid`]. Not a public consumer entrypoint.

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

use crate::{
    Grid,
    any_angle::AnyAngleSearchRequest,
    any_angle::geometry::{approximately_equal, segment_is_legal},
};
use condor_core::{BudgetExhausted, BudgetWatch, Point2};

/// Compact CSR adjacency for a static visibility graph.
///
/// `offsets[i]..offsets[i+1]` indexes directed edges of node `i` into
/// `neighbors` / `weights` (Euclidean segment lengths).
#[derive(Debug, Clone, PartialEq)]
pub(crate) struct VisibilityGraphCsr {
    /// Prefix offsets into `neighbors`/`weights` (`len == node_count + 1`).
    pub offsets: Vec<u32>,
    /// Directed neighbor node indices.
    pub neighbors: Vec<u32>,
    /// Euclidean weight of each directed edge.
    pub weights: Vec<f64>,
}

impl VisibilityGraphCsr {
    /// Number of directed adjacency entries in CSR storage.
    #[must_use]
    pub fn directed_edge_count(&self) -> usize {
        self.neighbors.len()
    }

    /// Approximate retained heap bytes for offsets, neighbors, and weights.
    #[must_use]
    pub fn retained_bytes(&self) -> usize {
        self.offsets.len() * size_of::<u32>()
            + self.neighbors.len() * size_of::<u32>()
            + self.weights.len() * size_of::<f64>()
    }

    /// Directed neighbors of `node` as `(neighbor_index, edge_weight)` pairs.
    ///
    /// # Panics
    ///
    /// Panics if `node` is outside `0..node_count` for this CSR.
    pub fn neighbors_of(&self, node: usize) -> impl Iterator<Item = (usize, f64)> + '_ {
        let start = self.offsets[node] as usize;
        let end = self.offsets[node + 1] as usize;
        (start..end).map(move |index| (self.neighbors[index] as usize, self.weights[index]))
    }
}

/// Counters recorded while building a visibility graph.
#[derive(Debug, Clone, Copy, Default, PartialEq, Eq)]
pub(crate) struct VisibilityGraphBuildStats {
    /// Pairwise LOS tests attempted (including rejected pairs).
    pub visibility_checks: usize,
    /// Undirected edges accepted after a legal segment check.
    pub accepted_undirected_edges: usize,
}

/// Counters recorded during one Dijkstra run on the visibility graph.
#[derive(Debug, Clone, Copy, Default, PartialEq, Eq)]
pub(crate) struct DijkstraSearchStats {
    /// Nodes marked settled (first pop with best cost).
    pub settled_nodes: usize,
    /// Successful distance decreases.
    pub relaxations: usize,
    /// Binary-heap pushes (including re-pushes after decrease).
    pub pushes: usize,
    /// Heap pops discarded as stale or dominated.
    pub stale_pops: usize,
    /// Peak open-set size during the run.
    pub frontier_peak: usize,
}

/// Builds an adjacency list and counts visibility tests for the given node set.
pub(crate) fn build_visibility_adjacency(
    grid: &Grid,
    nodes: &[Point2],
    segment_legal: fn(&Grid, Point2, Point2) -> bool,
    stats: &mut VisibilityGraphBuildStats,
) -> Vec<Vec<(usize, f64)>> {
    let node_count = nodes.len();
    let mut adjacency = vec![Vec::new(); node_count];
    for left in 0..node_count {
        for right in left + 1..node_count {
            stats.visibility_checks += 1;
            if !segment_legal(grid, nodes[left], nodes[right]) {
                continue;
            }
            let weight = nodes[left].distance_to(nodes[right]);
            adjacency[left].push((right, weight));
            adjacency[right].push((left, weight));
            stats.accepted_undirected_edges += 1;
        }
    }
    adjacency
}

/// Converts an adjacency list into CSR form with deterministic neighbor order.
pub(crate) fn adjacency_to_csr(adjacency: &[Vec<(usize, f64)>]) -> VisibilityGraphCsr {
    let node_count = adjacency.len();
    let mut offsets = Vec::with_capacity(node_count + 1);
    let mut neighbors = Vec::new();
    let mut weights = Vec::new();
    offsets.push(0);

    for edges in adjacency {
        for &(neighbor, weight) in edges {
            neighbors.push(neighbor as u32);
            weights.push(weight);
        }
        offsets.push(neighbors.len() as u32);
    }

    VisibilityGraphCsr {
        offsets,
        neighbors,
        weights,
    }
}

/// Builds CSR adjacency for all legal pairs among `nodes`.
pub(crate) fn build_visibility_csr(
    grid: &Grid,
    nodes: &[Point2],
    stats: &mut VisibilityGraphBuildStats,
) -> VisibilityGraphCsr {
    let adjacency = build_visibility_adjacency(grid, nodes, segment_is_legal, stats);
    adjacency_to_csr(&adjacency)
}

/// Runs Dijkstra on a prepared CSR plus sparse per-node overlay edges.
///
/// Overlay edges supplement CSR adjacency for prepared nodes and provide the
/// sole adjacency for query-local nodes appended beyond `prepared_count`.
pub(crate) fn run_dijkstra_csr_with_overlay(
    csr: &VisibilityGraphCsr,
    prepared_count: usize,
    overlay_edges: &[Vec<(usize, f64)>],
    start_index: usize,
    goal_index: usize,
    stats: &mut DijkstraSearchStats,
    watch: &BudgetWatch,
) -> Result<Option<Vec<Option<usize>>>, BudgetExhausted> {
    let node_count = overlay_edges.len();
    debug_assert!(prepared_count <= node_count);

    let mut dist = vec![f64::INFINITY; node_count];
    let mut predecessors = vec![None; node_count];
    let mut settled = vec![false; node_count];
    dist[start_index] = 0.0;

    let mut frontier = BinaryHeap::new();
    frontier.push(QueueEntry {
        node: start_index,
        cost: 0.0,
    });
    stats.pushes += 1;
    stats.frontier_peak = stats.frontier_peak.max(frontier.len());

    while let Some(current) = frontier.pop() {
        if settled[current.node] {
            stats.stale_pops += 1;
            continue;
        }
        if current.cost > dist[current.node] + 1e-15 {
            stats.stale_pops += 1;
            continue;
        }

        settled[current.node] = true;
        stats.settled_nodes += 1;
        if current.node == goal_index {
            return Ok(Some(predecessors));
        }

        watch.check(stats.settled_nodes)?;

        relax_neighbors(
            current.node,
            csr,
            prepared_count,
            overlay_edges,
            &mut dist,
            &mut predecessors,
            &mut settled,
            &mut frontier,
            stats,
        );
    }

    Ok(None)
}

/// Runs Dijkstra on a dense adjacency list (one-shot oracle path).
///
/// Returns predecessor indices when the goal settles, `Ok(None)` when no path
/// exists, or [`BudgetExhausted`] when `watch` stops expansion. Costs are
/// Euclidean edge weights; ties prefer the lower predecessor index.
pub(crate) fn run_dijkstra(
    adjacency: &[Vec<(usize, f64)>],
    start_index: usize,
    goal_index: usize,
    stats: &mut DijkstraSearchStats,
    watch: &BudgetWatch,
) -> Result<Option<Vec<Option<usize>>>, BudgetExhausted> {
    let node_count = adjacency.len();
    let mut dist = vec![f64::INFINITY; node_count];
    let mut predecessors = vec![None; node_count];
    let mut settled = vec![false; node_count];
    dist[start_index] = 0.0;

    let mut frontier = BinaryHeap::new();
    frontier.push(QueueEntry {
        node: start_index,
        cost: 0.0,
    });
    stats.pushes += 1;
    stats.frontier_peak = stats.frontier_peak.max(frontier.len());

    while let Some(current) = frontier.pop() {
        if settled[current.node] {
            stats.stale_pops += 1;
            continue;
        }
        if current.cost > dist[current.node] + 1e-15 {
            stats.stale_pops += 1;
            continue;
        }

        settled[current.node] = true;
        stats.settled_nodes += 1;
        if current.node == goal_index {
            return Ok(Some(predecessors));
        }

        watch.check(stats.settled_nodes)?;

        for &(neighbor, weight) in &adjacency[current.node] {
            relax_edge(
                current.node,
                neighbor,
                weight,
                &mut dist,
                &mut predecessors,
                &settled,
                &mut frontier,
                stats,
            );
        }
    }

    Ok(None)
}

#[allow(clippy::too_many_arguments)]
fn relax_neighbors(
    node: usize,
    csr: &VisibilityGraphCsr,
    prepared_count: usize,
    overlay_edges: &[Vec<(usize, f64)>],
    dist: &mut [f64],
    predecessors: &mut [Option<usize>],
    settled: &mut [bool],
    frontier: &mut BinaryHeap<QueueEntry>,
    stats: &mut DijkstraSearchStats,
) {
    if node < prepared_count {
        for (neighbor, weight) in csr.neighbors_of(node) {
            relax_edge(
                node,
                neighbor,
                weight,
                dist,
                predecessors,
                settled,
                frontier,
                stats,
            );
        }
    }

    for &(neighbor, weight) in &overlay_edges[node] {
        relax_edge(
            node,
            neighbor,
            weight,
            dist,
            predecessors,
            settled,
            frontier,
            stats,
        );
    }
}

#[allow(clippy::too_many_arguments)]
fn relax_edge(
    node: usize,
    neighbor: usize,
    weight: f64,
    dist: &mut [f64],
    predecessors: &mut [Option<usize>],
    settled: &[bool],
    frontier: &mut BinaryHeap<QueueEntry>,
    stats: &mut DijkstraSearchStats,
) {
    if settled[neighbor] {
        return;
    }
    let candidate = dist[node] + weight;
    let better = candidate < dist[neighbor]
        || (approximately_equal(candidate, dist[neighbor])
            && better_predecessor(node, predecessors[neighbor]));
    if better {
        dist[neighbor] = candidate;
        predecessors[neighbor] = Some(node);
        stats.relaxations += 1;
        frontier.push(QueueEntry {
            node: neighbor,
            cost: candidate,
        });
        stats.pushes += 1;
        stats.frontier_peak = stats.frontier_peak.max(frontier.len());
    }
}

/// Walks `predecessors` from `goal_index` back to the start and returns the vertex path.
pub(crate) fn reconstruct_path(
    nodes: &[Point2],
    predecessors: &[Option<usize>],
    goal_index: usize,
) -> Vec<Point2> {
    let mut path = vec![nodes[goal_index]];
    let mut current = goal_index;
    while let Some(previous) = predecessors[current] {
        path.push(nodes[previous]);
        if previous == current {
            break;
        }
        current = previous;
    }
    path.reverse();
    path
}

/// Drops intermediate collinear vertices when every simplified segment stays legal.
///
/// Applies only when the simplified polyline still passes `segment_legal` on each
/// consecutive pair; otherwise the original points are retained.
pub(crate) fn elide_collinear_with_predicate(
    grid: &Grid,
    points: &mut Vec<Point2>,
    segment_legal: fn(&Grid, Point2, Point2) -> bool,
) {
    if points.len() < 3 {
        return;
    }
    let mut simplified = Vec::with_capacity(points.len());
    simplified.push(points[0]);
    for index in 1..points.len() - 1 {
        let prev = simplified[simplified.len() - 1];
        let current = points[index];
        let next = points[index + 1];
        if !are_collinear(prev, current, next) {
            simplified.push(current);
        }
    }
    simplified.push(*points.last().expect("non-empty path"));
    if simplified
        .windows(2)
        .all(|pair| segment_legal(grid, pair[0], pair[1]))
    {
        *points = simplified;
    }
}

/// Collinear elision under the authority [`segment_is_legal`] predicate.
pub(crate) fn elide_collinear_points(grid: &Grid, points: &mut Vec<Point2>) {
    elide_collinear_with_predicate(grid, points, segment_is_legal);
}

/// Sorts by `(x, y)` then removes approximately-equal consecutive duplicates.
pub(crate) fn dedup_points(points: &mut Vec<Point2>) {
    points.sort_by(|left, right| {
        left.x
            .partial_cmp(&right.x)
            .unwrap_or(Ordering::Equal)
            .then_with(|| left.y.partial_cmp(&right.y).unwrap_or(Ordering::Equal))
    });
    points.dedup_by(|left, right| points_equal(*left, *right));
}

/// Approximate equality of two continuous vertices (shared any-angle tolerance).
#[must_use]
pub(crate) fn points_equal(left: Point2, right: Point2) -> bool {
    approximately_equal(left.x, right.x) && approximately_equal(left.y, right.y)
}

/// Index of `point` in `nodes` under [`points_equal`], if present.
pub(crate) fn node_index(nodes: &[Point2], point: Point2) -> Option<usize> {
    nodes.iter().position(|node| points_equal(*node, point))
}

/// Whether a built any-angle path still starts and ends at the request endpoints.
pub(crate) fn path_endpoints_match_request(
    path: &crate::any_angle::AnyAnglePath,
    request: AnyAngleSearchRequest,
) -> bool {
    path.points().first() == Some(&request.start) && path.points().last() == Some(&request.goal)
}

/// Inserts a bidirectional edge once; no-ops when `left == right` or the edge exists.
pub(crate) fn add_undirected_edge(
    adjacency: &mut [Vec<(usize, f64)>],
    left: usize,
    right: usize,
    weight: f64,
) {
    if left == right {
        return;
    }
    if !adjacency[left]
        .iter()
        .any(|(neighbor, _)| *neighbor == right)
    {
        adjacency[left].push((right, weight));
    }
    if !adjacency[right]
        .iter()
        .any(|(neighbor, _)| *neighbor == left)
    {
        adjacency[right].push((left, weight));
    }
}

fn are_collinear(a: Point2, b: Point2, c: Point2) -> bool {
    let abx = b.x - a.x;
    let aby = b.y - a.y;
    let bcx = c.x - b.x;
    let bcy = c.y - b.y;
    (abx * bcy - aby * bcx).abs() <= 1e-12
}

fn better_predecessor(candidate: usize, current: Option<usize>) -> bool {
    match current {
        None => true,
        Some(existing) => candidate < existing,
    }
}

#[derive(Debug, PartialEq)]
struct QueueEntry {
    node: usize,
    cost: f64,
}

impl Eq for QueueEntry {}

impl Ord for QueueEntry {
    fn cmp(&self, other: &Self) -> Ordering {
        other
            .cost
            .partial_cmp(&self.cost)
            .unwrap_or(Ordering::Equal)
            .then_with(|| self.node.cmp(&other.node))
    }
}

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