//! Conformance tests for `transform-core` TransformBuffer.
use std::f64::consts::FRAC_1_SQRT_2;
use transform_core::{Quat, StampedTransform, TfError, Transform, TransformBuffer, Vec3};

// ---------------------------------------------------------------------------
// Helpers
// ---------------------------------------------------------------------------

fn approx_eq_f64(a: f64, b: f64, eps: f64) -> bool {
    (a - b).abs() < eps
}

fn vec3_approx_eq(a: &Vec3, b: &Vec3, eps: f64) -> bool {
    approx_eq_f64(a.x, b.x, eps) && approx_eq_f64(a.y, b.y, eps) && approx_eq_f64(a.z, b.z, eps)
}

fn quat_approx_eq(a: &Quat, b: &Quat, eps: f64) -> bool {
    // Quaternions q and -q represent the same rotation.
    let dot = a.dot(b);
    approx_eq_f64(dot.abs(), 1.0, eps)
}

const EPS: f64 = 1e-9;

// ---------------------------------------------------------------------------
// broadcast + lookup exact timestamp
// ---------------------------------------------------------------------------

#[test]
fn broadcast_and_lookup_exact_timestamp() {
    let mut buf = TransformBuffer::new();
    let tf = Transform::translation(3.0, 4.0, 5.0);
    buf.broadcast("world", "sensor", tf.clone(), 1_000);
    let got = buf.lookup_transform("world", "sensor", 1_000).unwrap();
    assert!(vec3_approx_eq(&got.position, &tf.position, EPS));
}

#[test]
fn lookup_at_min_boundary_is_exact_match() {
    let mut buf = TransformBuffer::new();
    buf.broadcast("a", "b", Transform::translation(0.0, 0.0, 0.0), 100);
    buf.broadcast("a", "b", Transform::translation(1.0, 0.0, 0.0), 200);
    let got = buf.lookup_transform("a", "b", 100).unwrap();
    assert!(approx_eq_f64(got.position.x, 0.0, EPS));
}

#[test]
fn lookup_at_max_boundary_is_exact_match() {
    let mut buf = TransformBuffer::new();
    buf.broadcast("a", "b", Transform::translation(0.0, 0.0, 0.0), 100);
    buf.broadcast("a", "b", Transform::translation(1.0, 0.0, 0.0), 200);
    let got = buf.lookup_transform("a", "b", 200).unwrap();
    assert!(approx_eq_f64(got.position.x, 1.0, EPS));
}

// ---------------------------------------------------------------------------
// Interpolation
// ---------------------------------------------------------------------------

#[test]
fn lookup_interpolates_position_linearly() {
    let mut buf = TransformBuffer::new();
    buf.broadcast("world", "robot", Transform::translation(0.0, 0.0, 0.0), 0);
    buf.broadcast(
        "world",
        "robot",
        Transform::translation(1.0, 2.0, 3.0),
        1_000_000_000,
    );
    // Midpoint → half translation
    let got = buf.lookup_transform("world", "robot", 500_000_000).unwrap();
    assert!(
        approx_eq_f64(got.position.x, 0.5, EPS),
        "x={}",
        got.position.x
    );
    assert!(
        approx_eq_f64(got.position.y, 1.0, EPS),
        "y={}",
        got.position.y
    );
    assert!(
        approx_eq_f64(got.position.z, 1.5, EPS),
        "z={}",
        got.position.z
    );
}

#[test]
fn lookup_at_quarter_time_is_quarter_position() {
    let mut buf = TransformBuffer::new();
    buf.broadcast("w", "r", Transform::translation(0.0, 0.0, 0.0), 0);
    buf.broadcast("w", "r", Transform::translation(4.0, 0.0, 0.0), 1_000);
    let got = buf.lookup_transform("w", "r", 250).unwrap();
    assert!(
        approx_eq_f64(got.position.x, 1.0, EPS),
        "x={}",
        got.position.x
    );
}

// ---------------------------------------------------------------------------
// SLERP rotation
// ---------------------------------------------------------------------------

#[test]
fn slerp_at_t0_returns_start_rotation() {
    let q0 = Quat::identity();
    let q1 = Quat::new(0.0, 0.0, FRAC_1_SQRT_2, FRAC_1_SQRT_2); // 90° Z
    let result = q0.slerp(&q1, 0.0);
    assert!(quat_approx_eq(&result, &q0, 1e-7), "got {result:?}");
}

