rustsim_crowd/

integration.rs

//! `rustsim-core` integration for the crowd models.
//!
//! The pedestrian models in this crate operate on bare
//! [`Pedestrian`](crate::common::Pedestrian) structs with no identity
//! or lifecycle. This module adapts them to the core ABM engine so
//! that crowd agents can participate in [`StandardModel`] simulations,
//! be logged through the telemetry pipeline, and be extracted into
//! SoA `f64` buffers suitable for the GPU batch / persistent-store
//! paths in the `rustsim` umbrella crate.
//!
//! # Shape
//!
//! - [`CrowdAgent`] pairs a [`Pedestrian`](crate::common::Pedestrian)
//!   with an [`AgentId`]. It implements [`Agent`] and
//!   [`SoaExtractableF64`] with an **8-column** layout:
//!   `pos.x, pos.y, vel.x, vel.y, radius, desired_speed, dest.x, dest.y`.
//! - [`step_scratch_store`] drives one tick of any 2-D pedestrian
//!   model over a [`VecStore<CrowdAgent>`], using a caller-owned
//!   [`Scratch`](crate::broadphase::Scratch). The store's interior
//!   mutability is respected — each agent is borrowed immutably for
//!   the read phase and mutably for the write-back phase.
//!
//! The adapter is intentionally a thin sync layer rather than a
//! re-implementation of the physics: the source of truth for crowd
//! dynamics stays in the model modules, and this file only shuffles
//! data between `AgentStore` and `&mut [Pedestrian]`.
//!
//! # Example
//!
//! ```ignore
//! use rustsim_core::prelude::*;
//! use rustsim_crowd::prelude::*;
//! use rustsim_crowd::integration::{step_scratch_store, CrowdAgent, SocialForceModel};
//!
//! let mut store: VecStore<CrowdAgent> = VecStore::new();
//! store.insert(CrowdAgent {
//!     id: 0,
//!     ped: Pedestrian {
//!         pos: [0.0, 0.0],
//!         vel: [0.0, 0.0],
//!         radius: 0.25,
//!         desired_speed: 1.34,
//!         destination: [10.0, 0.0],
//!     },
//! });
//! let params = social_force::Params::default();
//! let mut scratch = Scratch::with_capacity(1, social_force::neighbor_cutoff(&params));
//! let mut peds = Vec::with_capacity(1);
//! for _ in 0..100 {
//!     step_scratch_store(&SocialForceModel, &mut store, &[], &params, 0.05, &mut scratch, &mut peds);
//! }
//! ```

use rustsim_core::prelude::{Agent, AgentId, AgentStore, SoaExtractableF64};

use crate::broadphase::Scratch;
use crate::common::{Pedestrian, WallSegment};

/// Identified pedestrian record for use with `rustsim-core` agent stores.
///
/// Wraps a [`Pedestrian`] with an [`AgentId`] so the crowd physics can
/// be driven through the same `StandardModel` / `VecStore` / telemetry
/// machinery used by the rest of the workspace.
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct CrowdAgent {
    /// Stable agent identifier assigned by the owning model.
    pub id: AgentId,
    /// Underlying pedestrian state.
    pub ped: Pedestrian,
}

impl Agent for CrowdAgent {
    #[inline]
    fn id(&self) -> AgentId {
        self.id
    }
}

/// SoA column layout — 8 columns: `pos.x, pos.y, vel.x, vel.y, radius,
/// desired_speed, dest.x, dest.y`. The 8-wide layout matches the
/// `DeviceSoaStore` expectations for `f64`-kernel launches.
impl SoaExtractableF64 for CrowdAgent {
    fn num_columns() -> usize {
        8
    }

