"""AZ-386 ``EskfStateEstimator`` — mandatory simple-baseline (IT-12).

Error-State Kalman Filter (ESKF) over a 16-state error vector:

============  =================================
Indices       Component
------------  ---------------------------------
``[0:3]``     position error (ENU world, m)
``[3:6]``     velocity error (ENU world, m/s)
``[6:9]``     orientation error (body-axis, rad)
``[9:12]``    accel bias error (body, m/s²)
``[12:15]``   gyro bias error (body, rad/s)
``[15]``      IMU dt offset (s)
============  =================================

The nominal state is kept separately (position, velocity,
body→world quaternion, biases, dt offset). Per the ESKF
formulation: the nominal state is updated by full nonlinear IMU
integration; the error covariance is propagated via a linearised
transition matrix; measurement updates correct the nominal state
through ``inject_error`` after the standard Joseph-form Kalman
update.

AC-2.1a engine-rule application: ESKF is the mandatory simple
baseline against which the iSAM2 estimator is compared (IT-12).
It uses ONLY NumPy — no GTSAM dependency in this module — so the
``BUILD_STATE_ESKF=ON`` binary can ship without GTSAM at all.

Per the C5 contract surface this estimator is composition-
selectable via ``config.components["c5_state"].strategy = "eskf"``
plus the ``BUILD_STATE_ESKF`` flag (ADR-002 build-time exclusion).

Honesty caveat — Invariant 7 deviation
=====================================

The C5 contract Invariant 7 says ``smoothed_history`` entries
carry ``smoothed=True``. The AZ-386 task spec (AC-6) overrides
this for ESKF specifically: the simple-baseline cannot smooth, so
its ``smoothed_history`` entries report ``smoothed=False``. This
is intentionally honest — and forces C8 outbound to use a routing
rule that doesn't conflate "smoothed=True" with "do not emit to
FC". The cross-task invariant lives in AZ-261's C8 outbound
filter; this module documents the deviation in the
``smoothed_history`` method docstring.
"""

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 uuid4

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._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__ = [
    "EskfStateEstimator",
    "create",
    "register",
]

_STRATEGY: Final[str] = "eskf"

# ENU world gravity vector — positive Z is up, gravity points down.
_GRAVITY_ENU: Final[np.ndarray] = np.array([0.0, 0.0, -9.80665], dtype=np.float64)

# State dimensions.
_N_STATE: Final[int] = 16
_IDX_POS: Final[slice] = slice(0, 3)
_IDX_VEL: Final[slice] = slice(3, 6)
_IDX_ROT: Final[slice] = slice(6, 9)
_IDX_BA: Final[slice] = slice(9, 12)
_IDX_BG: Final[slice] = slice(12, 15)
_IDX_DT: Final[int] = 15

# Initial covariance diagonals. Conservative — the contract permits
# defaults so a freshly-built estimator can run on synthetic
# fixtures without explicit operator priors.
_INIT_SIGMA_POS_M: Final[float] = 1.0
_INIT_SIGMA_VEL_MPS: Final[float] = 0.5
_INIT_SIGMA_ROT_RAD: Final[float] = 0.1
_INIT_SIGMA_BA: Final[float] = 0.05
_INIT_SIGMA_BG: Final[float] = 0.005
_INIT_SIGMA_DT: Final[float] = 0.001

# Process noise (per second, applied as dt * sigma² in the propagation).
_PROC_SIGMA_ACCEL: Final[float] = 0.2
_PROC_SIGMA_GYRO: Final[float] = 0.01
_PROC_SIGMA_BA: Final[float] = 0.001
_PROC_SIGMA_BG: Final[float] = 0.0001

# Fall-back measurement noise when the input DTO has no covariance.
_DEFAULT_MEAS_SIGMA: Final[float] = 0.1

# Filter divergence threshold (AC-9): Mahalanobis ``r^T S^{-1} r`` >
# (10 sigma)^2 = 100 triggers EstimatorFatalError + AC-5.2 fallback.
# The spec wording is "innovation exceeds 10x the measurement-covariance
# norm" — we read this as "10 standard deviations in the innovation
# covariance S = H P H^T + R", which is the standard Kalman-filter
# divergence gate.
_INNOV_DIVERGENCE_SIGMA: Final[float] = 10.0
_INNOV_DIVERGENCE_MAHAL_SQ: Final[float] = _INNOV_DIVERGENCE_SIGMA**2

# Smoothed-history circular buffer length (entries are forward-time
# estimates; per AC-6 they carry smoothed=False).
_HISTORY_DEPTH: Final[int] = 30

