condor_grid/

replanning.rs

//! Incremental grid replanning contracts for changing maps and costs.
//!
//! Two lanes share the initialize / update / replan lifecycle:
//!
//! | Lane | Trait | Coordinates | Primary solvers |
//! |------|-------|-------------|-----------------|
//! | Discrete | [`GridReplanner`] | integer [`Point`] cells | [`crate::algorithms::d_star_lite::DStarLite`], LPA* |
//! | Interpolated | [`InterpolatedGridReplanner`] | fractional [`Point2`] | [`crate::algorithms::field_d_star::FieldDStar`] |
//!
//! Discrete replanning returns the standard invalid/found/no-path [`SearchResult`];
//! interpolated replanning can also return partial or fallback
//! [`InterpolatedPathOutcome`]s. Prefer ordinary [`crate::Pathfinder`] search when
//! the grid is static, or [`crate::preprocessed_grid`] when it is static and queried
//! repeatedly.

use condor_core::Point2;
use serde::{Deserialize, Serialize};
use std::{collections::VecDeque, error::Error, fmt};

use crate::{
    grid::{Cell, Grid, GridEditError},
    point::Point,
    search::{SearchOutcome, SearchRequest, SearchResult},
};

const INTERPOLATED_EPSILON: f64 = 1e-9;

/// Discrete-grid replanner: initialize once, apply cell/cost deltas, then replan.
///
/// # Contract
///
/// - `initialize` must succeed before `replan`; implementations may return
///   [`crate::search::GridSearchError`] for invalid start/goal.
/// - Cell and cost updates are deferred until the next `replan` call.
/// - Outcomes are found / no-path via [`SearchResult`]; this lane does not
///   emit interpolated partial or fallback path kinds.
pub trait GridReplanner {
    /// Stable algorithm label for capture reports and solver identity.
    fn name(&self) -> &'static str;

    /// Cold-start search on the current grid snapshot.
    fn initialize(&mut self, grid: &Grid, request: SearchRequest) -> SearchResult;

    /// Record a walkability change; takes effect on the next [`Self::replan`].
    fn update_cell(&mut self, point: Point, cell: Cell);

    /// Record a traversal-cost change; takes effect on the next [`Self::replan`].
    ///
    /// # Errors
    ///
    /// Returns [`GridEditError`] when `point` is out of bounds or the cost is
    /// rejected by the underlying grid edit contract.
    fn update_cost(&mut self, point: Point, cost: usize) -> Result<(), GridEditError>;

    /// Recompute the shortest path, reusing prior search state when possible.
    fn replan(&mut self) -> SearchResult;
}

/// Cost model for continuous polylines over a discrete weighted grid.
#[derive(Debug, Clone, Copy, PartialEq, Eq, Serialize, Deserialize)]
#[serde(rename_all = "kebab-case")]
pub enum InterpolatedTraversalCostModel {
    /// Segment length weighted by the traversal costs of crossed cells (v0).
    CellLengthWeightedV0,
}

/// Fractional start/goal coordinates for interpolated grid search.
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct InterpolatedSearchRequest {
    /// Continuous-space start (mapped onto a discrete cell by floor).
    pub start: Point2,
    /// Continuous-space goal (mapped onto a discrete cell by floor).
    pub goal: Point2,
}

impl InterpolatedSearchRequest {
    /// Creates a fractional start/goal pair without validating grid membership.
    #[must_use]
    pub const fn new(start: Point2, goal: Point2) -> Self {
        Self { start, goal }
    }
}

/// Continuous polyline over a grid with an associated traversal cost.
///
/// Construct via [`Self::from_points`] (trusted cost) or
/// [`Self::from_points_on_grid`] (validated geometry + derived cost).
#[derive(Debug, Clone, PartialEq)]
pub struct InterpolatedPath {
    points: Vec<Point2>,
    cost: f64,
}

/// Interpolated path construction failed.
#[derive(Debug, Clone, Copy, PartialEq, thiserror::Error)]
#[non_exhaustive]
pub enum InterpolatedPathBuildError {
    /// Path had no vertices.
    #[error("interpolated paths must contain at least one point")]
    Empty,
    /// A vertex was NaN or infinite.
    #[error("interpolated path point {index} is not finite: {point:?}")]
    NonFinitePoint { index: usize, point: Point2 },
    /// A vertex fell outside the grid AABB.
    #[error("interpolated path point {index} is outside the {width}x{height} grid: {point:?}")]
    PointOutOfGrid {
        index: usize,
        point: Point2,
        width: usize,
        height: usize,
    },
    /// A vertex landed in a blocked cell.
    #[error("interpolated path point {index} is in blocked cell {cell:?}: {point:?}")]
    PointNotWalkable {
        index: usize,
        point: Point2,
        cell: Point,
    },
    /// A segment crossed non-walkable space under the cost probe.
    #[error("interpolated path segment {index} is not walkable from {start:?} to {end:?}")]
    SegmentNotWalkable {
        index: usize,
        start: Point2,
        end: Point2,
    },
    /// Segment cost was non-finite under the selected model.
    #[error("interpolated path segment {index} produced a non-finite cost under {cost_model:?}")]
    NonFiniteCost {
        index: usize,
        cost_model: InterpolatedTraversalCostModel,
    },
}

