# *****************************************************************************
# This program is free software; you can redistribute it and/or modify it under
# the terms of the GNU General Public License as published by the Free Software
# Foundation; either version 2 of the License, or (at your option) any later
# version.
#
# This program is distributed in the hope that it will be useful, but WITHOUT
# ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
# FOR A PARTICULAR PURPOSE.  See the GNU General Public License for more
# details.
#
# You should have received a copy of the GNU General Public License along with
# this program; if not, write to the Free Software Foundation, Inc.,
# 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
#
# Module authors:
#   Markus Zolliker <markus.zolliker@psi.ch>
#   Jael Celia Lorenzana <jael-celia.lorenzana@psi.ch>
#   Marek Bartkowiak <marek.bartkowiak@psi.ch>
#
# *****************************************************************************

"""soft PI control

recipe using the PImixin:

assume you have class Sensor inheriting from Readable, you create a new class:

   class SensorWithLoop(HasConvergence, PImixin, Sensor):
       pass

and this is an example cfg

    Mod('T_sample',
        'frappy_psi.<your driver module>.SensorWithLoop',
        'controlled T',
        meaning=['temperature', 20],
        output_module='htr_sample',
        p=1,
        i=0.01,
    )


recipe using PI:

example cfg:

    Mod('T_softloop',
        'frappy_psi.picontrol.PI',
        'softloop controlled Temperature mixing chamber',
        input_module = 'ts',
        output_module = 'htr_mix',
        control_active = 1,
        output_max = 80000,
        p = 2E6,
        i = 10000,
        tlim = 1.0,
       )
"""

import time
import math
from frappy.core import Readable, Writable, Parameter, Attached, IDLE, Property
from frappy.lib import clamp
from frappy.datatypes import LimitsType, EnumType, BoolType, FloatRange
from frappy.newmixins import HasOutputModule
from frappy_psi.convergence import HasConvergence


class PImixin(HasOutputModule, Writable):
    p = Parameter('proportional term', FloatRange(0), readonly=False)
    i = Parameter('integral term', FloatRange(0), readonly=False)
    # output_module is inherited
    output_range = Property('legacy output range', LimitsType(FloatRange()), default=(0,0))
    output_min = Parameter('min output', FloatRange(), default=0, readonly=False)
    output_max = Parameter('max output', FloatRange(), default=0, readonly=False)
    output_func = Parameter('output function',
                            EnumType(lin=0, square=1), readonly=False, default=0)
    value = Parameter(unit='K')
    _lastdiff = None
    _lasttime = 0
    _get_range = None  # a function get output range from output_module
    _overflow = 0

    def initModule(self):
        super().initModule()
        if self.output_range != (0, 0):  # legacy !
            self.output_min, self.output_max = self.output_range

    def doPoll(self):
        super().doPoll()
        if not self.control_active:
            return
        out = self.output_module
        self.status = IDLE, 'controlling'
        now = time.time()
        deltat = clamp(0, now-self._lasttime, 10)
        self._lasttime = now
        diff = self.target - self.value
        if self._lastdiff is None:
            self._lastdiff = diff
        deltadiff = diff - self._lastdiff
        self._lastdiff = diff
        output, omin, omax = self._cvt2int(out.target)
        output += self._overflow + self.p * deltadiff + self.i * deltat * diff
        if output < omin:
            self._overflow = max(omin - omax, output - omin)
            output = omin
        elif output > omax:
            self._overflow = min(omax - omin, output - omax)
            output = omax
        else:
            self._overflow = 0
        out.update_target(self.name, self._cvt2ext(output))

    def cvt2int_square(self, output):
        return (math.sqrt(max(0, clamp(x, *self._get_range()))) for x in (output, self.output_min, self.output_max))

    def cvt2ext_square(self, output):
        return output ** 2

    def cvt2int_lin(self, output):
        return (clamp(x, *self._get_range()) for x in (output, self.output_min, self.output_max))

    def cvt2ext_lin(self, output):
        return output

    def write_output_func(self, value):
        out = self.output_module
        if hasattr(out, 'max_target'):
            if hasattr(self, 'min_target'):
                self._get_range = lambda o=out: (o.read_min_target(), o.read_max_target())
            else:
                self._get_range = lambda o=out: (0, o.read_max_target())
        elif hasattr(out, 'limit'):  # mercury.HeaterOutput
            self._get_range = lambda o=out: (0, o.read_limit())
        else:
            if self.output_min == self.output_max == 0:
                self.output_max = 1
            self._get_range = lambda o=self: (o.output_min, o.output_max)
        if self.output_min == self.output_max == 0:
            self.output_min, self.output_max = self._get_range()
        self.output_func = value
        self._cvt2int = getattr(self, f'cvt2int_{self.output_func.name}')
        self._cvt2ext = getattr(self, f'cvt2ext_{self.output_func.name}')

    def write_control_active(self, value):
        super().write_control_active(value)
        if not value:
            self.output_module.write_target(0)

    def write_target(self, _):
        if not self.control_active:
            self.activate_control()


# unchecked!

class PI(HasConvergence, PImixin):
    input_module = Attached(Readable, 'the input module')

    def read_value(self):
        return self.input_module.value

    def read_status(self):
        return self.input_module.status


class PI2(PI):
    maxovershoot = Parameter('max. overshoot', FloatRange(0, 100, unit='%'), readonly=False, default=20)

    def doPoll(self):
        self.output_max = self.target * (1 + 0.01 * self.maxovershoot)
        self.output_min = self.target * (1 - 0.01 * self.maxovershoot)
        super().doPoll()

    def write_target(self, target):
        if not self.control_active:
            self.output.write_target(target)
        super().write_target(target)