"""dronecan_parser.py — Pure-Python conversion of DroneCAN/UAVCAN v0 message
objects to plain Python dicts, independent of ROS2.

Keeps the dronecan library dependency isolated so that unit tests can run
without the dronecan package or CAN hardware.

DroneCAN message types handled
-------------------------------
  uavcan.equipment.gnss.Fix2               DTID 1063
  uavcan.equipment.gnss.Auxiliary          DTID 1060
  uavcan.equipment.ahrs.RawIMU             DTID 1003
  uavcan.equipment.ahrs.MagneticFieldStrength2  DTID 1001
  uavcan.equipment.air_data.StaticPressure DTID 1028
  uavcan.equipment.air_data.StaticTemperature DTID 1029
  uavcan.protocol.NodeStatus               DTID 341
  uavcan.equipment.gnss.RTCMStream         DTID 1062  (TX: RTCM injection)

Fix2 status values
------------------
  0  NO_FIX
  1  TIME_ONLY
  2  2D_FIX
  3  3D_FIX

NodeStatus health / mode
-------------------------
  health  0=OK 1=WARNING 2=ERROR 3=CRITICAL
  mode    0=OPERATIONAL 1=INITIALIZATION 2=MAINTENANCE 3=SOFTWARE_UPDATE 7=OFFLINE
"""

from __future__ import annotations

from typing import Any, Dict, List, Optional

# ── DSDL constants ────────────────────────────────────────────────────────────

DTID_FIX2              = 1063
DTID_AUXILIARY         = 1060
DTID_RAW_IMU           = 1003
DTID_MAG_FIELD         = 1001
DTID_STATIC_PRESSURE   = 1028
DTID_STATIC_TEMPERATURE = 1029
DTID_NODE_STATUS       = 341
DTID_RTCM_STREAM       = 1062

# Fix2 status
FIX_NO_FIX    = 0
FIX_TIME_ONLY = 1
FIX_2D        = 2
FIX_3D        = 3

FIX_LABEL = {FIX_NO_FIX: "NO_FIX", FIX_TIME_ONLY: "TIME_ONLY",
             FIX_2D: "2D_FIX", FIX_3D: "3D_FIX"}

# NodeStatus health
HEALTH_OK       = 0
HEALTH_WARNING  = 1
HEALTH_ERROR    = 2
HEALTH_CRITICAL = 3

HEALTH_LABEL = {0: "OK", 1: "WARNING", 2: "ERROR", 3: "CRITICAL"}
MODE_LABEL   = {0: "OPERATIONAL", 1: "INITIALIZATION", 2: "MAINTENANCE",
                3: "SOFTWARE_UPDATE", 7: "OFFLINE"}

# RTCM stream: max bytes per DroneCAN frame
RTCM_MAX_CHUNK = 128


# ── Helpers ───────────────────────────────────────────────────────────────────

def _getf(obj: Any, *attrs: str, default: Any = None) -> Any:
    """Get nested attribute with fallback, tolerant of dronecan proxy objects."""
    for attr in attrs:
        try:
            obj = getattr(obj, attr)
        except AttributeError:
            return default
    return obj


def _list3(vec: Any, default: float = 0.0) -> List[float]:
    """Convert a dronecan 3-element array to a plain list of floats."""
    try:
        return [float(vec[0]), float(vec[1]), float(vec[2])]
    except (TypeError, IndexError):
        return [default, default, default]


def _list9(mat: Any, default: float = 0.0) -> List[float]:
    """Convert a dronecan 9-element array (row-major 3×3) to a plain list."""
    try:
        return [float(mat[i]) for i in range(9)]
    except (TypeError, IndexError):
        return [default] * 9


# ── Fix2 ─────────────────────────────────────────────────────────────────────

