shortestpath 0.10.0

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

// from https://docs.rs/pathfinding/4.8.0/pathfinding/directed/dijkstra/fn.dijkstra.html
//
// pub fn dijkstra<N, C, FN, IN, FS>(
//     start: &N,
//     successors: FN,
//     success: FS
// ) -> Option<(Vec<N>, C)>
// where
//     N: Eq + Hash + Clone,
//     C: Zero + Ord + Copy,
//     FN: FnMut(&N) -> IN,
//     IN: IntoIterator<Item = (N, C)>,
//     FS: FnMut(&N) -> bool,

use crate::distance::*;
use crate::gate::*;
use crate::gradient::*;
use std::iter::Iterator;

/// Trait representing a graph structure for pathfinding.
///
/// A `Mesh` defines the connectivity and structure of a graph used for pathfinding.
/// It abstracts over different types of graphs (2D grids, 3D volumes, arbitrary graphs)
/// and provides the core interface needed by the gradient-based pathfinding algorithm.
///
/// # Design
///
/// The mesh trait is designed to work with Dijkstra-style algorithms (see reference
/// implementation from the pathfinding crate above). Each node is identified by a
/// `usize` index, and the mesh provides:
/// - The total number of nodes
/// - The neighbors (successors) reachable from any given node
///
/// # Implementations
///
/// This crate provides several implementations:
/// - `mesh_2d::Full2D` - Fully connected 2D rectangular grid
/// - `mesh_2d::Compact2D` - 2D grid with walls/obstacles
/// - `mesh_3d::Full3D` - Fully connected 3D rectangular volume
/// - `mesh_3d::Compact3D` - 3D volume with walls/obstacles
/// - `mesh_topo::MeshWithTopology` - Graph with custom topology
///
/// # Example
///
/// ```
/// use shortestpath::{Mesh, Gate, mesh_2d::Full2D};
///
/// // Create a 3x3 grid
/// let mesh = Full2D::new(3, 3);
///
/// // Query mesh properties
/// assert_eq!(mesh.len(), 9); // 3x3 = 9 nodes
///
/// // Get successors (neighbors) of node 4 (center of grid)
/// let neighbors: Vec<Gate> = mesh.successors(4, false).collect();
/// // Center node has 8 neighbors (all surrounding cells)
/// assert_eq!(neighbors.len(), 8);
/// ```
pub trait Mesh {
    /// The iterator type returned by `successors()`.
    ///
    /// This must be an iterator that yields `Gate` objects representing
    /// reachable neighboring nodes and their distances.
    type IntoIter: Iterator<Item = Gate>;

    /// Returns an iterator over the successors (neighbors) of a given node.
    ///
    /// Each successor is represented as a `Gate` containing the neighbor's
    /// index and the distance/cost to reach it from the current node.
    ///
    /// # Arguments
    ///
    /// * `from` - The index of the node to query
    /// * `backward` - If `true`, iterate successors in reverse order
    ///
    /// # Returns
    ///
    /// An iterator yielding `Gate` objects for each reachable neighbor
    ///
    /// # Example
    ///
    /// ```
    /// use shortestpath::{Mesh, mesh_2d::Full2D};
    ///
    /// let mesh = Full2D::new(3, 3);
    ///
    /// // Get neighbors of top-left corner (index 0) in normal order
    /// let neighbors: Vec<_> = mesh.successors(0, false).collect();
    /// // Corner has 3 neighbors: right, down, and diagonal
    /// assert_eq!(neighbors.len(), 3);
    ///
    /// // Get neighbors in reverse order
    /// let neighbors_rev: Vec<_> = mesh.successors(0, true).collect();
    /// assert_eq!(neighbors_rev.len(), 3);
    /// ```
    fn successors(&self, from: usize, backward: bool) -> Self::IntoIter;

