shortestpath 0.10.0

// Copyright (C) 2025 Christian Mauduit <ufoot@ufoot.org>

//! Utility functions for finding the closest walkable cell to given coordinates.
//!
//! These functions search in expanding rings/shells around a target position
//! until they find a walkable cell. This is useful when:
//! - The exact position is a wall/obstacle
//! - The coordinates are outside the grid/volume bounds
//! - You need the nearest valid starting point for pathfinding
//!
//! # Example
//!
//! ```
//! use shortestpath::{xy_to_closest_index, mesh_2d::{Index2D, Compact2D}};
//!
//! // Grid with a wall in the center
//! let mesh = Compact2D::from_text("...\n.#.\n...").unwrap();
//!
//! // Position on a wall returns nearest walkable cell
//! let index = xy_to_closest_index(&mesh, 1, 1, false).unwrap();
//! assert_eq!(mesh.index_to_xy(index).unwrap(), (0, 0));
//! ```

use crate::mesh_2d::*;
use crate::mesh_3d::*;

/// Finds the index of the closest walkable cell to the given 2D coordinates.
///
/// Unlike [`Index2D::xy_to_index`] which requires exact coordinates of a walkable cell,
/// this function searches in expanding rings around the given position until it finds
/// a walkable cell.
///
/// The search uses Chebyshev distance (chessboard distance), checking cells
/// at increasing distances from the target point.
///
/// # Arguments
///
/// * `mesh` - The 2D mesh to search in
/// * `x` - The column coordinate (can be negative or outside grid bounds)
/// * `y` - The row coordinate (can be negative or outside grid bounds)
/// * `backward` - Controls search direction to avoid bias when multiple cells
///   are equidistant. When `false`, searches from top-left corner first.
///   When `true`, searches from bottom-right corner first.
///
/// # Returns
///
/// - `Some(index)` - The index of the closest walkable cell
/// - `None` - If the mesh is empty or has no walkable cells
///
/// # Example
///
/// ```
/// use shortestpath::{xy_to_closest_index, mesh_2d::{Index2D, Compact2D}};
///
/// // Grid with a wall in the center
/// let mesh = Compact2D::from_text("...\n.#.\n...").unwrap();
///
/// // Exact position works like xy_to_index
/// let index = xy_to_closest_index(&mesh, 0, 0, false).unwrap();
/// assert_eq!(mesh.index_to_xy(index).unwrap(), (0, 0));
///
/// // Position on a wall returns nearest walkable cell
/// // With backward=false, prefers top-left direction
/// let index = xy_to_closest_index(&mesh, 1, 1, false).unwrap();
/// assert_eq!(mesh.index_to_xy(index).unwrap(), (0, 0));
///
/// // With backward=true, prefers bottom-right direction
/// let index = xy_to_closest_index(&mesh, 1, 1, true).unwrap();
/// assert_eq!(mesh.index_to_xy(index).unwrap(), (2, 2));
/// ```
pub fn xy_to_closest_index(mesh: &impl Index2D, x: isize, y: isize, backward: bool) -> Option<usize> {
    let (width, height) = mesh.shape();

    // Handle empty mesh
    if width == 0 || height == 0 {
        return None;
    }

    let width_i = width as isize;
    let height_i = height as isize;

    // Maximum search radius needed to cover the entire grid from any point
    let max_radius = width.max(height) + x.unsigned_abs().max(y.unsigned_abs());

    for radius in 0..=max_radius {
        let radius_i = radius as isize;

        // Build range iterators based on backward to control search direction
        let y_range: Box<dyn Iterator<Item = isize>> = if backward {
            Box::new((-radius_i..=radius_i).rev())
        } else {
            Box::new(-radius_i..=radius_i)
        };

        // Search all cells at Chebyshev distance = radius
        for dy in y_range {
            let x_range: Box<dyn Iterator<Item = isize>> = if backward {
                Box::new((-radius_i..=radius_i).rev())
            } else {
                Box::new(-radius_i..=radius_i)
            };

            for dx in x_range {
                // Only check cells exactly at distance `radius` (on the ring)
                if dx.abs().max(dy.abs()) != radius_i {
                    continue;
                }

                let cx = x + dx;
                let cy = y + dy;

                // Skip if outside grid bounds
                if cx < 0 || cy < 0 || cx >= width_i || cy >= height_i {
                    continue;
                }

                // Try to get the index (will fail for walls in Compact meshes)
                if let Ok(index) = mesh.xy_to_index(cx as usize, cy as usize) {
                    return Some(index);
                }
            }
        }
    }

    None
}

