shortestpath 0.10.0

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

use crate::distance::*;
use crate::gate::*;
use crate::mesh::*;
use std::fmt;
use std::iter::Iterator;

/// Stores pathfinding results as a gradient field over a mesh.
///
/// A `Gradient` represents the solution to a pathfinding problem using Dijkstra's algorithm.
/// It maintains, for each node in a mesh, the shortest distance to a target and which
/// neighboring node to move to in order to reach that target.
///
/// # How it works
///
/// The gradient is computed by "spreading" distance values across the mesh:
/// 1. Initialize all nodes with maximum distance except the target(s) which have distance 0
/// 2. Iteratively update each node's distance based on its neighbors' distances
/// 3. Continue until no more updates occur (convergence)
///
/// Once computed, the gradient provides a complete pathfinding solution where any node
/// can follow its target pointer to eventually reach the goal.
///
/// # Fields
///
/// * `slots` - A vector of `Gate` objects, one per node in the mesh. Each gate stores
///   the target index (next node on the shortest path) and the total distance to the goal.
///
/// # Example
///
/// ```
/// use shortestpath::{Gradient, mesh_2d::Full2D};
///
/// // Create a 5x5 grid
/// let mesh = Full2D::new(5, 5);
/// let mut gradient = Gradient::from_mesh(&mesh);
///
/// // Set the center node (index 12) as the target with distance 0
/// gradient.set_distance(12, 0.0);
///
/// // Compute the gradient (spread distances to all nodes)
/// gradient.spread(&mesh);
///
/// // Now any node can query its distance to the target
/// let distance_to_target = gradient.get_distance(0);
/// // And which neighbor to move toward
/// let next_step = gradient.get_target(0);
/// ```
#[derive(Debug, Clone, PartialEq, Eq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub struct Gradient {
    /// Vector of gates, one per mesh node. Each gate contains the next step
    /// toward the target and the total distance to reach it.
    slots: Vec<Gate>,
}

impl Gradient {
    /// Creates a new gradient with the specified number of nodes.
    ///
    /// All nodes are initialized with maximum distance and point to themselves.
    /// This represents an unsolved pathfinding problem.
    ///
    /// # Arguments
    ///
    /// * `len` - The number of nodes in the gradient
    ///
    /// # Example
    ///
    /// ```
    /// use shortestpath::Gradient;
    ///
    /// let gradient = Gradient::with_len(25);
    /// ```
    pub fn with_len(len: usize) -> Self {
        let mut slots = Vec::with_capacity(len);
        for i in 0..len {
            slots.push(Gate::new(i, DISTANCE_MAX));
        }
        Gradient { slots }
    }

    /// Creates a new gradient sized to match the given mesh.
    ///
    /// This is a convenience constructor that creates a gradient with the same
    /// number of nodes as the mesh.
    ///
    /// # Arguments
    ///
    /// * `mesh` - The mesh to create a gradient for
    ///
    /// # Example
    ///
    /// ```
    /// use shortestpath::{Gradient, mesh_2d::Full2D};
    ///
    /// let mesh = Full2D::new(10, 10);
    /// let gradient = Gradient::from_mesh(&mesh);
    /// ```
    pub fn from_mesh(mesh: &impl Mesh) -> Self {
        Self::with_len(mesh.len())
    }

    /// Resets the gradient to its initial state.
    ///
    /// Sets all nodes to maximum distance (DISTANCE_MAX) and makes each node point to itself.
    /// This is useful when you want to reuse a gradient for a new pathfinding computation
    /// without allocating a new one.
    ///
    /// # Example
    ///
    /// ```
    /// use shortestpath::{Gradient, mesh_2d::Full2D};
    ///
    /// let mesh = Full2D::new(5, 5);
    /// let mut gradient = Gradient::from_mesh(&mesh);
    ///
    /// // Compute a path to center
    /// gradient.set_distance(12, 0.0);
    /// gradient.spread(&mesh);
    ///
    /// // Reset and compute a new path to a different target
    /// gradient.reset();
    /// gradient.set_distance(24, 0.0);
    /// gradient.spread(&mesh);
    /// ```
    pub fn reset(&mut self) {
        for (i, slot) in self.slots.iter_mut().enumerate() {
            slot.target = i as u32;
            slot.distance = DISTANCE_MAX;
        }
    }

    /// Spreads distance values forward through the mesh (from index 0 to end).
    ///
    /// Updates each node's distance based on its neighbors, iterating from the
    /// first node to the last. This is one iteration of the gradient computation.
    ///
    /// # Arguments
    ///
    /// * `mesh` - The mesh to spread across
    /// * `incr` - Distance increment added to unchanged nodes (0.0 for standard pathfinding)
    ///
    /// # Returns
    ///
    /// The number of nodes that were updated during this pass
    ///
    /// # Panics
    ///
    /// Panics if the gradient size doesn't match the mesh size
    pub fn spread_forward(&mut self, mesh: &impl Mesh, incr: f32) -> usize {
        if self.slots.len() != mesh.len() {
            panic!(
                "slots len {} does not match mesh len {}",
                self.slots.len(),
                mesh.len()
            )
        }
        let mut count = 0;
        for i in 0..mesh.len() {
            if self.spread_slot(i, mesh, incr) {
                count += 1;
            }
        }
        count
    }

    /// Spreads distance values backward through the mesh (from end to index 0).
    ///
    /// Updates each node's distance based on its neighbors, iterating from the
    /// last node to the first. This is one iteration of the gradient computation.
    ///
    /// # Arguments
    ///
    /// * `mesh` - The mesh to spread across
    /// * `incr` - Distance increment added to unchanged nodes (0.0 for standard pathfinding)
    ///
    /// # Returns
    ///
    /// The number of nodes that were updated during this pass
    ///
    /// # Panics
    ///
    /// Panics if the gradient size doesn't match the mesh size
    pub fn spread_backward(&mut self, mesh: &impl Mesh, incr: f32) -> usize {
        if self.slots.len() != mesh.len() {
            panic!(
                "slots len {} does not match mesh len {}",
                self.slots.len(),
                mesh.len()
            )
        }
        let mut count = 0;
        for i in (0..mesh.len()).rev() {
            if self.spread_slot(i, mesh, incr) {
                count += 1;
            }
        }
        count
    }

