#!/usr/bin/env python
# vim: ft=python ts=8 sts=4 sw=4 expandtab autoindent smartindent nocindent
#
# Class for device simulation using: Proportional-Integral-Derivative (PID)
#
# This 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)
#
#
"""
TODO:
* Look into making the update time based because the logic seems to assume constant (1 second) time
  - Derivator should be dE/dT but is just dE
  - Integrator should be sigma(dT*E) but is just sigma(E)
* Look into making the Derivator based on changes in the PV instead of the Error
"""

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
        # TODO: check the sign is correct
        self.D_value = self.Kd * (current_value - self.Derivator)
        self.Derivator = current_value

        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

if __name__ == "__main__":
#p=PID(3.0,0.4,1.2)
#p.setPoint(5.0)
#while True:
#     pid = p.update(measurement_value)
    p = PID(3.0, 0.4, 1.2)
    p.setPoint(20.0)
    pv = 20
    for i in range(0,61):
        print p.update(pv)