condor_grid/

mapf.rs

//! Collision-aware multi-agent pathfinding model, validator, and starter planner.
//!
//! [`MapfProblem`] combines a static 4-connected grid with time-indexed agent paths;
//! [`MapfProblem::validate_plan`] reports structural, vertex, and edge-swap conflicts.
//! [`MapfStarterPlanner`] plans agents in fixed order with reservations and returns a
//! solved or partial failed outcome, not an optimality guarantee. Prefer
//! [`crate::flow_field`] when agents merely share a goal and do not need collision
//! resolution. The harness owns corpus and conformance evidence for this lane.

use std::collections::{BTreeMap, BTreeSet, VecDeque};

use serde::Deserialize;

use crate::{Cell, Grid, Point};

/// One MAPF agent with a static start cell and a static goal cell.
///
/// Plans must occupy `start` at timestep 0, reach `goal`, then wait in place through the
/// planner horizon. Identity strings are opaque keys used in conflict reports.
#[derive(Debug, Clone, PartialEq, Eq)]
pub struct MapfAgent {
    /// Stable agent identity used in plans and conflict reports.
    pub agent_id: String,
    /// Required walkable start cell at timestep 0.
    pub start: Point,
    /// Required walkable goal; plans must reach and then stay on this cell.
    pub goal: Point,
}

impl MapfAgent {
    /// Creates an agent with the given identity and static endpoints.
    #[must_use]
    pub fn new(agent_id: impl Into<String>, start: Point, goal: Point) -> Self {
        Self {
            agent_id: agent_id.into(),
            start,
            goal,
        }
    }
}

/// Objective label used by the MAPF validation foundation.
///
/// Metrics are always computed for both labels; the objective only tags the
/// problem for reporting.
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub enum MapfObjective {
    /// Minimize the last agent arrival time.
    Makespan,
    /// Minimize the sum of individual first-arrival times.
    SumOfCosts,
}

impl MapfObjective {
    /// Returns the stable artifact slug for this objective.
    #[must_use]
    pub const fn slug(self) -> &'static str {
        match self {
            Self::Makespan => "makespan",
            Self::SumOfCosts => "sum-of-costs",
        }
    }
}

/// Static-grid MAPF problem: grid, agents, and objective label.
#[derive(Debug, Clone, PartialEq, Eq)]
pub struct MapfProblem {
    /// Stable problem identity for fixtures and capture reports.
    pub problem_id: String,
    /// Shared static map; movement is 4-way with wait-in-place.
    pub grid: Grid,
    /// Agents in planning order for the starter planner.
    pub agents: Vec<MapfAgent>,
    /// Reporting objective label (does not change validation rules).
    pub objective: MapfObjective,
}

impl MapfProblem {
    /// Validates `plan` against this problem's agents, grid, and conflict rules.
    ///
    /// Always returns a full [`MapfValidationReport`]; `valid` is true only when
    /// `conflicts` is empty.
    #[must_use]
    pub fn validate_plan(&self, plan: &MapfPlan) -> MapfValidationReport {
        validate_mapf_plan(self, plan)
    }
}

/// Deterministic fixed-order reservation-table starter baseline for MAPF problems.
///
/// # Contract
///
/// - Agents are planned **in problem order** with a shared reservation table.
/// - Search is bounded by the inclusive [`Self::max_timesteps`] horizon.
/// - Successful agent paths are padded to `max_timesteps + 1` positions
///   (wait-in-place after first arrival).
/// - Failure at any agent yields [`MapfStarterPlannerOutcome::Failed`] with a
///   partial plan for already-planned agents only.
/// - A fully built plan is re-validated before reporting success; validation
///   failures surface as [`MapfStarterPlannerFailure::UnvalidatedPlan`].
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub struct MapfStarterPlanner {
    max_timesteps: usize,
}

impl MapfStarterPlanner {
    /// Hard ceiling applied when constructing the planner (prevents runaway horizons).
    pub const MAX_TIMESTEPS: usize = 1 << 20;

