condor-pathfinding-geometry 0.4.0

//! Private candidate: lower-bound terminal hierarchy for fixed-source maps.
//!
//! # Current implementation (scaffold)
//!
//! Wraps [`crate::shortest_path_map::ContinuousShortestPathMap`] with direct
//! source→goal LOS short-circuit and fixed geometric distance shells used only
//! for admissible lower-bound inspection. Shells are **not** yet wired into
//! terminal pruning or ordered multi-terminal scan — exact answers fall through
//! to the baseline map.
//!
//! # Invariants
//!
//! - Terminal-region lower bounds are **admissible** under the same Euclidean
//!   and walkability predicates as [`crate::shortest_path_map`]: a pruned
//!   terminal must not hide the exact optimum. Uncertainty retains the terminal
//!   or falls through to the full exact scan.
//! - Immutable scene/source identity, endpoint validation, no-path, and path
//!   witness behavior remain identical to
//!   [`crate::shortest_path_map::ContinuousShortestPathMap`].
//! - Every accepted terminal still needs an exact source-to-terminal distance
//!   plus a walkable terminal-to-goal segment; bounds only order/reject
//!   uncompetitive terminals after an exact best is known.
//!
//! # Hypothesis and contract
//!
//! A hierarchy of admissible lower bounds over source-rooted terminal regions
//! can reject uncompetitive terminals before exact visibility work, improving
//! repeated fixed-source queries without changing map semantics. This is an
//! internal acceleration of the established fixed-source map, not a separate
//! approximate map.
//!
//! # Evidence and promotion
//!
//! Use `just test-fast continuous` and `just clippy-target continuous` while
//! implementing. Promotion requires `just test-geometry-conformance` and
//! reproducible repeated-query 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 crate::continuous::{PolygonPath, PolygonSearchResult};
use crate::polygonal::{Point2, PolygonScene};
use crate::shortest_path_map::{
    ContinuousShortestPathMap, PolygonShortestPathMap, PolygonShortestPathMapBuildError,
    PolygonShortestPathMapBuilder, PreparedContinuousShortestPathMap,
};

const EPSILON: f64 = 1e-9;

/// Builder: admissible terminal-region lower bounds over a source-rooted map.
///
/// Preprocess wraps [`ContinuousShortestPathMap`] and attaches Euclidean shell
/// radii for hierarchical lower bounds. Query still obtains the exact optimum
/// from the prepared map; shells supply admissible region labels so a future
/// ordered scan can prune only when
/// `dist[source→t] + |t−goal| ≥ best_exact` (never when uncertain).
#[derive(Debug, Clone, Copy, Default)]
pub struct ContinuousMapTerminalLowerBoundHierarchyBuilder;

impl ContinuousMapTerminalLowerBoundHierarchyBuilder {
    /// Stable portfolio identity for fixed-source terminal pruning.
    pub const CANDIDATE_ID: &str = "repeated-polygonal-fixed-source/terminal-lower-bound-hierarchy";
}

impl PolygonShortestPathMapBuilder for ContinuousMapTerminalLowerBoundHierarchyBuilder {
    type Map = PreparedTerminalLowerBoundHierarchyMap;

    fn name(&self) -> &'static str {
        "terminal-lower-bound-hierarchy"
    }

    fn preprocess(
        &self,
        scene: &PolygonScene,
        source: Point2,
    ) -> Result<Self::Map, PolygonShortestPathMapBuildError> {
        let inner = ContinuousShortestPathMap.preprocess(scene, source)?;
        let shells = build_distance_shells();
        Ok(PreparedTerminalLowerBoundHierarchyMap {
            scene: scene.clone(),
            inner,
            shells,
        })
    }
}

/// Prepared map with hierarchical admissible shells over source-rooted terminals.
#[derive(Debug, Clone)]
pub struct PreparedTerminalLowerBoundHierarchyMap {
    scene: PolygonScene,
    inner: PreparedContinuousShortestPathMap,
    /// Admissible shell radii around the source (strictly increasing).
    shells: Vec<f64>,
}

impl PreparedTerminalLowerBoundHierarchyMap {
    /// Number of hierarchical shell levels retained with this prepared map.
    #[must_use]
    pub fn shell_count(&self) -> usize {
        self.shells.len()
    }

    /// Admissible Euclidean lower bound from source to `goal` via shell strata.
    ///
    /// Returns the largest shell radius strictly less than `|source−goal|`, or
    /// `0.0` when the goal lies inside the innermost shell. Always ≤ the true
    /// Euclidean distance (hence admissible for free-space path cost).
    #[must_use]
    pub fn admissible_source_lower_bound(&self, goal: Point2) -> f64 {
        let euclidean = self.inner.source().distance_to(goal);
        let mut bound = 0.0;
        for &radius in &self.shells {
            if radius + EPSILON < euclidean {
                bound = radius;
            } else {
                break;
            }
        }
        debug_assert!(bound <= euclidean + EPSILON);
        bound
    }
}