impl InterpolatedPath {
    /// Builds a path from vertices and a precomputed cost without grid checks.
    ///
    /// Prefer [`Self::from_points_on_grid`] when geometry and cost must be
    /// validated against a concrete grid.
    ///
    /// # Errors
    ///
    /// Returns [`InterpolatedPathBuildError::Empty`] when `points` is empty.
    pub fn from_points(points: Vec<Point2>, cost: f64) -> Result<Self, InterpolatedPathBuildError> {
        if points.is_empty() {
            return Err(InterpolatedPathBuildError::Empty);
        }
        Ok(Self { points, cost })
    }

    /// Constructs a validated interpolated path and derives its cost from `grid`.
    ///
    /// # Errors
    ///
    /// Returns [`InterpolatedPathBuildError`] when `points` is empty, a point is
    /// non-finite, outside the grid, or blocked, a segment crosses non-walkable
    /// space, or the selected cost model produces a non-finite cost.
    pub fn from_points_on_grid(
        grid: &Grid,
        points: Vec<Point2>,
        cost_model: InterpolatedTraversalCostModel,
    ) -> Result<Self, InterpolatedPathBuildError> {
        if points.is_empty() {
            return Err(InterpolatedPathBuildError::Empty);
        }

        for (index, &point) in points.iter().enumerate() {
            if !point.x.is_finite() || !point.y.is_finite() {
                return Err(InterpolatedPathBuildError::NonFinitePoint { index, point });
            }
            if point.x < 0.0
                || point.y < 0.0
                || point.x >= grid.width() as f64
                || point.y >= grid.height() as f64
            {
                return Err(InterpolatedPathBuildError::PointOutOfGrid {
                    index,
                    point,
                    width: grid.width(),
                    height: grid.height(),
                });
            }

            let cell = Point::new(point.x.floor() as usize, point.y.floor() as usize);
            if !grid.is_walkable(cell) {
                return Err(InterpolatedPathBuildError::PointNotWalkable { index, point, cell });
            }
        }

        let mut cost = 0.0;
        for (index, pair) in points.windows(2).enumerate() {
            let segment_cost = interpolated_segment_cost(grid, pair[0], pair[1], cost_model)
                .ok_or(InterpolatedPathBuildError::SegmentNotWalkable {
                    index,
                    start: pair[0],
                    end: pair[1],
                })?;
            if !segment_cost.is_finite() || !(cost + segment_cost).is_finite() {
                return Err(InterpolatedPathBuildError::NonFiniteCost { index, cost_model });
            }
            cost += segment_cost;
        }

        Self::from_points(points, cost)
    }

    /// Ordered continuous vertices from start through the path end (inclusive).
    #[must_use]
    pub fn points(&self) -> &[Point2] {
        &self.points
    }

    /// First path vertex (request start for well-formed outcomes).
    #[must_use]
    pub fn start(&self) -> Point2 {
        self.points[0]
    }

    /// Last path vertex (goal, partial target, or fallback endpoint).
    #[must_use]
    pub fn goal(&self) -> Point2 {
        self.points[self.points.len() - 1]
    }

    /// Accumulated interpolated traversal cost under the active cost model.
    #[must_use]
    pub const fn cost(&self) -> f64 {
        self.cost
    }
}

/// Work counters for one interpolated search or replan (solver-local).
#[derive(Debug, Clone, Copy, Default, PartialEq, Serialize)]
pub struct InterpolatedSearchStats {
    /// Nodes expanded or visited during the search/replan step.
    pub visited_nodes: usize,
}

/// Path quality returned by an interpolated search (full, partial, or fallback).
///
/// All three variants own an [`InterpolatedPath`]; the discriminant is what
/// fixture packs and capture oracles assert via [`InterpolatedExpectedKind`].
#[derive(Debug, Clone, PartialEq)]
pub enum InterpolatedPathOutcome {
    /// Continuous path reaches the goal.
    Found(InterpolatedPath),
    /// Goal unreachable; path ends at the best partial target cell.
    PartialPath(InterpolatedPath),
    /// Degenerate partial path (typically start-only) used as last resort.
    Fallback(InterpolatedPath),
}

/// Invalid interpolated-search request or uninitialized replanner state.
#[non_exhaustive]
#[derive(Debug, Clone, Copy, PartialEq)]
pub enum InterpolatedSearchError {
    /// Start does not map to a walkable in-bounds cell.
    InvalidStart { point: Point2 },
    /// Goal does not map to a walkable in-bounds cell.
    InvalidGoal { point: Point2 },
    /// `replan` was called before a successful `initialize`.
    NotInitialized,
    /// Path materialization failed after a search candidate was produced.
    PathBuild { source: InterpolatedPathBuildError },
}

impl fmt::Display for InterpolatedSearchError {
    fn fmt(&self, formatter: &mut fmt::Formatter<'_>) -> fmt::Result {
        match self {
            Self::InvalidStart { point } => {
                write!(formatter, "invalid interpolated start: {point:?}")
            }
            Self::InvalidGoal { point } => {
                write!(formatter, "invalid interpolated goal: {point:?}")
            }
            Self::NotInitialized => {
                formatter.write_str("interpolated replanner is not initialized")
            }
            Self::PathBuild { source } => write!(formatter, "interpolated path build: {source}"),
        }
    }
}

impl Error for InterpolatedSearchError {
    fn source(&self) -> Option<&(dyn Error + 'static)> {
        match self {
            Self::PathBuild { source } => Some(source),
            Self::InvalidStart { .. } | Self::InvalidGoal { .. } | Self::NotInitialized => None,
        }
    }
}

