"""IMU unit, axis, bias, and saturation audit."""

from __future__ import annotations

from dataclasses import dataclass

import numpy as np

from .contracts import ImuSeries

G = 9.80665


@dataclass(frozen=True)
class ImuAuditReport:
    ok: bool
    gyro_bias_rad_s: np.ndarray
    static_acc_mean_m_s2: np.ndarray
    static_acc_norm_m_s2: float
    suggested_up_axis: int
    suggested_up_sign: float
    static_ratio: float
    notes: tuple[str, ...] = ()


def _static_mask(gyro: np.ndarray, acc: np.ndarray) -> np.ndarray:
    gyro_norm = np.linalg.norm(gyro, axis=1)
    acc_norm = np.linalg.norm(acc, axis=1)
    gyro_thr = max(0.02, float(np.percentile(gyro_norm, 20)) * 1.5)
    acc_thr_low = 0.7 * G
    acc_thr_high = 1.3 * G
    return (gyro_norm < gyro_thr) & (acc_norm > acc_thr_low) & (acc_norm < acc_thr_high)


def audit_imu(imu: ImuSeries) -> ImuAuditReport:
    """Audit normalized IMU samples and estimate a static gyro bias."""

    notes: list[str] = []
    mask = _static_mask(imu.gyro_rad_s, imu.acc_m_s2)
    static_ratio = float(np.mean(mask)) if mask.size else 0.0
    if static_ratio < 0.02:
        # Fall back to lowest-gyro percentile window.
        gyro_norm = np.linalg.norm(imu.gyro_rad_s, axis=1)
        cutoff = float(np.percentile(gyro_norm, 10))
        mask = gyro_norm <= cutoff
        notes.append("few gravity-consistent static samples; using lowest-gyro percentile")
        static_ratio = float(np.mean(mask))

    if not np.any(mask):
        notes.append("no static samples found")
        bias = np.zeros(3)
        acc_mean = np.zeros(3)
        acc_norm = 0.0
        up_axis = 2
        up_sign = 1.0
        ok = False
    else:
        bias = np.mean(imu.gyro_rad_s[mask], axis=0)
        acc_mean = np.mean(imu.acc_m_s2[mask], axis=0)
        acc_norm = float(np.linalg.norm(acc_mean))
        up_axis = int(np.argmax(np.abs(acc_mean)))
        up_sign = float(np.sign(acc_mean[up_axis]) or 1.0)
        if abs(acc_norm - G) > 2.5:
            notes.append(
                f"static |acc|={acc_norm:.3f} differs from g={G}; check units (expect m/s^2)"
            )
        gyro_peak = float(np.max(np.linalg.norm(imu.gyro_rad_s, axis=1)))
        if gyro_peak > 20.0:
            notes.append(
                f"peak |gyro|={gyro_peak:.1f} rad/s looks extreme; check whether data is deg/s"
            )
        ok = abs(acc_norm - G) < 3.5 or static_ratio > 0.05

    notes.append(
        f"suggested up axis index={up_axis} sign={up_sign:+.0f} (0=x,1=y,2=z)"
    )
    return ImuAuditReport(
        ok=ok,
        gyro_bias_rad_s=np.asarray(bias, dtype=float),
        static_acc_mean_m_s2=np.asarray(acc_mean, dtype=float),
        static_acc_norm_m_s2=float(acc_norm),
        suggested_up_axis=up_axis,
        suggested_up_sign=up_sign,
        static_ratio=static_ratio,
        notes=tuple(notes),
    )