"""PID position control of one robot joint, simulated (no hardware needed).

The joint: a motor turns an arm link. Physics in one line:
    J * acceleration = motor torque - friction * speed - gravity load
"""
import math


class PID:
    def __init__(self, kp, ki, kd, out_min=-math.inf, out_max=math.inf):
        self.kp, self.ki, self.kd = kp, ki, kd
        self.out_min, self.out_max = out_min, out_max
        self.integral = 0.0
        self.prev_measurement = None

    def update(self, target, measurement, dt):
        error = target - measurement

        # D: how fast is it changing? (use the measurement, not the error,
        # so a sudden new target does not cause a "kick")
        if self.prev_measurement is None:
            rate = 0.0
        else:
            rate = -(measurement - self.prev_measurement) / dt
        self.prev_measurement = measurement

        # I: add up the past, but only while the motor is not maxed out
        candidate = self.integral + error * dt
        out = self.kp * error + self.ki * candidate + self.kd * rate
        if self.out_min < out < self.out_max:
            self.integral = candidate          # anti-windup: freeze when saturated
        self.terms = (self.kp * error, self.ki * self.integral, self.kd * rate)
        out = sum(self.terms)
        return max(self.out_min, min(self.out_max, out))


class Joint:
    def __init__(self, inertia=0.02, friction=0.05, gravity=0.6):
        self.J, self.b, self.load = inertia, friction, gravity
        self.angle = 0.0      # rad
        self.speed = 0.0      # rad/s

    def step(self, torque, dt):
        accel = (torque - self.b * self.speed - self.load) / self.J
        self.speed += accel * dt
        self.angle += self.speed * dt
        return self.angle


def simulate(kp, ki, kd, target_deg=90.0, seconds=3.0, dt=0.001,
             gravity=0.6, torque_limit=3.0, windup_guard=True):
    joint = Joint(gravity=gravity)
    lim = torque_limit
    pid = PID(kp, ki, kd, -lim, lim)
    if not windup_guard:                       # for the "integral windup" demo
        pid.out_min, pid.out_max = -math.inf, math.inf
    target = math.radians(target_deg)
    t, angles, torques, terms = [], [], [], []
    for k in range(int(seconds / dt)):
        angle = joint.angle
        u = pid.update(target, angle, dt)
        if not windup_guard:
            u = max(-lim, min(lim, u))
        terms.append(pid.terms)
        joint.step(u, dt)
        t.append(k * dt)
        angles.append(math.degrees(angle))
        torques.append(u)
    return t, angles, torques, terms


def describe(angles, t, target=90.0):
    peak = max(angles)
    over = max(0.0, peak - target)
    final = sum(angles[-200:]) / 200
    # settling: last time outside ±2 % band
    band = 0.02 * target
    settle = 0.0
    for ti, a in zip(t, angles):
        if abs(a - target) > band:
            settle = ti
    return over, final, settle


if __name__ == "__main__":
    for name, g in [("P only", (8, 0, 0)), ("PD", (8, 0, 0.5)), ("PID", (8, 12, 0.5))]:
        t, a, _, _ = simulate(*g)
        over, final, settle = describe(a, t)
        print(f"{name:7s} Kp={g[0]:<4} Ki={g[1]:<4} Kd={g[2]:<4} "
              f"overshoot {over:5.1f} deg  final {final:6.2f} deg  settles {settle:4.2f} s")