rustsim-crowd 0.0.1

//! Anticipation Velocity Model (Xu, Chraibi & Seyfried 2021).
//!
//! An extension of the Collision-Free Speed model that chooses the walking
//! direction by *anticipating* the future positions of neighbours over a
//! look-ahead time horizon. For a discrete set of candidate directions
//! inside a forward cone around the desired direction, the model computes:
//!
//! 1. the *anticipated* clearance along that direction, assuming every
//!    neighbour keeps its current velocity for `anticipation_time` seconds,
//! 2. a smooth penalty for deviating from the goal direction.
//!
//! The candidate with the maximum score (clearance – deviation penalty) is
//! chosen, and speed is then set as in the Collision-Free Speed model with
//! the anticipated clearance replacing the instantaneous headroom.
//!
//! # References
//!
//! - Xu, Q., Chraibi, M., & Seyfried, A. (2021). "Anticipation in a
//!   velocity-based model for pedestrian dynamics". *Transportation
//!   Research Part C: Emerging Technologies*, 133, 103464.

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

/// Parameters for the Anticipation Velocity model.
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct Params {
    /// Safety time gap (s), as in the Collision-Free Speed model.
    pub time_gap: f64,
    /// Look-ahead horizon for neighbour motion prediction (s).
    pub anticipation_time: f64,
    /// Number of candidate directions sampled in the forward cone.
    pub num_directions: usize,
    /// Half-angle of the forward search cone (rad).
    pub cone_half_angle: f64,
    /// Penalty weight for deviating from the desired direction (per rad).
    pub deviation_weight: f64,
    /// Wall safety margin (m). Added to the agent's body radius when
    /// deciding whether a wall clips the anticipated free distance; i.e.
    /// a ray is considered blocked when its closest wall point sits
    /// within `radius + wall_range` of the ray. This matches the clean
    /// geometric treatment in Xu, Chraibi & Seyfried (2021) Eq. 7, in
    /// which walls act as static point obstacles with a small padding.
    ///
    /// Note: a `wall_strength` exponential-penalty field existed in
    /// versions ≤ 0.0.2; it was deprecated in 0.0.2 (unused since the
    /// Eq. 7 rewrite) and removed in 0.0.3. The geometric padding
    /// above is the only wall lever the AVM exposes.
    pub wall_range: 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,
            anticipation_time: 1.0,
            num_directions: 13,
            cone_half_angle: std::f64::consts::FRAC_PI_2,
            deviation_weight: 0.5,
            wall_range: 0.08,
            arrival_radius: 0.3,
        }
    }
}

impl Params {
    /// Validate this parameter set against `dt`.
    ///
    /// AVM is a first-order velocity-based model like CFS; there is no
    /// explicit-Euler CFL condition to check. Validation focuses on
    /// strictly-positive physical parameters, a non-zero candidate count,
    /// and a finite positive `dt`.
    pub fn validate(&self, dt: f64) -> Result<(), crate::error::CrowdError> {
        use crate::error::{require_count, require_dt, require_nonneg, require_positive};
        const M: &str = "AnticipationVelocity";
        require_dt(M, dt)?;
        require_positive(M, "time_gap", self.time_gap)?;
        require_positive(M, "anticipation_time", self.anticipation_time)?;
        require_count(M, "num_directions", self.num_directions)?;
        require_positive(M, "cone_half_angle", self.cone_half_angle)?;
        require_nonneg(M, "deviation_weight", self.deviation_weight)?;
        require_positive(M, "wall_range", self.wall_range)?;
        require_nonneg(M, "arrival_radius", self.arrival_radius)?;
        Ok(())
    }
}

/// Unit marker type implementing [`PedestrianModel`] for the Anticipation
/// Velocity model.
#[derive(Debug, Clone, Copy, Default)]
pub struct AnticipationVelocity;

impl PedestrianModel for AnticipationVelocity {
    type Params = Params;

    fn name(&self) -> &'static str {
        "Anticipation Velocity"
    }

    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 (e, s) = best_direction_and_headroom(i, peds, walls, params);
        let speed_cap = ((s - peds[i].radius) / params.time_gap).max(0.0);
        let v = peds[i]
            .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).
///
/// The anticipated-headroom scan only considers neighbours whose
/// predicted future position (`q.pos + q.vel * anticipation_time`)
/// projects onto the forward ray inside the safety envelope. In
/// practice `anticipation_time * max_speed + 2 * radius + 1 m`
/// provides a generous margin; this function assumes a 2.5 m/s
/// upper bound on pedestrian speed and a 0.5 m body-radius headroom.
#[inline]
pub fn neighbor_cutoff(params: &Params) -> f64 {
    params.anticipation_time * 2.5 + 1.5
}

