"""AZ-282 — `RansacFilter` AC suite (E-CC-HELPERS).

Covers the 10 ACs from `_docs/02_tasks/todo/AZ-282_ransac_filter.md` plus
the contract's "no upward imports" Layer 1 invariant via AST inspection.
"""

from __future__ import annotations

import ast
from pathlib import Path

import numpy as np
import pytest

from gps_denied_onboard.helpers import (
    RansacFilter,
    RansacFilterError,
    RansacResult,
)
from gps_denied_onboard.helpers.se3_utils import SE3, matrix_to_se3

# ---------------------------------------------------------------------------
# Fixtures


def _make_homography_correspondences(
    n: int, seed: int = 42, *, pure_translation: bool = False
) -> tuple[np.ndarray, np.ndarray]:
    """Return (correspondences, H) for ``n`` points warped through a fixed homography.

    ``pure_translation`` uses a translation-only H so cv2's fit lands at
    exactly the ground truth — used by the AC-1 atol=1e-6 zero-residual
    test. Other tests use the default mild projective transform.
    """
    rng = np.random.default_rng(seed)
    pts_a = rng.uniform(50.0, 950.0, size=(n, 2)).astype(np.float64)
    if pure_translation:
        H = np.array([[1.0, 0.0, 30.0], [0.0, 1.0, -20.0], [0.0, 0.0, 1.0]], dtype=np.float64)
    else:
        H = np.array(
            [
                [1.0, 0.05, 30.0],
                [-0.03, 1.0, -20.0],
                [0.0, 0.0, 1.0],
            ],
            dtype=np.float64,
        )
    pts_a_h = np.hstack([pts_a, np.ones((n, 1))])
    pts_b_h = (H @ pts_a_h.T).T
    pts_b = pts_b_h[:, :2] / pts_b_h[:, 2:3]
    correspondences = np.hstack([pts_a, pts_b])
    return correspondences, H


# ---------------------------------------------------------------------------
# AC-1: clean correspondences → 100 % inliers + ~0 residual.


def test_ac1_clean_correspondences_all_inliers() -> None:
    # Arrange — pure translation H so cv2's homography fit hits the
    # ground truth exactly and the AC-1 atol=1e-6 zero-residual gate holds.
    correspondences, _H = _make_homography_correspondences(n=100, pure_translation=True)

    # Act
    result = RansacFilter.filter_correspondences(
        correspondences, ransac_threshold_px=1.5, min_inliers=50
    )

    # Assert
    assert isinstance(result, RansacResult)
    assert result.inlier_count == 100
    assert result.outlier_count == 0
    assert result.median_residual_px == pytest.approx(0.0, abs=1e-6)


# ---------------------------------------------------------------------------
# AC-2: 80 inliers + 20 outliers → inlier count in [78, 82].


def test_ac2_mixed_correspondences_band() -> None:
    # Arrange
    clean, _H = _make_homography_correspondences(n=80, seed=7)
    # 20 outliers: random noise unrelated to H.
    rng = np.random.default_rng(7)
    outliers_a = rng.uniform(50.0, 950.0, size=(20, 2))
    outliers_b = rng.uniform(50.0, 950.0, size=(20, 2))
    outliers = np.hstack([outliers_a, outliers_b])
    correspondences = np.vstack([clean, outliers])

    # Act
    result = RansacFilter.filter_correspondences(
        correspondences, ransac_threshold_px=1.5, min_inliers=50
    )

    # Assert
    assert 78 <= result.inlier_count <= 82
    assert result.outlier_count == 100 - result.inlier_count


# ---------------------------------------------------------------------------
# AC-3: determinism — same input twice yields byte-equal RansacResult.


def test_ac3_determinism_byte_equal_outputs() -> None:
    # Arrange
    clean, _H = _make_homography_correspondences(n=80, seed=11)
    rng = np.random.default_rng(11)
    outliers = rng.uniform(50.0, 950.0, size=(20, 4))
    correspondences = np.vstack([clean, outliers])

    # Act
    r1 = RansacFilter.filter_correspondences(correspondences, 1.5, 50)
    r2 = RansacFilter.filter_correspondences(correspondences, 1.5, 50)

    # Assert
    assert r1.inlier_count == r2.inlier_count
    assert r1.outlier_count == r2.outlier_count
    np.testing.assert_array_equal(r1.inlier_correspondences, r2.inlier_correspondences)
    assert r1.median_residual_px == r2.median_residual_px


# ---------------------------------------------------------------------------
# AC-4: reprojection residual ~ 0 on clean inliers + known pose.


