rustsim-core 0.0.1

//! Tests for physical avoidance force primitives.

use rustsim_core::avoidance::*;

// ===========================================================================
// 2D desired force
// ===========================================================================

#[test]
fn desired_force_direction_positive_x() {
    let (fx, fy) = desired_force_2d(0.0, 0.0, 0.0, 0.0, 10.0, 0.0, 1.3, 0.5);
    assert!(fx > 0.0);
    assert!(fy.abs() < 1e-12);
}

#[test]
fn desired_force_direction_negative_y() {
    let (fx, fy) = desired_force_2d(5.0, 5.0, 0.0, 0.0, 5.0, 0.0, 1.3, 0.5);
    assert!(fx.abs() < 1e-12);
    assert!(fy < 0.0);
}

#[test]
fn desired_force_magnitude_scales_with_desired_speed() {
    let (fx1, _) = desired_force_2d(0.0, 0.0, 0.0, 0.0, 10.0, 0.0, 1.0, 0.5);
    let (fx2, _) = desired_force_2d(0.0, 0.0, 0.0, 0.0, 10.0, 0.0, 2.0, 0.5);
    assert!(
        fx2 > fx1,
        "higher desired speed should produce larger force"
    );
}

#[test]
fn desired_force_reduces_when_moving_toward_destination() {
    // Stationary agent
    let (fx_still, _) = desired_force_2d(0.0, 0.0, 0.0, 0.0, 10.0, 0.0, 1.3, 0.5);
    // Agent already moving toward destination at desired speed
    let (fx_moving, _) = desired_force_2d(0.0, 0.0, 1.3, 0.0, 10.0, 0.0, 1.3, 0.5);
    assert!(fx_still > fx_moving.abs());
    assert!(
        fx_moving.abs() < 1e-10,
        "force should be ~0 when at desired speed"
    );
}

#[test]
fn desired_force_zero_at_destination() {
    let (fx, fy) = desired_force_2d(5.0, 5.0, 1.0, 1.0, 5.0, 5.0, 1.3, 0.5);
    assert!(fx.abs() < 1e-10);
    assert!(fy.abs() < 1e-10);
}

// ===========================================================================
// 2D social repulsion
// ===========================================================================

#[test]
fn social_repulsion_sign_correct() {
    let params = SocialForceParams::default();
    // Neighbor at origin, agent to the right
    let (fx, fy) = social_repulsion_2d(2.0, 0.0, 0.0, 0.0, 0.25, 0.0, 0.0, 0.0, 0.0, 0.25, &params);
    assert!(
        fx > 0.0,
        "repulsion should push agent in +x (away from neighbor)"
    );
    assert!(fy.abs() < 1e-10);
}

#[test]
fn social_repulsion_decays_with_distance() {
    let params = SocialForceParams::default();
    let (fx_close, _) =
        social_repulsion_2d(1.0, 0.0, 0.0, 0.0, 0.25, 0.0, 0.0, 0.0, 0.0, 0.25, &params);
    let (fx_far, _) =
        social_repulsion_2d(5.0, 0.0, 0.0, 0.0, 0.25, 0.0, 0.0, 0.0, 0.0, 0.25, &params);
    assert!(
        fx_close > fx_far,
        "closer agents should feel stronger repulsion"
    );
}

#[test]
fn physical_contact_force_activates_on_overlap() {
    let params = SocialForceParams::default();
    let r = 0.25;

    // Overlapping: center distance < r_i + r_j = 0.5
    let (fx_overlap, _) =
        social_repulsion_2d(0.3, 0.0, 0.0, 0.0, r, 0.0, 0.0, 0.0, 0.0, r, &params);

    // Just touching: center distance = 0.5
    let (fx_touch, _) = social_repulsion_2d(0.5, 0.0, 0.0, 0.0, r, 0.0, 0.0, 0.0, 0.0, r, &params);

    // Apart: center distance > 0.5
    let (fx_apart, _) = social_repulsion_2d(1.0, 0.0, 0.0, 0.0, r, 0.0, 0.0, 0.0, 0.0, r, &params);

    assert!(
        fx_overlap > fx_touch,
        "overlap ({}) should produce stronger force than touching ({})",
        fx_overlap,
        fx_touch
    );
    assert!(
        fx_touch > fx_apart,
        "touching ({}) should produce stronger force than apart ({})",
        fx_touch,
        fx_apart
    );
}

#[test]
fn sliding_friction_opposes_tangential_motion() {
    let params = SocialForceParams::default();
    // Two overlapping agents; neighbor moves tangentially in +y
    let (_, fy_with_friction) =
        social_repulsion_2d(0.3, 0.0, 0.0, 0.0, 0.25, 0.0, 0.0, 0.0, 1.0, 0.25, &params);
    // Two overlapping agents; no tangential motion
    let (_, fy_no_friction) =
        social_repulsion_2d(0.3, 0.0, 0.0, 0.0, 0.25, 0.0, 0.0, 0.0, 0.0, 0.25, &params);
    // With friction, there should be a tangential component
    assert!(
        (fy_with_friction - fy_no_friction).abs() > 1e-6,
        "tangential friction should produce a nonzero difference"
    );
}

