"""AZ-918 — `mavlink_imu_to_si_flu` unit + frame conversion.

Pins the conversion contract so any future change to either the
constant or the body-frame transform is a deliberate, reviewed edit.

The helper sits between every MAVLink-IMU adapter and the
``nav.ImuSample`` / ``fc.ImuTelemetrySample`` boundary, so a silent
regression here would break C5 ESKF and FDR consumers simultaneously.
"""

from __future__ import annotations

import math

import pytest

from gps_denied_onboard.helpers.imu_units import (
    MG_TO_M_S2,
    MRAD_S_TO_RAD_S,
    mavlink_imu_to_si_flu,
)


def test_constants_match_si_definition() -> None:
    # Assert
    assert MG_TO_M_S2 == pytest.approx(9.80665e-3)
    assert MRAD_S_TO_RAD_S == pytest.approx(1.0e-3)


def test_stationary_frd_body_z_down_becomes_flu_body_z_up_one_g() -> None:
    # Arrange — Stationary, level UAV: gravity vector in MAVLink FRD
    # body frame points in +Z (down), so the measured specific force
    # is -gravity = -Z. Magnitude is exactly one standard gravity.
    raw_z_down_mg = -1000.0

    # Act
    accel_si_flu, gyro_si_flu = mavlink_imu_to_si_flu(
        xacc=0.0,
        yacc=0.0,
        zacc=raw_z_down_mg,
        xgyro=0.0,
        ygyro=0.0,
        zgyro=0.0,
    )

    # Assert — In ESKF/preintegrator FLU body the same specific force
    # should appear as +Z = +9.80665 m/s² (matching the C5 ESKF unit
    # test's stationary vector).
    assert accel_si_flu == pytest.approx((0.0, 0.0, 9.80665))
    assert gyro_si_flu == pytest.approx((0.0, 0.0, 0.0))


def test_frd_to_flu_negates_y_and_z_keeps_x() -> None:
    # Arrange — distinguishable components per axis.
    # Act
    accel_si_flu, gyro_si_flu = mavlink_imu_to_si_flu(
        xacc=100.0, yacc=200.0, zacc=-300.0,
        xgyro=10.0, ygyro=20.0, zgyro=-30.0,
    )

    # Assert — X stays positive; Y and Z flip sign per the FRD→FLU
    # body-frame transform.
    assert accel_si_flu == pytest.approx((
        +100.0 * MG_TO_M_S2,
        -200.0 * MG_TO_M_S2,
        +300.0 * MG_TO_M_S2,
    ))
    assert gyro_si_flu == pytest.approx((
        +10.0 * MRAD_S_TO_RAD_S,
        -20.0 * MRAD_S_TO_RAD_S,
        +30.0 * MRAD_S_TO_RAD_S,
    ))


def test_unit_magnitudes_match_first_csv_row_of_derkachi_fixture() -> None:
    # Arrange — CSV row 0 of `data_imu.csv` (Derkachi fixture). UAV is
    # in slow descent at ~0.88 m/s, near level, so |accel| should be
    # close to 1 g.
    raw_xacc, raw_yacc, raw_zacc = 21.0, -3.0, -984.0

    # Act
    accel_si_flu, _ = mavlink_imu_to_si_flu(
        xacc=raw_xacc, yacc=raw_yacc, zacc=raw_zacc,
        xgyro=0.0, ygyro=0.0, zgyro=0.0,
    )

    # Assert — magnitude is within 5% of one standard gravity (the
    # body is descending, not perfectly stationary, so a tight bound
    # is wrong; this test pins the order-of-magnitude only).
    mag = math.sqrt(sum(c * c for c in accel_si_flu))
    assert 9.30 < mag < 10.30, f"|a|={mag:.3f} m/s² is not near 1 g"


def test_zero_input_returns_zero_output() -> None:
    # Act
    accel, gyro = mavlink_imu_to_si_flu(
        xacc=0.0, yacc=0.0, zacc=0.0,
        xgyro=0.0, ygyro=0.0, zgyro=0.0,
    )

    # Assert
    assert accel == (0.0, 0.0, 0.0)
    assert gyro == (0.0, 0.0, 0.0)