import time
from qolab.hardware.basic import BasicInstrument

class PID(BasicInstrument):
    def __init__(self, Gp=0, Gi=0, Gd=0, sign=1):
        super().__init__()
        self.config['Device model'] = 'Generic Software PID loop'
        self.config['Device type']='PID loop'
        self.config['FnamePrefix'] = 'pid'
        self.deviceProperties = ['Gp', 'Gi', 'Gd', 'Sign', 'Enable' ];
        self.Gp = Gp
        self.Gi = Gi
        self.Gd = Gd
        self.sign = sign
        self.enable = True

        self.reset()

    def getGp(self):
        return self.Gp

    def setGp(self,val):
        self.Gp=val

    def getGi(self):
        return self.Gi

    def setGi(self,val):
        self.Gi=val

    def getGd(self):
        return self.Gd

    def setGd(self,val):
        self.Gd=val

    def getSign(self):
        return self.sign

    def setSign(self,val):
        self.sign=val

    def getEnable(self):
        return self.enable

    def setEnable(self,val):
        self.enable=val

    
    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;
        u *= self.sign
        if not self.enable:
            return 0
        return u