"""Contract tests for the official ``robot_interface`` compatibility API."""

from copy import deepcopy

import pytest

import robot_interface as sdk
from go1_pro_sdk import build_low_cmd_plain


class _FakeMCUClient:
    instances = []

    def __init__(self, mcu_ip, mcu_port, local_port):
        self.mcu_ip = mcu_ip
        self.mcu_port = mcu_port
        self.local_port = local_port or 49152
        self.next_state = sdk.LowState()
        self.next_state.motorState[sdk.FR_1].q = 1.23
        self.sent = []
        self.closed = False
        self.__class__.instances.append(self)

    def send(self, cmd):
        self.sent.append(deepcopy(cmd))
        return 616

    def recv_latest(self):
        return deepcopy(self.next_state)

    def recv_state(self, timeout):
        return deepcopy(self.next_state)

    def close(self):
        self.closed = True


@pytest.fixture
def fake_client(monkeypatch):
    _FakeMCUClient.instances.clear()
    monkeypatch.setattr(
        "go1_pro_sdk.compat.robot_interface.MCUClient",
        _FakeMCUClient,
    )
    return _FakeMCUClient


def test_official_names_and_data_types_are_available():
    assert sdk.LOWLEVEL == 0xff
    assert sdk.LeggedType.Go1 == 2
    assert sdk.Go1 is sdk.LeggedType.Go1
    assert sdk.FR_0 == 0
    assert sdk.RL_2 == 11
    assert sdk.BmsState().SOC == 0
    assert sdk.HighCmd().velocity == [0.0, 0.0]


def test_init_cmd_data_uses_official_stop_sentinels():
    udp = sdk.UDP(sdk.LOWLEVEL, 0, "192.168.123.10", 8007)
    cmd = sdk.LowCmd()

    assert udp.InitCmdData(cmd) is None
    assert cmd.levelFlag == sdk.LOWLEVEL
    assert len(cmd.motorCmd) == 20
    assert all(m.mode == 0x0A for m in cmd.motorCmd)
    assert all(m.q == sdk.PosStopF for m in cmd.motorCmd)
    assert all(m.dq == sdk.VelStopF for m in cmd.motorCmd)


def test_official_lowlevel_send_receive_sequence(fake_client):
    udp = sdk.UDP(sdk.LOWLEVEL, 0, "192.168.123.10", 8007)
    cmd = sdk.LowCmd()
    state = sdk.LowState()
    udp.InitCmdData(cmd)

    assert udp.Recv() == 858
    assert udp.GetRecv(state) is None
    assert state.motorState[sdk.FR_1].q == pytest.approx(1.23)

    cmd.motorCmd[sdk.FR_1].q = 1.2
    cmd.motorCmd[sdk.FR_1].dq = 0.0
    cmd.motorCmd[sdk.FR_1].Kp = 5.0
    cmd.motorCmd[sdk.FR_1].Kd = 1.0
    assert udp.SetSend(cmd) == 0
    assert udp.Send() == 616

    client = fake_client.instances[0]
    assert client.sent[0].motorCmd[sdk.FR_1].q == pytest.approx(1.2)
    assert udp.udpState.SendCount == 1
    assert udp.udpState.RecvCount == 1


def test_position_limit_skips_stop_sentinel():
    cmd = sdk.LowCmd()
    sdk.UDP(sdk.LOWLEVEL, 0, "192.168.123.10", 8007).InitCmdData(cmd)
    cmd.motorCmd[sdk.FR_1].q = 99.0

    safe = sdk.Safety(sdk.LeggedType.Go1)
    assert safe.PositionLimit(cmd) is None

    assert cmd.motorCmd[sdk.FR_0].q == sdk.PosStopF
    assert cmd.motorCmd[sdk.FR_1].q == pytest.approx(3.5)


def test_bms_command_is_serialized():
    cmd = sdk.LowCmd()
    cmd.bms.off = 0xA5
    cmd.bms.reserve = [1, 2, 3]

    plain = build_low_cmd_plain(cmd)
    assert plain[562:566] == b"\xa5\x01\x02\x03"


def test_official_reserved_field_shapes_are_serialized():
    cmd = sdk.LowCmd()
    cmd.SN = [0x04030201, 0x08070605]
    cmd.version = [0x01020304, 0x05060708]
    cmd.reserve = 0x44332211

    plain = build_low_cmd_plain(cmd)
    assert plain[4:12] == bytes.fromhex("0102030405060708")
    assert plain[12:20] == bytes.fromhex("0403020108070605")
    assert plain[606:610] == bytes.fromhex("11223344")


def test_fixed_size_official_arrays_are_validated():
    cmd = sdk.LowCmd()
    cmd.wirelessRemote = [0] * 39
    with pytest.raises(ValueError, match="wirelessRemote"):
        build_low_cmd_plain(cmd)


def test_highlevel_udp_reports_the_unsupported_boundary():
    udp = sdk.UDP(sdk.HIGHLEVEL, 8080, "192.168.123.161", 8082)
    cmd = sdk.HighCmd()
    udp.InitCmdData(cmd)
    udp.SetSend(cmd)

    with pytest.raises(NotImplementedError, match="HighCmd/HighState UDP"):
        udp.Send()