impl From<InterpolatedPathBuildError> for InterpolatedSearchError {
    fn from(source: InterpolatedPathBuildError) -> Self {
        Self::PathBuild { source }
    }
}

/// Grid-owned adapter for an interpolated found/no-path outcome.
///
/// The shared [`SearchOutcome`] operations remain available through
/// [`std::ops::Deref`],
/// while this wrapper owns the interpolated lane's path-quality semantics.
#[repr(transparent)]
#[derive(Debug, Clone, PartialEq)]
pub struct InterpolatedSearchOutcome(
    SearchOutcome<InterpolatedPathOutcome, InterpolatedSearchStats>,
);
/// Validation error or search outcome for the interpolated lane.
pub type InterpolatedSearchResult = Result<InterpolatedSearchOutcome, InterpolatedSearchError>;

/// Discriminant for [`InterpolatedPathOutcome`] without owning the path.
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub enum InterpolatedPathOutcomeKind {
    /// Full path to the goal.
    Found,
    /// Best reachable approach toward an unreachable goal.
    PartialPath,
    /// Start-only or otherwise degenerate fallback path.
    Fallback,
}

/// Borrowed view of a successful interpolated path plus its stats.
///
/// Produced by [`InterpolatedSearchOutcome::path_outcome`]; never constructed
/// for `NoPath` outcomes.
#[derive(Debug, Clone, Copy)]
pub struct InterpolatedPathOutcomeRef<'a> {
    kind: InterpolatedPathOutcomeKind,
    path: &'a InterpolatedPath,
    stats: &'a InterpolatedSearchStats,
}

impl<'a> InterpolatedPathOutcomeRef<'a> {
    /// Returns the path-quality discriminant.
    #[must_use]
    pub const fn kind(&self) -> InterpolatedPathOutcomeKind {
        self.kind
    }

    /// Maps this outcome onto the fixture/oracle expected-kind enum.
    ///
    /// Never returns [`InterpolatedExpectedKind::NoPath`] or the invalid
    /// endpoint kinds; those apply only to full search outcomes.
    #[must_use]
    pub fn expected_kind(&self) -> InterpolatedExpectedKind {
        match self.kind {
            InterpolatedPathOutcomeKind::Found => InterpolatedExpectedKind::Found,
            InterpolatedPathOutcomeKind::PartialPath => InterpolatedExpectedKind::PartialPath,
            InterpolatedPathOutcomeKind::Fallback => InterpolatedExpectedKind::Fallback,
        }
    }

    /// Returns the owned path reference carried by this outcome.
    #[must_use]
    pub const fn path(&self) -> &'a InterpolatedPath {
        self.path
    }

    /// Returns the work counters for the search/replan that produced this path.
    #[must_use]
    pub const fn stats(&self) -> &'a InterpolatedSearchStats {
        self.stats
    }

    /// Recomputes path cost from geometry under `cost_model`.
    ///
    /// Returns `None` when any segment is invalid on `grid` (oracle mismatch
    /// signal when compared to [`InterpolatedPath::cost`]).
    #[must_use]
    pub fn derived_cost(
        &self,
        grid: &Grid,
        cost_model: InterpolatedTraversalCostModel,
    ) -> Option<f64> {
        interpolated_path_cost(grid, self.path.points(), cost_model)
    }

    /// Returns the number of continuous vertices on the path (witness length).
    #[must_use]
    pub fn witness_points(&self) -> usize {
        self.path.points().len()
    }
}

impl InterpolatedPathOutcome {
    /// Returns the path-quality discriminant without cloning the path.
    #[must_use]
    pub const fn kind(&self) -> InterpolatedPathOutcomeKind {
        match self {
            Self::Found(_) => InterpolatedPathOutcomeKind::Found,
            Self::PartialPath(_) => InterpolatedPathOutcomeKind::PartialPath,
            Self::Fallback(_) => InterpolatedPathOutcomeKind::Fallback,
        }
    }

    /// Returns the path for any successful quality kind.
    #[must_use]
    pub const fn path(&self) -> &InterpolatedPath {
        match self {
            Self::Found(path) | Self::PartialPath(path) | Self::Fallback(path) => path,
        }
    }
}

/// Found outcome with a goal-reaching continuous path.
pub(crate) fn interpolated_found(
    path: InterpolatedPath,
    visited_nodes: usize,
) -> InterpolatedSearchResult {
    Ok(InterpolatedSearchOutcome::found(
        InterpolatedPathOutcome::Found(path),
        InterpolatedSearchStats { visited_nodes },
    ))
}

/// Path-bearing partial outcome (best incomplete route toward the goal).
pub(crate) fn interpolated_partial(
    path: InterpolatedPath,
    visited_nodes: usize,
) -> InterpolatedSearchResult {
    Ok(InterpolatedSearchOutcome::found(
        InterpolatedPathOutcome::PartialPath(path),
        InterpolatedSearchStats { visited_nodes },
    ))
}

/// Path-bearing fallback outcome when discrete connectivity is lost mid-route.
pub(crate) fn interpolated_fallback(
    path: InterpolatedPath,
    visited_nodes: usize,
) -> InterpolatedSearchResult {
    Ok(InterpolatedSearchOutcome::found(
        InterpolatedPathOutcome::Fallback(path),
        InterpolatedSearchStats { visited_nodes },
    ))
}

