import math

from edge.models import Keypoint


class OneEuroFilter:
    """1€ filter (Casiez et al.). 저지연 지터 감쇠."""

    def __init__(self, min_cutoff: float = 1.0, beta: float = 0.0, d_cutoff: float = 1.0):
        self.min_cutoff = min_cutoff
        self.beta = beta
        self.d_cutoff = d_cutoff
        self._x_prev: float | None = None
        self._dx_prev: float = 0.0
        self._t_prev: float | None = None

    @staticmethod
    def _alpha(cutoff: float, dt: float) -> float:
        tau = 1.0 / (2 * math.pi * cutoff)
        return 1.0 / (1.0 + tau / dt)

    def filter(self, value: float, t: float) -> float:
        if self._x_prev is None or self._t_prev is None or t <= self._t_prev:
            self._x_prev, self._t_prev, self._dx_prev = value, t, 0.0
            return value
        dt = t - self._t_prev
        dx = (value - self._x_prev) / dt
        a_d = self._alpha(self.d_cutoff, dt)
        dx_hat = a_d * dx + (1 - a_d) * self._dx_prev
        cutoff = self.min_cutoff + self.beta * abs(dx_hat)
        a = self._alpha(cutoff, dt)
        x_hat = a * value + (1 - a) * self._x_prev
        self._x_prev, self._dx_prev, self._t_prev = x_hat, dx_hat, t
        return x_hat


class KeypointSmoother:
    """관절 17개 각각 x,y에 독립 1€ 필터."""

    def __init__(self, min_cutoff: float = 1.0, beta: float = 0.007):
        self._fx = [OneEuroFilter(min_cutoff, beta) for _ in range(17)]
        self._fy = [OneEuroFilter(min_cutoff, beta) for _ in range(17)]

    def smooth(self, keypoints: list[Keypoint], t: float) -> list[Keypoint]:
        out = []
        for i, k in enumerate(keypoints):
            out.append(Keypoint(self._fx[i].filter(k.x, t),
                                self._fy[i].filter(k.y, t), k.conf))
        return out