    /// Returns the total number of nodes in the mesh.
    ///
    /// # Returns
    ///
    /// The total count of nodes/cells in this mesh
    ///
    /// # Example
    ///
    /// ```
    /// use shortestpath::{Mesh, mesh_2d::Full2D};
    ///
    /// let mesh = Full2D::new(5, 4);
    /// assert_eq!(mesh.len(), 20); // 5 * 4 = 20 nodes
    /// ```
    fn len(&self) -> usize;

    /// Returns `true` if the mesh contains no nodes.
    ///
    /// # Example
    ///
    /// ```
    /// use shortestpath::{Mesh, mesh_2d::Full2D};
    ///
    /// let empty_mesh = Full2D::new(0, 0);
    /// assert!(empty_mesh.is_empty());
    ///
    /// let mesh = Full2D::new(5, 4);
    /// assert!(!mesh.is_empty());
    /// ```
    fn is_empty(&self) -> bool {
        self.len() == 0
    }

    /// Filters the mesh to keep only nodes reachable from index 0.
    ///
    /// Returns a [`FilteredMesh`] containing only the connected component
    /// that includes the first node. This is useful when working with meshes
    /// that may contain multiple disconnected zones.
    ///
    /// # Note
    ///
    /// For `Compact2D`, `Compact25D`, and `Compact3D`, there are type-specific
    /// `unique()` methods that return the same type with rebuilt internal
    /// structures, avoiding runtime overhead. Those methods shadow this trait
    /// method when called directly on those types.
    ///
    /// # Example
    ///
    /// ```
    /// use shortestpath::{Mesh, Gradient, mesh_2d::Full2D};
    ///
    /// // A fully connected mesh has all nodes reachable
    /// let mesh = Full2D::new(3, 3);
    /// let filtered = mesh.unique();
    /// assert_eq!(filtered.len(), 9); // All nodes reachable
    /// ```
    fn unique(self) -> FilteredMesh<Self>
    where
        Self: Sized + PartialEq,
    {
        FilteredMesh::new(self)
    }
}

/// Compares two meshes for structural equality.
///
/// Two meshes are considered equal if they have the same length and
/// identical successors for each index.
///
/// # Arguments
///
/// * `a` - First mesh to compare
/// * `b` - Second mesh to compare
///
/// # Returns
///
/// `true` if the meshes have identical structure, `false` otherwise.
///
/// # Example
///
/// ```
/// use shortestpath::{mesh::meshes_equal, mesh_2d::Full2D};
///
/// let mesh1 = Full2D::new(3, 3);
/// let mesh2 = Full2D::new(3, 3);
/// let mesh3 = Full2D::new(4, 4);
///
/// assert!(meshes_equal(&mesh1, &mesh2));
/// assert!(!meshes_equal(&mesh1, &mesh3));
/// ```
pub fn meshes_equal(a: &impl Mesh, b: &impl Mesh) -> bool {
    if a.len() != b.len() {
        return false;
    }
    for i in 0..a.len() {
        let successors_a: Vec<Gate> = a.successors(i, false).collect();
        let successors_b: Vec<Gate> = b.successors(i, false).collect();
        if successors_a != successors_b {
            return false;
        }
    }
    true
}

/// Returns a vector of indices that are reachable from index 0.
///
/// This function spreads a gradient from index 0 and identifies all nodes
/// that can be reached. It's useful for filtering disconnected zones from a mesh.
///
/// # Arguments
///
/// * `mesh` - A reference to any type implementing the `Mesh` trait
///
/// # Returns
///
/// A `Vec<usize>` containing the indices of all nodes reachable from index 0,
/// in ascending order.
///
/// # Example
///
/// ```
/// use shortestpath::{Mesh, mesh::reachable_from_origin, mesh_2d::Compact2D};
///
/// // Grid with two separate zones divided by a wall
/// let mesh = Compact2D::from_text("##########\n#    #   #\n#    #   #\n##########\n").unwrap();
/// // Total 14 walkable cells (8 left + 6 right)
/// assert_eq!(mesh.len(), 14);
///
/// // Only 8 cells are reachable from index 0 (left zone)
/// let reachable = reachable_from_origin(&mesh);
/// assert_eq!(reachable.len(), 8);
/// ```
pub fn reachable_from_origin(mesh: &impl Mesh) -> Vec<usize> {
    if mesh.is_empty() {
        return vec![];
    }

    let mut gradient = Gradient::from_mesh(mesh);
    gradient.set_distance(0, 0.0);
    gradient.spread(mesh);

    (0..mesh.len())
        .filter(|&i| gradient.get_distance(i) < DISTANCE_MAX)
        .collect()
}

