rustsim-pathfinding 0.0.1

//! Route planning and step-along-route API for agents.
//!
//! Mirrors Julia Agents.jl `plan_route!`, `plan_best_route!`, and
//! `move_along_route!` functions. Agents store their planned routes in
//! a [`RoutePlanner`], then step along them one waypoint at a time.
//!
//! # Usage Pattern
//!
//! ```ignore
//! use rustsim_pathfinding::route::RoutePlanner;
//!
//! let mut planner = RoutePlanner::new();
//!
//! // Plan a route for agent 1
//! let waypoints = vec![(1.0, 2.0), (3.0, 4.0), (5.0, 6.0)];
//! planner.set_route(1, waypoints);
//!
//! // Move agent one step along the route at speed 1.5
//! let result = planner.move_along_route_2d(
//!     1,
//!     (0.5, 1.0),  // current position
//!     1.5,          // speed
//!     1.0,          // dt
//!     false,        // periodic
//!     100.0,        // extent_x (only used when periodic)
//!     100.0,        // extent_y
//! );
//! ```

use std::collections::{HashMap, VecDeque};

/// Per-agent route storage and movement controller.
///
/// Stores planned routes for agents (keyed by `AgentId = u64`) and
/// provides `move_along_route` functions that advance agents along
/// their planned paths at a given speed.
///
/// The generic parameter `W` is the waypoint type (e.g. `(f64, f64)`
/// or `(f64, f64, f64)`).
#[derive(Debug, Clone)]
pub struct RoutePlanner<W> {
    routes: HashMap<u64, VecDeque<W>>,
}

impl<W> Default for RoutePlanner<W> {
    fn default() -> Self {
        Self::new()
    }
}

impl<W> RoutePlanner<W> {
    /// Create a new empty route planner.
    pub fn new() -> Self {
        Self {
            routes: HashMap::new(),
        }
    }

    /// Set a planned route for an agent, replacing any existing route.
    pub fn set_route(&mut self, agent_id: u64, waypoints: Vec<W>) {
        self.routes.insert(agent_id, VecDeque::from(waypoints));
    }

    /// Check whether an agent has a non-empty route.
    pub fn has_route(&self, agent_id: u64) -> bool {
        self.routes
            .get(&agent_id)
            .map(|r| !r.is_empty())
            .unwrap_or(false)
    }

    /// Check whether an agent is stationary (has no route or route is empty).
    pub fn is_stationary(&self, agent_id: u64) -> bool {
        !self.has_route(agent_id)
    }

    /// Get the remaining waypoints for an agent (read-only).
    pub fn route(&self, agent_id: u64) -> Option<&VecDeque<W>> {
        self.routes.get(&agent_id)
    }

    /// Remove all route data for an agent.
    pub fn clear_route(&mut self, agent_id: u64) {
        self.routes.remove(&agent_id);
    }

    /// Remove all route data for all agents.
    pub fn clear_all(&mut self) {
        self.routes.clear();
    }

    /// Number of agents with stored routes.
    pub fn len(&self) -> usize {
        self.routes.len()
    }

    /// Whether no agents have stored routes.
    pub fn is_empty(&self) -> bool {
        self.routes.is_empty()
    }
}

/// 2D route planning result.
#[derive(Debug, Clone, Copy)]
pub struct MoveResult2D {
    /// New position after movement.
    pub position: (f64, f64),
    /// Whether the agent reached the end of its route.
    pub finished: bool,
}

