"""AZ-387 — smoothed past-keyframe → FDR side-channel.

Six ACs from ``_docs/02_tasks/done/AZ-387_c5_smoothed_history_fdr.md``:

- AC-1  iSAM2 emits smoothed past-keyframes to FDR.
- AC-2  Every emitted record carries ``smoothed=True``.
- AC-3  ESKF emits zero smoothed FDR records (deferred — AZ-386 not
        yet landed; documented here as a no-op cross-task invariant).
- AC-4  No leak to C8 outbound — enforced at the C8 boundary (AZ-261);
        this task documents the invariant.
- AC-5  Idempotency — emitting the same smoothed past-keyframe twice
        is prevented via the timestamp watermark.
- AC-6  FDR record shape — kind == ``c5.state.smoothed_history``;
        payload carries the documented fields.

Tests use the same iSAM2 fixtures as AZ-388 — a real
``GtsamIsam2StateEstimator`` with a seeded prior factor so
``current_estimate`` returns a non-empty estimate.
"""

from __future__ import annotations

from unittest import mock

import gtsam
import gtsam_unstable
import numpy as np
import pytest

from gps_denied_onboard.components.c5_state.config import C5StateConfig
from gps_denied_onboard.components.c5_state.gtsam_isam2_estimator import (
    GtsamIsam2StateEstimator,
    create,
)
from gps_denied_onboard.runtime_root.state_factory import clear_state_registry


@pytest.fixture(autouse=True)
def _registry_isolation():
    # Arrange
    clear_state_registry()
    yield
    clear_state_registry()


def _build_estimator(
    fdr_client: mock.MagicMock | None = None,
) -> tuple[GtsamIsam2StateEstimator, mock.MagicMock]:
    block = C5StateConfig(strategy="gtsam_isam2", keyframe_window_size=15)
    cfg = mock.MagicMock()
    cfg.components = {"c5_state": block}
    fdr = fdr_client if fdr_client is not None else mock.MagicMock()
    estimator, _ = create(
        config=cfg,
        imu_preintegrator=mock.MagicMock(),
        se3_utils=mock.MagicMock(),
        wgs_converter=mock.MagicMock(),
        fdr_client=fdr,
    )
    return estimator, fdr


def _seed_keys(estimator: GtsamIsam2StateEstimator, n: int = 3) -> list[int]:
    """Seed ``n`` POSE keys into the smoother + iSAM2 graph with
    strictly-increasing timestamps. Returns the list of inserted keys.
    """
    graph = gtsam.NonlinearFactorGraph()
    values = gtsam.Values()
    ts_map = gtsam_unstable.FixedLagSmootherKeyTimestampMap()
    noise = gtsam.noiseModel.Isotropic.Sigma(6, 0.1)
    keys: list[int] = []
    for i in range(n):
        key = gtsam.symbol("x", estimator._next_key_counter)
        estimator._next_key_counter += 1
        keys.append(key)
        pose = gtsam.Pose3()
        graph.add(gtsam.PriorFactorPose3(key, pose, noise))
        values.insert(key, pose)
        ts_map.insert((key, float(i) * 0.5))
    estimator._isam2_handle.update(graph, values, timestamps=ts_map)
    estimator._record_committed_pose_key(keys[-1])
    return keys


def _smoothed_history_calls(fdr: mock.MagicMock) -> list:
    return [
        call
        for call in fdr.enqueue.call_args_list
        if call.args and call.args[0].kind == "c5.state.smoothed_history"
    ]


# ---------------------------------------------------------------------
# AC-1: iSAM2 emits smoothed past-keyframes to FDR


def test_ac1_first_current_estimate_emits_smoothed_records() -> None:
    estimator, fdr = _build_estimator()
    keys = _seed_keys(estimator, n=3)

    estimator.current_estimate()

    calls = _smoothed_history_calls(fdr)
    assert len(calls) == len(keys)
    emitted_keys = {call.args[0].payload["pose_key"] for call in calls}
    assert emitted_keys == set(keys)


# ---------------------------------------------------------------------
# AC-2: smoothed=True on every record


def test_ac2_smoothed_flag_is_true() -> None:
    estimator, fdr = _build_estimator()
    _seed_keys(estimator, n=2)
    estimator.current_estimate()

    calls = _smoothed_history_calls(fdr)
    for call in calls:
        assert call.args[0].payload["smoothed"] is True


# ---------------------------------------------------------------------
# AC-5: idempotency — second call with no new keys emits nothing


def test_ac5_idempotent_no_new_keys_no_second_emission() -> None:
    estimator, fdr = _build_estimator()
    _seed_keys(estimator, n=3)
    estimator.current_estimate()
    initial_emit_count = len(_smoothed_history_calls(fdr))
    assert initial_emit_count == 3

    estimator.current_estimate()

    assert len(_smoothed_history_calls(fdr)) == initial_emit_count


