subtr-actor 0.5.0

use super::*;
use boxcars::Quaternion;
use boxcars::Vector3f;

fn sample_rigid_body(
    x: f32,
    y: f32,
    z: f32,
    velocity_x: f32,
    velocity_y: f32,
    velocity_z: f32,
) -> boxcars::RigidBody {
    boxcars::RigidBody {
        sleeping: false,
        location: Vector3f { x, y, z },
        rotation: Quaternion {
            x: 0.0,
            y: 0.0,
            z: 0.0,
            w: 1.0,
        },
        linear_velocity: Some(Vector3f {
            x: velocity_x,
            y: velocity_y,
            z: velocity_z,
        }),
        angular_velocity: Some(Vector3f {
            x: 0.0,
            y: 0.0,
            z: 0.0,
        }),
    }
}

#[test]
fn test_get_interpolated_rigid_body() {
    let start_body = boxcars::RigidBody {
        sleeping: false,
        location: Vector3f {
            x: 0.0,
            y: 0.0,
            z: 0.0,
        },
        rotation: Quaternion {
            x: 0.0,
            y: 0.0,
            z: 0.0,
            w: 1.0,
        },
        linear_velocity: Some(Vector3f {
            x: 0.0,
            y: 0.0,
            z: 0.0,
        }),
        angular_velocity: Some(Vector3f {
            x: 0.0,
            y: 0.0,
            z: 0.0,
        }),
    };
    let end_body = boxcars::RigidBody {
        sleeping: true,
        location: Vector3f {
            x: 1.0,
            y: 1.0,
            z: 1.0,
        },
        rotation: Quaternion {
            x: 0.0,
            y: 0.0,
            z: 1.0,
            w: 0.0,
        },
        linear_velocity: Some(Vector3f {
            x: 1.0,
            y: 1.0,
            z: 1.0,
        }),
        angular_velocity: Some(Vector3f {
            x: 1.0,
            y: 1.0,
            z: 1.0,
        }),
    };
    let start_time = 0.0f32;
    let end_time = 1.0f32;
    let time = 0.5f32;

    let result = get_interpolated_rigid_body(&start_body, start_time, &end_body, end_time, time);

    match result {
        Ok(interpolated_body) => {
            assert_eq!(interpolated_body.location.x, 0.5);
            assert_eq!(interpolated_body.location.y, 0.5);
            assert_eq!(interpolated_body.location.z, 0.5);
            // Add further assertions for rotation, linear_velocity and angular_velocity as needed.
        }
        Err(e) => panic!("Interpolation failed: {e:?}"),
    };
}

#[test]
fn test_find_update_in_direction() {
    let items = vec![1, 2, 3, 4, 5, 6, 7, 8, 9, 10];
    let current_index = 4; // Starting search from number 5
    let predicate = |&x: &i32| if x % 2 == 0 { Some(x) } else { None }; // Looking for the first even number

    // Test forward search.
    let result_forward =
        find_in_direction(&items, current_index, SearchDirection::Forward, predicate);
    // Check that the result is as expected.
    assert_eq!(result_forward, Some((5, 6))); // First even number after index 4 is 6 at index 5

    // Test backward search.
    let result_backward =
        find_in_direction(&items, current_index, SearchDirection::Backward, predicate);
    // Check that the result is as expected.
    assert_eq!(result_backward, Some((3, 4))); // First even number before index 4 is 4 at index 3
}

#[test]
fn test_touch_candidate_rank_prefers_recent_closest_approach() {
    let ball = sample_rigid_body(0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
    let near_but_static = sample_rigid_body(120.0, 0.0, 0.0, 0.0, 0.0, 0.0);
    let slightly_farther_but_recently_closer = sample_rigid_body(180.0, 0.0, 0.0, 1500.0, 0.0, 0.0);

    let near_rank = touch_candidate_rank(&ball, &near_but_static).unwrap();
    let recent_rank = touch_candidate_rank(&ball, &slightly_farther_but_recently_closer).unwrap();

    assert!(
        recent_rank < near_rank,
        "expected backtracked closest approach to outrank pure current distance: {recent_rank:?} !< {near_rank:?}"
    );
}

#[test]
fn test_touch_candidate_rank_penalizes_unreachable_far_candidates() {
    let ball = sample_rigid_body(0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
    let close_candidate = sample_rigid_body(200.0, 0.0, 0.0, 0.0, 0.0, 0.0);
    let far_candidate = sample_rigid_body(1200.0, 0.0, 0.0, 1000.0, 0.0, 0.0);

    let close_rank = touch_candidate_rank(&ball, &close_candidate).unwrap();
    let far_rank = touch_candidate_rank(&ball, &far_candidate).unwrap();

    assert!(
        close_rank < far_rank,
        "expected a far candidate outside the short contact window to rank worse: {close_rank:?} !< {far_rank:?}"
    );
}

#[test]
fn test_flip_reset_candidate_detects_airborne_underside_touch() {
    let ball = sample_rigid_body(0.0, 0.0, 6.0, 0.0, 0.0, 0.0);
    let player = sample_rigid_body(0.0, 0.0, 8.5, 0.0, 0.0, 0.0);

    let heuristic = mechanics::flip_reset::flip_reset_candidate(&ball, &player, 1.2)
        .expect("expected underside aerial contact to qualify as a flip-reset candidate");

    assert!(heuristic.confidence > 0.5);
    assert!(heuristic.local_ball_position.z < 0.0);
}

#[test]
fn test_flip_reset_candidate_rejects_front_bumper_like_touch() {
    let ball = sample_rigid_body(7.0, 0.0, 8.5, 0.0, 0.0, 0.0);
    let player = sample_rigid_body(0.0, 0.0, 8.5, 0.0, 0.0, 0.0);

    assert!(
        mechanics::flip_reset::flip_reset_candidate(&ball, &player, 1.2).is_none(),
        "expected front-facing touch geometry to be rejected"
    );
}

#[test]
fn test_flip_reset_candidate_rejects_low_ground_touch() {
    let ball = sample_rigid_body(0.0, 0.0, 1.2, 0.0, 0.0, 0.0);
    let player = sample_rigid_body(0.0, 0.0, 0.2, 0.0, 0.0, 0.0);

    assert!(
        mechanics::flip_reset::flip_reset_candidate(&ball, &player, 1.2).is_none(),
        "expected grounded touch geometry to be rejected"
    );
}