condor-pathfinding-grid 0.4.0

//! Private candidate contract: Yen k-shortest loopless paths for static weighted grids.
//!
//! **Hypothesis:** ranked loopless alternatives (Yen's algorithm over the
//! family Dijkstra kernel) give patrol-variation and corridor-redundancy
//! evidence that single-path and all-optimal enumeration cannot: the k best
//! paths may have *different* costs.
//!
//! **Non-negotiable behavior:** paths are loopless, strictly ranked by the
//! established weighted cost model (sum of entered-cell `traversal_cost`),
//! deterministic under cost ties, and the first path always equals the
//! Dijkstra optimum. Standard invalid/found/no-path outcome; a shared
//! [`BudgetWatch`] spans all inner searches.
//!
//! **Evidence and promotion:** ordinary `grid_core` route
//! (`just test-fast grid_core`, `just clippy-target grid_core`). Not a
//! [`crate::Pathfinder`], so grid conformance and the bench lane do not cover
//! it directly: promotion is a product-need decision (G-GRID sign-off) plus a
//! small bench-lane extension bounding k-path overhead against a single
//! Dijkstra query; the first returned path must stay pinned to the Dijkstra
//! optimum. This remains
//! private: no feature, fixture, target, or separate harness route.

use std::{
    cmp::Ordering,
    collections::{BTreeSet, BinaryHeap},
};

use crate::{
    grid::Grid,
    path::Path,
    search::{BudgetWatch, GridSearchError, SearchOutcome, SearchRequest, SearchStats},
};

/// Result type for ranked-path search: up to `k` paths or no-path, with stats.
pub type KShortestPathsResult = Result<SearchOutcome<Vec<Path>, SearchStats>, GridSearchError>;

/// Candidate ranked enumerator of the `k` shortest loopless paths.
///
/// Cost model: sums per-cell `traversal_cost` on 4-connected edges, matching
/// [`super::dijkstra::Dijkstra`]. Returns fewer than `k` paths when the grid
/// admits fewer loopless routes.
#[derive(Debug, Clone, Copy)]
pub struct YenKShortest {
    /// Maximum number of ranked paths to return.
    pub k: usize,
}

impl Default for YenKShortest {
    fn default() -> Self {
        Self { k: 3 }
    }
}

impl YenKShortest {
    /// Stable algorithm identifier for logs and future portfolio wiring.
    pub const NAME: &str = "yen-k-shortest";

    /// Finds up to `k` ranked loopless paths for `request`.
    pub fn k_shortest(&self, grid: &Grid, request: SearchRequest) -> KShortestPathsResult {
        crate::search::validate_request(grid, request)?;
        let (Some(start_index), Some(goal_index)) =
            (grid.index_of(request.start), grid.index_of(request.goal))
        else {
            return Ok(SearchOutcome::no_path(SearchStats { visited_nodes: 0 }));
        };

        if self.k == 0 {
            return Ok(SearchOutcome::no_path(SearchStats { visited_nodes: 0 }));
        }

        if request.start == request.goal {
            let path =
                Path::from_steps(vec![request.start]).expect("path contains at least one point");
            return Ok(SearchOutcome::found(
                vec![path],
                SearchStats { visited_nodes: 1 },
            ));
        }

        let watch = BudgetWatch::start(request.budget);
        let mut visited_nodes = 0usize;
        let no_bans = Bans::none(grid.cell_count());

        let Some(first) = shortest_with_bans(
            grid,
            start_index,
            goal_index,
            &no_bans,
            &watch,
            &mut visited_nodes,
        )?
        else {
            return Ok(SearchOutcome::no_path(SearchStats { visited_nodes }));
        };

        let mut accepted: Vec<RankedPath> = vec![first];
        let mut candidates: BTreeSet<RankedPath> = BTreeSet::new();

        while accepted.len() < self.k {
            let previous = accepted.last().expect("at least the first path exists");

            for spur_position in 0..previous.nodes.len() - 1 {
                let root = &previous.nodes[..=spur_position];
                let mut bans = Bans::none(grid.cell_count());

                for path in &accepted {
                    if path.nodes.len() > spur_position + 1 && path.nodes[..=spur_position] == *root
                    {
                        bans.edges
                            .insert((path.nodes[spur_position], path.nodes[spur_position + 1]));
                    }
                }
                for &node in &root[..spur_position] {
                    bans.nodes[node] = true;
                }

                let spur_start = root[spur_position];
                let Some(spur) = shortest_with_bans(
                    grid,
                    spur_start,
                    goal_index,
                    &bans,
                    &watch,
                    &mut visited_nodes,
                )?
                else {
                    continue;
                };

                let mut nodes = root.to_vec();
                nodes.extend_from_slice(&spur.nodes[1..]);
                let root_cost = segment_cost(grid, root);
                let candidate = RankedPath {
                    cost: root_cost + spur.cost,
                    nodes,
                };

                if !accepted.contains(&candidate) {
                    candidates.insert(candidate);
                }
            }

            let Some(next_best) = candidates.pop_first() else {
                break;
            };
            accepted.push(next_best);
        }

        let paths = accepted
            .into_iter()
            .map(|ranked| {
                let steps: Vec<_> = ranked
                    .nodes
                    .iter()
                    .map(|&index| grid.point_from_index(index))
                    .collect();
                Path::from_steps_with_cost(steps, ranked.cost)
                    .expect("path contains at least one point")
            })
            .collect();

        Ok(SearchOutcome::found(paths, SearchStats { visited_nodes }))
    }
}