impl RoutePlanner<(f64, f64)> {
    /// Move an agent one step along its 2D route at the given `speed` and timestep `dt`.
    ///
    /// The agent moves from `current_pos` toward the next waypoint. If it
    /// overshoots a waypoint, it continues toward the next one with the
    /// remaining distance budget (mirrors Agents.jl `move_along_route!`).
    ///
    /// Returns the new position and whether the route is now complete.
    /// If the agent has no route, returns `current_pos` unchanged.
    ///
    /// When `periodic` is true, movement wraps around `[0, extent_x) x [0, extent_y)`.
    #[allow(clippy::too_many_arguments)]
    pub fn move_along_route_2d(
        &mut self,
        agent_id: u64,
        current_pos: (f64, f64),
        speed: f64,
        mut dt: f64,
        periodic: bool,
        extent_x: f64,
        extent_y: f64,
    ) -> MoveResult2D {
        let route = match self.routes.get_mut(&agent_id) {
            Some(r) if !r.is_empty() => r,
            _ => {
                return MoveResult2D {
                    position: current_pos,
                    finished: true,
                };
            }
        };

        let mut from = current_pos;
        let mut next_pos = current_pos;

        while let Some(&waypoint) = route.front() {
            let dir = direction_2d(from, waypoint, periodic, extent_x, extent_y);
            let dist_to_wp = (dir.0 * dir.0 + dir.1 * dir.1).sqrt();

            if dist_to_wp < 1e-12 {
                // Already at this waypoint
                from = waypoint;
                route.pop_front();
                if route.is_empty() {
                    next_pos = waypoint;
                    break;
                }
                continue;
            }

            let move_dist = speed * dt;
            let unit_dir = (dir.0 / dist_to_wp, dir.1 / dist_to_wp);

            if move_dist >= dist_to_wp {
                // Overshoot: consume this waypoint and continue
                dt -= dist_to_wp / speed;
                from = waypoint;
                route.pop_front();
                if route.is_empty() {
                    next_pos = waypoint;
                    break;
                }
            } else {
                // Normal move
                next_pos = (
                    from.0 + unit_dir.0 * move_dist,
                    from.1 + unit_dir.1 * move_dist,
                );
                if periodic {
                    next_pos = normalize_pos_2d(next_pos, extent_x, extent_y);
                }
                break;
            }
        }

        let finished = route.is_empty();
        if finished {
            self.routes.remove(&agent_id);
        }

        MoveResult2D {
            position: next_pos,
            finished,
        }
    }

    /// Plan a route from a continuous A* path and store it.
    ///
    /// Convenience method: accepts the waypoints from
    /// [`ContinuousPath`](crate::continuous_astar::ContinuousPath).
    pub fn plan_from_path(&mut self, agent_id: u64, waypoints: Vec<(f64, f64)>) {
        self.set_route(agent_id, waypoints);
    }
}

/// 3D route planning result.
#[derive(Debug, Clone, Copy)]
pub struct MoveResult3D {
    /// New position after movement.
    pub position: (f64, f64, f64),
    /// Whether the agent reached the end of its route.
    pub finished: bool,
}

impl RoutePlanner<(f64, f64, f64)> {
    /// Move an agent one step along its 3D route at the given `speed` and timestep `dt`.
    ///
    /// Same semantics as [`move_along_route_2d`](RoutePlanner::move_along_route_2d)
    /// but in 3D.
    #[allow(clippy::too_many_arguments)]
    pub fn move_along_route_3d(
        &mut self,
        agent_id: u64,
        current_pos: (f64, f64, f64),
        speed: f64,
        mut dt: f64,
        periodic: bool,
        extent_x: f64,
        extent_y: f64,
        extent_z: f64,
    ) -> MoveResult3D {
        let route = match self.routes.get_mut(&agent_id) {
            Some(r) if !r.is_empty() => r,
            _ => {
                return MoveResult3D {
                    position: current_pos,
                    finished: true,
                };
            }
        };

        let mut from = current_pos;
        let mut next_pos = current_pos;

        while let Some(&waypoint) = route.front() {
            let dir = direction_3d(from, waypoint, periodic, extent_x, extent_y, extent_z);
            let dist_to_wp = (dir.0 * dir.0 + dir.1 * dir.1 + dir.2 * dir.2).sqrt();

            if dist_to_wp < 1e-12 {
                from = waypoint;
                route.pop_front();
                if route.is_empty() {
                    next_pos = waypoint;
                    break;
                }
                continue;
            }

            let move_dist = speed * dt;
            let unit_dir = (dir.0 / dist_to_wp, dir.1 / dist_to_wp, dir.2 / dist_to_wp);

            if move_dist >= dist_to_wp {
                dt -= dist_to_wp / speed;
                from = waypoint;
                route.pop_front();
                if route.is_empty() {
                    next_pos = waypoint;
                    break;
                }
            } else {
                next_pos = (
                    from.0 + unit_dir.0 * move_dist,
                    from.1 + unit_dir.1 * move_dist,
                    from.2 + unit_dir.2 * move_dist,
                );
                if periodic {
                    next_pos = normalize_pos_3d(next_pos, extent_x, extent_y, extent_z);
                }
                break;
            }
        }

        let finished = route.is_empty();
        if finished {
            self.routes.remove(&agent_id);
        }

        MoveResult3D {
            position: next_pos,
            finished,
        }
    }