/// Completed no-path outcome for the interpolated lane (valid request, no route).
pub(crate) const fn interpolated_not_found(visited_nodes: usize) -> InterpolatedSearchResult {
    Ok(InterpolatedSearchOutcome::no_path(
        InterpolatedSearchStats { visited_nodes },
    ))
}

impl InterpolatedSearchOutcome {
    /// Creates a path-bearing interpolated outcome.
    #[must_use]
    pub const fn found(path: InterpolatedPathOutcome, stats: InterpolatedSearchStats) -> Self {
        Self(SearchOutcome::found(path, stats))
    }

    /// Creates an interpolated outcome where no path was available.
    #[must_use]
    pub const fn no_path(stats: InterpolatedSearchStats) -> Self {
        Self(SearchOutcome::no_path(stats))
    }

    /// Borrows the underlying shared search outcome.
    #[must_use]
    pub const fn as_search_outcome(
        &self,
    ) -> &SearchOutcome<InterpolatedPathOutcome, InterpolatedSearchStats> {
        &self.0
    }

    /// Consumes this adapter and returns the underlying shared search outcome.
    #[must_use]
    pub fn into_search_outcome(
        self,
    ) -> SearchOutcome<InterpolatedPathOutcome, InterpolatedSearchStats> {
        self.0
    }

    /// Returns the path cost when a path outcome is present.
    #[must_use]
    pub fn cost(&self) -> Option<f64> {
        self.path().map(|outcome| outcome.path().cost())
    }

    /// Maps this search outcome onto the fixture/oracle expected-kind enum.
    ///
    /// `NoPath` becomes [`InterpolatedExpectedKind::NoPath`]; found outcomes
    /// preserve found / partial / fallback quality.
    #[must_use]
    pub fn expected_kind(&self) -> InterpolatedExpectedKind {
        match self.path() {
            Some(path) => match path {
                InterpolatedPathOutcome::Found(_) => InterpolatedExpectedKind::Found,
                InterpolatedPathOutcome::PartialPath(_) => InterpolatedExpectedKind::PartialPath,
                InterpolatedPathOutcome::Fallback(_) => InterpolatedExpectedKind::Fallback,
            },
            None => InterpolatedExpectedKind::NoPath,
        }
    }

    /// Borrows the path outcome when the search found a path of any quality.
    #[must_use]
    pub fn path_outcome(&self) -> Option<InterpolatedPathOutcomeRef<'_>> {
        self.path().map(|path| InterpolatedPathOutcomeRef {
            kind: path.kind(),
            path: path.path(),
            stats: self.stats(),
        })
    }
}

impl std::ops::Deref for InterpolatedSearchOutcome {
    type Target = SearchOutcome<InterpolatedPathOutcome, InterpolatedSearchStats>;

    fn deref(&self) -> &Self::Target {
        self.as_search_outcome()
    }
}

impl From<SearchOutcome<InterpolatedPathOutcome, InterpolatedSearchStats>>
    for InterpolatedSearchOutcome
{
    fn from(outcome: SearchOutcome<InterpolatedPathOutcome, InterpolatedSearchStats>) -> Self {
        Self(outcome)
    }
}

impl From<InterpolatedSearchOutcome>
    for SearchOutcome<InterpolatedPathOutcome, InterpolatedSearchStats>
{
    fn from(outcome: InterpolatedSearchOutcome) -> Self {
        outcome.into_search_outcome()
    }
}

/// Continuous-coordinate replanner over a discrete grid backing store.
///
/// # Contract
///
/// - Same lifecycle as [`GridReplanner`], but requests and outcomes use
///   fractional coordinates and [`InterpolatedPathOutcome`] quality kinds.
/// - `update_cell` / `update_cost` edit the discrete backing grid; continuous
///   endpoints stay fixed until a moving-goal extension changes them.
/// - `replan` without a prior successful `initialize` should return
///   [`InterpolatedSearchError::NotInitialized`].
pub trait InterpolatedGridReplanner {
    /// Stable algorithm label for capture reports and solver identity.
    fn name(&self) -> &'static str;

    /// Cold-start search from fractional start/goal on the current grid.
    fn initialize(
        &mut self,
        grid: &Grid,
        request: InterpolatedSearchRequest,
    ) -> InterpolatedSearchResult;

    /// Record a discrete walkability change; takes effect on the next [`Self::replan`].
    fn update_cell(&mut self, point: Point, cell: Cell);

    /// Record a discrete traversal-cost change; takes effect on the next [`Self::replan`].
    ///
    /// # Errors
    ///
    /// Returns [`GridEditError`] when the edit is rejected by the grid contract.
    fn update_cost(&mut self, point: Point, cost: usize) -> Result<(), GridEditError>;

    /// Recompute under the last request, reusing prior continuous search state when possible.
    fn replan(&mut self) -> InterpolatedSearchResult;
}

/// Interpolated replanner lane that also accepts moving-goal updates between replans.
///
/// `update_goal` records a new fractional goal; it takes effect on the next
/// [`InterpolatedGridReplanner::replan`]. Start remains the last initialize/replan start.
pub trait InterpolatedMovingGoalReplanner: InterpolatedGridReplanner {
    /// Queue a fractional goal change for the next replan.
    fn update_goal(&mut self, goal: Point2);
}

