"""位置式 PID 控制器（纯算法构件）。

对应 C++ 参考实现 reference-c++/visual_track_control/src/pid_controller.cpp
（规格：docs/物理追踪思路.md §6.2 距离控制律、§6.4 Coast 清积分）。

anti-windup（条件积分）：输出饱和且本拍积分增量（error·dt）仍在推深饱和
时冻结本步积分累加；判据刻意基于"增量方向"而非"积分项与输出同向"——
后者在误差反号后仍会冻结回卷，导致饱和残留。
"""

from dataclasses import dataclass

from .rate_limiter import clamp


@dataclass
class PidParams:
    """PID 参数（全部来自配置，代码不写死任何数值）。"""

    kp: float = 0.0
    ki: float = 0.0
    kd: float = 0.0
    integral_limit: float = 0.0         # 积分项限幅（<=0 表示不限幅）
    derivative_filter_coef: float = 0.0  # 微分一阶低通系数 ∈ [0,1)：0 = 不滤波
    output_limit: float = 0.0           # 输出限幅（<=0 表示不限幅）


class PidController:
    def __init__(self, params: PidParams) -> None:
        self._params = params
        self._integral = 0.0
        self._prev_error = 0.0
        self._filtered_derivative = 0.0
        self._last_output = 0.0
        self._has_prev_error = False

    def update(self, error: float, dt_s: float) -> float:
        """单步更新：输入误差与采样间隔（秒），返回限幅后的输出。

        dt_s <= 0 时保持上次输出（异常节拍保护）。
        """
        if dt_s <= 0.0:
            return self._last_output

        # 积分：候选值先钳位（积分限幅）
        integral_candidate = self._integral + error * dt_s
        if self._params.integral_limit > 0.0:
            integral_candidate = clamp(integral_candidate,
                                       -self._params.integral_limit,
                                       self._params.integral_limit)

        # 微分：一阶低通滤波（系数 ∈ [0,1)，0 = 不滤波）
        derivative = 0.0
        if self._has_prev_error:
            derivative = (error - self._prev_error) / dt_s
            coef = self._params.derivative_filter_coef
            derivative = coef * self._filtered_derivative + (1.0 - coef) * derivative
        self._filtered_derivative = derivative
        self._prev_error = error
        self._has_prev_error = True

        # 位置式合成
        integral_term = self._params.ki * integral_candidate
        raw = self._params.kp * error + integral_term + self._params.kd * derivative

        # anti-windup（条件积分）：输出饱和且本拍积分增量仍在推深饱和时冻结
        saturated = (self._params.output_limit > 0.0
                     and abs(raw) > self._params.output_limit)
        increment_pushes_deeper = (error * dt_s) * raw > 0.0
        if not (saturated and increment_pushes_deeper):
            self._integral = integral_candidate

        # 输出限幅
        if self._params.output_limit > 0.0:
            self._last_output = clamp(raw, -self._params.output_limit,
                                      self._params.output_limit)
        else:
            self._last_output = raw
        return self._last_output

    def reset_integral(self) -> None:
        """清空积分项（§6.4：Coast 时防止积分饱和）；保留微分历史与上次输出。"""
        self._integral = 0.0

    def reset(self) -> None:
        """清空全部内部状态（积分、微分历史、上次输出）。"""
        self._integral = 0.0
        self._prev_error = 0.0
        self._filtered_derivative = 0.0
        self._last_output = 0.0
        self._has_prev_error = False

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