/// Grid-accelerated step variant. Semantically equivalent to [`step`]
/// up to numerical noise for pairs inside `neighbor_cutoff(params)`.
#[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 (e, s) = best_direction_and_headroom_grid(i, peds, walls, params, grid, cutoff);
        let speed_cap = ((s - peds[i].radius) / params.time_gap).max(0.0);
        let v = peds[i]
            .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 (e, s) = best_direction_and_headroom_grid(i, peds, walls, params, grid, cutoff);
        let speed_cap = ((s - peds[i].radius) / params.time_gap).max(0.0);
        let v = peds[i]
            .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 AVM anticipated-neighbour headroom scan into 4-wide SIMD
/// chunks via [`crate::simd::avm_headroom_x4`]. Candidate-direction
/// sampling, wall clipping, speed tapering, and position integration stay
/// scalar so the public model contract matches the scalar scratch path.
///
/// Numerical contract: neighbour scan chunking can reorder equal-score edge
/// cases only through floating-point lane grouping, so this path is
/// tolerance-equivalent to [`step_scratch`]. `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 (e, s) = best_direction_and_headroom_grid_simd(i, peds, walls, params, grid, cutoff);
        let speed_cap = ((s - peds[i].radius) / params.time_gap).max(0.0);
        let v = peds[i]
            .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 (e, s) = best_direction_and_headroom_grid(i, peds_ro, walls, params, grid, cutoff);
        let speed_cap = ((s - peds_ro[i].radius) / params.time_gap).max(0.0);
        let v = peds_ro[i]
            .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 [`best_direction_and_headroom`].
fn best_direction_and_headroom_grid(
    i: usize,
    peds: &[Pedestrian],
    walls: &[WallSegment],
    params: &Params,
    grid: &NeighborGrid,
    cutoff: f64,
) -> (Vec2, f64) {
    let p = &peds[i];
    let e_dest = p.desired_direction();
    if e_dest == [0.0, 0.0] {
        return ([0.0, 0.0], 0.0);
    }

    let base_angle = e_dest[1].atan2(e_dest[0]);
    let m = params.num_directions.max(1);
    let half = params.cone_half_angle;

    let mut best_dir = e_dest;
    let mut best_headroom =
        anticipated_headroom_grid(i, peds, walls, &e_dest, params, grid, cutoff);
    let mut best_score = best_headroom;

    for k in 0..m {
        let t = if m == 1 {
            0.0
        } else {
            -1.0 + 2.0 * (k as f64) / ((m - 1) as f64)
        };
        let theta = base_angle + t * half;
        let dir = [theta.cos(), theta.sin()];
        let headroom = anticipated_headroom_grid(i, peds, walls, &dir, params, grid, cutoff);
        let deviation = params.deviation_weight * t.abs() * half;
        let score = headroom - deviation;
        if score > best_score {
            best_score = score;
            best_headroom = headroom;
            best_dir = dir;
        }
    }

    (best_dir, best_headroom)
}

#[cfg(feature = "simd")]
fn best_direction_and_headroom_grid_simd(
    i: usize,
    peds: &[Pedestrian],
    walls: &[WallSegment],
    params: &Params,
    grid: &NeighborGrid,
    cutoff: f64,
) -> (Vec2, f64) {
    let p = &peds[i];
    let e_dest = p.desired_direction();
    if e_dest == [0.0, 0.0] {
        return ([0.0, 0.0], 0.0);
    }

    let base_angle = e_dest[1].atan2(e_dest[0]);
    let m = params.num_directions.max(1);
    let half = params.cone_half_angle;

    let mut best_dir = e_dest;
    let mut best_headroom =
        anticipated_headroom_grid_simd(i, peds, walls, &e_dest, params, grid, cutoff);
    let mut best_score = best_headroom;

    for k in 0..m {
        let t = if m == 1 {
            0.0
        } else {
            -1.0 + 2.0 * (k as f64) / ((m - 1) as f64)
        };
        let theta = base_angle + t * half;
        let dir = [theta.cos(), theta.sin()];
        let headroom = anticipated_headroom_grid_simd(i, peds, walls, &dir, params, grid, cutoff);
        let deviation = params.deviation_weight * t.abs() * half;
        let score = headroom - deviation;
        if score > best_score {
            best_score = score;
            best_headroom = headroom;
            best_dir = dir;
        }
    }

    (best_dir, best_headroom)
}

/// Grid-backed version of [`anticipated_headroom`].
fn anticipated_headroom_grid(
    i: usize,
    peds: &[Pedestrian],
    walls: &[WallSegment],
    e: &Vec2,
    params: &Params,
    grid: &NeighborGrid,
    cutoff: f64,
) -> f64 {
    let p = &peds[i];
    let mut s = f64::INFINITY;

    grid.for_each_neighbor(i, cutoff, peds, |_j, q| {
        let q_future = add(q.pos, scale(q.vel, params.anticipation_time));
        let rel = sub(q_future, 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;
        }
    });

    for w in walls {
        // See the non-grid [`anticipated_headroom`] for the
        // paper-aligned rationale.
        let probe = add(p.pos, scale(*e, p.desired_speed * params.anticipation_time));
        let closest = closest_point_on_segment(probe, w.a, w.b);
        let rel = sub(closest, p.pos);
        let forward = dot(rel, *e);
        if forward <= 0.0 {
            continue;
        }
        let proj = scale(*e, forward);
        let lat = norm(sub(rel, proj));
        if lat > p.radius + params.wall_range {
            continue;
        }
        let d = norm(rel);
        if d < s {
            s = d;
        }
    }

    s
}

