rustsim-crowd 0.0.1

//! Collision-Free Speed Model (Tordeux, Chraibi & Seyfried 2016).
//!
//! A first-order velocity-based model. Each pedestrian *i* chooses:
//!
//! 1. A direction `e_i` that blends the desired direction toward its goal
//!    with a weighted sum of repulsive directions from every neighbour.
//! 2. A scalar speed `v_i = min(v0_i, max(0, (s_i - l_i) / T))`, where
//!    `s_i` is the free headroom along `e_i` toward the nearest neighbour
//!    and `T` is the safety time gap.
//!
//! The integration is explicit Euler on position only (the model is
//! kinematic, not dynamic).
//!
//! # References
//!
//! - Tordeux, A., Chraibi, M., & Seyfried, A. (2016). "Collision-free speed
//!   model for pedestrian dynamics". *Traffic and Granular Flow '15*,
//!   225–232.

use crate::broadphase::{NeighborGrid, Scratch};
use crate::common::{
    add, closest_point_on_segment, dot, norm, normalize, scale, sub, Pedestrian, PedestrianModel,
    Vec2, WallSegment,
};

/// Parameters for the Collision-Free Speed model.
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct Params {
    /// Safety time gap (s). Larger values produce more conservative walkers.
    pub time_gap: f64,
    /// Direction blending weight on the desired (goal) direction.
    pub alpha: f64,
    /// Interaction range for neighbour repulsion (m).
    pub interaction_range: f64,
    /// Interaction strength for neighbour repulsion (dimensionless).
    pub interaction_strength: f64,
    /// Wall interaction range (m).
    pub wall_range: f64,
    /// Wall interaction strength (dimensionless).
    pub wall_strength: f64,
    /// Arrival radius (m). See
    /// [`social_force::Params::arrival_radius`](crate::social_force::Params::arrival_radius).
    /// Default: 0.3 m.
    pub arrival_radius: f64,
}

impl Default for Params {
    fn default() -> Self {
        Self {
            time_gap: 1.06,
            alpha: 3.0,
            interaction_range: 0.1,
            interaction_strength: 5.0,
            wall_range: 0.08,
            wall_strength: 5.0,
            arrival_radius: 0.3,
        }
    }
}

impl Params {
    /// Validate this parameter set against `dt`.
    ///
    /// CFS is a first-order kinematic model: speeds are directly
    /// clamped from the collision-free headroom, so there is no
    /// explicit-Euler CFL condition to check. Validation therefore
    /// focuses on strictly-positive ranges and a finite positive `dt`.
    pub fn validate(&self, dt: f64) -> Result<(), crate::error::CrowdError> {
        use crate::error::{require_dt, require_nonneg, require_positive};
        const M: &str = "CollisionFreeSpeed";
        require_dt(M, dt)?;
        require_positive(M, "time_gap", self.time_gap)?;
        require_positive(M, "alpha", self.alpha)?;
        require_positive(M, "interaction_range", self.interaction_range)?;
        require_nonneg(M, "interaction_strength", self.interaction_strength)?;
        require_positive(M, "wall_range", self.wall_range)?;
        require_nonneg(M, "wall_strength", self.wall_strength)?;
        require_nonneg(M, "arrival_radius", self.arrival_radius)?;
        Ok(())
    }
}

/// Unit marker type implementing [`PedestrianModel`] for the Collision-Free
/// Speed model.
#[derive(Debug, Clone, Copy, Default)]
pub struct CollisionFreeSpeed;

impl PedestrianModel for CollisionFreeSpeed {
    type Params = Params;

    fn name(&self) -> &'static str {
        "Collision-Free Speed"
    }

    fn step(&self, peds: &mut [Pedestrian], walls: &[WallSegment], params: &Params, dt: f64) {
        #[allow(deprecated)]
        step(peds, walls, params, dt);
    }
}

