import numpy as np;


class Pid():
    def __init__(self):
        # Integrator
        self.yi = 0

        # Differentiator
        self.xd = 0

        # Auto windup
        self.y_min = 0.0
        self.y_max = 2.0
        self.diff_aw = 0.0

        # Output
        self.y = 0
        self.yc = 0

    @staticmethod
    def params(kp, ki, kd, rho):
        p = dict(kp=kp, ki=ki, kd=kd, rho=rho)
        return p

    def reset(self):
        self.yi = 0
        self.xd = 0
        self.diff_aw = 0.0

    def process(self, dt, params, err, derr=None):
        kp = params['kp']
        ki = params['ki']
        kd = params['kd']
        rho = params['rho']

        err_aw = err + self.diff_aw
        yi = rho*self.yi + ki*dt * err_aw

        if derr:
            yd = derr
        else:
            yd = err - self.xd

        _yp = kp * err
        _yi = yi
        _yd = kd/dt * yd

        self.y = _yp + _yi + _yd

        yaw = yi
        if yaw < self.y_min:
            self.diff_aw = abs(yaw - self.y_min)

        if yaw > self.y_max:
            self.diff_aw = -abs(yaw - self.y_max)

        self.yc = max(self.y_min, min(self.y_max, self.y))

        self.yi = yi
        self.xd = err

    def get_y(self):
        return self.y

    def get_yc(self):
        return self.yc