condor-pathfinding-grid 0.4.0

//! Private candidate: bidirectional radix-queue Dijkstra for weighted grids.
//!
//! **Hypothesis:** monotone radix frontiers from both endpoints can reduce
//! integer-weighted search overhead when reverse relaxation is made explicit.
//!
//! **Implementation note:** buckets are indexed by absolute path cost (Dial-style
//! dense vector), not a circular buffer sized by max edge weight. Correct for
//! positive integer costs; memory is O(max path cost), so high cell weights can
//! allocate large empty bucket vectors.
//!
//! **Non-negotiable behavior:** backward relaxation must charge the same
//! destination-cell traversal costs as the forward contract, and the meeting
//! rule must preserve checked accumulation, exact cost, path witness, no-path,
//! and invalid-input semantics. A queue threshold or one-sided early exit is
//! not a correctness proof.
//!
//! **Evidence and promotion:** ordinary `grid_core` route while implementing;
//! exact owner conformance and later weighted bench evidence. Remains private:
//! no feature, fixture, target, or separate harness route.

use std::collections::VecDeque;

use crate::{
    grid::Grid,
    path::Path,
    search::{BudgetWatch, Pathfinder, SearchRequest, SearchResult},
};

/// Online [`Pathfinder`] candidate: exact weighted bidirectional radix Dijkstra.
///
/// Cost model matches [`super::dijkstra::Dijkstra`]: forward edges charge the
/// entered cell's `traversal_cost`. Reverse edges charge the cell being left
/// (the forward destination), so meeting costs equal one-sided Dijkstra.
#[derive(Debug, Default, Clone, Copy)]
pub struct WeightedBidirectionalRadixDijkstra;

impl WeightedBidirectionalRadixDijkstra {
    /// Stable source-local candidate identity.
    pub const CANDIDATE_ID: &str = "weighted-grid/bidirectional-radix-dijkstra";
}