#[test]
fn social_repulsion_zero_at_same_position() {
    let params = SocialForceParams::default();
    let (fx, fy) = social_repulsion_2d(0.0, 0.0, 0.0, 0.0, 0.25, 0.0, 0.0, 0.0, 0.0, 0.25, &params);
    assert!(fx.abs() < 1e-10);
    assert!(fy.abs() < 1e-10);
}

// ===========================================================================
// 2D wall repulsion
// ===========================================================================

#[test]
fn wall_repulsion_pushes_agent_away() {
    let params = SocialForceParams::default();
    let wall = WallSegment::new(0.0, 0.0, 10.0, 0.0);
    let (_, fy) = wall_repulsion_2d(5.0, 0.1, 0.0, 0.0, 0.25, &wall, &params);
    assert!(
        fy > 0.0,
        "wall repulsion should push agent in +y (away from wall)"
    );
}

#[test]
fn wall_repulsion_decays_with_distance() {
    let params = SocialForceParams::default();
    let wall = WallSegment::new(0.0, 0.0, 10.0, 0.0);
    let (_, fy_close) = wall_repulsion_2d(5.0, 0.1, 0.0, 0.0, 0.25, &wall, &params);
    let (_, fy_far) = wall_repulsion_2d(5.0, 3.0, 0.0, 0.0, 0.25, &wall, &params);
    assert!(
        fy_close > fy_far,
        "closer to wall should feel stronger force"
    );
}

#[test]
fn wall_contact_force_activates_on_overlap() {
    let params = SocialForceParams::default();
    let wall = WallSegment::new(0.0, 0.0, 10.0, 0.0);
    // Agent center at y=0.1, radius=0.25 ? overlapping wall by 0.15
    let (_, fy_overlap) = wall_repulsion_2d(5.0, 0.1, 0.0, 0.0, 0.25, &wall, &params);
    // Agent center at y=0.5, radius=0.25 ? not overlapping
    let (_, fy_no_overlap) = wall_repulsion_2d(5.0, 0.5, 0.0, 0.0, 0.25, &wall, &params);
    assert!(
        fy_overlap > fy_no_overlap,
        "overlapping wall should produce much stronger force"
    );
}

// ===========================================================================
// 2D integration
// ===========================================================================

#[test]
fn integration_advances_position() {
    let params = SocialForceParams::default();
    let (new_x, _, new_vx, _) = integrate_euler_2d(0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.1, &params);
    assert!((new_vx - 1.0).abs() < 1e-12);
    assert!((new_x - 0.1).abs() < 1e-12);
}

#[test]
fn integration_applies_force() {
    let params = SocialForceParams::default();
    let (_, _, new_vx, _) = integrate_euler_2d(0.0, 0.0, 0.0, 0.0, 10.0, 0.0, 0.1, &params);
    assert!((new_vx - 1.0).abs() < 1e-12, "v = 0 + 10 * 0.1 = 1.0");
}

#[test]
fn integration_clamps_speed_2d() {
    let params = SocialForceParams {
        max_speed: 2.0,
        ..Default::default()
    };
    let (_, _, new_vx, new_vy) = integrate_euler_2d(0.0, 0.0, 0.0, 0.0, 1e6, 0.0, 0.1, &params);
    let speed = (new_vx * new_vx + new_vy * new_vy).sqrt();
    assert!(
        (speed - 2.0).abs() < 1e-10,
        "speed ({}) should be clamped to max_speed (2.0)",
        speed
    );
}

// ===========================================================================
// 3D variants
// ===========================================================================

#[test]
fn desired_force_3d_direction() {
    let (fx, fy, fz) = desired_force_3d(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 10.0, 1.3, 0.5);
    assert!(fx.abs() < 1e-12);
    assert!(fy.abs() < 1e-12);
    assert!(fz > 0.0, "force should point in +z toward destination");
}

#[test]
fn social_repulsion_3d_pushes_apart() {
    let params = SocialForceParams::default();
    let (fx, fy, fz) = social_repulsion_3d(
        0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.25, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.25, &params,
    );
    assert!(fx.abs() < 1e-10);
    assert!(fy.abs() < 1e-10);
    assert!(fz > 0.0, "repulsion should push in +z");
}

#[test]
fn wall_repulsion_3d_pushes_away() {
    let params = SocialForceParams::default();
    let wall = WallSegment3D::new(0.0, 0.0, 0.0, 10.0, 0.0, 0.0);
    let (_, fy, _) = wall_repulsion_3d(5.0, 0.1, 0.0, 0.0, 0.0, 0.0, 0.25, &wall, &params);
    assert!(fy > 0.0, "wall repulsion should push agent in +y");
}

