"""C5 ``GtsamIsam2StateEstimator`` — graph + factor adds (AZ-382 + AZ-383).

AZ-382 wired the GTSAM substrate (iSAM2 + ``IncrementalFixedLagSmoother``
+ ``NonlinearFactorGraph`` + ``Values``) and the four ``ISam2GraphHandleImpl``
real bodies. AZ-383 owns the three Protocol factor-add methods:

* ``add_vio(vio: VioOutput)`` — ``BetweenFactorPose3`` between
  consecutive pose keys with a noise model derived from
  ``vio.pose_covariance_6x6``.
* ``add_pose_anchor(pose: PoseEstimate)`` — mode-dispatched per
  ``pose.covariance_mode``: ``"marginals"`` → ``PriorFactorPose3`` +
  ``update``; ``"jacobian"`` → skip iSAM2 add (per the AZ-361 cross-task
  contract) but still bump ``_last_anchor_ns``.
* ``add_fc_imu(imu_window: ImuWindow)`` — feeds the shared
  ``ImuPreintegrator``, builds a ``CombinedImuFactor`` between the
  prev/curr (X, V, B) keyframe triple, and inserts initial values for
  the new keys.

Invariant 2 — every ``add_*`` call enforces non-decreasing timestamps;
out-of-order arrivals raise :class:`EstimatorDegradedError` and emit an
ERROR log. R05 — every successful factor add emits a DEBUG SUCCESS
log; every failure emits an ERROR FAILURE log AND raises through the
C5 error hierarchy.

The remaining three Protocol methods (``current_estimate`` /
``smoothed_history`` / ``health_snapshot``) still raise
``NotImplementedError`` naming AZ-384 — marginals + outputs land
there.
"""

from __future__ import annotations

import math
from collections import deque
from datetime import datetime, timezone
from typing import TYPE_CHECKING, Any, Final, Literal
from uuid import UUID, uuid4

import gtsam
import gtsam_unstable
import numpy as np
from numpy.linalg import LinAlgError

from gps_denied_onboard._types.geo import LatLonAlt
from gps_denied_onboard.clock.wall_clock import WallClock
from gps_denied_onboard._types.state import (
    EstimatorHealth,
    EstimatorOutput,
    IsamState,
    PoseSourceLabel,
    Quat,
)
from gps_denied_onboard.components.c5_state._fallback_watcher import (
    FallbackEngagementCallback,
    FallbackRecoveryCallback,
    FallbackSubscription,
    FallbackWatcher,
)
from gps_denied_onboard.components.c5_state._isam2_handle import (
    ISam2GraphHandle,
    ISam2GraphHandleImpl,
)
from gps_denied_onboard.components.c5_state._source_label_sm import (
    RejectionCallback,
    RejectionSubscription,
    SourceLabelStateMachine,
)
from gps_denied_onboard.components.c5_state.config import C5StateConfig
from gps_denied_onboard.components.c5_state.errors import (
    EstimatorAlreadyStartedError,
    EstimatorDegradedError,
    EstimatorFatalError,
    StateEstimatorConfigError,
)
from gps_denied_onboard.components.c5_state.interface import StateEstimator
from gps_denied_onboard.fdr_client.records import FdrRecord
from gps_denied_onboard.helpers.wgs_converter import WgsConverter
from gps_denied_onboard.logging import get_logger

if TYPE_CHECKING:
    from gps_denied_onboard._types.fc import GpsHealth, GpsSample
    from gps_denied_onboard._types.nav import ImuWindow, VioOutput
    from gps_denied_onboard._types.pose import PoseEstimate
    from gps_denied_onboard.clock import Clock
    from gps_denied_onboard.config import Config

__all__ = [
    "GtsamIsam2StateEstimator",
    "create",
    "register",
]


# D-C5-3 keyframe processing rate (3 Hz → 1/3 s per keyframe). The
# ``IncrementalFixedLagSmoother`` window is expressed in seconds, so we
# multiply ``K`` keyframes by this constant to derive the temporal
# window passed to GTSAM. Kept as a module-level constant rather than a
# config knob — D-C5-3 fixes the keyframe rate; the window size ``K``
# is the only operator-configurable lever.
_FRAME_PERIOD_S: Final[float] = 1.0 / 3.0

# Strategy slug used by the C5 composition factory and the
# ``register_state_estimator`` registry.
_STRATEGY: Final[str] = "gtsam_isam2"

# Default isotropic noise covariance used when an input DTO arrives
# without an explicit ``covariance_6x6``. 0.1 m / 0.1 rad sigmas match
# the documented BMI088-class defaults and let the iSAM2 solver
# converge without exploding.
_DEFAULT_POSE_SIGMA: Final[float] = 0.1

# AC-NEW-8 rolling-window length (60 s) for ``cov_norm_growing_for_s``.
# We keep ``(monotonic_ns, cov_norm)`` tuples for every successful
# ``current_estimate()`` and lazy-prune anything older than this on
# read.
_COV_NORM_WINDOW_NS: Final[int] = 60 * 1_000_000_000

# Default ENU origin when none was injected — equator, prime meridian,
# sea level. Behaviour is deliberately permissive so the estimator can
# run on synthetic fixtures without a wired-up satellite anchor; the
# composition root SHOULD call :meth:`set_enu_origin` before steady
# state. AZ-385 will tie origin selection to the spoof-promotion gate.
_DEFAULT_ENU_ORIGIN: Final[LatLonAlt] = LatLonAlt(lat_deg=0.0, lon_deg=0.0, alt_m=0.0)

# AZ-490 / ADR-010 default rotation sigma for the operator-origin
# prior factor. Translation sigmas are caller-supplied; the rotation
# component has no operator input, so we use a conservative 5° prior
# (a still-stationary drone with no compass calibration is comfortably
# inside a 5° envelope at the WGS84 frame).
_DEFAULT_TAKEOFF_ORIENTATION_SIGMA_RAD: Final[float] = math.radians(5.0)