/// A filtered view of a mesh containing only nodes reachable from index 0.
///
/// `FilteredMesh` wraps any type implementing [`Mesh`] and presents a view
/// containing only the connected component that includes index 0. This is useful
/// when working with meshes that may contain multiple disconnected zones.
///
/// The filtered mesh implements [`Mesh`] itself, so it can be used anywhere
/// a mesh is expected (with [`Gradient`], pathfinding algorithms, etc.).
///
/// # Performance
///
/// This wrapper adds a small overhead for index translation on each `successors()`
/// call. For performance-critical applications with Compact* meshes, consider using
/// the type-specific `unique()` methods which rebuild internal structures directly.
///
/// # Example
///
/// ```
/// use shortestpath::{Mesh, Gradient, mesh::FilteredMesh, mesh_2d::Compact2D};
///
/// // Grid with two separate zones divided by a wall
/// let mesh = Compact2D::from_text("##########\n#    #   #\n#    #   #\n##########\n").unwrap();
/// assert_eq!(mesh.len(), 14); // 8 left + 6 right
///
/// // Filter to only the connected zone
/// let filtered = FilteredMesh::new(mesh);
/// assert_eq!(filtered.len(), 8); // Only left zone
///
/// // Use like any other mesh
/// let mut gradient = Gradient::from_mesh(&filtered);
/// gradient.set_distance(0, 0.0);
/// gradient.spread(&filtered);
/// ```
#[derive(Debug, Clone, PartialEq, Eq)]
#[cfg_attr(
    feature = "serde",
    derive(serde::Serialize, serde::Deserialize),
    serde(bound(
        serialize = "M: serde::Serialize",
        deserialize = "M: serde::Deserialize<'de>"
    ))
)]
pub struct FilteredMesh<M: Mesh + PartialEq> {
    inner: M,
    /// Maps new (filtered) indices to original indices
    new_to_old: Vec<usize>,
    /// Maps original indices to new (filtered) indices, or usize::MAX if filtered out
    old_to_new: Vec<usize>,
}

impl<M: Mesh + PartialEq> FilteredMesh<M> {
    /// Creates a new filtered mesh containing only nodes reachable from index 0.
    ///
    /// # Arguments
    ///
    /// * `mesh` - The mesh to filter
    ///
    /// # Returns
    ///
    /// A `FilteredMesh` containing only the connected component from index 0.
    ///
    /// # Example
    ///
    /// ```
    /// use shortestpath::{Mesh, mesh::FilteredMesh, mesh_2d::Compact2D};
    ///
    /// let mesh = Compact2D::from_text("##########\n#    #   #\n#    #   #\n##########\n").unwrap();
    /// let filtered = FilteredMesh::new(mesh);
    /// assert_eq!(filtered.len(), 8);
    /// ```
    pub fn new(mesh: M) -> Self {
        let reachable = reachable_from_origin(&mesh);
        let old_len = mesh.len();

        let new_to_old = reachable;
        let mut old_to_new = vec![usize::MAX; old_len];
        for (new_idx, &old_idx) in new_to_old.iter().enumerate() {
            old_to_new[old_idx] = new_idx;
        }

        Self {
            inner: mesh,
            new_to_old,
            old_to_new,
        }
    }

    /// Returns a reference to the inner (unfiltered) mesh.
    pub fn inner(&self) -> &M {
        &self.inner
    }

