#!/usr/bin/env python3
"""s10_robot 协议层单元测试（不连机器人）。

运行::

    uv run python -m unittest discover -s tests -v
"""

from __future__ import annotations

import json
import struct
import sys
import threading
import unittest
from pathlib import Path

sys.path.insert(0, str(Path(__file__).resolve().parent.parent))

import s10_robot as s10
from s10_robot import (
    C_AXIS,
    C_HEARTBEAT,
    C_STATUS,
    FMT_JSON,
    HEADER_LEN,
    SYNC,
    T_HEARTBEAT,
    T_FAULT,
    T_MOTION,
    Frame,
    S10Client,
    S10CommandError,
    build_asdu,
    build_frame,
    parse_frames,
)


def make_frame(type_: int, command: int, items: dict, report_id: int = 0) -> bytes:
    return build_frame(build_asdu(type_, command, items), report_id)


class FrameTest(unittest.TestCase):
    def test_header_layout(self):
        asdu = build_asdu(0x00100064, 5, {})
        packet = build_frame(asdu, report_id=0x1234)
        self.assertEqual(packet[:4], SYNC)
        self.assertEqual(len(packet), HEADER_LEN + len(asdu))
        length, rid = struct.unpack_from("<HH", packet, 4)
        self.assertEqual(length, len(asdu))
        self.assertEqual(rid, 0x1234)
        self.assertEqual(packet[8], FMT_JSON)
        self.assertEqual(packet[9], 0)
        self.assertEqual(packet[10], 0x01)
        self.assertEqual(packet[11:16], b"\x00" * 5)

    def test_asdu_is_valid_json_with_numeric_ids(self):
        asdu = build_asdu(0x00100001, 0x00100002, {"X": 0.5})
        doc = json.loads(asdu)
        self.assertEqual(doc["PatrolDevice"]["Type"], 0x00100001)
        self.assertEqual(doc["PatrolDevice"]["Command"], 0x00100002)
        self.assertEqual(doc["PatrolDevice"]["Items"], {"X": 0.5})
        self.assertRegex(doc["PatrolDevice"]["Time"], r"^\d{4}-\d{2}-\d{2} \d{2}:\d{2}:\d{2}$")

    def test_parse_single(self):
        packet = make_frame(T_MOTION, C_AXIS, {"X": 0.1}, report_id=7)
        buf = bytearray(packet)
        frames = parse_frames(buf)
        self.assertEqual(len(frames), 1)
        frame = frames[0]
        self.assertEqual((frame.type, frame.command, frame.report_id), (T_MOTION, C_AXIS, 7))
        self.assertEqual(frame.items, {"X": 0.1})
        self.assertEqual(buf, bytearray())

    def test_parse_split_across_reads(self):
        packet = make_frame(T_HEARTBEAT, C_STATUS, {"BasicStatus": {"MotionState": 1}})
        buf = bytearray()
        frames = []
        for i in range(0, len(packet), 5):
            buf.extend(packet[i:i + 5])
            frames.extend(parse_frames(buf))
        self.assertEqual(len(frames), 1)
        self.assertEqual(frames[0].items["BasicStatus"]["MotionState"], 1)

    def test_parse_multiple_in_one_read(self):
        data = make_frame(T_HEARTBEAT, C_STATUS, {"i": 1}) + make_frame(
            T_MOTION, C_STATUS, {"i": 2}
        )
        frames = parse_frames(bytearray(data))
        self.assertEqual([f.items["i"] for f in frames], [1, 2])

    def test_resync_after_garbage(self):
        packet = make_frame(T_HEARTBEAT, C_STATUS, {"i": 1})
        frames = parse_frames(bytearray(b"\x00\xff\xeb\x91" + packet))
        self.assertEqual(len(frames), 1)
        self.assertEqual(frames[0].items, {"i": 1})

    def test_bad_json_is_skipped(self):
        asdu = b'{"PatrolDevice": not-json'
        bad = build_frame(asdu)
        good = make_frame(T_HEARTBEAT, C_STATUS, {"i": 1})
        frames = parse_frames(bytearray(bad + good))
        self.assertEqual(len(frames), 1)
        self.assertEqual(frames[0].items, {"i": 1})

    def test_real_device_response(self):
        # README 中实测返回的原始报文（无 ErrorMessage）
        payload = (
            '{"PatrolDevice":{"Command":5,"Items":{"ErrorCode":0},'
            '"Time":"2026-10-29 19:50:18.403","Type":1048676}}'
        ).encode()
        buf = bytearray(build_frame(payload, report_id=3))
        frames = parse_frames(buf)
        self.assertEqual(len(frames), 1)
        self.assertEqual(frames[0].type, 0x00100064)
        self.assertEqual(frames[0].command, 5)
        self.assertEqual(frames[0].items, {"ErrorCode": 0})
        self.assertEqual(frames[0].report_id, 3)

    def test_axis_asdu_too_long_raises(self):
        with self.assertRaises(ValueError):
            build_frame(b"x" * (s10.MAX_ASDU + 1))


