import numpy as np
import matplotlib.pyplot as plt

# 自適應 PID 結構模擬
class AdaptivePID:
    def __init__(self, Kp, Ki, Kd, output_limit, integral_limit, max_error, max_derivative):
        self.Kp_base = Kp
        self.Ki_base = Ki
        self.Kd_base = Kd
        self.output_limit = output_limit
        self.integral_limit = integral_limit
        self.max_error = max_error
        self.max_derivative = max_derivative
        self.integral = 0.0
        self.prev_error = 0.0

    def compute(self, error, dt):
        if dt <= 0.0001:
            dt = 0.001

        derivative = (error - self.prev_error) / dt
        error_ratio = min(1.0, abs(error) / self.max_error)
        derivative_ratio = min(1.0, abs(derivative) / self.max_derivative)

        Kp = self.Kp_base * error_ratio
        Kd = self.Kd_base * derivative_ratio
        Ki = self.Ki_base if abs(error) < (self.max_error * 0.3) else 0.0

        self.integral += error * dt
        self.integral = max(min(self.integral, self.integral_limit), -self.integral_limit)

        output = Kp * error + Ki * self.integral + Kd * derivative
        output = max(min(output, self.output_limit), -self.output_limit)

        self.prev_error = error
        return output

# 初始化 PID
pid = AdaptivePID(Kp=1.0, Ki=0.02, Kd=0.1, output_limit=120.0,
                  integral_limit=50.0, max_error=12.0, max_derivative=5.0)

# 模擬範圍 -12 ~ 12 mm 誤差
errors = np.linspace(-12, 12, 200)
dt = 0.02  # 模擬每筆的時間差
outputs = [pid.compute(e, dt) for e in errors]

# 畫圖
plt.figure(figsize=(10, 5))
plt.plot(errors, outputs, label="RPM Output")
plt.axhline(120, color='red', linestyle='--', linewidth=0.5, label="RPM Limit (+120)")
plt.axhline(-120, color='blue', linestyle='--', linewidth=0.5, label="RPM Limit (-120)")
plt.title("Adaptive PID: Error vs RPM Output")
plt.xlabel("Position Error (mm)")
plt.ylabel("RPM Output")
plt.grid(True)
plt.legend()
plt.tight_layout()
plt.show()