"""LowCmd 序列化测试 (PRO 格式)."""
import os
import pytest
from go1_pro_sdk import (
    Blowfish, LowCmd, MotorCmd, MotorMode,
    build_low_cmd_plain, build_low_cmd_encrypted,
)


STATE_FILE = os.path.join(
    os.path.dirname(__file__), '..', 'go1_pro_sdk', '_data', 'blowfish_state.bin'
)


@pytest.fixture
def bf():
    return Blowfish.from_state_file(STATE_FILE)


def test_plain_length_616():
    cmd = LowCmd()
    plain = build_low_cmd_plain(cmd)
    assert len(plain) == 616


def test_plain_head():
    cmd = LowCmd()
    plain = build_low_cmd_plain(cmd)
    assert plain[:2] == b'\xfe\xef'
    assert plain[2] == 0xff


def test_plain_sn_version_all_zero():
    """PRO 真实包 SN/version 全 0."""
    cmd = LowCmd()
    plain = build_low_cmd_plain(cmd)
    assert plain[4:12] == b'\x00' * 8     # SN
    assert plain[12:20] == b'\x00' * 8    # version


def test_plain_bandwidth_be():
    """bandWidth 是 BE 字节序 (3a c0, 不是 c0 3a)."""
    cmd = LowCmd()
    plain = build_low_cmd_plain(cmd)
    assert plain[20:22] == b'\x3a\xc0'


def test_plain_padding_zeros():
    """610..612 是固定 0000."""
    cmd = LowCmd()
    plain = build_low_cmd_plain(cmd)
    assert plain[610:612] == b'\x00\x00'


def test_plain_crc_at_612():
    """CRC 在 612..616, 不是 EDU 的 610..614."""
    cmd = LowCmd()
    plain = build_low_cmd_plain(cmd)
    from go1_pro_sdk.utils.common import gen_crc
    expected_crc = gen_crc(plain[:612])
    assert plain[612:616] == expected_crc


def test_encrypted_damping_matches_real(bf):
    """全 damping 加密后, 前 16B 应跟真实 Legged_sport 抓包一致."""
    cmd = LowCmd()
    enc = build_low_cmd_encrypted(cmd, bf)
    assert len(enc) == 616
    # 真实抓包前 16B (strace 抓的 Legged_sport sendto 内容)
    expected = bytes.fromhex('e79e1ccc4bae9cf812aa0354236b66e3')
    assert enc[:16] == expected


def test_set_motor_by_name():
    cmd = LowCmd()
    cmd.set_motor('FR_1', MotorCmd(mode=MotorMode.Servo, q=1.2, Kp=5, Kd=1))
    assert cmd.motorCmd[1].mode == MotorMode.Servo
    assert cmd.motorCmd[1].q == 1.2
    assert cmd.motorCmd[1].Kp == 5


def test_set_motor_by_index():
    cmd = LowCmd()
    cmd.set_motor(5, MotorCmd(q=2.0))
    assert cmd.motorCmd[5].q == 2.0


def test_all_damping():
    cmd = LowCmd()
    cmd.motorCmd[0].mode = MotorMode.Servo
    cmd.motorCmd[0].q = 1.5
    cmd.all_damping()
    assert cmd.motorCmd[0].mode == MotorMode.Damping
    assert cmd.motorCmd[0].q == 0.0