"""
accuracy_stats.py — UWB positioning accuracy statistics (Issue #634)

Pure numpy, no ROS2 dependency.
"""

from __future__ import annotations

import math
from dataclasses import dataclass, field

import numpy as np


@dataclass
class AccuracyStats:
    n_samples: int
    mean_x: float
    mean_y: float
    std_x: float
    std_y: float
    std_2d: float
    bias_x: float
    bias_y: float
    cep50: float
    cep95: float
    max_error: float
    rmse: float


def compute_stats(
    xs: np.ndarray,
    ys: np.ndarray,
    truth_x: float,
    truth_y: float,
) -> AccuracyStats:
    """
    Compute accuracy statistics from position samples.

    Parameters
    ----------
    xs, ys       : 1-D arrays of measured x / y positions (metres)
    truth_x/y    : known ground-truth position (metres)

    Raises
    ------
    ValueError if fewer than 2 samples or mismatched lengths.
    """
    xs = np.asarray(xs, dtype=float)
    ys = np.asarray(ys, dtype=float)
    n = len(xs)
    if n < 2:
        raise ValueError(f"Need at least 2 samples, got {n}")
    if len(ys) != n:
        raise ValueError("xs and ys must have the same length")

    errors = np.sqrt((xs - truth_x) ** 2 + (ys - truth_y) ** 2)

    mean_x = float(np.mean(xs))
    mean_y = float(np.mean(ys))
    std_x  = float(np.std(xs, ddof=1))
    std_y  = float(np.std(ys, ddof=1))

    return AccuracyStats(
        n_samples  = n,
        mean_x     = mean_x,
        mean_y     = mean_y,
        std_x      = std_x,
        std_y      = std_y,
        std_2d     = math.sqrt(std_x ** 2 + std_y ** 2),
        bias_x     = mean_x - truth_x,
        bias_y     = mean_y - truth_y,
        cep50      = float(np.percentile(errors, 50)),
        cep95      = float(np.percentile(errors, 95)),
        max_error  = float(np.max(errors)),
        rmse       = float(np.sqrt(np.mean(errors ** 2))),
    )


@dataclass
class RangeAccum:
    """Running accumulator for per-anchor range statistics."""
    anchor_id: int
    _samples: list = field(default_factory=list, repr=False)

    def add(self, range_m: float) -> None:
        self._samples.append(range_m)

    @property
    def count(self) -> int:
        return len(self._samples)

    @property
    def mean(self) -> float:
        return float(np.mean(self._samples)) if self._samples else 0.0

    @property
    def std(self) -> float:
        return float(np.std(self._samples, ddof=1)) if len(self._samples) >= 2 else 0.0