/// Connectivity probe for fractional endpoints mapped onto discrete cells.
///
/// Produced by [`query_interpolated_grid`]; does not construct a continuous path.
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub enum InterpolatedQueryResult {
    /// Both endpoints map to walkable cells that are 4-connected.
    Connected { start_cell: Point, goal_cell: Point },
    /// Both endpoints map, but no 4-connected discrete path exists.
    NoPath { start_cell: Point, goal_cell: Point },
    /// Start is non-finite, out of bounds, or blocked.
    InvalidStart,
    /// Goal is non-finite, out of bounds, or blocked.
    InvalidGoal,
}

/// Maps fractional start/goal onto cells and tests 4-connected reachability.
#[must_use]
pub fn query_interpolated_grid(
    grid: &Grid,
    request: InterpolatedSearchRequest,
) -> InterpolatedQueryResult {
    let Some(start_cell) = interpolated_point_to_cell(grid, request.start) else {
        return InterpolatedQueryResult::InvalidStart;
    };
    let Some(goal_cell) = interpolated_point_to_cell(grid, request.goal) else {
        return InterpolatedQueryResult::InvalidGoal;
    };

    if interpolated_cells_connected(grid, start_cell, goal_cell) {
        InterpolatedQueryResult::Connected {
            start_cell,
            goal_cell,
        }
    } else {
        InterpolatedQueryResult::NoPath {
            start_cell,
            goal_cell,
        }
    }
}

/// Total cost of a continuous polyline under `cost_model`, if every segment is valid.
#[must_use]
pub fn interpolated_path_cost(
    grid: &Grid,
    points: &[Point2],
    cost_model: InterpolatedTraversalCostModel,
) -> Option<f64> {
    if points.is_empty() {
        return None;
    }
    if points.len() == 1 {
        return interpolated_point_to_cell(grid, points[0]).map(|_| 0.0);
    }

    points.windows(2).try_fold(0.0, |acc, pair| {
        interpolated_segment_cost(grid, pair[0], pair[1], cost_model).map(|cost| acc + cost)
    })
}

/// Cost of one continuous segment under `cost_model`, if both ends map onto the grid.
#[must_use]
pub fn interpolated_segment_cost(
    grid: &Grid,
    start: Point2,
    end: Point2,
    cost_model: InterpolatedTraversalCostModel,
) -> Option<f64> {
    match cost_model {
        InterpolatedTraversalCostModel::CellLengthWeightedV0 => {
            interpolated_segment_cost_cell_length_weighted_v0(grid, start, end)
        }
    }
}

/// Expected outcome kind asserted by interpolated replan fixtures and oracles.
///
/// Aligns with [`InterpolatedPathOutcome`] plus `NoPath` and endpoint validation
/// failures so packs can encode the full observed surface.
#[derive(Debug, Clone, Copy, PartialEq, Eq, Serialize, Deserialize)]
#[serde(rename_all = "kebab-case")]
pub enum InterpolatedExpectedKind {
    /// Full continuous path to the goal.
    Found,
    /// Best partial path toward an unreachable goal.
    PartialPath,
    /// Degenerate fallback path.
    Fallback,
    /// No path of any quality was produced.
    NoPath,
    /// Start failed discrete mapping / walkability checks.
    InvalidStart,
    /// Goal failed discrete mapping / walkability checks.
    InvalidGoal,
}

/// Best partial path toward an unreachable goal: route to the closest reachable cell.
///
/// Returns `Ok(None)` when the pair is already connected or no reachable partial
/// target exists (callers should run a full search instead). Invalid endpoints are
/// returned as typed [`InterpolatedSearchError`] values.
///
/// # Errors
///
/// Returns [`InterpolatedSearchError`] when an endpoint is invalid or the selected
/// partial route cannot be represented as a valid interpolated path.
pub fn best_partial_interpolated_path(
    grid: &Grid,
    request: InterpolatedSearchRequest,
    cost_model: InterpolatedTraversalCostModel,
) -> Result<Option<InterpolatedPath>, InterpolatedSearchError> {
    let Some(start_cell) = interpolated_point_to_cell(grid, request.start) else {
        return Err(InterpolatedSearchError::InvalidStart {
            point: request.start,
        });
    };
    let Some(goal_cell) = interpolated_point_to_cell(grid, request.goal) else {
        return Err(InterpolatedSearchError::InvalidGoal {
            point: request.goal,
        });
    };
    if interpolated_cells_connected(grid, start_cell, goal_cell) {
        return Ok(None);
    }

    let (parents, distances) = reachable_interpolated_cells(grid, start_cell);
    let Some(target) = select_partial_target(grid, request, &distances) else {
        return Ok(None);
    };
    let Some(cell_path) = reconstruct_cell_path(grid, start_cell, target, &parents) else {
        return Ok(None);
    };

    let mut points = vec![request.start];
    for cell in cell_path.into_iter().skip(1) {
        let center = interpolated_cell_center(cell);
        if same_interpolated_point(points[points.len() - 1], center) {
            continue;
        }
        points.push(center);
    }

    InterpolatedPath::from_points_on_grid(grid, points, cost_model)
        .map(Some)
        .map_err(Into::into)
}

/// Fallback path when only the start cell is reachable (degenerate partial path).
///
/// # Errors
///
/// Returns [`InterpolatedSearchError`] when an endpoint is invalid or partial-path
/// construction fails.
pub fn best_fallback_interpolated_path(
    grid: &Grid,
    request: InterpolatedSearchRequest,
    cost_model: InterpolatedTraversalCostModel,
) -> Result<Option<InterpolatedPath>, InterpolatedSearchError> {
    let Some(path) = best_partial_interpolated_path(grid, request, cost_model)? else {
        return Ok(None);
    };
    Ok((path.points().len() == 1).then_some(path))
}

