"""iNav MSP2 + secondary-MAVLink adapter (AZ-394 / E-C8).

Outbound: encodes ``EstimatorOutput`` to MSP2_SENSOR_GPS and writes
through a YAMSPy-style transport (``send_RAW_msg(code, data)``).
Side-channel: STATUSTEXT on the SECONDARY MAVLink telemetry link
(NOT on the primary MSP2 link) — iNav supports MAVLink for telemetry
output but not for primary positioning per RESTRICT-COMM-2. The
secondary link is ALWAYS unsigned (Invariant 9).

Build flag: ``BUILD_FC_INAV``.
"""

from __future__ import annotations

import threading
from collections.abc import Callable
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._msp2_sensor_gps_encoder import (
    MSP2_SENSOR_GPS_CODE,
    encode_msp2_sensor_gps,
)
from gps_denied_onboard.components.c8_fc_adapter._outbound_provenance import (
    StatusTextTransitionRateLimiter,
)
from gps_denied_onboard.components.c8_fc_adapter._subscription import SubscriptionBus
from gps_denied_onboard.components.c8_fc_adapter.errors import (
    FcAdapterConfigError,
    FcEmitError,
    FcOpenError,
    SourceSetSwitchNotSupportedError,
)
from gps_denied_onboard.config import Config
from gps_denied_onboard.fdr_client.client import FdrClient
from gps_denied_onboard.logging import get_logger

__all__ = ["Msp2InavAdapter"]


_FIX_TYPE_3D: Final[int] = 3
_INAV_DEFAULT_SATS: Final[int] = 10


def _mav_severity(sev: Severity) -> int:
    return int(sev.value)