    /// Plan a route from a 3D continuous A* path.
    pub fn plan_from_path(&mut self, agent_id: u64, waypoints: Vec<(f64, f64, f64)>) {
        self.set_route(agent_id, waypoints);
    }
}

/// Grid (integer) route planner for `(usize, usize)` waypoints.
impl RoutePlanner<(usize, usize)> {
    /// Pop and return the next grid waypoint for an agent.
    ///
    /// This is the grid equivalent of `move_along_route!` --
    /// each call advances one step (one cell) along the planned path.
    /// Returns `None` if the agent has no remaining waypoints.
    pub fn next_waypoint(&mut self, agent_id: u64) -> Option<(usize, usize)> {
        let route = self.routes.get_mut(&agent_id)?;
        let wp = route.pop_front();
        if route.is_empty() {
            self.routes.remove(&agent_id);
        }
        wp
    }
}

// ---------------------------------------------------------------------------
// Helper functions
// ---------------------------------------------------------------------------

fn direction_2d(
    from: (f64, f64),
    to: (f64, f64),
    periodic: bool,
    extent_x: f64,
    extent_y: f64,
) -> (f64, f64) {
    let mut dx = to.0 - from.0;
    let mut dy = to.1 - from.1;
    if periodic {
        if dx > extent_x * 0.5 {
            dx -= extent_x;
        } else if dx < -extent_x * 0.5 {
            dx += extent_x;
        }
        if dy > extent_y * 0.5 {
            dy -= extent_y;
        } else if dy < -extent_y * 0.5 {
            dy += extent_y;
        }
    }
    (dx, dy)
}

fn direction_3d(
    from: (f64, f64, f64),
    to: (f64, f64, f64),
    periodic: bool,
    extent_x: f64,
    extent_y: f64,
    extent_z: f64,
) -> (f64, f64, f64) {
    let mut dx = to.0 - from.0;
    let mut dy = to.1 - from.1;
    let mut dz = to.2 - from.2;
    if periodic {
        if dx > extent_x * 0.5 {
            dx -= extent_x;
        } else if dx < -extent_x * 0.5 {
            dx += extent_x;
        }
        if dy > extent_y * 0.5 {
            dy -= extent_y;
        } else if dy < -extent_y * 0.5 {
            dy += extent_y;
        }
        if dz > extent_z * 0.5 {
            dz -= extent_z;
        } else if dz < -extent_z * 0.5 {
            dz += extent_z;
        }
    }
    (dx, dy, dz)
}

fn normalize_pos_2d(pos: (f64, f64), extent_x: f64, extent_y: f64) -> (f64, f64) {
    (
        ((pos.0 % extent_x) + extent_x) % extent_x,
        ((pos.1 % extent_y) + extent_y) % extent_y,
    )
}

