import numpy as np
import time

class LowPassFilter:
    def __init__(self, alpha):
        self.__set_alpha(alpha)
        self.__y = None

    def __set_alpha(self, alpha):
        if not (0.0 <= alpha <= 1.0):
            raise ValueError("alpha must be between 0.0 and 1.0")
        self.__alpha = alpha

    def __call__(self, value, timestamp=None, alpha=None):
        if alpha is not None:
            self.__set_alpha(alpha)
        if self.__y is None:
            self.__y = value
        else:
            self.__y = self.__alpha * value + (1.0 - self.__alpha) * self.__y
        return self.__y

    def last_value(self):
        return self.__y

class OneEuroFilter:
    """
    Adaptive low-pass filter for jitter suppression.
    Based on: http://www.lifl.fr/~casiez/1euro/
    """
    def __init__(self, freq, min_cutoff=1.0, beta=0.007, d_cutoff=1.0):
        self.__freq = freq
        self.__min_cutoff = min_cutoff
        self.__beta = beta
        self.__d_cutoff = d_cutoff
        self.__x_filt = LowPassFilter(self.__alpha(min_cutoff))
        self.__dx_filt = LowPassFilter(self.__alpha(d_cutoff))
        self.__last_time = None

    def __alpha(self, cutoff):
        tau = 1.0 / (2 * np.pi * cutoff)
        te = 1.0 / self.__freq
        return 1.0 / (1.0 + tau / te)

    def __call__(self, x, timestamp=None):
        # Update frequency if timestamp is provided
        if timestamp is not None and self.__last_time is not None:
            self.__freq = 1.0 / (timestamp - self.__last_time)
        self.__last_time = timestamp

        # Estimate the derivative
        prev_x = self.__x_filt.last_value()
        dx = 0.0 if prev_x is None else (x - prev_x) * self.__freq
        edx = self.__dx_filt(dx, alpha=self.__alpha(self.__d_cutoff))
        
        # Compute the optimal cutoff frequency
        cutoff = self.__min_cutoff + self.__beta * abs(edx)
        
        # Filter the value
        return self.__x_filt(x, alpha=self.__alpha(cutoff))

if __name__ == "__main__":
    # Quick test
    filter = OneEuroFilter(freq=30, min_cutoff=1.0, beta=0.01)
    
    data = [10.0, 10.1, 10.5, 13.0, 15.0, 15.1]
    timestamps = [0.0, 0.033, 0.066, 0.1, 0.133, 0.166]
    
    print("Testing OneEuroFilter:")
    for val, ts in zip(data, timestamps):
        filtered = filter(val, timestamp=ts)
        print(f"Input: {val:.2f}, Filtered: {filtered:.2f}")