import struct
import time

import pytest

from hitl import simulator as simulator_mod

GGA_SAMPLE = "$GNGGA,080112.000,3949.8105069,N,11616.6876082,E,4,44,0.42,48.502,M,-6.684,M,1.0,0409*73"


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


class FakeSerialEndpoint:
    def __init__(self, port: str | None, baudrate: int, timeout: float = 0.0):
        self.port = port
        self.baudrate = baudrate
        self.timeout = timeout
        self.opened = False
        self.writes: list[bytes] = []
        self._read_buffer = bytearray()

    def open(self):
        if self.port is None:
            return
        self.opened = True

    def close(self):
        self.opened = False

    def write(self, data: bytes):
        if not data:
            return
        self.writes.append(bytes(data))

    def read(self, size: int = 1) -> bytes:
        if not self._read_buffer:
            return b""
        size = min(size, len(self._read_buffer))
        chunk = self._read_buffer[:size]
        del self._read_buffer[:size]
        return bytes(chunk)

    def readline(self) -> bytes:
        return b""

    def inject(self, data: bytes):
        self._read_buffer.extend(data)


def test_simulator_emits_sensor_frames(monkeypatch):
    monkeypatch.setattr(simulator_mod, "SerialEndpoint", FakeSerialEndpoint)

    config = simulator_mod.HitlConfig(
        uart2_port="SIM",
        uart5_port=None,
        origin_gga=GGA_SAMPLE,
        initial_enu=(0.0, 0.0, 0.0),
    )
    sim = simulator_mod.HitlSimulator(config)
    received_controls: list[tuple[float, float]] = []
    sim.on_control = lambda f, t: received_controls.append((f, t))

    sim.start()
    time.sleep(0.25)

    frame = _build_control_frame(struct.pack("<ff", 0.5, 0.2))
    sim.uart2.inject(frame)
    time.sleep(0.05)
    sim.stop()

    gprmi_count = sum(1 for pkt in sim.uart2.writes if pkt.startswith(b"$GPFMI"))
    gpimu_count = sum(1 for pkt in sim.uart2.writes if pkt.startswith(b"$GPIMU"))

    assert gprmi_count >= 2
    assert gpimu_count >= 10
    assert received_controls, "控制帧未被解析"
    assert received_controls[-1][0] == pytest.approx(0.5, rel=0.1)
    assert received_controls[-1][1] == pytest.approx(0.2, rel=0.1)