    /// Spreads distance values in both directions through the mesh.
    ///
    /// Performs one forward pass followed by one backward pass. This can
    /// converge faster than spreading in only one direction.
    ///
    /// # Arguments
    ///
    /// * `mesh` - The mesh to spread across
    /// * `incr` - Distance increment added to unchanged nodes (0.0 for standard pathfinding)
    ///
    /// # Returns
    ///
    /// The total number of nodes updated in both passes
    ///
    /// # Panics
    ///
    /// Panics if the gradient size doesn't match the mesh size
    pub fn spread_both(&mut self, mesh: &impl Mesh, incr: f32) -> usize {
        self.spread_forward(mesh, incr) + self.spread_backward(mesh, incr)
    }

    /// Updates a single node's distance based on its neighbors.
    ///
    /// This is the core operation of Dijkstra's algorithm. It examines all neighbors
    /// of the given node and selects the one that provides the shortest path to the target.
    ///
    /// # Arguments
    ///
    /// * `here_index` - The index of the node to update
    /// * `mesh` - The mesh providing neighbor connectivity
    /// * `incr` - Distance increment for unchanged nodes (used to invalidate old paths)
    ///
    /// # Returns
    ///
    /// `true` if the node's distance or target was updated, `false` otherwise
    pub fn spread_slot(&mut self, here_index: usize, mesh: &impl Mesh, incr: f32) -> bool {
        let mut best = Gate::new(here_index, 0.0);
        let mut best_total = self.slots[here_index].distance;
        for neighbor in mesh.successors(here_index, false) {
            let neighbor_total = self.slots[neighbor.target()].distance + neighbor.distance;
            if neighbor_total < best_total {
                best = neighbor;
                best_total = neighbor_total;
            }
        }
        if best.target != here_index as u32 {
            self.slots[here_index] = Gate::new(
                best.target(),
                self.slots[best.target()].distance + best.distance,
            );
            true
        } else {
            // If nothing has changed, consider we're farther by "incr" which
            // avoids old paths to remain valid when they should not be.
            if incr != 0.0 {
                self.slots[here_index].distance += incr;
            }
            false
        }
    }

    /// Increments all distances in the gradient by a fixed amount.
    ///
    /// This can be used to invalidate or age existing pathfinding results,
    /// particularly useful in dynamic environments where the target moves.
    ///
    /// # Arguments
    ///
    /// * `incr` - The amount to add to each node's distance
    pub fn incr(&mut self, incr: f32) {
        self.slots.iter_mut().for_each(|i| i.distance += incr);
    }

    /// Computes the complete gradient by spreading until convergence.
    ///
    /// This repeatedly calls `spread_both()` until no nodes are updated,
    /// ensuring all nodes have their optimal distance and target values.
    /// This is the main method to use for computing a pathfinding solution.
    ///
    /// # Arguments
    ///
    /// * `mesh` - The mesh to compute the gradient over
    ///
    /// # Example
    ///
    /// ```
    /// use shortestpath::{Gradient, mesh_2d::Full2D};
    ///
    /// let mesh = Full2D::new(10, 10);
    /// let mut gradient = Gradient::from_mesh(&mesh);
    ///
    /// // Set target at center
    /// gradient.set_distance(55, 0.0);
    ///
    /// // Compute complete solution
    /// gradient.spread(&mesh);
    ///
    /// // Query results
    /// println!("Distance from corner: {}", gradient.get_distance(0));
    /// ```
    pub fn spread(&mut self, mesh: &impl Mesh) {
        while self.spread_both(mesh, 0.0) > 0 {}
    }

    /// Sets the distance for a specific node.
    ///
    /// Typically used to initialize target nodes with distance 0.0 before
    /// spreading the gradient.
    ///
    /// # Arguments
    ///
    /// * `slot_index` - The node index to set
    /// * `value` - The distance value to assign
    pub fn set_distance(&mut self, slot_index: usize, value: f32) {
        self.slots[slot_index].distance = value
    }

    /// Gets the distance value for a specific node.
    ///
    /// After spreading, this returns the shortest distance from this node
    /// to the nearest target.
    ///
    /// # Arguments
    ///
    /// * `slot_index` - The node index to query
    ///
    /// # Returns
    ///
    /// The distance to the nearest target
    pub fn get_distance(&self, slot_index: usize) -> f32 {
        self.slots[slot_index].distance
    }

    /// Gets the target index for a specific node.
    ///
    /// After spreading, this returns which neighboring node to move to
    /// in order to follow the shortest path to the target.
    ///
    /// # Arguments
    ///
    /// * `slot_index` - The node index to query
    ///
    /// # Returns
    ///
    /// The index of the next node on the shortest path
    pub fn get_target(&self, slot_index: usize) -> usize {
        self.slots[slot_index].target()
    }

    /// Gets the complete gate information for a specific node.
    ///
    /// Returns a reference to the `Gate` containing both the target index
    /// and distance for this node.
    ///
    /// # Arguments
    ///
    /// * `slot_index` - The node index to query
    ///
    /// # Returns
    ///
    /// A reference to the gate at this node
    pub fn get(&self, slot_index: usize) -> &Gate {
        &self.slots[slot_index]
    }

    /// Returns all possible moves from a given node, sorted by distance to goal.
    ///
    /// This method queries the mesh for all neighboring nodes (successors) from the
    /// given position and returns them sorted by their gradient distance (distance to
    /// the goal), with the closest neighbor first.
    ///
    /// The returned gates contain the move cost (edge weight from the mesh), not the
    /// gradient distance. The gradient distance is only used for sorting.
    ///
    /// # Arguments
    ///
    /// * `node_index` - The index of the node to get moves from
    /// * `mesh` - The mesh providing neighbor connectivity
    /// * `backward` - If `true`, iterate over successors in reverse order before sorting
    ///
    /// # Returns
    ///
    /// A `Vec<Gate>` sorted by gradient distance (lowest first). Each gate contains
    /// the neighbor's index and the move cost to reach it.
    ///
    /// # Example
    ///
    /// ```
    /// use shortestpath::{Gradient, mesh_2d::Full2D};
    ///
    /// let mesh = Full2D::new(5, 5);
    /// let mut gradient = Gradient::from_mesh(&mesh);
    /// gradient.set_distance(12, 0.0);
    /// gradient.spread(&mesh);
    ///
    /// // Get all possible moves from node 0, sorted by best to worst
    /// let moves = gradient.moves(&mesh, 0, false);
    /// // First move is the best (lowest total cost)
    /// if let Some(best_move) = moves.first() {
    ///     println!("Best move: to node {}, total cost: {}",
    ///              best_move.target, best_move.distance);
    /// }
    ///
    /// // Get moves with backward iteration over successors
    /// let moves_backward = gradient.moves(&mesh, 0, true);
    /// ```
    pub fn moves(&self, mesh: &impl Mesh, node_index: usize, backward: bool) -> Vec<Gate> {
        let mut moves: Vec<Gate> = mesh
            .successors(node_index, backward)
            .collect();

        // Sort by target's gradient distance (lowest first), but keep the move cost in the gate
        moves.sort_by(|a, b| {
            self.slots[a.target()]
                .distance
                .partial_cmp(&self.slots[b.target()].distance)
                .unwrap_or(std::cmp::Ordering::Equal)
        });

        moves
    }