/// One ranked loopless path as node indices, ordered by cost then nodes.
#[derive(Debug, Clone, PartialEq, Eq, PartialOrd, Ord)]
struct RankedPath {
    cost: usize,
    nodes: Vec<usize>,
}

/// Banned nodes and directed edges for one spur search.
struct Bans {
    nodes: Vec<bool>,
    edges: BTreeSet<(usize, usize)>,
}

impl Bans {
    fn none(cell_count: usize) -> Self {
        Self {
            nodes: vec![false; cell_count],
            edges: BTreeSet::new(),
        }
    }
}

/// Sum of entered-cell costs along `nodes` (the first node costs nothing).
fn segment_cost(grid: &Grid, nodes: &[usize]) -> usize {
    nodes[1..]
        .iter()
        .map(|&index| {
            grid.traversal_cost(grid.point_from_index(index))
                .expect("path nodes are walkable")
        })
        .sum()
}

/// Dijkstra from `start_index` to `goal_index` honoring `bans`.
fn shortest_with_bans(
    grid: &Grid,
    start_index: usize,
    goal_index: usize,
    bans: &Bans,
    watch: &BudgetWatch,
    visited_nodes: &mut usize,
) -> Result<Option<RankedPath>, GridSearchError> {
    if bans.nodes[start_index] || bans.nodes[goal_index] {
        return Ok(None);
    }

    let mut frontier = BinaryHeap::from([FrontierEntry {
        cost_so_far: 0,
        index: start_index,
    }]);
    let mut best_costs: Vec<Option<usize>> = vec![None; grid.cell_count()];
    let mut parents: Vec<Option<usize>> = vec![None; grid.cell_count()];
    best_costs[start_index] = Some(0);

    while let Some(entry) = frontier.pop() {
        if best_costs[entry.index] != Some(entry.cost_so_far) {
            continue;
        }

        *visited_nodes += 1;
        if let Err(reason) = watch.check(*visited_nodes) {
            return Err(crate::search::budget_error(reason));
        }

        if entry.index == goal_index {
            break;
        }

        let current = grid.point_from_index(entry.index);
        for neighbor in grid.neighbors4(current) {
            let neighbor_index = grid
                .index_of(neighbor)
                .expect("walkable neighbors must exist inside the grid");
            if bans.nodes[neighbor_index] || bans.edges.contains(&(entry.index, neighbor_index)) {
                continue;
            }

            let edge_cost = grid
                .traversal_cost(neighbor)
                .expect("walkable neighbors must have a traversal cost");
            let Some(next_cost) = entry.cost_so_far.checked_add(edge_cost) else {
                continue;
            };

            if best_costs[neighbor_index].is_some_and(|best| next_cost >= best) {
                continue;
            }

            best_costs[neighbor_index] = Some(next_cost);
            parents[neighbor_index] = Some(entry.index);
            frontier.push(FrontierEntry {
                cost_so_far: next_cost,
                index: neighbor_index,
            });
        }
    }

    let Some(goal_cost) = best_costs[goal_index] else {
        return Ok(None);
    };

    let mut nodes = vec![goal_index];
    let mut current = goal_index;
    while let Some(parent) = parents[current] {
        nodes.push(parent);
        current = parent;
    }
    nodes.reverse();
    debug_assert_eq!(current, start_index);

    Ok(Some(RankedPath {
        cost: goal_cost,
        nodes,
    }))
}