#[test]
fn slerp_at_t1_returns_end_rotation() {
    let q0 = Quat::identity();
    let q1 = Quat::new(0.0, 0.0, FRAC_1_SQRT_2, FRAC_1_SQRT_2); // 90° Z
    let result = q0.slerp(&q1, 1.0);
    assert!(quat_approx_eq(&result, &q1, 1e-7), "got {result:?}");
}

#[test]
fn slerp_halfway_between_identity_and_90_deg_is_45_deg() {
    // Identity to 90° around Z → midpoint should be 45°
    let q0 = Quat::identity();
    let q1 = Quat::new(0.0, 0.0, FRAC_1_SQRT_2, FRAC_1_SQRT_2); // 90° Z
    let midpoint = q0.slerp(&q1, 0.5);
    // 45° around Z: qz = sin(22.5°) = 0.3827, qw = cos(22.5°) = 0.9239
    let expected = Quat::new(0.0, 0.0, 0.3826834323650898, 0.9238795325112867);
    assert!(
        quat_approx_eq(&midpoint, &expected, 1e-7),
        "got {midpoint:?}"
    );
}

#[test]
fn interpolate_transform_blends_position_and_rotation() {
    let tf0 = Transform {
        position: Vec3::new(0.0, 0.0, 0.0),
        rotation: Quat::identity(),
    };
    let tf1 = Transform {
        position: Vec3::new(2.0, 0.0, 0.0),
        rotation: Quat::new(0.0, 0.0, FRAC_1_SQRT_2, FRAC_1_SQRT_2),
    };
    let mid = tf0.interpolate(&tf1, 0.5);
    assert!(approx_eq_f64(mid.position.x, 1.0, EPS));
    // rotation should be 45° around Z
    let expected_rot = Quat::new(0.0, 0.0, 0.3826834323650898, 0.9238795325112867);
    assert!(quat_approx_eq(&mid.rotation, &expected_rot, 1e-7));
}

// ---------------------------------------------------------------------------
// Error cases
// ---------------------------------------------------------------------------

#[test]
fn lookup_unknown_frame_pair_returns_frame_pair_not_found() {
    let buf = TransformBuffer::new();
    let err = buf.lookup_transform("world", "ghost", 0).unwrap_err();
    assert_eq!(
        err,
        TfError::FramePairNotFound {
            frame_from: "world".into(),
            frame_to: "ghost".into()
        }
    );
}

#[test]
fn lookup_before_min_returns_extrapolation_error() {
    let mut buf = TransformBuffer::new();
    buf.broadcast("a", "b", Transform::identity(), 1_000);
    buf.broadcast("a", "b", Transform::translation(1.0, 0.0, 0.0), 2_000);
    let err = buf.lookup_transform("a", "b", 500).unwrap_err();
    assert!(
        matches!(
            err,
            TfError::ExtrapolationError {
                requested_ns: 500,
                min_ns: 1_000,
                ..
            }
        ),
        "got {err}"
    );
}

#[test]
fn lookup_after_max_returns_extrapolation_error() {
    let mut buf = TransformBuffer::new();
    buf.broadcast("a", "b", Transform::identity(), 1_000);
    buf.broadcast("a", "b", Transform::translation(1.0, 0.0, 0.0), 2_000);
    let err = buf.lookup_transform("a", "b", 9_999).unwrap_err();
    assert!(
        matches!(
            err,
            TfError::ExtrapolationError {
                requested_ns: 9_999,
                max_ns: 2_000,
                ..
            }
        ),
        "got {err}"
    );
}

// ---------------------------------------------------------------------------
// History trimming
// ---------------------------------------------------------------------------

#[test]
fn buffer_trims_oldest_entries_at_max_history() {
    let mut buf = TransformBuffer::with_max_history(3);
    for i in 0u64..5 {
        buf.broadcast(
            "w",
            "r",
            Transform::translation(i as f64, 0.0, 0.0),
            i * 1_000,
        );
    }
    // Only the 3 most recent entries should remain: t=2000, 3000, 4000
    assert_eq!(buf.entry_count("w", "r"), 3);
    // Oldest kept entry is at t=2000
    let err = buf.lookup_transform("w", "r", 0).unwrap_err();
    assert!(
        matches!(err, TfError::ExtrapolationError { .. }),
        "got {err}"
    );
    // Latest entry should still be accessible
    let got = buf.lookup_transform("w", "r", 4_000).unwrap();
    assert!(approx_eq_f64(got.position.x, 4.0, EPS));
}

