#The recipe gives simple implementation of a Discrete Proportional-Integral-Derivative (PID) controller. PID controller gives output value for error between desired reference input and measurement feedback to minimize error value.
#More information: http://en.wikipedia.org/wiki/PID_controller
#
#cnr437@gmail.com
#
#######	Example	#########
#
#p=PID(3.0,0.4,1.2)
#p.setPoint(5.0)
#while True:
#     pid = p.update(measurement_value)
#
#


class PID:
    """
    Discrete PID control
    """
    
    def __init__(self, P=2.0, I=0.0, D=1.0, Derivator=0, Integrator=0, Integrator_max=500, Integrator_min=-500):
        
        self.Kp=P
        self.Ki=I
        self.Kd=D
        self.Derivator=Derivator
        self.Integrator=Integrator
        self.Integrator_max=Integrator_max
        self.Integrator_min=Integrator_min
        
        self.set_point=0.0
        self.error=0.0
        
    def update(self,current_value):
        """
        Calculate PID output value for given reference input and feedback
        """
        
        self.error = self.set_point - current_value
        
        self.P_value = self.Kp * self.error
        self.D_value = self.Kd * ( self.error - self.Derivator)
        self.Derivator = self.error
        
        self.Integrator = self.Integrator + self.error
        
        if self.Integrator > self.Integrator_max:
            self.Integrator = self.Integrator_max
        elif self.Integrator < self.Integrator_min:
            self.Integrator = self.Integrator_min
            
        self.I_value = self.Integrator * self.Ki
        
        PID = self.P_value + self.I_value + self.D_value
        
        return PID
    
    def setPoint(self,set_point):
        """
        Initilize the setpoint of PID
        """
        self.set_point = set_point
        self.Integrator=0
        self.Derivator=0
        
    def setIntegrator(self, Integrator):
        self.Integrator = Integrator
        
    def setDerivator(self, Derivator):
        self.Derivator = Derivator
        
    def setKp(self,P):
        self.Kp=P
    
    def setKi(self,I):
        self.Ki=I
                    
    def setKd(self,D):
        self.Kd=D

    def getPoint(self):
        return self.set_point

    def getError(self):
        return self.error

    def getIntegrator(self):
        return self.Integrator

    def getDerivator(self):
        return self.Derivator
    
def xEqualsVT(x,v,t):
    """ the new pos is just the old pos + vt
    """
    return x + v*t

def updatedX(sx,dx,vx,vd,dt,p):
    """ let the PID update the v
    """
    vd = p.update(dx-sx)
    return xEqualsVT(x,dvp.update(x),dt)


p = PID(0.6,0.4,0.3)
p.setPoint(0.0)

def loop(vars,p):
    """[xSk,xD,vSk,vDn,dt],p"""
    [xSk,xD,vSk,vDn,dt] = vars
    xSk += vSk*dt
    xD += vDn * dt
    vDn = p.update(xD-xSk)
    print xD-xSk
    return [xSk,xD,vSk,vDn,dt]