def test_ac4_reprojection_residual_zero_on_clean_pose() -> None:
    # Arrange
    # Identity pose. Pixel (x, y) back-projected to z=1 ray through K, then
    # re-projected through K with R=I, t=0 must land back on (x, y).
    K = np.array([[800.0, 0.0, 320.0], [0.0, 800.0, 240.0], [0.0, 0.0, 1.0]])
    distortion = np.zeros(5, dtype=np.float64)
    pose = matrix_to_se3(np.eye(4, dtype=np.float64))

    pts = np.array(
        [
            [100.0, 150.0, 100.0, 150.0],
            [200.0, 300.0, 200.0, 300.0],
            [400.0, 450.0, 400.0, 450.0],
            [500.0, 200.0, 500.0, 200.0],
        ]
    )

    # Act
    residual = RansacFilter.compute_reprojection_residual(pts, K, distortion, pose)

    # Assert
    assert residual == pytest.approx(0.0, abs=1e-6)


# ---------------------------------------------------------------------------
# AC-5: empty inlier array → NaN (no exception).


def test_ac5_empty_inliers_returns_nan() -> None:
    # Arrange
    empty = np.empty((0, 4), dtype=np.float64)
    K = np.eye(3, dtype=np.float64)
    distortion = np.zeros(5, dtype=np.float64)
    pose = matrix_to_se3(np.eye(4, dtype=np.float64))

    # Act
    residual = RansacFilter.compute_reprojection_residual(empty, K, distortion, pose)

    # Assert
    assert np.isnan(residual)


# ---------------------------------------------------------------------------
# AC-6: shape (N, 3) raises with shape message.


def test_ac6_invalid_correspondence_shape() -> None:
    # Arrange
    bad = np.zeros((10, 3), dtype=np.float64)

    # Act / Assert
    with pytest.raises(RansacFilterError, match=r"\(N, 4\)"):
        RansacFilter.filter_correspondences(bad, 1.5, 4)


# ---------------------------------------------------------------------------
# AC-7: non-positive threshold raises.


def test_ac7_non_positive_threshold() -> None:
    # Arrange
    correspondences, _H = _make_homography_correspondences(n=10)

    # Act / Assert
    with pytest.raises(RansacFilterError, match="positive"):
        RansacFilter.filter_correspondences(correspondences, -1.0, 4)


# ---------------------------------------------------------------------------
# AC-8: fewer than 4 correspondences raises.


def test_ac8_too_few_points() -> None:
    # Arrange
    too_few = np.zeros((3, 4), dtype=np.float64)

    # Act / Assert
    with pytest.raises(RansacFilterError, match="4"):
        RansacFilter.filter_correspondences(too_few, 1.5, 0)


# ---------------------------------------------------------------------------
# AC-9: K shape mismatch in residual call.


def test_ac9_K_shape_mismatch() -> None:
    # Arrange
    pts = np.zeros((4, 4), dtype=np.float64)
    bad_K = np.eye(4, dtype=np.float64)
    distortion = np.zeros(5, dtype=np.float64)
    pose = matrix_to_se3(np.eye(4, dtype=np.float64))

    # Act / Assert
    with pytest.raises(RansacFilterError, match=r"\(3, 3\)"):
        RansacFilter.compute_reprojection_residual(pts, bad_K, distortion, pose)


# ---------------------------------------------------------------------------
# AC-10: Layer 1 invariant — no `components.*` imports.


def test_ac10_no_upward_imports() -> None:
    # Arrange
    module_path = (
        Path(__file__).resolve().parents[2]
        / "src"
        / "gps_denied_onboard"
        / "helpers"
        / "ransac_filter.py"
    )
    source = module_path.read_text()
    tree = ast.parse(source)

    # Act
    forbidden: list[str] = []
    for node in ast.walk(tree):
        if isinstance(node, ast.Import):
            forbidden.extend(
                alias.name for alias in node.names if "gps_denied_onboard.components" in alias.name
            )
        elif isinstance(node, ast.ImportFrom):
            if node.module and "gps_denied_onboard.components" in node.module:
                forbidden.append(node.module)

    # Assert
    assert not forbidden, f"ransac_filter must not import components.*: {forbidden}"


# ---------------------------------------------------------------------------
# Additional guards: distortion shape contract.


@pytest.mark.parametrize("dist_shape", [(3,), (4,), (7,), (10,)])
def test_distortion_shape_contract(dist_shape: tuple[int, ...]) -> None:
    # Arrange
    pts = np.zeros((4, 4), dtype=np.float64)
    K = np.eye(3, dtype=np.float64)
    distortion = np.zeros(dist_shape, dtype=np.float64)
    pose = matrix_to_se3(np.eye(4, dtype=np.float64))

    # Act / Assert
    with pytest.raises(RansacFilterError, match=r"\(5,\) or \(8,\)"):
        RansacFilter.compute_reprojection_residual(pts, K, distortion, pose)


def test_returns_frozen_dataclass() -> None:
    import dataclasses

    # Arrange
    correspondences, _H = _make_homography_correspondences(n=20, seed=3)

    # Act
    result = RansacFilter.filter_correspondences(correspondences, 1.5, 4)

    # Assert
    with pytest.raises(dataclasses.FrozenInstanceError):
        result.inlier_count = 999  # type: ignore[misc]


def test_se3_alias_consistency() -> None:
    # Arrange / Act
    pose = matrix_to_se3(np.eye(4, dtype=np.float64))

    # Assert
    assert isinstance(pose, SE3)