// ---------------------------------------------------------------------------
// Out-of-order broadcast
// ---------------------------------------------------------------------------

#[test]
fn broadcast_out_of_order_timestamps_are_sorted() {
    let mut buf = TransformBuffer::new();
    buf.broadcast("w", "r", Transform::translation(2.0, 0.0, 0.0), 2_000);
    buf.broadcast("w", "r", Transform::translation(0.0, 0.0, 0.0), 0);
    buf.broadcast("w", "r", Transform::translation(1.0, 0.0, 0.0), 1_000);
    // Lookup midpoints
    let got = buf.lookup_transform("w", "r", 500).unwrap();
    assert!(
        approx_eq_f64(got.position.x, 0.5, EPS),
        "x={}",
        got.position.x
    );
}

// ---------------------------------------------------------------------------
// Overwrite existing timestamp
// ---------------------------------------------------------------------------

#[test]
fn broadcast_same_timestamp_overwrites_entry() {
    let mut buf = TransformBuffer::new();
    buf.broadcast("w", "r", Transform::translation(1.0, 0.0, 0.0), 1_000);
    buf.broadcast("w", "r", Transform::translation(99.0, 0.0, 0.0), 1_000);
    let got = buf.lookup_transform("w", "r", 1_000).unwrap();
    assert!(approx_eq_f64(got.position.x, 99.0, EPS));
    assert_eq!(
        buf.entry_count("w", "r"),
        1,
        "duplicate entry was not overwritten"
    );
}

// ---------------------------------------------------------------------------
// Multiple independent frame pairs
// ---------------------------------------------------------------------------

#[test]
fn multiple_frame_pairs_are_independent() {
    let mut buf = TransformBuffer::new();
    buf.broadcast("world", "robot_a", Transform::translation(1.0, 0.0, 0.0), 0);
    buf.broadcast("world", "robot_b", Transform::translation(0.0, 2.0, 0.0), 0);

    let a = buf.lookup_transform("world", "robot_a", 0).unwrap();
    let b = buf.lookup_transform("world", "robot_b", 0).unwrap();
    assert!(approx_eq_f64(a.position.x, 1.0, EPS));
    assert!(approx_eq_f64(b.position.y, 2.0, EPS));
    assert_eq!(buf.frame_pair_count(), 2);
}

// ---------------------------------------------------------------------------
// Serde round-trip
// ---------------------------------------------------------------------------

#[test]
fn transform_json_roundtrip() {
    let tf = Transform {
        position: Vec3::new(1.0, 2.0, 3.0),
        rotation: Quat::new(0.0, 0.0, FRAC_1_SQRT_2, FRAC_1_SQRT_2),
    };
    let json = serde_json::to_string(&tf).unwrap();
    let back: Transform = serde_json::from_str(&json).unwrap();
    assert!(vec3_approx_eq(&tf.position, &back.position, EPS));
    assert!(quat_approx_eq(&tf.rotation, &back.rotation, 1e-9));
}

// ---------------------------------------------------------------------------
// Vec3 arithmetic
// ---------------------------------------------------------------------------

#[test]
fn vec3_add_sub_scale_neg() {
    let a = Vec3::new(1.0, 2.0, 3.0);
    let b = Vec3::new(4.0, 5.0, 6.0);
    let sum = a.add(&b);
    assert!(vec3_approx_eq(&sum, &Vec3::new(5.0, 7.0, 9.0), EPS));
    let diff = b.sub(&a);
    assert!(vec3_approx_eq(&diff, &Vec3::new(3.0, 3.0, 3.0), EPS));
    let scaled = a.scale(2.0);
    assert!(vec3_approx_eq(&scaled, &Vec3::new(2.0, 4.0, 6.0), EPS));
    let neg = a.neg();
    assert!(vec3_approx_eq(&neg, &Vec3::new(-1.0, -2.0, -3.0), EPS));
}

#[test]
fn vec3_dot_and_length() {
    let a = Vec3::new(3.0, 4.0, 0.0);
    assert!(approx_eq_f64(a.dot(&a), 25.0, EPS));
    assert!(approx_eq_f64(a.length(), 5.0, EPS));
}

// ---------------------------------------------------------------------------
// Quat arithmetic
// ---------------------------------------------------------------------------

#[test]
fn quat_conjugate_of_identity_is_identity() {
    let q = Quat::identity();
    let c = q.conjugate();
    assert!(quat_approx_eq(&c, &Quat::identity(), EPS));
}