# Default ENU origin for WGS84 conversion before the composition
# root injects a real origin. Mirrors the iSAM2 default.
_DEFAULT_ENU_ORIGIN: Final[LatLonAlt] = LatLonAlt(lat_deg=0.0, lon_deg=0.0, alt_m=0.0)


class EskfStateEstimator(StateEstimator):
    """ESKF mandatory simple-baseline. NumPy-only; no GTSAM."""

    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._log = get_logger("c5_state.eskf_baseline")

        self._nominal_pos: np.ndarray = np.zeros(3, dtype=np.float64)
        self._nominal_vel: np.ndarray = np.zeros(3, dtype=np.float64)
        self._nominal_q: np.ndarray = np.array(
            [1.0, 0.0, 0.0, 0.0], dtype=np.float64
        )  # (w, x, y, z)
        self._nominal_ba: np.ndarray = np.zeros(3, dtype=np.float64)
        self._nominal_bg: np.ndarray = np.zeros(3, dtype=np.float64)
        self._nominal_dt: float = 0.0
        self._P: np.ndarray = self._initial_covariance()

        # The previous VIO frame's world-frame pose; we treat each
        # VIO arrival as a relative-pose measurement against the
        # nominal motion since the last VIO frame.
        self._prev_vio_pose: np.ndarray | None = None
        self._prev_vio_nominal_pos: np.ndarray | None = None
        self._prev_vio_nominal_q: np.ndarray | None = None

        self._last_added_ts_ns: int = 0
        # Wall-clock-equivalent monotonic timestamp of the most-recent
        # successful satellite anchor; used by ``last_satellite_anchor_age_ms``.
        # 0 means "no anchor yet"; ``current_estimate`` reports a very
        # large age in that case.
        self._last_anchor_ns: int = 0

        self._isam2_state: IsamState = IsamState.INIT
        self._enu_origin: LatLonAlt | None = None
        self._history: deque[EstimatorOutput] = deque(maxlen=_HISTORY_DEPTH)

        # AZ-490 cold-start ladder. Same semantics as
        # ``GtsamIsam2StateEstimator`` — see that class for the
        # field-level rationale (idempotency, conflict detection,
        # window-closed gate, FC-EKF fallback FDR).
        self._takeoff_origin_set: tuple[LatLonAlt, float, float] | None = None
        self._origin_source: Literal["manifest", "fc_ekf"] | None = None
        self._cold_start_window_closed: bool = False

        # AZ-385: source-label SM. Eagerly constructed; composition
        # root drives notify_gps_health + subscribe_spoof_rejection.
        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",
        )

        # AZ-388: AC-5.2 fallback watcher.
        self._fallback = FallbackWatcher(
            threshold_s=block.no_estimate_fallback_s,
            fdr_client=fdr_client,
            clock_ns=self._clock.monotonic_ns,
            producer_id="c5_state",
        )

        self._log.debug(
            "c5.state.eskf_initialised",
            extra={
                "kind": "c5.state.eskf_initialised",
                "kv": {"strategy": "eskf"},
            },
        )

    @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(strategy="eskf")
        if isinstance(block, C5StateConfig):
            return block
        raise StateEstimatorConfigError(
            f"config.components['c5_state'] must be a C5StateConfig; got {type(block).__name__}"
        )

    @staticmethod
    def _initial_covariance() -> np.ndarray:
        diag = np.empty(_N_STATE, dtype=np.float64)
        diag[_IDX_POS] = _INIT_SIGMA_POS_M**2
        diag[_IDX_VEL] = _INIT_SIGMA_VEL_MPS**2
        diag[_IDX_ROT] = _INIT_SIGMA_ROT_RAD**2
        diag[_IDX_BA] = _INIT_SIGMA_BA**2
        diag[_IDX_BG] = _INIT_SIGMA_BG**2
        diag[_IDX_DT] = _INIT_SIGMA_DT**2
        return np.diag(diag)

    # ------------------------------------------------------------------
    # Public injection points (mirror the iSAM2 estimator surface).

    def set_enu_origin(self, origin: LatLonAlt) -> None:
        self._enu_origin = origin

    def attach_source_label_state_machine(self, machine: Any) -> None:
        self._source_label_machine = machine

    def notify_gps_health(self, gps_health: GpsHealth, now_ns: int | None = None) -> None:
        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:
        machine = self._source_label_machine
        if not isinstance(machine, SourceLabelStateMachine):
            raise StateEstimatorConfigError(
                "subscribe_spoof_rejection requires the built-in SourceLabelStateMachine"
            )
        return machine.subscribe_rejection(callback)

    def check_fallback_state(self, now_ns: int) -> bool:
        return self._fallback.check_and_engage(now_ns)

    def subscribe_fallback_engaged(
        self, callback: FallbackEngagementCallback
    ) -> FallbackSubscription:
        return self._fallback.subscribe_engaged(callback)

    def subscribe_fallback_recovered(
        self, callback: FallbackRecoveryCallback
    ) -> FallbackSubscription:
        return self._fallback.subscribe_recovered(callback)

    # ------------------------------------------------------------------
    # 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 ESKF nominal state + position covariance from operator origin.

        See :meth:`StateEstimator.set_takeoff_origin` for the full
        contract. ESKF impl: sets the ENU origin to ``origin``,
        zeros the nominal position (the operator-supplied origin
        IS the local-ENU (0,0,0)), and writes the position block of
        ``_P`` to ``diag(sigma_horiz_m², sigma_horiz_m², sigma_vert_m²)``.
        """
        _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})"
            )

        self._enu_origin = origin
        self._nominal_pos = np.zeros(3, dtype=np.float64)
        # Position block of the error covariance — explicit overwrite
        # (the diagonal stays diagonal here; off-diagonal terms in
        # ``_P[0:3, ...]`` were zero at construction and we don't
        # want to lose those zeros).
        pos_var = np.diag(
            np.array(
                [sigma_horiz_m**2, sigma_horiz_m**2, sigma_vert_m**2],
                dtype=np.float64,
            )
        )
        self._P[_IDX_POS, _IDX_POS] = pos_var
        # Symmetrise defensively — the rest of the matrix may carry
        # accumulated cross-terms from earlier construction; the
        # symmetrise costs nothing and keeps the SPD invariant
        # observable on the next read.
        self._P = 0.5 * (self._P + self._P.T)

        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,
                },
            },
        )

    def _close_cold_start_window(self) -> None:
        """ESKF mirror of the iSAM2 helper — see that class for the contract."""
        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:
        """ESKF bounded-delta dispatch — mirror of the iSAM2 method."""
        machine = self._source_label_machine
        if not isinstance(machine, SourceLabelStateMachine):
            return None
        smoother_latlon: LatLonAlt | None
        try:
            smoother_latlon = self._enu_pose_to_wgs84()
        except Exception:
            smoother_latlon = None
        return machine.process_gps_sample(
            sample.position_wgs84,
            smoother_estimate=smoother_latlon,
            now_ns=now_ns,
        )

    # ------------------------------------------------------------------
    # Protocol: factor adds.

    def add_vio(self, vio: VioOutput) -> None:
        """Relative-pose measurement from C1 VIO.

        First VIO frame seeds ``_prev_vio_*``; subsequent frames feed
        the relative-pose update. The measurement is the SE(3) delta
        ``inv(prev_vio) @ curr_vio`` and the predicted measurement is
        the analogous nominal delta — both are projected to a 6-vector
        residual in the previous body frame.
        """
        self._close_cold_start_window()
        ts_ns = vio.emitted_at_ns
        self._guard_timestamp(ts_ns, source="vio")
        curr_pose = vio.relative_pose_T.matrix()

        if self._prev_vio_pose is None:
            self._prev_vio_pose = curr_pose
            self._prev_vio_nominal_pos = self._nominal_pos.copy()
            self._prev_vio_nominal_q = self._nominal_q.copy()
            self._last_added_ts_ns = ts_ns
            self._log.debug(
                "c5.state.eskf_add_vio_first_frame",
                extra={
                    "kind": "c5.state.eskf_add_vio_first_frame",
                    "kv": {"frame_id": str(vio.frame_id), "ts_ns": ts_ns},
                },
            )
            return

        prev_pose = self._prev_vio_pose
        measured_delta_world = curr_pose[:3, 3] - prev_pose[:3, 3]
        prev_nominal_pos = (
            self._prev_vio_nominal_pos
            if self._prev_vio_nominal_pos is not None
            else self._nominal_pos.copy()
        )
        predicted_delta_world = self._nominal_pos - prev_nominal_pos

        residual_pos = measured_delta_world - predicted_delta_world

        prev_R = prev_pose[:3, :3]
        curr_R_meas = prev_pose[:3, :3].T @ curr_pose[:3, :3]
        prev_q_nominal = (
            self._prev_vio_nominal_q
            if self._prev_vio_nominal_q is not None
            else self._nominal_q.copy()
        )
        curr_R_nominal = _quat_to_rot(prev_q_nominal).T @ _quat_to_rot(self._nominal_q)
        residual_rot = _rot_to_axis_angle(curr_R_meas @ curr_R_nominal.T)

        residual = np.concatenate([residual_pos, residual_rot])

        # Measurement Jacobian: the residual depends on the CURRENT
        # state's position + orientation only (the snapshot at the
        # previous frame is treated as a fixed reference). This is a
        # SIMPLIFICATION of the full ESKF relative-pose Jacobian —
        # honest about being a baseline.
        H = np.zeros((6, _N_STATE), dtype=np.float64)
        H[0:3, _IDX_POS] = np.eye(3)
        H[3:6, _IDX_ROT] = prev_R  # rotate body-frame perturbation back to world
        R_meas = _measurement_noise(vio.pose_covariance_6x6)

        try:
            self._kalman_update(H, residual, R_meas, source="vio")
        except EstimatorFatalError:
            self._isam2_state = IsamState.LOST
            raise
        except EstimatorDegradedError:
            raise
        except Exception as exc:
            self._log.error(
                "c5.state.eskf_add_vio_failed",
                extra={
                    "kind": "c5.state.eskf_add_vio_failed",
                    "kv": {"frame_id": str(vio.frame_id), "error": str(exc)},
                },
            )
            raise EstimatorDegradedError(f"eskf add_vio failed: {exc}") from exc

        self._prev_vio_pose = curr_pose
        self._prev_vio_nominal_pos = self._nominal_pos.copy()
        self._prev_vio_nominal_q = self._nominal_q.copy()
        self._last_added_ts_ns = ts_ns
        if self._isam2_state == IsamState.INIT:
            self._isam2_state = IsamState.TRACKING

    def add_pose_anchor(self, pose: PoseEstimate) -> None:
        """Absolute-pose measurement from C4.

        Per AC-4 the ESKF does NOT skip the JACOBIAN-mode anchor (it
        has no graph to throttle); it integrates every anchor as a
        regular measurement.
        """
        self._close_cold_start_window()
        ts_ns = int(pose.emitted_at)
        self._guard_timestamp(ts_ns, source="pose_anchor")
        meas_pose = self._pose_estimate_to_matrix(pose)

        # Both modes are treated identically by the ESKF — the
        # JACOBIAN exclusion is iSAM2-graph-specific. AC-4.
        self._last_anchor_ns = self._clock.monotonic_ns()

        residual_pos = meas_pose[:3, 3] - self._nominal_pos
        meas_R = meas_pose[:3, :3]
        nominal_R = _quat_to_rot(self._nominal_q)
        residual_rot = _rot_to_axis_angle(meas_R @ nominal_R.T)
        residual = np.concatenate([residual_pos, residual_rot])

        H = np.zeros((6, _N_STATE), dtype=np.float64)
        H[0:3, _IDX_POS] = np.eye(3)
        H[3:6, _IDX_ROT] = np.eye(3)
        R_meas = _measurement_noise(pose.covariance_6x6)

        try:
            self._kalman_update(H, residual, R_meas, source="pose_anchor")
        except EstimatorFatalError:
            self._isam2_state = IsamState.LOST
            raise
        except EstimatorDegradedError:
            raise
        except Exception as exc:
            self._log.error(
                "c5.state.eskf_add_pose_anchor_failed",
                extra={
                    "kind": "c5.state.eskf_add_pose_anchor_failed",
                    "kv": {"frame_id": str(pose.frame_id), "error": str(exc)},
                },
            )
            raise EstimatorDegradedError(f"eskf add_pose_anchor failed: {exc}") from exc

        self._notify_source_label_anchor(pose)
        self._last_added_ts_ns = ts_ns
        if self._isam2_state == IsamState.INIT:
            self._isam2_state = IsamState.TRACKING

    def add_fc_imu(self, imu_window: ImuWindow) -> None:
        """Predict nominal state + propagate covariance over the IMU window."""
        self._close_cold_start_window()
        self._guard_timestamp(imu_window.ts_end_ns, source="imu_window")
        samples = imu_window.samples
        if not samples:
            self._last_added_ts_ns = imu_window.ts_end_ns
            return

        prev_ts_ns = samples[0].ts_ns
        for sample in samples:
            dt = max(0.0, (sample.ts_ns - prev_ts_ns) / 1_000_000_000)
            if dt > 0.0:
                try:
                    self._predict(
                        dt=dt,
                        accel_meas=np.asarray(sample.accel_xyz, dtype=np.float64),
                        gyro_meas=np.asarray(sample.gyro_xyz, dtype=np.float64),
                    )
                except Exception as exc:
                    self._log.error(
                        "c5.state.eskf_predict_failed",
                        extra={
                            "kind": "c5.state.eskf_predict_failed",
                            "kv": {"ts_ns": sample.ts_ns, "error": str(exc)},
                        },
                    )
                    raise EstimatorDegradedError(f"eskf predict failed: {exc}") from exc
            prev_ts_ns = sample.ts_ns

        self._last_added_ts_ns = imu_window.ts_end_ns
        if self._isam2_state == IsamState.INIT:
            self._isam2_state = IsamState.TRACKING

    # ------------------------------------------------------------------
    # Protocol: outputs.

    def current_estimate(self) -> EstimatorOutput:
        """Forward-time estimate. ``smoothed=False`` (Invariant 7)."""
        now_ns = self._clock.monotonic_ns()
        self._fallback.check_and_engage(now_ns)

        cov6 = self._pose_covariance_6x6()
        try:
            _enforce_spd(cov6)
        except EstimatorFatalError:
            self._isam2_state = IsamState.LOST
            self._log.error(
                "c5.state.eskf_current_estimate_spd_failed",
                extra={
                    "kind": "c5.state.eskf_current_estimate_spd_failed",
                    "kv": {"cov_fro_norm": float(np.linalg.norm(cov6, ord="fro"))},
                },
            )
            raise

        emitted_at = self._clock.monotonic_ns()
        position_wgs84 = self._enu_pose_to_wgs84()
        orientation = _quat_to_quat_dto(self._nominal_q)
        velocity_world = (
            float(self._nominal_vel[0]),
            float(self._nominal_vel[1]),
            float(self._nominal_vel[2]),
        )
        last_anchor_age_ms = self._compute_last_anchor_age_ms(now_ns)
        source_label = self._derive_source_label()
        out = EstimatorOutput(
            frame_id=uuid4(),
            position_wgs84=position_wgs84,
            orientation_world_T_body=orientation,
            velocity_world_mps=velocity_world,
            covariance_6x6=cov6,
            source_label=source_label,
            last_satellite_anchor_age_ms=last_anchor_age_ms,
            smoothed=False,
            emitted_at=emitted_at,
        )
        self._history.append(out)
        self._fallback.mark_successful_estimate(emitted_at)
        return out

    def smoothed_history(self, n_keyframes: int) -> list[EstimatorOutput]:
        """Return up to ``n_keyframes`` recent forward-time entries.

        ESKF cannot smooth (it is forward-only). Per AZ-386 AC-6 and
        the "honest baseline" principle we surface the circular
        buffer of past forward-time estimates AS-IS, with
        ``smoothed=False`` — deviating from C5 contract Invariant 7
        which generally assumes ``smoothed_history`` is truly
        smoothed. The cross-task invariant (AZ-261 C8 outbound
        filter) MUST therefore not equate ``smoothed=True`` with "do
        not emit to FC"; it MUST use the dedicated routing rule
        documented in the C5 contract.
        """
        if n_keyframes <= 0:
            return []
        if not self._history:
            return []
        out: list[EstimatorOutput] = list(self._history)[-n_keyframes:]
        return [_with_smoothed_false(entry) for entry in out]

    def health_snapshot(self) -> EstimatorHealth:
        """O(1) accumulator read.

        Reports the simplified ESKF lifecycle: INIT until the first
        measurement, TRACKING after, DEGRADED on innovation-threshold
        breaches, LOST on SPD failure or filter divergence.
        ``keyframe_count`` mirrors the history buffer length;
        ``cov_norm_growing_for_s`` is set to 0.0 — the ESKF has no
        sliding window with which to estimate a covariance trend.
        """
        return EstimatorHealth(
            isam2_state=self._isam2_state,
            keyframe_count=len(self._history),
            cov_norm_growing_for_s=0.0,
            spoof_promotion_blocked=self._spoof_promotion_blocked(),
        )

    # ------------------------------------------------------------------
    # Internals: KF math.

    def _predict(self, *, dt: float, accel_meas: np.ndarray, gyro_meas: np.ndarray) -> None:
        # Compensate biases, rotate accel to world, integrate.
        accel_body = accel_meas - self._nominal_ba
        gyro_body = gyro_meas - self._nominal_bg
        R = _quat_to_rot(self._nominal_q)
        accel_world = R @ accel_body + _GRAVITY_ENU

        new_pos = self._nominal_pos + self._nominal_vel * dt + 0.5 * accel_world * dt * dt
        new_vel = self._nominal_vel + accel_world * dt
        delta_q = _quat_from_axis_angle(gyro_body * dt)
        new_q = _quat_normalise(_quat_mul(self._nominal_q, delta_q))

        self._nominal_pos = new_pos
        self._nominal_vel = new_vel
        self._nominal_q = new_q

        # Error covariance propagation. F is approximately:
        #   d(pos_err)/dt   = vel_err
        #   d(vel_err)/dt   = -R @ skew(accel_body) @ rot_err - R @ ba_err
        #   d(rot_err)/dt   = -skew(gyro_body) @ rot_err - bg_err
        # The dt and bias-dt couplings are captured as identity blocks.
        F = np.eye(_N_STATE, dtype=np.float64)
        F[_IDX_POS, _IDX_VEL] = np.eye(3) * dt
        F[_IDX_VEL, _IDX_ROT] = -R @ _skew(accel_body) * dt
        F[_IDX_VEL, _IDX_BA] = -R * dt
        F[_IDX_ROT, _IDX_ROT] = np.eye(3) - _skew(gyro_body) * dt
        F[_IDX_ROT, _IDX_BG] = -np.eye(3) * dt

        Q = np.zeros((_N_STATE, _N_STATE), dtype=np.float64)
        Q[_IDX_VEL, _IDX_VEL] = np.eye(3) * (_PROC_SIGMA_ACCEL**2) * dt
        Q[_IDX_ROT, _IDX_ROT] = np.eye(3) * (_PROC_SIGMA_GYRO**2) * dt
        Q[_IDX_BA, _IDX_BA] = np.eye(3) * (_PROC_SIGMA_BA**2) * dt
        Q[_IDX_BG, _IDX_BG] = np.eye(3) * (_PROC_SIGMA_BG**2) * dt

        self._P = F @ self._P @ F.T + Q
        # Cholesky-positive enforcement happens on the read path; we
        # keep the symmetrisation here to defend against accumulating
        # numerical asymmetry.
        self._P = 0.5 * (self._P + self._P.T)

    def _kalman_update(
        self,
        H: np.ndarray,
        residual: np.ndarray,
        R_meas: np.ndarray,
        *,
        source: str,
    ) -> None:
        """Joseph-form Kalman update with AC-9 divergence detection."""
        S = H @ self._P @ H.T + R_meas
        try:
            S_inv = np.linalg.inv(S)
        except LinAlgError as exc:
            raise EstimatorFatalError(f"eskf {source} S not invertible: {exc}") from exc

        mahal_sq = float(residual @ S_inv @ residual)
        if mahal_sq > _INNOV_DIVERGENCE_MAHAL_SQ:
            self._log.error(
                "c5.state.eskf_filter_divergence",
                extra={
                    "kind": "c5.state.eskf_filter_divergence",
                    "kv": {
                        "source": source,
                        "mahalanobis_sq": mahal_sq,
                        "threshold_sq": _INNOV_DIVERGENCE_MAHAL_SQ,
                    },
                },
            )
            raise EstimatorFatalError(
                f"eskf filter divergence on {source}: mahalanobis²={mahal_sq:.3f} > "
                f"{_INNOV_DIVERGENCE_MAHAL_SQ:.1f}"
            )
        K = self._P @ H.T @ S_inv

        delta_x = K @ residual
        self._inject_error(delta_x)

        I = np.eye(_N_STATE, dtype=np.float64)  # noqa: E741 — standard KF symbol
        I_minus_KH = I - K @ H
        self._P = I_minus_KH @ self._P @ I_minus_KH.T + K @ R_meas @ K.T
        self._P = 0.5 * (self._P + self._P.T)

    def _inject_error(self, delta_x: np.ndarray) -> None:
        self._nominal_pos = self._nominal_pos + delta_x[_IDX_POS]
        self._nominal_vel = self._nominal_vel + delta_x[_IDX_VEL]
        delta_q = _quat_from_axis_angle(delta_x[_IDX_ROT])
        self._nominal_q = _quat_normalise(_quat_mul(self._nominal_q, delta_q))
        self._nominal_ba = self._nominal_ba + delta_x[_IDX_BA]
        self._nominal_bg = self._nominal_bg + delta_x[_IDX_BG]
        self._nominal_dt = self._nominal_dt + float(delta_x[_IDX_DT])

    def _pose_covariance_6x6(self) -> np.ndarray:
        """Project the 16x16 error covariance onto pose (pos + rot)."""
        pose_idx = np.array([0, 1, 2, 6, 7, 8])
        return np.ascontiguousarray(self._P[np.ix_(pose_idx, pose_idx)], dtype=np.float64)

    def _enu_pose_to_wgs84(self) -> LatLonAlt:
        origin = self._enu_origin if self._enu_origin is not None else _DEFAULT_ENU_ORIGIN
        enu = np.array(
            [
                float(self._nominal_pos[0]),
                float(self._nominal_pos[1]),
                float(self._nominal_pos[2]),
            ],
            dtype=np.float64,
        )
        return WgsConverter.local_enu_to_latlonalt(origin, enu)

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

        WGS84 → ENU via the injected origin (matches the iSAM2
        estimator's helper); ``Quat`` → 3x3 rotation via the local
        ``_quat_to_rot`` helper applied to a freshly-built quaternion.
        """
        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"eskf add_pose_anchor: WGS84→ENU failed for frame {pose.frame_id}: {exc}"
            ) from exc
        q = pose.orientation_world_T_body
        rotation = _quat_to_rot(
            np.array([float(q.w), float(q.x), float(q.y), float(q.z)], dtype=np.float64)
        )
        matrix = np.eye(4, dtype=np.float64)
        matrix[:3, :3] = rotation
        matrix[:3, 3] = enu
        return matrix

    def _compute_last_anchor_age_ms(self, now_ns: int) -> int:
        if self._last_anchor_ns == 0:
            return now_ns // 1_000_000
        return max(0, (now_ns - self._last_anchor_ns) // 1_000_000)

    def _derive_source_label(self) -> PoseSourceLabel:
        machine = self._source_label_machine
        if machine is None:
            return PoseSourceLabel.VISUAL_PROPAGATED
        try:
            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:
        machine = self._source_label_machine
        if machine is None:
            return False
        try:
            return bool(machine.is_spoof_promotion_blocked())
        except Exception:
            return False

    def _notify_source_label_anchor(self, pose: PoseEstimate) -> None:
        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.eskf_source_label_anchor_notify_failed",
                extra={
                    "kind": "c5.state.eskf_source_label_anchor_notify_failed",
                    "kv": {"frame_id": str(pose.frame_id), "error": str(exc)},
                },
            )

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


