shortestpath 0.10.0

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

use crate::distance::*;
use crate::errors::*;
use crate::gate::*;
use crate::mesh::*;
use crate::mesh_3d::Index3D;
use crate::mesh_3d::Shape3D;

/// A fully connected 2.5D rectangular volume mesh.
///
/// `Full25D` represents a 2.5D volume where all cells are walkable and each cell
/// can connect to its neighbors. Unlike 2D grids, 2.5D cells have up to 10 neighbors:
/// - 8 neighbors in the current layer (same as 2D: 4 orthogonal + 4 diagonal)
/// - 2 vertical neighbors (directly above and below)
///
/// This is the simplest 2.5D mesh implementation with no obstacles or walls.
///
/// # Connectivity
///
/// Each cell has up to 10 neighbors:
/// - 4 orthogonal in-layer (N, S, E, W) with distance [`DISTANCE_STRAIGHT`] (1.0)
/// - 4 diagonal in-layer (NE, NW, SE, SW) with distance [`DISTANCE_DIAGONAL`] (√2)
/// - 2 vertical (up, down) with distance [`DISTANCE_STRAIGHT`] (1.0)
///
/// Edge, face, and corner cells have fewer neighbors.
///
/// # Memory
///
/// This mesh has minimal memory overhead - it only stores width, height, and depth,
/// computing successors on the fly.
///
/// # Example
///
/// ```
/// use shortestpath::{Mesh, Gradient, mesh_25d::Full25D};
///
/// // Create a 5x5x5 volume
/// let mesh = Full25D::new(5, 5, 5);
/// assert_eq!(mesh.len(), 125);
///
/// // Use it for pathfinding
/// let mut gradient = Gradient::from_mesh(&mesh);
/// gradient.set_distance(62, 0.0); // Center cell as target
/// gradient.spread(&mesh);
///
/// // All cells now have shortest path to center
/// assert!(gradient.get_distance(0) > 0.0);
/// ```
#[derive(Debug, Clone, PartialEq, Eq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub struct Full25D {
    /// Width of the volume (number of columns)
    width: usize,
    /// Height of the volume (number of rows)
    height: usize,
    /// Depth of the volume (number of layers)
    depth: usize,
}

impl Full25D {
    /// Creates a new fully connected 2.5D volume.
    ///
    /// # Arguments
    ///
    /// * `width` - Number of columns in the volume
    /// * `height` - Number of rows in the volume
    /// * `depth` - Number of layers in the volume
    ///
    /// # Example
    ///
    /// ```
    /// use shortestpath::mesh_25d::Full25D;
    ///
    /// let mesh = Full25D::new(10, 8, 6);
    /// ```
    pub fn new(width: usize, height: usize, depth: usize) -> Self {
        Self {
            width,
            height,
            depth,
        }
    }

    /// Returns self unchanged since a full mesh has no disconnected zones.
    ///
    /// This method shadows [`Mesh::unique()`] to avoid the overhead of
    /// [`FilteredMesh`] when all nodes are already connected.
    ///
    /// # Example
    ///
    /// ```
    /// use shortestpath::{Mesh, mesh_25d::Full25D};
    ///
    /// let mesh = Full25D::new(5, 5, 5);
    /// let same_mesh: Full25D = mesh.unique(); // Returns Self, not FilteredMesh
    /// assert_eq!(same_mesh.len(), 125);
    /// ```
    pub fn unique(self) -> Self {
        self
    }

    #[inline]
    pub(crate) fn successors(
        width: usize,
        height: usize,
        depth: usize,
        from: usize,
        backward: bool,
    ) -> std::vec::IntoIter<Gate> {
        let mut peers = Vec::<Gate>::with_capacity(8);
        if let Ok(xyz) = Self::index_to_xyz(width, height, depth, from) {
            let (x, y, z) = xyz;
            if x < width - 1 {
                peers.push(Gate::new(from + 1, DISTANCE_STRAIGHT));
                if y < height - 1 {
                    peers.push(Gate::new(from + 1 + width, DISTANCE_DIAGONAL));
                }
            }
            if y < height - 1 {
                peers.push(Gate::new(from + width, DISTANCE_STRAIGHT));
                if x > 0 {
                    peers.push(Gate::new(from - 1 + width, DISTANCE_DIAGONAL));
                }
            }
            if x > 0 {
                peers.push(Gate::new(from - 1, DISTANCE_STRAIGHT));
                if y > 0 {
                    peers.push(Gate::new(from - 1 - width, DISTANCE_DIAGONAL));
                }
            }
            if y > 0 {
                peers.push(Gate::new(from - width, DISTANCE_STRAIGHT));
                if x < width - 1 {
                    peers.push(Gate::new(from + 1 - width, DISTANCE_DIAGONAL));
                }
            }
            if z < depth - 1 {
                peers.push(Gate::new(from + (width * height), DISTANCE_STRAIGHT));
            }
            if z > 0 {
                peers.push(Gate::new(from - (width * height), DISTANCE_STRAIGHT));
            }
        }
        if backward {
            peers.reverse();
        }
        peers.into_iter()
    }

