"""Regression tests for G90 GNHPR parsing and host-time LiDAR association."""

from __future__ import annotations

import json
import sys
from pathlib import Path

import numpy as np

ROOT = Path(__file__).resolve().parents[1]
TOOLS = ROOT / "tools"
CODE = ROOT / "code"
sys.path.insert(0, str(TOOLS))
sys.path.insert(0, str(TOOLS / "rscap_v2"))
sys.path.insert(0, str(CODE))

from build_multisensor_npz import build_combined  # noqa: E402
from pipeline_common import parse_gnhpr  # noqa: E402
from rigorous_calibration import load_npz_xyz  # noqa: E402


def test_parse_gnhpr_fixed_heading():
    row = parse_gnhpr("$GNHPR,070411.40,354.7437,000.2518,000.0000,4,26,0.00,0999*58")
    assert row["type"] == "GNHPR"
    assert row["raw_heading_deg"] == 354.7437
    assert row["pitch_deg"] == 0.2518
    assert row["roll_deg"] == 0.0
    assert row["heading_quality"] == 4
    assert row["satellites"] == 26
    assert row["heading_valid"] is True


def test_host_time_uses_lidar_receive_time_and_preserves_device_time(tmp_path: Path):
    host_ns = 1_786_240_000_000_000_000
    device_ns = 1_500_000_000_000_000_000
    frame_dir = tmp_path / "lidar"
    frame_dir.mkdir()
    np.savez_compressed(
        frame_dir / "frame.npz",
        points=np.zeros((4, 4), dtype=np.float32),
        unix_time_ns=np.asarray([device_ns], dtype=np.int64),
        host_receive_utc_ns=np.asarray([host_ns], dtype=np.int64),
    )

    rtk = tmp_path / "rtk.jsonl"
    rows = [
        {
            "type": "GGA",
            "checksum_valid": True,
            "host_receive_utc_ns": host_ns + 20_000_000,
            "lat_deg": 31.0,
            "lon_deg": 121.0,
            "altitude_m": 10.0,
            "fix_quality": 4,
            "satellites": 20,
            "raw_line": "$GNGGA,...",
        },
        {
            "type": "GNHPR",
            "checksum_valid": True,
            "host_receive_utc_ns": host_ns - 10_000_000,
            "raw_heading_deg": 90.0,
            "pitch_deg": 1.0,
            "roll_deg": 0.0,
            "heading_quality": 4,
            "heading_solution": "GNHPR_QUALITY_4",
            "heading_valid": True,
            "satellites": 22,
            "raw_line": "$GNHPR,...",
        },
    ]
    rtk.write_text("".join(json.dumps(row) + "\n" for row in rows), encoding="utf-8")
    imu = tmp_path / "imu.jsonl"
    imu.write_text("", encoding="utf-8")

    out = tmp_path / "combined"
    summary = build_combined(
        [("STATION-01", frame_dir)],
        [rtk],
        [imu],
        out,
        time_basis="host",
        rtk_max_dt_ms=100.0,
    )

    assert summary["frames"] == 1
    assert summary["rtk_valid"] == 1
    assert summary["heading_valid"] == 1
    assert summary["rtk_fixed"] == 1
    with np.load(next((out / "frames").glob("*.npz")), allow_pickle=False) as frame:
        assert int(frame["lidar_association_time_ns"][0]) == host_ns
        assert int(frame["unix_time_ns"][0]) == device_ns
        assert int(frame["rtk_gga_dt_ns"][0]) == 20_000_000
        assert int(frame["rtk_heading_dt_ns"][0]) == -10_000_000


def test_registration_prefers_lidar_association_time(tmp_path: Path):
    host_ns = 1_786_240_000_000_000_000
    device_ns = 1_500_000_000_000_000_000
    source = tmp_path / "frame.npz"
    np.savez_compressed(
        source,
        points_raw=np.asarray(
            [[1000.0, 0.0, 0.0, 1.0], [2000.0, 90.0, 0.0, 1.0]],
            dtype=np.float32,
        ),
        unix_time_ns=np.asarray([device_ns], dtype=np.int64),
        lidar_association_time_ns=np.asarray([host_ns], dtype=np.int64),
        frame_counter=np.asarray([7], dtype=np.int64),
    )

    timestamp, counter, xyz = load_npz_xyz(source)
    assert timestamp == host_ns / 1e9
    assert counter == 7
    assert xyz.shape == (2, 3)