class GtsamIsam2StateEstimator(StateEstimator):
    """Production-default C5 estimator — iSAM2 + ``IncrementalFixedLagSmoother``.

    Holds the canonical GTSAM substrate per ADR-003. C4's
    ``OpenCVGtsamPoseEstimator`` calls into the same graph indirectly
    via the injected :class:`ISam2GraphHandle`; both components MUST
    run on the same ingest thread (composition root enforces this via
    :func:`bind_state_ingest_thread`).
    """

    def __init__(
        self,
        config: Config,
        *,
        imu_preintegrator: Any,
        se3_utils: Any,
        wgs_converter: Any,
        fdr_client: Any,
        clock: Clock | None = None,
    ) -> None:
        block = self._extract_block(config)

        self._config: Config = config
        self._block: C5StateConfig = block
        self._imu_preintegrator: Any = imu_preintegrator
        self._se3_utils: Any = se3_utils
        self._wgs_converter: Any = wgs_converter
        self._fdr_client: Any = fdr_client
        self._clock: Clock = clock if clock is not None else WallClock()

        self._isam2 = gtsam.ISAM2(gtsam.ISAM2Params())
        window_seconds: float = block.keyframe_window_size * _FRAME_PERIOD_S
        self._smoother = gtsam_unstable.IncrementalFixedLagSmoother(window_seconds)
        self._graph = gtsam.NonlinearFactorGraph()
        self._values = gtsam.Values()

        # ``_key_for_frame`` maps the input DTO's ``frame_id`` (int on
        # ``VioOutput`` + ``PoseEstimate``, UUID on the C5 output) to a
        # GTSAM ``Key``. The dict accepts any hashable.
        self._key_for_frame: dict[Any, int] = {}
        self._next_key_counter: int = 0
        # Initialised to 0 ⇒ ``last_anchor_age_ms`` returns
        # ``monotonic_ns() / 1e6`` (a very large number) until AZ-383
        # records the first satellite-anchored pose. The contract
        # documents this as the "no anchor yet" sentinel.
        self._last_anchor_ns: int = 0

        # AZ-383 state -----------------------------------------------------
        self._log = get_logger("c5_state.gtsam_isam2")
        # The handle is constructed by ``create(...)`` and assigned
        # post-init so the estimator can call back into it during
        # ``add_*``. AZ-383 (not AZ-382) introduced this back-reference;
        # before then the handle was held only by the composition root.
        self._isam2_handle: ISam2GraphHandle | None = None

        # Invariant 2 — strictly non-decreasing per-stream timestamps.
        # Stored as int ns on the wall-clock (datetime → ns for
        # VioOutput/PoseEstimate; ts_end_ns for ImuWindow). The merge
        # rule across the three streams is the composition root's job;
        # C5 only enforces the per-call monotonicity guard.
        self._last_added_ts_ns: int = 0

        # ``add_vio`` between-factor wiring.
        self._prev_vio: VioOutput | None = None

        # ``add_fc_imu`` uses an INDEPENDENT (X, V, B) keyframe chain
        # decoupled from the VIO/pose frame_id mapping — see the
        # implementation note on add_fc_imu below.
        self._imu_keyframe_counter: int = 0
        self._prev_imu_x_key: int | None = None
        self._prev_imu_v_key: int | None = None
        self._prev_imu_b_key: int | None = None

        # AZ-384 state -----------------------------------------------------
        # Last pose key with an actually committed value in iSAM2 (used by
        # ``current_estimate``). Set ONLY after a successful
        # ``handle.update`` that inserts a value for that key.
        self._last_committed_pose_key: int | None = None
        # ENU origin for ``current_estimate``'s pose conversion. None
        # means use ``_DEFAULT_ENU_ORIGIN``; AZ-385 wires the real
        # origin via :meth:`set_enu_origin` from the first satellite
        # anchor.
        self._enu_origin: LatLonAlt | None = None
        # Source-label state machine (AZ-385). Constructed eagerly
        # so ``current_estimate`` + ``health_snapshot`` always have a
        # real machine to query. The composition root feeds GPS
        # health updates via :meth:`notify_gps_health` (sourced from
        # C8 inbound, AZ-391) and subscribes the C8 GCS adapter to
        # rejection events via :meth:`subscribe_spoof_rejection`.
        # :meth:`attach_source_label_state_machine` remains available
        # as an override for tests that need to inject a stub.
        self._source_label_machine: SourceLabelStateMachine = SourceLabelStateMachine(
            spoof_promotion_min_stable_s=block.spoof_promotion_min_stable_s,
            spoof_promotion_visual_consistency_tol_m=block.spoof_promotion_visual_consistency_tol_m,
            spoof_promotion_bounded_delta_m=block.spoof_promotion_bounded_delta_m,
            fdr_client=fdr_client,
            clock_ns=self._clock.monotonic_ns,
            producer_id="c5_state",
        )
        # AC-NEW-8 rolling window of ``(ts_monotonic_ns, cov_norm)``
        # tuples for ``cov_norm_growing_for_s`` accounting.
        self._cov_norm_window: deque[tuple[int, float]] = deque()
        # iSAM2 lifecycle state. ``INIT`` until the first successful
        # ``current_estimate``; ``TRACKING`` after; transitions to
        # ``DEGRADED`` on an inflated covariance trend (AC-NEW-8) and
        # ``LOST`` on a fatal SPD failure or GTSAM exception.
        self._isam2_state: IsamState = IsamState.INIT

        # AZ-387 state -----------------------------------------------------
        # Watermark for the smoothed-history → FDR hook. We only emit a
        # smoothed past-keyframe to FDR if its smoother timestamp is
        # strictly greater than this watermark; the watermark is bumped
        # to the largest emitted timestamp after every call. Idempotent
        # by construction (AC-5 — same key never emitted twice). The
        # watermark is in seconds (float, smoother native units).
        self._smoothed_fdr_watermark_s: float = float("-inf")

        # AZ-388 state -----------------------------------------------------
        # AC-5.2 fallback watcher — engages when ``current_estimate``
        # cannot produce a fresh output for ``no_estimate_fallback_s``
        # (default 3.0 s). Composition root subscribes the C8 GCS
        # adapter via :meth:`subscribe_fallback_engaged` /
        # :meth:`subscribe_fallback_recovered` so the STATUSTEXT
        # mirror fires on the C8 outbound thread, not the C5 ingest
        # thread.
        self._fallback = FallbackWatcher(
            threshold_s=block.no_estimate_fallback_s,
            fdr_client=fdr_client,
            clock_ns=self._clock.monotonic_ns,
            producer_id="c5_state",
        )

        # AZ-490 cold-start ladder ----------------------------------------
        # ``_takeoff_origin_set`` records the operator-origin args so
        # AC-4 idempotency + AC-5 conflict checks can compare without
        # re-deriving from the seeded ``PriorFactorPose3``. None means
        # ``set_takeoff_origin`` has not been called yet.
        self._takeoff_origin_set: tuple[LatLonAlt, float, float] | None = None
        # ``_origin_source`` tracks which cold-start anchor won.
        # ``"manifest"`` is set by ``set_takeoff_origin`` (Principle
        # #11 primary); ``"fc_ekf"`` is set by the first ``add_*``
        # call when no manifest origin was supplied (legacy fallback).
        # AC-13 wants exactly one ``c5.cold_start_origin.set`` FDR
        # record per estimator; this field gates the emission.
        self._origin_source: Literal["manifest", "fc_ekf"] | None = None
        # ``_cold_start_window_closed`` flips True on the first
        # ``add_*`` call (vio / pose_anchor / fc_imu). After it
        # closes, ``set_takeoff_origin`` must raise
        # ``EstimatorAlreadyStartedError`` (AC-6).
        self._cold_start_window_closed: bool = False

        self._log.debug(
            "c5.state.isam2_initialised",
            extra={
                "kind": "c5.state.isam2_initialised",
                "kv": {
                    "keyframe_window_size": block.keyframe_window_size,
                    "window_seconds": window_seconds,
                    "total_factors_initial": 0,
                },
            },
        )

    @staticmethod
    def _extract_block(config: Config) -> C5StateConfig:
        components = getattr(config, "components", None) or {}
        block = components.get("c5_state") if isinstance(components, dict) else None
        if block is None:
            return C5StateConfig()
        if isinstance(block, C5StateConfig):
            return block
        raise StateEstimatorConfigError(
            f"config.components['c5_state'] must be a C5StateConfig; got {type(block).__name__}"
        )

    def attach_handle(self, handle: ISam2GraphHandle) -> None:
        """Wire the iSAM2 graph handle.

        Called once by :func:`create` after the handle is constructed
        against this estimator instance. The handle is required by
        every ``add_*`` method, so any ``add_*`` call against an
        estimator with no handle raises ``StateEstimatorConfigError``.
        """
        self._isam2_handle = handle

    def set_enu_origin(self, origin: LatLonAlt) -> None:
        """Set the WGS84 origin used by :meth:`current_estimate` for ENU→WGS84.

        The composition root SHOULD call this once at startup before
        steady-state ``current_estimate`` calls. AZ-385 (source-label
        gate) will eventually drive this from the first satellite-
        anchored frame. Before this is called the estimator falls
        back to :data:`_DEFAULT_ENU_ORIGIN` (equator / prime meridian
        / sea level) which is correct for tests but obviously wrong
        for flight.
        """
        self._enu_origin = origin

    def attach_source_label_state_machine(self, machine: Any) -> None:
        """Override the AZ-385 source-label / spoof-promotion state machine.

        Reserved for tests that need to inject a stub or alternative
        implementation. In production the estimator constructs a
        :class:`SourceLabelStateMachine` in ``__init__`` already; this
        override REPLACES that instance. The composition root should
        NOT call this in steady-state — it should drive the
        pre-built machine via :meth:`notify_gps_health` and
        :meth:`subscribe_spoof_rejection`.

        The injected object MUST expose ``current_label() -> PoseSourceLabel``
        and ``is_spoof_promotion_blocked() -> bool``.
        """
        self._source_label_machine = machine

    def notify_gps_health(self, gps_health: GpsHealth, now_ns: int | None = None) -> None:
        """Forward an FC ``GpsHealth`` observation to the AZ-385 state machine.

        Composition root wires C8 inbound's GPS-health subscription to
        this method. The machine may transition the spoof-promotion
        gate as a side-effect.
        """
        machine = self._source_label_machine
        if isinstance(machine, SourceLabelStateMachine):
            machine.notify_gps_health(gps_health, now_ns=now_ns)

    def subscribe_spoof_rejection(self, callback: RejectionCallback) -> RejectionSubscription:
        """Subscribe to AZ-385 spoof-rejection events.

        Composition root attaches C8's ``QgcTelemetryAdapter`` here so
        every reject mirrors as a MAVLink STATUSTEXT (severity
        WARNING, capped at 50 chars per AC-12). See
        :class:`SourceLabelStateMachine` for the exact callback
        signature.
        """
        machine = self._source_label_machine
        if not isinstance(machine, SourceLabelStateMachine):
            raise StateEstimatorConfigError(
                "subscribe_spoof_rejection requires the built-in "
                "SourceLabelStateMachine; replace via attach_source_label_state_machine "
                "with an instance that supports subscribe_rejection if needed."
            )
        return machine.subscribe_rejection(callback)

    # ------------------------------------------------------------------
    # AZ-388: AC-5.2 fallback public API.

    def check_fallback_state(self, now_ns: int) -> bool:
        """Idempotent AC-5.2 watchdog.

        C8 outbound's 5 Hz tick handler calls this so the FC IMU-only
        switch fires even when ``current_estimate`` isn't being
        called (e.g. the C5 ingest thread is starved). Returns the
        current ``_in_fallback`` state; engages if the threshold has
        elapsed and was previously idle. Rate-limited — at most one
        engagement signal per episode.
        """
        return self._fallback.check_and_engage(now_ns)

    def subscribe_fallback_engaged(
        self, callback: FallbackEngagementCallback
    ) -> FallbackSubscription:
        """Register a callback fired exactly once per fallback engagement.

        Composition root binds the C8 GCS adapter's STATUSTEXT here
        with the ``CRITICAL`` severity per AC-5; the callback fires
        on the same thread that won the engagement race (so the
        composition root MUST forward to the C8 outbound thread via
        a queue to honour Invariant 8).
        """
        return self._fallback.subscribe_engaged(callback)

    def subscribe_fallback_recovered(
        self, callback: FallbackRecoveryCallback
    ) -> FallbackSubscription:
        """Register a callback fired exactly once per fallback recovery."""
        return self._fallback.subscribe_recovered(callback)

    def key_for_frame(self, frame_id: UUID | int | str) -> int:
        """Return the GTSAM ``Key`` for ``frame_id``, assigning on first use.

        AZ-383 calls this from ``add_vio`` and ``add_pose_anchor`` to
        translate the input DTO's frame identifier into the integer
        key GTSAM uses. Per the C5 contract §"Key management policy"
        we reserve the ``'x'`` (pose) namespace; ``add_fc_imu``
        additionally uses ``'v'`` (velocity) and ``'b'`` (bias) via
        its own internal counter.
        """
        existing = self._key_for_frame.get(frame_id)
        if existing is not None:
            return existing
        new_key: int = gtsam.symbol("x", self._next_key_counter)
        self._key_for_frame[frame_id] = new_key
        self._next_key_counter += 1
        return new_key

    # ------------------------------------------------------------------
    # AZ-490: operator-origin cold-start entrypoint.

    def set_takeoff_origin(
        self,
        origin: LatLonAlt,
        sigma_horiz_m: float,
        sigma_vert_m: float,
    ) -> None:
        """Seed the cold-start prior with an operator-supplied origin (AZ-490).

        See :meth:`StateEstimator.set_takeoff_origin` for the full
        contract. This impl sets the local ENU origin to ``origin``,
        attaches one ``PriorFactorPose3`` at ``Pose3.Identity()`` to
        the next pose key, drives a single ``handle.update``, and
        emits one ``c5.cold_start_origin.set`` FDR record.
        """
        self._validate_takeoff_origin_args(origin, sigma_horiz_m, sigma_vert_m)

        if self._cold_start_window_closed:
            raise EstimatorAlreadyStartedError(
                "set_takeoff_origin called after the cold-start window closed; "
                "first add_* call sealed the operator-origin entrypoint"
            )

        if self._takeoff_origin_set is not None:
            prev_origin, prev_sh, prev_sv = self._takeoff_origin_set
            if prev_origin == origin and prev_sh == sigma_horiz_m and prev_sv == sigma_vert_m:
                return  # AC-4 — idempotent no-op
            raise StateEstimatorConfigError(
                "set_takeoff_origin re-called with conflicting args; "
                f"previous=(origin={prev_origin!r}, sigma_horiz_m={prev_sh}, "
                f"sigma_vert_m={prev_sv}); "
                f"new=(origin={origin!r}, sigma_horiz_m={sigma_horiz_m}, "
                f"sigma_vert_m={sigma_vert_m})"
            )

        handle = self._require_handle()
        self._enu_origin = origin

        prior_pose = gtsam.Pose3()  # Identity at ENU origin
        prior_key = gtsam.symbol("x", self._next_key_counter)
        self._next_key_counter += 1

        sigmas = np.array(
            [
                _DEFAULT_TAKEOFF_ORIENTATION_SIGMA_RAD,
                _DEFAULT_TAKEOFF_ORIENTATION_SIGMA_RAD,
                _DEFAULT_TAKEOFF_ORIENTATION_SIGMA_RAD,
                sigma_horiz_m,
                sigma_horiz_m,
                sigma_vert_m,
            ],
            dtype=np.float64,
        )
        noise = gtsam.noiseModel.Diagonal.Sigmas(sigmas)
        factor = gtsam.PriorFactorPose3(prior_key, prior_pose, noise)
        # AC-6 / Invariant 11a: do NOT advance ``_last_added_ts_ns`` —
        # this is a pre-takeoff seed, not a measurement; the first
        # subsequent ``add_*`` call still sees the unguarded baseline.
        ts_ns = self._clock.monotonic_ns()
        try:
            handle.add_factor(factor)
            self._values.insert(prior_key, prior_pose)
            timestamps = _make_timestamp_map([prior_key], ts_ns)
            handle.update(self._graph, self._values, timestamps)
        except (EstimatorDegradedError, EstimatorFatalError):
            raise
        except Exception as exc:
            self._log.error(
                "c5.state.set_takeoff_origin_failed",
                extra={
                    "kind": "c5.state.set_takeoff_origin_failed",
                    "kv": {"error": str(exc)},
                },
            )
            raise EstimatorDegradedError(f"set_takeoff_origin failed: {exc}") from exc

        self._reset_staging()
        self._record_committed_pose_key(prior_key)
        self._takeoff_origin_set = (origin, sigma_horiz_m, sigma_vert_m)
        self._origin_source = "manifest"
        self._emit_cold_start_origin_set_fdr(
            source="manifest",
            origin=origin,
            sigma_horiz_m=sigma_horiz_m,
            sigma_vert_m=sigma_vert_m,
        )
        self._log.info(
            "c5.cold_start_origin.set",
            extra={
                "kind": "c5.cold_start_origin.set",
                "kv": {
                    "source": "manifest",
                    "lat_deg": origin.lat_deg,
                    "lon_deg": origin.lon_deg,
                    "alt_m": origin.alt_m,
                    "sigma_horiz_m": sigma_horiz_m,
                    "sigma_vert_m": sigma_vert_m,
                },
            },
        )

    @staticmethod
    def _validate_takeoff_origin_args(
        origin: LatLonAlt,
        sigma_horiz_m: float,
        sigma_vert_m: float,
    ) -> None:
        if not (
            math.isfinite(origin.lat_deg)
            and math.isfinite(origin.lon_deg)
            and math.isfinite(origin.alt_m)
        ):
            raise StateEstimatorConfigError(
                f"set_takeoff_origin: non-finite component in origin {origin!r}"
            )
        if not (-90.0 <= origin.lat_deg <= 90.0):
            raise StateEstimatorConfigError(
                f"set_takeoff_origin: latitude {origin.lat_deg} outside WGS-84 [-90, 90]"
            )
        if not (-180.0 <= origin.lon_deg <= 180.0):
            raise StateEstimatorConfigError(
                f"set_takeoff_origin: longitude {origin.lon_deg} outside WGS-84 [-180, 180]"
            )
        if not (math.isfinite(sigma_horiz_m) and sigma_horiz_m > 0.0):
            raise StateEstimatorConfigError(
                f"set_takeoff_origin: sigma_horiz_m must be positive finite; got {sigma_horiz_m}"
            )
        if not (math.isfinite(sigma_vert_m) and sigma_vert_m > 0.0):
            raise StateEstimatorConfigError(
                f"set_takeoff_origin: sigma_vert_m must be positive finite; got {sigma_vert_m}"
            )

    def _close_cold_start_window(self) -> None:
        """Mark the cold-start window closed on the first ``add_*`` call.

        Idempotent — only the first invocation flips the flag and
        emits the legacy-fallback FDR record (AC-13). Subsequent
        calls are no-ops.
        """
        if self._cold_start_window_closed:
            return
        self._cold_start_window_closed = True
        if self._origin_source is None:
            self._origin_source = "fc_ekf"
            origin = self._enu_origin if self._enu_origin is not None else _DEFAULT_ENU_ORIGIN
            self._emit_cold_start_origin_set_fdr(
                source="fc_ekf",
                origin=origin,
                sigma_horiz_m=self._block.default_takeoff_origin_sigma_horiz_m,
                sigma_vert_m=self._block.default_takeoff_origin_sigma_vert_m,
            )

    def _emit_cold_start_origin_set_fdr(
        self,
        *,
        source: Literal["manifest", "fc_ekf"],
        origin: LatLonAlt,
        sigma_horiz_m: float,
        sigma_vert_m: float,
    ) -> None:
        if self._fdr_client is None:
            return
        record = FdrRecord(
            schema_version=1,
            ts=datetime.now(tz=timezone.utc).isoformat(),
            producer_id="c5_state",
            kind="c5.cold_start_origin.set",
            payload={
                "source": source,
                "lat_deg": origin.lat_deg,
                "lon_deg": origin.lon_deg,
                "alt_m": origin.alt_m,
                "sigma_horiz_m": sigma_horiz_m,
                "sigma_vert_m": sigma_vert_m,
            },
        )
        try:
            self._fdr_client.enqueue(record)
        except Exception as exc:
            self._log.warning(
                "c5.cold_start_origin.set_fdr_enqueue_failed",
                extra={
                    "kind": "c5.cold_start_origin.set_fdr_enqueue_failed",
                    "kv": {"source": source, "error": repr(exc)},
                },
            )

    def notify_gps_sample(self, sample: GpsSample, now_ns: int | None = None) -> str | None:
        """Bounded-delta gate dispatch for an inbound FC GPS sample (AZ-490).

        Looks up the smoother's current latlon (best-effort — returns
        ``None`` if no committed pose is available yet) and delegates
        to :meth:`SourceLabelStateMachine.process_gps_sample`. The
        return value mirrors that method's tri-state result
        (``"BOUNDED_DELTA_SOFT"`` / ``"REJECT"`` / ``None``); the
        composition root uses it to decide whether to enqueue the
        sample as a soft factor.
        """
        machine = self._source_label_machine
        if not isinstance(machine, SourceLabelStateMachine):
            return None
        smoother_latlon: LatLonAlt | None
        if self._last_committed_pose_key is None:
            smoother_latlon = None
        else:
            try:
                pose = self._pose_at_key(self._last_committed_pose_key)
                smoother_latlon = self._enu_pose_to_wgs84(pose)
            except EstimatorFatalError:
                smoother_latlon = None
        return machine.process_gps_sample(
            sample.position_wgs84,
            smoother_estimate=smoother_latlon,
            now_ns=now_ns,
        )

    # ------------------------------------------------------------------
    # AZ-383: factor-add bodies.

    def add_vio(self, vio: VioOutput) -> None:
        """Append a VIO ``BetweenFactorPose3`` to the running graph.

        First call after construction records the initial pose without
        emitting a factor (there's no previous frame to relate to);
        every subsequent call builds a between-factor between the
        previous and the current VIO frame's pose keys and triggers a
        single ``handle.update()`` per AC-7.
        """
        handle = self._require_handle()
        self._close_cold_start_window()
        ts_ns = vio.emitted_at_ns
        self._guard_timestamp(ts_ns, source="vio")

        curr_pose = vio.relative_pose_T
        curr_key = self.key_for_frame(vio.frame_id)

        if self._prev_vio is None:
            # First VIO frame — record its absolute pose as an initial
            # value but DO NOT emit a between-factor. AZ-388 (AC-5.2)
            # owns the first-keyframe seeding once the full chain lands.
            self._prev_vio = vio
            self._last_added_ts_ns = ts_ns
            self._log.debug(
                "c5.state.add_vio_first_frame",
                extra={
                    "kind": "c5.state.add_vio_first_frame",
                    "kv": {"frame_id": str(vio.frame_id), "ts_ns": ts_ns},
                },
            )
            return

        prev_pose = self._prev_vio.relative_pose_T
        prev_key = self.key_for_frame(self._prev_vio.frame_id)
        relative_pose = prev_pose.between(curr_pose)
        noise = _build_pose_noise(vio.pose_covariance_6x6)
        factor = gtsam.BetweenFactorPose3(prev_key, curr_key, relative_pose, noise)

        try:
            handle.add_factor(factor)
            self._values.insert(curr_key, curr_pose)
            timestamps = _make_timestamp_map([curr_key], ts_ns)
            handle.update(self._graph, self._values, timestamps)
        except EstimatorDegradedError:
            raise
        except Exception as exc:
            self._log.error(
                "c5.state.add_vio_failed",
                extra={
                    "kind": "c5.state.add_vio_failed",
                    "kv": {"frame_id": str(vio.frame_id), "error": str(exc)},
                },
            )
            raise EstimatorDegradedError(f"add_vio failed: {exc}") from exc

        self._reset_staging()
        self._prev_vio = vio
        self._last_added_ts_ns = ts_ns
        self._record_committed_pose_key(curr_key)
        self._log.debug(
            "c5.state.add_vio_ok",
            extra={
                "kind": "c5.state.add_vio_ok",
                "kv": {
                    "frame_id": str(vio.frame_id),
                    "prev_key": prev_key,
                    "curr_key": curr_key,
                },
            },
        )

    def add_pose_anchor(self, pose: PoseEstimate) -> None:
        """Add a C4 pose anchor; mode-dispatched per ``pose.covariance_mode``.

        ``"marginals"`` path builds a ``PriorFactorPose3`` on the pose
        key and triggers ``handle.update()``. ``"jacobian"`` path
        SKIPS the iSAM2 add per the AZ-361 contract — the running
        estimate is fed but the graph stops growing under throttle.
        Both paths bump ``_last_anchor_ns`` so the spoof-promotion
        gate (AZ-385) and ``last_anchor_age_ms`` see a recent anchor.
        """
        handle = self._require_handle()
        self._close_cold_start_window()
        ts_ns = int(pose.emitted_at)
        self._guard_timestamp(ts_ns, source="pose_anchor")

        pose_key = self.key_for_frame(pose.frame_id)
        mode = pose.covariance_mode.value

        # Both paths update the anchor freshness sentinel. The C5
        # contract documents this — even the throttled JACOBIAN path
        # counts as a recent anchor for AC-1.3 binning.
        self._last_anchor_ns = self._clock.monotonic_ns()

        if mode == "marginals":
            gtsam_pose = _pose_se3_to_gtsam(self._pose_estimate_to_matrix(pose))
            noise = _build_pose_noise(pose.covariance_6x6)
            factor = gtsam.PriorFactorPose3(pose_key, gtsam_pose, noise)
            try:
                handle.add_factor(factor)
                self._values.insert(pose_key, gtsam_pose)
                timestamps = _make_timestamp_map([pose_key], ts_ns)
                handle.update(self._graph, self._values, timestamps)
            except EstimatorDegradedError:
                raise
            except Exception as exc:
                self._log.error(
                    "c5.state.add_pose_anchor_failed",
                    extra={
                        "kind": "c5.state.add_pose_anchor_failed",
                        "kv": {"frame_id": str(pose.frame_id), "error": str(exc)},
                    },
                )
                raise EstimatorDegradedError(f"add_pose_anchor failed: {exc}") from exc
            self._reset_staging()
            self._record_committed_pose_key(pose_key)
            self._notify_source_label_anchor(pose)
            self._log.debug(
                "c5.state.add_pose_anchor_ok",
                extra={
                    "kind": "c5.state.add_pose_anchor_ok",
                    "kv": {
                        "frame_id": str(pose.frame_id),
                        "pose_key": pose_key,
                        "covariance_mode": mode,
                    },
                },
            )
        else:
            # JACOBIAN path — AZ-361 contract: skip the iSAM2 add to
            # keep the graph from growing under throttle.
            self._log.info(
                "c5.state.skip_isam2_jacobian_path",
                extra={
                    "kind": "c5.state.skip_isam2_jacobian_path",
                    "kv": {
                        "frame_id": str(pose.frame_id),
                        "pose_key": pose_key,
                        "covariance_mode": mode,
                    },
                },
            )

        self._last_added_ts_ns = ts_ns

    def add_fc_imu(self, imu_window: ImuWindow) -> None:
        """Build a ``CombinedImuFactor`` from a window of FC IMU samples.

        The window's samples are fed into the shared
        :class:`ImuPreintegrator`; ``reset_for_new_keyframe()``
        closes the PIM and yields the
        ``PreintegratedCombinedMeasurements`` used by the factor.

        The (X, V, B) keyframe chain advances per call:

        - First call primes the chain (no factor — no previous
          keyframe to relate to).
        - Subsequent calls add a factor between the previous and the
          current (X, V, B) triple and insert initial values for the
          new keys (identity Pose3, zero Velocity, zero Bias).

        This chain is intentionally independent of the VIO/pose
        frame_id mapping. The composition root will arrange the
        higher-level scheduling that fuses the chains; AZ-383 only
        owns the per-window factor add.
        """
        handle = self._require_handle()
        self._close_cold_start_window()
        self._guard_timestamp(imu_window.ts_end_ns, source="imu_window")

        try:
            self._imu_preintegrator.integrate_window(imu_window)
            closed_pim = self._imu_preintegrator.reset_for_new_keyframe()
        except Exception as exc:
            self._log.error(
                "c5.state.add_fc_imu_preintegrate_failed",
                extra={
                    "kind": "c5.state.add_fc_imu_preintegrate_failed",
                    "kv": {"ts_end_ns": imu_window.ts_end_ns, "error": str(exc)},
                },
            )
            raise EstimatorDegradedError(f"add_fc_imu preintegrate failed: {exc}") from exc

        curr_x_key = gtsam.symbol("x", self._next_key_counter)
        curr_v_key = gtsam.symbol("v", self._imu_keyframe_counter)
        curr_b_key = gtsam.symbol("b", self._imu_keyframe_counter)
        self._next_key_counter += 1
        self._imu_keyframe_counter += 1

        if self._prev_imu_x_key is None:
            # First IMU window — prime the chain. AZ-388 owns the
            # first-keyframe seeding (prior factor + initial values)
            # once the AC-5.2 fallback wiring lands.
            self._prev_imu_x_key = curr_x_key
            self._prev_imu_v_key = curr_v_key
            self._prev_imu_b_key = curr_b_key
            self._last_added_ts_ns = imu_window.ts_end_ns
            self._log.debug(
                "c5.state.add_fc_imu_first_window",
                extra={
                    "kind": "c5.state.add_fc_imu_first_window",
                    "kv": {
                        "ts_end_ns": imu_window.ts_end_ns,
                        "x_key": curr_x_key,
                        "v_key": curr_v_key,
                        "b_key": curr_b_key,
                    },
                },
            )
            return

        factor = gtsam.CombinedImuFactor(
            self._prev_imu_x_key,
            self._prev_imu_v_key,
            curr_x_key,
            curr_v_key,
            self._prev_imu_b_key,
            curr_b_key,
            closed_pim,
        )

        try:
            handle.add_factor(factor)
            # Initial values for the new triple — identity pose,
            # zero velocity, zero bias. The smoother will refine.
            self._values.insert(curr_x_key, gtsam.Pose3())
            self._values.insert(curr_v_key, np.zeros(3, dtype=np.float64))
            self._values.insert(curr_b_key, gtsam.imuBias.ConstantBias())
            timestamps = _make_timestamp_map(
                [curr_x_key, curr_v_key, curr_b_key], imu_window.ts_end_ns
            )
            handle.update(self._graph, self._values, timestamps)
        except EstimatorDegradedError:
            raise
        except Exception as exc:
            self._log.error(
                "c5.state.add_fc_imu_failed",
                extra={
                    "kind": "c5.state.add_fc_imu_failed",
                    "kv": {"ts_end_ns": imu_window.ts_end_ns, "error": str(exc)},
                },
            )
            raise EstimatorDegradedError(f"add_fc_imu failed: {exc}") from exc

        self._reset_staging()
        self._prev_imu_x_key = curr_x_key
        self._prev_imu_v_key = curr_v_key
        self._prev_imu_b_key = curr_b_key
        self._last_added_ts_ns = imu_window.ts_end_ns
        self._record_committed_pose_key(curr_x_key)
        self._log.debug(
            "c5.state.add_fc_imu_ok",
            extra={
                "kind": "c5.state.add_fc_imu_ok",
                "kv": {
                    "ts_end_ns": imu_window.ts_end_ns,
                    "x_key": curr_x_key,
                    "v_key": curr_v_key,
                    "b_key": curr_b_key,
                },
            },
        )

    # ------------------------------------------------------------------
    # AZ-384: marginals + output methods.

    def current_estimate(self) -> EstimatorOutput:
        """Recover the current posterior pose + 6x6 covariance.

        Walks ``_isam2.calculateEstimate()`` for the most-recently
        committed pose key, recovers the 6x6 Marginals covariance
        from the iSAM2 graph (D-C5-5 = (c)), enforces the
        SPD-positive-definite invariant (Invariant 10) before
        emission, and assembles a fresh :class:`EstimatorOutput`
        (Invariant 4 — never returns ``None`` on the steady-state
        path).
        """
        handle = self._require_handle()
        # AZ-388: AC-5.2 entry hook. Engages fallback if the
        # threshold has elapsed since the last successful estimate.
        # Idempotent / rate-limited.
        self._fallback.check_and_engage(self._clock.monotonic_ns())
        if self._last_committed_pose_key is None:
            raise EstimatorFatalError(
                "current_estimate: no committed pose key yet (graph empty); "
                "AC-5.2 fallback in C8 will trigger after no_estimate_fallback_s"
            )

        try:
            marginals = handle.compute_marginals()
            covariance = np.asarray(
                marginals.marginalCovariance(self._last_committed_pose_key),
                dtype=np.float64,
            )
        except EstimatorFatalError:
            self._isam2_state = IsamState.LOST
            raise
        except Exception as exc:
            self._isam2_state = IsamState.LOST
            self._log.error(
                "c5.state.current_estimate_marginals_failed",
                extra={
                    "kind": "c5.state.current_estimate_marginals_failed",
                    "kv": {
                        "pose_key": self._last_committed_pose_key,
                        "error": str(exc),
                    },
                },
            )
            raise EstimatorFatalError(f"compute_marginals failed: {exc}") from exc

        try:
            _enforce_spd(covariance)
        except EstimatorFatalError:
            self._isam2_state = IsamState.LOST
            self._log.error(
                "c5.state.current_estimate_spd_failed",
                extra={
                    "kind": "c5.state.current_estimate_spd_failed",
                    "kv": {
                        "pose_key": self._last_committed_pose_key,
                        "covariance_fro_norm": float(np.linalg.norm(covariance, ord="fro")),
                    },
                },
            )
            raise

        pose = self._pose_at_key(self._last_committed_pose_key)
        position_wgs84 = self._enu_pose_to_wgs84(pose)
        orientation = _quat_from_pose3(pose)
        velocity_world = self._latest_velocity_or_zero()
        last_anchor_age_ms = int(handle.last_anchor_age_ms())
        source_label = self._derive_source_label()
        emitted_at = self._clock.monotonic_ns()

        self._record_cov_norm_sample(emitted_at, covariance)
        if self._isam2_state == IsamState.INIT:
            self._isam2_state = IsamState.TRACKING
        if self._cov_norm_growing_for_s() > 0.0:
            # AC-NEW-8 — sustained monotone covariance growth signals
            # poor convergence. Surface DEGRADED but keep tracking;
            # only the SPD failure above flips us to LOST.
            self._isam2_state = IsamState.DEGRADED

        # AZ-388: AC-5.2 success hook. Resets the dwell timer and
        # fires the recovery signal if we were previously engaged.
        self._fallback.mark_successful_estimate(emitted_at)

        # AZ-387: smoothed-history → FDR side-channel. Emits any
        # newly-smoothed past-keyframes to FDR with ``smoothed=True``;
        # idempotent via the timestamp watermark. AC-4.5 forbids
        # routing smoothed estimates to C8 outbound — that filter is
        # enforced on the C8 side and documented as a cross-task
        # invariant. The hook is best-effort: a failure here is
        # logged but does NOT raise (the forward-time estimate has
        # already been computed).
        self._emit_smoothed_to_fdr_if_any(emitted_at, source_label, last_anchor_age_ms)

        return EstimatorOutput(
            frame_id=uuid4(),
            position_wgs84=position_wgs84,
            orientation_world_T_body=orientation,
            velocity_world_mps=velocity_world,
            covariance_6x6=covariance,
            source_label=source_label,
            last_satellite_anchor_age_ms=last_anchor_age_ms,
            smoothed=False,
            emitted_at=emitted_at,
        )

    def smoothed_history(self, n_keyframes: int) -> list[EstimatorOutput]:
        """Return up to ``min(n_keyframes, K)`` past smoothed estimates.

        Iterates the ``IncrementalFixedLagSmoother`` active timestamp
        map, sorts keys by ascending timestamp, takes the most recent
        ``min(n_keyframes, K)`` POSE keys (filters out velocity/bias
        keys), and emits an :class:`EstimatorOutput` per key with
        ``smoothed=True`` (Invariant 7). Out-of-window keyframes are
        not recoverable (Invariant 6).
        """
        handle = self._require_handle()
        if n_keyframes <= 0:
            return []

        try:
            active_values = self._smoother.calculateEstimate()
            ts_map = self._smoother.timestamps()
        except Exception:
            return []

        pose_entries: list[tuple[int, float]] = []
        for key in active_values.keys():
            ikey = int(key)
            if gtsam.symbolChr(ikey) != ord("x"):
                continue
            try:
                ts_sec = float(ts_map.at(ikey))
            except Exception:
                continue
            pose_entries.append((ikey, ts_sec))
        pose_entries.sort(key=lambda kt: kt[1])

        max_entries = min(n_keyframes, self._block.keyframe_window_size)
        selected = pose_entries[-max_entries:]
        if not selected:
            return []

        try:
            marginals = handle.compute_marginals()
        except Exception as exc:
            self._log.error(
                "c5.state.smoothed_history_marginals_failed",
                extra={
                    "kind": "c5.state.smoothed_history_marginals_failed",
                    "kv": {"error": str(exc)},
                },
            )
            raise EstimatorFatalError(f"smoothed_history marginals failed: {exc}") from exc

        last_anchor_age_ms = int(handle.last_anchor_age_ms())
        source_label = self._derive_source_label()
        emitted_at = self._clock.monotonic_ns()

        out: list[EstimatorOutput] = []
        for key, _ts in selected:
            try:
                cov = np.asarray(marginals.marginalCovariance(key), dtype=np.float64)
            except Exception as exc:
                self._log.error(
                    "c5.state.smoothed_history_per_key_failed",
                    extra={
                        "kind": "c5.state.smoothed_history_per_key_failed",
                        "kv": {"pose_key": key, "error": str(exc)},
                    },
                )
                continue
            _enforce_spd(cov)
            try:
                pose = active_values.atPose3(key)
            except Exception as exc:
                self._log.error(
                    "c5.state.smoothed_history_pose_read_failed",
                    extra={
                        "kind": "c5.state.smoothed_history_pose_read_failed",
                        "kv": {"pose_key": key, "error": str(exc)},
                    },
                )
                continue
            out.append(
                EstimatorOutput(
                    frame_id=uuid4(),
                    position_wgs84=self._enu_pose_to_wgs84(pose),
                    orientation_world_T_body=_quat_from_pose3(pose),
                    velocity_world_mps=(0.0, 0.0, 0.0),
                    covariance_6x6=cov,
                    source_label=source_label,
                    last_satellite_anchor_age_ms=last_anchor_age_ms,
                    smoothed=True,
                    emitted_at=emitted_at,
                )
            )
        return out

    def _emit_smoothed_to_fdr_if_any(
        self,
        emitted_at_ns: int,
        source_label: PoseSourceLabel,
        last_anchor_age_ms: int,
    ) -> None:
        """AZ-387 hook — emit newly-smoothed past-keyframes to FDR.

        AC-4.5 (revised): smoothed past-keyframes are an onboard-only
        side-channel for forensics. They MUST land in FDR (so post-
        flight tooling can refine the trajectory with later evidence)
        and they MUST NOT reach C8 outbound (the FC expects a
        forward-time stream). This hook walks the
        ``IncrementalFixedLagSmoother`` active POSE keys, filters to
        those whose smoother timestamp is strictly newer than the
        watermark, and enqueues one FDR record per key.

        Idempotency (AC-5): the watermark is the largest smoother
        timestamp ever emitted; the second call against the same
        smoother state emits zero records. Failure path: a marginals
        compute error logs ``c5.state.smoothed_history_marginals_failed``
        but does NOT raise (the forward-time estimate succeeded and
        was emitted to the caller already).
        """
        if self._fdr_client is None:
            return
        try:
            active_values = self._smoother.calculateEstimate()
            ts_map = self._smoother.timestamps()
        except Exception:
            return

        new_entries: list[tuple[int, float]] = []
        for key in active_values.keys():
            ikey = int(key)
            if gtsam.symbolChr(ikey) != ord("x"):
                continue
            try:
                ts_sec = float(ts_map.at(ikey))
            except Exception:
                continue
            if ts_sec <= self._smoothed_fdr_watermark_s:
                continue
            new_entries.append((ikey, ts_sec))
        if not new_entries:
            return

        new_entries.sort(key=lambda kt: kt[1])

        try:
            handle = self._require_handle()
            marginals = handle.compute_marginals()
        except Exception as exc:
            self._log.warning(
                "c5.state.smoothed_history_fdr_marginals_failed",
                extra={
                    "kind": "c5.state.smoothed_history_fdr_marginals_failed",
                    "kv": {"error": str(exc)},
                },
            )
            return

        for key, ts_sec in new_entries:
            try:
                cov = np.asarray(marginals.marginalCovariance(key), dtype=np.float64)
                _enforce_spd(cov)
                pose = active_values.atPose3(key)
            except Exception as exc:
                self._log.warning(
                    "c5.state.smoothed_history_fdr_per_key_failed",
                    extra={
                        "kind": "c5.state.smoothed_history_fdr_per_key_failed",
                        "kv": {"pose_key": key, "error": str(exc)},
                    },
                )
                continue
            position_wgs84 = self._enu_pose_to_wgs84(pose)
            orientation = _quat_from_pose3(pose)
            record = FdrRecord(
                schema_version=1,
                ts=datetime.now(tz=timezone.utc).isoformat(),
                producer_id="c5_state",
                kind="c5.state.smoothed_history",
                payload={
                    "pose_key": key,
                    "smoother_ts_s": ts_sec,
                    "emitted_at": emitted_at_ns,
                    "position_wgs84": {
                        "lat_deg": position_wgs84.lat_deg,
                        "lon_deg": position_wgs84.lon_deg,
                        "alt_m": position_wgs84.alt_m,
                    },
                    "orientation_world_T_body": {
                        "w": orientation.w,
                        "x": orientation.x,
                        "y": orientation.y,
                        "z": orientation.z,
                    },
                    "covariance_6x6": cov.tolist(),
                    "source_label": source_label.value,
                    "last_satellite_anchor_age_ms": last_anchor_age_ms,
                    "smoothed": True,
                },
            )
            try:
                self._fdr_client.enqueue(record)
            except Exception as exc:
                self._log.warning(
                    "c5.state.smoothed_history_fdr_enqueue_failed",
                    extra={
                        "kind": "c5.state.smoothed_history_fdr_enqueue_failed",
                        "kv": {"pose_key": key, "error": repr(exc)},
                    },
                )
                continue
            if ts_sec > self._smoothed_fdr_watermark_s:
                self._smoothed_fdr_watermark_s = ts_sec

    def health_snapshot(self) -> EstimatorHealth:
        """Return the current iSAM2 health snapshot.

        Cheap O(1) accumulator read — never touches Marginals
        (contract NFR: ``health_snapshot`` p99 ≤ 5 µs). Reports the
        cached :class:`IsamState`, the smoother's active keyframe
        count, the AC-NEW-8 ``cov_norm_growing_for_s`` rolling
        counter, and the spoof-promotion gate state via the injected
        :attr:`_source_label_machine` (default ``False`` if none).
        """
        return EstimatorHealth(
            isam2_state=self._isam2_state,
            keyframe_count=self._smoother_keyframe_count(),
            cov_norm_growing_for_s=self._cov_norm_growing_for_s(),
            spoof_promotion_blocked=self._spoof_promotion_blocked(),
        )

    # ------------------------------------------------------------------
    # Internals.

    def _require_handle(self) -> ISam2GraphHandle:
        if self._isam2_handle is None:
            raise StateEstimatorConfigError(
                "ISam2GraphHandle not attached; call attach_handle(...) before any add_*"
            )
        return self._isam2_handle

    def _guard_timestamp(self, ts_ns: int, *, source: str) -> None:
        if ts_ns < self._last_added_ts_ns:
            self._log.error(
                "c5.state.out_of_order",
                extra={
                    "kind": "c5.state.out_of_order",
                    "kv": {
                        "source": source,
                        "ts_ns": ts_ns,
                        "last_added_ts_ns": self._last_added_ts_ns,
                    },
                },
            )
            raise EstimatorDegradedError(
                f"out-of-order {source}: ts_ns={ts_ns} < last_added_ts_ns={self._last_added_ts_ns}"
            )

    def _reset_staging(self) -> None:
        """Clear the staging ``_graph`` + ``_values`` after a successful flush."""
        self._graph = gtsam.NonlinearFactorGraph()
        self._values = gtsam.Values()

    def _record_committed_pose_key(self, key: int) -> None:
        """Cache the most-recently committed pose key for ``current_estimate``.

        Called from every ``add_*`` success path AFTER
        ``handle.update`` returns; guarantees the key has a real
        value in ``_isam2`` before ``current_estimate`` tries to read
        it. The JACOBIAN path of ``add_pose_anchor`` deliberately
        does NOT call this — its pose is never committed to the
        iSAM2 graph (Invariant 3).
        """
        self._last_committed_pose_key = key

    def _pose_at_key(self, key: int) -> gtsam.Pose3:
        """Read ``Pose3`` for ``key`` from the current iSAM2 estimate.

        Raises :class:`EstimatorFatalError` if the key is missing or
        the read fails (e.g. GTSAM raised). The caller is responsible
        for guaranteeing the key was previously committed via
        :meth:`_record_committed_pose_key`.
        """
        try:
            values = self._isam2.calculateEstimate()
            return values.atPose3(key)
        except Exception as exc:
            raise EstimatorFatalError(f"_pose_at_key({key}) failed: {exc}") from exc

    def _enu_pose_to_wgs84(self, pose: gtsam.Pose3) -> LatLonAlt:
        """Convert a local-ENU pose's translation to WGS84.

        Uses the injected ``_enu_origin`` (or :data:`_DEFAULT_ENU_ORIGIN`
        when none was wired). The orientation is preserved verbatim
        as a body→world quaternion — only the translation needs the
        WGS coordinate change.
        """
        origin = self._enu_origin if self._enu_origin is not None else _DEFAULT_ENU_ORIGIN
        translation = np.asarray(pose.translation(), dtype=np.float64).reshape(3)
        try:
            return WgsConverter.local_enu_to_latlonalt(origin, translation)
        except Exception as exc:
            raise EstimatorFatalError(f"_enu_pose_to_wgs84 failed: {exc}") from exc

    def _pose_estimate_to_matrix(self, pose: PoseEstimate) -> np.ndarray:
        """Convert a C4 :class:`PoseEstimate` to a 4x4 homogeneous-transform matrix.

        WGS84 → ENU via the injected origin; ``Quat`` → 3x3 rotation
        via :func:`_quat_dto_to_rot`. The result is what GTSAM's
        ``Pose3`` constructor consumes.
        """
        origin = self._enu_origin if self._enu_origin is not None else _DEFAULT_ENU_ORIGIN
        try:
            enu = WgsConverter.latlonalt_to_local_enu(origin, pose.position_wgs84)
        except Exception as exc:
            raise EstimatorDegradedError(
                f"add_pose_anchor: WGS84→ENU failed for frame {pose.frame_id}: {exc}"
            ) from exc
        rotation = _quat_dto_to_rot(pose.orientation_world_T_body)
        matrix = np.eye(4, dtype=np.float64)
        matrix[:3, :3] = rotation
        matrix[:3, 3] = enu
        return matrix

    def _latest_velocity_or_zero(self) -> tuple[float, float, float]:
        """Read the most-recent IMU keyframe's velocity or fall back to zero.

        ``CombinedImuFactor`` writes a 3-vector velocity at the ``v``
        key for each IMU keyframe. When no IMU keyframe has been
        committed yet, returns ``(0, 0, 0)`` — the downstream consumer
        infers this from a ``DEAD_RECKONED`` / ``VISUAL_PROPAGATED``
        source label.
        """
        if self._prev_imu_v_key is None:
            return (0.0, 0.0, 0.0)
        try:
            values = self._isam2.calculateEstimate()
            raw = np.asarray(values.atVector(self._prev_imu_v_key), dtype=np.float64).reshape(3)
            return (float(raw[0]), float(raw[1]), float(raw[2]))
        except Exception:
            return (0.0, 0.0, 0.0)

    def _notify_source_label_anchor(self, pose: PoseEstimate) -> None:
        """Forward a successful satellite-anchor add to the AZ-385 SM.

        The estimator does not currently compute the visual-vs-GPS
        consistency delta itself (AZ-389 orthorectifier or the
        composition root will supply it externally once available).
        Until then we pass ``None``, which causes the state machine
        to classify any in-block promotion attempt as
        ``no_gps_observation`` — the conservative reject path.
        """
        machine = self._source_label_machine
        if not isinstance(machine, SourceLabelStateMachine):
            return
        try:
            machine.notify_satellite_anchor(
                now_ns=self._clock.monotonic_ns(),
                gps_consistency_delta_m=None,
            )
        except Exception as exc:
            self._log.error(
                "c5.state.source_label_anchor_notify_failed",
                extra={
                    "kind": "c5.state.source_label_anchor_notify_failed",
                    "kv": {"frame_id": str(pose.frame_id), "error": str(exc)},
                },
            )

    def _derive_source_label(self) -> PoseSourceLabel:
        """Read the source label from the AZ-385 state machine if wired.

        Falls back to ``VISUAL_PROPAGATED`` when no state machine is
        attached — the contract's neutral steady-state default before
        the spoof-promotion gate has any evidence either way.
        """
        if self._source_label_machine is None:
            return PoseSourceLabel.VISUAL_PROPAGATED
        try:
            label = self._source_label_machine.current_label()
        except Exception as exc:
            self._log.error(
                "c5.state.source_label_machine_failed",
                extra={
                    "kind": "c5.state.source_label_machine_failed",
                    "kv": {"error": str(exc)},
                },
            )
            return PoseSourceLabel.VISUAL_PROPAGATED
        if not isinstance(label, PoseSourceLabel):
            return PoseSourceLabel.VISUAL_PROPAGATED
        return label

    def _spoof_promotion_blocked(self) -> bool:
        """Query the AZ-385 state machine for the spoof gate state."""
        if self._source_label_machine is None:
            return False
        try:
            return bool(self._source_label_machine.is_spoof_promotion_blocked())
        except Exception:
            return False

    def _smoother_keyframe_count(self) -> int:
        """Count the smoother's active POSE keys.

        ``IncrementalFixedLagSmoother.calculateEstimate()`` returns
        the ``Values`` for every key still inside the sliding
        window; we filter to POSE keys (``'x'`` namespace) per
        Invariant 6.
        """
        try:
            keys = list(self._smoother.calculateEstimate().keys())
        except Exception:
            return 0
        return sum(1 for k in keys if gtsam.symbolChr(int(k)) == ord("x"))

    def _record_cov_norm_sample(self, ts_ns: int, covariance: np.ndarray) -> None:
        """Append a ``(ts_ns, fro_norm(cov))`` sample to the rolling window."""
        norm = float(np.linalg.norm(covariance, ord="fro"))
        self._cov_norm_window.append((ts_ns, norm))
        self._prune_cov_norm_window(ts_ns)

    def _prune_cov_norm_window(self, now_ns: int) -> None:
        cutoff = now_ns - _COV_NORM_WINDOW_NS
        while self._cov_norm_window and self._cov_norm_window[0][0] < cutoff:
            self._cov_norm_window.popleft()

    def _cov_norm_growing_for_s(self) -> float:
        """Return the length (s) of the latest strictly-rising suffix.

        Walks the rolling window from newest to oldest; accumulates
        time while each sample is greater than its successor. Resets
        the moment we find a non-rising step. Maps AC-NEW-8: "Resets
        to 0 on a non-rising frame".
        """
        if len(self._cov_norm_window) < 2:
            return 0.0
        samples = list(self._cov_norm_window)
        # Walk newest → second-newest; stop when the chain breaks.
        oldest_rising_idx = len(samples) - 1
        for i in range(len(samples) - 1, 0, -1):
            if samples[i][1] > samples[i - 1][1]:
                oldest_rising_idx = i - 1
            else:
                break
        if oldest_rising_idx == len(samples) - 1:
            return 0.0
        span_ns = samples[-1][0] - samples[oldest_rising_idx][0]
        return max(0.0, span_ns * 1e-9)