# ----------------------------------------------------------------------
# Module-level factory + register helpers.


def create(
    *,
    config: Config,
    imu_preintegrator: Any,
    se3_utils: Any,
    wgs_converter: Any,
    fdr_client: Any,
) -> tuple[StateEstimator, Any]:
    """Composition-root factory for the ESKF estimator.

    Returns ``(estimator, handle)`` to match the iSAM2 factory shape;
    the ESKF returns ``handle=None`` because it has no GTSAM graph.
    """
    estimator = EskfStateEstimator(
        config,
        imu_preintegrator=imu_preintegrator,
        se3_utils=se3_utils,
        wgs_converter=wgs_converter,
        fdr_client=fdr_client,
    )
    return estimator, None


def register() -> None:
    """Register :func:`create` under the ``"eskf"`` strategy slug."""
    from gps_denied_onboard.runtime_root.state_factory import register_state_estimator

    register_state_estimator(_STRATEGY, create)


# ----------------------------------------------------------------------
# Module-level math + DTO helpers (kept local; small and self-contained).


def _datetime_to_ns(dt: Any) -> int:
    return int(dt.timestamp() * 1_000_000_000)


def _pose_se3_to_array(pose_se3: Any) -> np.ndarray:
    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 arr


def _measurement_noise(covariance: Any | None) -> np.ndarray:
    if covariance is None:
        return np.eye(6, dtype=np.float64) * (_DEFAULT_MEAS_SIGMA**2)
    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 cov


