condor-pathfinding-grid 0.4.0

//! Private candidate contract: Edmonds-Karp flow bound for the MAPF family.
//!
//! **Hypothesis:** a unit-capacity max-flow over the agent→goal reachability
//! bipartite graph gives a cheap a-priori feasibility bound — how many agents
//! can simultaneously receive distinct reachable goals — before any expensive
//! per-agent planning or assignment runs.
//!
//! **Non-negotiable behavior:** the bound equals the maximum bipartite
//! matching size under 4-connected reachability; it is an upper bound on
//! solvable agent count, never a plan; endpoint validation matches the family
//! (walkable starts and goals required).
//!
//! **Evidence and promotion:** ordinary `mapf` route (`just test-fast mapf`,
//! `just clippy-target mapf`); promotion requires evidence that the bound
//! prunes real infeasible instances ahead of
//! [`crate::mapf::MapfStarterPlanner`]. This remains private: no feature,
//! fixture, target, or separate harness route.

use std::collections::VecDeque;

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

/// Failure to evaluate the flow bound.
#[derive(Debug, Clone, PartialEq, Eq)]
pub enum FlowBoundError {
    /// A start or goal cell is out of bounds or not walkable.
    InvalidEndpoint { point: Point },
}

/// Candidate max-flow feasibility bound for distinct-goal MAPF instances.
#[derive(Debug, Default, Clone, Copy)]
pub struct MapfFlowBounds;

impl MapfFlowBounds {
    /// Stable algorithm identifier for logs and future portfolio wiring.
    pub const NAME: &str = "mapf-edmonds-karp-bound";

    /// Maximum number of agents that can receive distinct reachable goals.
    pub fn max_assignable(
        &self,
        grid: &Grid,
        starts: &[Point],
        goals: &[Point],
    ) -> Result<usize, FlowBoundError> {
        for &point in starts.iter().chain(goals) {
            if !grid.is_walkable(point) {
                return Err(FlowBoundError::InvalidEndpoint { point });
            }
        }

        // Node ids: 0 = source, 1..=starts = agents,
        // starts+1..=starts+goals = goals, last = sink.
        let source = 0usize;
        let sink = starts.len() + goals.len() + 1;
        let mut network = FlowNetwork::new(sink + 1);

        for (agent, &start) in starts.iter().enumerate() {
            network.add_edge(source, 1 + agent, 1);
            let reachable = reachable_cells(grid, start);
            for (goal_position, &goal) in goals.iter().enumerate() {
                let goal_index = grid
                    .index_of(goal)
                    .expect("walkable goals are inside the grid");
                if reachable[goal_index] {
                    network.add_edge(1 + agent, 1 + starts.len() + goal_position, 1);
                }
            }
        }
        for goal_position in 0..goals.len() {
            network.add_edge(1 + starts.len() + goal_position, sink, 1);
        }

        Ok(network.max_flow(source, sink))
    }
}

/// BFS reachability mask from `start` over walkable 4-connected cells.
fn reachable_cells(grid: &Grid, start: Point) -> Vec<bool> {
    let start_index = grid
        .index_of(start)
        .expect("walkable starts are inside the grid");
    let mut reachable = vec![false; grid.cell_count()];
    let mut frontier = VecDeque::from([start_index]);
    reachable[start_index] = true;

    while let Some(current_index) = frontier.pop_front() {
        let current = grid.point_from_index(current_index);
        for neighbor in grid.neighbors4(current) {
            let neighbor_index = grid
                .index_of(neighbor)
                .expect("walkable neighbors must exist inside the grid");
            if !reachable[neighbor_index] {
                reachable[neighbor_index] = true;
                frontier.push_back(neighbor_index);
            }
        }
    }

    reachable
}

/// Adjacency-list residual network with paired forward/backward edges.
struct FlowNetwork {
    adjacency: Vec<Vec<usize>>,
    targets: Vec<usize>,
    capacities: Vec<usize>,
}

impl FlowNetwork {
    fn new(node_count: usize) -> Self {
        Self {
            adjacency: vec![Vec::new(); node_count],
            targets: Vec::new(),
            capacities: Vec::new(),
        }
    }

    fn add_edge(&mut self, from: usize, to: usize, capacity: usize) {
        self.adjacency[from].push(self.targets.len());
        self.targets.push(to);
        self.capacities.push(capacity);
        self.adjacency[to].push(self.targets.len());
        self.targets.push(from);
        self.capacities.push(0);
    }

    /// Edmonds-Karp: repeated BFS augmenting paths in the residual network.
    fn max_flow(&mut self, source: usize, sink: usize) -> usize {
        let mut total_flow = 0usize;

        loop {
            let mut incoming_edge: Vec<Option<usize>> = vec![None; self.adjacency.len()];
            let mut frontier = VecDeque::from([source]);

            'bfs: while let Some(node) = frontier.pop_front() {
                for &edge in &self.adjacency[node] {
                    let target = self.targets[edge];
                    if self.capacities[edge] > 0
                        && incoming_edge[target].is_none()
                        && target != source
                    {
                        incoming_edge[target] = Some(edge);
                        if target == sink {
                            break 'bfs;
                        }
                        frontier.push_back(target);
                    }
                }
            }

            let Some(mut edge) = incoming_edge[sink] else {
                return total_flow;
            };

            let mut bottleneck = usize::MAX;
            loop {
                bottleneck = bottleneck.min(self.capacities[edge]);
                let previous = self.targets[edge ^ 1];
                match incoming_edge[previous] {
                    Some(previous_edge) if previous != source => edge = previous_edge,
                    _ => break,
                }
            }

            let mut apply = incoming_edge[sink].expect("augmenting path was found");
            loop {
                self.capacities[apply] -= bottleneck;
                self.capacities[apply ^ 1] += bottleneck;
                let previous = self.targets[apply ^ 1];
                match incoming_edge[previous] {
                    Some(previous_edge) if previous != source => apply = previous_edge,
                    _ => break,
                }
            }

            total_flow += bottleneck;
        }
    }
}

#[cfg(test)]
mod tests {
    use crate::{
        grid::{Cell, Grid},
        mapf_flow_bounds::{FlowBoundError, MapfFlowBounds},
        point::Point,
    };

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

        let bound = MapfFlowBounds
            .max_assignable(&grid, &starts, &goals)
            .expect("endpoints are walkable");
        assert_eq!(bound, 2);
    }

    #[test]
    fn wall_limits_the_bound_to_reachable_pairs() {
        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");
        }
        // Both agents on the left, both goals on the right: nothing is assignable.
        let starts = [Point::new(0, 0), Point::new(0, 2)];
        let goals = [Point::new(4, 0), Point::new(4, 2)];

        let bound = MapfFlowBounds
            .max_assignable(&grid, &starts, &goals)
            .expect("endpoints are walkable");
        assert_eq!(bound, 0);
    }

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

        let bound = MapfFlowBounds
            .max_assignable(&grid, &starts, &goals)
            .expect("endpoints are walkable");
        assert_eq!(bound, 1);
    }

    #[test]
    fn blocked_endpoint_is_an_explicit_error() {
        let mut grid = Grid::new(3, 3).expect("grid dimensions are valid");
        grid.set_cell(Point::new(1, 1), Cell::Blocked)
            .expect("valid grid edit");

        assert_eq!(
            MapfFlowBounds.max_assignable(&grid, &[Point::new(1, 1)], &[Point::new(0, 0)]),
            Err(FlowBoundError::InvalidEndpoint {
                point: Point::new(1, 1)
            })
        );
    }
}