#[test]
fn quat_mul_identity_is_noop() {
    let q = Quat::new(0.0, FRAC_1_SQRT_2, 0.0, FRAC_1_SQRT_2).normalize();
    let result = q.mul(&Quat::identity());
    assert!(quat_approx_eq(&result, &q, 1e-9));
}

#[test]
fn quat_rotate_vec_90_deg_around_z() {
    // 90° rotation around +Z maps +X to +Y.
    let s = std::f64::consts::FRAC_PI_4.sin();
    let c = std::f64::consts::FRAC_PI_4.cos();
    let q = Quat::new(0.0, 0.0, s, c); // 90° around Z
    let v = Vec3::new(1.0, 0.0, 0.0);
    let rotated = q.rotate_vec(&v);
    assert!(vec3_approx_eq(&rotated, &Vec3::new(0.0, 1.0, 0.0), 1e-9));
}

// ---------------------------------------------------------------------------
// Transform::inverse and compose
// ---------------------------------------------------------------------------

#[test]
fn transform_inverse_compose_is_identity() {
    let tf = Transform {
        position: Vec3::new(1.0, 2.0, 3.0),
        rotation: Quat::identity(),
    };
    let composed = tf.compose(&tf.inverse());
    assert!(vec3_approx_eq(
        &composed.position,
        &Vec3::new(0.0, 0.0, 0.0),
        1e-9
    ));
    assert!(quat_approx_eq(&composed.rotation, &Quat::identity(), 1e-9));
}

#[test]
fn transform_compose_translation_only() {
    let t1 = Transform::translation(1.0, 0.0, 0.0);
    let t2 = Transform::translation(2.0, 0.0, 0.0);
    let combined = t1.compose(&t2);
    assert!(vec3_approx_eq(
        &combined.position,
        &Vec3::new(3.0, 0.0, 0.0),
        1e-9
    ));
}

// ---------------------------------------------------------------------------
// StampedTransform
// ---------------------------------------------------------------------------

#[test]
fn stamped_transform_fields_are_set() {
    let tf = Transform::translation(1.0, 2.0, 3.0);
    let stamped = StampedTransform::new("world", "robot", 1000, tf.clone());
    assert_eq!(stamped.frame_from, "world");
    assert_eq!(stamped.frame_to, "robot");
    assert_eq!(stamped.timestamp_ns, 1000);
    assert!(vec3_approx_eq(
        &stamped.transform.position,
        &tf.position,
        EPS
    ));
}

// ---------------------------------------------------------------------------
// broadcast_stamped
// ---------------------------------------------------------------------------

#[test]
fn broadcast_stamped_stores_entry() {
    let mut buf = TransformBuffer::new();
    let tf = Transform::translation(5.0, 0.0, 0.0);
    buf.broadcast_stamped(StampedTransform::new("a", "b", 100, tf));
    let result = buf.lookup_transform("a", "b", 100).unwrap();
    assert!(approx_eq_f64(result.position.x, 5.0, EPS));
}

// ---------------------------------------------------------------------------
// register_static fallback
// ---------------------------------------------------------------------------

#[test]
fn static_transform_returned_when_no_dynamic_data() {
    let mut buf = TransformBuffer::new();
    buf.register_static("base", "sensor", Transform::translation(0.1, 0.0, 0.0));
    let tf = buf.lookup_transform("base", "sensor", 9999).unwrap();
    assert!(approx_eq_f64(tf.position.x, 0.1, EPS));
}

#[test]
fn dynamic_data_returned_over_static() {
    let mut buf = TransformBuffer::new();
    buf.register_static("base", "sensor", Transform::translation(0.1, 0.0, 0.0));
    buf.broadcast("base", "sensor", Transform::translation(5.0, 0.0, 0.0), 100);
    let tf = buf.lookup_transform("base", "sensor", 100).unwrap();
    assert!(approx_eq_f64(tf.position.x, 5.0, EPS));
}

// ---------------------------------------------------------------------------
// lookup_transform_or_invert
// ---------------------------------------------------------------------------

#[test]
fn invert_fallback_used_when_direct_missing() {
    let mut buf = TransformBuffer::new();
    // Store b→a; lookup a→b should invert.
    buf.broadcast("b", "a", Transform::translation(3.0, 0.0, 0.0), 0);
    let tf = buf.lookup_transform_or_invert("a", "b", 0).unwrap();
    assert!(approx_eq_f64(tf.position.x, -3.0, 1e-9));
}