    /// Creates a bounded starter planner with an inclusive maximum timestep.
    ///
    /// Values above [`Self::MAX_TIMESTEPS`] are clamped.
    #[must_use]
    pub const fn new(max_timesteps: usize) -> Self {
        let max_timesteps = if max_timesteps > Self::MAX_TIMESTEPS {
            Self::MAX_TIMESTEPS
        } else {
            max_timesteps
        };
        Self { max_timesteps }
    }

    /// Inclusive maximum timestep used for search and path padding.
    #[must_use]
    pub const fn max_timesteps(self) -> usize {
        self.max_timesteps
    }

    /// Plans agents in problem order and validates the final plan before reporting success.
    #[must_use]
    pub fn plan(&self, problem: &MapfProblem) -> MapfStarterPlannerResult {
        let mut reservations = ReservationTable::default();
        let mut planned_paths = Vec::with_capacity(problem.agents.len());

        for agent in &problem.agents {
            let path = match self.plan_agent_path(problem, agent, &reservations) {
                Ok(path) => path,
                Err(reason) => {
                    return MapfStarterPlannerResult::failed(reason, MapfPlan::new(planned_paths));
                }
            };

            reservations.reserve_path(&path, self.max_timesteps);
            planned_paths.push(MapfAgentPath::new(
                agent.agent_id.clone(),
                pad_path_to_horizon(path, self.max_timesteps),
            ));
        }

        let plan = MapfPlan::new(planned_paths);
        let validation = problem.validate_plan(&plan);
        if validation.valid {
            MapfStarterPlannerResult::solved(plan, validation)
        } else {
            MapfStarterPlannerResult::failed(
                MapfStarterPlannerFailure::UnvalidatedPlan { validation },
                plan,
            )
        }
    }

    fn plan_agent_path(
        &self,
        problem: &MapfProblem,
        agent: &MapfAgent,
        reservations: &ReservationTable,
    ) -> Result<Vec<Point>, MapfStarterPlannerFailure> {
        if !problem.grid.is_walkable(agent.start) {
            return Err(MapfStarterPlannerFailure::InvalidStart {
                agent_id: agent.agent_id.clone(),
                point: agent.start,
            });
        }
        if !problem.grid.is_walkable(agent.goal) {
            return Err(MapfStarterPlannerFailure::InvalidGoal {
                agent_id: agent.agent_id.clone(),
                point: agent.goal,
            });
        }
        if reservations.vertex_reserved(0, agent.start) {
            return Err(MapfStarterPlannerFailure::StartReserved {
                agent_id: agent.agent_id.clone(),
                point: agent.start,
            });
        }

        let start_state = TimedPoint {
            timestep: 0,
            point: agent.start,
        };
        let mut frontier = VecDeque::from([start_state]);
        let mut parents = BTreeMap::from([(start_state, None)]);

        while let Some(state) = frontier.pop_front() {
            if state.point == agent.goal
                && reservations.vertex_available_from(
                    state.timestep,
                    agent.goal,
                    self.max_timesteps,
                )
            {
                return Ok(reconstruct_timed_path(state, &parents));
            }

            if state.timestep >= self.max_timesteps {
                continue;
            }

            for next_point in ordered_mapf_moves(&problem.grid, state.point, agent.goal) {
                let next_state = TimedPoint {
                    timestep: state.timestep + 1,
                    point: next_point,
                };
                if parents.contains_key(&next_state)
                    || !reservations.transition_allowed(state.timestep + 1, state.point, next_point)
                {
                    continue;
                }

                parents.insert(next_state, Some(state));
                frontier.push_back(next_state);
            }
        }

        Err(MapfStarterPlannerFailure::NoPathWithinHorizon {
            agent_id: agent.agent_id.clone(),
            max_timesteps: self.max_timesteps,
        })
    }
}

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

/// Result from the bounded MAPF starter planner.
#[derive(Debug, Clone, PartialEq, Eq)]
pub struct MapfStarterPlannerResult {
    /// Explicit solved or failed payload.
    pub outcome: MapfStarterPlannerOutcome,
}

