condor-pathfinding-grid 0.4.0

//! Static-grid [`Pathfinder`]: bidirectional unweighted BFS.
//!
//! Each search alternates independent start and goal frontiers until they meet, then
//! returns the usual invalid/found/no-path outcome. Cost is unit hop count and
//! [`Grid::traversal_cost`](crate::Grid::traversal_cost) is ignored. Prefer
//! [`super::bfs::Bfs`] for the simpler one-frontier unweighted baseline, or
//! [`super::astar::AStar`] when cell costs vary.

use std::collections::VecDeque;

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

/// Online [`Pathfinder`]: bidirectional BFS meeting in the middle.
///
/// Cost model: unit hop count; ignores `traversal_cost`. Prefer over one-sided BFS on
/// large uniform open fields; same metric as BFS when both terminate correctly.
#[derive(Debug, Default, Clone, Copy)]
pub struct BidirectionalBfs;

impl Pathfinder for BidirectionalBfs {
    fn name(&self) -> &'static str {
        "bidirectional-bfs"
    }

    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,
            );
        }

        let mut start_frontier = VecDeque::from([start_index]);
        let mut goal_frontier = VecDeque::from([goal_index]);
        let mut distances_from_start = vec![None; grid.cell_count()];
        let mut distances_from_goal = vec![None; grid.cell_count()];
        let mut parents_from_start = vec![None; grid.cell_count()];
        let mut parents_from_goal = vec![None; grid.cell_count()];
        let mut settled_any = vec![false; grid.cell_count()];
        let mut visited_nodes = 0;
        let mut expand_from_start = true;
        let watch = BudgetWatch::start(request.budget);

        distances_from_start[start_index] = Some(0);
        distances_from_goal[goal_index] = Some(0);

        while !start_frontier.is_empty() && !goal_frontier.is_empty() {
            let meeting = if expand_from_start {
                expand_frontier(
                    grid,
                    &mut start_frontier,
                    &mut distances_from_start,
                    &distances_from_goal,
                    &mut parents_from_start,
                    &mut settled_any,
                    &mut visited_nodes,
                )
            } else {
                expand_frontier(
                    grid,
                    &mut goal_frontier,
                    &mut distances_from_goal,
                    &distances_from_start,
                    &mut parents_from_goal,
                    &mut settled_any,
                    &mut visited_nodes,
                )
            };

            if let Some(meeting_index) = meeting {
                return crate::search::found(
                    reconstruct_path(
                        grid,
                        &parents_from_start,
                        &parents_from_goal,
                        start_index,
                        goal_index,
                        meeting_index,
                    ),
                    visited_nodes,
                );
            }

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

            expand_from_start = !expand_from_start;
        }

        crate::search::not_found(visited_nodes)
    }
}

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

fn expand_frontier(
    grid: &Grid,
    frontier: &mut VecDeque<usize>,
    distances: &mut [Option<usize>],
    other_distances: &[Option<usize>],
    parents: &mut [Option<usize>],
    settled_any: &mut [bool],
    visited_nodes: &mut usize,
) -> Option<usize> {
    let layer_len = frontier.len();
    let mut best_meeting = None;

    for _ in 0..layer_len {
        let current_index = frontier
            .pop_front()
            .expect("frontier layer length must match queued entries");

        if !settled_any[current_index] {
            settled_any[current_index] = true;
            *visited_nodes += 1;
        }

        let current_distance =
            distances[current_index].expect("queued nodes must have a known distance");
        if let Some(other_distance) = other_distances[current_index] {
            update_meeting_candidate(
                &mut best_meeting,
                current_index,
                current_distance + other_distance,
            );
        }

        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 distances[neighbor_index].is_some() {
                continue;
            }

            let next_distance = current_distance + 1;
            distances[neighbor_index] = Some(next_distance);
            parents[neighbor_index] = Some(current_index);
            frontier.push_back(neighbor_index);

            if let Some(other_distance) = other_distances[neighbor_index] {
                update_meeting_candidate(
                    &mut best_meeting,
                    neighbor_index,
                    next_distance + other_distance,
                );
            }
        }
    }

    best_meeting.map(|candidate| candidate.index)
}

fn update_meeting_candidate(
    best_meeting: &mut Option<MeetingCandidate>,
    index: usize,
    total_cost: usize,
) {
    let should_replace = best_meeting
        .is_none_or(|current| (total_cost, index) < (current.total_cost, current.index));

    if should_replace {
        *best_meeting = Some(MeetingCandidate { index, total_cost });
    }
}

fn reconstruct_path(
    grid: &Grid,
    parents_from_start: &[Option<usize>],
    parents_from_goal: &[Option<usize>],
    start_index: usize,
    goal_index: usize,
    meeting_index: 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_from_start[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_from_goal[current_index]
            .expect("meeting node must have a complete goal-side parent chain");
        steps.push(grid.point_from_index(current_index));
    }

    Path::from_steps(steps).expect("path contains at least one point")
}

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

    #[test]
    fn finds_a_shortest_path_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 bidirectional_bfs = BidirectionalBfs;
        let result = bidirectional_bfs.search(
            &grid,
            SearchRequest::new(Point::new(0, 0), Point::new(4, 4)),
        );

        assert!(result.as_ref().expect("valid search request").is_found());
        assert_eq!(
            result.as_ref().expect("valid search request").cost(),
            Some(8)
        );

        let path = result
            .as_ref()
            .expect("valid search request")
            .path()
            .expect("path should exist");
        assert_eq!(path.start(), Point::new(0, 0));
        assert_eq!(path.goal(), Point::new(4, 4));
        assert!(path.steps().contains(&Point::new(2, 2)));
    }

    #[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 bidirectional_bfs = BidirectionalBfs;
        let result = bidirectional_bfs.search(
            &grid,
            SearchRequest::new(Point::new(0, 0), Point::new(2, 2)),
        );

        assert!(!result.as_ref().expect("valid search request").is_found());
        assert_eq!(result.as_ref().expect("valid search request").cost(), None);
        assert!(
            result
                .as_ref()
                .expect("valid search request")
                .stats()
                .visited_nodes
                > 0
        );
    }
}