"""高层 MQTT 控制测试 (用 unittest.mock 替换 paho.mqtt, 不需要真实 broker)."""
import struct
from unittest.mock import MagicMock, patch
import pytest


@pytest.fixture
def mock_mqtt():
    """打补丁让 Go1MQTT 用 mock client, 不实际连接."""
    with patch('paho.mqtt.client.Client') as MockClient:
        instance = MagicMock()
        MockClient.return_value = instance
        yield instance


def test_velocity_default():
    from go1_pro_sdk import Velocity
    v = Velocity()
    assert v.vx == 0.0
    assert v.vy == 0.0
    assert v.vz == 0.0


def test_pose_named_arg():
    from go1_pro_sdk import Pose
    p = Pose(lean_left_right=0.5)
    assert p.lean_left_right == 0.5
    assert p.twist_left_right == 0.0


def test_mode_enum_string():
    from go1_pro_sdk import Mode
    assert Mode.STAND_UP.value == 'standUp'
    assert Mode.DANCE_1.value == 'dance1'
    assert Mode.DAMPING.value == 'damping'


def test_go1mqtt_connect_and_disconnect(mock_mqtt):
    from go1_pro_sdk.highlevel import Go1MQTT
    client = Go1MQTT(host='localhost', port=1883)
    mock_mqtt.connect.assert_called_once_with('localhost', 1883, 60)
    mock_mqtt.loop_start.assert_called_once()

    client.disconnect()
    mock_mqtt.loop_stop.assert_called()
    mock_mqtt.disconnect.assert_called()


def test_switch_mode_publishes_to_action(mock_mqtt):
    from go1_pro_sdk import Mode
    from go1_pro_sdk.highlevel import Go1MQTT
    client = Go1MQTT()

    client.switch_mode(Mode.STAND_UP)

    # 应该发送 controller/action = "standUp"
    mock_mqtt.publish.assert_called_with('controller/action', 'standUp', qos=1)


def test_switch_mode_string_also_works(mock_mqtt):
    from go1_pro_sdk.highlevel import Go1MQTT
    client = Go1MQTT()
    client.switch_mode('walk')
    mock_mqtt.publish.assert_called_with('controller/action', 'walk', qos=1)


def test_send_cmd_vel_publishes_4_floats(mock_mqtt):
    from go1_pro_sdk import Velocity
    from go1_pro_sdk.highlevel import Go1MQTT
    client = Go1MQTT()

    client.send_cmd_vel(Velocity(vx=0.3, vy=0.1, vz=-0.2))

    # 应该有 2 次 publish: 先归零再发实际
    assert mock_mqtt.publish.call_count >= 2
    last_call = mock_mqtt.publish.call_args_list[-1]
    topic, payload = last_call.args[0], last_call.args[1]
    assert topic == 'controller/stick'
    # 协议: (vy, vz, 0, vx) — unitree 反直觉的顺序
    vy, vz, _, vx = struct.unpack('ffff', payload)
    assert abs(vy - 0.1) < 1e-5
    assert abs(vz - (-0.2)) < 1e-5
    assert abs(vx - 0.3) < 1e-5


def test_send_cmd_vel_clips_to_1(mock_mqtt):
    from go1_pro_sdk import Velocity
    from go1_pro_sdk.highlevel import Go1MQTT
    client = Go1MQTT()

    client.send_cmd_vel(Velocity(vx=5.0, vy=-3.0, vz=0.0))   # 超界
    last_payload = mock_mqtt.publish.call_args_list[-1].args[1]
    vy, vz, _, vx = struct.unpack('ffff', last_payload)
    assert vx == 1.0
    assert vy == -1.0


def test_send_cmd_pose(mock_mqtt):
    from go1_pro_sdk import Pose
    from go1_pro_sdk.highlevel import Go1MQTT
    client = Go1MQTT()

    client.send_cmd_pose(Pose(lean_left_right=0.3, twist_left_right=0.0,
                              look_up_down=0.2, extend_squat=-0.5))
    payload = mock_mqtt.publish.call_args.args[1]
    ll, tw, lu, es = struct.unpack('ffff', payload)
    assert abs(ll - 0.3) < 1e-5
    assert abs(lu - 0.2) < 1e-5
    assert abs(es - (-0.5)) < 1e-5


def test_set_led_publishes_3_bytes(mock_mqtt):
    from go1_pro_sdk import LED
    from go1_pro_sdk.highlevel import Go1MQTT
    client = Go1MQTT()

    client.set_led(LED(255, 128, 0))
    last_call = mock_mqtt.publish.call_args
    assert last_call.args[0] == 'face_light/color'
    assert last_call.args[1] == bytes([255, 128, 0])


def test_set_led_clips_0_255(mock_mqtt):
    from go1_pro_sdk import LED
    from go1_pro_sdk.highlevel import Go1MQTT
    client = Go1MQTT()

    client.set_led(LED(300, -10, 100))
    assert mock_mqtt.publish.call_args.args[1] == bytes([255, 0, 100])


def test_go1_high_level_api_calls_mqtt(mock_mqtt):
    """Go1 类的方法应该都委托给 Go1MQTT."""
    from go1_pro_sdk import Go1, Velocity, LED

    with Go1() as dog:
        dog.stand_up()
        dog.dance_1()
        dog.walk(Velocity(vx=0.5))
        dog.set_led(LED(0, 255, 0))
        dog.stand_down()

    # 应该有多次 publish
    assert mock_mqtt.publish.call_count >= 5