    /// Returns the best move from a given node (lowest cost).
    ///
    /// This method finds the move with the lowest total distance cost to the goal.
    /// After the gradient has been spread, this will return the next step on the optimal path.
    /// Returns `None` if the current node is the target or if there are no successors.
    ///
    /// # Arguments
    ///
    /// * `node_index` - The index of the node to get the best move from
    /// * `mesh` - The mesh providing neighbor connectivity
    /// * `backward` - If `true`, iterate over successors in reverse order (affects tie-breaking)
    ///
    /// # Returns
    ///
    /// An `Option<Gate>` containing the best move, or `None` if there are no successors or
    /// if the current node is already at the target (distance 0).
    ///
    /// # Example
    ///
    /// ```
    /// use shortestpath::{Gradient, mesh_2d::Full2D};
    ///
    /// let mesh = Full2D::new(5, 5);
    /// let mut gradient = Gradient::from_mesh(&mesh);
    /// gradient.set_distance(12, 0.0);
    /// gradient.spread(&mesh);
    ///
    /// // Get the best move from node 0
    /// if let Some(best) = gradient.best(&mesh, 0, false) {
    ///     println!("Best move: to node {}, cost: {}", best.target, best.distance);
    /// }
    /// ```
    pub fn best(&self, mesh: &impl Mesh, node_index: usize, backward: bool) -> Option<Gate> {
        let current_distance = self.slots[node_index].distance;

        mesh.successors(node_index, backward)
            .filter(|gate| self.slots[gate.target()].distance < current_distance)
            .min_by(|a, b| {
                self.slots[a.target()]
                    .distance
                    .partial_cmp(&self.slots[b.target()].distance)
                    .unwrap_or(std::cmp::Ordering::Equal)
            })
    }

    /// Returns the worst move from a given node (highest cost).
    ///
    /// This method finds the move with the highest total distance cost to the goal.
    /// It only returns a move if it actually worsens the situation from the current node
    /// (i.e., the move's cost is strictly greater than the current node's distance).
    ///
    /// # Arguments
    ///
    /// * `node_index` - The index of the node to get the worst move from
    /// * `mesh` - The mesh providing neighbor connectivity
    /// * `backward` - If `true`, iterate over successors in reverse order (affects tie-breaking)
    ///
    /// # Returns
    ///
    /// An `Option<Gate>` containing the worst move, or `None` if there are no successors
    /// or if all moves have equal or lower cost than staying at the current node.
    ///
    /// # Example
    ///
    /// ```
    /// use shortestpath::{Gradient, mesh_2d::Full2D};
    ///
    /// let mesh = Full2D::new(5, 5);
    /// let mut gradient = Gradient::from_mesh(&mesh);
    /// gradient.set_distance(12, 0.0);
    /// gradient.spread(&mesh);
    ///
    /// // Get the worst move from node 0 (may be None if all moves improve the situation)
    /// if let Some(worst) = gradient.worst(&mesh, 0, false) {
    ///     println!("Worst move: to node {}, cost: {}", worst.target, worst.distance);
    /// }
    /// ```
    pub fn worst(&self, mesh: &impl Mesh, node_index: usize, backward: bool) -> Option<Gate> {
        let current_distance = self.slots[node_index].distance;

        mesh.successors(node_index, backward)
            .filter(|gate| self.slots[gate.target()].distance > current_distance)
            .max_by(|a, b| {
                self.slots[a.target()]
                    .distance
                    .partial_cmp(&self.slots[b.target()].distance)
                    .unwrap_or(std::cmp::Ordering::Equal)
            })
    }

    /// Follows the gradient from a starting node by repeatedly taking best moves.
    ///
    /// This method assumes the gradient has already been computed (via `spread()`).
    /// It repeatedly selects the best move from the current position until no more
    /// improving moves are available. It does not check if a target was reached -
    /// it simply returns whatever path it could follow.
    ///
    /// # Arguments
    ///
    /// * `mesh` - The mesh providing neighbor connectivity
    /// * `start_index` - The index of the starting node
    /// * `backward` - If `true`, iterate over successors in reverse order (affects tie-breaking)
    ///
    /// # Returns
    ///
    /// A `Vec<Gate>` containing the path followed from the start. Returns an empty vector
    /// if there are no improving moves from the start position.
    ///
    /// # Example
    ///
    /// ```
    /// use shortestpath::{Gradient, mesh_2d::Full2D};
    ///
    /// let mesh = Full2D::new(5, 5);
    /// let mut gradient = Gradient::from_mesh(&mesh);
    /// gradient.set_distance(24, 0.0); // Bottom-right corner
    /// gradient.spread(&mesh);
    ///
    /// // Follow the gradient from top-left (0)
    /// let path = gradient.follow(&mesh, 0, false);
    /// println!("Followed {} steps", path.len());
    /// ```
    pub fn follow(
        &self,
        mesh: &impl Mesh,
        start_index: usize,
        backward: bool,
    ) -> Vec<Gate> {
        let mut path = Vec::new();
        let mut current_index = start_index;
        let mut current_backward = backward;
        let max_steps = mesh.len(); // Maximum possible path length

        for _ in 0..max_steps {
            // Get the best move from current position
            if let Some(best_move) = self.best(mesh, current_index, current_backward) {
                current_index = best_move.target();
                path.push(best_move);
                // Alternate backward for next iteration
                current_backward = !current_backward;
            } else {
                // No best move available - stop following
                break;
            }
        }

        path
    }

