# ***** BEGIN LICENSE BLOCK *****
# Version: MPL 1.1/GPL 2.0/LGPL 2.1
#
# The contents of this file are subject to the Mozilla Public License Version
# 1.1 (the "License"); you may not use this file except in compliance with
# the License. You may obtain a copy of the License at
# http://www.mozilla.org/MPL/
#
# Software distributed under the License is distributed on an "AS IS" basis,
# WITHOUT WARRANTY OF ANY KIND, either express or implied. See the License
# for the specific language governing rights and limitations under the
# License.
#
# The Original Code is the Python Computer Graphics Kit.
#
# The Initial Developer of the Original Code is Matthias Baas.
# Portions created by the Initial Developer are Copyright (C) 2004
# the Initial Developer. All Rights Reserved.
#
# Contributor(s):
#
# Alternatively, the contents of this file may be used under the terms of
# either the GNU General Public License Version 2 or later (the "GPL"), or
# the GNU Lesser General Public License Version 2.1 or later (the "LGPL"),
# in which case the provisions of the GPL or the LGPL are applicable instead
# of those above. If you wish to allow use of your version of this file only
# under the terms of either the GPL or the LGPL, and not to allow others to
# use your version of this file under the terms of the MPL, indicate your
# decision by deleting the provisions above and replace them with the notice
# and other provisions required by the GPL or the LGPL. If you do not delete
# the provisions above, a recipient may use your version of this file under
# the terms of any one of the MPL, the GPL or the LGPL.
#
# ***** END LICENSE BLOCK *****
# $Id: pidcontroller.py,v 1.1.1.1 2004/12/12 14:31:11 mbaas Exp $

## \file pidcontroller.py
## Contains the PIDController class.

import component
import eventmanager, events
from scene import getScene
from slots import *
from cgtypes import *
import _core

# PIDController
class PIDController(component.Component):
    """PID controller.

    """

    exec slotPropertyCode("input")
    exec slotPropertyCode("output")
    exec slotPropertyCode("setpoint")
    exec slotPropertyCode("maxout")
    exec slotPropertyCode("minout")
    exec slotPropertyCode("Kp")
    exec slotPropertyCode("Ki")
    exec slotPropertyCode("Kd")

    def __init__(self,
                 name = "PIDController",
                 setpoint = 0.0,
                 Kp = 0.0,
                 Ki = 0.0,
                 Kd = 0.0,
                 maxout = 999999,
                 minout = -999999,
                 auto_insert = True):
        """Constructor.
        """
        
        component.Component.__init__(self, name, auto_insert)

        self.input_slot = DoubleSlot()
        self.setpoint_slot = DoubleSlot(setpoint)
        self.maxout_slot = DoubleSlot(maxout)
        self.minout_slot = DoubleSlot(minout)
        self.Kp_slot = DoubleSlot(Kp)
        self.Ki_slot = DoubleSlot(Ki)
        self.Kd_slot = DoubleSlot(Kd)

        self.output_slot = ProceduralDoubleSlot(self.computeOutput)

        self.addSlot("input", self.input_slot)
        self.addSlot("setpoint", self.setpoint_slot)
        self.addSlot("output", self.output_slot)
        self.addSlot("maxout", self.maxout_slot)
        self.addSlot("minout", self.minout_slot)
        self.addSlot("Kp", self.Kp_slot)
        self.addSlot("Ki", self.Ki_slot)
        self.addSlot("Kd", self.Kd_slot)

        self.input_slot.addDependent(self.output_slot)
        self.setpoint_slot.addDependent(self.output_slot)
        self.maxout_slot.addDependent(self.output_slot)
        self.minout_slot.addDependent(self.output_slot)
        self.Kp_slot.addDependent(self.output_slot)
        self.Ki_slot.addDependent(self.output_slot)
        self.Kd_slot.addDependent(self.output_slot)

        self._integral = 0.0
        self._prev_err = 0.0

        eventmanager.eventManager().connect(events.STEP_FRAME, self)
        eventmanager.eventManager().connect(events.RESET, self)
        

    def onStepFrame(self):
        err = self.setpoint-self.input
        dt = getScene().timer().timestep
        self._integral += dt*err

    def onReset(self):
        self._integral = 0.0
        self._prev_err = 0.0

    def computeOutput(self):
        err = self.setpoint-self.input
        dt = getScene().timer().timestep
        I = self._integral
        D = (err-self._prev_err)/dt
#        print "D:",D
        res = self.Kp*err + self.Ki*I + self.Kd*D
        
        self._prev_err = err
        
        maxout = self.maxout
        minout = self.minout
        if res>maxout:
            res = maxout
        elif res<minout:
            res = minout
            
        return res
