condor-pathfinding-geometry 0.4.0

//! Continuous polygonal path types and the [`PolygonPathfinder`] search trait.
//!
//! Online exact solvers in this owner crate (visibility graph, TFS) share one
//! contract: free-space geometry in a [`PolygonScene`], Euclidean polyline
//! cost on [`PolygonPath`], and [`SearchOutcome`] found/no-path with
//! [`PolygonSearchStats`]. Invalid free-space endpoints are
//! [`PolygonSearchError`] (`Err`); unreachable but valid endpoints are
//! `Ok(NoPath)`. Scene geometry validation
//! ([`PolygonValidationError`](crate::polygonal::PolygonValidationError)) is a
//! separate static/prep surface—not returned from `search` directly.
//!
//! # Prepared alternative
//!
//! Source-rooted repeated queries use prepared maps in
//! [`crate::shortest_path_map`], not this trait.

use crate::polygonal::{Point2, PolygonScene, PolygonSearchRequest};
use condor_core::{BudgetExhausted, SearchOutcome, SearchPathCost, SearchVisitStats};
use std::{error::Error, fmt};

/// Polygon path construction failed (empty polyline or invariant violation).
#[derive(Debug, Clone, Copy, PartialEq, Eq, thiserror::Error)]
#[non_exhaustive]
pub enum PolygonPathBuildError {
    /// Caller supplied an empty vertex list; paths require at least one point.
    #[error("polygon paths must contain at least one point")]
    Empty,
}

/// Non-empty free-space polyline in scene coordinates with Euclidean cost.
#[derive(Debug, Clone, PartialEq)]
pub struct PolygonPath {
    points: Vec<Point2>,
    cost: f64,
}

impl PolygonPath {
    /// Builds a path and sets cost to the sum of consecutive Euclidean segments.
    ///
    /// # Errors
    ///
    /// Returns [`PolygonPathBuildError::Empty`] when `points` is empty.
    pub fn from_points(points: Vec<Point2>) -> Result<Self, PolygonPathBuildError> {
        if points.is_empty() {
            return Err(PolygonPathBuildError::Empty);
        }
        let cost = points
            .windows(2)
            .map(|pair| pair[0].distance_to(pair[1]))
            .sum();
        Ok(Self { points, cost })
    }

    /// Builds a path with an explicit cost (callers must ensure consistency with the polyline).
    ///
    /// # Errors
    ///
    /// Returns [`PolygonPathBuildError::Empty`] when `points` is empty.
    pub fn from_points_with_cost(
        points: Vec<Point2>,
        cost: f64,
    ) -> Result<Self, PolygonPathBuildError> {
        if points.is_empty() {
            return Err(PolygonPathBuildError::Empty);
        }
        Ok(Self { points, cost })
    }

    /// Ordered free-space vertices from start through goal (inclusive).
    #[must_use]
    pub fn points(&self) -> &[Point2] {
        &self.points
    }

    /// Vertex count (always ≥ 1 for constructed paths).
    #[must_use]
    pub fn len(&self) -> usize {
        self.points.len()
    }

    /// Always `false` for successfully constructed paths (`from_points*` reject empty).
    #[must_use]
    pub fn is_empty(&self) -> bool {
        self.points.is_empty()
    }

    /// First vertex.
    ///
    /// # Panics
    ///
    /// Panics if the path is empty (cannot occur for values built via `from_points*`).
    #[must_use]
    pub fn start(&self) -> Point2 {
        self.points
            .first()
            .copied()
            .expect("polygon paths must contain at least one point")
    }

    /// Last vertex.
    ///
    /// # Panics
    ///
    /// Panics if the path is empty (cannot occur for values built via `from_points*`).
    #[must_use]
    pub fn goal(&self) -> Point2 {
        self.points
            .last()
            .copied()
            .expect("polygon paths must contain at least one point")
    }

    /// Euclidean polyline length, or the explicit value from `from_points_with_cost`.
    #[must_use]
    pub const fn cost(&self) -> f64 {
        self.cost
    }
}

/// Work counters for polygonal search (typically expanded graph vertices).
///
/// `visited_nodes` meaning is solver-specific (visibility nodes vs taut path
/// keys); comparable within one algorithm, not across solvers.
#[derive(Debug, Clone, Copy, Default, PartialEq)]
pub struct PolygonSearchStats {
    /// Algorithm-defined visit / expansion count for this search (lane-specific).
    pub visited_nodes: usize,
}

