"""AZ-392 — CovarianceProjector unit tests.

Covers all 7 ACs:
1. 6x6 -> 3x3 -> 2x2 projection correctness
2. AP returns meters (float)
3. iNav returns mm (uint16-clamped)
4. SPD violation raises FcEmitError + emits FDR ERROR
5. NaN guard
6. Bit-stable across calls (deterministic intermediate arithmetic)
7. iNav clamp at 65535 emits WARN log
"""

from __future__ import annotations

import logging
import math
from unittest import mock
from uuid import UUID

import numpy as np
import pytest

from gps_denied_onboard._types.geo import LatLonAlt
from gps_denied_onboard._types.state import (
    EstimatorOutput,
    PoseSourceLabel,
    Quat,
)
from gps_denied_onboard.components.c8_fc_adapter._covariance_projector import (
    CovarianceProjector,
)
from gps_denied_onboard.components.c8_fc_adapter.errors import FcEmitError
from gps_denied_onboard.fdr_client.records import FdrRecord

_TEST_FRAME_ID = UUID("00000000-0000-0000-0000-000000000007")


def _output(cov: np.ndarray | None, frame_id: UUID = _TEST_FRAME_ID) -> EstimatorOutput:
    return EstimatorOutput(
        frame_id=frame_id,
        position_wgs84=LatLonAlt(lat_deg=50.0, lon_deg=30.0, alt_m=100.0),
        orientation_world_T_body=Quat(w=1.0, x=0.0, y=0.0, z=0.0),
        velocity_world_mps=(0.0, 0.0, 0.0),
        covariance_6x6=cov if cov is not None else np.eye(6),  # placeholder; tests override
        source_label=PoseSourceLabel.VISUAL_PROPAGATED,
        last_satellite_anchor_age_ms=0,
        smoothed=False,
        emitted_at=0,
    )


def _output_with_cov(cov: np.ndarray | None) -> EstimatorOutput:
    """Variant that allows None covariance for missing-cov tests."""
    return EstimatorOutput(
        frame_id=_TEST_FRAME_ID,
        position_wgs84=LatLonAlt(lat_deg=50.0, lon_deg=30.0, alt_m=100.0),
        orientation_world_T_body=Quat(w=1.0, x=0.0, y=0.0, z=0.0),
        velocity_world_mps=(0.0, 0.0, 0.0),
        covariance_6x6=cov,  # type: ignore[arg-type]
        source_label=PoseSourceLabel.VISUAL_PROPAGATED,
        last_satellite_anchor_age_ms=0,
        smoothed=False,
        emitted_at=0,
    )


def _spd_6x6(sigma_xx: float = 4.0, sigma_yy: float = 9.0, sigma_xy: float = 0.0) -> np.ndarray:
    """Construct a 6x6 covariance whose 2x2 horizontal block has known eigenvalues."""
    cov = np.eye(6)
    cov[0, 0] = sigma_xx
    cov[1, 1] = sigma_yy
    cov[0, 1] = sigma_xy
    cov[1, 0] = sigma_xy
    return cov


def _fake_fdr_client() -> mock.MagicMock:
    client = mock.MagicMock()
    client.enqueue.return_value = "OK"
    return client


# ----------------------------------------------------------------------
# AC-1: 6x6 -> 3x3 -> 2x2 reduction correctness


def test_ac1_diagonal_covariance_returns_largest_sigma() -> None:
    # Arrange — sigma_yy > sigma_xx, so lambda_max == 9.0 and radius == 3.0
    proj = CovarianceProjector(fdr_client=_fake_fdr_client())
    out = _output(_spd_6x6(sigma_xx=4.0, sigma_yy=9.0))

    # Act
    radius_m = proj.to_ardupilot_horiz_accuracy_m(out)

    # Assert
    assert math.isclose(radius_m, 3.0, abs_tol=1e-9)


def test_ac1_with_off_diagonal_uses_eigenvalue() -> None:
    # Arrange — [[4, 1], [1, 4]] has eigenvalues 5 and 3; lambda_max = 5
    proj = CovarianceProjector(fdr_client=_fake_fdr_client())
    out = _output(_spd_6x6(sigma_xx=4.0, sigma_yy=4.0, sigma_xy=1.0))

    # Act
    radius_m = proj.to_ardupilot_horiz_accuracy_m(out)

    # Assert
    assert math.isclose(radius_m, math.sqrt(5.0), rel_tol=1e-12)


