"""Estimate missing IMU frames from sensor uptime and CSV ODR metadata."""

from __future__ import annotations

import argparse
from dataclasses import dataclass
import math
from pathlib import Path
import re

from scripts.run_imu_ekf import read_imu_csv


RESTART_PREVIOUS_MIN_S = 1.0
RESTART_CURRENT_MAX_S = 0.01
ODR_HZ_PATTERN = re.compile(r"(?P<hz>\d+(?:\.\d+)?)\s*Hz\b", re.IGNORECASE)


@dataclass(frozen=True)
class FrameLossResult:
    input_csv: Path
    odr_hz: float
    expected_period_s: float
    received_frames: int
    missing_frames: int
    segment_count: int

    @property
    def expected_frames(self) -> int:
        return self.received_frames + self.missing_frames

    @property
    def loss_rate(self) -> float:
        return self.missing_frames / self.expected_frames


def analyze_file(path: Path) -> FrameLossResult:
    path = Path(path)
    metadata, rows = read_imu_csv(path)
    odr_hz = _parse_odr_hz(metadata.get("odr", ""), path)
    expected_period_s = 1.0 / odr_hz
    tolerance_s = max(1.0e-12, expected_period_s * 1.0e-6)

    received_frames = 0
    missing_frames = 0
    segment_count = 0
    previous_time: float | None = None

    for data_row_index, row in enumerate(rows, start=1):
        current_time = row.sensor_uptime_s
        received_frames += 1
        if previous_time is None:
            segment_count = 1
            previous_time = current_time
            continue

        if current_time < previous_time:
            if _is_device_restart(previous_time, current_time):
                segment_count += 1
                previous_time = current_time
                continue
            raise ValueError(
                f"{path} timestamp decreased at data row {data_row_index}: "
                f"previous {previous_time}, current {current_time}"
            )

        delta_s = current_time - previous_time
        period_count = round(delta_s / expected_period_s)
        if period_count < 1 or abs(delta_s - period_count * expected_period_s) > tolerance_s:
            raise ValueError(
                f"{path} timestamp gap at data row {data_row_index} is not aligned to ODR: "
                f"delta {delta_s}, expected period {expected_period_s}"
            )
        missing_frames += period_count - 1
        previous_time = current_time

    if received_frames == 0:
        raise ValueError(f"{path} has no IMU rows")

    return FrameLossResult(
        input_csv=path,
        odr_hz=odr_hz,
        expected_period_s=expected_period_s,
        received_frames=received_frames,
        missing_frames=missing_frames,
        segment_count=segment_count,
    )


def main(argv: list[str] | None = None) -> int:
    parser = argparse.ArgumentParser(description="Estimate missing IMU frames from sensor_uptime_s.")
    parser.add_argument("csv_files", nargs="+", type=Path)
    args = parser.parse_args(argv)

    for path in args.csv_files:
        result = analyze_file(path)
        print(
            f"{result.input_csv}: odr={result.odr_hz:g}Hz, "
            f"received={result.received_frames}, missing={result.missing_frames}, "
            f"expected={result.expected_frames}, loss_rate={result.loss_rate:.9%}, "
            f"segments={result.segment_count}"
        )
    return 0


def _parse_odr_hz(value: str, path: Path) -> float:
    match = ODR_HZ_PATTERN.search(value)
    if match is None:
        raise ValueError(f"{path} odr metadata must contain a frequency in Hz")
    odr_hz = float(match.group("hz"))
    if not math.isfinite(odr_hz) or odr_hz <= 0.0:
        raise ValueError(f"{path} odr frequency must be finite and positive")
    return odr_hz


def _is_device_restart(previous_time: float, current_time: float) -> bool:
    return previous_time >= RESTART_PREVIOUS_MIN_S and current_time <= RESTART_CURRENT_MAX_S


if __name__ == "__main__":
    raise SystemExit(main())