    fn column_names() -> Vec<&'static str> {
        vec![
            "pos_x",
            "pos_y",
            "vel_x",
            "vel_y",
            "radius",
            "desired_speed",
            "dest_x",
            "dest_y",
        ]
    }

    fn extract_row(&self, columns: &mut [Vec<f64>]) {
        columns[0].push(self.ped.pos[0]);
        columns[1].push(self.ped.pos[1]);
        columns[2].push(self.ped.vel[0]);
        columns[3].push(self.ped.vel[1]);
        columns[4].push(self.ped.radius);
        columns[5].push(self.ped.desired_speed);
        columns[6].push(self.ped.destination[0]);
        columns[7].push(self.ped.destination[1]);
    }

    fn write_back_row(&mut self, columns: &[&[f64]], row: usize) {
        self.ped.pos = [columns[0][row], columns[1][row]];
        self.ped.vel = [columns[2][row], columns[3][row]];
        self.ped.radius = columns[4][row];
        self.ped.desired_speed = columns[5][row];
        self.ped.destination = [columns[6][row], columns[7][row]];
    }
}

/// Number of SoA columns in the [`CrowdAgent`] layout.
pub const NUM_COLUMNS: usize = 8;

/// Column index of `pos.x` in the [`CrowdAgent`] SoA layout.
pub const COL_POS_X: usize = 0;
/// Column index of `pos.y`.
pub const COL_POS_Y: usize = 1;
/// Column index of `vel.x`.
pub const COL_VEL_X: usize = 2;
/// Column index of `vel.y`.
pub const COL_VEL_Y: usize = 3;
/// Column index of `radius`.
pub const COL_RADIUS: usize = 4;
/// Column index of `desired_speed`.
pub const COL_DESIRED_SPEED: usize = 5;
/// Column index of `dest.x`.
pub const COL_DEST_X: usize = 6;
/// Column index of `dest.y`.
pub const COL_DEST_Y: usize = 7;

/// Unpack the [`CrowdAgent`] SoA layout into a `Pedestrian` buffer.
///
/// `peds_buf` is resized to `n` and filled row-by-row. This is the
/// column → AoS bridge used by [`step_columns_f64`]; it is also the
/// shape a future CUDA kernel will read from its column pointers.
///
/// # Panics
///
/// Panics if `columns.len() < NUM_COLUMNS` or if any column has fewer
/// than `n` entries.
pub fn unpack_columns_into(columns: &[Vec<f64>], n: usize, peds_buf: &mut Vec<Pedestrian>) {
    assert!(
        columns.len() >= NUM_COLUMNS,
        "CrowdAgent SoA needs {NUM_COLUMNS} columns, got {}",
        columns.len()
    );
    peds_buf.clear();
    peds_buf.reserve(n);
    let (pos_x, pos_y) = (&columns[COL_POS_X], &columns[COL_POS_Y]);
    let (vel_x, vel_y) = (&columns[COL_VEL_X], &columns[COL_VEL_Y]);
    let radius = &columns[COL_RADIUS];
    let desired_speed = &columns[COL_DESIRED_SPEED];
    let (dest_x, dest_y) = (&columns[COL_DEST_X], &columns[COL_DEST_Y]);
    for row in 0..n {
        peds_buf.push(Pedestrian {
            pos: [pos_x[row], pos_y[row]],
            vel: [vel_x[row], vel_y[row]],
            radius: radius[row],
            desired_speed: desired_speed[row],
            destination: [dest_x[row], dest_y[row]],
        });
    }
}
/// Write a `Pedestrian` buffer back into the [`CrowdAgent`] SoA columns.
///
/// # Panics
///
/// Panics if `columns.len() < NUM_COLUMNS` or any column is shorter
/// than `peds.len()`.
pub fn pack_columns_from(peds: &[Pedestrian], columns: &mut [Vec<f64>]) {
    assert!(
        columns.len() >= NUM_COLUMNS,
        "CrowdAgent SoA needs {NUM_COLUMNS} columns, got {}",
        columns.len()
    );
    for (row, p) in peds.iter().enumerate() {
        columns[COL_POS_X][row] = p.pos[0];
        columns[COL_POS_Y][row] = p.pos[1];
        columns[COL_VEL_X][row] = p.vel[0];
        columns[COL_VEL_Y][row] = p.vel[1];
        columns[COL_RADIUS][row] = p.radius;
        columns[COL_DESIRED_SPEED][row] = p.desired_speed;
        columns[COL_DEST_X][row] = p.destination[0];
        columns[COL_DEST_Y][row] = p.destination[1];
    }
}

