condor-pathfinding-navmesh 0.4.0

//! Static [`NavmeshPathfinder`]: cell-corridor BFS + funnel string-pull.
//!
//! # Surface
//!
//! One-shot over a validated static [`Navmesh`]. No
//! preprocess, no cross-query cache. For multi-query prepared routing use TRA*
//! builders in [`super::tra_star`]. For availability overlays, materialize a
//! static snapshot first—this solver never reads [`DynamicNavmeshState`](crate::DynamicNavmeshState).
//!
//! # Cost and behavior
//!
//! **Pipeline**: connectivity precheck → BFS cell corridor → portal chain →
//! midpoint seeds → [`pull_string`](crate::navmesh::funnel::pull_string) → walkability check.
//!
//! **Cost / stats**: returned path length is Euclidean geometric (no per-cell
//! weights); `visited_nodes` counts BFS cell expansions.

use std::collections::VecDeque;

use crate::navmesh::points_equal;
use crate::{
    Navmesh, NavmeshPathfinder, NavmeshQuery, NavmeshQueryResult, NavmeshSearchResult, Point2,
    PolygonPath,
};

/// Stateless corridor-BFS pathfinder with portal-midpoint funnel seeding.
///
/// Prefer when each query is independent and preprocess cost is not amortized.
/// Invalid endpoints map to [`NavmeshSearchError`](crate::NavmeshSearchError);
/// disconnected cell graphs yield no-path with zero expansions when the
/// connectivity precheck fails.
#[derive(Debug, Clone, Copy, Default, PartialEq, Eq)]
pub struct ChannelSearch;

impl NavmeshPathfinder for ChannelSearch {
    fn name(&self) -> &'static str {
        "channel-search"
    }

    fn search(&self, navmesh: &Navmesh, query: NavmeshQuery) -> NavmeshSearchResult {
        let (start_cell, goal_cell) = match navmesh.query(query) {
            NavmeshQueryResult::Connected {
                start_cell,
                goal_cell,
            } => (start_cell, goal_cell),
            NavmeshQueryResult::InvalidStart => {
                return Err(crate::NavmeshSearchError::InvalidStart { point: query.start });
            }
            NavmeshQueryResult::InvalidGoal => {
                return Err(crate::NavmeshSearchError::InvalidGoal { point: query.goal });
            }
            NavmeshQueryResult::NoPath { .. } => return crate::navmesh::search_not_found(0),
        };

        if points_equal(query.start, query.goal) {
            return crate::navmesh::search_found(
                PolygonPath::from_points(vec![query.start])
                    .expect("polygon path contains at least one point"),
                1,
            );
        }

        let (Some(cells), visited_nodes) =
            search_cell_corridor(navmesh, start_cell, goal_cell, query.budget)?
        else {
            return crate::navmesh::search_not_found(0);
        };

        let Some(corridor) = crate::navmesh::corridor::NavmeshCorridor::from_cells(
            navmesh,
            query.start,
            query.goal,
            &cells,
        ) else {
            return crate::navmesh::search_not_found(visited_nodes);
        };

        let initial_points = corridor
            .portals
            .iter()
            .map(portal_midpoint)
            .collect::<Vec<_>>();
        let adapted_points =
            crate::navmesh::funnel::pull_string(navmesh, &corridor, initial_points);

        if adapted_points.len() >= 2 && !navmesh.path_is_walkable(&adapted_points) {
            return crate::navmesh::search_not_found(visited_nodes);
        }

        crate::navmesh::search_found(
            PolygonPath::from_points(adapted_points)
                .expect("polygon path contains at least one point"),
            visited_nodes,
        )
    }
}

/// Budgeted BFS over cell adjacency; returns a cell sequence or no corridor.
pub(crate) fn search_cell_corridor(
    navmesh: &Navmesh,
    start_cell: usize,
    goal_cell: usize,
    budget: condor_core::SearchBudget,
) -> Result<(Option<Vec<usize>>, usize), crate::NavmeshSearchError> {
    let cell_count = navmesh.cells().len();
    if start_cell >= cell_count || goal_cell >= cell_count {
        return Ok((None, 0));
    }

    let mut seen = vec![false; cell_count];
    let mut parents = vec![None; cell_count];
    let mut frontier = VecDeque::from([start_cell]);
    let mut visited_nodes = 0;
    let watch = condor_core::BudgetWatch::start(budget);

    seen[start_cell] = true;
    parents[start_cell] = Some(start_cell);

    while let Some(cell_index) = frontier.pop_front() {
        visited_nodes += 1;
        if cell_index == goal_cell {
            return Ok((
                reconstruct_cell_path(&parents, start_cell, goal_cell),
                visited_nodes,
            ));
        }

        watch.check(visited_nodes)?;

        let mut neighbors = navmesh.neighbors(cell_index);
        neighbors.sort_unstable();
        for neighbor in neighbors {
            if neighbor >= seen.len() || seen[neighbor] {
                continue;
            }

            seen[neighbor] = true;
            parents[neighbor] = Some(cell_index);
            frontier.push_back(neighbor);
        }
    }

    Ok((None, visited_nodes))
}

fn reconstruct_cell_path(
    parents: &[Option<usize>],
    start_cell: usize,
    goal_cell: usize,
) -> Option<Vec<usize>> {
    let mut cells = vec![goal_cell];
    let mut current = goal_cell;

    while current != start_cell {
        let parent = parents[current]?;
        cells.push(parent);
        current = parent;
    }

    cells.reverse();
    Some(cells)
}

/// Midpoint of a portal segment used as the default funnel seed.
pub(crate) fn portal_midpoint(portal: &crate::NavmeshPortal) -> Point2 {
    Point2::new(
        (portal.start.x + portal.end.x) / 2.0,
        (portal.start.y + portal.end.y) / 2.0,
    )
}