fn interpolated_segment_cost_cell_length_weighted_v0(
    grid: &Grid,
    start: Point2,
    end: Point2,
) -> Option<f64> {
    interpolated_point_to_cell(grid, start)?;
    interpolated_point_to_cell(grid, end)?;

    let total_length = start.distance_to(end);
    if total_length <= INTERPOLATED_EPSILON {
        return Some(0.0);
    }

    let dx = end.x - start.x;
    let dy = end.y - start.y;
    let mut parameters = vec![0.0, 1.0];

    if dx.abs() > INTERPOLATED_EPSILON {
        for boundary in 1..grid.width() {
            let boundary = boundary as f64;
            let t = (boundary - start.x) / dx;
            if t > INTERPOLATED_EPSILON && t < 1.0 - INTERPOLATED_EPSILON {
                parameters.push(t);
            }
        }
    }

    if dy.abs() > INTERPOLATED_EPSILON {
        for boundary in 1..grid.height() {
            let boundary = boundary as f64;
            let t = (boundary - start.y) / dy;
            if t > INTERPOLATED_EPSILON && t < 1.0 - INTERPOLATED_EPSILON {
                parameters.push(t);
            }
        }
    }

    parameters.sort_by(|left, right| left.total_cmp(right));
    parameters.dedup_by(|left, right| (*left - *right).abs() <= INTERPOLATED_EPSILON);

    let mut cost = 0.0;
    for interval in parameters.windows(2) {
        let start_t = interval[0];
        let end_t = interval[1];
        if end_t - start_t <= INTERPOLATED_EPSILON {
            continue;
        }

        let midpoint = interpolate_segment(start, end, (start_t + end_t) / 2.0);
        let cell = interpolated_point_to_cell(grid, midpoint)?;
        let traversal_cost = grid.traversal_cost(cell)? as f64;
        cost += total_length * (end_t - start_t) * traversal_cost;
    }

    Some(cost)
}

fn interpolated_point_to_cell(grid: &Grid, point: Point2) -> Option<Point> {
    if !point.x.is_finite() || !point.y.is_finite() {
        return None;
    }
    if point.x < 0.0
        || point.y < 0.0
        || point.x >= grid.width() as f64
        || point.y >= grid.height() as f64
    {
        return None;
    }

    let cell = Point::new(point.x.floor() as usize, point.y.floor() as usize);
    grid.is_walkable(cell).then_some(cell)
}

fn reachable_interpolated_cells(
    grid: &Grid,
    start: Point,
) -> (Vec<Option<Point>>, Vec<Option<usize>>) {
    let mut parents = vec![None; grid.cell_count()];
    let mut distances = vec![None; grid.cell_count()];
    let mut queue = VecDeque::new();

    let start_index = grid
        .index_of(start)
        .expect("start point should index into the grid");
    distances[start_index] = Some(0);
    queue.push_back(start);

    while let Some(point) = queue.pop_front() {
        let point_index = grid
            .index_of(point)
            .expect("reachable point should index into the grid");
        let distance = distances[point_index].expect("reachable cells carry distance");
        for neighbor in grid.neighbors4(point) {
            if !grid.is_walkable(neighbor) {
                continue;
            }
            let neighbor_index = grid
                .index_of(neighbor)
                .expect("neighbor should index into the grid");
            if distances[neighbor_index].is_some() {
                continue;
            }

            parents[neighbor_index] = Some(point);
            distances[neighbor_index] = Some(distance + 1);
            queue.push_back(neighbor);
        }
    }

    (parents, distances)
}

fn select_partial_target(
    grid: &Grid,
    request: InterpolatedSearchRequest,
    distances: &[Option<usize>],
) -> Option<Point> {
    let mut best: Option<(Point, f64, usize)> = None;

    for (index, distance) in distances
        .iter()
        .copied()
        .enumerate()
        .take(grid.cell_count())
    {
        let Some(distance) = distance else {
            continue;
        };
        let point = grid.point_from_index(index);
        if !grid.is_walkable(point) {
            continue;
        }

        let goal_distance = interpolated_cell_center(point).distance_to(request.goal);
        match best {
            None => best = Some((point, goal_distance, distance)),
            Some((current_point, current_goal_distance, current_steps)) => {
                if goal_distance + INTERPOLATED_EPSILON < current_goal_distance
                    || ((goal_distance - current_goal_distance).abs() <= INTERPOLATED_EPSILON
                        && (distance < current_steps
                            || (distance == current_steps
                                && (point.y < current_point.y
                                    || (point.y == current_point.y && point.x < current_point.x)))))
                {
                    best = Some((point, goal_distance, distance));
                }
            }
        }
    }

    best.map(|(point, _, _)| point)
}

fn reconstruct_cell_path(
    grid: &Grid,
    start: Point,
    target: Point,
    parents: &[Option<Point>],
) -> Option<Vec<Point>> {
    let mut cursor = target;
    let mut path = vec![cursor];

    while cursor != start {
        let cursor_index = grid
            .index_of(cursor)
            .expect("path cursor should index into the grid");
        let parent = parents[cursor_index]?;
        cursor = parent;
        path.push(cursor);
    }

    path.reverse();
    Some(path)
}

fn interpolated_cell_center(point: Point) -> Point2 {
    Point2::new(point.x as f64 + 0.5, point.y as f64 + 0.5)
}

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