    /// Consumes the filtered mesh and returns the inner (unfiltered) mesh.
    pub fn into_inner(self) -> M {
        self.inner
    }

    /// Translates a filtered index to the original mesh index.
    ///
    /// # Panics
    ///
    /// Panics if `filtered_index` is out of bounds.
    pub fn to_original_index(&self, filtered_index: usize) -> usize {
        self.new_to_old[filtered_index]
    }

    /// Translates an original mesh index to the filtered index.
    ///
    /// Returns `None` if the original index was filtered out.
    pub fn to_filtered_index(&self, original_index: usize) -> Option<usize> {
        if original_index >= self.old_to_new.len() {
            return None;
        }
        let new_idx = self.old_to_new[original_index];
        if new_idx == usize::MAX {
            None
        } else {
            Some(new_idx)
        }
    }
}

impl<M: Mesh + PartialEq> Mesh for FilteredMesh<M> {
    type IntoIter = std::vec::IntoIter<Gate>;

    fn successors(&self, from: usize, backward: bool) -> Self::IntoIter {
        let old_idx = self.new_to_old[from];
        let successors: Vec<Gate> = self
            .inner
            .successors(old_idx, backward)
            .filter_map(|gate| {
                let new_target = self.old_to_new[gate.target()];
                if new_target != usize::MAX {
                    Some(Gate::new(new_target, gate.distance))
                } else {
                    None
                }
            })
            .collect();
        successors.into_iter()
    }

    fn len(&self) -> usize {
        self.new_to_old.len()
    }
}

#[cfg(test)]
mod tests {
    use super::*;
    use crate::mesh_2d::Compact2D;
    use crate::mesh_2d::Full2D;
    use crate::mesh_topo::{MeshWithTopology, Topology};

    /// A topology that creates a vertical wall dividing a grid into left and right zones.
    /// Blocks any movement where the x-coordinate crosses a threshold.
    struct VerticalWallTopology {
        width: usize,
        wall_x: usize, // x position where the wall is (blocks crossing this line)
    }

    impl VerticalWallTopology {
        fn new(width: usize, wall_x: usize) -> Self {
            Self { width, wall_x }
        }

        fn index_to_x(&self, index: usize) -> usize {
            index % self.width
        }
    }

    impl Topology for VerticalWallTopology {
        fn allowed(&self, from: usize, to: usize) -> crate::errors::Result<bool> {
            let from_x = self.index_to_x(from);
            let to_x = self.index_to_x(to);

            // Block if crossing the wall (one side < wall_x, other side >= wall_x)
            let from_left = from_x < self.wall_x;
            let to_left = to_x < self.wall_x;

            // Allow if both on same side
            Ok(from_left == to_left)
        }
    }

    #[test]
    fn test_filtered_mesh_with_topology() {
        // Create a 6x3 grid (18 nodes)
        let base_mesh = Full2D::new(6, 3);
        assert_eq!(base_mesh.len(), 18);

        // Create a topology with a vertical wall at x=3, dividing into:
        // - Left zone: x < 3 (9 nodes: indices with x in 0,1,2)
        // - Right zone: x >= 3 (9 nodes: indices with x in 3,4,5)
        let topology = VerticalWallTopology::new(6, 3);

        // Wrap with topology - now we have two disconnected zones
        let mesh_with_topo = MeshWithTopology::new(&base_mesh, &topology);
        assert_eq!(mesh_with_topo.len(), 18); // Still 18 nodes, but disconnected

        // Verify the topology actually creates disconnected zones
        // Node 0 (x=0) should not be able to reach node 3 (x=3)
        let successors_0: Vec<_> = mesh_with_topo.successors(0, false).collect();
        // From node 0 (corner), neighbors are 1 (x=1), 6 (x=0), 7 (x=1) - all in left zone
        assert!(successors_0.iter().all(|g| topology.allowed(0, g.target()).unwrap()));

        // Now filter to only the connected zone using FilteredMesh
        let filtered = FilteredMesh::new(mesh_with_topo);
        assert_eq!(filtered.len(), 9); // Only left zone (9 nodes reachable from 0)

        // Verify gradient works on the filtered mesh
        let mut gradient = Gradient::from_mesh(&filtered);
        gradient.set_distance(0, 0.0);
        gradient.spread(&filtered);

        // All 9 nodes should be reachable
        for i in 0..filtered.len() {
            assert!(
                gradient.get_distance(i) < DISTANCE_MAX,
                "Node {} should be reachable",
                i
            );
        }
    }

