"""
Seguimiento del movimiento de la mascota principal.

El cálculo original comparaba el centroide entre dos fotogramas consecutivos, y
nada más. Con la Tapo a 15 fps eso da falsos positivos por tres vías distintas:

  1. Mover la cámara: el detector encuentra "algo" antes y después del cambio de
     encuadre, y el salto entre ambas posiciones parece una carrera. Es el caso
     real que se dio el 2026-10-01.
  2. Parpadeo del detector: la caja se desplaza unos píxeles de un fotograma al
     siguiente sin que el animal se mueva.
  3. Cambio de sujeto: si hay dos animales, o el detector duda, el centroide
     salta de uno a otro.

Medir la velocidad fotograma a fotograma es muy ruidoso: con la caja temblando
+-2% del encuadre, la velocidad instantánea llega a 0.8 norm/s sin que nadie se
mueva. Por eso aquí la velocidad se mide como el **desplazamiento neto sobre una
ventana de tiempo** (0.2 s por defecto): el temblor se cancela, el movimiento
real no.

Salvaguardas:
  - Asociación con la detección más cercana a la anterior, no con la primera.
  - Descarte de saltos imposibles para un animal (cámara movida, cambio de sujeto).
  - Velocidad medida sobre una ventana, no entre dos fotogramas.
  - Exigencia de varios fotogramas seguidos por encima del umbral.
  - La escena puede reiniciar el seguimiento cuando cambia entera (lo detecta
    `pet_speed.py` comparando miniaturas de fotograma).
"""

import math
from collections import namedtuple

# Detección ya convertida a píxeles (caja) y a coordenadas normalizadas (centro)
Deteccion = namedtuple("Deteccion", "etiqueta confianza caja cx cy")


class MotionTracker:
    """Decide si la mascota seguida está corriendo."""

    def __init__(
        self,
        speed_threshold=0.30,
        frames_required=3,
        window=0.2,
        max_plausible_speed=3.0,
        max_history=10,
    ):
        self.speed_threshold = speed_threshold
        self.frames_required = frames_required
        self.window = window
        self.max_plausible_speed = max_plausible_speed
        self.max_history = max_history
        self.discarded = 0   # saltos imposibles descartados
        self.reset()

    def reset(self):
        """Olvida el seguimiento actual (cambio de escena, o pérdida del sujeto)."""
        self._historial = []
        self._racha = 0
        self.speed = 0.0

    def update(self, detecciones, timestamp):
        """
        detecciones: lista de Deteccion del fotograma actual (puede ir vacía).
        Devuelve (deteccion_elegida, velocidad, corriendo) o None si no hay nada.
        """
        elegida = self._asocia(detecciones)
        if elegida is None:
            self.reset()
            return None

        # ¿Salto imposible desde el fotograma anterior?
        if self._historial:
            t_prev, x_prev, y_prev = self._historial[-1]
            dt_prev = timestamp - t_prev
            if dt_prev > 0 and math.hypot(elegida.cx - x_prev, elegida.cy - y_prev) / dt_prev > self.max_plausible_speed:
                # Ningún animal cruza el encuadre varias veces por segundo: esto
                # es la cámara moviéndose o el detector saltando a otro objeto.
                self.discarded += 1
                self._historial = [(timestamp, elegida.cx, elegida.cy)]
                self._racha = 0
                self.speed = 0.0
                return elegida, 0.0, False

        self._historial.append((timestamp, elegida.cx, elegida.cy))
        while len(self._historial) > 2 and timestamp - self._historial[0][0] > self.window:
            self._historial.pop(0)
        if len(self._historial) > self.max_history:
            self._historial.pop(0)

        # Velocidad = desplazamiento neto sobre la ventana disponible
        t0, x0, y0 = self._historial[0]
        lapso = timestamp - t0
        if len(self._historial) < 2 or lapso < self.window * 0.5:
            # Todavía no hay ventana suficiente para juzgar
            self.speed = 0.0
            return elegida, 0.0, False

        self.speed = math.hypot(elegida.cx - x0, elegida.cy - y0) / lapso

        if self.speed > self.speed_threshold:
            self._racha += 1
        else:
            self._racha = 0

        return elegida, self.speed, self._racha >= self.frames_required

    def _asocia(self, detecciones):
        """Se queda con la detección más parecida a la que venía siguiendo."""
        if not detecciones:
            return None
        if not self._historial:
            # Sin historial, la más fiable
            return max(detecciones, key=lambda d: d.confianza)
        _, cx, cy = self._historial[-1]
        return min(detecciones, key=lambda d: math.hypot(d.cx - cx, d.cy - cy))