/// Request rejected before search, or budget hard stop mid-search.
///
/// Pathfinder-level endpoint errors and [`Self::BudgetExhausted`]. Unreachable but
/// walkable endpoints produce [`SearchOutcome::NoPath`], not these variants. Static
/// scene defects use [`PolygonValidationError`](crate::polygonal::PolygonValidationError)
/// on `validate*` / preprocess paths instead.
#[non_exhaustive]
#[derive(Debug, Clone, Copy, PartialEq)]
pub enum PolygonSearchError {
    /// Start fails [`PolygonScene::is_walkable`].
    InvalidStart {
        /// Off-free-space start that was rejected.
        point: Point2,
    },
    /// Goal fails [`PolygonScene::is_walkable`].
    InvalidGoal {
        /// Off-free-space goal that was rejected.
        point: Point2,
    },
    /// Caller search budget was exhausted before found/no-path completed.
    BudgetExhausted(BudgetExhausted),
}

impl fmt::Display for PolygonSearchError {
    fn fmt(&self, formatter: &mut fmt::Formatter<'_>) -> fmt::Result {
        match self {
            Self::InvalidStart { point } => write!(formatter, "invalid polygon start: {point:?}"),
            Self::InvalidGoal { point } => write!(formatter, "invalid polygon goal: {point:?}"),
            Self::BudgetExhausted(reason) => write!(formatter, "{reason}"),
        }
    }
}

impl Error for PolygonSearchError {}

/// Polygonal search return type: validation / budget `Err`, or found/no-path with stats.
///
/// `Ok(Found)` and `Ok(NoPath)` are completed searches; `Err` is not a proof of
/// unreachability (invalid endpoints or exhausted [`condor_core::SearchBudget`]).
pub type PolygonSearchResult =
    Result<SearchOutcome<PolygonPath, PolygonSearchStats>, PolygonSearchError>;

/// Builds `Ok(Found)` with the given polyline and expansion stats.
pub(crate) const fn found(path: PolygonPath, visited_nodes: usize) -> PolygonSearchResult {
    Ok(SearchOutcome::found(
        path,
        PolygonSearchStats { visited_nodes },
    ))
}

/// Builds `Ok(NoPath)` with expansion stats after exhaustive free-space search.
pub(crate) const fn not_found(visited_nodes: usize) -> PolygonSearchResult {
    Ok(SearchOutcome::no_path(PolygonSearchStats { visited_nodes }))
}

impl SearchPathCost for PolygonPath {
    type Cost = f64;

    fn path_cost(&self) -> Self::Cost {
        self.cost()
    }
}

impl SearchVisitStats for PolygonSearchStats {
    fn visited_nodes(&self) -> usize {
        self.visited_nodes
    }
}

/// Online exact pathfinder over a static polygonal obstacle scene.
///
/// Pair-search surface for one start–goal query. Implementations reject
/// endpoints that fail [`PolygonScene::is_walkable`] with [`PolygonSearchError`],
/// honor optional [`condor_core::SearchBudget`] on the request, and otherwise return
/// found/no-path with Euclidean polyline cost. Endpoints that pass the looser
/// walkability check but fail full [`PolygonScene::validate`] (for example sealed
/// boundary) typically yield `Ok(NoPath)` rather than `Err`—match the baseline
/// solvers when implementing. For many goals from one fixed source, use
/// [`crate::shortest_path_map`] instead of this online trait.
pub trait PolygonPathfinder {
    /// Stable algorithm identifier for benchmarks, logs, and solver portfolios.
    fn name(&self) -> &'static str;

    /// Runs one search on `scene` for `request`.
    ///
    /// Returns `Err` for non-walkable endpoints or exhausted budgets, `Ok(NoPath)`
    /// when no free-space route exists (including some sealed-endpoint cases after
    /// validation), and `Ok(Found)` with a non-empty polyline when a route is found.
    ///
    /// # Errors
    ///
    /// Returns [`PolygonSearchError`] when an endpoint is non-walkable or the caller
    /// budget is exhausted before found/no-path completes.
    fn search(&self, scene: &PolygonScene, request: PolygonSearchRequest) -> PolygonSearchResult;
}

/// Maps a shared budget failure into [`PolygonSearchError::BudgetExhausted`].
#[doc(hidden)]
pub const fn budget_error(reason: BudgetExhausted) -> PolygonSearchError {
    PolygonSearchError::BudgetExhausted(reason)
}

#[cfg(test)]
mod tests {
    use super::PolygonPath;
    use crate::polygonal::Point2;

    #[test]
    fn polygon_path_computes_euclidean_cost_from_points() {
        let path = PolygonPath::from_points(vec![
            Point2::new(1.0, 1.0),
            Point2::new(4.0, 5.0),
            Point2::new(7.0, 5.0),
        ])
        .expect("polygon path contains at least one point");

        assert_eq!(path.start(), Point2::new(1.0, 1.0));
        assert_eq!(path.goal(), Point2::new(7.0, 5.0));
        assert!((path.cost() - 8.0).abs() <= 1e-9);
        assert_eq!(path.len(), 3);
        assert!(!path.is_empty());
    }
}