fn interpolated_cells_connected(grid: &Grid, start: Point, goal: Point) -> bool {
    if start == goal {
        return true;
    }

    let mut seen = vec![false; grid.cell_count()];
    let mut frontier = std::collections::VecDeque::from([start]);
    let start_index = (start.y * grid.width()) + start.x;
    seen[start_index] = true;

    while let Some(cell) = frontier.pop_front() {
        if cell == goal {
            return true;
        }

        for neighbor in grid.neighbors4(cell) {
            let index = (neighbor.y * grid.width()) + neighbor.x;
            if seen[index] {
                continue;
            }
            seen[index] = true;
            frontier.push_back(neighbor);
        }
    }

    false
}

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

#[cfg(test)]
mod tests {
    use super::*;
    use crate::{
        Cell, InterpolatedGridReplanner, InterpolatedMovingGoalReplanner, SearchOutcome,
        algorithms::field_d_star::FieldDStar, best_partial_interpolated_path,
        interpolated_path_cost,
    };

    const EPSILON: f64 = 1e-9;

    #[test]
    fn on_grid_path_build_reports_precise_validation_variants() {
        let mut grid = Grid::new(3, 1).expect("grid dimensions are valid");
        let cost_model = InterpolatedTraversalCostModel::CellLengthWeightedV0;

        assert_eq!(
            InterpolatedPath::from_points_on_grid(&grid, Vec::new(), cost_model),
            Err(InterpolatedPathBuildError::Empty)
        );

        let non_finite = Point2::new(f64::NAN, 0.5);
        assert!(matches!(
            InterpolatedPath::from_points_on_grid(&grid, vec![non_finite], cost_model),
            Err(InterpolatedPathBuildError::NonFinitePoint { index: 0, point })
                if point.x.is_nan() && point.y == 0.5
        ));

        let outside = Point2::new(3.0, 0.5);
        assert_eq!(
            InterpolatedPath::from_points_on_grid(&grid, vec![outside], cost_model),
            Err(InterpolatedPathBuildError::PointOutOfGrid {
                index: 0,
                point: outside,
                width: 3,
                height: 1,
            })
        );

        grid.set_cell(Point::new(1, 0), Cell::Blocked)
            .expect("blocked test cell should be valid");
        let blocked = Point2::new(1.5, 0.5);
        assert_eq!(
            InterpolatedPath::from_points_on_grid(&grid, vec![blocked], cost_model),
            Err(InterpolatedPathBuildError::PointNotWalkable {
                index: 0,
                point: blocked,
                cell: Point::new(1, 0),
            })
        );

        let start = Point2::new(0.5, 0.5);
        let end = Point2::new(2.5, 0.5);
        assert_eq!(
            InterpolatedPath::from_points_on_grid(&grid, vec![start, end], cost_model),
            Err(InterpolatedPathBuildError::SegmentNotWalkable {
                index: 0,
                start,
                end,
            })
        );
    }

    #[test]
    fn partial_path_helper_distinguishes_not_applicable_from_a_path() {
        let mut grid = Grid::new(3, 1).expect("grid dimensions are valid");
        let request = InterpolatedSearchRequest::new(Point2::new(0.5, 0.5), Point2::new(2.5, 0.5));
        let cost_model = InterpolatedTraversalCostModel::CellLengthWeightedV0;

        let connected = best_partial_interpolated_path(&grid, request, cost_model)
            .expect("valid partial-path construction");
        assert!(connected.is_none(), "connected requests are not applicable");

        grid.set_cell(Point::new(1, 0), Cell::Blocked)
            .expect("barrier point should be valid");
        let partial = best_partial_interpolated_path(&grid, request, cost_model)
            .expect("valid partial-path construction")
            .expect("disconnected request should produce a partial path");
        assert_eq!(partial.points(), &[request.start]);
    }

