"""AP MAVLink 2.0 inbound decoder (AZ-391 / E-C8).

Reads frames from a pymavlink-style source (``recv_match`` interface),
translates the five contract message types to :class:`FcTelemetryFrame`
records, maintains the per-kind rings, and dispatches to subscribers.

Wire types we decode:

- ``RAW_IMU``      -> ``ImuTelemetrySample`` (kind=IMU_SAMPLE)
- ``ATTITUDE``     -> ``AttitudeSample``     (kind=ATTITUDE)
- ``GPS_RAW_INT``  -> ``GpsHealth``          (kind=GPS_HEALTH)
- ``HEARTBEAT``    -> ``FlightStateSignal``  (kind=MAV_STATE)
- ``STATUSTEXT``   -> spoofing-sentinel sideband (consumed internally)

The decoder is fed via an injected ``MAVLinkSource`` (Protocol with
``recv_match(...) -> message | None``), so tests can drive it
synchronously without a real serial port.
"""

from __future__ import annotations

import threading
from typing import TYPE_CHECKING, Any, Final, Protocol

from gps_denied_onboard._types.fc import (
    AttitudeSample,
    FcTelemetryFrame,
    FlightState,
    FlightStateSignal,
    GpsHealth,
    GpsStatus,
    ImuTelemetrySample,
    TelemetryKind,
)
from gps_denied_onboard._types.geo import LatLonAlt
from gps_denied_onboard.clock.wall_clock import WallClock
from gps_denied_onboard.components.c8_fc_adapter._subscription import SubscriptionBus
from gps_denied_onboard.components.c8_fc_adapter._telemetry_rings import TelemetryRing
from gps_denied_onboard.logging import get_logger

if TYPE_CHECKING:
    from gps_denied_onboard.clock import Clock

__all__ = [
    "AP_MESSAGE_TYPES",
    "MAVLinkSource",
    "PymavlinkInboundDecoder",
]


AP_MESSAGE_TYPES: Final[tuple[str, ...]] = (
    "RAW_IMU",
    "SCALED_IMU2",
    "ATTITUDE",
    "GPS_RAW_INT",
    "GPS2_RAW",
    "HEARTBEAT",
    "STATUSTEXT",
)

# MAVLink GPS_FIX_TYPE enum values (subset we map).
_FIX_TYPE_NO_FIX_OR_NONE: Final[frozenset[int]] = frozenset({0, 1})
_FIX_TYPE_2D: Final[int] = 2

# MAV_STATE values (subset we map).
_MAV_STATE_UNINIT: Final[int] = 0
_MAV_STATE_BOOT: Final[int] = 1
_MAV_STATE_CALIBRATING: Final[int] = 2
_MAV_STATE_STANDBY: Final[int] = 3
_MAV_STATE_ACTIVE: Final[int] = 4
_MAV_STATE_CRITICAL: Final[int] = 5
_MAV_STATE_EMERGENCY: Final[int] = 6
_MAV_STATE_POWEROFF: Final[int] = 7
_MAV_STATE_FLIGHT_TERMINATION: Final[int] = 8

# `base_mode` bit that signals MAV_MODE_FLAG_SAFETY_ARMED.
_MAV_MODE_FLAG_SAFETY_ARMED: Final[int] = 0x80

# Substrings in STATUSTEXT that promote subsequent GPS_RAW_INT decodes to SPOOFED.
_SPOOFING_SENTINELS: Final[tuple[str, ...]] = (
    "GPS spoofing",
    "GPS jamming",
)

_OUT_OF_ORDER_KIND: Final[str] = "c8.inbound.out_of_order_frame_dropped"
_DECODE_ERROR_KIND: Final[str] = "c8.inbound.decode_error"
_SPOOFING_DETECTED_KIND: Final[str] = "c8.inbound.spoofing_sentinel_seen"


class MAVLinkSource(Protocol):
    """Minimal pymavlink-mavutil interface the decoder consumes."""

    def recv_match(
        self,
        *,
        type: str | list[str] | None = None,
        blocking: bool = False,
        timeout: float | None = None,
    ) -> Any | None: ...