// ---------------------------------------------------------------------------
// lookup_transform_chain
// ---------------------------------------------------------------------------

#[test]
fn chain_lookup_composes_transforms() {
    let mut buf = TransformBuffer::new();
    buf.broadcast("a", "b", Transform::translation(1.0, 0.0, 0.0), 0);
    buf.broadcast("b", "c", Transform::translation(2.0, 0.0, 0.0), 0);
    let tf = buf.lookup_transform_chain(&["a", "b", "c"], 0).unwrap();
    assert!(approx_eq_f64(tf.position.x, 3.0, 1e-9));
}

#[test]
fn chain_lookup_with_fewer_than_2_frames_returns_invalid_argument() {
    let buf = TransformBuffer::new();
    let err = buf.lookup_transform_chain(&["a"], 0).unwrap_err();
    assert!(matches!(err, TfError::InvalidArgument(_)));
}

// ---------------------------------------------------------------------------
// clear_before and gc_before
// ---------------------------------------------------------------------------

#[test]
fn clear_before_removes_old_entries_for_pair() {
    let mut buf = TransformBuffer::new();
    buf.broadcast("a", "b", Transform::translation(1.0, 0.0, 0.0), 100);
    buf.broadcast("a", "b", Transform::translation(2.0, 0.0, 0.0), 200);
    buf.broadcast("a", "b", Transform::translation(3.0, 0.0, 0.0), 300);
    buf.clear_before("a", "b", 200);
    assert_eq!(buf.entry_count("a", "b"), 2); // ts=200 and ts=300 remain
}

#[test]
fn gc_before_prunes_all_pairs() {
    let mut buf = TransformBuffer::new();
    buf.broadcast("a", "b", Transform::translation(1.0, 0.0, 0.0), 50);
    buf.broadcast("a", "b", Transform::translation(2.0, 0.0, 0.0), 150);
    buf.broadcast("x", "y", Transform::translation(0.0, 0.0, 0.0), 50);
    buf.gc_before(100);
    assert_eq!(buf.entry_count("a", "b"), 1); // only ts=150 remains
    assert_eq!(buf.frame_pair_count(), 1); // x→y fully pruned, removed from map
}

// ---------------------------------------------------------------------------
// Static + dynamic mixed chain lookup
// ---------------------------------------------------------------------------

#[test]
fn chain_uses_static_hop_when_dynamic_pair_missing() {
    let mut buf = TransformBuffer::new();
    // a→b is dynamic, b→c is static.
    buf.broadcast("a", "b", Transform::translation(1.0, 0.0, 0.0), 0);
    buf.register_static("b", "c", Transform::translation(10.0, 0.0, 0.0));
    let tf = buf.lookup_transform_chain(&["a", "b", "c"], 0).unwrap();
    // The buffer looks up each hop individually; static fallback means b→c is found.
    assert!(
        approx_eq_f64(tf.position.x, 11.0, 1e-9),
        "x={}",
        tf.position.x
    );
}

// ---------------------------------------------------------------------------
// Quat slerp — antipodal quaternions take shortest arc
// ---------------------------------------------------------------------------

#[test]
fn slerp_antipodal_takes_shortest_path() {
    // q and -q represent the same rotation; slerp should still converge.
    let q0 = Quat::identity();
    // 90° around Z, expressed via its antipode (negated components).
    let q1_neg = Quat::new(0.0, 0.0, -FRAC_1_SQRT_2, -FRAC_1_SQRT_2);
    // With shortest-path correction, t=0.5 should be the same 45° result as
    // using the positive representative.
    let mid = q0.slerp(&q1_neg, 0.5);
    let expected = Quat::new(0.0, 0.0, 0.3826834323650898, 0.9238795325112867);
    assert!(
        quat_approx_eq(&mid, &expected, 1e-6),
        "slerp should use shortest arc; got {mid:?}"
    );
}

// ---------------------------------------------------------------------------
// Quat slerp — nearly-identical quaternions use linear fallback
// ---------------------------------------------------------------------------

#[test]
fn slerp_nearly_identical_quaternions_returns_normalized_lerp() {
    // Two quaternions that differ by a tiny epsilon; acos(dot) ≈ 0 branch.
    let q0 = Quat::identity();
    let q1 = Quat::new(1e-9, 0.0, 0.0, 1.0).normalize(); // essentially identity
    let result = q0.slerp(&q1, 0.5);
    assert!(
        quat_approx_eq(&result, &Quat::identity(), 1e-6),
        "got {result:?}"
    );
}
