"""`CovarianceProjector` — honest 6x6 -> 2x2 -> equivalent_radius (AZ-392 / E-C8).

The single source of truth for converting a 6x6 GTSAM ``Marginals``
covariance from C5's ``EstimatorOutput`` into the per-FC scalar
horizontal-accuracy field. Lives inside C8 (helper-only; not in the
public API per ``module-layout.md``).

Steps per Invariant 4 + AC-4.3:

1. ``cov_6x6 -> cov_3x3`` — top-left 3x3 block (position sub-matrix
   in the SE(3) parameterisation [x, y, z, rx, ry, rz]).
2. ``cov_3x3 -> cov_2x2`` — top-left 2x2 (horizontal) sub-matrix.
3. ``equivalent_radius = sqrt(largest_eigenvalue(cov_2x2))`` with the
   closed-form solution ``sqrt(0.5 * (sigma_xx + sigma_yy + sqrt(
   (sigma_xx - sigma_yy)**2 + 4 * sigma_xy**2)))`` (bit-stable; no
   numpy ``eigvalsh`` round-off drift).

Non-SPD / NaN inputs raise :class:`FcEmitError` BEFORE any per-FC
unit conversion runs (AC-6 / AC-7) and emit a single FDR record
``kind="c8.cov_projector.spd_violation"`` carrying the offending
``frame_id`` so C13 post-mortem tooling can correlate emit drops
with the upstream C5 frame.
"""

from __future__ import annotations

import math
from datetime import datetime, timezone
from typing import Any, Final

import numpy as np

from gps_denied_onboard._types.state import EstimatorOutput
from gps_denied_onboard.components.c8_fc_adapter.errors import FcEmitError
from gps_denied_onboard.fdr_client.client import FdrClient
from gps_denied_onboard.fdr_client.records import FdrRecord
from gps_denied_onboard.logging import get_logger

__all__ = ["CovarianceProjector"]


_SPD_VIOLATION_KIND: Final[str] = "c8.cov_projector.spd_violation"
_INAV_CLAMPED_KIND: Final[str] = "c8.cov_projector.inav_clamped"
# iNav MSP2_SENSOR_GPS.hPosAccuracy is a uint16 in millimetres.
_INAV_HPOS_MAX_MM: Final[int] = 65535


def _is_spd_2x2(m: np.ndarray) -> bool:
    """Strict 2x2 SPD check: symmetric, positive determinant, positive trace."""
    if not np.allclose(m, m.T, atol=1e-9):
        return False
    a, b = float(m[0, 0]), float(m[0, 1])
    d = float(m[1, 1])
    det = a * d - b * b
    trace = a + d
    return det > 0.0 and trace > 0.0


def _largest_eigenvalue_2x2(m: np.ndarray) -> float:
    """Closed-form largest eigenvalue of a 2x2 symmetric SPD matrix.

    The 2x2 SPD case is dominated by the closed-form
    ``0.5 * (a + d + sqrt((a - d)^2 + 4 * b^2))``; numpy's
    ``eigvalsh`` has the same answer up to floating-point round-off
    but allocates + dispatches to LAPACK. We prefer the closed-form
    for bit-stability on per-emit calls (AC-9 cross-FC equality
    relies on identical intermediate arithmetic).
    """
    a = float(m[0, 0])
    b = float(m[0, 1])
    d = float(m[1, 1])
    return 0.5 * (a + d + math.sqrt((a - d) * (a - d) + 4.0 * b * b))


