"""Safety 保护层测试."""
import pytest
from go1_pro_sdk import (
    LowCmd, MotorCmd, MotorMode,
    apply_safety, position_limit, power_protect,
    PowerProtectViolation,
    TAU_MAX,
)


class _FakeMotor:
    tauEst = 0.0
    q = 0.0


class _FakeState:
    def __init__(self):
        self.motorState = [_FakeMotor() for _ in range(20)]


def test_position_limit_clamps_hip_over():
    cmd = LowCmd()
    cmd.motorCmd[0].q = 5.0  # FR_0 hip 超限
    position_limit(cmd)
    assert cmd.motorCmd[0].q == 0.78


def test_position_limit_clamps_thigh_under():
    cmd = LowCmd()
    cmd.motorCmd[1].q = -3.0
    position_limit(cmd)
    assert cmd.motorCmd[1].q == -0.60


def test_power_protect_clamps():
    cmd = LowCmd()
    for i in range(12):
        cmd.motorCmd[i].tau = 10.0
    state = _FakeState()
    n = power_protect(cmd, state, factor=1)
    # factor=1 → 限制到 TAU_MAX * 0.1
    assert cmd.motorCmd[0].tau == TAU_MAX['hip'] * 0.1
    assert cmd.motorCmd[2].tau == TAU_MAX['knee'] * 0.1


def test_power_protect_critical_command_raises():
    cmd = LowCmd()
    cmd.motorCmd[2].tau = 200.0  # > 5 × 35.55
    with pytest.raises(PowerProtectViolation):
        power_protect(cmd, _FakeState(), factor=5)


def test_power_protect_overload_actual_raises():
    """实测 tauEst 已超 max, 也应 raise."""
    cmd = LowCmd()
    cmd.motorCmd[2].tau = 1.0
    state = _FakeState()
    state.motorState[2].tauEst = 40.0  # > 35.55
    with pytest.raises(PowerProtectViolation):
        power_protect(cmd, state, factor=1)


def test_apply_safety_degraded_mode():
    """raise_on_critical=False 应当降级到全 damping."""
    cmd = LowCmd()
    cmd.motorCmd[2].tau = 200.0
    cmd.motorCmd[2].Kp = 10
    apply_safety(cmd, _FakeState(), power_factor=5,
                 position_limit_on=False, raise_on_critical=False)
    assert cmd.motorCmd[0].tau == 0
    assert cmd.motorCmd[0].Kp == 0
    assert cmd.motorCmd[0].mode == 0


def test_apply_safety_normal_flow():
    """正常命令应能通过. 返回 LowCmd."""
    cmd = LowCmd()
    cmd.motorCmd[0].q = 0.5
    cmd.motorCmd[0].tau = 1.0
    result = apply_safety(cmd, _FakeState(), power_factor=5)
    assert result is cmd
    assert cmd.motorCmd[0].q == 0.5    # 在限位内, 不变