"""Concrete ``ISam2GraphHandle`` — owned by AZ-382 / E-C5.

C4 (``OpenCVGtsamPoseEstimator``) calls ``add_factor`` / ``update`` /
``compute_marginals`` against this handle, NOT against C5 directly —
ADR-003 says C5 owns the graph; this handle is the typed seam C4 uses
to drive it without importing C5 internals.

AZ-381 shipped the Protocol surface and a NotImplementedError
skeleton; AZ-382 replaces the four method bodies with the real GTSAM
calls against the estimator's ``_isam2`` + ``_smoother`` instances.
The Protocol surface does not change between AZ-381 and AZ-382.

Defensive logging (R05 mitigation): every mutation logs SUCCESS or
FAILURE with structured fields. Silent factor-add failures bit this
codebase during the prototype — the contract now mandates the
defensive trace.
"""

from __future__ import annotations

from typing import TYPE_CHECKING, Any, Protocol, runtime_checkable

import gtsam
import gtsam_unstable

from gps_denied_onboard.components.c5_state.errors import (
    EstimatorDegradedError,
    EstimatorFatalError,
)
from gps_denied_onboard.logging import get_logger

if TYPE_CHECKING:
    from gps_denied_onboard.components.c5_state.gtsam_isam2_estimator import (
        GtsamIsam2StateEstimator,
    )

__all__ = ["ISam2GraphHandle", "ISam2GraphHandleImpl"]


@runtime_checkable
class ISam2GraphHandle(Protocol):
    """C4 ↔ C5 seam over the shared iSAM2 graph (ADR-003).

    Owned by C5; held by reference inside ``OpenCVGtsamPoseEstimator``.
    The handle is passed during composition (``state_factory``
    returns the tuple; ``pose_factory`` accepts it as a positional
    argument) and never crosses thread boundaries — see Invariant 1
    of both the C4 and C5 contracts.
    """

    def add_factor(self, factor: Any) -> None: ...

    def update(self, graph: Any, values: Any, timestamps: Any | None = None) -> None: ...

    def compute_marginals(self) -> Any: ...

    def last_anchor_age_ms(self) -> int: ...


