# -*- coding: utf-8 -*-

"""
author  Benoit Dubois
brief   PID servo
licence GPL v3+
"""

DEFAULT_PID_OMAX = 32000
DEFAULT_PID_INT_MAX = DEFAULT_PID_OMAX * 16
DEFAULT_PID_OSCALE = 1

class PIDfixed(object):

    def __init__(self, kp=1, ki=0, sp=0, osp=0, imax=DEFAULT_PID_INT_MAX,
                 omax=DEFAULT_PID_OMAX, oscale=DEFAULT_PID_OSCALE):
        """
        kp, ki: proportional and integral parameters
        sp: setpoint
        osp: output setpoint (output bias)
        imax: integrator maximum value
        omax: output range is [-omax, +omax]
        oscale: output scaling, output is devided by 2**oscale
        """
        self.kp = kp
        self.ki = ki
        self.sp = sp
        self.osp = osp
        self.imax = imax
        self.omax = omax
        self.oscale = oscale
        self.un = 0
        self._int_en = True
        self._sum = 0

    def compute(self, y0):
        eps = self.sp - y0
        if self._int_en is True:
            sum_ = min(self.imax, max(-self.imax, self._sum + eps))
        else:
            sum_ = 0
        vn = (self.kp * eps +  self.ki * sum_) >> self.oscale
        vn += self.osp
        self.un = min(self.omax, max(-self.omax, vn))
        self._sum = sum_ + self.un - vn # Anti windup
        return self.un

    @property
    def integrator(self):
        return self._sum

    def enable_int(self, value=True):
        self._int_en = value

    def reset(self):
        self._sum = 0