# ----------------------------------------------------------------------
# Module-level helpers.


def create(
    *,
    config: Config,
    imu_preintegrator: Any,
    se3_utils: Any,
    wgs_converter: Any,
    fdr_client: Any,
) -> tuple[StateEstimator, ISam2GraphHandle]:
    """Composition-root factory — returns ``(estimator, handle)`` tuple.

    Construction order matters: the estimator is built first, the
    handle is wired against it (the handle holds the estimator
    reference for graph access), and the estimator is back-wired with
    the handle so its ``add_*`` methods can call into it.
    """
    estimator = GtsamIsam2StateEstimator(
        config,
        imu_preintegrator=imu_preintegrator,
        se3_utils=se3_utils,
        wgs_converter=wgs_converter,
        fdr_client=fdr_client,
    )
    handle = ISam2GraphHandleImpl(estimator)
    estimator.attach_handle(handle)
    return estimator, handle


def register() -> None:
    """Register :func:`create` under the ``"gtsam_isam2"`` strategy slug.

    Called by per-binary bootstrap modules under the
    ``BUILD_STATE_GTSAM_ISAM2`` flag, and by unit-test fixtures that
    exercise the factory path. Deliberately NOT called at module
    import — ADR-002 requires the bootstrap module to be the single
    register-call site so the build-flag gate stays the only place
    that decides which strategies are linked.
    """
    from gps_denied_onboard.runtime_root.state_factory import register_state_estimator

    register_state_estimator(_STRATEGY, create)