/// Free-function step for callers that do not need trait dispatch.
///
/// **Deprecated.** O(n²) reference path with per-tick heap allocation.
/// Use [`step_scratch`] (zero-alloc) or [`step_with_grid`] (broadphase)
/// in production. Retained for parity tests and back-compat.
#[deprecated(
    since = "0.0.3",
    note = "O(n²) reference path with per-tick heap allocation; use \
            `step_scratch` (zero-alloc) or `step_with_grid` (broadphase) \
            instead. See docs/rustsim-crowd.md P1-7."
)]
#[allow(clippy::needless_range_loop)]
pub fn step(peds: &mut [Pedestrian], walls: &[WallSegment], params: &Params, dt: f64) {
    let n = peds.len();
    let mut new_vel = vec![[0.0f64; 2]; n];

    for i in 0..n {
        let p = &peds[i];
        let e = chosen_direction(i, peds, walls, params);
        let s = free_headroom(i, peds, &e);
        let effective_radius = p.radius;
        let speed_cap = ((s - effective_radius) / params.time_gap).max(0.0);
        let v = p
            .effective_desired_speed(params.arrival_radius)
            .min(speed_cap);
        new_vel[i] = scale(e, v);
    }

    for (p, v) in peds.iter_mut().zip(new_vel.iter()) {
        p.vel = *v;
        p.pos = add(p.pos, scale(p.vel, dt));
    }
}

/// Recommended neighbour cutoff radius for grid queries (metres).
///
/// Interaction weights decay as `exp(-clearance / interaction_range)`,
/// so at a clearance of `8 * interaction_range` the contribution is
/// below `5e-4 * interaction_strength`. The returned cutoff adds a
/// 2 m buffer to also cover the forward headroom scan distance
/// (`desired_speed * time_gap ≈ 1.4 m` at free-flow defaults).
#[inline]
pub fn neighbor_cutoff(params: &Params) -> f64 {
    8.0 * params.interaction_range + 2.0
}

/// Grid-accelerated step variant. Semantically equivalent to [`step`]
/// up to numerical noise for pairs inside `neighbor_cutoff(params)`.
///
/// The caller owns `grid`; rebuild it once per tick.
#[allow(clippy::needless_range_loop)]
pub fn step_with_grid(
    peds: &mut [Pedestrian],
    walls: &[WallSegment],
    params: &Params,
    dt: f64,
    grid: &NeighborGrid,
) {
    let n = peds.len();
    let cutoff = neighbor_cutoff(params);
    let mut new_vel = vec![[0.0f64; 2]; n];

    for i in 0..n {
        let p = &peds[i];
        let e = chosen_direction_grid(i, peds, walls, params, grid, cutoff);
        let s = free_headroom_grid(i, peds, &e, grid, cutoff);
        let effective_radius = p.radius;
        let speed_cap = ((s - effective_radius) / params.time_gap).max(0.0);
        let v = p
            .effective_desired_speed(params.arrival_radius)
            .min(speed_cap);
        new_vel[i] = scale(e, v);
    }

    for (p, v) in peds.iter_mut().zip(new_vel.iter()) {
        p.vel = *v;
        p.pos = add(p.pos, scale(p.vel, dt));
    }
}

/// Zero-allocation step variant. See
/// [`social_force::step_scratch`](crate::social_force::step_scratch)
/// for the motivation.
#[allow(clippy::needless_range_loop)]
pub fn step_scratch(
    peds: &mut [Pedestrian],
    walls: &[WallSegment],
    params: &Params,
    dt: f64,
    scratch: &mut Scratch,
) {
    let n = peds.len();
    let cutoff = neighbor_cutoff(params);
    scratch.prepare(peds);
    let (new_vel, grid) = scratch.split();

    for i in 0..n {
        let p = &peds[i];
        let e = chosen_direction_grid(i, peds, walls, params, grid, cutoff);
        let s = free_headroom_grid(i, peds, &e, grid, cutoff);
        let speed_cap = ((s - p.radius) / params.time_gap).max(0.0);
        let v = p
            .effective_desired_speed(params.arrival_radius)
            .min(speed_cap);
        new_vel[i] = scale(e, v);
    }

    for (p, v) in peds.iter_mut().zip(new_vel.iter()) {
        p.vel = *v;
        p.pos = add(p.pos, scale(p.vel, dt));
    }
}