    /// Finds the shortest path from start to target by computing the gradient.
    ///
    /// This is a complete pathfinding solution that:
    /// 1. Resets the gradient
    /// 2. Sets the target distance to 0
    /// 3. Spreads the gradient across the mesh
    /// 4. Follows the gradient from start to target
    ///
    /// This method modifies the gradient. If you want to follow an existing gradient
    /// without recomputing it, use `follow()` instead.
    ///
    /// # Arguments
    ///
    /// * `start_index` - The index of the starting node
    /// * `target_index` - The index of the target node
    /// * `mesh` - The mesh providing neighbor connectivity
    /// * `backward` - If `true`, iterate over successors in reverse order (affects tie-breaking)
    ///
    /// # Returns
    ///
    /// An `Option<Vec<Gate>>` containing the path from start to target if a path exists.
    /// Returns `None` if the target is unreachable.
    ///
    /// # Example
    ///
    /// ```
    /// use shortestpath::{Gradient, mesh_2d::Full2D};
    ///
    /// let mesh = Full2D::new(5, 5);
    /// let mut gradient = Gradient::from_mesh(&mesh);
    ///
    /// // Find path from top-left (0) to bottom-right (24)
    /// // This automatically computes the gradient
    /// if let Some(path) = gradient.find(&mesh, 0, 24, false) {
    ///     println!("Path found with {} steps", path.len());
    /// }
    /// ```
    pub fn find(
        &mut self,
        mesh: &impl Mesh,
        start_index: usize,
        target_index: usize,
        backward: bool,
    ) -> Option<Vec<Gate>> {
        // Reset and initialize gradient
        self.reset();
        self.set_distance(target_index, 0.0);
        self.spread(mesh);

        // Follow the computed gradient
        let path = self.follow(mesh, start_index, backward);

        // Check if we reached the target
        if path.is_empty() {
            // Empty path - check if start is already at target
            if start_index == target_index {
                Some(path)
            } else {
                None
            }
        } else {
            // Check if last gate leads to target
            if path.last().unwrap().target == target_index as u32 {
                Some(path)
            } else {
                None
            }
        }
    }
}

impl std::fmt::Display for Gradient {
    fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
        let total = self.slots.len();
        let targets = self.slots.iter().filter(|s| s.distance == 0.0).count();
        let unreachable = self
            .slots
            .iter()
            .filter(|s| s.distance == DISTANCE_MAX)
            .count();
        let reachable = total - unreachable;