    #[inline]
    pub(crate) fn len(width: usize, height: usize, depth: usize) -> usize {
        width * height * depth
    }

    #[inline]
    pub(crate) fn index_to_xyz(
        width: usize,
        height: usize,
        depth: usize,
        index: usize,
    ) -> Result<(usize, usize, usize)> {
        if index >= width * height * depth {
            return Err(Error::invalid_index(index));
        }
        Ok((
            index % width,
            (index / width) % height,
            index / (height * width),
        ))
    }

    #[inline]
    pub(crate) fn xyz_to_index(
        width: usize,
        height: usize,
        depth: usize,
        x: usize,
        y: usize,
        z: usize,
    ) -> Result<usize> {
        if x >= width || y >= height || z >= depth {
            return Err(Error::invalid_xyz(x, y, z));
        }
        Ok((z * height + y) * width + x)
    }

    #[inline]
    pub(crate) fn shape(width: usize, height: usize, depth: usize) -> (usize, usize, usize) {
        (width, height, depth)
    }
}

impl Mesh for Full25D {
    type IntoIter = std::vec::IntoIter<Gate>;
    fn successors(&self, from: usize, backward: bool) -> std::vec::IntoIter<Gate> {
        Self::successors(self.width, self.height, self.depth, from, backward)
    }

    fn len(&self) -> usize {
        Self::len(self.width, self.height, self.depth)
    }
}

impl Index3D for Full25D {
    fn index_to_xyz(&self, index: usize) -> Result<(usize, usize, usize)> {
        Self::index_to_xyz(self.width, self.height, self.depth, index)
    }

    fn xyz_to_index(&self, x: usize, y: usize, z: usize) -> Result<usize> {
        Self::xyz_to_index(self.width, self.height, self.depth, x, y, z)
    }
}

