import time

class PID:

    def __init__(self, setpoint = 0):
        
        self.setpoint = float(setpoint)
        
        self.Kp = 0
        self.Ki = 0
        self.Kd = 0
        
        self.min_out = 0
        self.max_out = 1000
        
        self.Reset()
        
    def SetMin(self, min_out):
        self.min_out = min_out
        
    def SetMax(self, max_out):
        self.max_out = max_out
    
    def SetSetpoint(self, setpoint):
        self.setpoint = float(setpoint)

    def SetKp(self, Kp):
        self.Kp = float(Kp)

    def SetKi(self, Ki):
        self.Ki = float(Ki)

    def SetKd(self, Kd):
        self.Kd = float(Kd)

    def Reset(self):
        
        self.current_time = time.time()
        self.last_time = self.current_time

        self.last_error = 0
        
        self.Cp = 0
        self.Ci = 0
        self.Cd = 0

    def Update(self, value):
        
        print("PID input: " + str(value))
        
        error = self.setpoint - value
        self.current_time = time.time()
        dt = self.current_time - self.last_time
        de = error - self.last_error
        
        self.Cp = self.Kp * error
        self.Ci += error * dt
        
        self.Cd = 0
        if dt > 0:
            self.Cd = de/dt
        
        self.last_time = self.current_time
        self.last_error = error
        
        output = self.Cp + (self.Ki * self.Ci) + (self.Kd * self.Cd)
        if output > self.max_out:
            output = self.max_out
        if output < self.min_out:
            output = self.min_out
        
        print("PID output: " + str(output))
        return output