class ISam2GraphHandleImpl(ISam2GraphHandle):
    """Real iSAM2 graph handle — drives the estimator's ``_isam2`` + ``_smoother``.

    Every mutation is wrapped in success/failure logging per R05;
    individual failures are translated into the C5 error hierarchy
    (``EstimatorDegradedError`` for recoverable graph-add issues,
    ``EstimatorFatalError`` for solver failures the calling thread
    cannot recover from).

    This impl also satisfies C4's narrower consumer-side
    ``c4_pose._isam2_handle.ISam2GraphHandle`` Protocol (which
    requires only ``get_pose_key(frame_id) -> int``) so the same
    instance can be passed from ``state_factory`` to
    ``pose_factory.build_pose_estimator`` without an adapter.
    """

    def __init__(self, estimator: GtsamIsam2StateEstimator) -> None:
        self._estimator = estimator
        self._log = get_logger("c5_state.isam2_handle")

    def get_pose_key(self, frame_id: int) -> int:
        """Map a C4 ``frame_id`` to the GTSAM pose key.

        Satisfies the C4 ``ISam2GraphHandle`` Protocol stub
        (AZ-355) by delegating to the estimator's
        ``key_for_frame`` — the canonical C5-side name for the
        same ``'x'`` namespace lookup.
        """
        return self._estimator.key_for_frame(frame_id)

    def add_factor(self, factor: Any) -> None:
        """Append ``factor`` to the pending ``NonlinearFactorGraph``.

        The estimator's ``_graph`` is a staging buffer that AZ-383
        flushes into ``_isam2`` + ``_smoother`` on every keyframe.
        Per the C5 contract this method is the only sanctioned entry
        point for C4 → C5 factor adds — direct mutation of
        ``estimator._graph`` from outside this handle violates the
        seam.
        """
        try:
            self._estimator._graph.add(factor)
        except Exception as exc:
            self._log.error(
                "c5.state.add_factor_failed",
                extra={
                    "kind": "c5.state.add_factor_failed",
                    "kv": {
                        "factor_type": type(factor).__name__,
                        "error": str(exc),
                    },
                },
            )
            raise EstimatorDegradedError(
                f"add_factor failed for {type(factor).__name__}: {exc}"
            ) from exc
        self._log.debug(
            "c5.state.add_factor_ok",
            extra={
                "kind": "c5.state.add_factor_ok",
                "kv": {
                    "factor_type": type(factor).__name__,
                    "graph_size": self._estimator._graph.size(),
                },
            },
        )

    def update(self, graph: Any, values: Any, timestamps: Any | None = None) -> None:
        """Apply ``(graph, values)`` to iSAM2 AND advance the smoother.

        The smoother requires a ``FixedLagSmootherKeyTimestampMap``;
        when the caller passes ``None`` we substitute an empty map so
        the call doesn't trip on the GTSAM C++ signature. AZ-383 will
        populate the map with per-key arrival timestamps so the
        sliding window can evict aged keyframes.

        ``EstimatorFatalError`` is raised on either iSAM2 or smoother
        failure — both indicate solver state the calling thread
        cannot recover from in-flight; the runtime root's AC-5.2
        fallback (AZ-388) catches it and drops C5 outputs entirely.
        """
        timestamps_map = (
            timestamps
            if timestamps is not None
            else gtsam_unstable.FixedLagSmootherKeyTimestampMap()
        )
        try:
            self._estimator._isam2.update(graph, values)
        except Exception as exc:
            self._log.error(
                "c5.state.isam2_update_failed",
                extra={
                    "kind": "c5.state.isam2_update_failed",
                    "kv": {
                        "graph_size": getattr(graph, "size", lambda: -1)(),
                        "error": str(exc),
                    },
                },
            )
            raise EstimatorFatalError(f"iSAM2.update failed: {exc}") from exc

        try:
            self._estimator._smoother.update(graph, values, timestamps_map)
        except Exception as exc:
            self._log.error(
                "c5.state.smoother_update_failed",
                extra={
                    "kind": "c5.state.smoother_update_failed",
                    "kv": {
                        "graph_size": getattr(graph, "size", lambda: -1)(),
                        "error": str(exc),
                    },
                },
            )
            raise EstimatorFatalError(f"IncrementalFixedLagSmoother.update failed: {exc}") from exc

        self._log.debug(
            "c5.state.update_ok",
            extra={
                "kind": "c5.state.update_ok",
                "kv": {
                    "graph_size": getattr(graph, "size", lambda: -1)(),
                    "values_size": getattr(values, "size", lambda: -1)(),
                },
            },
        )

    def compute_marginals(self) -> Any:
        """Return a ``gtsam.Marginals`` snapshot of the current iSAM2 state.

        Built on demand from ``getFactorsUnsafe()`` + ``calculateEstimate()``
        rather than cached — AZ-384 will decide on the cache layer
        once the access pattern from C4 + the C13 smoothed-history
        path is real.
        """
        return gtsam.Marginals(
            self._estimator._isam2.getFactorsUnsafe(),
            self._estimator._isam2.calculateEstimate(),
        )

    def last_anchor_age_ms(self) -> int:
        """Milliseconds since the last satellite-anchored pose was added.

        Returns a very large number until AZ-383 records the first
        anchor (``_last_anchor_ns`` is initialised to 0 in the
        estimator constructor). This matches the C5 contract's
        documented "no anchor yet" sentinel.

        Reads the estimator's injected :class:`Clock` so replay /
        unit-test runs see deterministic age values.
        """
        return (
            self._estimator._clock.monotonic_ns() - self._estimator._last_anchor_ns
        ) // 1_000_000
