rustsim-pathfinding 0.0.1

//! Cost metrics for A* pathfinding.
//!
//! Mirrors Julia Agents.jl `Pathfinding.CostMetric` system. Cost metrics
//! estimate the travel cost between two grid cells for use as the A* heuristic
//! and/or edge cost.
//!
//! # Built-in metrics
//!
//! | Metric | Description |
//! |--------|-------------|
//! | [`DirectDistance`] | Shortest diagonal/orthogonal path cost (default) |
//! | [`MaxDistance`] | Maximum absolute coordinate difference (Chebyshev) |
//! | [`Manhattan`] | Sum of absolute coordinate differences (L1) |
//! | [`PenaltyMap`] | Base metric + per-cell penalty difference (height maps) |

use thiserror::Error;

/// Trait for cost metrics used in A* pathfinding on 2D grids.
///
/// Implementing this trait allows custom cost metrics to be plugged into
/// the grid A* algorithm.
pub trait CostMetric: std::fmt::Debug {
    /// Estimate the cost of traveling from `from` to `to`.
    ///
    /// For heuristic use, this should be admissible (never overestimate).
    fn delta_cost(
        &self,
        from: (usize, usize),
        to: (usize, usize),
        periodic: bool,
        width: usize,
        height: usize,
        diagonal: bool,
    ) -> f64;
}

/// Direct-distance metric -- the default.
///
/// Estimates cost as the shortest diagonal+orthogonal path between two cells.
/// When diagonal movement is enabled, uses `cost_diagonal * min_delta + cost_cardinal * (max_delta - min_delta)`.
/// Otherwise, uses `cost_cardinal * manhattan_distance`.
///
/// `cost_cardinal` defaults to `1.0`, `cost_diagonal` defaults to `?2`.
///
/// Mirrors Agents.jl `DirectDistance`.
#[derive(Debug, Clone)]
pub struct DirectDistance {
    /// Cost of an orthogonal step (horizontal/vertical).
    pub cost_cardinal: f64,
    /// Cost of a diagonal step.
    pub cost_diagonal: f64,
}

impl DirectDistance {
    /// Create with default costs (cardinal = 1.0, diagonal = ?2).
    pub fn new() -> Self {
        Self {
            cost_cardinal: 1.0,
            cost_diagonal: std::f64::consts::SQRT_2,
        }
    }

    /// Create with custom step costs.
    pub fn with_costs(cost_cardinal: f64, cost_diagonal: f64) -> Self {
        Self {
            cost_cardinal,
            cost_diagonal,
        }
    }
}

impl Default for DirectDistance {
    fn default() -> Self {
        Self::new()
    }
}

impl CostMetric for DirectDistance {
    fn delta_cost(
        &self,
        from: (usize, usize),
        to: (usize, usize),
        periodic: bool,
        width: usize,
        height: usize,
        diagonal: bool,
    ) -> f64 {
        let (dx, dy) = delta_with_periodic(from, to, periodic, width, height);
        if diagonal {
            let min_d = dx.min(dy) as f64;
            let max_d = dx.max(dy) as f64;
            self.cost_diagonal * min_d + self.cost_cardinal * (max_d - min_d)
        } else {
            self.cost_cardinal * (dx + dy) as f64
        }
    }
}

/// Maximum-coordinate-difference metric (Chebyshev distance).
///
/// Cost = `max(|dx|, |dy|)`.
///
/// Mirrors Agents.jl `MaxDistance`.
#[derive(Debug, Clone, Copy)]
pub struct MaxDistance;

impl CostMetric for MaxDistance {
    fn delta_cost(
        &self,
        from: (usize, usize),
        to: (usize, usize),
        periodic: bool,
        width: usize,
        height: usize,
        _diagonal: bool,
    ) -> f64 {
        let (dx, dy) = delta_with_periodic(from, to, periodic, width, height);
        dx.max(dy) as f64
    }
}