impl Pathfinder for WeightedBidirectionalRadixDijkstra {
    fn name(&self) -> &'static str {
        "bi-radix-dijkstra"
    }

    fn search(&self, grid: &Grid, request: SearchRequest) -> SearchResult {
        crate::search::validate_request(grid, request)?;
        let Some(start_index) = grid.index_of(request.start) else {
            return crate::search::not_found(0);
        };
        let Some(goal_index) = grid.index_of(request.goal) else {
            return crate::search::not_found(0);
        };

        if !grid.is_walkable(request.start) || !grid.is_walkable(request.goal) {
            return crate::search::not_found(0);
        }

        if !grid.is_reachable(request.start, request.goal) {
            return crate::search::not_found(0);
        }

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

        // Dial-style radix buckets keyed by path cost (monotone for positive integer weights).
        let mut forward_buckets: Vec<VecDeque<usize>> = Vec::new();
        let mut reverse_buckets: Vec<VecDeque<usize>> = Vec::new();
        let mut forward_dist = vec![None; grid.cell_count()];
        let mut reverse_dist = vec![None; grid.cell_count()];
        let mut parents_forward = vec![None; grid.cell_count()];
        let mut parents_reverse = vec![None; grid.cell_count()];
        let mut visited_nodes = 0usize;
        let watch = BudgetWatch::start(request.budget);

        forward_dist[start_index] = Some(0);
        reverse_dist[goal_index] = Some(0);
        ensure_bucket(&mut forward_buckets, 0);
        ensure_bucket(&mut reverse_buckets, 0);
        forward_buckets[0].push_back(start_index);
        reverse_buckets[0].push_back(goal_index);

        let mut best_meeting: Option<Meeting> = None;
        let mut min_forward = 0usize;
        let mut min_reverse = 0usize;

        loop {
            let forward_empty = next_nonempty(&forward_buckets, &mut min_forward).is_none();
            let reverse_empty = next_nonempty(&reverse_buckets, &mut min_reverse).is_none();
            if forward_empty && reverse_empty {
                break;
            }

            // Expand the side with the smaller current radix key (balanced bi-Dijkstra).
            let expand_forward = match (forward_empty, reverse_empty) {
                (false, true) => true,
                (true, false) => false,
                (false, false) => min_forward <= min_reverse,
                (true, true) => break,
            };

            if expand_forward {
                let Some(current_index) = pop_bucket(&mut forward_buckets, min_forward) else {
                    continue;
                };
                let Some(cost_so_far) = forward_dist[current_index] else {
                    continue;
                };
                if cost_so_far != min_forward {
                    // Stale radix entry.
                    continue;
                }

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

                if let Some(rev) = reverse_dist[current_index] {
                    let Some(total) = cost_so_far.checked_add(rev) else {
                        continue;
                    };
                    consider_meeting(&mut best_meeting, current_index, total);
                }

                // Prune when both mins already exceed best meeting.
                if let Some(best) = best_meeting
                    && min_forward.saturating_add(min_reverse) >= best.total_cost
                    && !reverse_empty
                {
                    break;
                }

                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");
                    // Forward: charge destination-cell traversal cost.
                    let edge_cost = grid
                        .traversal_cost(neighbor)
                        .expect("walkable neighbors must have a traversal cost");
                    let Some(next_cost) = cost_so_far.checked_add(edge_cost) else {
                        continue;
                    };
                    if forward_dist[neighbor_index].is_some_and(|best| next_cost >= best) {
                        continue;
                    }
                    forward_dist[neighbor_index] = Some(next_cost);
                    parents_forward[neighbor_index] = Some(current_index);
                    ensure_bucket(&mut forward_buckets, next_cost);
                    forward_buckets[next_cost].push_back(neighbor_index);
                    if next_cost < min_forward {
                        min_forward = next_cost;
                    }

                    if let Some(rev) = reverse_dist[neighbor_index] {
                        let Some(total) = next_cost.checked_add(rev) else {
                            continue;
                        };
                        consider_meeting(&mut best_meeting, neighbor_index, total);
                    }
                }
            } else {
                let Some(current_index) = pop_bucket(&mut reverse_buckets, min_reverse) else {
                    continue;
                };
                let Some(cost_so_far) = reverse_dist[current_index] else {
                    continue;
                };
                if cost_so_far != min_reverse {
                    continue;
                }

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

                if let Some(fwd) = forward_dist[current_index] {
                    let Some(total) = cost_so_far.checked_add(fwd) else {
                        continue;
                    };
                    consider_meeting(&mut best_meeting, current_index, total);
                }

                if let Some(best) = best_meeting
                    && min_forward.saturating_add(min_reverse) >= best.total_cost
                    && !forward_empty
                {
                    break;
                }

                let current = grid.point_from_index(current_index);
                let leave_cost = grid
                    .traversal_cost(current)
                    .expect("walkable reverse source must have a traversal cost");
                for neighbor in grid.neighbors4(current) {
                    let neighbor_index = grid
                        .index_of(neighbor)
                        .expect("walkable neighbors must exist inside the grid");
                    // Reverse edge current → neighbor corresponds to forward
                    // neighbor → current, which charged leave_cost (destination).
                    let Some(next_cost) = cost_so_far.checked_add(leave_cost) else {
                        continue;
                    };
                    if reverse_dist[neighbor_index].is_some_and(|best| next_cost >= best) {
                        continue;
                    }
                    reverse_dist[neighbor_index] = Some(next_cost);
                    parents_reverse[neighbor_index] = Some(current_index);
                    ensure_bucket(&mut reverse_buckets, next_cost);
                    reverse_buckets[next_cost].push_back(neighbor_index);
                    if next_cost < min_reverse {
                        min_reverse = next_cost;
                    }

                    if let Some(fwd) = forward_dist[neighbor_index] {
                        let Some(total) = next_cost.checked_add(fwd) else {
                            continue;
                        };
                        consider_meeting(&mut best_meeting, neighbor_index, total);
                    }
                }
            }
        }

        let Some(meeting) = best_meeting else {
            return crate::search::not_found(visited_nodes);
        };

        crate::search::found(
            reconstruct_path(
                grid,
                &parents_forward,
                &parents_reverse,
                start_index,
                goal_index,
                meeting.index,
                meeting.total_cost,
            ),
            visited_nodes,
        )
    }
}

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

fn consider_meeting(best: &mut Option<Meeting>, index: usize, total_cost: usize) {
    let replace =
        best.is_none_or(|current| (total_cost, index) < (current.total_cost, current.index));
    if replace {
        *best = Some(Meeting { index, total_cost });
    }
}

