"""ArduPilot MAVLink 2.0 adapter (AZ-393 outbound, AZ-395 signing, E-C8).

Composes :class:`PymavlinkInboundDecoder` (AZ-391) for the inbound
producer side and the local outbound encoder for `emit_external_position` +
`emit_status_text`. Implements `FcAdapter` per the contract.

Production wiring (composition-root, AZ-393 / AZ-395 follow-up):
- Constructor receives the WgsConverter (AZ-279), CovarianceProjector
  (AZ-392), FdrClient (AZ-273), and a clock callable.
- `open(port, signing_key)` opens the pymavlink connection on the
  configured port and, if signing is enabled in config, completes
  the per-flight signing handshake (AZ-395).
- A single composition-root thread is bound for outbound emit via
  :func:`bind_outbound_emit_thread`; emitting from a second thread
  raises :class:`RuntimeError`.

Build flag: ``BUILD_FC_ARDUPILOT_PLANE``.
"""

from __future__ import annotations

import os
import secrets
import threading
from collections.abc import Callable
from datetime import datetime, timezone
from typing import TYPE_CHECKING, Any, Final

from gps_denied_onboard._types.emitted import EmittedExternalPosition
from gps_denied_onboard._types.fc import (
    FcKind,
    FlightState,
    FlightStateSignal,
    PortConfig,
    Severity,
    Subscription,
    TelemetryCallback,
)
from gps_denied_onboard._types.geo import LatLonAlt
from gps_denied_onboard._types.state import EstimatorOutput
from gps_denied_onboard.clock.wall_clock import WallClock
from gps_denied_onboard.components.c8_fc_adapter._covariance_projector import (
    CovarianceProjector,
)

if TYPE_CHECKING:
    from gps_denied_onboard.clock import Clock
from gps_denied_onboard.components.c8_fc_adapter._inbound_mavlink import (
    PymavlinkInboundDecoder,
)
from gps_denied_onboard.components.c8_fc_adapter._outbound_provenance import (
    StatusTextTransitionRateLimiter,
    source_label_to_float,
)
from gps_denied_onboard.components.c8_fc_adapter._subscription import SubscriptionBus
from gps_denied_onboard.components.c8_fc_adapter.errors import (
    FcEmitError,
    FcOpenError,
    SigningHandshakeError,
    SourceSetSwitchError,
)
from gps_denied_onboard.config import Config
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__ = ["PymavlinkArdupilotAdapter"]


_GPS_FIX_TYPE_3D: Final[int] = 3
_NAMED_VALUE_FLOAT_NAME: Final[str] = "src_lbl"
_SIGNING_KEY_LEN: Final[int] = 32
_BUILD_DEV_STATIC_KEY_ENV: Final[str] = "BUILD_DEV_STATIC_KEY"
# ArduPilot-specific command (dialect: ardupilotmega).
_MAV_CMD_SET_EKF_SOURCE_SET: Final[int] = 42007
_MAV_RESULT_ACCEPTED: Final[int] = 0
_SWITCH_RATE_LIMIT_S: Final[float] = 1.0


# Maps Severity enum to MAVLink statustext severity numeric value.
def _mav_severity(sev: Severity) -> int:
    return int(sev.value)


