rustsim 0.0.1

//! Integration test: counter-flow corridor with physical avoidance.
//!
//! Two opposing streams of pedestrians walk toward each other through a
//! corridor bounded by walls. The Social Force Model with physical contact
//! forces drives avoidance behavior.
//!
//! Validates:
//! 1. Agents avoid each other and reach their destinations.
//! 2. Wall repulsion keeps agents inside the corridor.
//! 3. Physical contact forces prevent persistent body overlaps.
//! 4. Deterministic replay for a fixed seed.

use rand::rngs::StdRng;
use rand::{Rng, SeedableRng};
use rustsim::prelude::*;
use rustsim_spaces::continuous::{ContinuousPos, ContinuousSpace2D};
use std::time::Instant;

// ---- Parameters ----

const CORRIDOR_X: f64 = 40.0;
const CORRIDOR_Y: f64 = 8.0;
const NUM_AGENTS: usize = 60; // 30 per direction
const DT: f64 = 0.05;
const BODY_RADIUS: f64 = 0.25;
const DESIRED_SPEED: f64 = 1.3;
const SEARCH_RADIUS: f64 = 3.0;
const NUM_STEPS: usize = 5000; // 250 seconds

// ---- Agent ----

#[derive(Debug, Clone)]
struct Pedestrian {
    id: AgentId,
    x: f64,
    y: f64,
    vx: f64,
    vy: f64,
    dest_x: f64,
    dest_y: f64,
    radius: f64,
    desired_speed: f64,
    arrived: bool,
}

impl Agent for Pedestrian {
    fn id(&self) -> AgentId {
        self.id
    }
}

// ---- Helper wrapper for initial space registration ----

#[derive(Debug, Clone)]
struct PosWrapper {
    id: AgentId,
    pos: ContinuousPos,
}

impl Agent for PosWrapper {
    fn id(&self) -> AgentId {
        self.id
    }
}

impl PositionedAgent for PosWrapper {
    type Position = ContinuousPos;
    fn position(&self) -> &ContinuousPos {
        &self.pos
    }
    fn set_position(&mut self, pos: ContinuousPos) {
        self.pos = pos;
    }
}

// ---- Properties ----

#[derive(Debug, Clone)]
struct CorridorProps {
    walls: Vec<WallSegment>,
    sfm_params: SocialForceParams,
    dt: f64,
    arrivals: usize,
}

// ---- Model type ----

type CorridorModel = StandardModel<
    ContinuousSpace2D,
    Pedestrian,
    HashMapStore<Pedestrian>,
    CorridorProps,
    StdRng,
    Fastest,
>;

// ---- Agent step ----

fn ped_step(
    agent: &mut Pedestrian,
    ctx: &mut StepContext<'_, ContinuousSpace2D, Pedestrian, CorridorProps, StdRng, Fastest>,
) {
    if agent.arrived {
        return;
    }

    let props = ctx.properties();
    let dt = props.dt;
    let params = &props.sfm_params;

    // Check arrival.
    let dx = agent.dest_x - agent.x;
    let dy = agent.dest_y - agent.y;
    if (dx * dx + dy * dy).sqrt() < 0.5 {
        agent.arrived = true;
        agent.vx = 0.0;
        agent.vy = 0.0;
        return;
    }

    // 1. Desired force.
    let (fdx, fdy) = desired_force_2d(
        agent.x,
        agent.y,
        agent.vx,
        agent.vy,
        agent.dest_x,
        agent.dest_y,
        agent.desired_speed,
        params.tau,
    );
    let mut fx = fdx;
    let mut fy = fdy;

    // 2. Social repulsion from neighbors.
    let nearby = ctx
        .space()
        .nearby_ids_euclidean(&ContinuousPos::new(agent.x, agent.y), SEARCH_RADIUS);

    for &nid in &nearby {
        if nid == agent.id {
            continue;
        }
        if let Some(npos) = ctx.space().agent_position(nid) {
            // Neighbor velocity approximated as zero (we do not have it
            // from the space; a real simulation would use messaging).
            let (sfx, sfy) = social_repulsion_2d(
                agent.x,
                agent.y,
                agent.vx,
                agent.vy,
                agent.radius,
                npos.x,
                npos.y,
                0.0,
                0.0,
                BODY_RADIUS,
                params,
            );
            fx += sfx;
            fy += sfy;
        }
    }

    // 3. Wall repulsion.
    for wall in &ctx.properties().walls {
        let (wfx, wfy) = wall_repulsion_2d(
            agent.x,
            agent.y,
            agent.vx,
            agent.vy,
            agent.radius,
            wall,
            params,
        );
        fx += wfx;
        fy += wfy;
    }

    // 4. Integrate.
    let (new_x, new_y, new_vx, new_vy) =
        integrate_euler_2d(agent.x, agent.y, agent.vx, agent.vy, fx, fy, dt, params);

    // Clamp to corridor bounds.
    agent.x = new_x.clamp(0.01, CORRIDOR_X - 0.01);
    agent.y = new_y.clamp(0.01, CORRIDOR_Y - 0.01);
    agent.vx = new_vx;
    agent.vy = new_vy;
}

