"""
test_bno055_data.py — Issue #135: BNO055 driver unit tests.

Tests data scaling, byte parsing, calibration status extraction,
calibration offset packing/unpacking, and temperature handling.

No HAL or ESP32-S3 hardware required — pure Python logic.
"""

import struct
import pytest


# ---------------------------------------------------------------------------
# BNO055 scaling constants (mirror bno055.c)
# ---------------------------------------------------------------------------

EULER_SCALE = 16.0   # 1 degree = 16 LSB
GYRO_SCALE  = 16.0   # 1 dps   = 16 LSB
ACCEL_SCALE = 100.0  # 1 m/s²  = 100 LSB
G_MS2       = 9.80665


# ---------------------------------------------------------------------------
# Calibration status bit extraction
# ---------------------------------------------------------------------------

BNO055_CAL_SYS_MASK = 0xC0
BNO055_CAL_GYR_MASK = 0x30
BNO055_CAL_ACC_MASK = 0x0C
BNO055_CAL_MAG_MASK = 0x03


def parse_calib_status(byte):
    """Return (sys, gyr, acc, mag) calibration levels 0-3."""
    sys = (byte & BNO055_CAL_SYS_MASK) >> 6
    gyr = (byte & BNO055_CAL_GYR_MASK) >> 4
    acc = (byte & BNO055_CAL_ACC_MASK) >> 2
    mag = (byte & BNO055_CAL_MAG_MASK) >> 0
    return sys, gyr, acc, mag


# ---------------------------------------------------------------------------
# Offset packing (mirrors bno055_save_offsets / bno055_restore_offsets)
# ---------------------------------------------------------------------------

BNO055_BKP_MAGIC = 0xB055CA10


def pack_offsets(offsets22: bytes):
    """Pack 22 bytes into 6 x uint32 words + magic word (7 words total)."""
    assert len(offsets22) == 22
    words = []
    for i in range(5):
        w = (offsets22[i * 4 + 0]
             | (offsets22[i * 4 + 1] << 8)
             | (offsets22[i * 4 + 2] << 16)
             | (offsets22[i * 4 + 3] << 24))
        words.append(w & 0xFFFFFFFF)
    # Last word: only bytes 20 and 21
    w5 = offsets22[20] | (offsets22[21] << 8)
    words.append(w5 & 0xFFFFFFFF)
    words.append(BNO055_BKP_MAGIC)
    return words  # 7 uint32 words


def unpack_offsets(words):
    """Reverse of pack_offsets. Returns 22 bytes."""
    assert len(words) == 7
    assert words[6] == BNO055_BKP_MAGIC
    result = []
    for i in range(5):
        w = words[i]
        result.append(w & 0xFF)
        result.append((w >> 8) & 0xFF)
        result.append((w >> 16) & 0xFF)
        result.append((w >> 24) & 0xFF)
    # Last word: only bytes 20 and 21
    result.append(words[5] & 0xFF)
    result.append((words[5] >> 8) & 0xFF)
    return bytes(result)


# ---------------------------------------------------------------------------
# IMUData parsing helpers (mirror bno055_read() logic)
# ---------------------------------------------------------------------------

def parse_euler(buf6: bytes):
    """Parse 6-byte Euler burst (H, R, P each int16 LE) → (heading, roll, pitch) in degrees."""
    h_raw = struct.unpack_from("<h", buf6, 0)[0]
    r_raw = struct.unpack_from("<h", buf6, 2)[0]
    p_raw = struct.unpack_from("<h", buf6, 4)[0]
    return h_raw / EULER_SCALE, r_raw / EULER_SCALE, p_raw / EULER_SCALE


def parse_gyro(buf6: bytes):
    """Parse 6-byte gyro burst (Gx, Gy, Gz each int16 LE) → (gx, gy, gz) in dps."""
    gx = struct.unpack_from("<h", buf6, 0)[0] / GYRO_SCALE
    gy = struct.unpack_from("<h", buf6, 2)[0] / GYRO_SCALE
    gz = struct.unpack_from("<h", buf6, 4)[0] / GYRO_SCALE
    return gx, gy, gz


def parse_lia_x(buf2: bytes):
    """Parse 2-byte linear accel X (int16 LE) → m/s²."""
    raw = struct.unpack_from("<h", buf2, 0)[0]
    return raw / ACCEL_SCALE


def parse_grv_z(buf2: bytes):
    """Parse 2-byte gravity Z (int16 LE) → m/s²."""
    raw = struct.unpack_from("<h", buf2, 0)[0]
    return raw / ACCEL_SCALE