class PymavlinkArdupilotAdapter:
    """ArduPilot Plane FcAdapter (MAVLink 2.0).

    Threading: outbound (``emit_*``) is bound to a single composition-
    root thread; inbound (decode loop) runs on its own thread inside
    ``PymavlinkInboundDecoder.run_decode_loop`` (AZ-391).
    """

    def __init__(
        self,
        *,
        config: Config,
        wgs_converter: Any,
        covariance_projector: CovarianceProjector,
        fdr_client: FdrClient,
        clock: Clock | None = None,
        flight_id: str = "",
        connect_factory: Callable[[str, int], Any] | None = None,
    ) -> None:
        self._config = config
        self._wgs_converter = wgs_converter
        self._cov_projector = covariance_projector
        self._fdr_client = fdr_client
        self._clock: Clock = clock if clock is not None else WallClock()
        self._flight_id = flight_id
        self._connect_factory = connect_factory
        self._signing_failure_threshold = max(1, int(config.fc.signing_failure_threshold))
        self._log = get_logger("c8_fc_adapter.ap_adapter")
        # Wire state ------------------------------------------------------
        self._connection: Any = None
        self._signing_key: bytearray | None = None
        self._opened = False
        self._sequence_number = 0
        self._first_emit_logged = False
        self._open_emit_thread_ident: int | None = None
        self._signing_failure_logged_at_count = 0
        # D-C8-2 source-set switch state (AZ-396).
        self._last_switch_attempt_ns: int = 0
        self._last_switch_succeeded: bool = False
        # Inbound bus + decoder (lazily constructed inside ``open``).
        self._bus = SubscriptionBus()
        self._inbound: PymavlinkInboundDecoder | None = None
        self._inbound_thread: threading.Thread | None = None
        # Outbound provenance rate limiter; wraps the injected Clock as a
        # float-seconds callable (the limiter's existing API contract).
        self._provenance = StatusTextTransitionRateLimiter(
            send_statustext=self._send_statustext_internal,
            clock=self._monotonic_s,
        )

    # ------------------------------------------------------------------
    # FcAdapter Protocol implementation

    def open(self, port: PortConfig, signing_key: bytes | None) -> None:
        if self._opened:
            raise FcOpenError("PymavlinkArdupilotAdapter already opened")
        if port.fc_kind is not FcKind.ARDUPILOT_PLANE:
            raise FcOpenError(
                f"PymavlinkArdupilotAdapter requires FcKind.ARDUPILOT_PLANE; got {port.fc_kind!r}"
            )
        try:
            self._connection = self._connect(port)
        except Exception as exc:
            raise FcOpenError(f"pymavlink connect failed: {exc!r}") from exc
        # Per-flight signing key handling (AZ-395). ----------------------
        # In production, `signing_key_source = "ephemeral_per_flight"`; we
        # generate a fresh 32-byte key. The caller (composition root) is
        # also free to pass an explicit signing_key for test paths.
        source = self._config.fc.signing_key_source
        if source == "none":
            pass
        elif source == "ephemeral_per_flight":
            self._signing_key = self._materialise_signing_key(signing_key)
            try:
                self._setup_signing(self._signing_key)
            except Exception as exc:
                self._handle_signing_handshake_failure(exc)
                raise SigningHandshakeError(f"AP signing handshake failed: {exc!r}") from exc
            self._fdr_signing_event(
                kind="c8.ap.signing_key_rotated",
                kv={"flight_id": self._flight_id, "key_age_s": 0},
            )
        elif source == "dev_static":
            if os.environ.get(_BUILD_DEV_STATIC_KEY_ENV) != "ON":
                raise FcOpenError(
                    "signing_key_source='dev_static' requires "
                    f"{_BUILD_DEV_STATIC_KEY_ENV}=ON at build time"
                )
            self._signing_key = self._materialise_dev_static_key()
            try:
                self._setup_signing(self._signing_key)
            except Exception as exc:
                self._handle_signing_handshake_failure(exc)
                raise SigningHandshakeError(f"AP signing handshake failed: {exc!r}") from exc
            self._log.warning(
                "c8.ap.signing_dev_static_key",
                extra={"kind": "c8.ap.signing_dev_static_key", "kv": {}},
            )
        else:
            raise FcOpenError(f"unknown signing_key_source={source!r}")
        # Inbound decoder + thread.
        self._inbound = PymavlinkInboundDecoder(self._connection, self._bus)
        thread = threading.Thread(
            target=self._inbound.run_decode_loop,
            name="c8.ap.inbound",
            daemon=True,
        )
        self._inbound_thread = thread
        thread.start()
        self._open_emit_thread_ident = None
        self._opened = True

    def close(self) -> None:
        if not self._opened:
            return
        if self._inbound is not None:
            self._inbound.stop()
        # Zeroise the signing-key buffer BEFORE we let it be deallocated
        # (AZ-395 AC-7 / Invariant 10).
        if self._signing_key is not None:
            self._zeroise_signing_key(self._signing_key)
            self._log.info(
                "c8.ap.signing_key_zeroised",
                extra={"kind": "c8.ap.signing_key_zeroised", "kv": {}},
            )
            self._signing_key = None
        try:
            if self._connection is not None and hasattr(self._connection, "close"):
                self._connection.close()
        finally:
            self._opened = False
            self._connection = None
            self._inbound = None
            self._open_emit_thread_ident = None
            self._first_emit_logged = False

    def subscribe_telemetry(self, callback: TelemetryCallback) -> Subscription:
        return self._bus.subscribe(callback)

    def emit_external_position(self, output: EstimatorOutput) -> EmittedExternalPosition:
        if not self._opened or self._connection is None:
            raise FcEmitError("adapter not opened")
        self._enforce_single_writer()
        if output.smoothed:
            self._log_emit_failed("smoothed_rejected", output.frame_id)
            raise FcEmitError("smoothed output cannot be emitted to FC (Invariant 6)")
        horiz_accuracy_m = self._cov_projector.to_ardupilot_horiz_accuracy_m(output)
        wgs = self._extract_wgs84(output)
        emitted_at = self._clock.monotonic_ns()
        self._sequence_number += 1
        seq = self._sequence_number
        try:
            self._connection.mav.gps_input_send(
                int(self._clock_us()),
                0,  # gps_id (primary)
                0,  # ignore_flags
                0,  # time_week_ms
                0,  # time_week
                _GPS_FIX_TYPE_3D,
                int(wgs.lat_deg * 1e7),
                int(wgs.lon_deg * 1e7),
                float(wgs.alt_m),
                0.0,  # hdop
                0.0,  # vdop
                0.0,  # vn
                0.0,  # ve
                0.0,  # vd
                0.0,  # speed_accuracy
                float(horiz_accuracy_m),
                0.0,  # vert_accuracy
                10,  # satellites_visible (synthetic; cosmetic for AP EKF)
                0,  # yaw
            )
            self._connection.mav.named_value_float_send(
                int(self._clock_ms_boot()),
                _NAMED_VALUE_FLOAT_NAME.encode("utf-8"),
                source_label_to_float(output.source_label),
            )
        except Exception as exc:
            self._log_emit_failed(repr(exc), output.frame_id)
            raise FcEmitError(f"AP outbound wire emit failed: {exc!r}") from exc
        # AC-NEW-4 / AZ-395 AC-6: poll signing-failure counter post-emit;
        # only log on the transition past the configured threshold.
        self._poll_signing_failure_counter()
        # Source-label transition STATUSTEXT (AC-4).
        self._provenance.note_label_and_maybe_emit(output.source_label, severity=Severity.INFO)
        if not self._first_emit_logged:
            self._first_emit_logged = True
            self._log.info(
                "c8.ap.first_emit",
                extra={
                    "kind": "c8.ap.first_emit",
                    "kv": {"frame_id": str(output.frame_id), "seq": seq},
                },
            )
        self._log.debug(
            "c8.ap.emit",
            extra={
                "kind": "c8.ap.emit",
                "kv": {
                    "frame_seq": seq,
                    "horiz_accuracy_m": horiz_accuracy_m,
                    "source_label": output.source_label,
                },
            },
        )
        return EmittedExternalPosition(
            fc_kind=FcKind.ARDUPILOT_PLANE,
            horiz_accuracy_m=horiz_accuracy_m,
            source_label=output.source_label,
            emitted_at=emitted_at,
            sequence_number=seq,
        )

    def emit_status_text(self, msg: str, severity: Severity) -> None:
        if not self._opened or self._connection is None:
            raise FcEmitError("adapter not opened")
        self._enforce_single_writer()
        self._send_statustext_internal(msg, severity)

    def request_source_set_switch(self) -> None:
        """D-C8-2 source-set switch (AZ-396 / AC-NEW-2).

        Sends ``MAV_CMD_SET_EKF_SOURCE_SET`` (ardupilotmega cmd 42007)
        with ``param1 = config.fc.spoof_recovery_source_set`` and waits
        up to ``config.fc.source_set_switch_timeout_ms`` for the FC's
        ``COMMAND_ACK``. Idempotence per Invariant 11: a second call
        within ``_SWITCH_RATE_LIMIT_S`` of the previous attempt is
        no-op'd; a call after a successful switch logs INFO + STATUSTEXT
        but does NOT re-issue.
        """
        if not self._opened or self._connection is None:
            raise FcEmitError("adapter not opened")
        self._enforce_single_writer()
        now_ns = self._clock.monotonic_ns()
        if self._last_switch_attempt_ns:
            elapsed_s = (now_ns - self._last_switch_attempt_ns) / 1_000_000_000
            if elapsed_s < _SWITCH_RATE_LIMIT_S:
                self._log.info(
                    "c8.ap.source_set_switch_rate_limited",
                    extra={
                        "kind": "c8.ap.source_set_switch_rate_limited",
                        "kv": {"elapsed_s": round(elapsed_s, 3)},
                    },
                )
                return
            if self._last_switch_succeeded:
                self._log.info(
                    "c8.ap.source_set_switch_already_active",
                    extra={"kind": "c8.ap.source_set_switch_already_active", "kv": {}},
                )
                self._send_statustext_internal("src-set already active", Severity.INFO)
                return
        self._last_switch_attempt_ns = now_ns
        source_set = int(self._config.fc.spoof_recovery_source_set)
        timeout_ms = int(self._config.fc.source_set_switch_timeout_ms)
        try:
            self._connection.mav.command_long_send(
                getattr(self._connection, "target_system", 1),
                getattr(self._connection, "target_component", 1),
                _MAV_CMD_SET_EKF_SOURCE_SET,
                0,
                float(source_set),
                0.0,
                0.0,
                0.0,
                0.0,
                0.0,
                0.0,
            )
        except Exception as exc:
            self._handle_source_set_switch_failure(
                reason=f"command_long_send failed: {exc!r}", source_set=source_set
            )
            raise SourceSetSwitchError(f"MAV_CMD_SET_EKF_SOURCE_SET send failed: {exc!r}") from exc
        ack = self._wait_for_command_ack(_MAV_CMD_SET_EKF_SOURCE_SET, timeout_ms=timeout_ms)
        if ack is None:
            self._handle_source_set_switch_failure(
                reason=f"ACK timeout after {timeout_ms}ms", source_set=source_set
            )
            raise SourceSetSwitchError(f"ACK timeout after {timeout_ms}ms")
        result = int(getattr(ack, "result", -1))
        if result != _MAV_RESULT_ACCEPTED:
            self._handle_source_set_switch_failure(
                reason=f"ACK result={result}", source_set=source_set
            )
            raise SourceSetSwitchError(f"MAV_CMD_SET_EKF_SOURCE_SET rejected: result={result}")
        self._last_switch_succeeded = True
        self._log.info(
            "c8.ap.source_set_switch_executed",
            extra={
                "kind": "c8.ap.source_set_switch_executed",
                "kv": {"source_set": source_set},
            },
        )
        self._fdr_signing_event(
            kind="c8.ap.source_set_switch_executed",
            kv={"source_set": source_set, "flight_id": self._flight_id},
        )
        self._send_statustext_internal(f"src-set switched to {source_set}", Severity.INFO)

    def current_flight_state(self) -> FlightStateSignal:
        if self._inbound is None:
            raise FcEmitError("current_flight_state requires open(); no inbound decoder yet")
        latest = self._inbound.state_ring.peek_latest()
        if latest is None:
            return FlightStateSignal(
                state=FlightState.INIT,
                last_valid_gps_hint_wgs84=None,
                last_valid_gps_age_ms=None,
                captured_at=self._clock.monotonic_ns(),
            )
        payload = latest.payload
        assert isinstance(payload, FlightStateSignal)
        return payload

    # ------------------------------------------------------------------
    # Internals

    def _enforce_single_writer(self) -> None:
        cur = threading.get_ident()
        if self._open_emit_thread_ident is None:
            self._open_emit_thread_ident = cur
            return
        if self._open_emit_thread_ident != cur:
            raise RuntimeError(
                "PymavlinkArdupilotAdapter outbound is single-writer; "
                f"first thread={self._open_emit_thread_ident}, this thread={cur}"
            )

    def _connect(self, port: PortConfig) -> Any:
        if self._connect_factory is not None:
            return self._connect_factory(port.device, port.baud)
        # Lazy import so the test path with an injected factory does NOT
        # require pymavlink at module-import time.
        from pymavlink import mavutil

        return mavutil.mavlink_connection(
            port.device,
            baud=port.baud,
            dialect="common",
            mavlink_version="2.0",
        )

    def _setup_signing(self, key: bytearray) -> None:
        if not hasattr(self._connection, "setup_signing"):
            return
        # pymavlink's setup_signing accepts bytes; pass the buffer (it
        # converts to bytes internally so the buffer here stays the
        # authoritative copy we zeroise on close).
        self._connection.setup_signing(bytes(key))

    def _materialise_signing_key(self, supplied: bytes | None) -> bytearray:
        if supplied is None:
            return bytearray(secrets.token_bytes(_SIGNING_KEY_LEN))
        if len(supplied) != _SIGNING_KEY_LEN:
            raise SigningHandshakeError(
                f"signing_key must be {_SIGNING_KEY_LEN} bytes; got {len(supplied)}"
            )
        return bytearray(supplied)

    def _materialise_dev_static_key(self) -> bytearray:
        """Hex-decode FcConfig.dev_static_signing_key into a 32-byte buffer."""
        hex_str = self._config.fc.dev_static_signing_key.strip()
        try:
            raw = bytes.fromhex(hex_str)
        except ValueError as exc:
            raise SigningHandshakeError(
                "FcConfig.dev_static_signing_key must be valid hex"
            ) from exc
        if len(raw) != _SIGNING_KEY_LEN:
            raise SigningHandshakeError(
                f"FcConfig.dev_static_signing_key decodes to {len(raw)} bytes; "
                f"expected {_SIGNING_KEY_LEN}"
            )
        return bytearray(raw)

    @staticmethod
    def _zeroise_signing_key(buf: bytearray) -> None:
        for i in range(len(buf)):
            buf[i] = 0

    def _handle_signing_handshake_failure(self, exc: BaseException) -> None:
        self._log.error(
            f"c8.ap.signing_handshake_failed: {exc!r}",
            extra={
                "kind": "c8.ap.signing_handshake_failed",
                "kv": {"error_class": type(exc).__name__},
            },
        )
        self._fdr_signing_event(
            kind="c8.ap.signing_handshake_failed",
            kv={
                "flight_id": self._flight_id,
                "error_class": type(exc).__name__,
            },
        )

    def _poll_signing_failure_counter(self) -> None:
        if self._signing_key is None:
            return
        count = getattr(self._connection.mav, "signing", None)
        if count is None:
            return
        failure_count = int(getattr(count, "sig_count", 0) or 0)
        if failure_count <= self._signing_failure_logged_at_count:
            return
        if failure_count < self._signing_failure_threshold:
            return
        # Transition past the threshold — log + FDR + STATUSTEXT, no raise.
        self._signing_failure_logged_at_count = failure_count
        self._log.error(
            f"c8.ap.signing_failure: count={failure_count}",
            extra={
                "kind": "c8.ap.signing_failure",
                "kv": {"failure_count": failure_count},
            },
        )
        self._fdr_signing_event(
            kind="c8.ap.signing_failure",
            kv={
                "flight_id": self._flight_id,
                "failure_count": failure_count,
            },
        )
        try:
            self._send_statustext_internal(
                f"signing failure count={failure_count}", Severity.WARNING
            )
        except Exception:
            # STATUSTEXT failure on the same UART is non-fatal; the
            # ERROR log + FDR already captured the event.
            pass

    def _send_statustext_internal(self, msg: str, severity: Severity) -> None:
        if self._connection is None:
            return
        try:
            text = msg.encode("utf-8")[:50]
            self._connection.mav.statustext_send(_mav_severity(severity), text)
        except Exception as exc:
            self._log.debug(
                f"c8.ap.statustext_failed: {exc!r}",
                extra={
                    "kind": "c8.ap.statustext_failed",
                    "kv": {"error": repr(exc)},
                },
            )

    def _log_emit_failed(self, reason: str, frame_id: Any) -> None:
        self._log.error(
            f"c8.ap.emit_failed: {reason}",
            extra={
                "kind": "c8.ap.emit_failed",
                "kv": {"reason": reason, "frame_id": str(frame_id)},
            },
        )

    def _wait_for_command_ack(self, command_id: int, *, timeout_ms: int) -> Any | None:
        """Wait up to ``timeout_ms`` for a `COMMAND_ACK` for ``command_id``.

        Returns the ACK message on match, or ``None`` on timeout. Other
        COMMAND_ACK messages (for unrelated commands) are ignored.
        """
        deadline = self._monotonic_s() + (timeout_ms / 1000.0)
        while True:
            remaining = deadline - self._monotonic_s()
            if remaining <= 0:
                return None
            try:
                msg = self._connection.recv_match(
                    type="COMMAND_ACK", blocking=True, timeout=remaining
                )
            except Exception as exc:
                self._log.debug(
                    f"c8.ap.recv_match_failed: {exc!r}",
                    extra={
                        "kind": "c8.ap.recv_match_failed",
                        "kv": {"error": repr(exc)},
                    },
                )
                return None
            if msg is None:
                return None
            if int(getattr(msg, "command", -1)) == command_id:
                return msg

    def _handle_source_set_switch_failure(self, *, reason: str, source_set: int) -> None:
        self._last_switch_succeeded = False
        self._log.error(
            f"c8.ap.source_set_switch_failed: {reason}",
            extra={
                "kind": "c8.ap.source_set_switch_failed",
                "kv": {"reason": reason, "source_set": source_set},
            },
        )
        self._fdr_signing_event(
            kind="c8.ap.source_set_switch_failed",
            kv={
                "reason": reason,
                "source_set": source_set,
                "flight_id": self._flight_id,
            },
        )
        try:
            self._send_statustext_internal(f"src-set switch failed: {reason}", Severity.ERROR)
        except Exception:
            # STATUSTEXT is a best-effort operator-facing notification; the
            # canonical failure surface is the structured log + FDR event
            # already emitted above. A serial-write failure here must not
            # mask the primary failure path.
            pass

    def _extract_wgs84(self, output: EstimatorOutput) -> LatLonAlt:
        """Pull the WGS84 fix that C5 produced.

        Per the C5 contract v1.0.0, the estimator emits
        ``EstimatorOutput.position_wgs84`` directly (the
        composition root no longer injects an enricher; the
        conversion happens inside C5's ``current_estimate`` path
        using the shared :class:`WgsConverter`). A missing field
        is a composition bug — fail loudly rather than guess.
        """
        wgs = output.position_wgs84
        if not isinstance(wgs, LatLonAlt):
            raise FcEmitError(
                f"EstimatorOutput.position_wgs84 must be a LatLonAlt; got {type(wgs).__name__}"
            )
        return wgs

    def _monotonic_s(self) -> float:
        return self._clock.monotonic_ns() / 1_000_000_000

    def _clock_us(self) -> int:
        return self._clock.monotonic_ns() // 1_000

    def _clock_ms_boot(self) -> int:
        return self._clock.monotonic_ns() // 1_000_000

    def _fdr_signing_event(self, *, kind: str, kv: dict[str, Any]) -> None:
        record = FdrRecord(
            schema_version=1,
            ts=datetime.now(tz=timezone.utc).isoformat(),
            producer_id="c8_fc_adapter",
            kind="log",
            payload={
                "level": "INFO" if kind.endswith("_rotated") else "ERROR",
                "component": "c8_fc_adapter",
                "kind": kind,
                "msg": kind,
                "kv": kv,
            },
        )
        try:
            self._fdr_client.enqueue(record)
        except Exception as exc:
            self._log.debug(
                f"c8.ap.fdr_enqueue_failed: {exc!r}",
                extra={
                    "kind": "c8.ap.fdr_enqueue_failed",
                    "kv": {"error": repr(exc), "downstream_kind": kind},
                },
            )