impl PolygonShortestPathMap for PreparedTerminalLowerBoundHierarchyMap {
    fn name(&self) -> &'static str {
        "terminal-lower-bound-hierarchy"
    }

    fn source(&self) -> Point2 {
        self.inner.source()
    }

    fn query(&self, goal: Point2) -> PolygonSearchResult {
        let source = self.inner.source();

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

        if self.scene.validate_goal(goal).is_err() {
            return Err(crate::continuous::PolygonSearchError::InvalidGoal { point: goal });
        }

        // Hierarchy is consulted for admissible bounds; when the direct segment
        // is free its Euclidean cost is optimal and no terminal scan is needed.
        let _region_lb = self.admissible_source_lower_bound(goal);
        if self.scene.segment_is_walkable(source, goal) {
            let cost = source.distance_to(goal);
            debug_assert!(_region_lb <= cost + EPSILON);
            return crate::continuous::found(
                PolygonPath::from_points_with_cost(vec![source, goal], cost)
                    .expect("polygon path contains at least one point"),
                1,
            );
        }

        // Exact multi-terminal scan: hierarchy must not hide the optimum, so
        // the published answer is the established map's exact query. Shell LBs
        // remain available for ordered re-scans that only prune after a best
        // exact cost is known (LB ≥ best ⇒ uncompetitive).
        self.inner.query(goal)
    }
}

/// Build increasing Euclidean shell radii for region labeling.
///
/// Shells are scene-independent and therefore admissible as coarse lower-bound
/// strata: a goal outside shell `k` has Euclidean distance at least
/// `shells[k]` from the source.
fn build_distance_shells() -> Vec<f64> {
    vec![1.0, 2.0, 4.0, 8.0, 16.0, 32.0, 64.0, 128.0]
}

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::ContinuousMapTerminalLowerBoundHierarchyBuilder;
    use crate::polygonal::{Point2, Polygon, PolygonScene, WorldBounds};
    use crate::shortest_path_map::{
        ContinuousShortestPathMap, PolygonShortestPathMap, PolygonShortestPathMapBuilder,
    };

    #[test]
    fn query_parity_vs_continuous_shortest_path_map() {
        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(4.0, 4.0),
                Point2::new(6.0, 4.0),
                Point2::new(6.0, 8.0),
                Point2::new(4.0, 8.0),
            ])],
        };
        let source = Point2::new(1.0, 1.0);
        let builder = ContinuousMapTerminalLowerBoundHierarchyBuilder;
        let map = builder
            .preprocess(&scene, source)
            .expect("hierarchy map should preprocess");
        let baseline = ContinuousShortestPathMap
            .preprocess(&scene, source)
            .expect("baseline map should preprocess");

        assert_eq!(
            ContinuousMapTerminalLowerBoundHierarchyBuilder::CANDIDATE_ID,
            "repeated-polygonal-fixed-source/terminal-lower-bound-hierarchy"
        );
        assert_eq!(builder.name(), "terminal-lower-bound-hierarchy");
        assert_eq!(map.name(), "terminal-lower-bound-hierarchy");
        assert_eq!(map.source(), source);
        assert!(map.shell_count() > 0);
        assert!(map.admissible_source_lower_bound(Point2::new(10.0, 1.0)) <= 9.0 + 1e-9);

        for goal in [
            Point2::new(5.0, 1.0),
            Point2::new(10.0, 5.0),
            Point2::new(10.0, 10.0),
            Point2::new(1.0, 1.0),
        ] {
            let candidate = map.query(goal).expect("valid goal");
            let reference = baseline.query(goal).expect("valid goal");
            assert_eq!(
                candidate.is_found(),
                reference.is_found(),
                "found parity for goal {goal:?}"
            );
            match (candidate.cost(), reference.cost()) {
                (Some(left), Some(right)) => {
                    assert!(
                        (left - right).abs() <= 1e-9,
                        "cost parity for goal {goal:?}: {left} vs {right}"
                    );
                }
                (None, None) => {}
                other => panic!("cost shape mismatch for goal {goal:?}: {other:?}"),
            }
        }
    }

    #[test]
    fn no_path_for_separator_goal() {
        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 map = ContinuousMapTerminalLowerBoundHierarchyBuilder
            .preprocess(&scene, Point2::new(2.0, 5.0))
            .expect("source should preprocess");

        let result = map
            .query(Point2::new(8.0, 5.0))
            .expect("valid goal endpoint");

        assert!(!result.is_found());
        assert!(result.path().is_none());
        assert_eq!(result.cost(), None);
        assert_eq!(
            ContinuousMapTerminalLowerBoundHierarchyBuilder::CANDIDATE_ID,
            "repeated-polygonal-fixed-source/terminal-lower-bound-hierarchy"
        );
    }
}