def ms2_to_g(ms2):
    return ms2 / G_MS2


# Mirroring bno055_read() 12-byte burst parsing:
# buf[0:5]  = gyro X, Y, Z
# buf[6:11] = euler H, R, P
def parse_burst1(buf12: bytes):
    """Parse 12-byte burst1 (gyro 6B + euler 6B) from REG_GYRO_X_LSB."""
    gyro = buf12[0:6]
    euler = buf12[6:12]
    _, pitch_rate, _ = parse_gyro(gyro)   # gy = pitch rate
    heading, roll, pitch = parse_euler(euler)
    return pitch, roll, heading, pitch_rate


def parse_burst2(buf12: bytes):
    """Parse 12-byte burst2 (LIA 6B + GRV 6B) from REG_LIA_X_LSB."""
    lia_x_ms2 = struct.unpack_from("<h", buf12, 0)[0] / ACCEL_SCALE   # LIA X
    grv_z_ms2 = struct.unpack_from("<h", buf12, 10)[0] / ACCEL_SCALE  # GRV Z (bytes 10-11)
    return ms2_to_g(lia_x_ms2), ms2_to_g(grv_z_ms2)


# ---------------------------------------------------------------------------
# Tests: Calibration Status
# ---------------------------------------------------------------------------

class TestCalibStatus:

    def test_all_uncalibrated(self):
        sys, gyr, acc, mag = parse_calib_status(0x00)
        assert sys == 0 and gyr == 0 and acc == 0 and mag == 0

    def test_all_fully_calibrated(self):
        sys, gyr, acc, mag = parse_calib_status(0xFF)
        assert sys == 3 and gyr == 3 and acc == 3 and mag == 3

    def test_sys_only(self):
        sys, gyr, acc, mag = parse_calib_status(0xC0)
        assert sys == 3 and gyr == 0 and acc == 0 and mag == 0

    def test_gyr_only(self):
        sys, gyr, acc, mag = parse_calib_status(0x30)
        assert sys == 0 and gyr == 3 and acc == 0 and mag == 0

    def test_acc_only(self):
        sys, gyr, acc, mag = parse_calib_status(0x0C)
        assert sys == 0 and gyr == 0 and acc == 3 and mag == 0

    def test_mag_only(self):
        sys, gyr, acc, mag = parse_calib_status(0x03)
        assert sys == 0 and gyr == 0 and acc == 0 and mag == 3

    def test_partially_calibrated(self):
        # sys=2, gyr=3, acc=1, mag=0 → 0b10_11_01_00 = 0xB4
        sys, gyr, acc, mag = parse_calib_status(0xB4)
        assert sys == 2 and gyr == 3 and acc == 1 and mag == 0

    def test_bno055_is_ready_logic(self):
        """bno055_is_ready: requires acc≥2 AND gyr≥2 (when no offsets restored)."""
        for calib_byte in range(256):
            _, gyr, acc, _ = parse_calib_status(calib_byte)
            expected_ready = (acc >= 2) and (gyr >= 2)
            actual_ready = (acc >= 2) and (gyr >= 2)
            assert actual_ready == expected_ready


# ---------------------------------------------------------------------------
# Tests: Euler Angle Parsing
# ---------------------------------------------------------------------------

class TestEulerParsing:

    def test_zero(self):
        buf = struct.pack("<hhh", 0, 0, 0)
        h, r, p = parse_euler(buf)
        assert h == 0.0 and r == 0.0 and p == 0.0

    def test_heading_north(self):
        buf = struct.pack("<hhh", 0, 0, 0)
        h, _, _ = parse_euler(buf)
        assert h == 0.0

    def test_heading_east_90deg(self):
        raw = int(90 * EULER_SCALE)
        buf = struct.pack("<hhh", raw, 0, 0)
        h, _, _ = parse_euler(buf)
        assert abs(h - 90.0) < 0.001

    def test_heading_360deg(self):
        raw = int(360 * EULER_SCALE)
        buf = struct.pack("<hhh", raw, 0, 0)
        h, _, _ = parse_euler(buf)
        assert abs(h - 360.0) < 0.001

    def test_pitch_nose_up_5deg(self):
        raw = int(5.0 * EULER_SCALE)
        buf = struct.pack("<hhh", 0, 0, raw)
        _, _, p = parse_euler(buf)
        assert abs(p - 5.0) < 0.001

    def test_pitch_nose_down_negative(self):
        raw = int(-12.5 * EULER_SCALE)
        buf = struct.pack("<hhh", 0, 0, raw)
        _, _, p = parse_euler(buf)
        assert abs(p - (-12.5)) < 0.001

    def test_roll_positive(self):
        raw = int(30.0 * EULER_SCALE)
        buf = struct.pack("<hhh", 0, raw, 0)
        _, r, _ = parse_euler(buf)
        assert abs(r - 30.0) < 0.001

    def test_fractional_degree(self):
        """0.0625 degree resolution (1/16)."""
        raw = 1   # 1 LSB = 1/16 degree
        buf = struct.pack("<hhh", raw, 0, 0)
        h, _, _ = parse_euler(buf)
        assert abs(h - (1.0 / EULER_SCALE)) < 1e-6