def _skew(v: np.ndarray) -> np.ndarray:
    return np.array(
        [
            [0.0, -v[2], v[1]],
            [v[2], 0.0, -v[0]],
            [-v[1], v[0], 0.0],
        ],
        dtype=np.float64,
    )


def _quat_to_rot(q: np.ndarray) -> np.ndarray:
    w, x, y, z = float(q[0]), float(q[1]), float(q[2]), float(q[3])
    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 _quat_mul(a: np.ndarray, b: np.ndarray) -> np.ndarray:
    aw, ax, ay, az = a
    bw, bx, by, bz = b
    return np.array(
        [
            aw * bw - ax * bx - ay * by - az * bz,
            aw * bx + ax * bw + ay * bz - az * by,
            aw * by - ax * bz + ay * bw + az * bx,
            aw * bz + ax * by - ay * bx + az * bw,
        ],
        dtype=np.float64,
    )


def _quat_normalise(q: np.ndarray) -> np.ndarray:
    n = float(np.linalg.norm(q))
    if n == 0.0:
        return np.array([1.0, 0.0, 0.0, 0.0], dtype=np.float64)
    return q / n


def _quat_from_axis_angle(axis_angle: np.ndarray) -> np.ndarray:
    """Build a unit quaternion from a 3-vector axis-angle (rad)."""
    theta = float(np.linalg.norm(axis_angle))
    if theta < 1e-12:
        return np.array([1.0, 0.0, 0.0, 0.0], dtype=np.float64)
    half = theta * 0.5
    s = float(np.sin(half)) / theta
    return np.array(
        [
            float(np.cos(half)),
            float(axis_angle[0]) * s,
            float(axis_angle[1]) * s,
            float(axis_angle[2]) * s,
        ],
        dtype=np.float64,
    )