/// Manhattan distance metric (L1 / taxicab distance).
///
/// Cost = `|dx| + |dy|`.
#[derive(Debug, Clone, Copy)]
pub struct Manhattan;

impl CostMetric for Manhattan {
    fn delta_cost(
        &self,
        from: (usize, usize),
        to: (usize, usize),
        periodic: bool,
        width: usize,
        height: usize,
        _diagonal: bool,
    ) -> f64 {
        let (dx, dy) = delta_with_periodic(from, to, periodic, width, height);
        (dx + dy) as f64
    }
}

/// Errors returned by penalty-map validation.
#[derive(Debug, Clone, Copy, PartialEq, Eq, Error)]
pub enum PenaltyMapError {
    #[error("penalty-map dimensions must be positive")]
    InvalidDimensions,
    #[error("penalty map size mismatch: expected {expected}, got {actual}")]
    SizeMismatch { expected: usize, actual: usize },
}

/// Penalty-map metric -- base distance plus per-cell penalty difference.
///
/// Cost = `base_metric.delta_cost(from, to) + |penalty[from] - penalty[to]|`.
///
/// The penalty map is a 2D array (row-major, `height x width`) of integer
/// penalties. This is useful for modeling terrain height, road quality, etc.
///
/// Mirrors Agents.jl `PenaltyMap`.
///
/// # Example
///
/// ```
/// use rustsim_pathfinding::metrics::{PenaltyMap, DirectDistance};
///
/// // 5x5 grid with a hill in the center
/// let mut penalties = vec![0i32; 25];
/// penalties[2 * 5 + 2] = 100; // cell (2,2) is expensive
///
/// let metric = PenaltyMap::new(penalties, 5, 5, DirectDistance::new()).unwrap();
/// ```
#[derive(Debug)]
pub struct PenaltyMap {
    /// Flat penalty array in row-major order (`penalties[y * width + x]`).
    penalties: Vec<i32>,
    map_width: usize,
    #[allow(dead_code)]
    map_height: usize,
    /// Underlying distance metric.
    base_metric: Box<dyn CostMetric>,
}

// Manual Debug for the boxed trait object is already handled by #[derive(Debug)] on the struct,
// since Box<dyn CostMetric> requires CostMetric: Debug.

impl PenaltyMap {
    /// Create a penalty map metric.
    pub fn new(
        penalties: Vec<i32>,
        width: usize,
        height: usize,
        base: impl CostMetric + 'static,
    ) -> Result<Self, PenaltyMapError> {
        if width == 0 || height == 0 {
            return Err(PenaltyMapError::InvalidDimensions);
        }
        let expected = width * height;
        if penalties.len() != expected {
            return Err(PenaltyMapError::SizeMismatch {
                expected,
                actual: penalties.len(),
            });
        }
        Ok(Self {
            penalties,
            map_width: width,
            map_height: height,
            base_metric: Box::new(base),
        })
    }

    /// Read-only access to the penalty values.
    pub fn penalties(&self) -> &[i32] {
        &self.penalties
    }

    /// Mutable access to the penalty values (e.g. for dynamic terrain).
    pub fn penalties_mut(&mut self) -> &mut [i32] {
        &mut self.penalties
    }

    /// Get the penalty at a grid cell.
    pub fn penalty_at(&self, x: usize, y: usize) -> i32 {
        self.penalties[y * self.map_width + x]
    }
}

impl CostMetric for PenaltyMap {
    fn delta_cost(
        &self,
        from: (usize, usize),
        to: (usize, usize),
        periodic: bool,
        width: usize,
        height: usize,
        diagonal: bool,
    ) -> f64 {
        let base = self
            .base_metric
            .delta_cost(from, to, periodic, width, height, diagonal);
        let pen_from = self.penalties[from.1 * self.map_width + from.0];
        let pen_to = self.penalties[to.1 * self.map_width + to.0];
        base + (pen_to - pen_from).unsigned_abs() as f64
    }
}