/// Zero-alloc SoA step over `CrowdAgent` columns.
///
/// Consumes parallel `Vec<f64>` columns in the 8-column [`CrowdAgent`]
/// layout, runs one zero-alloc tick of `model`, and writes the columns
/// back in place. `peds_buf` and `scratch` are caller-owned and reused
/// across ticks.
///
/// This is the shape that matches `rustsim::cpu_batch_step_f64`'s
/// closure signature and is the direct transliteration target for a
/// future `.cu` kernel: the SoA column contract here (names, order,
/// precision) is identical to what a CUDA launch would receive as its
/// column pointers. On CPU today it is a thin glue layer on top of
/// [`CrowdStep::step`] — the physics live in the model modules and are
/// bit-exactly the same as the AoS hot path.
///
/// # Example
///
/// ```ignore
/// use rustsim::cpu_batch_step_f64;
/// use rustsim_crowd::prelude::*;
/// use rustsim_crowd::integration::{step_columns_f64, SocialForceModel};
/// use rustsim_crowd::social_force;
///
/// let params = social_force::Params::default();
/// let mut scratch = Scratch::with_capacity(n, social_force::neighbor_cutoff(&params));
/// let mut peds_buf: Vec<Pedestrian> = Vec::with_capacity(n);
/// cpu_batch_step_f64::<CrowdAgent, _, _>(&store, |cols, n| {
///     step_columns_f64(
///         &SocialForceModel,
///         cols,
///         n,
///         &[],
///         &params,
///         0.05,
///         &mut scratch,
///         &mut peds_buf,
///     );
/// });
/// ```
#[allow(clippy::too_many_arguments)]
pub fn step_columns_f64<M>(
    model: &M,
    columns: &mut [Vec<f64>],
    n: usize,
    walls: &[WallSegment],
    params: &M::Params,
    dt: f64,
    scratch: &mut Scratch,
    peds_buf: &mut Vec<Pedestrian>,
) where
    M: CrowdStep,
{
    unpack_columns_into(columns, n, peds_buf);
    model.step(peds_buf, walls, params, dt, scratch);
    pack_columns_from(peds_buf, columns);
}

/// Abstract 2-D crowd model with a zero-alloc step entry point.
///
/// Implemented by the unit marker types in this module so
/// [`step_scratch_store`] can dispatch over any of the five 2-D models
/// through a single generic bound.
pub trait CrowdStep {
    /// Model-specific parameter bundle.
    type Params;

    /// Run one zero-alloc tick over `peds` using `scratch`.
    fn step(
        &self,
        peds: &mut [Pedestrian],
        walls: &[WallSegment],
        params: &Self::Params,
        dt: f64,
        scratch: &mut Scratch,
    );
}

macro_rules! impl_crowd_step {
    ($model:ident, $module:ident) => {
        /// `CrowdStep` adapter for the
        #[doc = concat!("[`", stringify!($module), "`](crate::", stringify!($module), ") model.")]
        #[derive(Debug, Clone, Copy, Default)]
        pub struct $model;

        impl CrowdStep for $model {
            type Params = crate::$module::Params;

            #[inline]
            fn step(
                &self,
                peds: &mut [Pedestrian],
                walls: &[WallSegment],
                params: &Self::Params,
                dt: f64,
                scratch: &mut Scratch,
            ) {
                crate::$module::step_scratch(peds, walls, params, dt, scratch);
            }
        }
    };
}

impl_crowd_step!(SocialForceModel, social_force);
impl_crowd_step!(CollisionFreeSpeedModel, collision_free_speed);
impl_crowd_step!(
    GeneralizedCentrifugalForceModel,
    generalized_centrifugal_force
);
impl_crowd_step!(AnticipationVelocityModel, anticipation_velocity);
impl_crowd_step!(OptimalStepsModel, optimal_steps);

