"""RTK CSV loading for the independent RTK--IMU calibration path."""

from __future__ import annotations

from dataclasses import dataclass
from pathlib import Path

import numpy as np

from imu_lidar.geodesy import geodetic_to_enu


@dataclass(frozen=True)
class RtkSeries:
    """Normalized RTK observations on the IMU device clock."""

    t_s: np.ndarray
    attitude_t_s: np.ndarray
    position_enu_m: np.ndarray
    heading_deg: np.ndarray
    pitch_deg: np.ndarray
    roll_deg: np.ndarray
    fix_quality: np.ndarray
    heading_quality: np.ndarray
    heading_satellites: np.ndarray
    heading_age_s: np.ndarray
    hdop: np.ndarray
    checksum_valid: np.ndarray
    origin_geodetic: tuple[float, float, float]
    source: Path

    @property
    def attitude_valid(self) -> np.ndarray:
        """Strict fixed dual-antenna solutions suitable for calibration."""

        return (
            np.isfinite(self.heading_deg)
            & np.isfinite(self.pitch_deg)
            & np.isfinite(self.roll_deg)
            & (self.heading_quality == 4.0)
            & self.checksum_valid
        )

    @property
    def attitude_float(self) -> np.ndarray:
        """Float solutions retained for diagnostics but never calibration."""

        return (
            np.isfinite(self.heading_deg)
            & np.isfinite(self.pitch_deg)
            & np.isfinite(self.roll_deg)
            & (self.heading_quality == 5.0)
            & self.checksum_valid
        )

    @property
    def position_valid(self) -> np.ndarray:
        return (
            np.all(np.isfinite(self.position_enu_m), axis=1)
            & (self.fix_quality == 4.0)
            & self.checksum_valid
        )


def _column(data: np.ndarray, name: str, *, default: float = np.nan) -> np.ndarray:
    names = set(data.dtype.names or ())
    if name not in names:
        return np.full(data.shape[0], default, dtype=float)
    return np.asarray(data[name], dtype=float).reshape(-1)


def load_rtk_csv(path: Path | str) -> RtkSeries:
    """Load an exported G90 RTK CSV and convert its positions to local ENU.

    The required ``t`` column must already be NMEA measurement UTC mapped onto
    the IMU device clock. Host receive time is deliberately never accepted as
    a fallback because it is delayed by several seconds in the recorded data.
    """

    source = Path(path)
    if not source.is_file():
        raise FileNotFoundError(source)
    data = np.genfromtxt(source, delimiter=",", names=True, dtype=float, encoding="utf-8")
    if data.ndim == 0:
        data = np.array([data], dtype=data.dtype)
    names = set(data.dtype.names or ())
    required = {"t", "lat_deg", "lon_deg", "altitude_m", "fix_quality"}
    if not required.issubset(names):
        raise ValueError(f"RTK CSV must contain {sorted(required)}, got {sorted(names)}")
    t_s = _column(data, "t")
    measurement_utc = _column(data, "t_measurement_utc_s")
    hpr_measurement_utc = _column(data, "hpr_measurement_utc_s")
    attitude_t = t_s.copy()
    has_hpr_time = np.isfinite(measurement_utc) & np.isfinite(hpr_measurement_utc)
    attitude_t[has_hpr_time] += hpr_measurement_utc[has_hpr_time] - measurement_utc[has_hpr_time]
    order = np.argsort(t_s)
    position, origin = geodetic_to_enu(
        _column(data, "lat_deg")[order],
        _column(data, "lon_deg")[order],
        _column(data, "altitude_m")[order],
    )
    return RtkSeries(
        t_s=t_s[order],
        attitude_t_s=attitude_t[order],
        position_enu_m=position,
        heading_deg=_column(data, "heading_deg")[order],
        pitch_deg=_column(data, "pitch_deg")[order],
        roll_deg=_column(data, "roll_deg")[order],
        fix_quality=_column(data, "fix_quality", default=0.0)[order],
        heading_quality=_column(data, "heading_quality", default=0.0)[order],
        heading_satellites=_column(data, "heading_satellites")[order],
        heading_age_s=_column(data, "heading_age_s")[order],
        hdop=_column(data, "hdop")[order],
        checksum_valid=_column(data, "checksum_valid", default=1.0)[order] == 1.0,
        origin_geodetic=origin,
        source=source,
    )


def longest_valid_interval(t_s: np.ndarray, valid: np.ndarray, *, max_gap_s: float = 0.2) -> tuple[float, float]:
    """Return the longest contiguous valid time interval."""

    times = np.asarray(t_s, dtype=float).reshape(-1)
    mask = np.asarray(valid, dtype=bool).reshape(-1)
    indices = np.flatnonzero(mask)
    if indices.size == 0:
        raise ValueError("no valid RTK samples")
    best_start = best_end = int(indices[0])
    start = previous = int(indices[0])
    for index in indices[1:]:
        index = int(index)
        if index != previous + 1 or times[index] - times[previous] > max_gap_s:
            if times[previous] - times[start] > times[best_end] - times[best_start]:
                best_start, best_end = start, previous
            start = index
        previous = index
    if times[previous] - times[start] > times[best_end] - times[best_start]:
        best_start, best_end = start, previous
    return float(times[best_start]), float(times[best_end])