#[cfg(feature = "simd")]
fn anticipated_headroom_grid_simd(
    i: usize,
    peds: &[Pedestrian],
    walls: &[WallSegment],
    e: &Vec2,
    params: &Params,
    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::avm_headroom_x4(p, *e, 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]),
        ];
        s = s.min(crate::simd::avm_headroom_x4(p, *e, buf, params));
    }

    for w in walls {
        let probe = add(p.pos, scale(*e, p.desired_speed * params.anticipation_time));
        let closest = closest_point_on_segment(probe, w.a, w.b);
        let rel = sub(closest, p.pos);
        let forward = dot(rel, *e);
        if forward <= 0.0 {
            continue;
        }
        let proj = scale(*e, forward);
        let lat = norm(sub(rel, proj));
        if lat > p.radius + params.wall_range {
            continue;
        }
        let d = norm(rel);
        if d < s {
            s = d;
        }
    }

    s
}

/// Returns the best forward direction and its anticipated free distance.
pub fn best_direction_and_headroom(
    i: usize,
    peds: &[Pedestrian],
    walls: &[WallSegment],
    params: &Params,
) -> (Vec2, f64) {
    let p = &peds[i];
    let e_dest = p.desired_direction();
    if e_dest == [0.0, 0.0] {
        return ([0.0, 0.0], 0.0);
    }

    let base_angle = e_dest[1].atan2(e_dest[0]);
    let m = params.num_directions.max(1);
    let half = params.cone_half_angle;

    let mut best_dir = e_dest;
    let mut best_headroom = anticipated_headroom(i, peds, walls, &e_dest, params);
    let mut best_score = best_headroom;

    for k in 0..m {
        let t = if m == 1 {
            0.0
        } else {
            -1.0 + 2.0 * (k as f64) / ((m - 1) as f64)
        };
        let theta = base_angle + t * half;
        let dir = [theta.cos(), theta.sin()];
        let headroom = anticipated_headroom(i, peds, walls, &dir, params);
        let deviation = params.deviation_weight * t.abs() * half;
        let score = headroom - deviation;
        if score > best_score {
            best_score = score;
            best_headroom = headroom;
            best_dir = dir;
        }
    }

    (best_dir, best_headroom)
}

/// Distance to the closest predicted collision along direction `e`, where
/// every neighbour is extrapolated with constant velocity for
/// `anticipation_time` seconds. Walls are treated as static.
pub fn anticipated_headroom(
    i: usize,
    peds: &[Pedestrian],
    walls: &[WallSegment],
    e: &Vec2,
    params: &Params,
) -> f64 {
    let p = &peds[i];
    let mut s = f64::INFINITY;

    for (j, q) in peds.iter().enumerate() {
        if i == j {
            continue;
        }
        // Predict neighbour position after the anticipation horizon.
        let q_future = add(q.pos, scale(q.vel, params.anticipation_time));
        let rel = sub(q_future, p.pos);
        let forward = dot(rel, *e);
        if forward <= 0.0 {
            continue;
        }
        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;
        }
    }

    for w in walls {
        // Treat the closest point on the wall segment as a static
        // point obstacle; block the ray iff its lateral distance to
        // that point is within the padded body radius. Matches the
        // geometric wall handling in Xu, Chraibi & Seyfried (2021)
        // Eq. 7.
        let probe = add(p.pos, scale(*e, p.desired_speed * params.anticipation_time));
        let closest = closest_point_on_segment(probe, w.a, w.b);
        let rel = sub(closest, p.pos);
        let forward = dot(rel, *e);
        if forward <= 0.0 {
            continue;
        }
        let proj = scale(*e, forward);
        let lat = norm(sub(rel, proj));
        if lat > p.radius + params.wall_range {
            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_advances_at_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);
        let v = norm(peds[0].vel);
        assert!((v - 1.34).abs() < 1e-6);
    }

    #[test]
    fn deviates_around_static_obstacle() {
        // Agent walking east, with a static blocker directly in front.
        let mut peds = vec![
            Pedestrian {
                pos: [0.0, 0.0],
                vel: [0.0, 0.0],
                radius: 0.2,
                desired_speed: 1.34,
                destination: [10.0, 0.0],
            },
            Pedestrian {
                pos: [3.0, 0.0],
                vel: [0.0, 0.0],
                radius: 0.2,
                desired_speed: 0.0,
                destination: [3.0, 0.0],
            },
        ];
        for _ in 0..200 {
            step(&mut peds, &[], &Params::default(), 0.05);
        }
        // The agent must pass the blocker (x > 3.5) without overlap.
        assert!(peds[0].pos[0] > 3.5);
        let d = norm(sub(peds[0].pos, peds[1].pos));
        assert!(d >= peds[0].radius + peds[1].radius - 0.05);
    }

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