fn ensure_bucket(buckets: &mut Vec<VecDeque<usize>>, cost: usize) {
    if buckets.len() <= cost {
        buckets.resize_with(cost + 1, VecDeque::new);
    }
}

fn next_nonempty(buckets: &[VecDeque<usize>], min_cost: &mut usize) -> Option<usize> {
    while *min_cost < buckets.len() {
        if !buckets[*min_cost].is_empty() {
            return Some(*min_cost);
        }
        *min_cost += 1;
    }
    None
}

fn pop_bucket(buckets: &mut [VecDeque<usize>], cost: usize) -> Option<usize> {
    buckets.get_mut(cost)?.pop_front()
}

fn reconstruct_path(
    grid: &Grid,
    parents_forward: &[Option<usize>],
    parents_reverse: &[Option<usize>],
    start_index: usize,
    goal_index: usize,
    meeting_index: usize,
    total_cost: usize,
) -> Path {
    let mut steps = vec![grid.point_from_index(meeting_index)];

    let mut current_index = meeting_index;
    while current_index != start_index {
        current_index = parents_forward[current_index]
            .expect("meeting node must have a complete start-side parent chain");
        steps.push(grid.point_from_index(current_index));
    }
    steps.reverse();

    current_index = meeting_index;
    while current_index != goal_index {
        current_index = parents_reverse[current_index]
            .expect("meeting node must have a complete goal-side parent chain");
        steps.push(grid.point_from_index(current_index));
    }

    Path::from_steps_with_cost(steps, total_cost).expect("path contains at least one point")
}

#[cfg(test)]
mod tests {
    use crate::{
        algorithms::{
            dijkstra::Dijkstra,
            weighted_bidirectional_radix_dijkstra::WeightedBidirectionalRadixDijkstra,
        },
        grid::{Cell, Grid},
        point::Point,
        search::{BudgetExhausted, GridSearchError, Pathfinder, SearchBudget, SearchRequest},
    };

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

        let candidate = WeightedBidirectionalRadixDijkstra
            .search(&grid, request)
            .expect("endpoints are walkable");
        let baseline = Dijkstra
            .search(&grid, request)
            .expect("endpoints are walkable");

        assert!(candidate.is_found());
        assert_eq!(candidate.cost(), baseline.cost());
    }

    #[test]
    fn matches_dijkstra_cost_on_a_weighted_detour() {
        let mut grid = Grid::new(4, 3).expect("grid dimensions are valid");
        grid.set_traversal_cost(Point::new(1, 1), 10)
            .expect("valid cost edit");
        grid.set_traversal_cost(Point::new(2, 1), 10)
            .expect("valid cost edit");
        let request = SearchRequest::new(Point::new(0, 1), Point::new(3, 1));

        let candidate = WeightedBidirectionalRadixDijkstra
            .search(&grid, request)
            .expect("endpoints are walkable");
        let baseline = Dijkstra
            .search(&grid, request)
            .expect("endpoints are walkable");

        assert!(candidate.is_found());
        assert_eq!(candidate.cost(), baseline.cost());
    }

    #[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 result = WeightedBidirectionalRadixDijkstra
            .search(
                &grid,
                SearchRequest::new(Point::new(0, 0), Point::new(2, 2)),
            )
            .expect("endpoints are walkable");
        assert!(!result.is_found());
    }

    #[test]
    fn expansion_budget_stops_before_goal() {
        let grid = Grid::new(6, 1).expect("grid dimensions are valid");
        let request = SearchRequest::new(Point::new(0, 0), Point::new(5, 0))
            .with_budget(SearchBudget::max_expansions(2));
        let error = WeightedBidirectionalRadixDijkstra
            .search(&grid, request)
            .expect_err("budget should exhaust on a long corridor");
        assert_eq!(
            error,
            GridSearchError::BudgetExhausted(BudgetExhausted::Expansions {
                limit: 2,
                expansions: 2
            })
        );
    }

    #[test]
    fn retains_candidate_id() {
        assert_eq!(
            WeightedBidirectionalRadixDijkstra::CANDIDATE_ID,
            "weighted-grid/bidirectional-radix-dijkstra"
        );
    }
}