condor-pathfinding-grid 0.4.0

//! Private candidate contract: Kuhn-Munkres target assignment for the MAPF family.
//!
//! **Hypothesis:** optimal agent→goal assignment (the Hungarian algorithm over
//! a BFS hop-distance matrix) before per-agent planning reduces total plan
//! cost versus the starter planner's fixed agent order, without touching the
//! collision model.
//!
//! **Non-negotiable behavior:** the assignment minimizes the sum of start→goal
//! shortest hop distances; every agent receives exactly one distinct goal;
//! infeasible instances (an agent that can reach no goal, or fewer goals than
//! agents) report an explicit error instead of a silent partial assignment.
//!
//! **Evidence and promotion:** ordinary `mapf` route (`just test-fast mapf`,
//! `just clippy-target mapf`); promotion requires sum-of-costs evidence against
//! [`crate::mapf::MapfStarterPlanner`] fixed-order plans. This remains private:
//! no feature, fixture, target, or separate harness route.

use std::collections::VecDeque;

use crate::{grid::Grid, point::Point};

/// Failure to compute a complete distinct-goal assignment.
#[derive(Debug, Clone, PartialEq, Eq)]
pub enum TargetAssignmentError {
    /// A start or goal cell is out of bounds or not walkable.
    InvalidEndpoint { point: Point },
    /// Fewer goals than agents: a complete assignment cannot exist.
    NotEnoughGoals { agents: usize, goals: usize },
    /// No complete assignment exists under reachability.
    Infeasible,
}

/// Complete minimum-total-distance agent→goal assignment.
#[derive(Debug, Clone, PartialEq, Eq)]
pub struct TargetAssignment {
    /// `goal_of[agent]` is the index into the input goal slice.
    pub goal_of: Vec<usize>,
    /// Sum of assigned start→goal shortest hop distances.
    pub total_distance: usize,
}

/// Candidate optimal target assigner over BFS hop distances.
#[derive(Debug, Default, Clone, Copy)]
pub struct MapfTargetAssignment;

impl MapfTargetAssignment {
    /// Stable algorithm identifier for logs and future portfolio wiring.
    pub const NAME: &str = "mapf-kuhn-munkres-assignment";

    /// Assigns each start a distinct goal minimizing total hop distance.
    pub fn assign(
        &self,
        grid: &Grid,
        starts: &[Point],
        goals: &[Point],
    ) -> Result<TargetAssignment, TargetAssignmentError> {
        if starts.len() > goals.len() {
            return Err(TargetAssignmentError::NotEnoughGoals {
                agents: starts.len(),
                goals: goals.len(),
            });
        }
        for &point in starts.iter().chain(goals) {
            if !grid.is_walkable(point) {
                return Err(TargetAssignmentError::InvalidEndpoint { point });
            }
        }
        if starts.is_empty() {
            return Ok(TargetAssignment {
                goal_of: Vec::new(),
                total_distance: 0,
            });
        }

        // cost[agent][goal] in hop distance; unreachable pairs stay forbidden.
        let cost_matrix: Vec<Vec<Option<usize>>> = starts
            .iter()
            .map(|&start| hop_distances(grid, start, goals))
            .collect();

        hungarian(&cost_matrix).ok_or(TargetAssignmentError::Infeasible)
    }
}

/// BFS hop distances from `start` to each goal (`None` when unreachable).
fn hop_distances(grid: &Grid, start: Point, goals: &[Point]) -> Vec<Option<usize>> {
    let start_index = grid
        .index_of(start)
        .expect("walkable starts are inside the grid");
    let mut distances: Vec<Option<usize>> = vec![None; grid.cell_count()];
    let mut frontier = VecDeque::from([start_index]);
    distances[start_index] = Some(0);

    while let Some(current_index) = frontier.pop_front() {
        let current = grid.point_from_index(current_index);
        let current_distance = distances[current_index].expect("frontier cells carry a distance");
        for neighbor in grid.neighbors4(current) {
            let neighbor_index = grid
                .index_of(neighbor)
                .expect("walkable neighbors must exist inside the grid");
            if distances[neighbor_index].is_none() {
                distances[neighbor_index] = Some(current_distance + 1);
                frontier.push_back(neighbor_index);
            }
        }
    }

    goals
        .iter()
        .map(|&goal| {
            grid.index_of(goal)
                .and_then(|goal_index| distances[goal_index])
        })
        .collect()
}