def parse_fix2(msg: Any) -> Optional[Dict[str, Any]]:
    """Parse a uavcan.equipment.gnss.Fix2 message.

    Parameters
    ----------
    msg : dronecan message object (event.message / event.transfer.payload)

    Returns
    -------
    dict with keys:
        lat_deg        float   degrees (WGS84)
        lon_deg        float   degrees (WGS84)
        alt_msl_m      float   metres above MSL
        alt_ellipsoid_m float  metres above WGS84 ellipsoid
        ned_vel        list[3] [north, east, down] m/s
        fix_type       int     0..3
        fix_label      str
        sats_used      int
        pdop           float
        hdop           float
        vdop           float
        pos_cov        list[9] row-major 3×3 position covariance, m²
        undulation_m   float   WGS84 geoid undulation (m)
        gnss_timestamp_us int  microseconds (GPS epoch)
    """
    if msg is None:
        return None
    try:
        lat = float(_getf(msg, "latitude_deg_1e8",  default=0)) * 1e-8
        lon = float(_getf(msg, "longitude_deg_1e8", default=0)) * 1e-8
        alt_msl = float(_getf(msg, "height_msl_mm",       default=0)) * 1e-3
        alt_ell = float(_getf(msg, "height_ellipsoid_mm",  default=0)) * 1e-3

        ned = _list3(_getf(msg, "ned_velocity"))

        fix_type = int(_getf(msg, "status", default=FIX_NO_FIX))

        # sats_used is uint7, may be named differently across DSDL revisions
        sats = int(_getf(msg, "sats_used", default=0))

        pdop = float(_getf(msg, "pdop", default=99.0))
        hdop = float(_getf(msg, "hdop", default=99.0))
        vdop = float(_getf(msg, "vdop", default=99.0))
        undulation = float(_getf(msg, "undulation", default=0.0))

        # position_covariance: optional, may be absent in older DSDL
        pos_cov_raw = _getf(msg, "position_covariance")
        pos_cov = _list9(pos_cov_raw) if pos_cov_raw is not None else [0.0] * 9

        gnss_ts = int(_getf(msg, "gnss_timestamp", default=0))

        return {
            "lat_deg":          lat,
            "lon_deg":          lon,
            "alt_msl_m":        alt_msl,
            "alt_ellipsoid_m":  alt_ell,
            "ned_vel":          ned,
            "fix_type":         fix_type,
            "fix_label":        FIX_LABEL.get(fix_type, f"UNKNOWN({fix_type})"),
            "sats_used":        sats,
            "pdop":             pdop,
            "hdop":             hdop,
            "vdop":             vdop,
            "pos_cov":          pos_cov,
            "undulation_m":     undulation,
            "gnss_timestamp_us": gnss_ts,
        }
    except Exception:
        return None


# ── RawIMU ────────────────────────────────────────────────────────────────────

def parse_raw_imu(msg: Any) -> Optional[Dict[str, Any]]:
    """Parse a uavcan.equipment.ahrs.RawIMU message.

    Returns
    -------
    dict with keys:
        gyro_xyz     list[3]  rad/s
        accel_xyz    list[3]  m/s²
        dt_s         float    integration interval (seconds)
        gyro_cov     list[9]  row-major 3×3 covariance
        accel_cov    list[9]
    """
    if msg is None:
        return None
    try:
        gyro  = _list3(_getf(msg, "rate_gyro_latest"))
        accel = _list3(_getf(msg, "accelerometer_latest"))
        dt    = float(_getf(msg, "integration_interval", default=0.0))

        gyro_cov  = _list9(_getf(msg, "rate_gyro_covariance"))
        accel_cov = _list9(_getf(msg, "accelerometer_covariance"))

        return {
            "gyro_xyz":  gyro,
            "accel_xyz": accel,
            "dt_s":      dt,
            "gyro_cov":  gyro_cov,
            "accel_cov": accel_cov,
        }
    except Exception:
        return None


# ── MagneticFieldStrength2 ────────────────────────────────────────────────────