def _rot_to_axis_angle(R: np.ndarray) -> np.ndarray:
    """Inverse of ``_quat_from_axis_angle ∘ _quat_to_rot`` for small rotations."""
    trace = float(np.trace(R))
    cos_theta = max(-1.0, min(1.0, (trace - 1.0) * 0.5))
    theta = float(np.arccos(cos_theta))
    if theta < 1e-12:
        return np.zeros(3, dtype=np.float64)
    sin_theta = float(np.sin(theta))
    if abs(sin_theta) < 1e-12:
        # Pi rotation — pick the axis from the largest diagonal entry.
        diag = np.diag(R)
        i = int(np.argmax(diag))
        axis = np.zeros(3, dtype=np.float64)
        axis[i] = float(np.sqrt((diag[i] + 1.0) * 0.5))
        return axis * theta
    return (
        np.array(
            [
                (R[2, 1] - R[1, 2]) / (2.0 * sin_theta),
                (R[0, 2] - R[2, 0]) / (2.0 * sin_theta),
                (R[1, 0] - R[0, 1]) / (2.0 * sin_theta),
            ],
            dtype=np.float64,
        )
        * theta
    )


def _rot_to_quat(R: np.ndarray) -> np.ndarray:
    """Convert a 3x3 rotation matrix to a unit quaternion (w, x, y, z)."""
    trace = R[0, 0] + R[1, 1] + R[2, 2]
    if trace > 0.0:
        s = 0.5 / float(np.sqrt(trace + 1.0))
        w = 0.25 / s
        x = (R[2, 1] - R[1, 2]) * s
        y = (R[0, 2] - R[2, 0]) * s
        z = (R[1, 0] - R[0, 1]) * s
    else:
        if R[0, 0] > R[1, 1] and R[0, 0] > R[2, 2]:
            s = 2.0 * float(np.sqrt(1.0 + R[0, 0] - R[1, 1] - R[2, 2]))
            w = (R[2, 1] - R[1, 2]) / s
            x = 0.25 * s
            y = (R[0, 1] + R[1, 0]) / s
            z = (R[0, 2] + R[2, 0]) / s
        elif R[1, 1] > R[2, 2]:
            s = 2.0 * float(np.sqrt(1.0 + R[1, 1] - R[0, 0] - R[2, 2]))
            w = (R[0, 2] - R[2, 0]) / s
            x = (R[0, 1] + R[1, 0]) / s
            y = 0.25 * s
            z = (R[1, 2] + R[2, 1]) / s
        else:
            s = 2.0 * float(np.sqrt(1.0 + R[2, 2] - R[0, 0] - R[1, 1]))
            w = (R[1, 0] - R[0, 1]) / s
            x = (R[0, 2] + R[2, 0]) / s
            y = (R[1, 2] + R[2, 1]) / s
            z = 0.25 * s
    return _quat_normalise(np.array([w, x, y, z], dtype=np.float64))