class ClientTest(unittest.TestCase):
    def setUp(self):
        self.client = S10Client()

    def tearDown(self):
        self.client.close()

    def test_command_error(self):
        err = S10CommandError(0xE006, "Mismatched request client")
        self.assertIn("0xE006", str(err))
        self.assertEqual(err.code, 0xE006)

    def test_move_range_validation(self):
        class FakeSock:
            def sendall(self, _packet):
                pass

        self.client._sock = FakeSock()  # type: ignore[assignment]
        with self.assertRaises(ValueError):
            self.client.move(x=1.5)
        with self.assertRaises(ValueError):
            self.client.drive_velocity(x=2.0)
        self.client.move(x=0.5, yaw=-0.5)  # 合法，不应抛异常

    def test_successful_request_resolves_pending(self):
        client = self.client
        sent = []

        class FakeSock:
            def sendall(self, packet):
                sent.append(packet)
                rid = struct.unpack_from("<H", packet, 6)[0]
                # 模拟机器人在同一连接上异步回包
                threading.Thread(
                    target=lambda: client._dispatch(
                        Frame(T_MOTION, C_AXIS, "", {"ErrorCode": 0}, rid, FMT_JSON, 0, 1)
                    ),
                    daemon=True,
                ).start()

        client._sock = FakeSock()  # type: ignore[assignment]
        result = client._request(T_MOTION, C_AXIS, {"X": 0.0}, timeout=2)
        self.assertEqual(result, {"ErrorCode": 0})
        self.assertEqual(len(sent), 1)

    def test_error_response_raises(self):
        client = self.client

        class FakeSock:
            def sendall(self, packet):
                rid = struct.unpack_from("<H", packet, 6)[0]
                client._dispatch(
                    Frame(T_MOTION, C_AXIS, "",
                          {"ErrorCode": 0xE006, "ErrorMessage": "Mismatched request client"},
                          rid, FMT_JSON, 0, 1)
                )

        client._sock = FakeSock()  # type: ignore[assignment]
        with self.assertRaises(S10CommandError) as ctx:
            client._request(T_MOTION, C_AXIS, {}, timeout=2)
        self.assertEqual(ctx.exception.code, 0xE006)

    def test_status_frames_update_snapshot(self):
        client = self.client
        client._dispatch(
            Frame(T_HEARTBEAT, C_STATUS, "",
                  {"BasicStatus": {"MotionState": 17, "Gait": 0x1001, "HES": 0}},
                  0, FMT_JSON, 0, 1)
        )
        self.assertEqual(client.basic_status["MotionState"], 17)
        self.assertEqual(client.wait_motion_state({17}, timeout=0.5), 17)

    def test_fault_start_and_stop(self):
        client = self.client
        client._dispatch(
            Frame(T_FAULT, C_STATUS, "",
                  {"ErrorList": [{"Code": 0x8014, "Type": 1, "Name": "joint_position_over_limit",
                                  "Severities": [4]}]},
                  0, FMT_JSON, 0, 1)
        )
        self.assertEqual(len(client.active_errors), 1)
        client._dispatch(
            Frame(T_FAULT, C_STATUS, "",
                  {"ErrorList": [{"Code": 0x8014, "Type": 2}]}, 0, FMT_JSON, 0, 1)
        )
        self.assertEqual(client.active_errors, [])

    def test_estop_aborts_wait(self):
        client = self.client
        client._dispatch(
            Frame(T_HEARTBEAT, C_STATUS, "",
                  {"BasicStatus": {"MotionState": 1, "HES": 1}}, 0, FMT_JSON, 0, 1)
        )
        with self.assertRaises(s10.S10Error):
            client.wait_motion_state({17}, timeout=0.5)

    def test_wait_timeout(self):
        with self.assertRaises(s10.S10Timeout):
            self.client.wait_motion_state({17}, timeout=0.2)


if __name__ == "__main__":
    unittest.main(verbosity=2)