# ----------------------------------------------------------------------
# Module-level pure helpers (AZ-383).


def _datetime_to_ns(dt: Any) -> int:
    """Convert ``dt.timestamp() * 1e9`` to int ns.

    Accepts any object exposing a ``timestamp()`` method (e.g.
    :class:`datetime.datetime`); avoids importing ``datetime`` here
    so test stubs can pass synthetic time objects.
    """
    return int(dt.timestamp() * 1_000_000_000)


def _pose_se3_to_gtsam(pose_se3: Any) -> gtsam.Pose3:
    """Wrap a 4x4 numpy SE(3) matrix into ``gtsam.Pose3``."""
    arr = np.asarray(pose_se3, dtype=np.float64)
    if arr.shape != (4, 4):
        raise ValueError(f"pose_se3 must be 4x4; got shape {arr.shape}")
    return gtsam.Pose3(arr)


def _quat_dto_to_rot(q: Quat) -> np.ndarray:
    """Convert a :class:`Quat` DTO (scalar-first, body→world) to a 3x3 rotation."""
    w, x, y, z = float(q.w), float(q.x), float(q.y), float(q.z)
    return np.array(
        [
            [1 - 2 * (y * y + z * z), 2 * (x * y - z * w), 2 * (x * z + y * w)],
            [2 * (x * y + z * w), 1 - 2 * (x * x + z * z), 2 * (y * z - x * w)],
            [2 * (x * z - y * w), 2 * (y * z + x * w), 1 - 2 * (x * x + y * y)],
        ],
        dtype=np.float64,
    )


