# *****************************************************************************
# 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 = 'ts',
        output = '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
from frappy.lib import clamp
from frappy.datatypes import LimitsType, EnumType, BoolType, FloatRange
from frappy.mixins 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 = Parameter('min output',
                             LimitsType(FloatRange()), default=(0, 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
    _clamp_limits = None

    def doPoll(self):
        super().doPoll()
        if self._clamp_limits is None:
            out = self.output_module
            if hasattr(out, 'max_target'):
                if hasattr(self, 'min_target'):
                    self._clamp_limits = lambda v, o=out: clamp(v, o.read_min_target(), o.read_max_target())
                else:
                    self._clamp_limits = lambda v, o=out: clamp(v, 0, o.read_max_target())
            elif hasattr(out, 'limit'):  # mercury.HeaterOutput
                self._clamp_limits = lambda v, o=out: clamp(v, 0, o.read_limit())
            else:
                self._clamp_limits = lambda v: v
            if self.output_range == (0.0, 0.0):
                self.output_range = (0, self._clamp_limits(float('inf')))
        if not self.control_active:
            return
        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
        out = self.output_module
        output = out.target
        if self.output_func == 'square':
            output = math.sqrt(max(0, output))
        output += self.p * deltadiff + self.i * deltat * diff
        if self.output_func == 'square':
            output = output ** 2
        output = self._clamp_limits(output)
        out.update_target(self.name, clamp(output, *self.output_range))

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

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


class PI(Writable):
    """temporary, but working version from Marek"""
    input = Attached(Readable, 'the input module')
    output = Attached(Writable, 'the output module')
    output_max = Parameter('max output value', FloatRange(0), readonly=False)
    p = Parameter('proportional term', FloatRange(0), readonly=False)
    i = Parameter('integral term', FloatRange(0), readonly=False)
    control_active = Parameter('control flag', BoolType(), readonly=False, default=False)
    value = Parameter(unit='K')
    tlim = Parameter('max Temperature', FloatRange(0), readonly=False)
    _lastdiff = None
    _lasttime = 0
    _lastvalue = 0

    def doPoll(self):
        super().doPoll()
        if not self.control_active:
            return
        self.value = self.input.value
        self.status = IDLE, 'controlling'
        now = time.time()
        deltat = min(10.0, now-self._lasttime)
        self._lasttime = now
        if self.value != self._lastvalue:
            diff = self.target - self.value   # calculate the difference to target
            self._lastvalue = self.value
        # else ? (diff is undefined!)
        if self.value > self.tlim:
            self.write_control_active(False)
            return
        if self._lastdiff is None:
            self._lastdiff = diff
        deltadiff = diff - self._lastdiff   # calculate the change in deltaT
        self._lastdiff = diff
        output = self.output.target
        output += self.p * deltadiff + self.i * deltat * diff
        if output > self.output_max:
            output = self.output_max
        elif output < 0:
            output = 0
        self.output.write_target(output)

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

    def write_target(self, value):
        self.control_active = True


# proposal for replacing above PI class, inheriting from PImixin
# additional features:
# - is a Drivable, using the convergence criteria from HasConvergence
# - tries to determine the output limits automatically
# unchecked!

class PIproposed(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