#[test]
fn integration_3d_clamps_speed() {
    let params = SocialForceParams {
        max_speed: 3.0,
        ..Default::default()
    };
    let (_, _, _, nvx, nvy, nvz) =
        integrate_euler_3d(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e6, 1e6, 1e6, 0.1, &params);
    let speed = (nvx * nvx + nvy * nvy + nvz * nvz).sqrt();
    assert!((speed - 3.0).abs() < 1e-10);
}

// ===========================================================================
// WallSegment geometry
// ===========================================================================

#[test]
fn wall_segment_closest_point_midpoint() {
    let wall = WallSegment::new(0.0, 0.0, 10.0, 0.0);
    let (cx, cy) = wall.closest_point(5.0, 3.0);
    assert!((cx - 5.0).abs() < 1e-12);
    assert!(cy.abs() < 1e-12);
}

#[test]
fn wall_segment_closest_point_clamps_to_start() {
    let wall = WallSegment::new(0.0, 0.0, 10.0, 0.0);
    let (cx, cy) = wall.closest_point(-5.0, 3.0);
    assert!(cx.abs() < 1e-12);
    assert!(cy.abs() < 1e-12);
}

#[test]
fn wall_segment_closest_point_clamps_to_end() {
    let wall = WallSegment::new(0.0, 0.0, 10.0, 0.0);
    let (cx, cy) = wall.closest_point(15.0, 3.0);
    assert!((cx - 10.0).abs() < 1e-12);
    assert!(cy.abs() < 1e-12);
}

#[test]
fn wall_segment_3d_closest_point_midpoint() {
    let wall = WallSegment3D::new(0.0, 0.0, 0.0, 0.0, 0.0, 10.0);
    let (cx, cy, cz) = wall.closest_point(3.0, 4.0, 5.0);
    assert!(cx.abs() < 1e-12);
    assert!(cy.abs() < 1e-12);
    assert!((cz - 5.0).abs() < 1e-12);
}

// ===========================================================================
// Default params
// ===========================================================================

#[test]
fn default_params_are_helbing_2000() {
    let p = SocialForceParams::default();
    assert!((p.tau - 0.5).abs() < 1e-12);
    assert!((p.a_social - 2000.0).abs() < 1e-6);
    assert!((p.b_social - 0.08).abs() < 1e-12);
    assert!((p.k_body - 1.2e5).abs() < 1e-6);
    assert!((p.kappa_friction - 2.4e5).abs() < 1e-6);
    assert!((p.max_speed - 2.5).abs() < 1e-12);
}

// ===========================================================================
// Randomized invariants
// ===========================================================================

#[test]
fn social_repulsion_is_symmetric_in_magnitude() {
    let params = SocialForceParams::default();
    // Force on A from B
    let (fx_ab, fy_ab) = social_repulsion_2d(
        1.0, 0.5, 0.3, -0.2, 0.25, 0.0, 0.0, -0.1, 0.4, 0.25, &params,
    );
    // Force on B from A
    let (fx_ba, fy_ba) = social_repulsion_2d(
        0.0, 0.0, -0.1, 0.4, 0.25, 1.0, 0.5, 0.3, -0.2, 0.25, &params,
    );
    // Newton's third law (approximately, friction breaks exact symmetry but
    // the normal components should be equal and opposite)
    // Check that forces point in opposite directions
    let dot = fx_ab * fx_ba + fy_ab * fy_ba;
    assert!(dot < 0.0, "forces should be roughly opposite directions");
}

#[test]
fn repulsion_force_is_always_positive_radial() {
    use rand::rngs::StdRng;
    use rand::{Rng, SeedableRng};

    let params = SocialForceParams::default();
    let mut rng = StdRng::seed_from_u64(77);

    for _ in 0..200 {
        let x: f64 = rng.gen_range(-10.0..10.0);
        let y: f64 = rng.gen_range(-10.0..10.0);
        let nx: f64 = rng.gen_range(-10.0..10.0);
        let ny: f64 = rng.gen_range(-10.0..10.0);

        let dx = x - nx;
        let dy = y - ny;
        let dist = (dx * dx + dy * dy).sqrt();
        if dist < 1e-6 {
            continue;
        }

        let (fx, fy) = social_repulsion_2d(x, y, 0.0, 0.0, 0.25, nx, ny, 0.0, 0.0, 0.25, &params);

        // The force's projection onto the separation vector should be positive
        // (repulsive, pointing away from neighbor) when velocities are zero.
        let radial = fx * dx / dist + fy * dy / dist;
        assert!(
            radial >= -1e-6,
            "radial force should be non-negative (repulsive); got {} at dist={}",
            radial,
            dist
        );
    }
}