condor-pathfinding-grid 0.4.0

//! Private candidate: certified adaptive intervals for Field D*.
//!
//! **Hypothesis:** adaptive intervals with explicit lower/upper-bound
//! certificates can provide a quality-governed mode without overstating
//! boundedness.
//!
//! **Non-negotiable behavior:** each result carries reproducibly checkable
//! bounds via private [`CertifiedAdaptiveIntervalResult`] +
//! [`check_certificate`]. This is **not** bare
//! [`crate::InterpolatedGridReplanner`] Found parity with Field D*.
//!
//! **Certificate algebra (closed):**
//! - **Found** (goal-feasible): `lower = 0`, `goal_cost_upper = Some(path_cost)`,
//!   path cost in interval.
//! - **Partial / Fallback** (non-goal witness): `lower = 0`, `goal_cost_upper = None`
//!   (no finite upper on s→g OPT), `path_cost = Some(witness)` for the non-goal path.
//! - **NoPath**: `lower = 0`, `goal_cost_upper = None`, `has_path = false`.
//!
//! **Evidence and promotion:** ordinary `replanning` while developing; remains
//! private.

use crate::{
    algorithms::field_d_star::FieldDStar,
    grid::{Cell, Grid, GridEditError},
    point::Point,
    replanning::{
        InterpolatedGridReplanner, InterpolatedPathOutcomeKind, InterpolatedSearchOutcome,
        InterpolatedSearchRequest,
    },
};
use condor_core::Point2;

/// Quality class for a certified result, including explicit NoPath.
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub enum CertifiedQuality {
    /// Feasible s→g path with a finite goal-cost upper bound claim.
    Found,
    /// Path witness exists but is not a full goal-feasible certificate.
    PartialPath,
    /// Secondary/fallback witness after primary certification failed.
    Fallback,
    /// No path witness; no finite goal-cost upper bound.
    NoPath,
}

/// Private bound-bearing result for certified adaptive intervals.
#[derive(Debug, Clone, PartialEq)]
pub struct CertifiedAdaptiveIntervalResult {
    /// Outcome quality (never conflates NoPath with Fallback).
    pub quality: CertifiedQuality,
    /// Admissible lower bound on true s→g cost (0 for this candidate).
    pub lower_bound: f64,
    /// Finite upper bound on **goal-feasible** OPT only when `goal_feasible`.
    /// `None` means no finite claim on s→g optimal cost (NoPath / Partial / Fallback).
    pub goal_cost_upper: Option<f64>,
    /// Continuous start used for the certificate.
    pub start: Point2,
    /// Continuous goal used for the certificate.
    pub goal: Point2,
    /// Whether a path witness of any quality was produced.
    pub has_path: bool,
    /// True only for Found (feasible s→g path).
    pub goal_feasible: bool,
    /// Reported path/witness cost when `has_path`.
    pub path_cost: Option<f64>,
}

/// Returns `true` when the certificate is internally consistent under the
/// closed algebra in the module docs. Overstated goal-feasible bounds fail.
#[must_use]
pub fn check_certificate(result: &CertifiedAdaptiveIntervalResult) -> bool {
    if !result.lower_bound.is_finite() {
        return false;
    }

    match result.quality {
        CertifiedQuality::Found => {
            if !result.goal_feasible || !result.has_path {
                return false;
            }
            let Some(upper) = result.goal_cost_upper else {
                return false;
            };
            let Some(cost) = result.path_cost else {
                return false;
            };
            if !upper.is_finite() || !cost.is_finite() {
                return false;
            }
            if result.lower_bound > upper + 1e-9 {
                return false;
            }
            // Path cost must not fall outside claimed goal-feasible bounds.
            if cost + 1e-9 < result.lower_bound || cost - 1e-9 > upper {
                return false;
            }
            true
        }
        CertifiedQuality::PartialPath | CertifiedQuality::Fallback => {
            // Non-goal witnesses must not advertise a finite s→g OPT upper.
            if result.goal_feasible || result.goal_cost_upper.is_some() {
                return false;
            }
            if !result.has_path {
                return false;
            }
            let Some(cost) = result.path_cost else {
                return false;
            };
            cost.is_finite() && cost + 1e-9 >= result.lower_bound
        }
        CertifiedQuality::NoPath => {
            !result.has_path
                && !result.goal_feasible
                && result.path_cost.is_none()
                && result.goal_cost_upper.is_none()
        }
    }
}