impl MapfStarterPlannerResult {
    /// Returns true when the outcome is a validated full plan.
    #[must_use]
    pub const fn is_solved(&self) -> bool {
        matches!(self.outcome, MapfStarterPlannerOutcome::Solved { .. })
    }

    /// Returns the solved plan only when the planner outcome is solved.
    #[must_use]
    pub const fn solved_plan(&self) -> Option<&MapfPlan> {
        match &self.outcome {
            MapfStarterPlannerOutcome::Solved { plan, .. } => Some(plan),
            MapfStarterPlannerOutcome::Failed { .. } => None,
        }
    }

    /// Returns the partial plan only when the planner outcome is failed.
    #[must_use]
    pub const fn partial_plan(&self) -> Option<&MapfPlan> {
        match &self.outcome {
            MapfStarterPlannerOutcome::Solved { .. } => None,
            MapfStarterPlannerOutcome::Failed { partial_plan, .. } => Some(partial_plan),
        }
    }

    /// Returns any plan payload carried by the result.
    ///
    /// Use [`Self::solved_plan`] or [`Self::partial_plan`] when the outcome
    /// distinction matters.
    #[must_use]
    pub const fn plan(&self) -> Option<&MapfPlan> {
        match &self.outcome {
            MapfStarterPlannerOutcome::Solved { plan, .. } => Some(plan),
            MapfStarterPlannerOutcome::Failed { partial_plan, .. } => Some(partial_plan),
        }
    }

    /// Returns the validation report when present (solved plans always include one;
    /// failed plans include it only for [`MapfStarterPlannerFailure::UnvalidatedPlan`]).
    #[must_use]
    pub const fn validation_report(&self) -> Option<&MapfValidationReport> {
        match &self.outcome {
            MapfStarterPlannerOutcome::Solved { validation, .. } => Some(validation),
            MapfStarterPlannerOutcome::Failed {
                reason: MapfStarterPlannerFailure::UnvalidatedPlan { validation, .. },
                ..
            } => Some(validation),
            MapfStarterPlannerOutcome::Failed { .. } => None,
        }
    }

    fn solved(plan: MapfPlan, validation: MapfValidationReport) -> Self {
        Self {
            outcome: MapfStarterPlannerOutcome::Solved { plan, validation },
        }
    }

    fn failed(reason: MapfStarterPlannerFailure, partial_plan: MapfPlan) -> Self {
        Self {
            outcome: MapfStarterPlannerOutcome::Failed {
                reason,
                partial_plan,
            },
        }
    }
}

/// Explicit solved or failed starter-planner outcome.
#[derive(Debug, Clone, PartialEq, Eq)]
pub enum MapfStarterPlannerOutcome {
    /// Fully planned agents with a validation report that reported no conflicts.
    Solved {
        plan: MapfPlan,
        validation: MapfValidationReport,
    },
    /// Planning stopped early or post-plan validation failed.
    Failed {
        reason: MapfStarterPlannerFailure,
        /// Agents planned successfully before failure (may be empty).
        partial_plan: MapfPlan,
    },
}

/// Explicit bounded-failure reason for the starter planner.
#[derive(Debug, Clone, PartialEq, Eq)]
pub enum MapfStarterPlannerFailure {
    /// Agent start is blocked or out of bounds.
    InvalidStart { agent_id: String, point: Point },
    /// Agent goal is blocked or out of bounds.
    InvalidGoal { agent_id: String, point: Point },
    /// Another agent already reserved the start vertex at timestep 0.
    StartReserved { agent_id: String, point: Point },
    /// No reservation-safe path within the inclusive horizon.
    NoPathWithinHorizon {
        agent_id: String,
        max_timesteps: usize,
    },
    /// All agents planned, but re-validation found conflicts.
    UnvalidatedPlan { validation: MapfValidationReport },
}