    #[test]
    fn test_filtered_mesh_with_topology_via_unique() {
        // Same test but using the Mesh::unique() method
        let base_mesh = Full2D::new(6, 3);
        let topology = VerticalWallTopology::new(6, 3);
        let mesh_with_topo = MeshWithTopology::new(&base_mesh, &topology);

        // Use the trait method unique() which returns FilteredMesh
        let filtered = mesh_with_topo.unique();
        assert_eq!(filtered.len(), 9);

        // Verify we can still translate indices
        // Index 0 in filtered should be index 0 in original (top-left corner)
        assert_eq!(filtered.to_original_index(0), 0);
    }

    #[test]
    fn test_filtered_mesh_basic() {
        // Grid with two separate zones
        let mesh =
            Compact2D::from_text("##########\n#    #   #\n#    #   #\n##########\n").unwrap();
        assert_eq!(mesh.len(), 14); // 8 left + 6 right

        let filtered = FilteredMesh::new(mesh);
        assert_eq!(filtered.len(), 8); // Only left zone
    }

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

        let mut gradient = Gradient::from_mesh(&filtered);
        gradient.set_distance(0, 0.0);
        gradient.spread(&filtered);

        // All nodes should be reachable now
        for i in 0..filtered.len() {
            assert!(gradient.get_distance(i) < DISTANCE_MAX);
        }
    }

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

        // Index 0 should map to itself (first reachable node)
        assert_eq!(filtered.to_original_index(0), 0);

        // Original index 0 should map to filtered index 0
        assert_eq!(filtered.to_filtered_index(0), Some(0));

        // Some indices in the right zone should be filtered out
        // (they would have original indices >= 8)
        assert_eq!(filtered.to_filtered_index(13), None);
    }

    #[test]
    fn test_filtered_mesh_fully_connected() {
        // Mesh with no disconnected zones
        let mesh = Compact2D::from_text("...\n...\n...").unwrap();
        assert_eq!(mesh.len(), 9);

        let filtered = FilteredMesh::new(mesh);
        assert_eq!(filtered.len(), 9); // All nodes kept
    }

    #[test]
    fn test_filtered_mesh_empty() {
        let mesh = Compact2D::from_text("###\n###\n###").unwrap();
        assert_eq!(mesh.len(), 0);

        let filtered = FilteredMesh::new(mesh);
        assert_eq!(filtered.len(), 0);
    }

    #[test]
    #[cfg(feature = "serde")]
    fn test_filtered_mesh_serde() {
        let mesh =
            Compact2D::from_text("##########\n#    #   #\n#    #   #\n##########\n").unwrap();
        let filtered = FilteredMesh::new(mesh);
        assert_eq!(filtered.len(), 8);

        // Serialize
        let json = serde_json::to_string(&filtered).unwrap();

        // Deserialize
        let deserialized: FilteredMesh<Compact2D> = serde_json::from_str(&json).unwrap();

        // Verify
        assert_eq!(filtered.len(), deserialized.len());
        assert_eq!(
            filtered.to_original_index(0),
            deserialized.to_original_index(0)
        );

        // Verify it still works as a mesh
        let mut gradient = Gradient::from_mesh(&deserialized);
        gradient.set_distance(0, 0.0);
        gradient.spread(&deserialized);
        for i in 0..deserialized.len() {
            assert!(gradient.get_distance(i) < DISTANCE_MAX);
        }
    }
}