"""VO-only diagnostic for EuRoC MH_01.

Bypasses the full pipeline (ESKF, satellite matching, recovery) and drives
ORBVisualOdometry directly on raw EuRoC frames. Reports the per-frame tracking
success rate so we can distinguish:

  - ORB can track EuRoC frames  → vo_success=0 in the full pipeline means the
    ESKF/satellite layer is the problem, not VO itself.
  - ORB cannot track EuRoC frames → VO backend is the root cause; need to swap
    to a real SuperPoint+LightGlue model or cuVSLAM.

Skipped if EuRoC MH_01 is not installed.
"""

from __future__ import annotations

from pathlib import Path

import cv2
import numpy as np
import pytest

from gps_denied.core.vo import ORBVisualOdometry
from gps_denied.schemas import CameraParameters
from gps_denied.testing.datasets.euroc import EuRoCAdapter

# Match CI-tier frame cap used in test_euroc.py
EUROC_MH01_MAX_FRAMES = 100

# EuRoC cam0 intrinsics from the dataset sensor.yaml (focal length ≈ 458.654 px)
# Expressed as physical params so CameraParameters can build K.
# sensor_width chosen so focal_length * (res_w / sensor_width) ≈ 458 px.
EUROC_CAM_PARAMS = CameraParameters(
    focal_length=4.586,      # mm (synthetic — tuned so f_px ≈ 458 px)
    sensor_width=4.586,      # mm → f_px = 4.586 * (752/4.586) = 752... no
    sensor_height=4.586,
    resolution_width=752,
    resolution_height=480,
    # principal_point omitted → uses image centre (376, 240)
)

# Actually compute correct params: f_px = focal_length * (res_w / sensor_w)
# We want f_px = 458.654. Set focal_length=458.654, sensor_width=752 so that
# f_px = 458.654 * (752/752) = 458.654.
EUROC_CAM_PARAMS = CameraParameters(
    focal_length=458.654,
    sensor_width=752.0,
    sensor_height=480.0,
    resolution_width=752,
    resolution_height=480,
)

# Minimum acceptable VO tracking rate to call the integration healthy.
MIN_TRACKING_RATE = 0.70  # 70 % of consecutive frame pairs


@pytest.mark.e2e
@pytest.mark.needs_dataset
@pytest.mark.asyncio
async def test_euroc_mh01_orb_tracking_rate(euroc_mh01_root: Path):
    """ORB VO should track ≥70 % of consecutive EuRoC frame pairs.

    If this xfails it means ORB itself cannot engage on EuRoC imagery — the
    root cause of vo_success=0 in the full pipeline is the VO backend, not the
    ESKF or satellite layer.
    """
    adapter = EuRoCAdapter(euroc_mh01_root)
    frames = list(adapter.iter_frames())[:EUROC_MH01_MAX_FRAMES]

    vo = ORBVisualOdometry()
    success_count = 0
    total_pairs = 0
    prev_image: np.ndarray | None = None

    for frame in frames:
        img = cv2.imread(frame.image_path, cv2.IMREAD_GRAYSCALE)
        if img is None:
            continue
        if prev_image is not None:
            pose = vo.compute_relative_pose(prev_image, img, EUROC_CAM_PARAMS)
            total_pairs += 1
            if pose is not None and pose.tracking_good:
                success_count += 1
        prev_image = img

    tracking_rate = success_count / max(1, total_pairs)

    # Always report numbers — visible in pytest -v output even on xfail.
    summary = (
        f"ORB tracking: {success_count}/{total_pairs} pairs "
        f"({tracking_rate*100:.1f}%)"
    )

    if tracking_rate < MIN_TRACKING_RATE:
        pytest.xfail(
            f"{summary} — below {MIN_TRACKING_RATE*100:.0f}% threshold. "
            "ORB struggles on EuRoC indoor imagery; upgrading to real "
            "SuperPoint+LightGlue or cuVSLAM is required to make the full "
            "pipeline viable on this dataset."
        )

    assert tracking_rate >= MIN_TRACKING_RATE, summary


@pytest.mark.e2e
@pytest.mark.needs_dataset
@pytest.mark.asyncio
async def test_euroc_mh01_orb_reports_nonzero_inliers(euroc_mh01_root: Path):
    """Sanity: ORB must find at least some inlier matches on at least one pair.

    If *every* pair has zero inliers the images are likely being loaded
    incorrectly (wrong path, wrong colour mode, etc.).
    """
    adapter = EuRoCAdapter(euroc_mh01_root)
    frames = list(adapter.iter_frames())[:20]  # small — just a sanity check

    vo = ORBVisualOdometry()
    any_inliers = False
    prev_image: np.ndarray | None = None

    for frame in frames:
        img = cv2.imread(frame.image_path, cv2.IMREAD_GRAYSCALE)
        if img is None:
            continue
        if prev_image is not None:
            pose = vo.compute_relative_pose(prev_image, img, EUROC_CAM_PARAMS)
            if pose is not None and pose.inlier_count > 0:
                any_inliers = True
                break
        prev_image = img

    assert any_inliers, (
        "ORB found zero inliers across the first 20 EuRoC frame pairs. "
        "Check that image paths are correct and images load as valid grayscale arrays."
    )