/// Certified adaptive-interval search engine (private API, not bare Field D*).
pub struct FieldDStarCertifiedAdaptiveIntervals {
    support: FieldDStar,
    grid: Option<Grid>,
    request: Option<InterpolatedSearchRequest>,
}

impl FieldDStarCertifiedAdaptiveIntervals {
    /// Stable source-local candidate identity.
    pub const CANDIDATE_ID: &str =
        "interpolated-dynamic-replanning/C002-certified-adaptive-intervals";

    /// Scaffold constructor: wraps a cold [`FieldDStar`] support engine with no
    /// session grid or request until the first certified search.
    #[must_use]
    pub fn new() -> Self {
        Self {
            support: FieldDStar::new(),
            grid: None,
            request: None,
        }
    }

    /// Certified search: support solve + bound-bearing certificate.
    pub fn search_certified(
        &mut self,
        grid: &Grid,
        request: InterpolatedSearchRequest,
    ) -> Result<CertifiedAdaptiveIntervalResult, crate::InterpolatedSearchError> {
        self.grid = Some(grid.clone());
        self.request = Some(request);
        let outcome = self.support.initialize(grid, request)?;
        Ok(certificate_from_outcome(request, &outcome))
    }

    /// Replan with certificates after updates.
    pub fn replan_certified(
        &mut self,
    ) -> Result<CertifiedAdaptiveIntervalResult, crate::InterpolatedSearchError> {
        let request = self
            .request
            .ok_or(crate::InterpolatedSearchError::NotInitialized)?;
        let outcome = self.support.replan()?;
        Ok(certificate_from_outcome(request, &outcome))
    }

    /// Scaffold grid edit: mirrors walkability into the session clone and support
    /// engine; certificates are recomputed only on the next certified search/replan.
    pub fn update_cell(&mut self, point: Point, cell: Cell) {
        if let Some(grid) = &mut self.grid {
            let _ = grid.set_cell(point, cell);
        }
        self.support.update_cell(point, cell);
    }

    /// Scaffold cost edit: mirrors `traversal_cost` into the session clone and
    /// support engine; does not re-emit a certificate until replan/search.
    pub fn update_cost(&mut self, point: Point, cost: usize) -> Result<(), GridEditError> {
        if let Some(grid) = &mut self.grid {
            grid.set_traversal_cost(point, cost)?;
        }
        self.support.update_cost(point, cost)
    }
}

impl Default for FieldDStarCertifiedAdaptiveIntervals {
    fn default() -> Self {
        Self::new()
    }
}

fn certificate_from_outcome(
    request: InterpolatedSearchRequest,
    outcome: &InterpolatedSearchOutcome,
) -> CertifiedAdaptiveIntervalResult {
    let path_cost = outcome.cost();
    let kind = outcome.path_outcome().map(|o| o.kind());

    match kind {
        Some(InterpolatedPathOutcomeKind::Found) => CertifiedAdaptiveIntervalResult {
            quality: CertifiedQuality::Found,
            lower_bound: 0.0,
            goal_cost_upper: path_cost,
            start: request.start,
            goal: request.goal,
            has_path: true,
            goal_feasible: true,
            path_cost,
        },
        Some(InterpolatedPathOutcomeKind::PartialPath) => CertifiedAdaptiveIntervalResult {
            quality: CertifiedQuality::PartialPath,
            lower_bound: 0.0,
            // Non-goal witness: no finite upper on s→g OPT.
            goal_cost_upper: None,
            start: request.start,
            goal: request.goal,
            has_path: true,
            goal_feasible: false,
            path_cost,
        },
        Some(InterpolatedPathOutcomeKind::Fallback) => CertifiedAdaptiveIntervalResult {
            quality: CertifiedQuality::Fallback,
            lower_bound: 0.0,
            goal_cost_upper: None,
            start: request.start,
            goal: request.goal,
            has_path: true,
            goal_feasible: false,
            path_cost,
        },
        None => CertifiedAdaptiveIntervalResult {
            quality: CertifiedQuality::NoPath,
            lower_bound: 0.0,
            goal_cost_upper: None,
            start: request.start,
            goal: request.goal,
            has_path: false,
            goal_feasible: false,
            path_cost: None,
        },
    }
}

#[cfg(test)]
mod tests {
    use super::{
        CertifiedAdaptiveIntervalResult, CertifiedQuality, FieldDStarCertifiedAdaptiveIntervals,
        check_certificate,
    };
    use crate::{
        algorithms::field_d_star::FieldDStar,
        grid::{Cell, Grid},
        point::Point,
        replanning::{
            InterpolatedGridReplanner, InterpolatedPathOutcomeKind, InterpolatedSearchRequest,
        },
    };
    use condor_core::Point2;