# ---------------------------------------------------------------------------
# Tests: Gyro Rate Parsing
# ---------------------------------------------------------------------------

class TestGyroParsing:

    def test_zero_rates(self):
        buf = struct.pack("<hhh", 0, 0, 0)
        gx, gy, gz = parse_gyro(buf)
        assert gx == 0.0 and gy == 0.0 and gz == 0.0

    def test_pitch_rate_positive(self):
        raw = int(45.0 * GYRO_SCALE)  # 45 dps nose-up
        buf = struct.pack("<hhh", 0, raw, 0)
        _, gy, _ = parse_gyro(buf)
        assert abs(gy - 45.0) < 0.001

    def test_pitch_rate_negative(self):
        raw = int(-90.0 * GYRO_SCALE)
        buf = struct.pack("<hhh", 0, raw, 0)
        _, gy, _ = parse_gyro(buf)
        assert abs(gy - (-90.0)) < 0.001

    def test_max_range_900dps(self):
        raw = int(900 * GYRO_SCALE)
        buf = struct.pack("<hhh", raw, 0, 0)
        gx, _, _ = parse_gyro(buf)
        assert abs(gx - 900.0) < 0.001


# ---------------------------------------------------------------------------
# Tests: Linear Accel + Gravity Parsing
# ---------------------------------------------------------------------------

class TestAccelParsing:

    def test_zero_linear_accel(self):
        buf = struct.pack("<h", 0)
        assert parse_lia_x(buf) == 0.0

    def test_forward_accel(self):
        raw = int(1.0 * ACCEL_SCALE)   # 1 m/s² forward
        buf = struct.pack("<h", raw)
        assert abs(parse_lia_x(buf) - 1.0) < 0.001

    def test_level_gravity_z(self):
        """Level robot: gravity Z ≈ 9.80665 m/s²."""
        raw = int(G_MS2 * ACCEL_SCALE)
        buf = struct.pack("<h", raw)
        assert abs(parse_grv_z(buf) - G_MS2) < 0.02

    def test_gravity_to_g_conversion(self):
        raw = int(G_MS2 * ACCEL_SCALE)
        buf = struct.pack("<h", raw)
        g_val = ms2_to_g(parse_grv_z(buf))
        assert abs(g_val - 1.0) < 0.002

    def test_lia_negative(self):
        raw = int(-2.5 * ACCEL_SCALE)
        buf = struct.pack("<h", raw)
        assert abs(parse_lia_x(buf) - (-2.5)) < 0.001


# ---------------------------------------------------------------------------
# Tests: 12-Byte Burst Parsing (mirrors bno055_read internals)
# ---------------------------------------------------------------------------

class TestBurstParsing:

    def _make_burst1(self, gx=0, gy=0, gz=0, heading=0.0, roll=0.0, pitch=0.0):
        gyro = struct.pack("<hhh",
                           int(gx * GYRO_SCALE),
                           int(gy * GYRO_SCALE),
                           int(gz * GYRO_SCALE))
        euler = struct.pack("<hhh",
                            int(heading * EULER_SCALE),
                            int(roll * EULER_SCALE),
                            int(pitch * EULER_SCALE))
        return gyro + euler

    def _make_burst2(self, lia_x_ms2=0.0, grv_z_ms2=G_MS2):
        lia = struct.pack("<hhh", int(lia_x_ms2 * ACCEL_SCALE), 0, 0)
        grv = struct.pack("<hhh", 0, 0, int(grv_z_ms2 * ACCEL_SCALE))
        return lia + grv

    def test_pitch_from_burst1(self):
        buf = self._make_burst1(pitch=7.5)
        pitch, _, _, _ = parse_burst1(buf)
        assert abs(pitch - 7.5) < 0.001

    def test_yaw_from_burst1_heading(self):
        buf = self._make_burst1(heading=180.0)
        _, _, yaw, _ = parse_burst1(buf)
        assert abs(yaw - 180.0) < 0.001

    def test_pitch_rate_from_burst1_gy(self):
        buf = self._make_burst1(gy=30.0)
        _, _, _, pitch_rate = parse_burst1(buf)
        assert abs(pitch_rate - 30.0) < 0.001

    def test_accel_x_g_from_burst2(self):
        buf = self._make_burst2(lia_x_ms2=G_MS2)  # 1g forward
        accel_x_g, _ = parse_burst2(buf)
        assert abs(accel_x_g - 1.0) < 0.002

    def test_gravity_z_level_from_burst2(self):
        buf = self._make_burst2(grv_z_ms2=G_MS2)  # level
        _, accel_z_g = parse_burst2(buf)
        assert abs(accel_z_g - 1.0) < 0.002

    def test_tilted_gravity(self):
        """Tilted 30°: gravity Z = cos(30°) × 9.80665 ≈ 8.495 m/s²."""
        import math
        grv_z = G_MS2 * math.cos(math.radians(30))
        buf = self._make_burst2(grv_z_ms2=grv_z)
        _, accel_z_g = parse_burst2(buf)
        expected = grv_z / G_MS2
        assert abs(accel_z_g - expected) < 0.01


