"""VPAIRAdapter unit tests with a fabricated layout matching the real VPAIR sample.

Real VPAIR poses_query.txt columns:
  filepath,x,y,z,undulation,roll,pitch,yaw,landcover
  x/y/z = ECEF metres; roll/pitch/yaw = radians; landcover = int tag (ignored).
"""

from pathlib import Path

import numpy as np
import pytest

from gps_denied.testing.datasets.base import (
    DatasetNotAvailableError,
    PlatformClass,
)
from gps_denied.testing.datasets.vpair import VPAIRAdapter

# ECEF for a point at roughly lat=50.737°, lon=7.095°, alt=350m (Bonn/Eifel region).
# Chosen to hit the real VPAIR geographic area so the adapter's conversion
# produces plausible numbers the tests can assert on.
_ECEF_ROW0 = (4023518.0, 510303.75, 4906569.65)
_ECEF_ROW1 = (4023484.45, 510291.89, 4906597.63)


@pytest.fixture
def fake_vpair_root(tmp_path: Path) -> Path:
    (tmp_path / "queries").mkdir()
    for fn in ("00001.png", "00002.png"):
        # minimal PNG header bytes; OpenCV won't actually need to read these in
        # unit tests (adapter only stores paths).
        (tmp_path / "queries" / fn).write_bytes(b"\x89PNG\r\n\x1a\n")
    (tmp_path / "poses_query.txt").write_text(
        "filepath,x,y,z,undulation,roll,pitch,yaw,landcover\n"
        f"queries/00001.png,{_ECEF_ROW0[0]},{_ECEF_ROW0[1]},{_ECEF_ROW0[2]},"
        "47.8,-0.0073764682747423,0.0095759807154536,-0.0762864127755165,1\n"
        f"queries/00002.png,{_ECEF_ROW1[0]},{_ECEF_ROW1[1]},{_ECEF_ROW1[2]},"
        "47.8,0.0266015380620956,-0.0029512757901102,-0.0540984831750392,3\n"
    )
    return tmp_path


def test_raises_when_missing(tmp_path: Path):
    with pytest.raises(DatasetNotAvailableError):
        VPAIRAdapter(tmp_path / "nope")


def test_raises_when_poses_file_missing(tmp_path: Path):
    (tmp_path / "queries").mkdir()
    # no poses_query.txt
    with pytest.raises(DatasetNotAvailableError):
        VPAIRAdapter(tmp_path)


def test_capabilities_no_raw_imu(fake_vpair_root: Path):
    adapter = VPAIRAdapter(fake_vpair_root)
    cap = adapter.capabilities
    assert cap.has_raw_imu is False
    assert cap.platform_class == PlatformClass.FIXED_WING


def test_iter_frames_indices_and_paths(fake_vpair_root: Path):
    adapter = VPAIRAdapter(fake_vpair_root)
    frames = list(adapter.iter_frames())
    assert len(frames) == 2
    assert frames[0].frame_idx == 0
    assert Path(frames[0].image_path).name == "00001.png"
    assert frames[1].frame_idx == 1


def test_iter_frames_synthesizes_timestamps_at_5hz(fake_vpair_root: Path):
    adapter = VPAIRAdapter(fake_vpair_root)
    frames = list(adapter.iter_frames())
    # 5 Hz → 200 ms = 200_000_000 ns
    assert frames[0].timestamp_ns == 0
    assert frames[1].timestamp_ns == 200_000_000


def test_iter_imu_empty(fake_vpair_root: Path):
    adapter = VPAIRAdapter(fake_vpair_root)
    assert list(adapter.iter_imu()) == []


def test_iter_ground_truth_converts_ecef_to_wgs84(fake_vpair_root: Path):
    adapter = VPAIRAdapter(fake_vpair_root)
    gt = list(adapter.iter_ground_truth())
    assert len(gt) == 2
    # Bonn/Eifel area — rough bounds
    assert 50.0 < gt[0].lat < 51.5
    assert 6.5 < gt[0].lon < 8.0
    # Altitude above ellipsoid, hundreds of metres
    assert 100.0 < gt[0].alt < 700.0


def test_iter_ground_truth_converts_euler_to_unit_quaternion(fake_vpair_root: Path):
    adapter = VPAIRAdapter(fake_vpair_root)
    gt = list(adapter.iter_ground_truth())
    for pose in gt:
        norm = np.sqrt(pose.qx**2 + pose.qy**2 + pose.qz**2 + pose.qw**2)
        assert norm == pytest.approx(1.0, abs=1e-10)
    # First row roll/pitch/yaw are small angles ≈0 → quaternion close to identity
    # qw should be close to 1 (but not exactly; roll/pitch/yaw != 0)
    assert gt[0].qw > 0.99