import time

class PID:
    def __init__(self, Gp=0, Gi=0, Gd=0, sign=1):
        self.Gp = Gp
        self.Gi = Gi
        self.Gd = Gd
        self.sign = sign

        self.reset()
    
    def reset(self):
        self.err_1dt_back = 0
        self.err_2dt_back = 0
        self.err_now      = 0
        self.last_update  = time.time()

    def feedback(self, err):
        # PID feedback
        # see https://en.wikipedia.org/wiki/PID_controller#Pseudocode
        self.err_2dt_back = self.err_1dt_back;
        self.err_1dt_back = self.err_now;
        self.err_now = err

        tnow = time.time()
        dt = tnow - self.last_update
        self.last_update = tnow
        A0= self.Gp+self.Gi*dt + self.Gd/dt;
        A1= -self.Gp-2*self.Gd/dt;
        A2= self.Gd/dt;

        u = A0*self.err_now + A1*self.err_1dt_back + A2*self.err_2dt_back;
        return u*self.sign