fn normalize_pos_3d(
    pos: (f64, f64, f64),
    extent_x: f64,
    extent_y: f64,
    extent_z: f64,
) -> (f64, f64, f64) {
    (
        ((pos.0 % extent_x) + extent_x) % extent_x,
        ((pos.1 % extent_y) + extent_y) % extent_y,
        ((pos.2 % extent_z) + extent_z) % extent_z,
    )
}

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

    #[test]
    fn route_planner_basic() {
        let mut planner = RoutePlanner::<(f64, f64)>::new();
        assert!(planner.is_stationary(1));

        planner.set_route(1, vec![(5.0, 5.0), (10.0, 10.0)]);
        assert!(planner.has_route(1));
        assert!(!planner.is_stationary(1));

        planner.clear_route(1);
        assert!(planner.is_stationary(1));
    }

    #[test]
    fn move_along_route_exact_waypoint() {
        let mut planner = RoutePlanner::new();
        planner.set_route(1, vec![(10.0, 0.0)]);

        // Speed=10, dt=1 -> move exactly 10 units
        let result = planner.move_along_route_2d(1, (0.0, 0.0), 10.0, 1.0, false, 100.0, 100.0);
        assert!((result.position.0 - 10.0).abs() < 1e-10);
        assert!((result.position.1 - 0.0).abs() < 1e-10);
        assert!(result.finished);
    }

    #[test]
    fn move_along_route_partial() {
        let mut planner = RoutePlanner::new();
        planner.set_route(1, vec![(10.0, 0.0)]);

        // Speed=5, dt=1 -> move 5 units toward (10,0) from (0,0) = (5,0)
        let result = planner.move_along_route_2d(1, (0.0, 0.0), 5.0, 1.0, false, 100.0, 100.0);
        assert!((result.position.0 - 5.0).abs() < 1e-10);
        assert!(!result.finished);
    }

    #[test]
    fn move_along_route_overshoot_chain() {
        let mut planner = RoutePlanner::new();
        planner.set_route(1, vec![(3.0, 0.0), (6.0, 0.0), (10.0, 0.0)]);

        // Speed=5, dt=1 -> pass (3,0), continue 2 more toward (6,0) => (5,0)
        let result = planner.move_along_route_2d(1, (0.0, 0.0), 5.0, 1.0, false, 100.0, 100.0);
        assert!((result.position.0 - 5.0).abs() < 1e-10);
        assert!(!result.finished);
    }

    #[test]
    fn move_along_route_no_route() {
        let mut planner = RoutePlanner::<(f64, f64)>::new();
        let result = planner.move_along_route_2d(1, (5.0, 5.0), 10.0, 1.0, false, 100.0, 100.0);
        assert!((result.position.0 - 5.0).abs() < 1e-10);
        assert!(result.finished);
    }

    #[test]
    fn move_along_route_3d_basic() {
        let mut planner = RoutePlanner::new();
        planner.set_route(1, vec![(10.0, 0.0, 0.0)]);

        let result =
            planner.move_along_route_3d(1, (0.0, 0.0, 0.0), 10.0, 1.0, false, 100.0, 100.0, 100.0);
        assert!((result.position.0 - 10.0).abs() < 1e-10);
        assert!(result.finished);
    }

    #[test]
    fn grid_route_next_waypoint() {
        let mut planner = RoutePlanner::new();
        planner.set_route(1, vec![(1, 0), (2, 0), (3, 0)]);

        assert_eq!(planner.next_waypoint(1), Some((1, 0)));
        assert_eq!(planner.next_waypoint(1), Some((2, 0)));
        assert_eq!(planner.next_waypoint(1), Some((3, 0)));
        assert_eq!(planner.next_waypoint(1), None);
        assert!(planner.is_stationary(1));
    }

    #[test]
    fn move_along_route_2d_periodic() {
        let mut planner = RoutePlanner::new();
        // On a 10x10 periodic space, (9,5) to (1,5) should go through the wrap
        planner.set_route(1, vec![(1.0, 5.0)]);

        let result = planner.move_along_route_2d(1, (9.0, 5.0), 1.0, 1.0, true, 10.0, 10.0);
        // Direction from 9 to 1 wrapping = +2, distance=2, speed=1 => move 1 unit => x=0.0
        assert!((result.position.0).abs() < 1e-10 || (result.position.0 - 10.0).abs() < 1e-10);
        assert!(!result.finished);
    }
}