def parse_mag(msg: Any) -> Optional[Dict[str, Any]]:
    """Parse a uavcan.equipment.ahrs.MagneticFieldStrength2 message.

    Returns
    -------
    dict with keys:
        sensor_id     int
        field_ga      list[3]  [x, y, z] Gauss
        field_cov     list[9]  row-major 3×3 covariance (Gauss²)
    """
    if msg is None:
        return None
    try:
        sensor_id = int(_getf(msg, "sensor_id", default=0))
        field_ga  = _list3(_getf(msg, "magnetic_field_ga"))
        field_cov = _list9(_getf(msg, "magnetic_field_covariance"))
        return {
            "sensor_id": sensor_id,
            "field_ga":  field_ga,
            "field_cov": field_cov,
        }
    except Exception:
        return None


# ── StaticPressure / StaticTemperature ───────────────────────────────────────

def parse_static_pressure(msg: Any) -> Optional[Dict[str, Any]]:
    """Parse a uavcan.equipment.air_data.StaticPressure message.

    Returns
    -------
    dict with keys:
        pressure_pa       float  Pascals
        pressure_variance float  Pa²
    """
    if msg is None:
        return None
    try:
        return {
            "pressure_pa":       float(_getf(msg, "static_pressure",          default=0.0)),
            "pressure_variance": float(_getf(msg, "static_pressure_variance", default=0.0)),
        }
    except Exception:
        return None


def parse_static_temperature(msg: Any) -> Optional[Dict[str, Any]]:
    """Parse a uavcan.equipment.air_data.StaticTemperature message.

    Returns
    -------
    dict with keys:
        temperature_k float  Kelvin
    """
    if msg is None:
        return None
    try:
        return {
            "temperature_k": float(_getf(msg, "static_temperature", default=273.15)),
        }
    except Exception:
        return None


# ── NodeStatus ────────────────────────────────────────────────────────────────

def parse_node_status(msg: Any, source_node_id: int) -> Optional[Dict[str, Any]]:
    """Parse a uavcan.protocol.NodeStatus message.

    Returns
    -------
    dict with keys:
        node_id        int
        uptime_sec     int
        health         int  0=OK .. 3=CRITICAL
        health_label   str
        mode           int
        mode_label     str
        vendor_code    int
    """
    if msg is None:
        return None
    try:
        health      = int(_getf(msg, "health",                        default=0))
        mode        = int(_getf(msg, "mode",                          default=0))
        uptime      = int(_getf(msg, "uptime_sec",                    default=0))
        vendor_code = int(_getf(msg, "vendor_specific_status_code",   default=0))
        return {
            "node_id":      source_node_id,
            "uptime_sec":   uptime,
            "health":       health,
            "health_label": HEALTH_LABEL.get(health, f"UNKNOWN({health})"),
            "mode":         mode,
            "mode_label":   MODE_LABEL.get(mode, f"UNKNOWN({mode})"),
            "vendor_code":  vendor_code,
        }
    except Exception:
        return None


# ── RTCM stream chunking ──────────────────────────────────────────────────────

def chunk_rtcm(data: bytes, sequence_id: int) -> List[Dict[str, Any]]:
    """Split a raw RTCM byte string into DroneCAN RTCMStream message dicts.

    Each chunk carries up to RTCM_MAX_CHUNK bytes.  The caller must convert
    these dicts into actual dronecan.uavcan.equipment.gnss.RTCMStream objects
    and broadcast them on the bus.

    Parameters
    ----------
    data        : raw RTCM correction bytes
    sequence_id : monotonically increasing counter (uint16, wraps at 65535)

    Returns
    -------
    List of dicts:  {"sequence_id": int, "data": bytes}
    """
    chunks = []
    for i in range(0, len(data), RTCM_MAX_CHUNK):
        chunks.append({
            "sequence_id": sequence_id & 0xFFFF,
            "data": data[i:i + RTCM_MAX_CHUNK],
        })
        sequence_id += 1
    return chunks
