# controllers.py
import os, sys, time


def _pid_log(msg: str):
    """PID 内部日志，直接写文件 + 刷盘"""
    try:
        log_dir = os.path.join(os.path.dirname(sys.executable), "logs")
        os.makedirs(log_dir, exist_ok=True)
        log_file = os.path.join(log_dir, "control_debug.log")
        with open(log_file, "a", encoding="utf-8") as f:
            f.write(f"[{time.strftime('%H:%M:%S.%f')[:-3]}] [PID] {msg}\n")
            f.flush()
            os.fsync(f.fileno())
    except Exception:
        pass


class IncrementalPID:
    """增量式PID控制器"""

    def __init__(self, kp: float, ki: float, kd: float, dt: float,
                 out_min: float, out_max: float, xa_full: float = 1062.5):
        # PID参数
        self.kp = kp
        self.ki = ki
        self.kd = kd
        self.dt = dt  # 默认50HZ执行周期防止除零
        self.motor_max = 300.0
        self.du_max = self.motor_max * self.dt
        self.dead_area = 240.0
        self.xa_full = xa_full  # 总限幅（最大行程）

        # self.kp = 1.392
        # self.ki = 30.2
        # self.kd = 0.000485
        # self.dt = 0.0059

        # 限幅设置
        self.out_min = out_min
        self.out_max = out_max

        # 输入输出
        self.target_pressure = 0.0  # 参考值（设定压力大小）
        self.current_pressure = 0.0  # 反馈值
        self.error = 0.0  # 当前误差

        # 计算系数
        self.a0 = 0.0
        self.a1 = 0.0
        self.a2 = 0.0
        self._calculate_coefficients()

        # 控制器状态
        self.prev_error = 0.0  # 前次误差 e(k-1)
        self.prev_error2 = 0.0  # 前前次误差 e(k-2)
        self.output = 0.0  # 控制器总输出

    def _calculate_coefficients(self):
        """重新计算增量式PID系数"""
        if self.dt <= 0:
            return
        self.a0 = self.kp + (self.ki * self.dt / 2.0) + (2.0 * self.kd / self.dt)
        self.a1 = -self.kp + (self.ki * self.dt / 2.0) - (4.0 * self.kd / self.dt)
        self.a2 = (2.0 * self.kd) / self.dt

    def update_pressure_values(self, current_pressure, target_pressure):
        """更新当前压力和目标压力值"""
        self.current_pressure = current_pressure
        self.target_pressure = target_pressure

    def update(self, du_max=None):
        # 计算当前误差
        self.error = -(self.target_pressure - self.current_pressure)

        # if abs(self.error) < 1:
        #     return self.output  # 误差过小，直接返回当前输出

        # 计算控制增量
        delta = (self.a0 * self.error + self.a1 * self.prev_error + self.a2 * self.prev_error2)

        if du_max is not None:
            self.du_max = du_max
        else:
            self.du_max = self.get_du_max(self.target_pressure)

        # 纯 Python 限幅（替代 np.clip）
        if delta > self.du_max:
            delta = self.du_max
        elif delta < -self.du_max:
            delta = -self.du_max

        # 计算新输出
        new_output = self.output + delta
        # print(f"output:{self.output}, delta:{delta}, new_output:{new_output}")

        # 应用输出限幅
        new_output = max(self.out_min, min(self.out_max, new_output))

        # 更新历史状态
        self.prev_error2 = self.prev_error
        self.prev_error = self.error
        self.output = new_output
        return new_output

    def reset(self):
        """重置PID控制器状态（保留参数）"""
        self.prev_error = 0.0
        self.prev_error2 = 0.0
        self.output = 0.0

    def update_parameters(self, kp: float, ki: float, kd: float):
        self.kp = kp
        self.ki = ki
        self.kd = kd
        self._calculate_coefficients()

    def set_du_max(self, value):
        """设置 du_max（供外部模块通过方法调用设置，避免跨 .pyd 属性写入 crash）"""
        self.du_max = value

    def get_du_max(self, target_pressure):
        """根据目标压力计算 PID 最大增量限幅（纯 Python 线性插值）"""
        x = float(target_pressure)
        dt = float(self.dt)

        if x <= 0.0:
            val = 500.0 * dt

        elif x >= 200.0:
            val = 250.0 * dt

        elif x <= 100.0:
            # 0~100：从 500 线性下降到 300
            val = (500.0 - 2.0 * x) * dt

        else:
            # 100~200：从 300 线性下降到 250
            val = (350.0 - 0.5 * x) * dt

        return val

    def init_v(self, position_x):
        v = (self.xa_full - position_x) / (self.xa_full - self.dead_area) * 100
        return v