"""
_vo_drift.py — Visual odometry drift detector helpers (no ROS2 deps).

Algorithm
---------
Two independent odometry streams (visual and wheel) are compared over a
sliding time window.  Drift is measured as the absolute difference in
cumulative path length travelled by each source over that window:

    drift_m = |path_length(vo_window) − path_length(wheel_window)|

Using cumulative path length (sum of inter-sample Euclidean steps) rather
than straight-line displacement makes the measure robust to circular motion
where start and end positions are the same.

Drift is flagged when drift_m ≥ drift_threshold_m.

Public API
----------
  OdomSample          — namedtuple(t, x, y)
  OdomBuffer          — deque of OdomSamples with time-window trimming
  compute_drift()     — compare two OdomBuffers and return DriftResult
  DriftResult         — namedtuple(drift_m, vo_path_m, wheel_path_m,
                                   is_drifting, window_s, n_vo, n_wheel)
"""

from __future__ import annotations

import math
from collections import deque
from typing import NamedTuple, Sequence


class OdomSample(NamedTuple):
    t: float    # monotonic timestamp (seconds)
    x: float    # position x (metres)
    y: float    # position y (metres)


class DriftResult(NamedTuple):
    drift_m:     float   # |vo_path − wheel_path| (metres)
    vo_path_m:   float   # cumulative path of VO source over window (metres)
    wheel_path_m: float  # cumulative path of wheel source over window (metres)
    is_drifting: bool    # True when drift_m >= threshold
    window_s:    float   # actual time span of data used (seconds)
    n_vo:        int     # number of VO samples in window
    n_wheel:     int     # number of wheel samples in window


class OdomBuffer:
    """
    Rolling buffer of OdomSamples trimmed to the last `max_age_s` seconds.

    Parameters
    ----------
    max_age_s : float  — samples older than this are discarded (seconds)
    """

    def __init__(self, max_age_s: float = 10.0) -> None:
        self._max_age = max_age_s
        self._buf: deque[OdomSample] = deque()

    # ── Public ────────────────────────────────────────────────────────────────

    def push(self, sample: OdomSample) -> None:
        """Append a sample and evict anything older than max_age_s."""
        self._buf.append(sample)
        self._trim(sample.t)

    def window(self, window_s: float, now: float) -> list[OdomSample]:
        """Return samples within the last window_s seconds of `now`."""
        cutoff = now - window_s
        return [s for s in self._buf if s.t >= cutoff]

    def clear(self) -> None:
        self._buf.clear()

    def __len__(self) -> int:
        return len(self._buf)

    # ── Internal ──────────────────────────────────────────────────────────────

    def _trim(self, now: float) -> None:
        cutoff = now - self._max_age
        while self._buf and self._buf[0].t < cutoff:
            self._buf.popleft()


# ── Core computation ──────────────────────────────────────────────────────────

def compute_drift(
    vo_buf:          OdomBuffer,
    wheel_buf:       OdomBuffer,
    window_s:        float,
    drift_threshold_m: float,
    now:             float,
) -> DriftResult:
    """
    Compare VO and wheel odometry path lengths over the last `window_s`.

    Parameters
    ----------
    vo_buf            : OdomBuffer of visual odometry samples
    wheel_buf         : OdomBuffer of wheel odometry samples
    window_s          : comparison window width (seconds)
    drift_threshold_m : drift_m threshold for is_drifting flag
    now               : current time (same scale as OdomSample.t)

    Returns
    -------
    DriftResult — zero drift if either buffer has fewer than 2 samples.
    """
    vo_samples    = vo_buf.window(window_s, now)
    wheel_samples = wheel_buf.window(window_s, now)

    if len(vo_samples) < 2 or len(wheel_samples) < 2:
        return DriftResult(
            drift_m=0.0, vo_path_m=0.0, wheel_path_m=0.0,
            is_drifting=False,
            window_s=0.0, n_vo=len(vo_samples), n_wheel=len(wheel_samples),
        )

    vo_path    = _path_length(vo_samples)
    wheel_path = _path_length(wheel_samples)
    drift_m    = abs(vo_path - wheel_path)

    # Actual data span = latest timestamp − earliest across both buffers
    t_min = min(vo_samples[0].t,    wheel_samples[0].t)
    t_max = max(vo_samples[-1].t,   wheel_samples[-1].t)
    actual_window = t_max - t_min

    return DriftResult(
        drift_m=drift_m,
        vo_path_m=vo_path,
        wheel_path_m=wheel_path,
        is_drifting=drift_m >= drift_threshold_m,
        window_s=actual_window,
        n_vo=len(vo_samples),
        n_wheel=len(wheel_samples),
    )


def _path_length(samples: Sequence[OdomSample]) -> float:
    """Sum of Euclidean inter-sample distances."""
    total = 0.0
    for i in range(1, len(samples)):
        dx = samples[i].x - samples[i - 1].x
        dy = samples[i].y - samples[i - 1].y
        total += math.sqrt(dx * dx + dy * dy)
    return total