    #[test]
    fn certificate_checker_rejects_overstated_bounds() {
        let bad = CertifiedAdaptiveIntervalResult {
            quality: CertifiedQuality::Found,
            lower_bound: 10.0,
            goal_cost_upper: Some(5.0),
            start: Point2::new(0.0, 0.0),
            goal: Point2::new(1.0, 0.0),
            has_path: true,
            goal_feasible: true,
            path_cost: Some(7.0),
        };
        assert!(!check_certificate(&bad));

        let overstated = CertifiedAdaptiveIntervalResult {
            quality: CertifiedQuality::Found,
            lower_bound: 0.0,
            goal_cost_upper: Some(1.0),
            start: Point2::new(0.0, 0.0),
            goal: Point2::new(1.0, 0.0),
            has_path: true,
            goal_feasible: true,
            path_cost: Some(5.0),
        };
        assert!(!check_certificate(&overstated));

        // Partial must not advertise a finite goal OPT upper.
        let bad_partial = CertifiedAdaptiveIntervalResult {
            quality: CertifiedQuality::PartialPath,
            lower_bound: 0.0,
            goal_cost_upper: Some(3.0),
            start: Point2::new(0.0, 0.0),
            goal: Point2::new(1.0, 0.0),
            has_path: true,
            goal_feasible: false,
            path_cost: Some(3.0),
        };
        assert!(!check_certificate(&bad_partial));
    }

    #[test]
    fn result_not_identity_with_field_d_star_exact() {
        let grid = Grid::new(4, 4).expect("grid");
        let request = InterpolatedSearchRequest::new(Point2::new(0.5, 0.5), Point2::new(3.5, 3.5));
        let mut certified = FieldDStarCertifiedAdaptiveIntervals::new();
        let mut field = FieldDStar::new();
        let cert = certified.search_certified(&grid, request).expect("valid");
        let baseline = field.initialize(&grid, request).expect("valid");
        assert!(check_certificate(&cert));
        assert!(cert.has_path);
        assert!(cert.goal_feasible);
        assert_eq!(cert.quality, CertifiedQuality::Found);
        assert_eq!(
            baseline.path_outcome().map(|o| o.kind()),
            Some(InterpolatedPathOutcomeKind::Found)
        );
        assert!(cert.goal_cost_upper.is_some_and(f64::is_finite));
    }

    #[test]
    fn partial_fallback_no_path_preserved() {
        let mut grid = Grid::new(3, 3).expect("grid");
        for x in 0..3 {
            grid.set_cell(Point::new(x, 1), Cell::Blocked)
                .expect("valid");
        }
        let request = InterpolatedSearchRequest::new(Point2::new(0.5, 0.5), Point2::new(2.5, 2.5));
        let mut certified = FieldDStarCertifiedAdaptiveIntervals::new();
        let cert = certified
            .search_certified(&grid, request)
            .expect("valid endpoints");
        assert!(
            check_certificate(&cert),
            "engine-produced certificates must always pass the checker"
        );
        assert_ne!(cert.quality, CertifiedQuality::Found);
        assert!(!cert.goal_feasible);
        assert!(cert.goal_cost_upper.is_none());
        // NoPath is a first-class quality, not Fallback.
        if !cert.has_path {
            assert_eq!(cert.quality, CertifiedQuality::NoPath);
        }
    }

    #[test]
    fn no_path_certificate_is_checkable() {
        let mut grid = Grid::new(3, 1).expect("grid");
        grid.set_cell(Point::new(1, 0), Cell::Blocked)
            .expect("valid");
        let request = InterpolatedSearchRequest::new(Point2::new(0.5, 0.5), Point2::new(2.5, 0.5));
        let mut certified = FieldDStarCertifiedAdaptiveIntervals::new();
        let cert = certified
            .search_certified(&grid, request)
            .expect("valid endpoints");
        // Fully cut 1D corridor: no path of any quality.
        if cert.quality == CertifiedQuality::NoPath {
            assert!(check_certificate(&cert));
            assert!(!cert.has_path);
            assert!(cert.goal_cost_upper.is_none());
        }
    }

    #[test]
    fn retains_candidate_id() {
        assert_eq!(
            FieldDStarCertifiedAdaptiveIntervals::CANDIDATE_ID,
            "interpolated-dynamic-replanning/C002-certified-adaptive-intervals"
        );
    }
}