    #[test]
    fn shared_result_surface_classifies_all_interpolated_outcomes() {
        let grid = Grid::new(2, 1).expect("grid dimensions are valid");
        let cost_model = InterpolatedTraversalCostModel::CellLengthWeightedV0;
        let path = InterpolatedPath::from_points_on_grid(
            &grid,
            vec![Point2::new(0.5, 0.5), Point2::new(1.5, 0.5)],
            cost_model,
        )
        .expect("test path should build");

        for (result, expected_kind, expected_outcome_kind, expected_visits) in [
            (
                InterpolatedSearchOutcome::found(
                    InterpolatedPathOutcome::Found(path.clone()),
                    InterpolatedSearchStats { visited_nodes: 3 },
                ),
                InterpolatedExpectedKind::Found,
                Some(InterpolatedPathOutcomeKind::Found),
                3,
            ),
            (
                InterpolatedSearchOutcome::found(
                    InterpolatedPathOutcome::PartialPath(path.clone()),
                    InterpolatedSearchStats { visited_nodes: 5 },
                ),
                InterpolatedExpectedKind::PartialPath,
                Some(InterpolatedPathOutcomeKind::PartialPath),
                5,
            ),
            (
                InterpolatedSearchOutcome::found(
                    InterpolatedPathOutcome::Fallback(path.clone()),
                    InterpolatedSearchStats { visited_nodes: 7 },
                ),
                InterpolatedExpectedKind::Fallback,
                Some(InterpolatedPathOutcomeKind::Fallback),
                7,
            ),
            (
                InterpolatedSearchOutcome::no_path(InterpolatedSearchStats { visited_nodes: 11 }),
                InterpolatedExpectedKind::NoPath,
                None,
                11,
            ),
        ] {
            assert_eq!(result.is_found(), expected_outcome_kind.is_some());
            assert_eq!(result.path().is_some(), expected_outcome_kind.is_some());
            assert_eq!(result.stats().visited_nodes, expected_visits);
            assert_eq!(result.expected_kind(), expected_kind);
            assert_eq!(result.cost(), expected_outcome_kind.map(|_| path.cost()));
            let outcome = result.path_outcome();
            assert_eq!(outcome.map(|outcome| outcome.kind()), expected_outcome_kind);

            if let Some(outcome) = outcome {
                assert_eq!(outcome.expected_kind(), expected_kind);
                assert_eq!(outcome.path().points(), path.points());
                assert_eq!(outcome.path().cost(), path.cost());
                assert_eq!(outcome.witness_points(), path.points().len());
                let derived_cost = outcome
                    .derived_cost(&grid, cost_model)
                    .expect("path-bearing outcome should derive a cost");
                assert!((derived_cost - path.cost()).abs() <= EPSILON);
            }

            let shared: SearchOutcome<InterpolatedPathOutcome, InterpolatedSearchStats> =
                result.clone().into();
            let wrapped = InterpolatedSearchOutcome::from(shared.clone());
            assert_eq!(wrapped.as_search_outcome(), &shared);
            let round_trip: SearchOutcome<InterpolatedPathOutcome, InterpolatedSearchStats> =
                wrapped.into();
            assert_eq!(round_trip, shared);
        }

        assert!(matches!(
            InterpolatedSearchError::InvalidStart {
                point: Point2::new(-1.0, 0.5),
            },
            InterpolatedSearchError::InvalidStart { .. }
        ));
        assert!(matches!(
            InterpolatedSearchError::InvalidGoal {
                point: Point2::new(2.5, 0.5),
            },
            InterpolatedSearchError::InvalidGoal { .. }
        ));
    }

    #[test]
    fn shared_result_surface_reuses_fixed_and_moving_goal_field_d_star_outputs() {
        let mut partial_grid = Grid::new(4, 1).expect("grid dimensions are valid");
        let request = InterpolatedSearchRequest::new(Point2::new(0.5, 0.5), Point2::new(3.5, 0.5));
        let mut replanner = FieldDStar::new();

        let initial = replanner
            .initialize(&partial_grid, request)
            .expect("test request should be valid");
        let initial_outcome = initial
            .path_outcome()
            .expect("initial result should expose a path outcome");
        assert_eq!(initial.expected_kind(), InterpolatedExpectedKind::Found);
        assert_eq!(initial_outcome.kind(), InterpolatedPathOutcomeKind::Found);
        assert_eq!(
            initial_outcome.derived_cost(
                &partial_grid,
                InterpolatedTraversalCostModel::CellLengthWeightedV0
            ),
            Some(initial_outcome.path().cost())
        );

        let partial_barrier = Point::new(2, 0);
        partial_grid
            .set_cell(partial_barrier, Cell::Blocked)
            .expect("partial barrier should be valid");
        replanner.update_cell(partial_barrier, Cell::Blocked);
        let partial = replanner.replan().expect("test request should be valid");
        let partial_outcome = partial
            .path_outcome()
            .expect("partial-path result should expose a path outcome");
        assert_eq!(
            partial.expected_kind(),
            InterpolatedExpectedKind::PartialPath
        );
        assert_eq!(
            partial_outcome.kind(),
            InterpolatedPathOutcomeKind::PartialPath
        );
        let derived_partial_cost = partial_outcome
            .derived_cost(
                &partial_grid,
                InterpolatedTraversalCostModel::CellLengthWeightedV0,
            )
            .expect("partial-path result should derive a cost");
        let explicit_partial_cost = interpolated_path_cost(
            &partial_grid,
            partial_outcome.path().points(),
            InterpolatedTraversalCostModel::CellLengthWeightedV0,
        )
        .expect("partial-path result should stay valid on the grid");
        assert!((derived_partial_cost - explicit_partial_cost).abs() <= EPSILON);

        replanner.update_goal(Point2::new(4.5, 0.5));
        assert_eq!(
            replanner.replan(),
            Err(InterpolatedSearchError::InvalidGoal {
                point: Point2::new(4.5, 0.5),
            })
        );

        let mut fallback_grid = Grid::new(3, 1).expect("grid dimensions are valid");
        let fallback_request =
            InterpolatedSearchRequest::new(Point2::new(0.5, 0.5), Point2::new(2.5, 0.5));
        replanner
            .initialize(&fallback_grid, fallback_request)
            .expect("test request should be valid");
        let fallback_barrier = Point::new(1, 0);
        fallback_grid
            .set_cell(fallback_barrier, Cell::Blocked)
            .expect("fallback barrier should be valid");
        replanner.update_cell(fallback_barrier, Cell::Blocked);
        let fallback = replanner.replan().expect("test request should be valid");
        let fallback_outcome = fallback
            .path_outcome()
            .expect("fallback result should expose a path outcome");
        assert_eq!(fallback.expected_kind(), InterpolatedExpectedKind::Fallback);
        assert_eq!(
            fallback_outcome.kind(),
            InterpolatedPathOutcomeKind::Fallback
        );
        assert_eq!(fallback_outcome.witness_points(), 1);
    }
}