text: # -*- coding: utf-8 -*-
  #
  # OneEuroFilter.py -
  #
  # Author: Nicolas Roussel (nicolas.roussel@inria.fr)
  # Copyright 2019 Inria
  # 
  # BSD License https://opensource.org/licenses/BSD-3-Clause
  #
  # Redistribution and use in source and binary forms, with or without
  # modification, are permitted provided that the following conditions are met:
  #
  #  1. Redistributions of source code must retain the above copyright notice, this list of conditions
  # and the following disclaimer.
  #
  # 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions
  # and the following disclaimer in the documentation and/or other materials provided with the distribution.
  # 
  # 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or
  # promote products derived from this software without specific prior written permission.
  
  # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, 
  # INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
  # DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
  # SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
  # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 
  # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
  # SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  #
  
  import math
  
  # ----------------------------------------------------------------------------
  
  class LowPassFilter(object):
  
      def __init__(self, alpha):
          self.__setAlpha(alpha)
          self.__y = self.__s = None
  
      def __setAlpha(self, alpha):
          alpha = float(alpha)
          if alpha<=0 or alpha>1.0:
              raise ValueError("alpha (%s) should be in (0.0, 1.0]"%alpha)
          self.__alpha = alpha
  
      def __call__(self, value, timestamp=None, alpha=None):        
          if alpha is not None:
              self.__setAlpha(alpha)
          if self.__y is None:
              s = value
          else:
              s = self.__alpha*value + (1.0-self.__alpha)*self.__s
          self.__y = value
          self.__s = s
          return s
  
      def lastValue(self):
          return self.__y
  
  # ----------------------------------------------------------------------------
  
  class OneEuroFilter(object):
  
      def __init__(self, freq, mincutoff=1.0, beta=0.0, dcutoff=1.0):
          if freq<=0:
              raise ValueError("freq should be >0")
          if mincutoff<=0:
              raise ValueError("mincutoff should be >0")
          if dcutoff<=0:
              raise ValueError("dcutoff should be >0")
          self.__freq = float(freq)
          self.__mincutoff = float(mincutoff)
          self.__beta = float(beta)
          self.__dcutoff = float(dcutoff)
          self.__x = LowPassFilter(self.__alpha(self.__mincutoff))
          self.__dx = LowPassFilter(self.__alpha(self.__dcutoff))
          self.__lasttime = None
          
      def __alpha(self, cutoff):
          te    = 1.0 / self.__freq
          tau   = 1.0 / (2*math.pi*cutoff)
          return  1.0 / (1.0 + tau/te)
  
      def __call__(self, x, timestamp=None):
          # ---- update the sampling frequency based on timestamps
          if self.__lasttime and timestamp:
              self.__freq = 1.0 / (timestamp-self.__lasttime)
          self.__lasttime = timestamp
          # ---- estimate the current variation per second
          prev_x = self.__x.lastValue()
          dx = 0.0 if prev_x is None else (x-prev_x)*self.__freq # FIXME: 0.0 or value?
          edx = self.__dx(dx, timestamp, alpha=self.__alpha(self.__dcutoff))
          # ---- use it to update the cutoff frequency
          cutoff = self.__mincutoff + self.__beta*math.fabs(edx)
          # ---- filter the given value
          return self.__x(x, timestamp, alpha=self.__alpha(cutoff))
  
  # ----------------------------------------------------------------------------
  
  if __name__=="__main__":
  
      import random
  
      duration = 10.0 # seconds
      
      config = {
          'freq': 120,       # Hz
          'mincutoff': 1.0,  # FIXME
          'beta': 1.0,       # FIXME
          'dcutoff': 1.0     # this one should be ok
          }
      
      print("#SRC OneEuroFilter.py")
      print("#CFG %s"%config)
      print("#LOG timestamp, signal, noisy, filtered")
      
      f = OneEuroFilter(**config)
      timestamp = 0.0 # seconds
      while timestamp<duration:
          signal = math.sin(timestamp)
          noisy = signal + (random.random()-0.5)/5.0
          filtered = f(noisy, timestamp)
          print("{0}, {1}, {2}, {3}".format(timestamp, signal, noisy, filtered))
          timestamp += 1.0/config["freq"]