import numpy as np


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

        # Differentiator
        self.err = 0

        # Auto windup
        self.y_min = -1.0
        self.y_max = 1.0

        # Output
        self.y = 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.err = 0

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

        self.yi = rho*self.yi + ki*dt * err
        yd = kd/dt*(err - self.err)
        yp = kp * err

        # Reset yi on error sign changes
        if yp > self.y_max or yp < self.y_min:
            self.yi = 0

        self.err = err

        y = yp + self.yi + yd

        self.y = max(self.y_min, min(self.y_max, y))
        print("yi = {}".format(self.yi))

    def get_y(self):
        return self.y