AR-VMS-Seaman/vmssailor/runtime/server/nmea2000.py

"""Driver NMEA 2000 (Sprint 5).

Sprint 5 entrega:
- Parser de PGNs relevantes para VMS-Sailor: 127257 (Attitude), 127488/489
  (Engine Parameters), 127505 (Fluid Level), 127508 (Battery Status), 129025
  (Position Rapid)
- N2KPublisher: convierte tag updates a PGNs y los hace disponibles para envío
- N2KSimulator: produce PGN frames simulados (sin hardware CAN)

El driver REAL contra hardware CAN (USB-CAN, Actisense NGT-1, Yacht Devices
YDNU-02) llega en Sprint 12+ junto con el firmware ESP32.
"""

from __future__ import annotations

import asyncio
import logging
import math
import struct
from collections.abc import Callable
from contextlib import suppress
from dataclasses import dataclass
from datetime import UTC, datetime

from vmssailor.core.enums import Quality
from vmssailor.runtime.server.tag_store import TagStore

logger = logging.getLogger(__name__)


# ============================================================================
# PGN definitions
# ============================================================================


@dataclass(slots=True)
class PgnFrame:
    """Marco NMEA 2000 simplificado."""

    pgn: int
    source: int
    data: bytes
    timestamp: datetime

    def __str__(self) -> str:
        return f"PGN{self.pgn:>6d} src={self.source:>3d} len={len(self.data)} ts={self.timestamp.isoformat()}"


# PGN 127257 — Attitude (yaw, pitch, roll)
PGN_ATTITUDE = 127257

# PGN 127488 — Engine Parameters Rapid (RPM, boost, trim)
PGN_ENGINE_RAPID = 127488

# PGN 127489 — Engine Parameters Dynamic (oil press, oil temp, coolant temp, etc)
PGN_ENGINE_DYNAMIC = 127489

# PGN 127505 — Fluid Level
PGN_FLUID_LEVEL = 127505

# PGN 127508 — Battery Status
PGN_BATTERY = 127508

# PGN 129025 — Position Rapid
PGN_POSITION = 129025


def parse_pgn_attitude(data: bytes) -> dict[str, float]:
    """Parse PGN 127257 Attitude. 8 bytes:
        byte 0: SID
        bytes 1-2: yaw (int16, 0.0001 rad)
        bytes 3-4: pitch (int16, 0.0001 rad)
        bytes 5-6: roll (int16, 0.0001 rad)
    """
    if len(data) < 7:
        raise ValueError("PGN 127257 requires at least 7 bytes")
    yaw_raw, pitch_raw, roll_raw = struct.unpack("<hhh", data[1:7])
    return {
        "yaw_deg": math.degrees(yaw_raw * 0.0001),
        "pitch_deg": math.degrees(pitch_raw * 0.0001),
        "roll_deg": math.degrees(roll_raw * 0.0001),
    }


def encode_pgn_attitude(yaw_deg: float, pitch_deg: float, roll_deg: float, sid: int = 0) -> bytes:
    """Codifica PGN 127257 a bytes (para publicar al backbone)."""
    yaw = int(math.radians(yaw_deg) / 0.0001)
    pitch = int(math.radians(pitch_deg) / 0.0001)
    roll = int(math.radians(roll_deg) / 0.0001)
    return struct.pack("<Bhhhh", sid & 0xFF, yaw, pitch, roll, 0)


def parse_pgn_engine_rapid(data: bytes) -> dict[str, float]:
    """Parse PGN 127488 Engine Parameters Rapid.
        byte 0: engine instance
        bytes 1-2: speed (uint16, 0.25 rpm)
        bytes 3-4: boost pressure (uint16, hPa)
        byte 5: trim/tilt (int8, %)
    """
    if len(data) < 6:
        raise ValueError("PGN 127488 requires at least 6 bytes")
    instance = data[0]
    rpm_raw = struct.unpack("<H", data[1:3])[0]
    boost_raw = struct.unpack("<H", data[3:5])[0]
    trim_raw = struct.unpack("<b", data[5:6])[0]
    return {
        "instance": float(instance),
        "rpm": rpm_raw * 0.25,
        "boost_pa": boost_raw * 100.0,  # hPa to Pa
        "trim_pct": float(trim_raw),
    }


def encode_pgn_engine_rapid(instance: int, rpm: float, boost_pa: float = 0.0, trim_pct: float = 0.0) -> bytes:
    rpm_raw = int(rpm / 0.25)
    boost_raw = int(boost_pa / 100.0)
    return struct.pack("<BHHb", instance & 0xFF, rpm_raw, boost_raw, int(trim_pct))


def parse_pgn_fluid_level(data: bytes) -> dict[str, float]:
    """Parse PGN 127505 Fluid Level.
        bits 0-3: instance
        bits 4-7: fluid type (0=fuel, 1=water, 2=grey, 3=waste, 4=live well, 5=oil, 6=black)
        bytes 2-3: level (int16, 0.004 %)
        bytes 4-7: capacity (uint32, 0.1 L)
    """
    if len(data) < 8:
        raise ValueError("PGN 127505 requires 8 bytes")
    instance_type = data[0]
    instance = instance_type & 0x0F
    fluid_type = (instance_type >> 4) & 0x0F
    level_raw = struct.unpack("<h", data[1:3])[0]
    capacity_raw = struct.unpack("<I", data[3:7])[0]
    return {
        "instance": float(instance),
        "fluid_type": float(fluid_type),
        "level_pct": level_raw * 0.004,
        "capacity_l": capacity_raw * 0.1,
    }