/// Time-indexed path for one agent (`positions[t]` is the cell at timestep `t`).
#[derive(Debug, Clone, PartialEq, Eq)]
pub struct MapfAgentPath {
    /// Must match a problem agent id.
    pub agent_id: String,
    /// Positions from timestep 0 through the plan horizon.
    pub positions: Vec<Point>,
}

impl MapfAgentPath {
    /// Creates a time-indexed path; does not validate geometry or conflicts.
    #[must_use]
    pub fn new(agent_id: impl Into<String>, positions: Vec<Point>) -> Self {
        Self {
            agent_id: agent_id.into(),
            positions,
        }
    }
}

/// Time-stepped multi-agent plan indexed by agent paths.
#[derive(Debug, Clone, PartialEq, Eq)]
pub struct MapfPlan {
    /// One path per planned agent (order is not significant for validation).
    pub agent_paths: Vec<MapfAgentPath>,
}

impl MapfPlan {
    /// Creates a plan from agent paths without running validation.
    #[must_use]
    pub fn new(agent_paths: Vec<MapfAgentPath>) -> Self {
        Self { agent_paths }
    }
}

/// Validation issue or conflict found in a MAPF plan.
///
/// Structural issues (missing paths, illegal moves) are reported alongside
/// multi-agent vertex and edge-swap conflicts. Any non-empty conflict list
/// makes [`MapfValidationReport::valid`] false.
#[derive(Debug, Clone, PartialEq, Eq)]
pub enum MapfConflict {
    /// Problem start is not walkable.
    InvalidStart { agent_id: String, point: Point },
    /// Problem goal is not walkable.
    InvalidGoal { agent_id: String, point: Point },
    /// Agent path has zero positions.
    EmptyAgentPath { agent_id: String },
    /// Problem agent has no path entry in the plan.
    MissingAgentPath { agent_id: String },
    /// Plan path references an unknown agent id.
    UnknownAgentPath { agent_id: String },
    /// More than one path for the same agent id.
    DuplicateAgentPath { agent_id: String },
    /// Path timestep 0 does not match the problem start.
    StartMismatch {
        agent_id: String,
        expected: Point,
        actual: Point,
    },
    /// Path visits a cell outside the grid.
    OutOfBoundsCell {
        agent_id: String,
        timestep: usize,
        point: Point,
    },
    /// Path visits a blocked cell.
    BlockedCell {
        agent_id: String,
        timestep: usize,
        point: Point,
    },
    /// Consecutive positions are not a legal 4-way step or wait.
    IllegalTransition {
        agent_id: String,
        timestep: usize,
        from: Point,
        to: Point,
    },
    /// Path never occupies the agent goal.
    GoalNotReached {
        agent_id: String,
        expected: Point,
        actual: Point,
    },
    /// Agent left the goal after first arrival.
    GoalDeparted {
        agent_id: String,
        timestep: usize,
        goal: Point,
        actual: Point,
    },
    /// Two or more agents occupy the same cell at the same timestep.
    Vertex {
        timestep: usize,
        point: Point,
        agent_ids: Vec<String>,
    },
    /// Two agents swap edges between consecutive timesteps.
    EdgeSwap {
        timestep: usize,
        from: Point,
        to: Point,
        agent_a: String,
        agent_b: String,
    },
}

impl MapfConflict {
    /// Returns the stable kebab-case kind label for capture/export.
    #[must_use]
    pub const fn kind(&self) -> &'static str {
        match self {
            Self::InvalidStart { .. } => "invalid-start",
            Self::InvalidGoal { .. } => "invalid-goal",
            Self::EmptyAgentPath { .. } => "empty-agent-path",
            Self::MissingAgentPath { .. } => "missing-agent-path",
            Self::UnknownAgentPath { .. } => "unknown-agent-path",
            Self::DuplicateAgentPath { .. } => "duplicate-agent-path",
            Self::StartMismatch { .. } => "start-mismatch",
            Self::OutOfBoundsCell { .. } => "out-of-bounds-cell",
            Self::BlockedCell { .. } => "blocked-cell",
            Self::IllegalTransition { .. } => "illegal-transition",
            Self::GoalNotReached { .. } => "goal-not-reached",
            Self::GoalDeparted { .. } => "goal-departed",
            Self::Vertex { .. } => "vertex",
            Self::EdgeSwap { .. } => "edge-swap",
        }
    }
}