/// Rayon-parallel companion to [`CrowdStep`].
///
/// Implemented on every model marker behind the optional `rayon`
/// feature. The contract mirrors the per-model `step_scratch_par`
/// functions: the call must be **bit-exact** with the serial
/// [`CrowdStep::step`] on the same inputs (no cross-thread float
/// reduction; per-agent scratch slots only). This invariant is what
/// lets [`step_scratch_store_par`] and [`step_scratch_store_observed_par`]
/// transparently substitute for their serial counterparts in
/// production deployments above ~5 000 agents — telemetry, ID
/// ordering, write-back semantics, and observed state are all
/// identical to the serial path. Below ~5 000 agents the rayon
/// dispatch cost dominates and the serial entry points are faster.
#[cfg(feature = "rayon")]
pub trait CrowdStepPar: CrowdStep {
    /// Run one zero-alloc, rayon-parallel tick over `peds` using
    /// `scratch`. Bit-exact with [`CrowdStep::step`].
    fn step_par(
        &self,
        peds: &mut [Pedestrian],
        walls: &[WallSegment],
        params: &Self::Params,
        dt: f64,
        scratch: &mut Scratch,
    );
}

#[cfg(feature = "rayon")]
macro_rules! impl_crowd_step_par {
    ($model:ident, $module:ident) => {
        impl CrowdStepPar for $model {
            #[inline]
            fn step_par(
                &self,
                peds: &mut [Pedestrian],
                walls: &[WallSegment],
                params: &Self::Params,
                dt: f64,
                scratch: &mut Scratch,
            ) {
                crate::$module::step_scratch_par(peds, walls, params, dt, scratch);
            }
        }
    };
}

#[cfg(feature = "rayon")]
impl_crowd_step_par!(SocialForceModel, social_force);
#[cfg(feature = "rayon")]
impl_crowd_step_par!(CollisionFreeSpeedModel, collision_free_speed);
#[cfg(feature = "rayon")]
impl_crowd_step_par!(
    GeneralizedCentrifugalForceModel,
    generalized_centrifugal_force
);
#[cfg(feature = "rayon")]
impl_crowd_step_par!(AnticipationVelocityModel, anticipation_velocity);
#[cfg(feature = "rayon")]
impl_crowd_step_par!(OptimalStepsModel, optimal_steps);

/// Per-agent post-step observation hook.
///
/// Invoked by [`step_scratch_store_observed`] **after** the model has
/// finished the tick and the updated state has been written back to
/// the store, so `ped` reflects the post-tick position, velocity, and
/// destination. Closes the P1 "telemetry hooks" item from
/// `docs/rustsim-crowd.md`: downstream code can forward each row into
/// the workspace's `TelemetryPipeline::push_row`, write it to a CSV
/// logger, update a live heatmap, or compute rolling flow-density
/// statistics — without `rustsim-crowd` itself taking a dependency on
/// any sink implementation.
///
/// The trait is blanket-implemented for every `FnMut(AgentId,
/// &Pedestrian)`, so in practice callers pass a closure:
///
/// ```ignore
/// step_scratch_store_observed(
///     &SocialForceModel, &mut store, &walls, &params, dt,
///     &mut scratch, &mut peds_buf,
///     |id, ped| pipeline.push_row(&[tick.into(), id.into(),
///         ped.pos[0].into(), ped.pos[1].into(),
///         ped.vel[0].into(), ped.vel[1].into()])?,
/// );
/// ```
///
/// Implementations must not panic on valid inputs; the observer is
/// called inside the tick loop and a panic aborts the whole tick.
pub trait CrowdObserver {
    /// Observe the post-tick state of one pedestrian.
    fn observe(&mut self, agent_id: AgentId, ped: &Pedestrian);
}

impl<F> CrowdObserver for F
where
    F: FnMut(AgentId, &Pedestrian),
{
    #[inline]
    fn observe(&mut self, agent_id: AgentId, ped: &Pedestrian) {
        self(agent_id, ped);
    }
}