        writeln!(f, "Gradient ({} nodes):", total)?;
        writeln!(f, "  Targets: {}", targets)?;
        writeln!(f, "  Reachable: {} ({:.1}%)", reachable, (reachable as f64 / total as f64) * 100.0)?;
        write!(f, "  Unreachable: {} ({:.1}%)", unreachable, (unreachable as f64 / total as f64) * 100.0)
    }
}

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

    #[test]
    fn test_spread_forward() {
        let rect = Full2D::new(5, 5);
        let mut grad = Gradient::from_mesh(&rect);
        grad.set_distance(12, 0.0);
        grad.spread_forward(&rect, 0.0);
        // row 0
        assert_eq!(DISTANCE_MAX, grad.get_distance(0));
        assert_eq!(0, grad.get_target(0));
        assert_eq!(DISTANCE_MAX, grad.get_distance(1));
        assert_eq!(1, grad.get_target(1));
        assert_eq!(DISTANCE_MAX, grad.get_distance(2));
        assert_eq!(2, grad.get_target(2));
        assert_eq!(DISTANCE_MAX, grad.get_distance(3));
        assert_eq!(3, grad.get_target(3));
        assert_eq!(DISTANCE_MAX, grad.get_distance(4));
        assert_eq!(4, grad.get_target(4));
        // row 1
        assert_eq!(DISTANCE_MAX, grad.get_distance(5));
        assert_eq!(5, grad.get_target(5));
        assert_eq!(DISTANCE_DIAGONAL, grad.get_distance(6));
        assert_eq!(12, grad.get_target(6));
        assert_eq!(DISTANCE_STRAIGHT, grad.get_distance(7));
        assert_eq!(12, grad.get_target(7));
        assert_eq!(DISTANCE_DIAGONAL, grad.get_distance(8));
        assert_eq!(12, grad.get_target(8));
        assert_eq!(DISTANCE_STRAIGHT + DISTANCE_DIAGONAL, grad.get_distance(9));
        assert_eq!(8, grad.get_target(9));
        // row 2
        assert_eq!(2.0 * DISTANCE_DIAGONAL, grad.get_distance(10));
        assert_eq!(6, grad.get_target(10));
        assert_eq!(DISTANCE_STRAIGHT, grad.get_distance(11));
        assert_eq!(12, grad.get_target(11));
        assert_eq!(DISTANCE_MIN, grad.get_distance(12));
        assert_eq!(12, grad.get_target(12));
        assert_eq!(DISTANCE_STRAIGHT, grad.get_distance(13));
        assert_eq!(12, grad.get_target(13));
        assert_eq!(2.0 * DISTANCE_STRAIGHT, grad.get_distance(14));
        assert_eq!(13, grad.get_target(14));
        // row 3
        assert_eq!(DISTANCE_STRAIGHT + DISTANCE_DIAGONAL, grad.get_distance(15));
        assert_eq!(11, grad.get_target(15));
        assert_eq!(DISTANCE_DIAGONAL, grad.get_distance(16));
        assert_eq!(12, grad.get_target(16));
        assert_eq!(DISTANCE_STRAIGHT, grad.get_distance(17));
        assert_eq!(12, grad.get_target(17));
        assert_eq!(DISTANCE_DIAGONAL, grad.get_distance(18));
        assert_eq!(12, grad.get_target(18));
        assert_eq!(DISTANCE_STRAIGHT + DISTANCE_DIAGONAL, grad.get_distance(19));
        assert_eq!(18, grad.get_target(19));
        // row 4
        assert_eq!(2.0 * DISTANCE_DIAGONAL, grad.get_distance(20));
        assert_eq!(16, grad.get_target(20));
        assert_eq!(DISTANCE_STRAIGHT + DISTANCE_DIAGONAL, grad.get_distance(21));
        assert_eq!(16, grad.get_target(21));
        assert_eq!(2.0 * DISTANCE_STRAIGHT, grad.get_distance(22));
        assert_eq!(17, grad.get_target(22));
        assert_eq!(DISTANCE_STRAIGHT + DISTANCE_DIAGONAL, grad.get_distance(23));
        assert_eq!(17, grad.get_target(23));
        assert_eq!(2.0 * DISTANCE_DIAGONAL, grad.get_distance(24));
        assert_eq!(18, grad.get_target(24));
    }

    #[test]
    fn test_spread_backward() {
        let rect = Full2D::new(5, 5);
        let mut grad = Gradient::from_mesh(&rect);
        grad.set_distance(12, 0.0);
        grad.spread_backward(&rect, 0.0);
        // row 5
        assert_eq!(DISTANCE_MAX, grad.get_distance(23));
        assert_eq!(24, grad.get_target(24));
        assert_eq!(DISTANCE_MAX, grad.get_distance(23));
        assert_eq!(23, grad.get_target(23));
        assert_eq!(DISTANCE_MAX, grad.get_distance(22));
        assert_eq!(22, grad.get_target(22));
        assert_eq!(DISTANCE_MAX, grad.get_distance(21));
        assert_eq!(21, grad.get_target(21));
        assert_eq!(DISTANCE_MAX, grad.get_distance(20));
        assert_eq!(20, grad.get_target(20));
        // row 4
        assert_eq!(DISTANCE_MAX, grad.get_distance(19));
        assert_eq!(19, grad.get_target(19));
        assert_eq!(DISTANCE_DIAGONAL, grad.get_distance(18));
        assert_eq!(12, grad.get_target(18));
        assert_eq!(DISTANCE_STRAIGHT, grad.get_distance(17));
        assert_eq!(12, grad.get_target(17));
        assert_eq!(DISTANCE_DIAGONAL, grad.get_distance(16));
        assert_eq!(12, grad.get_target(16));
        assert_eq!(DISTANCE_STRAIGHT + DISTANCE_DIAGONAL, grad.get_distance(15));
        assert_eq!(16, grad.get_target(15));
        // row 3
        assert_eq!(2.0 * DISTANCE_DIAGONAL, grad.get_distance(14));
        assert_eq!(18, grad.get_target(14));
        assert_eq!(DISTANCE_STRAIGHT, grad.get_distance(13));
        assert_eq!(12, grad.get_target(13));
        assert_eq!(DISTANCE_MIN, grad.get_distance(12));
        assert_eq!(12, grad.get_target(12));
        assert_eq!(DISTANCE_STRAIGHT, grad.get_distance(11));
        assert_eq!(12, grad.get_target(11));
        assert_eq!(2.0 * DISTANCE_STRAIGHT, grad.get_distance(10));
        assert_eq!(11, grad.get_target(10));
        // row 1
        assert_eq!(DISTANCE_STRAIGHT + DISTANCE_DIAGONAL, grad.get_distance(9));
        assert_eq!(13, grad.get_target(9));
        assert_eq!(DISTANCE_DIAGONAL, grad.get_distance(8));
        assert_eq!(12, grad.get_target(8));
        assert_eq!(DISTANCE_STRAIGHT, grad.get_distance(7));
        assert_eq!(12, grad.get_target(7));
        assert_eq!(DISTANCE_DIAGONAL, grad.get_distance(6));
        assert_eq!(12, grad.get_target(6));
        assert_eq!(DISTANCE_STRAIGHT + DISTANCE_DIAGONAL, grad.get_distance(5));
        assert_eq!(6, grad.get_target(5));
        // row 0
        assert_eq!(2.0 * DISTANCE_DIAGONAL, grad.get_distance(4));
        assert_eq!(8, grad.get_target(4));
        assert_eq!(DISTANCE_STRAIGHT + DISTANCE_DIAGONAL, grad.get_distance(3));
        assert_eq!(8, grad.get_target(3));
        assert_eq!(2.0 * DISTANCE_STRAIGHT, grad.get_distance(2));
        assert_eq!(7, grad.get_target(2));
        assert_eq!(DISTANCE_STRAIGHT + DISTANCE_DIAGONAL, grad.get_distance(1));
        assert_eq!(7, grad.get_target(1));
        assert_eq!(2.0 * DISTANCE_DIAGONAL, grad.get_distance(0));
        assert_eq!(6, grad.get_target(0));
    }

    #[test]
    fn test_spread() {
        let rect = Full2D::new(5, 5);
        let mut grad = Gradient::from_mesh(&rect);
        grad.set_distance(12, 0.0);
        grad.spread(&rect);
        // row 0
        assert_eq!(2.0 * DISTANCE_DIAGONAL, grad.get_distance(0));
        assert_eq!(6, grad.get_target(0));
        assert_eq!(DISTANCE_STRAIGHT + DISTANCE_DIAGONAL, grad.get_distance(1));
        assert_eq!(7, grad.get_target(1));
        assert_eq!(2.0 * DISTANCE_STRAIGHT, grad.get_distance(2));
        assert_eq!(7, grad.get_target(2));
        assert_eq!(DISTANCE_STRAIGHT + DISTANCE_DIAGONAL, grad.get_distance(3));
        assert_eq!(8, grad.get_target(3));
        assert_eq!(2.0 * DISTANCE_DIAGONAL, grad.get_distance(4));
        assert_eq!(8, grad.get_target(4));
        // row 1
        assert_eq!(DISTANCE_STRAIGHT + DISTANCE_DIAGONAL, grad.get_distance(5));
        assert_eq!(6, grad.get_target(5));
        assert_eq!(DISTANCE_DIAGONAL, grad.get_distance(6));
        assert_eq!(12, grad.get_target(6));
        assert_eq!(DISTANCE_STRAIGHT, grad.get_distance(7));
        assert_eq!(12, grad.get_target(7));
        assert_eq!(DISTANCE_DIAGONAL, grad.get_distance(8));
        assert_eq!(12, grad.get_target(8));
        assert_eq!(DISTANCE_STRAIGHT + DISTANCE_DIAGONAL, grad.get_distance(9));
        assert_eq!(8, grad.get_target(9));
        // row 2
        assert_eq!(2.0 * DISTANCE_STRAIGHT, grad.get_distance(10));
        assert_eq!(11, grad.get_target(10));
        assert_eq!(DISTANCE_STRAIGHT, grad.get_distance(11));
        assert_eq!(12, grad.get_target(11));
        assert_eq!(DISTANCE_MIN, grad.get_distance(12));
        assert_eq!(12, grad.get_target(12));
        assert_eq!(DISTANCE_STRAIGHT, grad.get_distance(13));
        assert_eq!(12, grad.get_target(13));
        assert_eq!(2.0 * DISTANCE_STRAIGHT, grad.get_distance(14));
        assert_eq!(13, grad.get_target(14));
        // row 3
        assert_eq!(DISTANCE_STRAIGHT + DISTANCE_DIAGONAL, grad.get_distance(15));
        assert_eq!(11, grad.get_target(15));
        assert_eq!(DISTANCE_DIAGONAL, grad.get_distance(16));
        assert_eq!(12, grad.get_target(16));
        assert_eq!(DISTANCE_STRAIGHT, grad.get_distance(17));
        assert_eq!(12, grad.get_target(17));
        assert_eq!(DISTANCE_DIAGONAL, grad.get_distance(18));
        assert_eq!(12, grad.get_target(18));
        assert_eq!(DISTANCE_STRAIGHT + DISTANCE_DIAGONAL, grad.get_distance(19));
        assert_eq!(18, grad.get_target(19));
        // row 4
        assert_eq!(2.0 * DISTANCE_DIAGONAL, grad.get_distance(20));
        assert_eq!(16, grad.get_target(20));
        assert_eq!(DISTANCE_STRAIGHT + DISTANCE_DIAGONAL, grad.get_distance(21));
        assert_eq!(16, grad.get_target(21));
        assert_eq!(2.0 * DISTANCE_STRAIGHT, grad.get_distance(22));
        assert_eq!(17, grad.get_target(22));
        assert_eq!(DISTANCE_STRAIGHT + DISTANCE_DIAGONAL, grad.get_distance(23));
        assert_eq!(17, grad.get_target(23));
        assert_eq!(2.0 * DISTANCE_DIAGONAL, grad.get_distance(24));
        assert_eq!(18, grad.get_target(24));
    }

    #[test]
    #[cfg(feature = "serde")]
    fn test_serde() {
        let mut gradient = Gradient::with_len(10);
        gradient.set_distance(5, 3.14);

        let json = serde_json::to_string(&gradient).unwrap();
        let deserialized: Gradient = serde_json::from_str(&json).unwrap();

        assert_eq!(gradient.get_distance(5), deserialized.get_distance(5));
        assert_eq!(gradient.get_target(5), deserialized.get_target(5));
    }

    #[test]
    fn test_moves() {
        let rect = Full2D::new(5, 5);
        let mut grad = Gradient::from_mesh(&rect);
        grad.set_distance(12, 0.0); // Center of 5x5 grid
        grad.spread(&rect);

        // Test moves from corner (node 0)
        // From 0, possible neighbors are: 1 (right), 5 (down), 6 (diagonal)
        let moves = grad.moves(&rect, 0, false);
        assert_eq!(moves.len(), 3);

        // The best move should be diagonal to node 6 (closest to target at 12)
        assert_eq!(moves[0].target, 6);
        // Distance is the move cost (diagonal move)
        assert_eq!(moves[0].distance, DISTANCE_DIAGONAL);

        // Verify moves are sorted by gradient distance (not by move cost)
        for i in 0..moves.len() - 1 {
            assert!(grad.get_distance(moves[i].target()) <= grad.get_distance(moves[i + 1].target()));
        }

        // Test moves from a node adjacent to the target (node 7, directly above 12)
        let moves_from_7 = grad.moves(&rect, 7, false);
        assert!(moves_from_7.len() > 0);

        // The best move from 7 should be to 12 (straight down)
        assert_eq!(moves_from_7[0].target, 12);
        // Distance is the move cost (straight move)
        assert_eq!(moves_from_7[0].distance, DISTANCE_STRAIGHT);

        // Test moves from the target itself (node 12)
        let moves_from_target = grad.moves(&rect, 12, false);
        // All moves from the target should have cost >= 0
        for mov in moves_from_target {
            assert!(mov.distance >= 0.0);
        }

        // Test backward iteration - should still be sorted by gradient distance after
        let moves_backward = grad.moves(&rect, 0, true);
        assert_eq!(moves_backward.len(), 3);
        // Still sorted by gradient distance
        for i in 0..moves_backward.len() - 1 {
            assert!(grad.get_distance(moves_backward[i].target()) <= grad.get_distance(moves_backward[i + 1].target()));
        }
        // Best move should still be the same
        assert_eq!(moves_backward[0].target, 6);
    }

    #[test]
    fn test_moves_backward_tiebreaking() {
        // Simple 2x2 grid test: from (0,0) to (1,1)
        // There are 3 paths with different costs:
        // - Diagonal: cost = sqrt(2) ≈ 1.414 (best)
        // - Horizontal then vertical: cost = 1 + 1 = 2.0 (tied)
        // - Vertical then horizontal: cost = 1 + 1 = 2.0 (tied)
        let rect = Full2D::new(2, 2);
        let mut grad = Gradient::from_mesh(&rect);

        // Set target at (1,1) which is node 3
        // Grid layout: 0 1
        //              2 3
        grad.set_distance(3, 0.0);
        grad.spread(&rect);

        // Get moves from (0,0) which is node 0
        // Neighbors of 0: 1 (right), 2 (down), 3 (diagonal)
        let moves_forward = grad.moves(&rect, 0, false);
        let moves_backward = grad.moves(&rect, 0, true);

        // Both should have 3 moves
        assert_eq!(moves_forward.len(), 3);
        assert_eq!(moves_backward.len(), 3);

        // Best move is diagonal (same in both)
        assert_eq!(moves_forward[0].target, 3); // diagonal is best
        assert_eq!(moves_backward[0].target, 3); // diagonal is best

        // The tied moves (horizontal and vertical) have same gradient distance (1.0 to goal)
        // so their order depends on iteration direction
        // Forward:  diagonal, horizontal(1), vertical(2)
        // Backward: diagonal, vertical(2), horizontal(1)
        assert_eq!(moves_forward[1].target, 1); // right (horizontal first)
        assert_eq!(moves_forward[2].target, 2); // down (vertical second)

        assert_eq!(moves_backward[1].target, 2); // down (vertical first)
        assert_eq!(moves_backward[2].target, 1); // right (horizontal second)

        // Verify they have equal move costs (both are straight moves = 1.0)
        assert_eq!(moves_forward[1].distance, DISTANCE_STRAIGHT);
        assert_eq!(moves_forward[2].distance, DISTANCE_STRAIGHT);
        assert_eq!(moves_backward[1].distance, DISTANCE_STRAIGHT);
        assert_eq!(moves_backward[2].distance, DISTANCE_STRAIGHT);
    }

    #[test]
    fn test_moves_returns_cost_not_gradient_distance() {
        // Test that moves() returns the MOVE COST, not the gradient distance
        // Use a 5x5 grid with target at bottom-right corner (24)
        //
        // Grid layout (with approximate gradient distances from target 24):
        //   0   1   2   3   4
        //  5.7 4.4 3.4 2.4 1.4
        //
        //   5   6   7   8   9
        //  4.4 3.4 2.4 1.4 1.0
        //
        //  10  11  12  13  14
        //  3.4 2.4 1.4 1.0 1.0
        //
        //  15  16  17  18  19
        //  2.4 1.4 1.0 1.0 1.0
        //
        //  20  21  22  23  24
        //  1.4 1.0 1.0 1.0 0.0  <- target
        //
        let rect = Full2D::new(5, 5);
        let mut grad = Gradient::from_mesh(&rect);
        grad.set_distance(24, 0.0);
        grad.spread(&rect);

        // From node 12 (center), neighbors are:
        // - 6 (diagonal up-left):   gradient dist ~3.4, move cost = DIAGONAL (~1.414)
        // - 7 (straight up):        gradient dist ~2.4, move cost = STRAIGHT (1.0)
        // - 8 (diagonal up-right):  gradient dist ~1.4, move cost = DIAGONAL (~1.414)
        // - 11 (straight left):     gradient dist ~2.4, move cost = STRAIGHT (1.0)
        // - 13 (straight right):    gradient dist ~1.0, move cost = STRAIGHT (1.0)
        // - 16 (diagonal down-left): gradient dist ~1.4, move cost = DIAGONAL (~1.414)
        // - 17 (straight down):     gradient dist ~1.0, move cost = STRAIGHT (1.0)
        // - 18 (diagonal down-right): gradient dist ~1.0, move cost = DIAGONAL (~1.414)

        let moves = grad.moves(&rect, 12, false);
        assert_eq!(moves.len(), 8);

        // First moves should be toward the target (lower-right quadrant)
        // Node 18 (diagonal to target) has gradient distance ~1.0 (DISTANCE_DIAGONAL from 24)
        // But the MOVE COST from 12 to 18 is DISTANCE_DIAGONAL

        // Find the move to node 18 (diagonal down-right toward target)
        let move_to_18 = moves.iter().find(|m| m.target == 18).unwrap();
        // The distance should be the MOVE COST (diagonal), NOT the gradient distance
        assert_eq!(move_to_18.distance, DISTANCE_DIAGONAL);
        // Verify gradient distance is different (it's DISTANCE_DIAGONAL from target)
        assert_eq!(grad.get_distance(18), DISTANCE_DIAGONAL);
        // In this case they happen to be equal, let's check a clearer case

        // Find the move to node 6 (diagonal up-left, away from target)
        let move_to_6 = moves.iter().find(|m| m.target == 6).unwrap();
        // Move cost is DIAGONAL (~1.414)
        assert_eq!(move_to_6.distance, DISTANCE_DIAGONAL);
        // But gradient distance is much larger (3 * DIAGONAL from target)
        // Node 6 is at (1,1), target is at (4,4), so 3 diagonal steps
        assert_eq!(grad.get_distance(6), 3.0 * DISTANCE_DIAGONAL);
        // These are clearly different!
        assert_ne!(move_to_6.distance, grad.get_distance(6));

        // Find the move to node 7 (straight up)
        let move_to_7 = moves.iter().find(|m| m.target == 7).unwrap();
        // Move cost is STRAIGHT (1.0)
        assert_eq!(move_to_7.distance, DISTANCE_STRAIGHT);
        // Gradient distance is larger (node 7 is far from target 24)
        // The key assertion: move cost != gradient distance
        assert!(
            grad.get_distance(7) > 2.0,
            "Node 7 should be far from target"
        );
        assert_ne!(move_to_7.distance, grad.get_distance(7));

        // Verify sorting is by gradient distance, not by move cost
        // moves[0] should have lowest gradient distance
        // moves[7] should have highest gradient distance
        for i in 0..moves.len() - 1 {
            let curr_grad_dist = grad.get_distance(moves[i].target());
            let next_grad_dist = grad.get_distance(moves[i + 1].target());
            assert!(
                curr_grad_dist <= next_grad_dist,
                "moves should be sorted by gradient distance: {} <= {} (targets {} and {})",
                curr_grad_dist,
                next_grad_dist,
                moves[i].target,
                moves[i + 1].target
            );
        }

        // The last move should be to node 6 (furthest from target)
        assert_eq!(moves[7].target, 6);
        assert_eq!(grad.get_distance(6), 3.0 * DISTANCE_DIAGONAL);
    }

    #[test]
    fn test_best_worst() {
        // Use a 5x5 grid with target at center
        let rect = Full2D::new(5, 5);
        let mut grad = Gradient::from_mesh(&rect);
        grad.set_distance(12, 0.0); // Center of 5x5 grid
        grad.spread(&rect);

        // Test best() from corner (node 0)
        let best = grad.best(&rect, 0, false);
        // After spread(), best() should find the neighbor with the lowest distance to target
        assert!(best.is_some(), "Should have a best move from corner");
        let best_gate = best.unwrap();
        // The best neighbor should have lower distance than current
        assert!(
            grad.get_distance(best_gate.target()) < grad.get_distance(0),
            "Best move should lead to node with lower distance"
        );

        // Test worst() from corner (node 0)
        // All neighbors should have lower distances (they're closer to target)
        // So worst() should return None (no move worsens the situation)
        let worst = grad.worst(&rect, 0, false);
        assert!(
            worst.is_none(),
            "All neighbors closer to target, no worst move"
        );

        // Test that backward affects tie-breaking for best()
        // Use 2x2 grid where horizontal and vertical have equal cost
        let rect2x2 = Full2D::new(2, 2);
        let mut grad2x2 = Gradient::from_mesh(&rect2x2);
        grad2x2.set_distance(3, 0.0);
        grad2x2.spread(&rect2x2);

        // After spread(), best() finds neighbors with lower distances
        let best_forward = grad2x2.best(&rect2x2, 0, false);
        let best_backward = grad2x2.best(&rect2x2, 0, true);

        assert!(best_forward.is_some(), "Should find best move");
        assert!(best_backward.is_some(), "Should find best move");

        // Both should point to the diagonal neighbor (node 3) as it's closest
        assert_eq!(best_forward.unwrap().target, 3);
        assert_eq!(best_backward.unwrap().target, 3);

        // Test worst on 2x2 grid - all neighbors have lower distances to target
        // So worst() should return None
        let worst_forward = grad2x2.worst(&rect2x2, 0, false);
        let worst_backward = grad2x2.worst(&rect2x2, 0, true);

        assert!(
            worst_forward.is_none(),
            "All neighbors closer to target, no worst move"
        );
        assert!(
            worst_backward.is_none(),
            "All neighbors closer to target, no worst move"
        );
    }

    #[test]
    fn test_best_worst_no_successors() {
        // Test with a 1x1 grid where the only node has no successors (itself)
        let rect1x1 = Full2D::new(1, 1);
        let grad1x1 = Gradient::from_mesh(&rect1x1);

        // Node 0 in a 1x1 grid has no neighbors (empty successors list)
        let best = grad1x1.best(&rect1x1, 0, false);
        let worst = grad1x1.worst(&rect1x1, 0, false);

        // Should return None since there are no successors
        assert!(best.is_none());
        assert!(worst.is_none());
    }

    #[test]
    fn test_maze_pathfinding() {
        use crate::mesh_2d::{Compact2D, Index2D};

        // Create a maze with walls (#) and walkable cells (.)
        // Start at (1,1), goal at (width-2, height-2) = (8,6)
        let map = "\
##########
#........#
#.##.###.#
#.....#..#
#.#####.##
#.#...#..#
#.#.#.##.#
#...#....#
##########";

        let mesh = Compact2D::from_text(map).expect("Failed to parse map");

        // Get dimensions
        let (width, height) = mesh.shape();
        assert_eq!(width, 10);
        assert_eq!(height, 9);

        // Convert (x,y) coordinates to mesh indices
        let start_xy = (1, 1);
        let goal_xy = (width - 2, height - 2);
        assert_eq!(goal_xy, (8, 7)); // (8, 7) should be walkable

        let start_index = mesh
            .xy_to_index(start_xy.0, start_xy.1)
            .unwrap_or_else(|e| panic!("Start {:?} should be walkable: {:?}", start_xy, e));
        let goal_index = mesh
            .xy_to_index(goal_xy.0, goal_xy.1)
            .unwrap_or_else(|e| panic!("Goal {:?} should be walkable: {:?}", goal_xy, e));

        // Create gradient with goal as target
        let mut gradient = Gradient::from_mesh(&mesh);
        gradient.set_distance(goal_index, 0.0);
        gradient.spread(&mesh);

        // Follow the gradient from start to goal
        let path = gradient.follow(&mesh, start_index, false);

        // Verify we found a path
        assert!(
            !path.is_empty(),
            "Should find a path. Start distance: {}, Goal distance: {}",
            gradient.get_distance(start_index),
            gradient.get_distance(goal_index)
        );

        // Verify path length (should be 12 steps for this specific maze)
        assert_eq!(path.len(), 12, "Path should have 12 steps");

        // Verify first gate - should move to node 1 (moving right from start)
        assert_eq!(path[0].target, 1, "First move should be to node 1");
        assert_eq!(path[0].distance, 1.0, "First move should be straight (cost 1.0)");

        // Verify last gate - should arrive at goal
        assert_eq!(
            path.last().unwrap().target,
            goal_index as u32,
            "Path should end at goal"
        );
        assert_eq!(
            path.last().unwrap().distance,
            1.0,
            "Last move should be straight (cost 1.0)"
        );

        // Verify path stability - the exact sequence of moves should be deterministic
        // Each move should improve the distance to the goal
        let mut current_distance = gradient.get_distance(start_index);
        for gate in &path {
            let next_distance = gradient.get_distance(gate.target());
            assert!(
                next_distance < current_distance,
                "Each move should improve distance: current={}, next={}",
                current_distance,
                next_distance
            );
            current_distance = next_distance;
        }

        // Verify final distance is 0 (reached goal)
        assert_eq!(gradient.get_distance(goal_index), 0.0);
    }

    #[test]
    fn test_find() {
        use crate::distance::*;

        // Test the complete find() method that does everything
        let rect = Full2D::new(5, 5);
        let mut grad = Gradient::from_mesh(&rect);

        // find() should compute the gradient and return the path
        let path = grad.find(&rect, 0, 24, false);

        assert!(path.is_some(), "Should find a path from corner to corner");
        let path = path.unwrap();
        assert!(!path.is_empty(), "Path should not be empty");
        assert_eq!(
            path.last().unwrap().target,
            24,
            "Path should end at target (24)"
        );

        // Verify gradient was computed correctly
        assert_eq!(grad.get_distance(24), 0.0, "Target should have distance 0");
        assert_ne!(
            grad.get_distance(0),
            DISTANCE_MAX,
            "Start should not have max distance"
        );

        // Can call find() again with a different target - it will reset and recompute
        let path2 = grad.find(&rect, 0, 12, false);
        assert!(path2.is_some(), "Should find path to different target");
        assert_eq!(grad.get_distance(12), 0.0, "New target should have distance 0");
        assert_ne!(
            grad.get_distance(24),
            0.0,
            "Old target should no longer have distance 0"
        );
    }

    #[test]
    fn test_reset() {
        use crate::distance::*;

        let rect = Full2D::new(5, 5);
        let mut grad = Gradient::from_mesh(&rect);

        // Initially all nodes should have DISTANCE_MAX and point to themselves
        for i in 0..rect.len() {
            assert_eq!(grad.get_distance(i), DISTANCE_MAX);
            assert_eq!(grad.get_target(i), i);
        }

        // Set target and spread
        grad.set_distance(12, 0.0);
        grad.spread(&rect);

        // After spread, distances should be different
        assert_ne!(grad.get_distance(0), DISTANCE_MAX);
        assert_eq!(grad.get_distance(12), 0.0);

        // Reset should restore initial state
        grad.reset();

        // After reset, all nodes should have DISTANCE_MAX again
        for i in 0..rect.len() {
            assert_eq!(
                grad.get_distance(i),
                DISTANCE_MAX,
                "Node {} should have DISTANCE_MAX after reset",
                i
            );
            assert_eq!(
                grad.get_target(i),
                i,
                "Node {} should point to itself after reset",
                i
            );
        }

        // Can now compute a new gradient with different target
        grad.set_distance(24, 0.0); // Bottom-right instead of center
        grad.spread(&rect);
        assert_eq!(grad.get_distance(24), 0.0);
        assert_ne!(grad.get_distance(0), DISTANCE_MAX);
    }
}