"""pid_controller_test.py —— PidController 单元测试。

移植自 reference-c++/visual_track_control/tests/pid_controller_test.cpp。
覆盖（物理追踪思路 §6.2/§6.4）：位置式 P/I/D 三项、积分钳位、输出限幅、
异常节拍保护、anti-windup 条件积分（C1 修复回归）、ResetIntegral / Reset。
"""

import pytest

from visual_track_control.pid_controller import PidController, PidParams


def make_params(kp, ki, kd, integral_limit, output_limit, filter_coef=0.0):
    return PidParams(kp=kp, ki=ki, kd=kd, integral_limit=integral_limit,
                     derivative_filter_coef=filter_coef, output_limit=output_limit)


def test_proportional_only():
    pid = PidController(make_params(2.0, 0.0, 0.0, 0.0, 0.0))
    assert pid.update(3.0, 0.1) == 6.0
    assert pid.update(-1.5, 0.1) == -3.0


def test_integral_accumulates():
    pid = PidController(make_params(0.0, 1.0, 0.0, 0.0, 0.0))
    # 两拍 error=2、dt=0.5：积分累计 1.0 → 2.0
    assert pid.update(2.0, 0.5) == 1.0
    assert pid.update(2.0, 0.5) == 2.0


def test_integral_clamped_to_limit():
    pid = PidController(make_params(0.0, 1.0, 0.0, integral_limit=1.0, output_limit=0.0))
    for _ in range(5):
        pid.update(10.0, 1.0)
    # 积分候选值被钳位在 1.0 → 输出恒为 1.0
    assert pid.last_output == 1.0


def test_output_clamped_to_limit():
    pid = PidController(make_params(10.0, 0.0, 0.0, 0.0, output_limit=3.0))
    assert pid.update(1.0, 0.1) == 3.0
    assert pid.update(-1.0, 0.1) == -3.0


def test_anti_windup_freeze_while_saturated_same_direction():
    # kp=0、ki=2、output_limit=5：error=+1 持续饱和时积分冻结，
    # 输出恒钉在 +5（10 拍累计也不会让积分继续增长）。
    pid = PidController(make_params(0.0, 2.0, 0.0, 0.0, 5.0))
    for _ in range(10):
        assert pid.update(10.0, 1.0) == 5.0


def test_anti_windup_unwinds_after_error_flips():
    # C1 回归：误差反号后积分必须立即回卷（旧判据"积分与输出同向"会
    # 冻结回卷、输出一直钉在 +5）。
    # kp=0、ki=2、limit=5：+10 饱和 3 拍、积分冻结在 0；
    # error=-1 首拍 → 输出 2×(-1) = -2（立即回卷）；
    # 持续负误差使输出到达负饱和 -5（积分冻结在 -2，输出 = clamp(-6)）。
    pid = PidController(make_params(0.0, 2.0, 0.0, 0.0, 5.0))
    for _ in range(3):
        assert pid.update(10.0, 1.0) == 5.0
    assert pid.update(-1.0, 1.0) == -2.0  # 首拍立即回卷
    out = -2.0
    for _ in range(11):
        out = pid.update(-1.0, 1.0)
    assert out == pytest.approx(-5.0, abs=1e-9)  # 旧判据此处仍为 +5.0


def test_derivative_first_step_is_zero():
    pid = PidController(make_params(0.0, 0.0, 1.0, 0.0, 0.0))
    # 首拍无微分历史：微分项为 0
    assert pid.update(5.0, 1.0) == 0.0


def test_derivative_with_low_pass_filter():
    pid = PidController(make_params(0.0, 0.0, 1.0, 0.0, 0.0, filter_coef=0.5))
    pid.update(0.0, 1.0)  # 建立微分历史（导数 0）
    # 第二拍原始导数 1.0，低通后 0.5×0 + 0.5×1.0 = 0.5
    assert pid.update(1.0, 1.0) == 0.5


def test_zero_or_negative_dt_holds_last_output():
    pid = PidController(make_params(1.0, 0.0, 0.0, 0.0, 0.0))
    assert pid.update(2.0, 0.1) == 2.0
    assert pid.update(99.0, 0.0) == 2.0
    assert pid.update(99.0, -1.0) == 2.0


def test_reset_integral_clears_only_integral():
    pid = PidController(make_params(0.0, 1.0, 1.0, 0.0, 0.0))
    pid.update(2.0, 1.0)  # 积分 = 2
    pid.reset_integral()
    # 积分清零、微分历史保留：输出应为 kd×(0−2)/1 = −2
    assert pid.update(0.0, 1.0) == -2.0


def test_reset_clears_everything():
    pid = PidController(make_params(1.0, 1.0, 1.0, 0.0, 0.0))
    pid.update(5.0, 1.0)
    pid.reset()
    assert pid.last_output == 0.0
    # 全清后等同全新控制器：首拍无微分历史
    assert pid.update(3.0, 1.0) == 3.0 + 3.0  # kp·e + ki·∫e