/// No-op observer used by [`step_scratch_store`] to share code with
/// [`step_scratch_store_observed`]. Monomorphization erases the
/// observer entirely in the non-observing path.
#[derive(Debug, Clone, Copy, Default)]
struct NoopObserver;

impl CrowdObserver for NoopObserver {
    #[inline]
    fn observe(&mut self, _agent_id: AgentId, _ped: &Pedestrian) {}
}

/// Zero-allocation `AgentStore` adapter for any 2-D crowd model.
///
/// Unpacks every agent's [`Pedestrian`] into `peds_buf` (which the
/// caller owns and reuses across ticks), runs one model tick via
/// [`CrowdStep::step`], and writes the updated state back into the
/// store. `peds_buf` is cleared at the start of every call; its
/// capacity is retained, so after the first tick this function does
/// not allocate.
///
/// The `AgentId → pedestrian` ordering is stable across the extract
/// and write-back phases because both iterate `store.iter_ids()`.
pub fn step_scratch_store<M, S>(
    model: &M,
    store: &mut S,
    walls: &[WallSegment],
    params: &M::Params,
    dt: f64,
    scratch: &mut Scratch,
    peds_buf: &mut Vec<Pedestrian>,
) where
    M: CrowdStep,
    S: AgentStore<CrowdAgent>,
{
    step_scratch_store_observed(
        model,
        store,
        walls,
        params,
        dt,
        scratch,
        peds_buf,
        &mut NoopObserver,
    );
}

/// Observed variant of [`step_scratch_store`]: identical semantics,
/// plus a post-writeback callback for every agent.
///
/// `observer.observe(id, &ped)` is invoked once per agent, in the
/// same order as `store.iter_ids()`, with `ped` holding the
/// post-tick state that was just written back. The observer sees
/// the same state the next [`AgentStore::get`] call would return.
///
/// This is the production telemetry entry point: a closure that
/// forwards into `rustsim::TelemetryPipeline::push_row` (or any
/// other sink — CSV, Parquet, in-memory buffer, Prometheus counter)
/// gets per-agent per-tick coverage with zero allocation on the hot
/// path beyond what the sink itself may do.
///
/// Panic safety: if `observer.observe` panics the writeback loop
/// unwinds and the store is left in a partially-updated state (rows
/// before the panicking index are already committed; rows after it
/// have not yet been observed but have been written back). Callers
/// that cannot tolerate that should make `observer.observe` infallible
/// or forward its errors via captured state.
#[allow(clippy::too_many_arguments)]
pub fn step_scratch_store_observed<M, S, O>(
    model: &M,
    store: &mut S,
    walls: &[WallSegment],
    params: &M::Params,
    dt: f64,
    scratch: &mut Scratch,
    peds_buf: &mut Vec<Pedestrian>,
    observer: &mut O,
) where
    M: CrowdStep,
    S: AgentStore<CrowdAgent>,
    O: CrowdObserver + ?Sized,
{
    let ids = store.iter_ids();
    peds_buf.clear();
    peds_buf.reserve(ids.len());
    for &id in &ids {
        if let Some(agent) = store.get(id) {
            peds_buf.push(agent.ped);
        }
    }

    model.step(peds_buf, walls, params, dt, scratch);

    // Write-back. The store's `get_mut` returns a `RefMut`, so we
    // hold one mutable borrow at a time — safe across ticks.
    for (row, &id) in ids.iter().enumerate() {
        if let Some(mut agent) = store.get_mut(id) {
            agent.ped = peds_buf[row];
        }
    }

    // Notify the observer after every row has been written back, so
    // it sees the authoritative post-tick state.
    for (row, &id) in ids.iter().enumerate() {
        observer.observe(id, &peds_buf[row]);
    }
}