/// Objective metrics derived from a plan (first-arrival based).
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub struct MapfPlanMetrics {
    /// Maximum first-arrival timestep across agents with paths.
    pub makespan: usize,
    /// Sum of first-arrival timesteps across agents with paths.
    pub sum_of_costs: usize,
}

/// Complete MAPF plan validation result.
#[derive(Debug, Clone, PartialEq, Eq)]
pub struct MapfValidationReport {
    /// True only when `conflicts` is empty.
    pub valid: bool,
    /// Structural issues and multi-agent conflicts in discovery order.
    pub conflicts: Vec<MapfConflict>,
    /// Makespan and sum-of-costs computed from first arrivals.
    pub metrics: MapfPlanMetrics,
}

#[derive(Debug, Clone, Copy, PartialEq, Eq, PartialOrd, Ord)]
struct TimedPoint {
    timestep: usize,
    point: Point,
}

#[derive(Debug, Default, Clone, PartialEq, Eq)]
struct ReservationTable {
    vertices: BTreeSet<(usize, Point)>,
    edges: BTreeSet<(usize, Point, Point)>,
}

impl ReservationTable {
    fn reserve_path(&mut self, path: &[Point], max_timesteps: usize) {
        let Some(&last) = path.last() else {
            return;
        };

        for timestep in 0..=max_timesteps {
            let point = path.get(timestep).copied().unwrap_or(last);
            self.vertices.insert((timestep, point));

            if timestep > 0 {
                let previous = path.get(timestep - 1).copied().unwrap_or(last);
                self.edges.insert((timestep, previous, point));
            }
        }
    }

    fn vertex_reserved(&self, timestep: usize, point: Point) -> bool {
        self.vertices.contains(&(timestep, point))
    }

    fn vertex_available_from(
        &self,
        start_timestep: usize,
        point: Point,
        max_timestep: usize,
    ) -> bool {
        (start_timestep..=max_timestep).all(|timestep| !self.vertex_reserved(timestep, point))
    }

    fn transition_allowed(&self, timestep: usize, from: Point, to: Point) -> bool {
        !self.vertex_reserved(timestep, to) && !self.edges.contains(&(timestep, to, from))
    }
}

/// Cardinal neighbors plus a wait-in-place move, ordered by Manhattan-to-goal.
fn ordered_mapf_moves(grid: &Grid, current: Point, goal: Point) -> Vec<Point> {
    let mut candidates = Vec::with_capacity(5);
    candidates.push(current);
    candidates.extend(grid.neighbors4(current));
    candidates.sort_by_key(|point| (manhattan_distance(*point, goal), *point));
    candidates
}

fn reconstruct_timed_path(
    end_state: TimedPoint,
    parents: &BTreeMap<TimedPoint, Option<TimedPoint>>,
) -> Vec<Point> {
    let mut states = vec![end_state];
    let mut current = end_state;

    while let Some(parent) = parents
        .get(&current)
        .expect("timed path reconstruction requires known states")
    {
        states.push(*parent);
        current = *parent;
    }

    states.reverse();
    states.into_iter().map(|state| state.point).collect()
}

/// Pads to `max_timesteps + 1` positions (timestep 0 through horizon) by holding the goal.
fn pad_path_to_horizon(mut path: Vec<Point>, max_timesteps: usize) -> Vec<Point> {
    let target_len = max_timesteps + 1;
    let last = path
        .last()
        .copied()
        .expect("planned agent paths always include a start point");
    path.resize(target_len, last);
    path
}

fn manhattan_distance(from: Point, to: Point) -> usize {
    from.x.abs_diff(to.x) + from.y.abs_diff(to.y)
}

