"""rate_limiter_test.py —— Clamp / SlewRateLimiter 单元测试。

移植自 reference-c++/visual_track_control/tests/rate_limiter_test.cpp。
覆盖（物理追踪思路 §8、状态机设计 S6）：静态限幅、变化率限幅步进、
首条指令从零渐变、异常节拍保护、Reset 归零。
"""

import pytest

from visual_track_control.rate_limiter import SlewRateLimiter, clamp


def test_clamp_clamps_value():
    assert clamp(5.0, -2.0, 2.0) == 2.0
    assert clamp(-5.0, -2.0, 2.0) == -2.0
    assert clamp(1.5, -2.0, 2.0) == 1.5
    # 边界值：落在区间内
    assert clamp(2.0, -2.0, 2.0) == 2.0
    assert clamp(-2.0, -2.0, 2.0) == -2.0


def test_first_command_ramps_from_zero():
    # S6：初始输出为 0，首条大指令只能按变化率渐变
    lim = SlewRateLimiter(2.0)
    assert lim.limit(10.0, 0.5) == 1.0    # 0 + 2×0.5
    assert lim.limit(10.0, 0.5) == 2.0    # 继续渐变
    assert lim.limit(10.0, 10.0) == 10.0  # 步长足够 → 到达目标


def test_tracks_target_within_step():
    lim = SlewRateLimiter(10.0)
    # 步长 10×1=10，目标 3 在步长内 → 直接到达
    assert lim.limit(3.0, 1.0) == 3.0
    assert lim.limit(3.0, 1.0) == 3.0  # 已在目标：保持


def test_negative_direction():
    lim = SlewRateLimiter(2.0)
    assert lim.limit(-10.0, 0.5) == -1.0
    assert lim.limit(-10.0, 0.5) == -2.0


def test_zero_or_negative_dt_holds():
    lim = SlewRateLimiter(2.0)
    lim.limit(10.0, 1.0)  # 输出 2.0
    assert lim.limit(10.0, 0.0) == 2.0
    assert lim.limit(10.0, -1.0) == 2.0


def test_zero_rate_limit_freezes_output():
    lim = SlewRateLimiter(0.0)
    assert lim.limit(5.0, 1.0) == 0.0


def test_reset_to_value():
    lim = SlewRateLimiter(2.0)
    lim.limit(10.0, 1.0)
    lim.reset()
    assert lim.last_output == 0.0
    lim.reset(3.5)
    assert lim.last_output == 3.5
    # 复位后从新值继续渐变
    assert lim.limit(10.0, 1.0) == 5.5  # 3.5 + 2×1


def test_custom_initial_value():
    lim = SlewRateLimiter(1.0, initial=5.0)
    assert lim.limit(10.0, 1.0) == 6.0