def _quat_to_quat_dto(q: np.ndarray) -> Quat:
    return Quat(w=float(q[0]), x=float(q[1]), y=float(q[2]), z=float(q[3]))


def _enforce_spd(cov: np.ndarray) -> None:
    """Run a Cholesky decomposition; raise :class:`EstimatorFatalError` on failure."""
    try:
        np.linalg.cholesky(cov)
    except LinAlgError as exc:
        raise EstimatorFatalError(f"covariance not SPD: {exc}") from exc


def _validate_takeoff_origin_args(
    origin: LatLonAlt,
    sigma_horiz_m: float,
    sigma_vert_m: float,
) -> None:
    """AZ-490 input validation — same rules for both estimator impls."""
    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 _with_smoothed_false(out: EstimatorOutput) -> EstimatorOutput:
    """Return a copy of ``out`` with ``smoothed=False``.

    The buffer entries are appended with ``smoothed=False`` already
    (forward-time path); this helper is a defensive belt-and-suspenders
    in case a future refactor flips the flag at append time.
    """
    if out.smoothed is False:
        return out
    return EstimatorOutput(
        frame_id=out.frame_id,
        position_wgs84=out.position_wgs84,
        orientation_world_T_body=out.orientation_world_T_body,
        velocity_world_mps=out.velocity_world_mps,
        covariance_6x6=out.covariance_6x6,
        source_label=out.source_label,
        last_satellite_anchor_age_ms=out.last_satellite_anchor_age_ms,
        smoothed=False,
        emitted_at=out.emitted_at,
    )