class CovarianceProjector:
    """Honest 6x6 -> 2x2 -> equivalent_radius projector for C8 outbound emit."""

    def __init__(self, fdr_client: FdrClient) -> None:
        self._fdr_client = fdr_client
        self._log = get_logger("c8_fc_adapter.cov_projector")

    def to_ardupilot_horiz_accuracy_m(self, output: EstimatorOutput) -> float:
        """Project a 6x6 covariance to meters for AP ``GPS_INPUT.horiz_accuracy``."""
        radius_m = self._equivalent_radius_m(output)
        return radius_m

    def to_inav_h_pos_accuracy_mm(self, output: EstimatorOutput) -> int:
        """Project a 6x6 covariance to millimeters for iNav ``MSP2_SENSOR_GPS.hPosAccuracy``.

        Clamps at ``_INAV_HPOS_MAX_MM`` (uint16); emits a single WARN
        log per clamp event (AC-5).
        """
        radius_m = self._equivalent_radius_m(output)
        # Round half-up to int; built-in round() uses banker's rounding,
        # so add 0.5 + math.floor for the AC-4 spec.
        radius_mm = math.floor(radius_m * 1000.0 + 0.5)
        if radius_mm > _INAV_HPOS_MAX_MM:
            self._log.warning(
                f"c8.cov_projector.inav_clamped: {radius_mm} -> {_INAV_HPOS_MAX_MM}",
                extra={
                    "kind": _INAV_CLAMPED_KIND,
                    "kv": {
                        "radius_mm_raw": radius_mm,
                        "clamped_to": _INAV_HPOS_MAX_MM,
                        "frame_id": str(output.frame_id),
                    },
                },
            )
            return _INAV_HPOS_MAX_MM
        return radius_mm

    def _equivalent_radius_m(self, output: EstimatorOutput) -> float:
        """Shared 6x6 -> 3x3 -> 2x2 -> sqrt(lambda_max) reduction."""
        cov_6x6 = output.covariance_6x6
        if cov_6x6 is None:
            self._fdr_log_violation(reason="missing", frame_id=output.frame_id)
            raise FcEmitError("missing covariance from C5; refusing emit")
        cov_arr = np.asarray(cov_6x6, dtype=np.float64)
        if cov_arr.shape != (6, 6):
            self._fdr_log_violation(
                reason="bad_shape",
                frame_id=output.frame_id,
                extra={"shape": list(cov_arr.shape)},
            )
            raise FcEmitError(f"covariance_6x6 must be 6x6; got shape={cov_arr.shape}")
        if not np.all(np.isfinite(cov_arr)):
            self._fdr_log_violation(reason="nan_or_inf", frame_id=output.frame_id)
            raise FcEmitError("NaN covariance from C5; refusing emit")
        cov_3x3 = cov_arr[:3, :3]
        cov_2x2 = cov_3x3[:2, :2]
        if not _is_spd_2x2(cov_2x2):
            self._fdr_log_violation(
                reason="non_spd",
                frame_id=output.frame_id,
                extra={
                    "cov_2x2": cov_2x2.tolist(),
                },
            )
            raise FcEmitError("non-SPD covariance from C5; refusing emit")
        lam = _largest_eigenvalue_2x2(cov_2x2)
        if lam <= 0.0 or not math.isfinite(lam):
            self._fdr_log_violation(
                reason="degenerate_eigenvalue",
                frame_id=output.frame_id,
                extra={"lambda": lam},
            )
            raise FcEmitError("degenerate covariance eigenvalue; refusing emit")
        return math.sqrt(lam)

    def _fdr_log_violation(
        self,
        *,
        reason: str,
        frame_id: Any,
        extra: dict | None = None,
    ) -> None:
        payload: dict = {"reason": reason, "frame_id": str(frame_id)}
        if extra:
            payload.update(extra)
        # The FDR schema closes ``kind`` to the documented set; we
        # use the ``log`` kind which carries an arbitrary ``kv`` per the
        # AZ-272 contract so this projector's WARN can survive
        # roundtrip without a schema bump.
        record = FdrRecord(
            schema_version=1,
            ts=datetime.now(tz=timezone.utc).isoformat(),
            producer_id="c8_fc_adapter",
            kind="log",
            payload={
                "level": "ERROR",
                "component": "c8_fc_adapter",
                "frame_id": frame_id,
                "kind": _SPD_VIOLATION_KIND,
                "msg": "covariance projector rejected emit",
                "kv": payload,
            },
        )
        try:
            self._fdr_client.enqueue(record)
        except Exception as exc:
            # FDR enqueue failure must not mask the SPD failure path.
            self._log.error(
                "cov_projector.fdr_enqueue_failed",
                extra={
                    "kind": "c8.cov_projector.fdr_enqueue_failed",
                    "kv": {"error": repr(exc), "reason": reason},
                },
            )