/// SIMD-vectorised drop-in replacement for [`step_scratch`].
///
/// Lifts the neighbour scans used by CFS direction choice and forward
/// headroom into 4-wide SIMD chunks via [`crate::simd::cfs_direction_x4`]
/// and [`crate::simd::cfs_headroom_x4`]. Wall interaction, desired-speed
/// tapering, and position integration stay scalar, matching the scalar
/// zero-allocation path.
///
/// Numerical contract: lane summation can reorder the direction accumulator,
/// so this path is tolerance-equivalent to [`step_scratch`], not bit-exact.
/// `tests/simd_tolerance.rs` pins the current envelope.
#[cfg(feature = "simd")]
#[allow(clippy::needless_range_loop)]
pub fn step_scratch_simd(
    peds: &mut [Pedestrian],
    walls: &[WallSegment],
    params: &Params,
    dt: f64,
    scratch: &mut Scratch,
) {
    let n = peds.len();
    let cutoff = neighbor_cutoff(params);
    scratch.prepare(peds);
    let (new_vel, grid) = scratch.split();

    for i in 0..n {
        let p = &peds[i];
        let e = chosen_direction_grid_simd(i, peds, walls, params, grid, cutoff);
        let s = free_headroom_grid_simd(i, peds, &e, grid, cutoff);
        let speed_cap = ((s - p.radius) / params.time_gap).max(0.0);
        let v = p
            .effective_desired_speed(params.arrival_radius)
            .min(speed_cap);
        new_vel[i] = scale(e, v);
    }

    for (p, v) in peds.iter_mut().zip(new_vel.iter()) {
        p.vel = *v;
        p.pos = add(p.pos, scale(p.vel, dt));
    }
}

/// Rayon-parallel drop-in replacement for [`step_scratch`].
///
/// See [`social_force::step_scratch_par`](crate::social_force::step_scratch_par)
/// for the contract. Enable the `rayon` feature to use this entry
/// point.
#[cfg(feature = "rayon")]
#[allow(clippy::needless_range_loop)]
pub fn step_scratch_par(
    peds: &mut [Pedestrian],
    walls: &[WallSegment],
    params: &Params,
    dt: f64,
    scratch: &mut Scratch,
) {
    use rayon::prelude::*;

    let cutoff = neighbor_cutoff(params);
    scratch.prepare(peds);
    let (new_vel, grid) = scratch.split();
    let peds_ro: &[Pedestrian] = peds;

    new_vel.par_iter_mut().enumerate().for_each(|(i, v_slot)| {
        let p = &peds_ro[i];
        let e = chosen_direction_grid(i, peds_ro, walls, params, grid, cutoff);
        let s = free_headroom_grid(i, peds_ro, &e, grid, cutoff);
        let speed_cap = ((s - p.radius) / params.time_gap).max(0.0);
        let v = p
            .effective_desired_speed(params.arrival_radius)
            .min(speed_cap);
        *v_slot = scale(e, v);
    });

    for (p, v) in peds.iter_mut().zip(new_vel.iter()) {
        p.vel = *v;
        p.pos = add(p.pos, scale(p.vel, dt));
    }
}

/// Grid-backed version of [`chosen_direction`].
fn chosen_direction_grid(
    i: usize,
    peds: &[Pedestrian],
    walls: &[WallSegment],
    params: &Params,
    grid: &NeighborGrid,
    cutoff: f64,
) -> Vec2 {
    let p = &peds[i];
    let e_dest = p.desired_direction();
    let mut acc = scale(e_dest, params.alpha);

    grid.for_each_neighbor(i, cutoff, peds, |_j, q| {
        let diff = sub(p.pos, q.pos);
        let d = norm(diff);
        if d < 1e-9 {
            return;
        }
        let e_ij = scale(diff, 1.0 / d);
        let clearance = d - (p.radius + q.radius);
        let weight =
            params.interaction_strength * (-clearance.max(0.0) / params.interaction_range).exp();
        acc = add(acc, scale(e_ij, weight));
    });

    for w in walls {
        let closest = closest_point_on_segment(p.pos, w.a, w.b);
        let diff = sub(p.pos, closest);
        let d = norm(diff);
        if d < 1e-9 {
            continue;
        }
        let e_iw = scale(diff, 1.0 / d);
        let clearance = d - p.radius;
        let weight = params.wall_strength * (-clearance.max(0.0) / params.wall_range).exp();
        acc = add(acc, scale(e_iw, weight));
    }

    normalize(acc)
}