def test_ac5_idempotent_new_keys_only_emitted_once() -> None:
    estimator, fdr = _build_estimator()
    _seed_keys(estimator, n=2)
    estimator.current_estimate()
    assert len(_smoothed_history_calls(fdr)) == 2

    # Add another key with a strictly newer timestamp.
    new_keys = _seed_keys(estimator, n=1)
    # The new key in the second `_seed_keys` call shares the
    # smoother window — the second key's timestamp is 0.0 (same as
    # the first seeded key from the first batch). The watermark
    # mechanism filters everything <= the watermark, so a key
    # with ts_s = 0.0 against a watermark of 1.0 (the third key of
    # the first batch) is filtered. Use a clearly newer timestamp.
    graph = gtsam.NonlinearFactorGraph()
    values = gtsam.Values()
    ts_map = gtsam_unstable.FixedLagSmootherKeyTimestampMap()
    noise = gtsam.noiseModel.Isotropic.Sigma(6, 0.1)
    fresh_key = gtsam.symbol("x", estimator._next_key_counter)
    estimator._next_key_counter += 1
    pose = gtsam.Pose3()
    graph.add(gtsam.PriorFactorPose3(fresh_key, pose, noise))
    values.insert(fresh_key, pose)
    ts_map.insert((fresh_key, 5.0))  # well past the watermark
    estimator._isam2_handle.update(graph, values, timestamps=ts_map)
    estimator._record_committed_pose_key(fresh_key)

    fdr.enqueue.reset_mock()
    estimator.current_estimate()

    calls = _smoothed_history_calls(fdr)
    emitted_keys = {call.args[0].payload["pose_key"] for call in calls}
    assert fresh_key in emitted_keys
    # Both ``new_keys[0]`` (ts=0.0) and the original first-batch
    # keys are below the watermark, so they should NOT re-emit.
    assert new_keys[0] not in emitted_keys


# ---------------------------------------------------------------------
# AC-6: FDR record shape


def test_ac6_fdr_record_has_required_shape() -> None:
    estimator, fdr = _build_estimator()
    _seed_keys(estimator, n=1)
    estimator.current_estimate()

    calls = _smoothed_history_calls(fdr)
    assert len(calls) == 1
    record = calls[0].args[0]
    assert record.kind == "c5.state.smoothed_history"
    assert record.producer_id == "c5_state"
    assert record.schema_version == 1
    payload = record.payload
    assert payload["smoothed"] is True
    # Documented payload fields per the impl docstring.
    expected_keys = {
        "pose_key",
        "smoother_ts_s",
        "emitted_at",
        "position_wgs84",
        "orientation_world_T_body",
        "covariance_6x6",
        "source_label",
        "last_satellite_anchor_age_ms",
        "smoothed",
    }
    assert set(payload.keys()) == expected_keys
    assert set(payload["position_wgs84"].keys()) == {"lat_deg", "lon_deg", "alt_m"}
    assert set(payload["orientation_world_T_body"].keys()) == {"w", "x", "y", "z"}
    # 6x6 covariance round-trips as a 6x6 list.
    cov = np.asarray(payload["covariance_6x6"], dtype=np.float64)
    assert cov.shape == (6, 6)


# ---------------------------------------------------------------------
# Robustness: no FDR client → no crash


def test_estimator_without_fdr_client_does_not_crash_on_hook() -> None:
    estimator, _ = _build_estimator()
    estimator._fdr_client = None  # simulate no-FDR composition
    _seed_keys(estimator, n=2)

    # Should not raise — hook is a no-op without FDR.
    estimator.current_estimate()


# ---------------------------------------------------------------------
# Robustness: marginals failure logs a warning but does NOT raise


def test_hook_marginals_failure_does_not_raise() -> None:
    estimator, fdr = _build_estimator()
    _seed_keys(estimator, n=1)

    # Patch the handle's compute_marginals to raise. The hook is
    # called AFTER ``current_estimate`` computed its own marginals
    # (which succeed against the seeded prior), so we patch only
    # subsequent calls.
    real_compute = estimator._isam2_handle.compute_marginals
    call_count = {"n": 0}

    def _flaky() -> object:
        call_count["n"] += 1
        if call_count["n"] == 1:
            return real_compute()
        raise RuntimeError("marginals diverged on smoother")

    with mock.patch.object(estimator._isam2_handle, "compute_marginals", side_effect=_flaky):
        # The forward-time marginals succeed; the hook fails — the
        # caller still sees the estimate, no exception propagates.
        estimator.current_estimate()

    # No smoothed FDR records emitted on the failure path.
    assert len(_smoothed_history_calls(fdr)) == 0


# ---------------------------------------------------------------------
# AC-3 / AC-4 invariant docs (ESKF + C8 leak)


def test_ac3_ac4_cross_task_invariants_documented() -> None:
    # AC-3 ESKF no-op — AZ-386 not yet landed; the AZ-386 estimator
    # will install a no-op smoothed-FDR hook (ESKF doesn't smooth).
    # AC-4 C8 outbound filter — owned by AZ-261; documented as a
    # cross-task invariant. This test serves as the marker in the
    # test suite that both invariants are owned and tracked.
    assert True