impl Shape3D for Full25D {
    fn shape(&self) -> (usize, usize, usize) {
        Self::shape(self.width, self.height, self.depth)
    }
}

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

    #[test]
    fn test_xyz_w0() {
        let r = Full25D::new(0, 10, 10);
        r.xyz_to_index(0, 0, 0).expect_err("out of bound");
        r.xyz_to_index(0, 1, 0).expect_err("out of bound");
        r.xyz_to_index(1, 0, 0).expect_err("out of bound");
        r.xyz_to_index(1, 1, 1).expect_err("out of bound");
    }

    #[test]
    fn test_xyz_h0() {
        let r = Full25D::new(10, 0, 10);
        r.xyz_to_index(0, 0, 0).expect_err("out of bound");
        r.xyz_to_index(0, 1, 0).expect_err("out of bound");
        r.xyz_to_index(1, 0, 0).expect_err("out of bound");
        r.xyz_to_index(1, 1, 1).expect_err("out of bound");
    }

    #[test]
    fn test_xyz_d0() {
        let r = Full25D::new(10, 10, 0);
        r.xyz_to_index(0, 0, 0).expect_err("out of bound");
        r.xyz_to_index(0, 1, 0).expect_err("out of bound");
        r.xyz_to_index(1, 0, 0).expect_err("out of bound");
        r.xyz_to_index(1, 1, 1).expect_err("out of bound");
    }

    #[test]
    fn test_xyz_w0h0d0() {
        let r = Full25D::new(0, 0, 0);
        r.xyz_to_index(0, 0, 0).expect_err("out of bound");
        r.xyz_to_index(0, 1, 0).expect_err("out of bound");
        r.xyz_to_index(1, 0, 0).expect_err("out of bound");
        r.xyz_to_index(1, 1, 1).expect_err("out of bound");
    }

    #[test]
    fn test_xyz_w4h3d2() {
        let r = Full25D::new(4, 3, 2);
        assert_eq!(0, r.xyz_to_index(0, 0, 0).expect("out of bound"));
        assert_eq!(11, r.xyz_to_index(3, 2, 0).expect("out of bound"));
        assert_eq!(15, r.xyz_to_index(3, 0, 1).expect("out of bound"));
        assert_eq!(20, r.xyz_to_index(0, 2, 1).expect("out of bound"));
        r.xyz_to_index(4, 0, 0).expect_err("out of bound");
        r.xyz_to_index(0, 3, 0).expect_err("out of bound");
        r.xyz_to_index(0, 0, 2).expect_err("out of bound");
    }

    #[test]
    fn test_index_w0() {
        let r = Full25D::new(0, 10, 10);
        r.index_to_xyz(0).expect_err("out of bound");
        r.index_to_xyz(1).expect_err("out of bound");
    }

    #[test]
    fn test_index_h0() {
        let r = Full25D::new(10, 0, 10);
        r.index_to_xyz(0).expect_err("out of bound");
        r.index_to_xyz(1).expect_err("out of bound");
    }

    #[test]
    fn test_index_d0() {
        let r = Full25D::new(10, 10, 0);
        r.index_to_xyz(0).expect_err("out of bound");
        r.index_to_xyz(1).expect_err("out of bound");
    }

    #[test]
    fn test_index_w0h0d0() {
        let r = Full25D::new(0, 0, 0);
        r.index_to_xyz(0).expect_err("out of bound");
        r.index_to_xyz(1).expect_err("out of bound");
    }

    #[test]
    fn test_index_w4h3d2() {
        let r = Full25D::new(4, 3, 2);
        assert_eq!((0, 0, 0), r.index_to_xyz(0).expect("out of bound"));
        assert_eq!((3, 2, 0), r.index_to_xyz(11).expect("out of bound"));
        assert_eq!((3, 0, 1), r.index_to_xyz(15).expect("out of bound"));
        assert_eq!((0, 2, 1), r.index_to_xyz(20).expect("out of bound"));
        r.index_to_xyz(24).expect_err("out of bound");
    }

    #[test]
    fn test_full_rectangle_basic_w4h3() {
        let r = Full25D::new(4, 3, 2);
        assert_eq!(
            vec![
                Gate::new(1, DISTANCE_STRAIGHT),
                Gate::new(5, DISTANCE_DIAGONAL),
                Gate::new(4, DISTANCE_STRAIGHT),
                Gate::new(12, DISTANCE_STRAIGHT)
            ],
            r.successors(0, false).collect::<Vec<Gate>>()
        );
        assert_eq!(
            vec![
                Gate::new(2, DISTANCE_STRAIGHT),
                Gate::new(6, DISTANCE_DIAGONAL),
                Gate::new(5, DISTANCE_STRAIGHT),
                Gate::new(4, DISTANCE_DIAGONAL),
                Gate::new(0, DISTANCE_STRAIGHT),
                Gate::new(13, DISTANCE_STRAIGHT)
            ],
            r.successors(1, false).collect::<Vec<Gate>>(),
        );
        assert_eq!(
            vec![
                Gate::new(3, DISTANCE_STRAIGHT),
                Gate::new(7, DISTANCE_DIAGONAL),
                Gate::new(6, DISTANCE_STRAIGHT),
                Gate::new(5, DISTANCE_DIAGONAL),
                Gate::new(1, DISTANCE_STRAIGHT),
                Gate::new(14, DISTANCE_STRAIGHT)
            ],
            r.successors(2, false).collect::<Vec<Gate>>()
        );
        assert_eq!(
            vec![
                Gate::new(7, DISTANCE_STRAIGHT),
                Gate::new(6, DISTANCE_DIAGONAL),
                Gate::new(2, DISTANCE_STRAIGHT),
                Gate::new(15, DISTANCE_STRAIGHT)
            ],
            r.successors(3, false).collect::<Vec<Gate>>()
        );

        assert_eq!(
            vec![
                Gate::new(5, DISTANCE_STRAIGHT),
                Gate::new(9, DISTANCE_DIAGONAL),
                Gate::new(8, DISTANCE_STRAIGHT),
                Gate::new(0, DISTANCE_STRAIGHT),
                Gate::new(1, DISTANCE_DIAGONAL),
                Gate::new(16, DISTANCE_STRAIGHT)
            ],
            r.successors(4, false).collect::<Vec<Gate>>()
        );
        assert_eq!(
            vec![
                Gate::new(6, DISTANCE_STRAIGHT),
                Gate::new(10, DISTANCE_DIAGONAL),
                Gate::new(9, DISTANCE_STRAIGHT),
                Gate::new(8, DISTANCE_DIAGONAL),
                Gate::new(4, DISTANCE_STRAIGHT),
                Gate::new(0, DISTANCE_DIAGONAL),
                Gate::new(1, DISTANCE_STRAIGHT),
                Gate::new(2, DISTANCE_DIAGONAL),
                Gate::new(17, DISTANCE_STRAIGHT)
            ],
            r.successors(5, false).collect::<Vec<Gate>>()
        );
        assert_eq!(
            vec![
                Gate::new(7, DISTANCE_STRAIGHT),
                Gate::new(11, DISTANCE_DIAGONAL),
                Gate::new(10, DISTANCE_STRAIGHT),
                Gate::new(9, DISTANCE_DIAGONAL),
                Gate::new(5, DISTANCE_STRAIGHT),
                Gate::new(1, DISTANCE_DIAGONAL),
                Gate::new(2, DISTANCE_STRAIGHT),
                Gate::new(3, DISTANCE_DIAGONAL),
                Gate::new(18, DISTANCE_STRAIGHT)
            ],
            r.successors(6, false).collect::<Vec<Gate>>()
        );
        assert_eq!(
            vec![
                Gate::new(11, DISTANCE_STRAIGHT),
                Gate::new(10, DISTANCE_DIAGONAL),
                Gate::new(6, DISTANCE_STRAIGHT),
                Gate::new(2, DISTANCE_DIAGONAL),
                Gate::new(3, DISTANCE_STRAIGHT),
                Gate::new(19, DISTANCE_STRAIGHT)
            ],
            r.successors(7, false).collect::<Vec<Gate>>()
        );

        assert_eq!(
            vec![
                Gate::new(9, DISTANCE_STRAIGHT),
                Gate::new(4, DISTANCE_STRAIGHT),
                Gate::new(5, DISTANCE_DIAGONAL),
                Gate::new(20, DISTANCE_STRAIGHT)
            ],
            r.successors(8, false).collect::<Vec<Gate>>()
        );
        assert_eq!(
            vec![
                Gate::new(10, DISTANCE_STRAIGHT),
                Gate::new(8, DISTANCE_STRAIGHT),
                Gate::new(4, DISTANCE_DIAGONAL),
                Gate::new(5, DISTANCE_STRAIGHT),
                Gate::new(6, DISTANCE_DIAGONAL),
                Gate::new(21, DISTANCE_STRAIGHT)
            ],
            r.successors(9, false).collect::<Vec<Gate>>()
        );
        assert_eq!(
            vec![
                Gate::new(11, DISTANCE_STRAIGHT),
                Gate::new(9, DISTANCE_STRAIGHT),
                Gate::new(5, DISTANCE_DIAGONAL),
                Gate::new(6, DISTANCE_STRAIGHT),
                Gate::new(7, DISTANCE_DIAGONAL),
                Gate::new(22, DISTANCE_STRAIGHT)
            ],
            r.successors(10, false).collect::<Vec<Gate>>()
        );
        assert_eq!(
            vec![
                Gate::new(10, DISTANCE_STRAIGHT),
                Gate::new(6, DISTANCE_DIAGONAL),
                Gate::new(7, DISTANCE_STRAIGHT),
                Gate::new(23, DISTANCE_STRAIGHT)
            ],
            r.successors(11, false).collect::<Vec<Gate>>()
        );
        assert_eq!(
            vec![
                Gate::new(13, DISTANCE_STRAIGHT),
                Gate::new(17, DISTANCE_DIAGONAL),
                Gate::new(16, DISTANCE_STRAIGHT),
                Gate::new(0, DISTANCE_STRAIGHT)
            ],
            r.successors(12, false).collect::<Vec<Gate>>()
        );
        assert_eq!(
            vec![
                Gate::new(14, DISTANCE_STRAIGHT),
                Gate::new(18, DISTANCE_DIAGONAL),
                Gate::new(17, DISTANCE_STRAIGHT),
                Gate::new(16, DISTANCE_DIAGONAL),
                Gate::new(12, DISTANCE_STRAIGHT),
                Gate::new(1, DISTANCE_STRAIGHT)
            ],
            r.successors(13, false).collect::<Vec<Gate>>(),
        );
        assert_eq!(
            vec![
                Gate::new(15, DISTANCE_STRAIGHT),
                Gate::new(19, DISTANCE_DIAGONAL),
                Gate::new(18, DISTANCE_STRAIGHT),
                Gate::new(17, DISTANCE_DIAGONAL),
                Gate::new(13, DISTANCE_STRAIGHT),
                Gate::new(2, DISTANCE_STRAIGHT)
            ],
            r.successors(14, false).collect::<Vec<Gate>>()
        );
        assert_eq!(
            vec![
                Gate::new(19, DISTANCE_STRAIGHT),
                Gate::new(18, DISTANCE_DIAGONAL),
                Gate::new(14, DISTANCE_STRAIGHT),
                Gate::new(3, DISTANCE_STRAIGHT)
            ],
            r.successors(15, false).collect::<Vec<Gate>>()
        );

        assert_eq!(
            vec![
                Gate::new(17, DISTANCE_STRAIGHT),
                Gate::new(21, DISTANCE_DIAGONAL),
                Gate::new(20, DISTANCE_STRAIGHT),
                Gate::new(12, DISTANCE_STRAIGHT),
                Gate::new(13, DISTANCE_DIAGONAL),
                Gate::new(4, DISTANCE_STRAIGHT)
            ],
            r.successors(16, false).collect::<Vec<Gate>>()
        );
        assert_eq!(
            vec![
                Gate::new(18, DISTANCE_STRAIGHT),
                Gate::new(22, DISTANCE_DIAGONAL),
                Gate::new(21, DISTANCE_STRAIGHT),
                Gate::new(20, DISTANCE_DIAGONAL),
                Gate::new(16, DISTANCE_STRAIGHT),
                Gate::new(12, DISTANCE_DIAGONAL),
                Gate::new(13, DISTANCE_STRAIGHT),
                Gate::new(14, DISTANCE_DIAGONAL),
                Gate::new(5, DISTANCE_STRAIGHT)
            ],
            r.successors(17, false).collect::<Vec<Gate>>()
        );
        assert_eq!(
            vec![
                Gate::new(19, DISTANCE_STRAIGHT),
                Gate::new(23, DISTANCE_DIAGONAL),
                Gate::new(22, DISTANCE_STRAIGHT),
                Gate::new(21, DISTANCE_DIAGONAL),
                Gate::new(17, DISTANCE_STRAIGHT),
                Gate::new(13, DISTANCE_DIAGONAL),
                Gate::new(14, DISTANCE_STRAIGHT),
                Gate::new(15, DISTANCE_DIAGONAL),
                Gate::new(6, DISTANCE_STRAIGHT)
            ],
            r.successors(18, false).collect::<Vec<Gate>>()
        );
        assert_eq!(
            vec![
                Gate::new(23, DISTANCE_STRAIGHT),
                Gate::new(22, DISTANCE_DIAGONAL),
                Gate::new(18, DISTANCE_STRAIGHT),
                Gate::new(14, DISTANCE_DIAGONAL),
                Gate::new(15, DISTANCE_STRAIGHT),
                Gate::new(7, DISTANCE_STRAIGHT)
            ],
            r.successors(19, false).collect::<Vec<Gate>>()
        );

        assert_eq!(
            vec![
                Gate::new(21, DISTANCE_STRAIGHT),
                Gate::new(16, DISTANCE_STRAIGHT),
                Gate::new(17, DISTANCE_DIAGONAL),
                Gate::new(8, DISTANCE_STRAIGHT)
            ],
            r.successors(20, false).collect::<Vec<Gate>>()
        );
        assert_eq!(
            vec![
                Gate::new(22, DISTANCE_STRAIGHT),
                Gate::new(20, DISTANCE_STRAIGHT),
                Gate::new(16, DISTANCE_DIAGONAL),
                Gate::new(17, DISTANCE_STRAIGHT),
                Gate::new(18, DISTANCE_DIAGONAL),
                Gate::new(9, DISTANCE_STRAIGHT)
            ],
            r.successors(21, false).collect::<Vec<Gate>>()
        );
        assert_eq!(
            vec![
                Gate::new(23, DISTANCE_STRAIGHT),
                Gate::new(21, DISTANCE_STRAIGHT),
                Gate::new(17, DISTANCE_DIAGONAL),
                Gate::new(18, DISTANCE_STRAIGHT),
                Gate::new(19, DISTANCE_DIAGONAL),
                Gate::new(10, DISTANCE_STRAIGHT)
            ],
            r.successors(22, false).collect::<Vec<Gate>>()
        );
        assert_eq!(
            vec![
                Gate::new(22, DISTANCE_STRAIGHT),
                Gate::new(18, DISTANCE_DIAGONAL),
                Gate::new(19, DISTANCE_STRAIGHT),
                Gate::new(11, DISTANCE_STRAIGHT)
            ],
            r.successors(23, false).collect::<Vec<Gate>>()
        );
    }

    #[test]
    #[cfg(feature = "serde")]
    fn test_serde() {
        let mesh = Full25D::new(5, 5, 5);
        let json = serde_json::to_string(&mesh).unwrap();
        let deserialized: Full25D = serde_json::from_str(&json).unwrap();

        assert_eq!(mesh.len(), deserialized.len());
        assert_eq!(mesh.shape(), deserialized.shape());
    }
}