import struct
from datetime import datetime, timezone

import pytest

from hitl import protocols


def _calc_checksum(sentence: str) -> str:
    cs = 0
    for ch in sentence[1:]:
        cs ^= ord(ch)
    return f"{cs:02X}"


def test_build_gprmi_sentence_format():
    ts = datetime(2025, 11, 21, 8, 1, 12, tzinfo=timezone.utc)
    sentence = protocols.build_gprmi_sentence(
        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,
    ).decode("ascii")

    assert sentence.startswith("$GPRMI,")
    assert sentence.endswith("\r\n")
    body, checksum = sentence.strip()[0:sentence.index("*")], sentence[sentence.index("*") + 1 : sentence.index("*") + 3]
    assert checksum == _calc_checksum(body)
    fields = body.split(",")
    assert len(fields) == 24  # $GPRMI + 23 个字段


def test_build_gpimu_sentence_format():
    ts = datetime(2025, 11, 21, 8, 1, 12, tzinfo=timezone.utc)
    sentence = protocols.build_gpimu_sentence(
        timestamp=ts,
        accel_g=(0.01, -0.02, -0.99),
        gyro_deg_s=(0.1, 0.2, -0.3),
        temperature_c=29.5,
    ).decode("ascii")

    assert sentence.startswith("$GPIMU,")
    body = sentence.strip()[0:sentence.index("*")]
    checksum = sentence[sentence.index("*") + 1 : sentence.index("*") + 3]
    assert checksum == _calc_checksum(body)
    assert len(body.split(",")) == 9  # $GPIMU + 8 字段


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)