def _build_pose_noise(covariance: Any | None) -> gtsam.noiseModel.Base:
    """Build a 6-DoF Gaussian noise model from a 6x6 covariance.

    Falls back to an isotropic default when the input is ``None`` —
    the C5 contract permits this for early-flight VIO frames where
    the covariance is not yet computed; the iSAM2 solver penalises
    over-tight priors so the default is conservative.
    """
    if covariance is None:
        return gtsam.noiseModel.Isotropic.Sigma(6, _DEFAULT_POSE_SIGMA)
    cov = np.asarray(covariance, dtype=np.float64)
    if cov.shape != (6, 6):
        raise ValueError(f"covariance_6x6 must be 6x6; got shape {cov.shape}")
    return gtsam.noiseModel.Gaussian.Covariance(cov)


def _make_timestamp_map(
    keys: list[int], ts_ns: int
) -> gtsam_unstable.FixedLagSmootherKeyTimestampMap:
    """Build a ``FixedLagSmootherKeyTimestampMap`` for the smoother.

    The smoother needs per-key arrival timestamps in seconds (its
    sliding-window evict logic uses them); we feed every newly
    inserted key the same window-end timestamp.
    """
    ts_map = gtsam_unstable.FixedLagSmootherKeyTimestampMap()
    ts_seconds = ts_ns * 1e-9
    for key in keys:
        ts_map.insert((key, ts_seconds))
    return ts_map


