"""iNav MSP2 inbound decoder (AZ-391 / E-C8).

MSP2 is request/response over a serial transport (YAMSPy ``MSPy``);
unlike MAVLink it doesn't push telemetry. The decoder runs a poll
loop that calls into a thin ``MspSource`` adapter and translates the
returned dictionaries into the unified :class:`FcTelemetryFrame`
shape (same payload types as the AP path so consumers don't care
which FC the wire originated from).

iNav has no spoofing-detection telemetry (RESTRICT-COMM-2 +
spec AC-5); ``GpsStatus.SPOOFED`` is unreachable on this path.

Tests drive the decoder via :meth:`feed_one_tick` which calls the
``MspSource`` poll methods once — no real serial port required.
"""

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__ = [
    "Msp2InavInboundDecoder",
    "MspSource",
]


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


class MspSource(Protocol):
    """Minimal MSPy-shaped interface the decoder polls.

    Concrete production implementation wraps a ``yamspy.MSPy``
    connection; the test harness uses a deterministic dict-returning
    fake.
    """

    def read_imu(self) -> dict[str, Any]: ...

    def read_attitude(self) -> dict[str, Any]: ...

    def read_gps(self) -> dict[str, Any]: ...

    def read_flight_state(self) -> dict[str, Any]: ...


class Msp2InavInboundDecoder:
    """iNav MSP2 polling decoder + dispatcher."""

    def __init__(
        self,
        source: MspSource,
        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_msp2")
        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._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_tick(self) -> int:
        """Poll source once for IMU/attitude/GPS/state; returns # dispatched frames.

        IMU and attitude are polled every tick; GPS + state can be
        polled at a lower rate by the production driver — we still
        poll them here for AC simplicity.
        """
        dispatched = 0
        for handler in (self._tick_imu, self._tick_attitude, self._tick_gps, self._tick_state):
            try:
                if handler():
                    dispatched += 1
            except Exception as exc:
                self._log_decode_error(handler.__name__, repr(exc))
        return dispatched

    def run_poll_loop(self, *, period_s: float = 0.01) -> None:
        """Continuous polling loop; honours :meth:`stop`.

        Sleeps via the injected :class:`Clock` so replay binaries (which
        wire a ``TlogDerivedClock``) advance instantly while the live
        binary blocks for ``period_s`` between ticks.
        """
        period_ns = int(period_s * 1_000_000_000)
        while not self._stop_flag.is_set():
            self.feed_one_tick()
            self._clock.sleep_until_ns(self._clock.monotonic_ns() + period_ns)

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

    # ------------------------------------------------------------------
    # Per-tick handlers

    def _tick_imu(self) -> bool:
        raw = self._source.read_imu()
        if raw is None:
            return False
        # iNav MSP2 IMU dict: 'accelerometer': [x, y, z], 'gyroscope': [x, y, z]
        # plus a 'time_ms' if the source surfaces it (YAMSPy doesn't include
        # FC-side time, so we use monotonic_ns at decode boundary).
        accel = tuple(float(v) for v in raw["accelerometer"])
        gyro = tuple(float(v) for v in raw["gyroscope"])
        if len(accel) != 3 or len(gyro) != 3:
            raise ValueError(
                f"iNav IMU dict shape: expected 3-vectors, got accel={accel}, gyro={gyro}"
            )
        sensor_ts_ns = self._clock.monotonic_ns()
        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 _tick_attitude(self) -> bool:
        raw = self._source.read_attitude()
        if raw is None:
            return False
        # iNav attitude dict: 'angx' (roll), 'angy' (pitch), 'heading' (yaw) — in degrees
        # The MSP_ATTITUDE message returns: roll/pitch as 10ths of a deg,
        # heading as -180..180 deg. We accept the YAMSPy-decoded dict shape
        # which uses the same keys.
        roll_rad = float(raw["angx"]) * (3.141592653589793 / 180.0)
        pitch_rad = float(raw["angy"]) * (3.141592653589793 / 180.0)
        yaw_rad = float(raw["heading"]) * (3.141592653589793 / 180.0)
        sensor_ts_ns = self._clock.monotonic_ns()
        payload = AttitudeSample(
            ts_ns=sensor_ts_ns,
            roll_rad=roll_rad,
            pitch_rad=pitch_rad,
            yaw_rad=yaw_rad,
        )
        return self._dispatch(TelemetryKind.ATTITUDE, payload, ring=self.attitude_ring)

    def _tick_gps(self) -> bool:
        raw = self._source.read_gps()
        if raw is None:
            return False
        # iNav MSP_RAW_GPS / MSP2_INAV_GPS dict:
        # 'fix' (0/1/2), 'numSat', 'lat' (deg * 1e7), 'lon' (deg * 1e7), 'alt' (m)
        # We translate iNav's coarser fix flag (0=no fix, 1=2D, 2+=3D) to our enum.
        fix = int(raw["fix"])
        if fix == 0:
            status = GpsStatus.NO_FIX
        elif fix == 1:
            status = GpsStatus.DEGRADED
        else:
            status = GpsStatus.STABLE
        captured_at = self._clock.monotonic_ns()
        if fix >= 2:
            lat_deg = float(raw["lat"]) / 1e7
            lon_deg = float(raw["lon"]) / 1e7
            alt_m = float(raw.get("alt", 0.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
        payload = GpsHealth(status=status, fix_age_ms=0, captured_at=captured_at)
        return self._dispatch(TelemetryKind.GPS_HEALTH, payload, ring=self.gps_ring)

    def _tick_state(self) -> bool:
        raw = self._source.read_flight_state()
        if raw is None:
            return False
        # iNav flight-state dict shape (subset we honour):
        # 'armed': bool, 'in_flight': bool, 'failsafe': bool
        captured_at = self._clock.monotonic_ns()
        if raw.get("failsafe", False):
            state = FlightState.FAILED
        elif raw.get("in_flight", False):
            state = FlightState.IN_FLIGHT
        elif raw.get("armed", False):
            state = FlightState.ARMED
        elif raw.get("initialised", False):
            state = FlightState.ON_GROUND
        else:
            state = FlightState.INIT
        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)

    # ------------------------------------------------------------------
    # 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=False,  # iNav has no signing per RESTRICT-COMM-2
        )
        ring.push(frame)
        self._bus.dispatch(frame)
        return True

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