import struct
from datetime import datetime, timezone

import pytest

from hitl import protocols


def _checksum_matches(frame: bytes) -> bool:
    calc = sum(frame[:-4]) & 0xFFFF
    stored = struct.unpack("<H", frame[-4:-2])[0]
    return calc == stored


def test_build_im23a_nav_frame_format():
    ts = datetime(2025, 11, 21, 8, 1, 12, tzinfo=timezone.utc)
    frame = protocols.build_im23a_nav_frame(
        timestamp=ts,
        lat_deg=39.8301751,
        lon_deg=116.2781268,
        alt_m=48.5,
        east_vel=0.2,
        north_vel=0.1,
        up_vel=0.0,
        heading_deg=123.4,
        pitch_deg=1.2,
        roll_deg=-0.6,
        accel_bias=(0.0, 0.0, 0.0),
        gyro_bias=(0.0, 0.0, 0.0),
        temperature_c=29.5,
        status_flags=4,
    )

    assert frame.startswith(b"fmin")
    assert frame.endswith(b"ed")
    assert len(frame) == protocols.IM23A_NAV_LEN
    assert _checksum_matches(frame)

    # 解码部分字段验证顺序
    ts_val = struct.unpack("<d", frame[4:12])[0]
    assert ts_val == pytest.approx(8 * 3600 + 1 * 60 + 12, rel=1e-6)
    lat = struct.unpack("<d", frame[12:20])[0]
    lon = struct.unpack("<d", frame[20:28])[0]
    assert lat == pytest.approx(39.8301751)
    assert lon == pytest.approx(116.2781268)


def test_build_im23a_imu_frame_format():
    ts = datetime(2025, 11, 21, 8, 1, 12, tzinfo=timezone.utc)
    frame = protocols.build_im23a_imu_frame(
        timestamp=ts,
        accel_g=(0.01, -0.02, -0.99),
        gyro_deg_s=(0.1, 0.2, -0.3),
    )

    assert frame.startswith(b"fmim")
    assert frame.endswith(b"ed")
    assert len(frame) == protocols.IM23A_IMU_LEN
    assert _checksum_matches(frame)

    accel_x = struct.unpack("<f", frame[12:16])[0]
    gyro_z = struct.unpack("<f", frame[28:32])[0]
    assert accel_x == pytest.approx(0.01)
    assert gyro_z == pytest.approx(-0.3)


def _build_control_frame(payload: bytes) -> bytes:
    header = b"\xAA\x55"
    frame_type = b"\x10"
    length = struct.pack("<H", len(payload))
    checksum = (sum(frame_type + length + payload)) & 0xFFFF
    footer = b"\x0D\x0A"
    return header + frame_type + length + payload + struct.pack("<H", checksum) + footer


def test_pythonlink_decoder_float_payload():
    received = []
    decoder = protocols.PythonLinkDecoder(lambda frame: received.append(frame))
    frame = _build_control_frame(struct.pack("<ff", 1.2, -0.4))
    decoder.feed(frame[:5])
    decoder.feed(frame[5:])

    assert len(received) == 1
    assert received[0].forward == pytest.approx(1.2)
    assert received[0].turn == pytest.approx(-0.4)


def test_pythonlink_decoder_pwm_payload():
    received = []
    decoder = protocols.PythonLinkDecoder(lambda frame: received.append(frame))
    frame = _build_control_frame(struct.pack("<HH", 2000, 1600))
    decoder.feed(frame)

    assert len(received) == 1
    assert received[0].forward == pytest.approx(0.3, rel=0.05)  # (1600-1500)/500 * 1.5
    assert received[0].turn == pytest.approx(1.5708, rel=0.05)