/// Finds the index of the closest walkable cell to the given 3D coordinates.
///
/// Unlike [`Index3D::xyz_to_index`] which requires exact coordinates of a walkable cell,
/// this function searches in expanding shells around the given position until it finds
/// a walkable cell.
///
/// The search uses Chebyshev distance (chessboard distance in 3D), checking cells
/// at increasing distances from the target point.
///
/// # Arguments
///
/// * `mesh` - The 3D mesh to search in
/// * `x` - The column coordinate (can be negative or outside volume bounds)
/// * `y` - The row coordinate (can be negative or outside volume bounds)
/// * `z` - The depth/layer coordinate (can be negative or outside volume bounds)
/// * `backward` - Controls search direction to avoid bias when multiple cells
///   are equidistant. When `false`, searches from top-left-front corner first.
///   When `true`, searches from bottom-right-back corner first.
///
/// # Returns
///
/// - `Some(index)` - The index of the closest walkable cell
/// - `None` - If the mesh is empty or has no walkable cells
///
/// # Example
///
/// ```
/// use shortestpath::{xyz_to_closest_index, mesh_3d::{Index3D, Full3D}};
///
/// let mesh = Full3D::new(5, 5, 5);
///
/// // Exact position works like xyz_to_index
/// let index = xyz_to_closest_index(&mesh, 2, 2, 2, false).unwrap();
/// assert_eq!(mesh.index_to_xyz(index).unwrap(), (2, 2, 2));
///
/// // Position outside bounds returns nearest cell in bounds
/// // From (-1, 0, 0), the nearest cell is (0, 0, 0)
/// let index = xyz_to_closest_index(&mesh, -1, 0, 0, false).unwrap();
/// assert_eq!(mesh.index_to_xyz(index).unwrap(), (0, 0, 0));
/// ```
pub fn xyz_to_closest_index(
    mesh: &impl Index3D,
    x: isize,
    y: isize,
    z: isize,
    backward: bool,
) -> Option<usize> {
    let (width, height, depth) = mesh.shape();

    // Handle empty mesh
    if width == 0 || height == 0 || depth == 0 {
        return None;
    }

    let width_i = width as isize;
    let height_i = height as isize;
    let depth_i = depth as isize;

    // Maximum search radius needed to cover the entire volume from any point
    let max_radius = width
        .max(height)
        .max(depth)
        + x.unsigned_abs().max(y.unsigned_abs()).max(z.unsigned_abs());

    for radius in 0..=max_radius {
        let radius_i = radius as isize;

        // Build range iterators based on backward to control search direction
        let z_range: Box<dyn Iterator<Item = isize>> = if backward {
            Box::new((-radius_i..=radius_i).rev())
        } else {
            Box::new(-radius_i..=radius_i)
        };

        // Search all cells at Chebyshev distance = radius (on the shell)
        for dz in z_range {
            let y_range: Box<dyn Iterator<Item = isize>> = if backward {
                Box::new((-radius_i..=radius_i).rev())
            } else {
                Box::new(-radius_i..=radius_i)
            };

            for dy in y_range {
                let x_range: Box<dyn Iterator<Item = isize>> = if backward {
                    Box::new((-radius_i..=radius_i).rev())
                } else {
                    Box::new(-radius_i..=radius_i)
                };

                for dx in x_range {
                    // Only check cells exactly at distance `radius` (on the shell)
                    if dx.abs().max(dy.abs()).max(dz.abs()) != radius_i {
                        continue;
                    }

                    let cx = x + dx;
                    let cy = y + dy;
                    let cz = z + dz;

                    // Skip if outside volume bounds
                    if cx < 0
                        || cy < 0
                        || cz < 0
                        || cx >= width_i
                        || cy >= height_i
                        || cz >= depth_i
                    {
                        continue;
                    }

                    // Try to get the index (will fail for walls in Compact meshes)
                    if let Ok(index) = mesh.xyz_to_index(cx as usize, cy as usize, cz as usize) {
                        return Some(index);
                    }
                }
            }
        }
    }

    None
}

#[cfg(test)]
mod tests {
    use super::*;

    #[test]
    fn test_xy_to_closest_index_exact() {
        let mesh = Compact2D::from_text("...\n...\n...").unwrap();

        // Exact position should work
        let index = xy_to_closest_index(&mesh, 1, 1, false).unwrap();
        assert_eq!(mesh.index_to_xy(index).unwrap(), (1, 1));
    }

    #[test]
    fn test_xy_to_closest_index_wall() {
        let mesh = Compact2D::from_text("...\n.#.\n...").unwrap();

        // Position on wall, backward=false prefers top-left
        let index = xy_to_closest_index(&mesh, 1, 1, false).unwrap();
        assert_eq!(mesh.index_to_xy(index).unwrap(), (0, 0));

        // Position on wall, backward=true prefers bottom-right
        let index = xy_to_closest_index(&mesh, 1, 1, true).unwrap();
        assert_eq!(mesh.index_to_xy(index).unwrap(), (2, 2));
    }

    #[test]
    fn test_xy_to_closest_index_outside() {
        let mesh = Full2D::new(5, 5);

        // Position outside bounds
        let index = xy_to_closest_index(&mesh, -1, -1, false).unwrap();
        assert_eq!(mesh.index_to_xy(index).unwrap(), (0, 0));

        let index = xy_to_closest_index(&mesh, 10, 10, false).unwrap();
        assert_eq!(mesh.index_to_xy(index).unwrap(), (4, 4));
    }

    #[test]
    fn test_xy_to_closest_index_empty() {
        let mesh = Full2D::new(0, 0);
        assert_eq!(xy_to_closest_index(&mesh, 0, 0, false), None);
    }

    #[test]
    fn test_xy_to_closest_index_all_walls() {
        let mesh = Compact2D::from_text("###\n###\n###").unwrap();
        assert_eq!(xy_to_closest_index(&mesh, 1, 1, false), None);
    }

    #[test]
    fn test_xyz_to_closest_index_exact() {
        let mesh = Full3D::new(5, 5, 5);

        let index = xyz_to_closest_index(&mesh, 2, 2, 2, false).unwrap();
        assert_eq!(mesh.index_to_xyz(index).unwrap(), (2, 2, 2));
    }

    #[test]
    fn test_xyz_to_closest_index_outside() {
        let mesh = Full3D::new(5, 5, 5);

        let index = xyz_to_closest_index(&mesh, -1, 0, 0, false).unwrap();
        assert_eq!(mesh.index_to_xyz(index).unwrap(), (0, 0, 0));
    }

    #[test]
    fn test_xyz_to_closest_index_empty() {
        let mesh = Full3D::new(0, 0, 0);
        assert_eq!(xyz_to_closest_index(&mesh, 0, 0, 0, false), None);
    }
}