#[cfg(feature = "simd")]
fn chosen_direction_grid_simd(
    i: usize,
    peds: &[Pedestrian],
    walls: &[WallSegment],
    params: &Params,
    grid: &NeighborGrid,
    cutoff: f64,
) -> Vec2 {
    let p = &peds[i];
    let e_dest = p.desired_direction();
    let mut acc = scale(e_dest, params.alpha);

    let mut idxs: [Option<usize>; 4] = [None, None, None, None];
    let mut filled: usize = 0;
    grid.for_each_neighbor(i, cutoff, peds, |j, _q| {
        idxs[filled] = Some(j);
        filled += 1;
        if filled == 4 {
            let buf: [Option<&Pedestrian>; 4] = [
                Some(&peds[idxs[0].unwrap()]),
                Some(&peds[idxs[1].unwrap()]),
                Some(&peds[idxs[2].unwrap()]),
                Some(&peds[idxs[3].unwrap()]),
            ];
            acc = add(acc, crate::simd::cfs_direction_x4(p, buf, params));
            idxs = [None, None, None, None];
            filled = 0;
        }
    });
    if filled > 0 {
        let buf: [Option<&Pedestrian>; 4] = [
            idxs[0].map(|k| &peds[k]),
            idxs[1].map(|k| &peds[k]),
            idxs[2].map(|k| &peds[k]),
            idxs[3].map(|k| &peds[k]),
        ];
        acc = add(acc, crate::simd::cfs_direction_x4(p, buf, params));
    }

    for w in walls {
        let closest = closest_point_on_segment(p.pos, w.a, w.b);
        let diff = sub(p.pos, closest);
        let d = norm(diff);
        if d < 1e-9 {
            continue;
        }
        let e_iw = scale(diff, 1.0 / d);
        let clearance = d - p.radius;
        let weight = params.wall_strength * (-clearance.max(0.0) / params.wall_range).exp();
        acc = add(acc, scale(e_iw, weight));
    }

    normalize(acc)
}

/// Grid-backed version of [`free_headroom`].
fn free_headroom_grid(
    i: usize,
    peds: &[Pedestrian],
    e: &Vec2,
    grid: &NeighborGrid,
    cutoff: f64,
) -> f64 {
    let p = &peds[i];
    let mut s = f64::INFINITY;
    grid.for_each_neighbor(i, cutoff, peds, |_j, q| {
        let rel = sub(q.pos, p.pos);
        let forward = dot(rel, *e);
        if forward <= 0.0 {
            return;
        }
        let proj = scale(*e, forward);
        let lat = norm(sub(rel, proj));
        if lat > p.radius + q.radius {
            return;
        }
        let d = norm(rel);
        if d < s {
            s = d;
        }
    });
    s
}

#[cfg(feature = "simd")]
fn free_headroom_grid_simd(
    i: usize,
    peds: &[Pedestrian],
    e: &Vec2,
    grid: &NeighborGrid,
    cutoff: f64,
) -> f64 {
    let p = &peds[i];
    let mut s = f64::INFINITY;
    let mut idxs: [Option<usize>; 4] = [None, None, None, None];
    let mut filled: usize = 0;
    grid.for_each_neighbor(i, cutoff, peds, |j, _q| {
        idxs[filled] = Some(j);
        filled += 1;
        if filled == 4 {
            let buf: [Option<&Pedestrian>; 4] = [
                Some(&peds[idxs[0].unwrap()]),
                Some(&peds[idxs[1].unwrap()]),
                Some(&peds[idxs[2].unwrap()]),
                Some(&peds[idxs[3].unwrap()]),
            ];
            s = s.min(crate::simd::cfs_headroom_x4(p, *e, buf));
            idxs = [None, None, None, None];
            filled = 0;
        }
    });
    if filled > 0 {
        let buf: [Option<&Pedestrian>; 4] = [
            idxs[0].map(|k| &peds[k]),
            idxs[1].map(|k| &peds[k]),
            idxs[2].map(|k| &peds[k]),
            idxs[3].map(|k| &peds[k]),
        ];
        s = s.min(crate::simd::cfs_headroom_x4(p, *e, buf));
    }
    s
}

