"""限幅与变化率限制（纯算法构件）。

对应 C++ 参考实现 reference-c++/visual_track_control/include/visual_track_control/rate_limiter.h
（规格：docs/物理追踪思路.md §8、状态机设计 S6 首条指令限幅）。
"""

def clamp(v: float, lo: float, hi: float) -> float:
    """静态限幅：将 v 钳制在 [lo, hi]（调用方保证 lo <= hi）。"""
    return min(max(v, lo), hi)


class SlewRateLimiter:
    """变化率限幅器：限制输出每秒最大变化量（单位由调用方决定）。

    初值置 0 时天然实现"首条指令突变抑制"（S6）：切换进入视觉跟踪后
    首条指令从零按加速度限幅渐变。
    """

    def __init__(self, rate_limit: float, initial: float = 0.0) -> None:
        self._rate_limit = rate_limit
        self._output = initial

    def limit(self, target: float, dt_s: float) -> float:
        """朝 target 推进一个节拍；dt_s <= 0 或 rate_limit <= 0 时保持当前输出。"""
        if dt_s <= 0.0 or self._rate_limit <= 0.0:
            return self._output
        max_step = self._rate_limit * dt_s
        self._output = clamp(target, self._output - max_step, self._output + max_step)
        return self._output

    def reset(self, value: float = 0.0) -> None:
        """复位输出值（刹车/重置时归零，保证恢复跟踪后指令从零渐变）。"""
        self._output = value

    @property
    def last_output(self) -> float:
        return self._output