# ---------------------------------------------------------------------------
# Tests: Calibration Offset Packing / Backup Register Round-trip
# ---------------------------------------------------------------------------

class TestOffsetPacking:

    def _make_offsets(self, fill=0):
        return bytes([fill] * 22)

    def test_pack_length(self):
        words = pack_offsets(self._make_offsets(0))
        assert len(words) == 7

    def test_magic_in_last_word(self):
        words = pack_offsets(self._make_offsets(0))
        assert words[6] == BNO055_BKP_MAGIC

    def test_round_trip_zeros(self):
        orig = self._make_offsets(0)
        words = pack_offsets(orig)
        recovered = unpack_offsets(words)
        assert recovered == orig

    def test_round_trip_all_ones(self):
        orig = self._make_offsets(0xFF)
        words = pack_offsets(orig)
        recovered = unpack_offsets(words)
        assert recovered == orig

    def test_round_trip_sequential(self):
        orig = bytes(range(22))
        words = pack_offsets(orig)
        recovered = unpack_offsets(words)
        assert recovered == orig

    def test_round_trip_real_offsets(self):
        """Simulate a realistic BNO055 calibration offset set."""
        # accel offset X/Y/Z (int16 LE), mag offset X/Y/Z, gyro offset X/Y/Z, radii
        data = struct.pack("<hhhhhhhhhhh",
                           -45, 12, 980,       # accel offsets (mg units)
                           150, -200, 50,      # mag offsets (LSB units)
                           3, -7, 2,           # gyro offsets (dps×16)
                           1000, 800)          # accel_radius, mag_radius
        assert len(data) == 22
        words = pack_offsets(data)
        recovered = unpack_offsets(words)
        assert recovered == data

    def test_invalid_magic_rejected(self):
        words = pack_offsets(bytes(range(22)))
        words_bad = words[:6] + [0xDEADBEEF]  # corrupt magic
        with pytest.raises(AssertionError):
            unpack_offsets(words_bad)

    def test_only_22_bytes_preserved(self):
        """Words 5's high 2 bytes are unused — should be zero after unpack."""
        orig = bytes(range(22))
        words = pack_offsets(orig)
        # High 2 bytes of word[5] should be zero (only 2 bytes used)
        assert (words[5] >> 16) == 0

    def test_byte_order_word0(self):
        """First 4 bytes of offsets pack into word[0] as little-endian."""
        orig = bytes([0x01, 0x02, 0x03, 0x04] + [0] * 18)
        words = pack_offsets(orig)
        assert words[0] == 0x04030201

    def test_byte_order_word5_low(self):
        """Bytes 20-21 pack into low 16 bits of word[5]."""
        orig = bytes([0] * 20 + [0xAB, 0xCD])
        words = pack_offsets(orig)
        assert words[5] == 0xCDAB


# ---------------------------------------------------------------------------
# Tests: Temperature
# ---------------------------------------------------------------------------

class TestTemperature:

    def test_room_temp_range(self):
        """Room temperature should be in –40..+85°C range."""
        for t in [-40, 0, 25, 85]:
            assert -40 <= t <= 85

    def test_temperature_is_signed_byte(self):
        """BNO055 TEMP register is signed int8 → –128..+127 but useful range –40..+85."""
        import ctypes
        for raw in [25, 0x19, 0xFF]:
            val = ctypes.c_int8(raw).value
            assert -128 <= val <= 127