# ----------------------------------------------------------------------
# Module-level pure helpers (AZ-384).


def _quat_from_pose3(pose: gtsam.Pose3) -> Quat:
    """Build a scalar-first :class:`Quat` from a ``gtsam.Pose3``.

    Uses ``Rot3.toQuaternion`` to get the body→world unit
    quaternion; we explicitly normalise to defend against numerical
    drift inside the iSAM2 estimate.
    """
    rot = pose.rotation()
    q = rot.toQuaternion()
    w, x, y, z = float(q.w()), float(q.x()), float(q.y()), float(q.z())
    norm = (w * w + x * x + y * y + z * z) ** 0.5
    if norm < 1e-12:
        return Quat(w=1.0, x=0.0, y=0.0, z=0.0)
    return Quat(w=w / norm, x=x / norm, y=y / norm, z=z / norm)


def _enforce_spd(covariance: np.ndarray) -> None:
    """Cholesky-check ``covariance``; raise ``EstimatorFatalError`` on failure.

    The C5 contract (Invariant 10) demands every emitted
    ``covariance_6x6`` is symmetric positive-definite. We use
    Cholesky-decomposition as the defensive check because it's the
    same primitive iSAM2 uses internally — a failure here means the
    posterior is numerically broken, NOT merely degraded.
    """
    try:
        np.linalg.cholesky(covariance)
    except LinAlgError as exc:
        raise EstimatorFatalError(f"covariance failed SPD invariant: {exc}") from exc