class Msp2InavAdapter:
    """iNav FcAdapter (MSP2 primary + unsigned MAVLink secondary)."""

    def __init__(
        self,
        *,
        config: Config,
        wgs_converter: Any,
        covariance_projector: CovarianceProjector,
        fdr_client: FdrClient,
        clock: Clock | None = None,
        msp_connect_factory: Callable[[str, int], Any] | None = None,
        secondary_mavlink_factory: Callable[[], 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._msp_connect_factory = msp_connect_factory
        self._secondary_mavlink_factory = secondary_mavlink_factory
        self._log = get_logger("c8_fc_adapter.inav_adapter")
        # Wire state ------------------------------------------------------
        self._msp: Any = None
        self._secondary_mav: Any = None
        self._opened = False
        self._sequence_number = 0
        self._first_emit_logged = False
        self._open_emit_thread_ident: int | None = None
        # Inbound bus stub — AZ-394 doesn't wire the inbound (iNav
        # polling decoder lands in AZ-391; the per-adapter inbound
        # composition happens in a follow-up batch).
        self._bus = SubscriptionBus()
        # Provenance rate-limiter for the secondary MAVLink STATUSTEXT;
        # the limiter expects a float-seconds clock, so we wrap the
        # injected Clock's ns reading.
        self._provenance = StatusTextTransitionRateLimiter(
            send_statustext=self._send_statustext_secondary,
            clock=lambda: self._clock.monotonic_ns() / 1_000_000_000,
        )

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

    def open(self, port: PortConfig, signing_key: bytes | None) -> None:
        if self._opened:
            raise FcOpenError("Msp2InavAdapter already opened")
        if port.fc_kind is not FcKind.INAV:
            raise FcOpenError(f"Msp2InavAdapter requires FcKind.INAV; got {port.fc_kind!r}")
        if signing_key is not None:
            # Invariant 2 / RESTRICT-COMM-2 — iNav has no signing.
            raise FcAdapterConfigError("iNav does not support MAVLink signing per RESTRICT-COMM-2")
        try:
            self._msp = self._connect_msp(port)
        except Exception as exc:
            raise FcOpenError(f"MSP2 connect failed: {exc!r}") from exc
        try:
            self._secondary_mav = (
                self._secondary_mavlink_factory()
                if self._secondary_mavlink_factory is not None
                else None
            )
        except Exception as exc:
            self._log.warning(
                f"c8.inav.secondary_mavlink_open_failed: {exc!r}",
                extra={
                    "kind": "c8.inav.secondary_mavlink_open_failed",
                    "kv": {"error": repr(exc)},
                },
            )
            self._secondary_mav = None
        self._opened = True

    def close(self) -> None:
        if not self._opened:
            return
        try:
            for conn in (self._msp, self._secondary_mav):
                if conn is not None and hasattr(conn, "close"):
                    try:
                        conn.close()
                    except Exception:
                        # close() on a half-broken serial/socket handle is
                        # itself best-effort; the wrapping finally still
                        # nulls the references so subsequent reopens see a
                        # clean slate.
                        pass
        finally:
            self._opened = False
            self._msp = None
            self._secondary_mav = 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._msp 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)")
        h_pos_accuracy_mm = self._cov_projector.to_inav_h_pos_accuracy_mm(output)
        wgs = self._extract_wgs84(output)
        emitted_at = self._clock.monotonic_ns()
        self._sequence_number = (self._sequence_number + 1) & 0xFF
        seq = self._sequence_number
        payload = encode_msp2_sensor_gps(
            fix_type=_FIX_TYPE_3D,
            satellites_visible=_INAV_DEFAULT_SATS,
            h_pos_accuracy_mm=h_pos_accuracy_mm,
            longitude_e7=int(wgs.lon_deg * 1e7),
            latitude_e7=int(wgs.lat_deg * 1e7),
            msl_altitude_cm=int(wgs.alt_m * 100.0),
        )
        try:
            self._msp.send_RAW_msg(MSP2_SENSOR_GPS_CODE, payload)
        except Exception as exc:
            self._log_emit_failed(repr(exc), output.frame_id)
            raise FcEmitError(f"iNav MSP2 outbound wire emit failed: {exc!r}") from exc
        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.inav.first_emit",
                extra={
                    "kind": "c8.inav.first_emit",
                    "kv": {"frame_id": str(output.frame_id), "seq": seq},
                },
            )
        self._log.debug(
            "c8.inav.emit",
            extra={
                "kind": "c8.inav.emit",
                "kv": {
                    "frame_seq": seq,
                    "h_pos_accuracy_mm": h_pos_accuracy_mm,
                    "source_label": output.source_label,
                },
            },
        )
        return EmittedExternalPosition(
            fc_kind=FcKind.INAV,
            horiz_accuracy_m=h_pos_accuracy_mm / 1000.0,
            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:
            raise FcEmitError("adapter not opened")
        self._enforce_single_writer()
        self._send_statustext_secondary(msg, severity)

    def request_source_set_switch(self) -> None:
        raise SourceSetSwitchNotSupportedError("iNav: no MAV_CMD_SET_EKF_SOURCE_SET equivalent")

    def current_flight_state(self) -> FlightStateSignal:
        # Inbound iNav state ring is not wired into this adapter shell;
        # the inbound decoder (AZ-391) exposes its own state_ring that
        # the composition root can read directly. Default to INIT here.
        return FlightStateSignal(
            state=FlightState.INIT,
            last_valid_gps_hint_wgs84=None,
            last_valid_gps_age_ms=None,
            captured_at=self._clock.monotonic_ns(),
        )

    # ------------------------------------------------------------------
    # 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(
                "Msp2InavAdapter outbound is single-writer; "
                f"first thread={self._open_emit_thread_ident}, this thread={cur}"
            )

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

        return MSPy(device=port.device, baudrate=port.baud)

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

    def _extract_wgs84(self, output: EstimatorOutput) -> LatLonAlt:
        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 _log_emit_failed(self, reason: str, frame_id: Any) -> None:
        self._log.error(
            f"c8.inav.emit_failed: {reason}",
            extra={
                "kind": "c8.inav.emit_failed",
                "kv": {"reason": reason, "frame_id": str(frame_id)},
            },
        )