// ---- Model step: update space positions, count arrivals, remove arrived ----

fn model_step(model: &mut CorridorModel) {
    // Update space positions.
    let agent_data: Vec<(AgentId, f64, f64, bool)> = model
        .agents()
        .map(|a| (a.id(), a.x, a.y, a.arrived))
        .collect();

    let mut newly_arrived = 0usize;
    for (id, x, y, arrived) in &agent_data {
        let _ = model
            .space_mut()
            .move_agent_pos(*id, ContinuousPos::new(*x, *y));
        if *arrived {
            newly_arrived += 1;
        }
    }

    // Remove arrived agents.
    let to_remove: Vec<AgentId> = model
        .agents()
        .filter(|a| a.arrived)
        .map(|a| a.id())
        .collect();
    for id in to_remove {
        model.remove_agent(id);
    }

    model.properties_mut().arrivals += newly_arrived;
}

// ---- Helpers ----

fn build_corridor_model(seed: u64) -> CorridorModel {
    let mut rng = StdRng::seed_from_u64(seed);
    let mut space = ContinuousSpace2D::new(CORRIDOR_X, CORRIDOR_Y, false, SEARCH_RADIUS).unwrap();
    let mut store = HashMapStore::new();

    let walls = vec![
        WallSegment::new(0.0, 0.0, CORRIDOR_X, 0.0), // bottom
        WallSegment::new(0.0, CORRIDOR_Y, CORRIDOR_X, CORRIDOR_Y), // top
    ];

    // Helbing 1995-style parameters tuned for a counter-flow test.
    // Social repulsion is moderate so agents can negotiate passage.
    let sfm_params = SocialForceParams {
        a_social: 2.1,
        b_social: 0.3,
        k_body: 1.2e5,
        kappa_friction: 2.4e5,
        a_wall: 10.0,
        b_wall: 0.2,
        k_wall: 1.2e5,
        kappa_wall: 2.4e5,
        max_speed: 2.5,
        tau: 0.5,
        mass: 80.0,
    };

    for i in 0..NUM_AGENTS {
        let id_counter = (i as u64) + 1;
        let going_right = i < NUM_AGENTS / 2;
        let (x, dest_x) = if going_right {
            (rng.gen_range(1.0..5.0), CORRIDOR_X - 1.0)
        } else {
            (rng.gen_range(CORRIDOR_X - 5.0..CORRIDOR_X - 1.0), 1.0)
        };
        let y = rng.gen_range(1.0..CORRIDOR_Y - 1.0);

        let ped = Pedestrian {
            id: id_counter,
            x,
            y,
            vx: 0.0,
            vy: 0.0,
            dest_x,
            dest_y: rng.gen_range(1.0..CORRIDOR_Y - 1.0),
            radius: BODY_RADIUS,
            desired_speed: DESIRED_SPEED + rng.gen_range(-0.1..0.1),
            arrived: false,
        };

        // Register with space via wrapper.
        let wrapper = PosWrapper {
            id: ped.id,
            pos: ContinuousPos::new(ped.x, ped.y),
        };
        <ContinuousSpace2D as SpaceInteraction<PosWrapper>>::add_agent(&mut space, &wrapper)
            .expect("initial placement should succeed");

        store.insert(ped);
    }

    let props = CorridorProps {
        walls,
        sfm_params,
        dt: DT,
        arrivals: 0,
    };

    CorridorModel::new(
        store,
        space,
        Fastest::new(),
        props,
        StdRng::seed_from_u64(seed),
        Some(Box::new(ped_step)),
        Some(model_step),
        true,
    )
}

