from __future__ import annotations

from pathlib import Path

import numpy as np

from imu_lidar.geodesy import geodetic_to_enu
from imu_lidar.geometry import so3_exp
from imu_lidar.imu_preintegration import preintegrate_gyro, preintegrate_imu
from imu_lidar.rtk_attitude import gnhpr_to_baseline_enu, gnhpr_to_rotation_enu_rtk
from imu_lidar.rtk_imu_translation import _preintegrate_translation_interval
from imu_lidar.rtk_io import load_rtk_csv


def test_geodetic_to_enu_has_expected_axis_and_scale() -> None:
    enu, origin = geodetic_to_enu(
        np.array([0.0, 0.0, 1e-5]),
        np.array([0.0, 1e-5, 0.0]),
        np.array([10.0, 10.0, 10.0]),
    )
    assert origin == (0.0, 0.0, 10.0)
    assert np.allclose(enu[0], 0.0, atol=1e-8)
    assert np.allclose(enu[1], [1.1131949, 0.0, 0.0], atol=2e-4)
    assert np.allclose(enu[2], [0.0, 1.1057428, 0.0], atol=2e-4)


def test_gnhpr_heading_maps_north_clockwise_into_enu() -> None:
    rotations = gnhpr_to_rotation_enu_rtk(
        np.array([0.0, 90.0]),
        np.zeros(2),
        np.zeros(2),
    )
    assert np.allclose(rotations[0][:, 0], [0.0, 1.0, 0.0], atol=1e-12)
    assert np.allclose(rotations[1][:, 0], [1.0, 0.0, 0.0], atol=1e-12)


def test_gnhpr_baseline_is_main_to_secondary_and_ignores_roll() -> None:
    baseline = gnhpr_to_baseline_enu(
        np.array([0.0, 90.0]),
        np.array([30.0, 0.0]),
    )
    assert np.allclose(baseline[0], [0.0, np.sqrt(0.75), 0.5], atol=1e-12)
    assert np.allclose(baseline[1], [1.0, 0.0, 0.0], atol=1e-12)

    rotations = gnhpr_to_rotation_enu_rtk(
        np.array([90.0, 90.0]),
        np.zeros(2),
        np.array([0.0, 45.0]),
    )
    assert np.allclose(rotations[0], rotations[1], atol=1e-12)


def test_rtk_loader_uses_hpr_measurement_time_for_attitude(tmp_path: Path) -> None:
    path = tmp_path / "rtk.csv"
    path.write_text(
        "t,t_measurement_utc_s,hpr_measurement_utc_s,lat_deg,lon_deg,altitude_m,"
        "fix_quality,heading_deg,pitch_deg,roll_deg,heading_quality,hdop\n"
        "10.0,1000.0,1000.05,30.0,114.0,20.0,4,12.0,1.0,0.0,4,0.6\n"
        "10.1,1000.1,1000.10,30.0,114.0,20.0,4,13.0,1.0,0.0,4,0.6\n",
        encoding="utf-8",
    )
    loaded = load_rtk_csv(path)
    assert np.allclose(loaded.t_s, [10.0, 10.1])
    assert np.allclose(loaded.attitude_t_s, [10.05, 10.1])
    assert np.all(loaded.attitude_valid)


def test_rtk_loader_accepts_only_checksum_valid_fixed_hpr(tmp_path: Path) -> None:
    path = tmp_path / "rtk.csv"
    path.write_text(
        "t,lat_deg,lon_deg,altitude_m,fix_quality,heading_deg,pitch_deg,roll_deg,"
        "heading_quality,checksum_valid\n"
        "0.0,30.0,114.0,20.0,4,10.0,0.0,0.0,4,1\n"
        "0.1,30.0,114.0,20.0,4,11.0,0.0,0.0,5,1\n"
        "0.2,30.0,114.0,20.0,4,12.0,0.0,0.0,4,0\n",
        encoding="utf-8",
    )
    loaded = load_rtk_csv(path)
    assert loaded.attitude_valid.tolist() == [True, False, False]
    assert loaded.attitude_float.tolist() == [False, True, False]


def test_fast_local_preintegration_matches_reference() -> None:
    times = np.linspace(0.0, 1.0, 101)
    gyro = np.tile(np.array([0.03, -0.02, 0.15]), (times.size, 1))
    acc = np.tile(np.array([0.4, -0.2, 9.7]), (times.size, 1))
    reference_rotation = preintegrate_gyro(times, gyro, 0.13, 0.87)
    assert np.allclose(reference_rotation.delta_R, so3_exp(gyro[0] * 0.74), atol=1e-10)

    reference = preintegrate_imu(times, gyro, acc, 0.13, 0.87)
    dp, dv, jp, jv, duration = _preintegrate_translation_interval(
        times, gyro, acc, 0.13, 0.87, np.zeros(3)
    )
    assert np.isclose(duration, 0.74)
    assert np.allclose(dp, reference.delta_p, atol=1e-10)
    assert np.allclose(dv, reference.delta_v, atol=1e-10)
    assert np.allclose(jp, reference.J_ba[6:9], atol=1e-10)
    assert np.allclose(jv, reference.J_ba[3:6], atol=1e-10)