fn validate_mapf_plan(problem: &MapfProblem, plan: &MapfPlan) -> MapfValidationReport {
    let mut conflicts = validate_problem(problem);
    let agent_by_id = problem
        .agents
        .iter()
        .map(|agent| (agent.agent_id.as_str(), agent))
        .collect::<BTreeMap<_, _>>();
    let mut paths_by_agent: BTreeMap<&str, &MapfAgentPath> = BTreeMap::new();

    for agent_path in &plan.agent_paths {
        let agent_id = agent_path.agent_id.as_str();
        if !agent_by_id.contains_key(agent_id) {
            conflicts.push(MapfConflict::UnknownAgentPath {
                agent_id: agent_path.agent_id.clone(),
            });
            continue;
        }
        if paths_by_agent.insert(agent_id, agent_path).is_some() {
            conflicts.push(MapfConflict::DuplicateAgentPath {
                agent_id: agent_path.agent_id.clone(),
            });
        }
    }

    for agent in &problem.agents {
        let Some(path) = paths_by_agent.get(agent.agent_id.as_str()) else {
            conflicts.push(MapfConflict::MissingAgentPath {
                agent_id: agent.agent_id.clone(),
            });
            continue;
        };

        validate_agent_path(problem, agent, path, &mut conflicts);
    }

    conflicts.extend(vertex_conflicts(plan));
    conflicts.extend(edge_swap_conflicts(plan));

    let metrics = metrics_for(problem, &paths_by_agent);
    let valid = conflicts.is_empty();
    MapfValidationReport {
        valid,
        conflicts,
        metrics,
    }
}

fn validate_problem(problem: &MapfProblem) -> Vec<MapfConflict> {
    let mut conflicts = Vec::new();
    for agent in &problem.agents {
        if !problem.grid.is_walkable(agent.start) {
            conflicts.push(MapfConflict::InvalidStart {
                agent_id: agent.agent_id.clone(),
                point: agent.start,
            });
        }
        if !problem.grid.is_walkable(agent.goal) {
            conflicts.push(MapfConflict::InvalidGoal {
                agent_id: agent.agent_id.clone(),
                point: agent.goal,
            });
        }
    }
    conflicts
}

fn validate_agent_path(
    problem: &MapfProblem,
    agent: &MapfAgent,
    path: &MapfAgentPath,
    conflicts: &mut Vec<MapfConflict>,
) {
    let Some(first) = path.positions.first().copied() else {
        conflicts.push(MapfConflict::EmptyAgentPath {
            agent_id: agent.agent_id.clone(),
        });
        return;
    };

    if first != agent.start {
        conflicts.push(MapfConflict::StartMismatch {
            agent_id: agent.agent_id.clone(),
            expected: agent.start,
            actual: first,
        });
    }

    for (timestep, &point) in path.positions.iter().enumerate() {
        match problem.grid.cell(point) {
            Some(Cell::Open) => {}
            Some(Cell::Blocked) => conflicts.push(MapfConflict::BlockedCell {
                agent_id: agent.agent_id.clone(),
                timestep,
                point,
            }),
            None => conflicts.push(MapfConflict::OutOfBoundsCell {
                agent_id: agent.agent_id.clone(),
                timestep,
                point,
            }),
        }
    }

    for (transition_index, pair) in path.positions.windows(2).enumerate() {
        if !problem.grid.segment_is_walkable(pair[0], pair[1]) {
            conflicts.push(MapfConflict::IllegalTransition {
                agent_id: agent.agent_id.clone(),
                timestep: transition_index + 1,
                from: pair[0],
                to: pair[1],
            });
        }
    }

    let Some(first_arrival) = path.positions.iter().position(|&point| point == agent.goal) else {
        conflicts.push(MapfConflict::GoalNotReached {
            agent_id: agent.agent_id.clone(),
            expected: agent.goal,
            actual: *path
                .positions
                .last()
                .expect("non-empty path should still have a last point"),
        });
        return;
    };

    if let Some((offset, &actual)) = path.positions[first_arrival + 1..]
        .iter()
        .enumerate()
        .find(|(_, point)| **point != agent.goal)
    {
        conflicts.push(MapfConflict::GoalDeparted {
            agent_id: agent.agent_id.clone(),
            timestep: first_arrival + 1 + offset,
            goal: agent.goal,
            actual,
        });
    }
}