# ----------------------------------------------------------------------
# AC-2: AP returns meters (float)


def test_ac2_ap_returns_float_meters() -> None:
    # Arrange
    proj = CovarianceProjector(fdr_client=_fake_fdr_client())
    out = _output(_spd_6x6(sigma_xx=1.0, sigma_yy=1.0))

    # Act
    result = proj.to_ardupilot_horiz_accuracy_m(out)

    # Assert
    assert isinstance(result, float)
    assert math.isclose(result, 1.0, abs_tol=1e-12)


# ----------------------------------------------------------------------
# AC-3: iNav returns mm (int)


def test_ac3_inav_returns_int_millimeters() -> None:
    # Arrange — sigma == 1 m^2 -> radius = 1 m = 1000 mm
    proj = CovarianceProjector(fdr_client=_fake_fdr_client())
    out = _output(_spd_6x6(sigma_xx=1.0, sigma_yy=1.0))

    # Act
    result = proj.to_inav_h_pos_accuracy_mm(out)

    # Assert
    assert isinstance(result, int)
    assert result == 1000


def test_ac3_inav_rounds_half_up() -> None:
    # Arrange — radius = sqrt(0.5) ~ 0.7071067... m -> 707.1067 mm -> round-half-up -> 707
    proj = CovarianceProjector(fdr_client=_fake_fdr_client())
    out = _output(_spd_6x6(sigma_xx=0.5, sigma_yy=0.5))

    # Act
    result = proj.to_inav_h_pos_accuracy_mm(out)

    # Assert
    assert result == 707


# ----------------------------------------------------------------------
# AC-4 + AC-5: SPD / NaN violations


def test_ac4_non_spd_raises_fc_emit_error_and_logs_fdr() -> None:
    # Arrange — negative determinant 2x2 block
    fdr = _fake_fdr_client()
    proj = CovarianceProjector(fdr_client=fdr)
    cov = _spd_6x6()
    cov[0, 0] = -1.0  # break positive-definiteness
    out = _output(cov)

    # Act + Assert
    with pytest.raises(FcEmitError, match=r"non-SPD"):
        proj.to_ardupilot_horiz_accuracy_m(out)

    # Assert — exactly one FDR ERROR record with the violation kv
    assert fdr.enqueue.call_count == 1
    record: FdrRecord = fdr.enqueue.call_args.args[0]
    assert record.kind == "log"
    assert record.payload["kv"]["reason"] == "non_spd"
    assert record.payload["kv"]["frame_id"] == str(_TEST_FRAME_ID)


def test_ac4_asymmetric_2x2_rejected() -> None:
    # Arrange
    proj = CovarianceProjector(fdr_client=_fake_fdr_client())
    cov = np.eye(6)
    cov[0, 1] = 1.0
    cov[1, 0] = 2.0  # ≠ cov[0, 1] → asymmetric
    out = _output(cov)

    # Act + Assert
    with pytest.raises(FcEmitError, match=r"non-SPD"):
        proj.to_ardupilot_horiz_accuracy_m(out)


def test_ac5_nan_covariance_rejected() -> None:
    # Arrange
    fdr = _fake_fdr_client()
    proj = CovarianceProjector(fdr_client=fdr)
    cov = _spd_6x6()
    cov[2, 2] = math.nan
    out = _output(cov)

    # Act + Assert
    with pytest.raises(FcEmitError, match=r"NaN"):
        proj.to_ardupilot_horiz_accuracy_m(out)
    assert fdr.enqueue.call_args.args[0].payload["kv"]["reason"] == "nan_or_inf"


def test_ac5_inf_covariance_rejected() -> None:
    # Arrange
    proj = CovarianceProjector(fdr_client=_fake_fdr_client())
    cov = _spd_6x6()
    cov[2, 2] = math.inf
    out = _output(cov)

    # Act + Assert
    with pytest.raises(FcEmitError):
        proj.to_ardupilot_horiz_accuracy_m(out)


def test_ac5_missing_covariance_rejected() -> None:
    # Arrange
    fdr = _fake_fdr_client()
    proj = CovarianceProjector(fdr_client=fdr)
    out = _output_with_cov(cov=None)

    # Act + Assert
    with pytest.raises(FcEmitError, match=r"missing"):
        proj.to_ardupilot_horiz_accuracy_m(out)
    assert fdr.enqueue.call_args.args[0].payload["kv"]["reason"] == "missing"