/// Rayon-parallel drop-in replacement for [`step_scratch_store`].
///
/// Identical contract — same `peds_buf` reuse, same `iter_ids()`
/// extract / write-back ordering, same returned store state — but
/// the per-agent kernel runs on a rayon thread pool via
/// [`CrowdStepPar::step_par`]. Only the `M::step_par` call differs;
/// the surrounding extract / write-back loops are still serial.
///
/// Use this in production deployments where the rayon-parallel
/// kernel is faster than the serial one (typically N > ~5 000
/// agents on multi-core CPUs). Bit-exact with the serial path —
/// the regression tests in `tests/rayon_bit_exact.rs` and
/// `social_force::tests::step_scratch_par_matches_step_scratch_bit_exact`
/// pin the kernel-level invariant; this function only changes
/// which kernel is invoked.
#[cfg(feature = "rayon")]
#[allow(clippy::too_many_arguments)]
pub fn step_scratch_store_par<M, S>(
    model: &M,
    store: &mut S,
    walls: &[WallSegment],
    params: &M::Params,
    dt: f64,
    scratch: &mut Scratch,
    peds_buf: &mut Vec<Pedestrian>,
) where
    M: CrowdStepPar,
    S: AgentStore<CrowdAgent>,
{
    step_scratch_store_observed_par(
        model,
        store,
        walls,
        params,
        dt,
        scratch,
        peds_buf,
        &mut NoopObserver,
    );
}

