from __future__ import annotations

import numpy as np


class TargetLimiter:
    """部署侧限幅器，避免策略输出在真实机上突然跳变。"""

    def __init__(self, *, max_step_rad: float, lower: np.ndarray, upper: np.ndarray) -> None:
        self.max_step_rad = float(max_step_rad)
        self.lower = lower.astype(np.float32)
        self.upper = upper.astype(np.float32)
        self._previous: np.ndarray | None = None

    def reset(self, initial: np.ndarray) -> None:
        self._previous = np.asarray(initial, dtype=np.float32).copy()

    def apply(self, target: np.ndarray) -> np.ndarray:
        target = np.asarray(target, dtype=np.float32)
        if self._previous is None:
            self.reset(target)
            return np.clip(target, self.lower, self.upper)
        delta = np.clip(target - self._previous, -self.max_step_rad, self.max_step_rad)
        limited = np.clip(self._previous + delta, self.lower, self.upper)
        self._previous = limited.copy()
        return limited
