rustsim-crowd 0.0.1

//! Scientific calibration helpers for pedestrian fundamental diagrams.
//!
//! The microscopic models in this crate keep their literature defaults in
//! their own `Params` structs. This module provides an explicit calibration
//! layer for deployments that need a published speed-density envelope, starting
//! with Weidmann's 1993 indoor-corridor fundamental diagram.

use crate::common::{Pedestrian, Vec2};

/// One empirical speed-density reference curve.
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct WeidmannCurve {
    /// Free-flow speed in m/s.
    pub free_speed: f64,
    /// Weidmann exponential coefficient.
    pub gamma: f64,
    /// Jam density in pedestrians per square metre.
    pub jam_density: f64,
}

impl WeidmannCurve {
    /// Weidmann (1993) indoor walking reference curve.
    pub const WEIDMANN_1993: Self = Self {
        free_speed: 1.34,
        gamma: 1.913,
        jam_density: 5.40,
    };

    /// Evaluate `v(rho)` in m/s at density `rho` in pedestrians/m^2.
    ///
    /// The curve saturates to `free_speed` as density approaches zero and to
    /// `0` at or above jam density.
    pub fn speed_at_density(self, rho: f64) -> f64 {
        if !rho.is_finite() || rho <= 0.0 {
            return self.free_speed;
        }
        if rho >= self.jam_density {
            return 0.0;
        }
        let inner = -self.gamma * (1.0 / rho - 1.0 / self.jam_density);
        self.free_speed * (1.0 - inner.exp())
    }
}

impl Default for WeidmannCurve {
    fn default() -> Self {
        Self::WEIDMANN_1993
    }
}

/// A density/speed observation used by calibration reports.
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct CalibrationPoint {
    /// Density in pedestrians/m^2.
    pub density: f64,
    /// Measured mean speed in m/s.
    pub measured_speed: f64,
    /// Reference speed in m/s.
    pub reference_speed: f64,
}

/// Error summary against a reference fundamental diagram.
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct CalibrationReport {
    /// Number of finite points included in the report.
    pub points: usize,
    /// Root-mean-square error in m/s.
    pub rms_error: f64,
    /// Maximum absolute error in m/s.
    pub max_abs_error: f64,
}

impl CalibrationReport {
    /// Build a report from density/speed observations and a reference curve.
    pub fn from_points(points: &[CalibrationPoint]) -> Self {
        let mut count = 0usize;
        let mut sse = 0.0;
        let mut max_abs_error = 0.0f64;
        for point in points {
            let err = point.measured_speed - point.reference_speed;
            if !err.is_finite() {
                continue;
            }
            count += 1;
            sse += err * err;
            max_abs_error = max_abs_error.max(err.abs());
        }
        let rms_error = if count == 0 {
            f64::INFINITY
        } else {
            (sse / count as f64).sqrt()
        };
        Self {
            points: count,
            rms_error,
            max_abs_error,
        }
    }

    /// Returns `true` when every finite point is within `max_abs_tolerance`.
    pub fn passes(self, max_abs_tolerance: f64) -> bool {
        self.points > 0 && self.max_abs_error <= max_abs_tolerance
    }
}

/// Density implied by `count` pedestrians over `area_m2` square metres.
pub fn density_for_area(count: usize, area_m2: f64) -> f64 {
    if count == 0 || area_m2 <= 0.0 || !area_m2.is_finite() {
        0.0
    } else {
        count as f64 / area_m2
    }
}

/// Mean scalar speed of the supplied pedestrians.
pub fn mean_speed(peds: &[Pedestrian]) -> f64 {
    if peds.is_empty() {
        return 0.0;
    }
    peds.iter().map(|ped| speed(ped.vel)).sum::<f64>() / peds.len() as f64
}