/// Compute the minimum absolute delta between two grid positions,
/// accounting for periodic wrapping.
fn delta_with_periodic(
    from: (usize, usize),
    to: (usize, usize),
    periodic: bool,
    width: usize,
    height: usize,
) -> (usize, usize) {
    if periodic {
        let dx_raw = (from.0 as isize - to.0 as isize).unsigned_abs();
        let dy_raw = (from.1 as isize - to.1 as isize).unsigned_abs();
        let dx = dx_raw.min(width - dx_raw);
        let dy = dy_raw.min(height - dy_raw);
        (dx, dy)
    } else {
        let dx = (from.0 as isize - to.0 as isize).unsigned_abs();
        let dy = (from.1 as isize - to.1 as isize).unsigned_abs();
        (dx, dy)
    }
}

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

    #[test]
    fn direct_distance_cardinal() {
        let m = DirectDistance::new();
        let cost = m.delta_cost((0, 0), (3, 0), false, 10, 10, false);
        assert!((cost - 3.0).abs() < 1e-10);
    }

    #[test]
    fn direct_distance_diagonal() {
        let m = DirectDistance::new();
        // (0,0) -> (2,2): 2 diagonal steps = 2*?2
        let cost = m.delta_cost((0, 0), (2, 2), false, 10, 10, true);
        assert!((cost - 2.0 * std::f64::consts::SQRT_2).abs() < 1e-10);
    }

    #[test]
    fn direct_distance_mixed() {
        let m = DirectDistance::new();
        // (0,0) -> (3,1): 1 diagonal + 2 cardinal = ?2 + 2
        let cost = m.delta_cost((0, 0), (3, 1), false, 10, 10, true);
        assert!((cost - (std::f64::consts::SQRT_2 + 2.0)).abs() < 1e-10);
    }

    #[test]
    fn max_distance_basic() {
        let cost = MaxDistance.delta_cost((0, 0), (3, 5), false, 10, 10, true);
        assert!((cost - 5.0).abs() < 1e-10);
    }

    #[test]
    fn manhattan_basic() {
        let cost = Manhattan.delta_cost((0, 0), (3, 5), false, 10, 10, false);
        assert!((cost - 8.0).abs() < 1e-10);
    }

    #[test]
    fn penalty_map_basic() {
        let mut pens = vec![0i32; 25];
        pens[2 * 5 + 2] = 100; // (2,2) has penalty 100
        let m = PenaltyMap::new(pens, 5, 5, DirectDistance::new()).unwrap();

        // From (0,0) penalty=0 to (2,2) penalty=100: base + 100
        let cost = m.delta_cost((0, 0), (2, 2), false, 5, 5, true);
        let base = DirectDistance::new().delta_cost((0, 0), (2, 2), false, 5, 5, true);
        assert!((cost - (base + 100.0)).abs() < 1e-10);
    }

    #[test]
    fn periodic_distance() {
        let m = DirectDistance::new();
        // On a 10x10 periodic grid, (0,0) to (9,0) should be 1 step, not 9
        let cost = m.delta_cost((0, 0), (9, 0), true, 10, 10, false);
        assert!((cost - 1.0).abs() < 1e-10);
    }

    #[test]
    fn penalty_map_invalid_dimensions() {
        let pens = vec![0i32; 25];
        let result = PenaltyMap::new(pens, 0, 5, DirectDistance::new());
        assert!(matches!(result, Err(PenaltyMapError::InvalidDimensions)));
    }

    #[test]
    fn penalty_map_size_mismatch() {
        let pens = vec![0i32; 24]; // Wrong size
        let result = PenaltyMap::new(pens, 5, 5, DirectDistance::new());
        assert!(matches!(
            result,
            Err(PenaltyMapError::SizeMismatch {
                expected: 25,
                actual: 24
            })
        ));
    }
}