/// Compute the blended direction for pedestrian `i`.
#[inline]
pub fn chosen_direction(
    i: usize,
    peds: &[Pedestrian],
    walls: &[WallSegment],
    params: &Params,
) -> Vec2 {
    let p = &peds[i];
    let e_dest = p.desired_direction();
    let mut acc = scale(e_dest, params.alpha);

    for (j, q) in peds.iter().enumerate() {
        if i == j {
            continue;
        }
        let diff = sub(p.pos, q.pos);
        let d = norm(diff);
        if d < 1e-9 {
            continue;
        }
        let e_ij = scale(diff, 1.0 / d);
        let clearance = d - (p.radius + q.radius);
        let weight =
            params.interaction_strength * (-clearance.max(0.0) / params.interaction_range).exp();
        acc = add(acc, scale(e_ij, weight));
    }

    for w in walls {
        let closest = closest_point_on_segment(p.pos, w.a, w.b);
        let diff = sub(p.pos, closest);
        let d = norm(diff);
        if d < 1e-9 {
            continue;
        }
        let e_iw = scale(diff, 1.0 / d);
        let clearance = d - p.radius;
        let weight = params.wall_strength * (-clearance.max(0.0) / params.wall_range).exp();
        acc = add(acc, scale(e_iw, weight));
    }

    normalize(acc)
}

/// Minimum free distance along direction `e` to the closest neighbour in front
/// of pedestrian `i`. Returns `f64::INFINITY` if no neighbour is in front.
#[inline]
pub fn free_headroom(i: usize, peds: &[Pedestrian], e: &Vec2) -> f64 {
    let p = &peds[i];
    let mut s = f64::INFINITY;
    for (j, q) in peds.iter().enumerate() {
        if i == j {
            continue;
        }
        let rel = sub(q.pos, p.pos);
        let forward = dot(rel, *e);
        if forward <= 0.0 {
            continue;
        }
        // Lateral displacement at the projected point.
        let proj = scale(*e, forward);
        let lat = norm(sub(rel, proj));
        if lat > p.radius + q.radius {
            continue;
        }
        let d = norm(rel);
        if d < s {
            s = d;
        }
    }
    s
}

#[cfg(test)]
#[allow(deprecated)] // intentional: pins grid/scratch equivalence vs the deprecated O(n²) `step`.
mod tests {
    use super::*;

    #[test]
    fn single_agent_reaches_free_flow() {
        let mut peds = vec![Pedestrian {
            pos: [0.0, 0.0],
            vel: [0.0, 0.0],
            radius: 0.2,
            desired_speed: 1.34,
            destination: [20.0, 0.0],
        }];
        step(&mut peds, &[], &Params::default(), 0.1);
        assert!((peds[0].vel[0] - 1.34).abs() < 1e-6);
    }

    #[test]
    fn follower_slows_behind_leader() {
        let mut peds = vec![
            Pedestrian {
                pos: [0.0, 0.0],
                vel: [0.0, 0.0],
                radius: 0.2,
                desired_speed: 1.34,
                destination: [100.0, 0.0],
            },
            Pedestrian {
                pos: [-0.5, 0.0],
                vel: [0.0, 0.0],
                radius: 0.2,
                desired_speed: 1.34,
                destination: [100.0, 0.0],
            },
        ];
        step(&mut peds, &[], &Params::default(), 0.1);
        assert!(
            peds[1].vel[0] < peds[0].vel[0],
            "follower must be slower than free-flowing leader"
        );
    }

    #[test]
    fn head_on_pair_does_not_overlap() {
        let mut peds = vec![
            Pedestrian {
                pos: [-4.0, 0.1],
                vel: [0.0, 0.0],
                radius: 0.2,
                desired_speed: 1.34,
                destination: [4.0, 0.1],
            },
            Pedestrian {
                pos: [4.0, -0.1],
                vel: [0.0, 0.0],
                radius: 0.2,
                desired_speed: 1.34,
                destination: [-4.0, -0.1],
            },
        ];
        for _ in 0..300 {
            step(&mut peds, &[], &Params::default(), 0.05);
        }
        let d = norm(sub(peds[0].pos, peds[1].pos));
        assert!(d >= peds[0].radius + peds[1].radius - 1e-3);
    }
}