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

import ast
from copy import deepcopy
import inspect
from pathlib import Path

import pytest

import robot_interface as sdk
from go1_pro_sdk import LowState as NativeLowState
from go1_pro_sdk import build_low_cmd_plain
from examples.example_official_compatible_position import initialize_transport


HIGHLEVEL = 0xee
LOWLEVEL = 0xff
POS_STOP_F = 2.146e9
FR_0 = 0
FR_1 = 1


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 = NativeLowState()
        self.next_state.motorState[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.LeggedType.Go1 != 2
    assert int(sdk.LeggedType.Go1) == 2
    assert sdk.Go1 is sdk.LeggedType.Go1
    assert sdk.BmsState().SOC == 0
    assert sdk.HighCmd().velocity == [0.0, 0.0]
    assert not hasattr(sdk, "LOWLEVEL")
    assert not hasattr(sdk, "FR_0")
    assert not hasattr(sdk, "__all__")
    assert {name for name in dir(sdk) if not name.startswith("_")} == {
        "LeggedType", "Aliengo", "A1", "Go1", "B1",
        "RecvEnum", "nonBlock", "block", "blockTimeout",
        "UDP", "Safety", "BmsCmd", "BmsState", "Cartesian", "IMU",
        "LED", "MotorState", "MotorCmd", "LowState", "LowCmd",
        "HighState", "HighCmd", "UDPState",
    }
    with pytest.raises(TypeError):
        sdk.Safety(2)


@pytest.mark.parametrize(
    ("factory", "kwargs"),
    [
        (sdk.MotorCmd, {"q": 1.0}),
        (sdk.LowCmd, {"levelFlag": LOWLEVEL}),
        (sdk.Cartesian, {"x": 1.0}),
        (sdk.HighCmd, {"mode": 2}),
    ],
)
def test_official_struct_constructors_are_no_arg_only(factory, kwargs):
    with pytest.raises(TypeError):
        factory(**kwargs)


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

    assert udp.InitCmdData(cmd) is None
    assert cmd.levelFlag == LOWLEVEL
    assert len(cmd.motorCmd) == 20
    assert cmd.head == [0xfe, 0xef]
    assert cmd.SN == [0, 0]
    assert cmd.version == [0, 0]
    assert isinstance(cmd.motorCmd[0], sdk.MotorCmd)
    assert isinstance(cmd.motorCmd[0].mode, int)
    assert isinstance(cmd.bms, sdk.BmsCmd)
    assert isinstance(cmd.wirelessRemote, list)
    assert isinstance(cmd.reserve, int)
    assert isinstance(cmd.crc, int)
    assert not hasattr(cmd, "set_motor")
    assert not hasattr(cmd, "all_damping")
    assert not hasattr(cmd.motorCmd[0], "to_bytes")
    assert all(m.mode == 0x0A for m in cmd.motorCmd)
    assert all(m.q == POS_STOP_F for m in cmd.motorCmd)
    assert all(m.dq == 16000.0 for m in cmd.motorCmd)
    plain = build_low_cmd_plain(cmd)
    assert len(plain) == 616
    assert plain[:4] == b"\xfe\xef\xff\x00"


def test_official_lowlevel_send_receive_sequence(fake_client):
    udp = sdk.UDP(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[FR_1].q == pytest.approx(1.23)
    assert isinstance(state.head, list)
    assert isinstance(state.SN, list)
    assert isinstance(state.imu.rpy, list)
    assert isinstance(state.imu, sdk.IMU)
    assert isinstance(state.motorState[0], sdk.MotorState)
    assert isinstance(state.bms, sdk.BmsState)
    assert not hasattr(state, "remote")
    assert not hasattr(state.motorState[0], "estimated_current")
    assert not hasattr(state.bms, "voltage_v")
    assert isinstance(state.reserve, int)
    assert isinstance(state.crc, int)

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

    client = fake_client.instances[0]
    assert client.sent[0].motorCmd[FR_1].q == pytest.approx(1.2)
    assert not hasattr(udp, "udpState")
    assert not hasattr(udp, "accessible")
    assert not hasattr(udp, "close")


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

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

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


def test_position_protect_requires_official_explicit_limit():
    safe = sdk.Safety(sdk.LeggedType.Go1)
    with pytest.raises(TypeError):
        safe.PositionProtect(sdk.LowCmd(), sdk.LowState())


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(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()


def test_portable_initialization_sends_only_stop_sentinels_before_control():
    class RecordingUDP:
        def __init__(self):
            self.cmd = None
            self.sent_q = []
            self.recv_count = 0

        def SetSend(self, cmd):
            self.cmd = cmd
            return 0

        def Send(self):
            self.sent_q.append([motor.q for motor in self.cmd.motorCmd])
            return 616

        def Recv(self):
            self.recv_count += 1
            return 858 if self.recv_count >= 2 else 0

        def GetRecv(self, state):
            return None

    udp = RecordingUDP()
    cmd = sdk.LowCmd()
    state = sdk.LowState()
    sdk.UDP(LOWLEVEL, 0, "192.168.123.10", 8007).InitCmdData(cmd)

    initialize_transport(udp, cmd, state, frames=2, dt=0)

    assert len(udp.sent_q) == 2
    assert all(all(q == POS_STOP_F for q in frame) for frame in udp.sent_q)


def test_portable_example_only_uses_official_module_exports():
    source_path = (
        Path(__file__).parents[1]
        / "examples"
        / "example_official_compatible_position.py"
    )
    tree = ast.parse(source_path.read_text())
    sdk_attributes = {
        node.attr
        for node in ast.walk(tree)
        if isinstance(node, ast.Attribute)
        and isinstance(node.value, ast.Name)
        and node.value.id == "sdk"
    }
    assert sdk_attributes <= {
        "UDP", "Safety", "LeggedType", "LowCmd", "LowState",
    }
    called_attributes = {
        node.func.attr
        for node in ast.walk(tree)
        if isinstance(node, ast.Call)
        and isinstance(node.func, ast.Attribute)
    }
    assert called_attributes.isdisjoint({
        "set_motor", "all_damping", "wake_mcu", "safe_stop", "close",
    })


def test_official_struct_field_snapshot():
    expected = {
        "BmsCmd": {"off", "reserve"},
        "BmsState": {
            "version_h", "version_l", "bms_status", "SOC", "current",
            "cycle", "BQ_NTC", "MCU_NTC", "cell_vol",
        },
        "Cartesian": {"x", "y", "z"},
        "IMU": {"quaternion", "gyroscope", "accelerometer", "rpy", "temperature"},
        "LED": {"r", "g", "b"},
        "MotorState": {
            "mode", "q", "dq", "ddq", "tauEst", "q_raw", "dq_raw",
            "ddq_raw", "temperature", "reserve",
        },
        "MotorCmd": {"mode", "q", "dq", "tau", "Kp", "Kd", "reserve"},
        "LowState": {
            "head", "levelFlag", "frameReserve", "SN", "version",
            "bandWidth", "imu", "motorState", "bms", "footForce",
            "footForceEst", "tick", "wirelessRemote", "reserve", "crc",
        },
        "LowCmd": {
            "head", "levelFlag", "frameReserve", "SN", "version",
            "bandWidth", "motorCmd", "bms", "wirelessRemote", "reserve", "crc",
        },
        "HighState": {
            "head", "levelFlag", "frameReserve", "SN", "version",
            "bandWidth", "imu", "motorState", "bms", "footForce",
            "footForceEst", "mode", "progress", "gaitType",
            "footRaiseHeight", "position", "bodyHeight", "velocity",
            "yawSpeed", "rangeObstacle", "footPosition2Body",
            "footSpeed2Body", "wirelessRemote", "reserve", "crc",
        },
        "HighCmd": {
            "head", "levelFlag", "frameReserve", "SN", "version",
            "bandWidth", "mode", "gaitType", "speedLevel",
            "footRaiseHeight", "bodyHeight", "position", "euler",
            "velocity", "yawSpeed", "bms", "led", "wirelessRemote",
            "reserve", "crc",
        },
        "UDPState": {
            "TotalCount", "SendCount", "RecvCount", "SendError",
            "FlagError", "RecvCRCError", "RecvLoseError",
        },
    }
    for class_name, field_names in expected.items():
        factory = getattr(sdk, class_name)
        assert str(inspect.signature(factory)) == "()"
        instance = factory()
        assert not (field_names - set(dir(instance)))


def test_official_method_snapshot():
    assert {
        "SetIpPort", "SetRecvTimeout", "SetDisconnectTime",
        "SetAccessibleTime", "Send", "Recv", "InitCmdData", "SetSend",
        "GetRecv",
    } <= set(dir(sdk.UDP))
    assert str(inspect.signature(sdk.Safety.PositionLimit)) == "(self, lowcmd)"
    assert str(inspect.signature(sdk.Safety.PowerProtect)) == "(self, lowcmd, lowstate, factor)"
    assert str(inspect.signature(sdk.Safety.PositionProtect)) == "(self, lowcmd, lowstate, limit)"