#[derive(Debug, Clone, Copy, PartialEq, Eq)]
struct FrontierEntry {
    cost_so_far: usize,
    index: usize,
}

impl Ord for FrontierEntry {
    fn cmp(&self, other: &Self) -> Ordering {
        other
            .cost_so_far
            .cmp(&self.cost_so_far)
            .then_with(|| other.index.cmp(&self.index))
    }
}

impl PartialOrd for FrontierEntry {
    fn partial_cmp(&self, other: &Self) -> Option<Ordering> {
        Some(self.cmp(other))
    }
}

#[cfg(test)]
mod tests {
    use crate::{
        algorithms::{dijkstra::Dijkstra, yen_k_shortest::YenKShortest},
        grid::{Cell, Grid},
        point::Point,
        search::{Pathfinder, SearchRequest},
    };

    #[test]
    fn first_path_matches_dijkstra_and_costs_are_ranked() {
        let mut grid = Grid::new(5, 5).expect("grid dimensions are valid");
        grid.set_cell(Point::new(2, 2), Cell::Blocked)
            .expect("valid grid edit");
        let request = SearchRequest::new(Point::new(0, 2), Point::new(4, 2));

        let outcome = YenKShortest { k: 4 }
            .k_shortest(&grid, request)
            .expect("endpoints are walkable");
        let dijkstra = Dijkstra
            .search(&grid, request)
            .expect("endpoints are walkable");

        let paths = outcome.path().expect("open detour exists");
        assert!(!paths.is_empty());
        assert_eq!(Some(paths[0].cost()), dijkstra.cost());
        for window in paths.windows(2) {
            assert!(window[0].cost() <= window[1].cost());
        }
    }

    #[test]
    fn paths_are_loopless_and_distinct() {
        let grid = Grid::new(4, 3).expect("grid dimensions are valid");
        let outcome = YenKShortest { k: 3 }
            .k_shortest(
                &grid,
                SearchRequest::new(Point::new(0, 1), Point::new(3, 1)),
            )
            .expect("endpoints are walkable");

        let paths = outcome.path().expect("open grid has paths");
        assert_eq!(paths.len(), 3);
        for path in paths {
            let mut seen = std::collections::BTreeSet::new();
            for step in path.steps() {
                assert!(seen.insert((step.x, step.y)), "path revisits {step:?}");
            }
        }
        for first_position in 0..paths.len() {
            for second_position in first_position + 1..paths.len() {
                assert_ne!(
                    paths[first_position].steps(),
                    paths[second_position].steps()
                );
            }
        }
    }

    #[test]
    fn corridor_returns_single_path() {
        let mut grid = Grid::new(5, 3).expect("grid dimensions are valid");
        for x in 0..5 {
            for y in [0usize, 2] {
                grid.set_cell(Point::new(x, y), Cell::Blocked)
                    .expect("valid grid edit");
            }
        }
        let outcome = YenKShortest { k: 5 }
            .k_shortest(
                &grid,
                SearchRequest::new(Point::new(0, 1), Point::new(4, 1)),
            )
            .expect("endpoints are walkable");
        assert_eq!(outcome.path().map(Vec::len), Some(1));
    }

    #[test]
    fn reports_when_no_path_exists() {
        let mut grid = Grid::new(3, 3).expect("grid dimensions are valid");
        for x in 0..3 {
            grid.set_cell(Point::new(x, 1), Cell::Blocked)
                .expect("valid grid edit");
        }
        let outcome = YenKShortest::default()
            .k_shortest(
                &grid,
                SearchRequest::new(Point::new(0, 0), Point::new(2, 2)),
            )
            .expect("endpoints are walkable");
        assert!(!outcome.is_found());
    }
}