"""MSP2_SENSOR_GPS wire-format encoder/decoder (AZ-394 / E-C8).

iNav-specific message ``MSP2_SENSOR_GPS = 0x1F03`` — host-to-FC
external GPS injection. YAMSPy's MSP code table is BetaFlight-derived
and does NOT include this iNav-only code, so we encode the payload by
hand and ship it through the transport's ``send_RAW_msg(code, data)``
entry point.

Wire payload (little-endian, packed; iNav source of truth:
``inav/src/main/io/msp.c`` — search ``MSP2_SENSOR_GPS``):

| Offset | Field                 | Type   | Notes                              |
|--------|-----------------------|--------|------------------------------------|
| 0      | instance              | u8     | 0 (single external GPS instance)   |
| 1      | gps_week              | u16    | 0 (unused for external injection)  |
| 3      | ms_tow                | u32    | 0 (unused)                         |
| 7      | fix_type              | u8     | 3 (3D fix); contract Invariant 5   |
| 8      | satellites_visible    | u8     | cosmetic; we send 10                |
| 9      | h_pos_accuracy_mm     | u16    | from CovarianceProjector (clamped) |
| 11     | v_pos_accuracy_mm     | u16    | 0 (we don't project vertical)      |
| 13     | h_vel_accuracy_mm_s   | u16    | 0                                  |
| 15     | hdop                  | u16    | 0                                  |
| 17     | longitude_e7          | i32    | wgs84 lon * 1e7                    |
| 21     | latitude_e7           | i32    | wgs84 lat * 1e7                    |
| 25     | msl_altitude_cm       | i32    | wgs84 alt * 100                    |
| 29     | ned_vel_north_cm_s    | i32    | 0                                  |
| 33     | ned_vel_east_cm_s     | i32    | 0                                  |
| 37     | ned_vel_down_cm_s     | i32    | 0                                  |
| 41     | ground_course_cdeg    | u16    | 0                                  |
| 43     | true_yaw_cdeg         | u16    | 0                                  |
| 45     | year                  | u16    | 0                                  |
| 47     | month                 | u8     | 0                                  |
| 48     | day                   | u8     | 0                                  |
| 49     | hour                  | u8     | 0                                  |
| 50     | min                   | u8     | 0                                  |
| 51     | sec                   | u8     | 0                                  |
| 52     | (end)                 |        | total length = 52 bytes            |
"""

from __future__ import annotations

import struct
from dataclasses import dataclass
from typing import Final

__all__ = [
    "MSP2_SENSOR_GPS_CODE",
    "MSP2_SENSOR_GPS_PAYLOAD_LEN",
    "Msp2SensorGpsPayload",
    "decode_msp2_sensor_gps",
    "encode_msp2_sensor_gps",
]


MSP2_SENSOR_GPS_CODE: Final[int] = 0x1F03
MSP2_SENSOR_GPS_PAYLOAD_LEN: Final[int] = 52

# little-endian, no padding; matches inav's __packed__ struct layout.
_PACK_FMT: Final[str] = "<BHIBBHHHHiiiiiiHHHBBBBB"


@dataclass(frozen=True, slots=True)
class Msp2SensorGpsPayload:
    """Decoded MSP2_SENSOR_GPS payload (subset we exercise)."""

    instance: int
    fix_type: int
    satellites_visible: int
    h_pos_accuracy_mm: int
    longitude_e7: int
    latitude_e7: int
    msl_altitude_cm: int


def encode_msp2_sensor_gps(
    *,
    fix_type: int,
    satellites_visible: int,
    h_pos_accuracy_mm: int,
    longitude_e7: int,
    latitude_e7: int,
    msl_altitude_cm: int,
    ned_vel_north_cm_s: int = 0,
    ned_vel_east_cm_s: int = 0,
    ned_vel_down_cm_s: int = 0,
) -> bytes:
    """Encode an MSP2_SENSOR_GPS payload (52 bytes)."""
    return struct.pack(
        _PACK_FMT,
        0,  # instance
        0,  # gps_week
        0,  # ms_tow
        int(fix_type),
        int(satellites_visible),
        int(h_pos_accuracy_mm) & 0xFFFF,
        0,  # v_pos_accuracy_mm
        0,  # h_vel_accuracy_mm_s
        0,  # hdop
        int(longitude_e7),
        int(latitude_e7),
        int(msl_altitude_cm),
        int(ned_vel_north_cm_s),
        int(ned_vel_east_cm_s),
        int(ned_vel_down_cm_s),
        0,  # ground_course_cdeg
        0,  # true_yaw_cdeg
        0,  # year
        0,
        0,
        0,
        0,
        0,  # month/day/hour/min/sec
    )


def decode_msp2_sensor_gps(buf: bytes) -> Msp2SensorGpsPayload:
    """Decode the subset of MSP2_SENSOR_GPS we exercise."""
    if len(buf) != MSP2_SENSOR_GPS_PAYLOAD_LEN:
        raise ValueError(
            f"MSP2_SENSOR_GPS expects {MSP2_SENSOR_GPS_PAYLOAD_LEN} bytes; got {len(buf)}"
        )
    unpacked = struct.unpack(_PACK_FMT, buf)
    (
        instance,
        _gps_week,
        _ms_tow,
        fix_type,
        satellites_visible,
        h_pos_accuracy_mm,
        _v_pos_acc,
        _h_vel_acc,
        _hdop,
        longitude_e7,
        latitude_e7,
        msl_altitude_cm,
        _vn,
        _ve,
        _vd,
        _gc,
        _yaw,
        _year,
        _mo,
        _d,
        _h,
        _mn,
        _s,
    ) = unpacked
    return Msp2SensorGpsPayload(
        instance=instance,
        fix_type=fix_type,
        satellites_visible=satellites_visible,
        h_pos_accuracy_mm=h_pos_accuracy_mm,
        longitude_e7=longitude_e7,
        latitude_e7=latitude_e7,
        msl_altitude_cm=msl_altitude_cm,
    )
