condor_grid/

flow_field.rs

//! Same-goal flow field for amortizing many independent 4-connected paths.
//!
//! [`FlowFieldBuilder::preprocess`] builds a reverse-BFS integration and direction
//! field for one walkable goal on a uniform-cost grid; [`PreparedFlowField::sample_path`]
//! then returns the standard found/no-path [`SearchResult`] for each start. Its unit
//! hop cost matches BFS distance. This is not collision-aware MAPF: use [`crate::mapf`]
//! when agents must reserve time and avoid one another. The harness owns corpus and
//! conformance evidence for this lane.

use std::collections::VecDeque;

use crate::{
    Grid, Path, Point,
    search::{GridSearchError, SearchResult},
};

/// Cardinal step stored in a prepared flow field cell.
///
/// [`Self::None`] marks the goal, blocked/out-of-bounds probes, or unreachable cells.
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub enum FlowDirection {
    /// Unreachable or goal cell (no step).
    None,
    /// Step toward decreasing x.
    Left,
    /// Step toward increasing x.
    Right,
    /// Step toward decreasing y.
    Up,
    /// Step toward increasing y.
    Down,
}

impl FlowDirection {
    /// Returns the `(dx, dy)` step for this direction, or `None` for [`Self::None`].
    #[must_use]
    pub const fn delta(self) -> Option<(isize, isize)> {
        match self {
            Self::None => None,
            Self::Left => Some((-1, 0)),
            Self::Right => Some((1, 0)),
            Self::Up => Some((0, -1)),
            Self::Down => Some((0, 1)),
        }
    }
}

/// Builds a [`PreparedFlowField`] for one goal on a uniform-cost 4-connected grid.
///
/// Preprocess is reverse BFS (integration + directions). Rejects non-unit
/// `traversal_cost`. Prefer when many agents share one static goal without MAPF.
#[derive(Debug, Clone, Copy, Default)]
pub struct FlowFieldBuilder;

impl FlowFieldBuilder {
    /// Creates a default builder (stateless).
    #[must_use]
    pub const fn new() -> Self {
        Self
    }

    /// Preprocess a static grid for many agents sharing `goal`.
    ///
    /// # Errors
    ///
    /// Returns [`FlowFieldBuildError::InvalidGoal`] when `goal` is blocked or
    /// out of bounds, and [`FlowFieldBuildError::NonUniformCost`] when any
    /// walkable cell has a traversal cost other than `1`.
    pub fn preprocess(
        &self,
        grid: &Grid,
        goal: Point,
    ) -> Result<PreparedFlowField, FlowFieldBuildError> {
        if !grid.contains(goal) || !grid.is_walkable(goal) {
            return Err(FlowFieldBuildError::InvalidGoal { goal });
        }
        ensure_uniform_costs(grid)?;

        let cell_count = grid.cell_count();
        let mut integration = vec![None; cell_count];
        let mut directions = vec![FlowDirection::None; cell_count];
        let goal_index = grid.index_of(goal).expect("goal in bounds");

        let mut queue = VecDeque::from([goal_index]);
        integration[goal_index] = Some(0u32);

        while let Some(index) = queue.pop_front() {
            let cost = integration[index].expect("enqueued cells have integration");
            let point = grid.point_from_index(index);
            for next in grid.neighbors4(point) {
                let Some(next_index) = grid.index_of(next) else {
                    continue;
                };
                if integration[next_index].is_some() {
                    continue;
                }
                integration[next_index] = Some(cost + 1);
                queue.push_back(next_index);
            }
        }

        for index in 0..cell_count {
            let point = grid.point_from_index(index);
            if !grid.is_walkable(point) {
                continue;
            }
            let Some(here) = integration[index] else {
                continue;
            };
            if point == goal {
                directions[index] = FlowDirection::None;
                continue;
            }

            let mut best_dir = FlowDirection::None;
            let mut best_cost = here;
            for neighbor in grid.neighbors4(point) {
                let Some(n_index) = grid.index_of(neighbor) else {
                    continue;
                };
                let Some(n_cost) = integration[n_index] else {
                    continue;
                };
                if n_cost >= best_cost {
                    continue;
                }
                best_cost = n_cost;
                best_dir = if neighbor.x + 1 == point.x {
                    FlowDirection::Left
                } else if neighbor.x == point.x + 1 {
                    FlowDirection::Right
                } else if neighbor.y + 1 == point.y {
                    FlowDirection::Up
                } else {
                    FlowDirection::Down
                };
            }
            directions[index] = best_dir;
        }

        Ok(PreparedFlowField {
            grid: grid.clone(),
            goal,
            integration,
            directions,
        })
    }
}

/// Prepared same-goal flow field (integration + direction per cell).
///
/// Immutable after [`FlowFieldBuilder::preprocess`]. Safe to sample from many
/// independent starts without mutating the field.
#[derive(Debug, Clone)]
pub struct PreparedFlowField {
    grid: Grid,
    goal: Point,
    integration: Vec<Option<u32>>,
    directions: Vec<FlowDirection>,
}