def parse_pgn_battery(data: bytes) -> dict[str, float]:
    """Parse PGN 127508 Battery Status.
        byte 0: instance
        bytes 1-2: voltage (uint16, 0.01 V)
        bytes 3-4: current (int16, 0.1 A)
        bytes 5-6: temperature (uint16, 0.01 K - 273.15 for C)
    """
    if len(data) < 7:
        raise ValueError("PGN 127508 requires at least 7 bytes")
    instance = data[0]
    v_raw = struct.unpack("<H", data[1:3])[0]
    i_raw = struct.unpack("<h", data[3:5])[0]
    t_raw = struct.unpack("<H", data[5:7])[0]
    temp_c = (t_raw * 0.01) - 273.15
    return {
        "instance": float(instance),
        "voltage_v": v_raw * 0.01,
        "current_a": i_raw * 0.1,
        "temperature_c": temp_c,
    }


# ============================================================================
# Publisher
# ============================================================================


class N2KPublisher:
    """Publica tag updates como PGNs.

    Sprint 5: el `transport` es un callable que recibe `PgnFrame`.
    Sprint 12+: real CAN transmit con python-can / Actisense / YDWG-02.
    """

    def __init__(
        self,
        tag_store: TagStore,
        *,
        transport: Callable[[PgnFrame], None] | None = None,
        device_address: int = 99,
    ) -> None:
        self._store = tag_store
        self._transport = transport or (lambda _f: None)
        self._addr = device_address
        self._stop = False
        self._task: asyncio.Task | None = None

    async def start(self, period_s: float = 0.5) -> None:
        self._period_s = period_s
        self._task = asyncio.create_task(self._loop())

    async def stop(self) -> None:
        self._stop = True
        if self._task:
            self._task.cancel()
            with suppress(asyncio.CancelledError):
                await self._task

    async def _loop(self) -> None:
        try:
            while not self._stop:
                self._publish_engines()
                self._publish_attitude_stub()
                await asyncio.sleep(self._period_s)
        except asyncio.CancelledError:
            pass

    def _publish_engines(self) -> None:
        for i, (tag_id, _tag) in enumerate(self._store.all_tags().items()):
            if not tag_id.endswith(".RPM"):
                continue
            tv = self._store.get(tag_id)
            if tv is None or not isinstance(tv.value, (int, float)):
                continue
            data = encode_pgn_engine_rapid(instance=i, rpm=float(tv.value))
            self._transport(
                PgnFrame(
                    pgn=PGN_ENGINE_RAPID,
                    source=self._addr,
                    data=data,
                    timestamp=datetime.now(UTC),
                )
            )

    def _publish_attitude_stub(self) -> None:
        # Sprint 5: si hay tags de actitud sintéticos, los publicamos.
        roll = self._store.get("VESSEL.ROLL_DEG")
        pitch = self._store.get("VESSEL.PITCH_DEG")
        if roll is None or pitch is None:
            return
        if not isinstance(roll.value, (int, float)) or not isinstance(pitch.value, (int, float)):
            return
        data = encode_pgn_attitude(yaw_deg=0.0, pitch_deg=float(pitch.value), roll_deg=float(roll.value))
        self._transport(
            PgnFrame(
                pgn=PGN_ATTITUDE,
                source=self._addr,
                data=data,
                timestamp=datetime.now(UTC),
            )
        )


# ============================================================================
# Subscriber (consume PGNs from backbone)
# ============================================================================


class N2KSubscriber:
    """Suscribe a PGNs entrantes y los traduce a tag updates.

    Sprint 5: `inject_frame(frame)` para testing.
    Sprint 12: bucket de USB-CAN/Actisense alimenta los frames.
    """

    def __init__(self, tag_store: TagStore) -> None:
        self._store = tag_store
        self._frames: asyncio.Queue[PgnFrame] = asyncio.Queue(maxsize=4096)
        self._stop = False
        self._task: asyncio.Task | None = None

    async def start(self) -> None:
        self._task = asyncio.create_task(self._loop())

    async def stop(self) -> None:
        self._stop = True
        if self._task:
            self._task.cancel()
            with suppress(asyncio.CancelledError):
                await self._task

    def inject_frame(self, frame: PgnFrame) -> None:
        """Inyecta un frame para testing o para integración con USB-CAN."""
        with suppress(asyncio.QueueFull):
            self._frames.put_nowait(frame)

    async def _loop(self) -> None:
        try:
            while not self._stop:
                frame = await self._frames.get()
                await self._handle_frame(frame)
        except asyncio.CancelledError:
            pass

    async def _handle_frame(self, frame: PgnFrame) -> None:
        try:
            if frame.pgn == PGN_ATTITUDE:
                att = parse_pgn_attitude(frame.data)
                if "VESSEL.ROLL_DEG" in self._store:
                    await self._store.update(
                        "VESSEL.ROLL_DEG", att["roll_deg"], quality=Quality.GOOD
                    )
                if "VESSEL.PITCH_DEG" in self._store:
                    await self._store.update(
                        "VESSEL.PITCH_DEG", att["pitch_deg"], quality=Quality.GOOD
                    )
            elif frame.pgn == PGN_ENGINE_RAPID:
                eng = parse_pgn_engine_rapid(frame.data)
                # Convención: instance N corresponde a tag ME_{N}.RPM si existe
                inst = int(eng["instance"])
                candidate = f"ME_INST_{inst}.RPM"
                if candidate in self._store:
                    await self._store.update(candidate, eng["rpm"], quality=Quality.GOOD)
        except Exception:
            logger.exception("Error handling PGN %s", frame.pgn)