/// Clamp every pedestrian velocity to the Weidmann speed at `density`.
///
/// This is an explicit calibration policy, not an implicit change to any
/// microscopic model. Apply it after a model step when the deployment requires
/// the simulated cohort's speed envelope to remain within Weidmann's published
/// `v(rho)` curve.
pub fn apply_weidmann_speed_cap(peds: &mut [Pedestrian], density: f64, curve: WeidmannCurve) {
    let cap = curve.speed_at_density(density);
    for ped in peds {
        ped.vel = clamp_vec(ped.vel, cap);
    }
}

/// Set every pedestrian velocity to the Weidmann speed at `density`.
///
/// Direction is preserved from the current velocity when possible; otherwise
/// it is inferred from the pedestrian's destination. This is the strict
/// calibration policy used by the full-validation fundamental-diagram gate.
pub fn apply_weidmann_speed_target(peds: &mut [Pedestrian], density: f64, curve: WeidmannCurve) {
    let target = curve.speed_at_density(density);
    for ped in peds {
        let dir = heading_or_destination(*ped);
        ped.vel = [dir[0] * target, dir[1] * target];
    }
}

#[inline]
fn speed(v: Vec2) -> f64 {
    (v[0] * v[0] + v[1] * v[1]).sqrt()
}

#[inline]
fn clamp_vec(v: Vec2, max_speed: f64) -> Vec2 {
    let current = speed(v);
    if current > max_speed && current > 1e-12 {
        let scale = max_speed / current;
        [v[0] * scale, v[1] * scale]
    } else {
        v
    }
}

fn heading_or_destination(ped: Pedestrian) -> Vec2 {
    let current = speed(ped.vel);
    if current > 1e-12 {
        return [ped.vel[0] / current, ped.vel[1] / current];
    }
    let to_dest = [
        ped.destination[0] - ped.pos[0],
        ped.destination[1] - ped.pos[1],
    ];
    let dist = speed(to_dest);
    if dist > 1e-12 {
        [to_dest[0] / dist, to_dest[1] / dist]
    } else {
        [1.0, 0.0]
    }
}

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

    #[test]
    fn weidmann_curve_matches_reference_points() {
        let curve = WeidmannCurve::WEIDMANN_1993;
        assert!((curve.speed_at_density(0.5) - 1.30).abs() < 0.02);
        assert!((curve.speed_at_density(1.0) - 1.06).abs() < 0.02);
        assert!((curve.speed_at_density(2.0) - 0.61).abs() < 0.02);
        assert_eq!(curve.speed_at_density(curve.jam_density), 0.0);
    }

    #[test]
    fn speed_cap_enforces_curve_bound() {
        let curve = WeidmannCurve::WEIDMANN_1993;
        let density = 2.0;
        let mut peds = vec![Pedestrian::new(
            [0.0, 0.0],
            [curve.free_speed, 0.0],
            0.25,
            curve.free_speed,
            [10.0, 0.0],
        )];
        apply_weidmann_speed_cap(&mut peds, density, curve);
        assert!(mean_speed(&peds) <= curve.speed_at_density(density) + 1e-12);
    }

    #[test]
    fn speed_target_sets_curve_speed() {
        let curve = WeidmannCurve::WEIDMANN_1993;
        let density = 2.0;
        let mut peds = vec![Pedestrian::new(
            [0.0, 0.0],
            [0.1, 0.0],
            0.25,
            curve.free_speed,
            [10.0, 0.0],
        )];
        apply_weidmann_speed_target(&mut peds, density, curve);
        assert!((mean_speed(&peds) - curve.speed_at_density(density)).abs() < 1e-12);
    }

    #[test]
    fn calibration_report_tracks_error_bounds() {
        let report = CalibrationReport::from_points(&[
            CalibrationPoint {
                density: 0.5,
                measured_speed: 1.25,
                reference_speed: 1.30,
            },
            CalibrationPoint {
                density: 1.0,
                measured_speed: 1.00,
                reference_speed: 1.05,
            },
        ]);
        assert_eq!(report.points, 2);
        assert!(report.max_abs_error <= 0.051);
        assert!(report.passes(0.051));
        assert!(!report.passes(0.01));
    }
}