// ===========================================================================
// Tests
// ===========================================================================

#[test]
fn counter_flow_corridor_agents_reach_destinations() {
    let mut model = build_corridor_model(42);

    let t0 = Instant::now();
    model.step_n(NUM_STEPS);
    let elapsed_ms = t0.elapsed().as_millis();

    let arrivals = model.properties().arrivals;
    let remaining = model.agents_len();

    eprintln!(
        "[counter-flow] {}/{} arrived, {} remaining, {} ms ({} steps)",
        arrivals, NUM_AGENTS, remaining, elapsed_ms, NUM_STEPS
    );

    // At least a third should arrive (counter-flow is inherently slow;
    // the key validation is that agents DO navigate past each other).
    assert!(
        arrivals >= NUM_AGENTS / 3,
        "at least a third of agents should arrive; got {}/{}",
        arrivals,
        NUM_AGENTS
    );
}

#[test]
fn agents_stay_inside_corridor_bounds() {
    let mut model = build_corridor_model(99);

    for _ in 0..500 {
        model.step();

        for agent in model.agents() {
            assert!(
                agent.x >= 0.0 && agent.x <= CORRIDOR_X,
                "agent {} x={} out of corridor bounds",
                agent.id,
                agent.x
            );
            assert!(
                agent.y >= 0.0 && agent.y <= CORRIDOR_Y,
                "agent {} y={} out of corridor bounds",
                agent.id,
                agent.y
            );
        }
    }
}

#[test]
fn physical_contact_prevents_persistent_overlap() {
    let mut model = build_corridor_model(123);

    // Run 1000 steps then check overlap.
    model.step_n(1000);

    // Collect positions of non-arrived agents.
    let positions: Vec<(AgentId, f64, f64)> = model
        .agents()
        .filter(|a| !a.arrived)
        .map(|a| (a.id(), a.x, a.y))
        .collect();

    let mut severe_overlaps = 0;
    for i in 0..positions.len() {
        for j in (i + 1)..positions.len() {
            let dx = positions[i].1 - positions[j].1;
            let dy = positions[i].2 - positions[j].2;
            let dist = (dx * dx + dy * dy).sqrt();
            let min_dist = BODY_RADIUS * 2.0;
            // Allow mild overlap from discrete integration, but not severe.
            if dist < min_dist * 0.3 {
                severe_overlaps += 1;
            }
        }
    }

    let active_agents = positions.len();
    // Allow up to 5% of agent pairs to have severe overlap.
    let max_allowed = (active_agents as f64 * 0.05).ceil() as usize;
    assert!(
        severe_overlaps <= max_allowed,
        "too many severe overlaps: {} (max {}). Physical contact forces should prevent this.",
        severe_overlaps,
        max_allowed
    );

    eprintln!(
        "[overlap check] {} active agents, {} severe overlaps (threshold: {})",
        active_agents, severe_overlaps, max_allowed
    );
}

#[test]
fn deterministic_replay() {
    let mut model1 = build_corridor_model(7);
    model1.step_n(200);
    let checksum1: f64 = model1.agents().map(|a| a.x + a.y + a.vx + a.vy).sum();
    let arrivals1 = model1.properties().arrivals;

    let mut model2 = build_corridor_model(7);
    model2.step_n(200);
    let checksum2: f64 = model2.agents().map(|a| a.x + a.y + a.vx + a.vy).sum();
    let arrivals2 = model2.properties().arrivals;

    assert_eq!(arrivals1, arrivals2, "arrival counts should match");
    assert!(
        (checksum1 - checksum2).abs() < 1e-6,
        "checksums should match: {} vs {}",
        checksum1,
        checksum2
    );
}