class PymavlinkInboundDecoder:
    """Per-AP-adapter MAVLink 2.0 inbound decoder + dispatcher."""

    def __init__(
        self,
        source: MAVLinkSource,
        bus: SubscriptionBus,
        *,
        imu_ring_capacity: int = 200,
        attitude_ring_capacity: int = 100,
        gps_ring_capacity: int = 20,
        state_ring_capacity: int = 10,
        clock: Clock | None = None,
    ) -> None:
        self._source = source
        self._bus = bus
        self._clock: Clock = clock if clock is not None else WallClock()
        self._log = get_logger("c8_fc_adapter.inbound_mavlink")
        self.imu_ring: TelemetryRing[FcTelemetryFrame] = TelemetryRing(
            imu_ring_capacity, kind_name="imu"
        )
        self.attitude_ring: TelemetryRing[FcTelemetryFrame] = TelemetryRing(
            attitude_ring_capacity, kind_name="attitude"
        )
        self.gps_ring: TelemetryRing[FcTelemetryFrame] = TelemetryRing(
            gps_ring_capacity, kind_name="gps_health"
        )
        self.state_ring: TelemetryRing[FcTelemetryFrame] = TelemetryRing(
            state_ring_capacity, kind_name="flight_state"
        )
        self._last_ts_ns: dict[TelemetryKind, int] = {}
        self._spoof_sentinel_seen_at: int | None = None
        # AC-5.1 warm-start hint: cached on first GPS_RAW_INT with 3D+ fix.
        self._warm_start_hint: LatLonAlt | None = None
        self._warm_start_hint_at: int | None = None
        self._stop_flag = threading.Event()
        self._lock = threading.Lock()

    @property
    def warm_start_hint(self) -> LatLonAlt | None:
        with self._lock:
            return self._warm_start_hint

    def feed_one_message(self, msg: Any) -> bool:
        """Decode one MAVLink message synchronously (test-friendly entrypoint).

        Returns ``True`` if a frame was dispatched, ``False`` on drop /
        unknown type / decode error. Production use prefers
        :meth:`run_decode_loop` which pulls from ``source.recv_match``.
        """
        if msg is None:
            return False
        try:
            msg_type = msg.get_type() if hasattr(msg, "get_type") else type(msg).__name__
        except Exception as exc:
            self._log_decode_error("get_type_failed", repr(exc))
            return False
        try:
            if msg_type in ("RAW_IMU", "SCALED_IMU2"):
                return self._handle_imu(msg)
            if msg_type == "ATTITUDE":
                return self._handle_attitude(msg)
            if msg_type in ("GPS_RAW_INT", "GPS2_RAW"):
                return self._handle_gps(msg)
            if msg_type == "HEARTBEAT":
                return self._handle_heartbeat(msg)
            if msg_type == "STATUSTEXT":
                self._handle_statustext(msg)
                return False
        except Exception as exc:
            self._log_decode_error(msg_type, repr(exc))
            return False
        return False

    def run_decode_loop(self, *, poll_timeout_s: float = 0.5) -> None:
        """Continuous decode loop; honours :meth:`stop`."""
        while not self._stop_flag.is_set():
            try:
                msg = self._source.recv_match(
                    type=list(AP_MESSAGE_TYPES),
                    blocking=True,
                    timeout=poll_timeout_s,
                )
            except Exception as exc:
                self._log_decode_error("recv_match_failed", repr(exc))
                continue
            self.feed_one_message(msg)

    def stop(self) -> None:
        self._stop_flag.set()

    # ------------------------------------------------------------------
    # Per-type handlers

    def _handle_imu(self, msg: Any) -> bool:
        # pymavlink RAW_IMU exposes raw signed-16 ticks; we forward as-is.
        accel = (float(msg.xacc), float(msg.yacc), float(msg.zacc))
        gyro = (float(msg.xgyro), float(msg.ygyro), float(msg.zgyro))
        # `time_usec` is FC-side; we override with monotonic_ns at decode boundary
        # for the FdrTelemetryFrame.received_at; the payload's ts_ns is the
        # FC sensor stamp (epoch us -> ns).
        sensor_ts_ns = int(msg.time_usec) * 1000
        payload = ImuTelemetrySample(
            ts_ns=sensor_ts_ns,
            accel_xyz=accel,
            gyro_xyz=gyro,
        )
        return self._dispatch(TelemetryKind.IMU_SAMPLE, payload, ring=self.imu_ring)

    def _handle_attitude(self, msg: Any) -> bool:
        sensor_ts_ns = int(msg.time_boot_ms) * 1_000_000
        payload = AttitudeSample(
            ts_ns=sensor_ts_ns,
            roll_rad=float(msg.roll),
            pitch_rad=float(msg.pitch),
            yaw_rad=float(msg.yaw),
        )
        return self._dispatch(TelemetryKind.ATTITUDE, payload, ring=self.attitude_ring)

    def _handle_gps(self, msg: Any) -> bool:
        fix_type = int(msg.fix_type)
        status = self._map_fix_type(fix_type)
        if status is GpsStatus.STABLE:
            status = self._maybe_promote_to_spoofed_or_non_spoofed()
        captured_at = self._clock.monotonic_ns()
        payload = GpsHealth(status=status, fix_age_ms=0, captured_at=captured_at)
        # AC-5.1: cache warm-start hint on first 3D+ fix.
        if fix_type >= 3:
            lat_deg = int(msg.lat) / 1e7
            lon_deg = int(msg.lon) / 1e7
            alt_m = int(msg.alt) / 1000.0
            with self._lock:
                if self._warm_start_hint is None:
                    self._warm_start_hint = LatLonAlt(lat_deg, lon_deg, alt_m)
                    self._warm_start_hint_at = captured_at
        return self._dispatch(TelemetryKind.GPS_HEALTH, payload, ring=self.gps_ring)

    def _handle_heartbeat(self, msg: Any) -> bool:
        captured_at = self._clock.monotonic_ns()
        state = self._map_mav_state(
            system_status=int(msg.system_status),
            base_mode=int(msg.base_mode),
        )
        with self._lock:
            hint = self._warm_start_hint
            hint_at = self._warm_start_hint_at
        last_age_ms: int | None = None
        if hint_at is not None:
            last_age_ms = max(0, (captured_at - hint_at) // 1_000_000)
        payload = FlightStateSignal(
            state=state,
            last_valid_gps_hint_wgs84=hint,
            last_valid_gps_age_ms=last_age_ms,
            captured_at=captured_at,
        )
        return self._dispatch(TelemetryKind.MAV_STATE, payload, ring=self.state_ring)

    def _handle_statustext(self, msg: Any) -> None:
        text = msg.text
        if isinstance(text, bytes):
            text = text.decode("utf-8", errors="replace")
        if not any(sentinel.lower() in text.lower() for sentinel in _SPOOFING_SENTINELS):
            return
        captured_at = self._clock.monotonic_ns()
        with self._lock:
            self._spoof_sentinel_seen_at = captured_at
        self._log.warning(
            f"c8.inbound.spoofing_sentinel_seen: {text!r}",
            extra={
                "kind": _SPOOFING_DETECTED_KIND,
                "kv": {"text": text, "captured_at": captured_at},
            },
        )

    # ------------------------------------------------------------------
    # Dispatch helpers

    def _dispatch(
        self,
        kind: TelemetryKind,
        payload: Any,
        *,
        ring: TelemetryRing[FcTelemetryFrame],
    ) -> bool:
        received_at = self._clock.monotonic_ns()
        last = self._last_ts_ns.get(kind)
        if last is not None and received_at <= last:
            self._log.warning(
                f"c8.inbound.out_of_order_frame_dropped: kind={kind.name} "
                f"prev_ns={last} this_ns={received_at}",
                extra={
                    "kind": _OUT_OF_ORDER_KIND,
                    "kv": {
                        "telemetry_kind": kind.name,
                        "prev_ns": last,
                        "this_ns": received_at,
                    },
                },
            )
            return False
        self._last_ts_ns[kind] = received_at
        frame = FcTelemetryFrame(
            kind=kind,
            payload=payload,
            received_at=received_at,
            signed=True,  # AP path uses MAVLink 2.0 signing per AZ-395 (assumed true)
        )
        ring.push(frame)
        self._bus.dispatch(frame)
        return True

    def _log_decode_error(self, msg_type: str, error_repr: str) -> None:
        self._log.debug(
            f"c8.inbound.decode_error: msg_type={msg_type} {error_repr}",
            extra={
                "kind": _DECODE_ERROR_KIND,
                "kv": {"msg_type": msg_type, "error": error_repr},
            },
        )

    @staticmethod
    def _map_fix_type(fix_type: int) -> GpsStatus:
        if fix_type in _FIX_TYPE_NO_FIX_OR_NONE:
            return GpsStatus.NO_FIX
        if fix_type == _FIX_TYPE_2D:
            return GpsStatus.DEGRADED
        return GpsStatus.STABLE

    def _maybe_promote_to_spoofed_or_non_spoofed(self) -> GpsStatus:
        # AC-3: AP has no in-band signed-fix flag; we use STATUSTEXT
        # spoofing sentinels seen within the last 5 s as a SPOOFED signal.
        with self._lock:
            sentinel_at = self._spoof_sentinel_seen_at
        if sentinel_at is None:
            return GpsStatus.STABLE
        now = self._clock.monotonic_ns()
        if (now - sentinel_at) <= 5 * 1_000_000_000:
            return GpsStatus.SPOOFED
        return GpsStatus.STABLE

    @staticmethod
    def _map_mav_state(*, system_status: int, base_mode: int) -> FlightState:
        if system_status in (_MAV_STATE_UNINIT, _MAV_STATE_BOOT, _MAV_STATE_CALIBRATING):
            return FlightState.INIT
        if system_status in (
            _MAV_STATE_CRITICAL,
            _MAV_STATE_EMERGENCY,
            _MAV_STATE_FLIGHT_TERMINATION,
        ):
            return FlightState.FAILED
        if system_status == _MAV_STATE_ACTIVE:
            return FlightState.IN_FLIGHT
        if system_status == _MAV_STATE_STANDBY:
            if base_mode & _MAV_MODE_FLAG_SAFETY_ARMED:
                return FlightState.ARMED
            return FlightState.ON_GROUND
        if system_status == _MAV_STATE_POWEROFF:
            return FlightState.ON_GROUND
        return FlightState.INIT