impl PreparedFlowField {
    /// Stable algorithm label for capture reports.
    #[must_use]
    pub fn name(&self) -> &'static str {
        "flow-field"
    }

    /// Grid snapshot used during preprocess.
    #[must_use]
    pub fn grid(&self) -> &Grid {
        &self.grid
    }

    /// Shared goal the field was built for.
    #[must_use]
    pub fn goal(&self) -> Point {
        self.goal
    }

    /// Grid width in cells.
    #[must_use]
    pub fn width(&self) -> usize {
        self.grid.width()
    }

    /// Grid height in cells.
    #[must_use]
    pub fn height(&self) -> usize {
        self.grid.height()
    }

    /// Distance-to-goal in grid steps, if the cell can reach the goal.
    #[must_use]
    pub fn integration_at(&self, point: Point) -> Option<u32> {
        let index = self.grid.index_of(point)?;
        self.integration[index]
    }

    /// Greedy cardinal step stored for `point`, or [`FlowDirection::None`] when
    /// out of bounds, blocked, goal, or unreachable.
    #[must_use]
    pub fn direction_at(&self, point: Point) -> FlowDirection {
        self.grid
            .index_of(point)
            .map(|index| self.directions[index])
            .unwrap_or(FlowDirection::None)
    }

    /// Greedy walk along the flow field from `start` toward the goal.
    ///
    /// On uniform unit-cost 4-way grids, length matches A\*. Stops on goal,
    /// stuck cell, or `max_steps` (default: width\*height).
    ///
    /// # Errors
    ///
    /// Returns [`GridSearchError::InvalidStart`] when `start` is blocked or
    /// outside the prepared grid.
    pub fn sample_path(&self, start: Point) -> SearchResult {
        self.sample_path_limited(
            start,
            self.grid.width().saturating_mul(self.grid.height()).max(1),
        )
    }

    /// Samples at most `max_steps` flow edges.
    ///
    /// # Errors
    ///
    /// Returns [`GridSearchError::InvalidStart`] when `start` is blocked or
    /// outside the prepared grid. [`GridSearchError::StepLimitReached`] reports
    /// a reachable sample truncated by `max_steps`.
    pub fn sample_path_limited(&self, start: Point, max_steps: usize) -> SearchResult {
        if !self.grid.is_walkable(start) {
            return Err(GridSearchError::InvalidStart { point: start });
        }
        if self.integration_at(start).is_none() {
            return crate::search::not_found(0);
        }
        if start == self.goal {
            return crate::search::found(
                Path::from_steps(vec![start]).expect("flow paths always contain their start"),
                1,
            );
        }

        let mut steps = vec![start];
        let mut current = start;
        let mut visited = 1usize;

        for _ in 0..max_steps {
            let dir = self.direction_at(current);
            let Some((dx, dy)) = dir.delta() else {
                break;
            };
            let nx = current.x as isize + dx;
            let ny = current.y as isize + dy;
            if nx < 0 || ny < 0 {
                break;
            }
            let next = Point::new(nx as usize, ny as usize);
            if !self.grid.contains(next) || !self.grid.is_walkable(next) {
                break;
            }
            // Stop if the field would bounce between two cells.
            if steps.len() >= 2 && steps[steps.len() - 2] == next {
                break;
            }
            steps.push(next);
            visited += 1;
            current = next;
            if current == self.goal {
                return crate::search::found(
                    Path::from_steps(steps).expect("flow paths always contain their start"),
                    visited,
                );
            }
        }

        if current == self.goal {
            crate::search::found(
                Path::from_steps(steps).expect("flow paths always contain their start"),
                visited,
            )
        } else {
            Err(GridSearchError::StepLimitReached {
                max_steps,
                reached: current,
            })
        }
    }
}

/// Error when building a flow field.
#[derive(Debug, Clone, PartialEq, Eq, thiserror::Error)]
#[non_exhaustive]
pub enum FlowFieldBuildError {
    /// Goal is blocked or outside the grid.
    #[error("flow field goal {goal:?} must be a walkable in-bounds cell")]
    InvalidGoal { goal: Point },
    /// Grid has a non-unit traversal cost (v0 supports uniform cost only).
    #[error("flow field supports only uniform cost; cell {point:?} has cost {cost}")]
    NonUniformCost { point: Point, cost: usize },
}

fn ensure_uniform_costs(grid: &Grid) -> Result<(), FlowFieldBuildError> {
    for y in 0..grid.height() {
        for x in 0..grid.width() {
            let p = Point::new(x, y);
            if let Some(cost) = grid.traversal_cost(p)
                && cost != 1
            {
                return Err(FlowFieldBuildError::NonUniformCost { point: p, cost });
            }
        }
    }
    Ok(())
}