/// Observed, rayon-parallel variant of [`step_scratch_store_par`].
///
/// Mirrors [`step_scratch_store_observed`] (same observer contract,
/// same post-writeback order) and runs the model kernel through
/// [`CrowdStepPar::step_par`]. The observer loop itself remains
/// serial so it can hold non-`Send` state (e.g. a
/// `&mut TelemetryPipeline`) and so observation order is
/// deterministic in `iter_ids()` order.
#[cfg(feature = "rayon")]
#[allow(clippy::too_many_arguments)]
pub fn step_scratch_store_observed_par<M, S, O>(
    model: &M,
    store: &mut S,
    walls: &[WallSegment],
    params: &M::Params,
    dt: f64,
    scratch: &mut Scratch,
    peds_buf: &mut Vec<Pedestrian>,
    observer: &mut O,
) where
    M: CrowdStepPar,
    S: AgentStore<CrowdAgent>,
    O: CrowdObserver + ?Sized,
{
    let ids = store.iter_ids();
    peds_buf.clear();
    peds_buf.reserve(ids.len());
    for &id in &ids {
        if let Some(agent) = store.get(id) {
            peds_buf.push(agent.ped);
        }
    }

    model.step_par(peds_buf, walls, params, dt, scratch);

    for (row, &id) in ids.iter().enumerate() {
        if let Some(mut agent) = store.get_mut(id) {
            agent.ped = peds_buf[row];
        }
    }

    for (row, &id) in ids.iter().enumerate() {
        observer.observe(id, &peds_buf[row]);
    }
}

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

    fn agent_at(id: AgentId, x: f64, dest: f64) -> CrowdAgent {
        CrowdAgent {
            id,
            ped: Pedestrian {
                pos: [x, 0.0],
                vel: [0.0, 0.0],
                radius: 0.25,
                desired_speed: 1.34,
                destination: [dest, 0.0],
            },
        }
    }

    #[test]
    fn soa_round_trip_is_identity() {
        let a = agent_at(7, 1.5, 9.0);
        let mut cols: Vec<Vec<f64>> = (0..CrowdAgent::num_columns()).map(|_| Vec::new()).collect();
        a.extract_row(&mut cols);
        let col_refs: Vec<&[f64]> = cols.iter().map(|c| c.as_slice()).collect();
        let mut b = agent_at(7, 0.0, 0.0);
        b.write_back_row(&col_refs, 0);
        assert_eq!(a, b);
    }

    #[test]
    fn step_scratch_store_advances_toward_destination() {
        let mut store: VecStore<CrowdAgent> = VecStore::new();
        store.insert(agent_at(0, 0.0, 10.0));
        let params = crate::social_force::Params::default();
        let mut scratch = Scratch::with_capacity(1, crate::social_force::neighbor_cutoff(&params));
        let mut buf: Vec<Pedestrian> = Vec::with_capacity(1);

        for _ in 0..100 {
            step_scratch_store(
                &SocialForceModel,
                &mut store,
                &[],
                &params,
                0.05,
                &mut scratch,
                &mut buf,
            );
        }

        let pos_x = store.get(0).unwrap().ped.pos[0];
        assert!(pos_x > 1.0, "agent should have advanced: pos_x={pos_x}");
    }

    #[test]
    fn step_scratch_store_matches_direct_step_scratch() {
        // Driving N=8 agents through the store adapter must produce
        // the same trajectory (to machine precision) as calling
        // `step_scratch` directly on a plain slice.
        let make = || -> Vec<CrowdAgent> {
            (0..8)
                .map(|k| {
                    let x = (k as f64) * 1.2;
                    agent_at(k as AgentId, x, x + 5.0)
                })
                .collect()
        };
        let a = make();
        let b = make();

        // Path A — direct slice.
        let mut peds_a: Vec<Pedestrian> = a.iter().map(|c| c.ped).collect();
        let params = crate::social_force::Params::default();
        let cutoff = crate::social_force::neighbor_cutoff(&params);
        let mut scratch_a = Scratch::with_capacity(peds_a.len(), cutoff);
        for _ in 0..30 {
            crate::social_force::step_scratch(&mut peds_a, &[], &params, 0.05, &mut scratch_a);
        }

        // Path B — AgentStore adapter.
        let mut store: VecStore<CrowdAgent> = VecStore::new();
        for agent in b {
            store.insert(agent);
        }
        let mut scratch_b = Scratch::with_capacity(8, cutoff);
        let mut buf: Vec<Pedestrian> = Vec::with_capacity(8);
        for _ in 0..30 {
            step_scratch_store(
                &SocialForceModel,
                &mut store,
                &[],
                &params,
                0.05,
                &mut scratch_b,
                &mut buf,
            );
        }

        for (i, direct) in peds_a.iter().enumerate() {
            let via_store = store.get(i as AgentId).unwrap().ped;
            assert_eq!(direct.pos, via_store.pos, "position diverged at agent {i}");
            assert_eq!(direct.vel, via_store.vel, "velocity diverged at agent {i}");
        }
    }

    #[test]
    fn step_columns_f64_matches_direct_step_scratch() {
        // Running N=8 agents through the SoA-column adapter must
        // produce bit-exact trajectories against `step_scratch` on a
        // plain slice. This pins the SoA contract that a future CUDA
        // kernel will target: same column order, same precision, same
        // physics.
        let make = || -> Vec<Pedestrian> {
            (0..8)
                .map(|k| {
                    let x = (k as f64) * 1.2;
                    Pedestrian {
                        pos: [x, 0.0],
                        vel: [0.0, 0.0],
                        radius: 0.25,
                        desired_speed: 1.34,
                        destination: [x + 5.0, 0.0],
                    }
                })
                .collect()
        };
        let mut peds_aos = make();
        let peds_soa = make();

        let params = crate::social_force::Params::default();
        let cutoff = crate::social_force::neighbor_cutoff(&params);

        // Path A — AoS `step_scratch`.
        let mut scratch_a = Scratch::with_capacity(peds_aos.len(), cutoff);
        for _ in 0..30 {
            crate::social_force::step_scratch(&mut peds_aos, &[], &params, 0.05, &mut scratch_a);
        }

        // Path B — SoA `step_columns_f64`. Build 8 columns of length n.
        let n = peds_soa.len();
        let mut columns: Vec<Vec<f64>> = (0..NUM_COLUMNS).map(|_| Vec::with_capacity(n)).collect();
        for p in &peds_soa {
            columns[COL_POS_X].push(p.pos[0]);
            columns[COL_POS_Y].push(p.pos[1]);
            columns[COL_VEL_X].push(p.vel[0]);
            columns[COL_VEL_Y].push(p.vel[1]);
            columns[COL_RADIUS].push(p.radius);
            columns[COL_DESIRED_SPEED].push(p.desired_speed);
            columns[COL_DEST_X].push(p.destination[0]);
            columns[COL_DEST_Y].push(p.destination[1]);
        }
        let mut scratch_b = Scratch::with_capacity(n, cutoff);
        let mut peds_buf: Vec<Pedestrian> = Vec::with_capacity(n);
        for _ in 0..30 {
            step_columns_f64(
                &SocialForceModel,
                &mut columns,
                n,
                &[],
                &params,
                0.05,
                &mut scratch_b,
                &mut peds_buf,
            );
        }

        for i in 0..n {
            assert_eq!(
                peds_aos[i].pos,
                [columns[COL_POS_X][i], columns[COL_POS_Y][i]],
                "position diverged at agent {i}"
            );
            assert_eq!(
                peds_aos[i].vel,
                [columns[COL_VEL_X][i], columns[COL_VEL_Y][i]],
                "velocity diverged at agent {i}"
            );
        }
    }

    #[test]
    fn observer_sees_post_tick_state_in_id_order() {
        // Pin the observer contract: one callback per agent, in
        // `store.iter_ids()` order, with the `Pedestrian` argument
        // holding the state that was just written back.
        let mut store: VecStore<CrowdAgent> = VecStore::new();
        for k in 0..4 {
            store.insert(agent_at(
                k as AgentId,
                (k as f64) * 1.5,
                (k as f64) * 1.5 + 5.0,
            ));
        }
        let params = crate::social_force::Params::default();
        let mut scratch = Scratch::with_capacity(4, crate::social_force::neighbor_cutoff(&params));
        let mut buf: Vec<Pedestrian> = Vec::with_capacity(4);

        let mut seen: Vec<(AgentId, [f64; 2])> = Vec::new();
        step_scratch_store_observed(
            &SocialForceModel,
            &mut store,
            &[],
            &params,
            0.05,
            &mut scratch,
            &mut buf,
            &mut |id: AgentId, ped: &Pedestrian| {
                seen.push((id, ped.pos));
            },
        );

        assert_eq!(seen.len(), 4);
        for (row, (id, pos)) in seen.iter().enumerate() {
            assert_eq!(*id, row as AgentId);
            let stored = store.get(*id).unwrap().ped.pos;
            assert_eq!(*pos, stored, "observer saw stale pos at agent {id}");
        }
    }

    #[test]
    fn observed_step_matches_unobserved_step() {
        // With a no-op observer, `step_scratch_store_observed` must
        // produce the same trajectory as `step_scratch_store`.
        let make = || -> Vec<CrowdAgent> {
            (0..6)
                .map(|k| agent_at(k as AgentId, (k as f64) * 1.0, (k as f64) * 1.0 + 5.0))
                .collect()
        };
        let params = crate::social_force::Params::default();
        let cutoff = crate::social_force::neighbor_cutoff(&params);

        let mut store_a: VecStore<CrowdAgent> = VecStore::new();
        for a in make() {
            store_a.insert(a);
        }
        let mut scratch_a = Scratch::with_capacity(6, cutoff);
        let mut buf_a: Vec<Pedestrian> = Vec::with_capacity(6);

        let mut store_b: VecStore<CrowdAgent> = VecStore::new();
        for a in make() {
            store_b.insert(a);
        }
        let mut scratch_b = Scratch::with_capacity(6, cutoff);
        let mut buf_b: Vec<Pedestrian> = Vec::with_capacity(6);

        for _ in 0..20 {
            step_scratch_store(
                &SocialForceModel,
                &mut store_a,
                &[],
                &params,
                0.05,
                &mut scratch_a,
                &mut buf_a,
            );
            step_scratch_store_observed(
                &SocialForceModel,
                &mut store_b,
                &[],
                &params,
                0.05,
                &mut scratch_b,
                &mut buf_b,
                &mut |_id: AgentId, _p: &Pedestrian| {},
            );
        }

        for id in 0..6u64 {
            let a = store_a.get(id).unwrap().ped;
            let b = store_b.get(id).unwrap().ped;
            assert_eq!(a.pos, b.pos, "pos diverged at agent {id}");
            assert_eq!(a.vel, b.vel, "vel diverged at agent {id}");
        }
    }
}