/// Rectangular Kuhn-Munkres over forbidden-aware costs (rows <= columns).
///
/// Classic potential formulation in O(rows^2 * columns); `None` entries are
/// modeled as a forbidden sentinel and rejected if any row must use one.
fn hungarian(cost_matrix: &[Vec<Option<usize>>]) -> Option<TargetAssignment> {
    let rows = cost_matrix.len();
    let columns = cost_matrix[0].len();
    const FORBIDDEN: u64 = 1 << 40;
    let cost = |row: usize, column: usize| -> u64 {
        cost_matrix[row][column].map_or(FORBIDDEN, |value| value as u64)
    };

    // 1-based potentials over rows and columns; way[j] backtracks the
    // augmenting alternating chain of column j.
    let mut row_potential = vec![0i64; rows + 1];
    let mut column_potential = vec![0i64; columns + 1];
    let mut assigned_row = vec![0usize; columns + 1];
    let mut way = vec![0usize; columns + 1];

    for row in 1..=rows {
        assigned_row[0] = row;
        let mut current_column = 0usize;
        let mut min_values = vec![i64::MAX; columns + 1];
        let mut used = vec![false; columns + 1];

        loop {
            used[current_column] = true;
            let active_row = assigned_row[current_column];
            let mut delta = i64::MAX;
            let mut next_column = 0usize;

            for column in 1..=columns {
                if used[column] {
                    continue;
                }
                let reduced = cost(active_row - 1, column - 1) as i64
                    - row_potential[active_row]
                    - column_potential[column];
                if reduced < min_values[column] {
                    min_values[column] = reduced;
                    way[column] = current_column;
                }
                if min_values[column] < delta {
                    delta = min_values[column];
                    next_column = column;
                }
            }

            for column in 0..=columns {
                if used[column] {
                    row_potential[assigned_row[column]] += delta;
                    column_potential[column] -= delta;
                } else {
                    min_values[column] -= delta;
                }
            }

            current_column = next_column;
            if assigned_row[current_column] == 0 {
                break;
            }
        }

        loop {
            let previous_column = way[current_column];
            assigned_row[current_column] = assigned_row[previous_column];
            current_column = previous_column;
            if current_column == 0 {
                break;
            }
        }
    }

    let mut goal_of = vec![usize::MAX; rows];
    let mut total: u64 = 0;
    for (column, &row) in assigned_row.iter().enumerate().skip(1) {
        if row == 0 {
            continue;
        }
        let pair_cost = cost(row - 1, column - 1);
        if pair_cost >= FORBIDDEN {
            return None;
        }
        goal_of[row - 1] = column - 1;
        total += pair_cost;
    }

    if goal_of.contains(&usize::MAX) {
        return None;
    }

    Some(TargetAssignment {
        goal_of,
        total_distance: usize::try_from(total).expect("hop totals fit in usize"),
    })
}

#[cfg(test)]
mod tests {
    use crate::{
        grid::{Cell, Grid},
        mapf_target_assignment::{MapfTargetAssignment, TargetAssignmentError},
        point::Point,
    };

    #[test]
    fn crossing_agents_get_the_nearer_goals() {
        let grid = Grid::new(10, 1).expect("grid dimensions are valid");
        let starts = [Point::new(0, 0), Point::new(9, 0)];
        let goals = [Point::new(8, 0), Point::new(1, 0)];

        let assignment = MapfTargetAssignment
            .assign(&grid, &starts, &goals)
            .expect("assignment is feasible");

        // Swapped assignment (1 + 1) beats the naive order (8 + 8).
        assert_eq!(assignment.goal_of, vec![1, 0]);
        assert_eq!(assignment.total_distance, 2);
    }

    #[test]
    fn extra_goals_are_left_unassigned() {
        let grid = Grid::new(5, 5).expect("grid dimensions are valid");
        let starts = [Point::new(0, 0)];
        let goals = [Point::new(4, 4), Point::new(0, 1)];

        let assignment = MapfTargetAssignment
            .assign(&grid, &starts, &goals)
            .expect("assignment is feasible");
        assert_eq!(assignment.goal_of, vec![1]);
        assert_eq!(assignment.total_distance, 1);
    }

    #[test]
    fn unreachable_goal_is_avoided() {
        let mut grid = Grid::new(5, 3).expect("grid dimensions are valid");
        for y in 0..3 {
            grid.set_cell(Point::new(2, y), Cell::Blocked)
                .expect("valid grid edit");
        }
        let starts = [Point::new(0, 0)];
        let goals = [Point::new(4, 0), Point::new(1, 0)];

        let assignment = MapfTargetAssignment
            .assign(&grid, &starts, &goals)
            .expect("assignment is feasible");
        assert_eq!(assignment.goal_of, vec![1]);
    }

    #[test]
    fn infeasible_when_all_goals_are_walled_off() {
        let mut grid = Grid::new(5, 3).expect("grid dimensions are valid");
        for y in 0..3 {
            grid.set_cell(Point::new(2, y), Cell::Blocked)
                .expect("valid grid edit");
        }
        let starts = [Point::new(0, 0)];
        let goals = [Point::new(4, 0)];

        assert_eq!(
            MapfTargetAssignment.assign(&grid, &starts, &goals),
            Err(TargetAssignmentError::Infeasible)
        );
    }

    #[test]
    fn too_few_goals_is_an_explicit_error() {
        let grid = Grid::new(3, 3).expect("grid dimensions are valid");
        let starts = [Point::new(0, 0), Point::new(2, 2)];
        let goals = [Point::new(1, 1)];

        assert_eq!(
            MapfTargetAssignment.assign(&grid, &starts, &goals),
            Err(TargetAssignmentError::NotEnoughGoals {
                agents: 2,
                goals: 1
            })
        );
    }
}