fn vertex_conflicts(plan: &MapfPlan) -> Vec<MapfConflict> {
    let mut occupancy: BTreeMap<(usize, Point), Vec<String>> = BTreeMap::new();
    let horizon = plan
        .agent_paths
        .iter()
        .map(|path| path.positions.len())
        .max()
        .unwrap_or(0);

    for path in &plan.agent_paths {
        for timestep in 0..horizon {
            let Some(point) = path_position_at_or_after(path, timestep) else {
                continue;
            };
            occupancy
                .entry((timestep, point))
                .or_default()
                .push(path.agent_id.clone());
        }
    }

    occupancy
        .into_iter()
        .filter_map(|((timestep, point), mut agent_ids)| {
            if agent_ids.len() < 2 {
                return None;
            }
            agent_ids.sort();
            Some(MapfConflict::Vertex {
                timestep,
                point,
                agent_ids,
            })
        })
        .collect()
}

fn edge_swap_conflicts(plan: &MapfPlan) -> Vec<MapfConflict> {
    let mut conflicts = Vec::new();
    let horizon = plan
        .agent_paths
        .iter()
        .map(|path| path.positions.len())
        .max()
        .unwrap_or(0);

    for (left_index, left) in plan.agent_paths.iter().enumerate() {
        for right in plan.agent_paths.iter().skip(left_index + 1) {
            for timestep in 1..horizon {
                let Some(left_from) = path_position_at_or_after(left, timestep - 1) else {
                    continue;
                };
                let Some(left_to) = path_position_at_or_after(left, timestep) else {
                    continue;
                };
                let Some(right_from) = path_position_at_or_after(right, timestep - 1) else {
                    continue;
                };
                let Some(right_to) = path_position_at_or_after(right, timestep) else {
                    continue;
                };

                if left_from == right_to && left_to == right_from && left_from != left_to {
                    conflicts.push(MapfConflict::EdgeSwap {
                        timestep,
                        from: left_from,
                        to: left_to,
                        agent_a: left.agent_id.clone(),
                        agent_b: right.agent_id.clone(),
                    });
                }
            }
        }
    }
    conflicts
}

fn path_position_at_or_after(path: &MapfAgentPath, timestep: usize) -> Option<Point> {
    path.positions
        .get(timestep)
        .copied()
        .or_else(|| path.positions.last().copied())
}

fn metrics_for(
    problem: &MapfProblem,
    paths_by_agent: &BTreeMap<&str, &MapfAgentPath>,
) -> MapfPlanMetrics {
    let mut makespan = 0usize;
    let mut sum_of_costs = 0usize;

    for agent in &problem.agents {
        let Some(path) = paths_by_agent.get(agent.agent_id.as_str()) else {
            continue;
        };
        if path.positions.is_empty() {
            continue;
        }

        let first_arrival = path
            .positions
            .iter()
            .position(|&point| point == agent.goal)
            .unwrap_or_else(|| path.positions.len() - 1);
        makespan = makespan.max(first_arrival);
        sum_of_costs += first_arrival;
    }

    MapfPlanMetrics {
        makespan,
        sum_of_costs,
    }
}

impl<'de> Deserialize<'de> for MapfObjective {
    fn deserialize<D>(deserializer: D) -> Result<Self, D::Error>
    where
        D: serde::Deserializer<'de>,
    {
        let value = String::deserialize(deserializer)?;
        match value.as_str() {
            "makespan" => Ok(Self::Makespan),
            "sum-of-costs" => Ok(Self::SumOfCosts),
            _ => Err(serde::de::Error::custom(format!(
                "unsupported MAPF objective '{value}'"
            ))),
        }
    }
}