def test_ac5_wrong_shape_rejected() -> None:
    # Arrange
    fdr = _fake_fdr_client()
    proj = CovarianceProjector(fdr_client=fdr)
    out = _output_with_cov(cov=np.eye(5))

    # Act + Assert
    with pytest.raises(FcEmitError, match=r"6x6"):
        proj.to_ardupilot_horiz_accuracy_m(out)
    assert fdr.enqueue.call_args.args[0].payload["kv"]["reason"] == "bad_shape"


# ----------------------------------------------------------------------
# AC-6: bit-stable across calls


def test_ac6_bit_stable_repeated_calls() -> None:
    # Arrange — repeated calls on identical input yield bit-identical output
    proj = CovarianceProjector(fdr_client=_fake_fdr_client())
    out = _output(_spd_6x6(sigma_xx=3.7, sigma_yy=2.1, sigma_xy=0.4))

    # Act
    results = [proj.to_ardupilot_horiz_accuracy_m(out) for _ in range(20)]

    # Assert — every call produces exactly the same float
    assert len(set(results)) == 1


def test_ac6_ap_and_inav_round_trip_consistent() -> None:
    # Arrange — AP m * 1000 (round-half-up) must equal iNav mm
    proj = CovarianceProjector(fdr_client=_fake_fdr_client())
    out = _output(_spd_6x6(sigma_xx=2.0, sigma_yy=2.0))

    # Act
    radius_m = proj.to_ardupilot_horiz_accuracy_m(out)
    radius_mm = proj.to_inav_h_pos_accuracy_mm(out)

    # Assert
    assert radius_mm == math.floor(radius_m * 1000.0 + 0.5)


# ----------------------------------------------------------------------
# AC-7: iNav clamp at 65535


def test_ac7_inav_clamps_at_uint16_max(caplog: pytest.LogCaptureFixture) -> None:
    # Arrange — sigma huge enough that radius_mm > 65535
    proj = CovarianceProjector(fdr_client=_fake_fdr_client())
    huge_sigma = 1e8  # radius ~ sqrt(1e8) m = 10000 m = 10_000_000 mm
    out = _output(_spd_6x6(sigma_xx=huge_sigma, sigma_yy=huge_sigma))

    # Act
    with caplog.at_level(logging.WARNING, logger="c8_fc_adapter.cov_projector"):
        result = proj.to_inav_h_pos_accuracy_mm(out)

    # Assert
    assert result == 65535
    clamp_records = [
        r for r in caplog.records if getattr(r, "kind", None) == "c8.cov_projector.inav_clamped"
    ]
    assert len(clamp_records) == 1
    assert clamp_records[0].kv["clamped_to"] == 65535
    assert clamp_records[0].kv["frame_id"] == str(_TEST_FRAME_ID)


def test_ac7_inav_exact_at_uint16_max_not_clamped() -> None:
    # Arrange — pick sigma_xx so that radius_mm == 65535 exactly
    # radius_m = 65.535, sigma = 65.535^2 = 4294.838...
    sigma = (65.535) ** 2
    proj = CovarianceProjector(fdr_client=_fake_fdr_client())
    out = _output(_spd_6x6(sigma_xx=sigma, sigma_yy=sigma))

    # Act
    result = proj.to_inav_h_pos_accuracy_mm(out)

    # Assert — at the boundary, no clamp
    assert result == 65535


# ----------------------------------------------------------------------
# NFR: perf (loose budget — projector must be fast enough for 20 Hz emit)


def test_nfr_perf_projector_under_100us_per_call() -> None:
    # Arrange
    import time

    proj = CovarianceProjector(fdr_client=_fake_fdr_client())
    out = _output(_spd_6x6(sigma_xx=2.0, sigma_yy=3.0, sigma_xy=0.5))

    iters = 1000

    # Act
    start = time.perf_counter()
    for _ in range(iters):
        proj.to_ardupilot_horiz_accuracy_m(out)
    avg_s = (time.perf_counter() - start) / iters

    # Assert
    assert avg_s < 100e-6, f"avg {avg_s * 1e6:.1f}us > 100us budget"