#[cfg(test)]
mod tests {
    use super::*;

    #[test]
    fn starter_planner_reports_explicit_bounded_failure() {
        let problem = MapfProblem {
            problem_id: "starter-bounded-failure".to_string(),
            grid: Grid::new(3, 1).expect("grid dimensions are valid"),
            agents: vec![MapfAgent::new("alpha", Point::new(0, 0), Point::new(2, 0))],
            objective: MapfObjective::SumOfCosts,
        };

        let result = MapfStarterPlanner::new(1).plan(&problem);
        assert!(!result.is_solved());
        assert!(result.solved_plan().is_none());
        assert!(
            result
                .partial_plan()
                .expect("failed result carries partial plan")
                .agent_paths
                .is_empty()
        );
        match result.outcome {
            MapfStarterPlannerOutcome::Failed {
                reason:
                    MapfStarterPlannerFailure::NoPathWithinHorizon {
                        agent_id,
                        max_timesteps,
                    },
                partial_plan,
            } => {
                assert_eq!(agent_id, "alpha");
                assert_eq!(max_timesteps, 1);
                assert!(partial_plan.agent_paths.is_empty());
            }
            other => panic!("expected bounded no-path failure, got {other:?}"),
        }
    }

    #[test]
    fn starter_planner_reports_invalid_start_for_blocked_start_cell() {
        let mut grid = Grid::new(3, 1).expect("grid dimensions are valid");
        grid.set_cell(Point::new(0, 0), Cell::Blocked)
            .expect("grid edit should succeed");
        let problem = MapfProblem {
            problem_id: "starter-invalid-start".to_string(),
            grid,
            agents: vec![MapfAgent::new("alpha", Point::new(0, 0), Point::new(2, 0))],
            objective: MapfObjective::SumOfCosts,
        };

        match MapfStarterPlanner::new(64).plan(&problem).outcome {
            MapfStarterPlannerOutcome::Failed {
                reason: MapfStarterPlannerFailure::InvalidStart { agent_id, point },
                ..
            } => {
                assert_eq!(agent_id, "alpha");
                assert_eq!(point, Point::new(0, 0));
            }
            other => panic!("expected invalid-start failure, got {other:?}"),
        }
    }

    #[test]
    fn starter_planner_reports_invalid_goal_for_blocked_goal_cell() {
        let mut grid = Grid::new(3, 1).expect("grid dimensions are valid");
        grid.set_cell(Point::new(2, 0), Cell::Blocked)
            .expect("grid edit should succeed");
        let problem = MapfProblem {
            problem_id: "starter-invalid-goal".to_string(),
            grid,
            agents: vec![MapfAgent::new("alpha", Point::new(0, 0), Point::new(2, 0))],
            objective: MapfObjective::SumOfCosts,
        };

        match MapfStarterPlanner::new(64).plan(&problem).outcome {
            MapfStarterPlannerOutcome::Failed {
                reason: MapfStarterPlannerFailure::InvalidGoal { agent_id, point },
                ..
            } => {
                assert_eq!(agent_id, "alpha");
                assert_eq!(point, Point::new(2, 0));
            }
            other => panic!("expected invalid-goal failure, got {other:?}"),
        }
    }

    #[test]
    fn starter_planner_saturates_unbounded_horizon() {
        let planner = MapfStarterPlanner::new(usize::MAX);
        assert_eq!(planner.max_timesteps(), MapfStarterPlanner::MAX_TIMESTEPS);

        let problem = MapfProblem {
            problem_id: "starter-unbounded-horizon".to_string(),
            grid: Grid::new(5, 5).expect("grid dimensions are valid"),
            agents: vec![MapfAgent::new("alpha", Point::new(0, 0), Point::new(4, 4))],
            objective: MapfObjective::SumOfCosts,
        };

        assert!(planner.plan(&problem).is_solved());
    }
}