venus/axiom-nmea/raymarine_nmea/protocol/decoder.py

"""
Raymarine packet decoder.

Decodes Raymarine LightHouse protobuf packets and extracts sensor data
into structured Python objects.
"""

from typing import Dict, List, Optional, Any
from dataclasses import dataclass, field as dc_field
import time

from .parser import ProtobufParser, ProtoField
from .constants import (
    WIRE_VARINT,
    WIRE_FIXED64,
    WIRE_LENGTH,
    WIRE_FIXED32,
    HEADER_SIZE,
    RAD_TO_DEG,
    MS_TO_KTS,
    FEET_TO_M,
    KELVIN_OFFSET,
    PA_TO_MBAR,
    Fields,
    GPSFields,
    HeadingFields,
    SOGCOGFields,
    DepthFields,
    WindFields,
    TemperatureFields,
    TankFields,
    BatteryFields,
    EngineFields,
    ValidationRanges,
    TANK_STATUS_WASTE,
)


@dataclass
class DecodedData:
    """Container for decoded sensor values from a single packet.

    This is a lightweight container for data extracted from one packet.
    For aggregated data across multiple packets, use SensorData.
    """
    # Position
    latitude: Optional[float] = None
    longitude: Optional[float] = None

    # Navigation
    heading_deg: Optional[float] = None
    cog_deg: Optional[float] = None
    sog_kts: Optional[float] = None

    # Wind
    twd_deg: Optional[float] = None   # True Wind Direction
    tws_kts: Optional[float] = None   # True Wind Speed
    awa_deg: Optional[float] = None   # Apparent Wind Angle
    aws_kts: Optional[float] = None   # Apparent Wind Speed

    # Depth
    depth_m: Optional[float] = None

    # Temperature
    water_temp_c: Optional[float] = None
    air_temp_c: Optional[float] = None

    # Barometric pressure
    pressure_mbar: Optional[float] = None

    # Tanks: dict of tank_id -> level percentage
    tanks: Dict[int, float] = dc_field(default_factory=dict)

    # Batteries: dict of battery_id -> voltage
    batteries: Dict[int, float] = dc_field(default_factory=dict)

    # Decode timestamp
    timestamp: float = dc_field(default_factory=time.time)

    def has_data(self) -> bool:
        """Check if any data was decoded."""
        return (
            self.latitude is not None or
            self.longitude is not None or
            self.heading_deg is not None or
            self.cog_deg is not None or
            self.sog_kts is not None or
            self.twd_deg is not None or
            self.tws_kts is not None or
            self.depth_m is not None or
            self.water_temp_c is not None or
            self.air_temp_c is not None or
            self.pressure_mbar is not None or
            bool(self.tanks) or
            bool(self.batteries)
        )


class RaymarineDecoder:
    """Decodes Raymarine packets using proper protobuf parsing.

    This decoder implements field-based parsing of Raymarine's protobuf
    protocol. It extracts all supported sensor types and validates values.

    Example:
        decoder = RaymarineDecoder()
        result = decoder.decode(packet_bytes)
        if result.latitude:
            print(f"GPS: {result.latitude}, {result.longitude}")
    """

    def __init__(self, verbose: bool = False):
        """Initialize the decoder.

        Args:
            verbose: If True, print decoded values
        """
        self.verbose = verbose

    def decode(self, packet: bytes) -> DecodedData:
        """Decode a single Raymarine packet.

        Args:
            packet: Raw packet bytes (including 20-byte header)

        Returns:
            DecodedData containing all extracted values
        """
        result = DecodedData()

        # Need at least header + some protobuf data
        if len(packet) < HEADER_SIZE + 20:
            return result

        # Skip fixed header, protobuf starts at offset 0x14
        proto_data = packet[HEADER_SIZE:]

        # Parse protobuf - collect repeated fields:
        # Field 14 = Engine data (contains battery voltage at 14.3.4)
        # Field 16 = Tank data
        # Field 20 = House battery data
        parser = ProtobufParser(proto_data)
        fields = parser.parse_message(collect_repeated={14, 16, 20})

        if not fields:
            return result

        # Extract GPS from Field 2
        if Fields.GPS_POSITION in fields:
            gps_field = fields[Fields.GPS_POSITION]
            if gps_field.children:
                self._extract_gps(gps_field.children, result)

        # Extract Heading from Field 3
        if Fields.HEADING in fields:
            heading_field = fields[Fields.HEADING]
            if heading_field.children:
                self._extract_heading(heading_field.children, result)

        # Extract SOG/COG from Field 5 (primary source for SOG/COG)
        if Fields.SOG_COG in fields:
            sog_cog_field = fields[Fields.SOG_COG]
            if sog_cog_field.children:
                self._extract_sog_cog(sog_cog_field.children, result)

        # Extract Wind from Field 13
        if Fields.WIND_NAVIGATION in fields:
            wind_field = fields[Fields.WIND_NAVIGATION]
            if wind_field.children:
                self._extract_wind(wind_field.children, result)

        # Extract Depth from Field 7 (only in larger packets)
        if Fields.DEPTH in fields:
            depth_field = fields[Fields.DEPTH]
            if depth_field.children:
                self._extract_depth(depth_field.children, result)

        # Extract Temperature from Field 15
        if Fields.TEMPERATURE in fields:
            temp_field = fields[Fields.TEMPERATURE]
            if temp_field.children:
                self._extract_temperature(temp_field.children, result)

        # Extract Tank data from Field 16 (repeated)
        if Fields.TANK_DATA in fields:
            tank_fields = fields[Fields.TANK_DATA]  # This is a list
            self._extract_tanks(tank_fields, result)

        # Extract Battery data from Field 20 (repeated) - house batteries
        if Fields.HOUSE_BATTERY in fields:
            battery_fields = fields[Fields.HOUSE_BATTERY]  # This is a list
            self._extract_batteries(battery_fields, result)

        # Extract Engine battery data from Field 14 (repeated)
        if Fields.ENGINE_DATA in fields:
            engine_fields = fields[Fields.ENGINE_DATA]  # This is a list
            self._extract_engine_batteries(engine_fields, result)

        return result

    def _extract_gps(
        self,
        fields: Dict[int, ProtoField],
        result: DecodedData
    ) -> None:
        """Extract GPS position from Field 2's children."""
        lat = None
        lon = None

        # Field 1 = Latitude
        if GPSFields.LATITUDE in fields:
            f = fields[GPSFields.LATITUDE]
            if f.wire_type == WIRE_FIXED64:
                lat = ProtobufParser.decode_double(f.value)

        # Field 2 = Longitude
        if GPSFields.LONGITUDE in fields:
            f = fields[GPSFields.LONGITUDE]
            if f.wire_type == WIRE_FIXED64:
                lon = ProtobufParser.decode_double(f.value)

        # Validate lat/lon
        if lat is not None and lon is not None:
            if (ValidationRanges.LATITUDE_MIN <= lat <= ValidationRanges.LATITUDE_MAX and
                ValidationRanges.LONGITUDE_MIN <= lon <= ValidationRanges.LONGITUDE_MAX):
                # Check not at null island
                if (abs(lat) > ValidationRanges.NULL_ISLAND_THRESHOLD or
                    abs(lon) > ValidationRanges.NULL_ISLAND_THRESHOLD):
                    result.latitude = lat
                    result.longitude = lon
                    if self.verbose:
                        print(f"GPS: {lat:.6f}, {lon:.6f}")

    def _extract_heading(
        self,
        fields: Dict[int, ProtoField],
        result: DecodedData
    ) -> None:
        """Extract heading from Field 3's children."""
        # Field 2 = Heading in radians
        if HeadingFields.HEADING_RAD in fields:
            f = fields[HeadingFields.HEADING_RAD]
            if f.wire_type == WIRE_FIXED32:
                val = ProtobufParser.decode_float(f.value)
                if val is not None:
                    if ValidationRanges.ANGLE_MIN <= val <= ValidationRanges.ANGLE_MAX:
                        heading_deg = (val * RAD_TO_DEG) % 360
                        result.heading_deg = heading_deg
                        if self.verbose:
                            print(f"Heading: {heading_deg:.1f}°")

    def _extract_sog_cog(
        self,
        fields: Dict[int, ProtoField],
        result: DecodedData
    ) -> None:
        """Extract SOG and COG from Field 5's children.

        Field 5 contains GPS-derived navigation data.
        Field 5.1 = COG (shows most variation in real data)
        Field 5.3 = SOG (confirmed with real data)
        """
        # Field 5.1 = COG (Course Over Ground) in radians - confirmed with real data
        if SOGCOGFields.COG_RAD in fields:
            f = fields[SOGCOGFields.COG_RAD]
            if f.wire_type == WIRE_FIXED32:
                val = ProtobufParser.decode_float(f.value)
                if val is not None:
                    if ValidationRanges.ANGLE_MIN <= val <= ValidationRanges.ANGLE_MAX:
                        cog_deg = (val * RAD_TO_DEG) % 360
                        result.cog_deg = cog_deg
                        if self.verbose:
                            print(f"COG: {cog_deg:.1f}°")

        # Field 5.3 = SOG (Speed Over Ground) in m/s - confirmed with real data
        if SOGCOGFields.SOG_MS in fields:
            f = fields[SOGCOGFields.SOG_MS]
            if f.wire_type == WIRE_FIXED32:
                val = ProtobufParser.decode_float(f.value)
                if val is not None:
                    if ValidationRanges.SPEED_MIN <= val <= ValidationRanges.SPEED_MAX:
                        sog_kts = val * MS_TO_KTS
                        result.sog_kts = sog_kts
                        if self.verbose:
                            print(f"SOG: {sog_kts:.1f} kts")

    def _extract_wind(
        self,
        fields: Dict[int, ProtoField],
        result: DecodedData
    ) -> None:
        """Extract wind data from Field 13's children."""
        # Field 4 = True Wind Direction (radians)
        if WindFields.TRUE_WIND_DIRECTION in fields:
            f = fields[WindFields.TRUE_WIND_DIRECTION]
            if f.wire_type == WIRE_FIXED32:
                val = ProtobufParser.decode_float(f.value)
                if val is not None:
                    if ValidationRanges.ANGLE_MIN <= val <= ValidationRanges.ANGLE_MAX:
                        twd_deg = (val * RAD_TO_DEG) % 360
                        result.twd_deg = twd_deg
                        if self.verbose:
                            print(f"TWD: {twd_deg:.1f}°")

        # Field 5 = True Wind Speed (m/s)
        if WindFields.TRUE_WIND_SPEED in fields:
            f = fields[WindFields.TRUE_WIND_SPEED]
            if f.wire_type == WIRE_FIXED32:
                val = ProtobufParser.decode_float(f.value)
                if val is not None:
                    if ValidationRanges.SPEED_MIN <= val <= ValidationRanges.SPEED_MAX:
                        tws_kts = val * MS_TO_KTS
                        result.tws_kts = tws_kts
                        if self.verbose:
                            print(f"TWS: {tws_kts:.1f} kts")

        # Field 6 = Apparent Wind Speed (m/s)
        if WindFields.APPARENT_WIND_SPEED in fields:
            f = fields[WindFields.APPARENT_WIND_SPEED]
            if f.wire_type == WIRE_FIXED32:
                val = ProtobufParser.decode_float(f.value)
                if val is not None:
                    if ValidationRanges.SPEED_MIN <= val <= ValidationRanges.SPEED_MAX:
                        aws_kts = val * MS_TO_KTS
                        result.aws_kts = aws_kts
                        if self.verbose:
                            print(f"AWS: {aws_kts:.1f} kts")

    def _extract_depth(
        self,
        fields: Dict[int, ProtoField],
        result: DecodedData
    ) -> None:
        """Extract depth from Field 7's children."""
        if DepthFields.DEPTH_METERS in fields:
            f = fields[DepthFields.DEPTH_METERS]
            if f.wire_type == WIRE_FIXED32:
                val = ProtobufParser.decode_float(f.value)
                if val is not None:
                    if ValidationRanges.DEPTH_MIN < val <= ValidationRanges.DEPTH_MAX:
                        result.depth_m = val
                        if self.verbose:
                            depth_ft = val / FEET_TO_M
                            print(f"Depth: {depth_ft:.1f} ft ({val:.2f} m)")

    def _extract_temperature(
        self,
        fields: Dict[int, ProtoField],
        result: DecodedData
    ) -> None:
        """Extract temperature and pressure from Field 15's children."""
        # Field 1 = Barometric Pressure (Pascals)
        if TemperatureFields.BAROMETRIC_PRESSURE in fields:
            f = fields[TemperatureFields.BAROMETRIC_PRESSURE]
            if f.wire_type == WIRE_FIXED32:
                val = ProtobufParser.decode_float(f.value)
                if val is not None:
                    if ValidationRanges.PRESSURE_MIN <= val <= ValidationRanges.PRESSURE_MAX:
                        pressure_mbar = val * PA_TO_MBAR
                        result.pressure_mbar = pressure_mbar
                        if self.verbose:
                            print(f"Pressure: {pressure_mbar:.1f} mbar")

        # Field 3 = Air Temperature (Kelvin)
        if TemperatureFields.AIR_TEMP in fields:
            f = fields[TemperatureFields.AIR_TEMP]
            if f.wire_type == WIRE_FIXED32:
                val = ProtobufParser.decode_float(f.value)
                if val is not None:
                    if ValidationRanges.AIR_TEMP_MIN <= val <= ValidationRanges.AIR_TEMP_MAX:
                        temp_c = val - KELVIN_OFFSET
                        result.air_temp_c = temp_c
                        if self.verbose:
                            print(f"Air Temp: {temp_c:.1f}°C")

        # Field 9 = Water Temperature (Kelvin)
        if TemperatureFields.WATER_TEMP in fields:
            f = fields[TemperatureFields.WATER_TEMP]
            if f.wire_type == WIRE_FIXED32:
                val = ProtobufParser.decode_float(f.value)
                if val is not None:
                    if ValidationRanges.WATER_TEMP_MIN <= val <= ValidationRanges.WATER_TEMP_MAX:
                        temp_c = val - KELVIN_OFFSET
                        result.water_temp_c = temp_c
                        if self.verbose:
                            print(f"Water Temp: {temp_c:.1f}°C")

    def _extract_tanks(
        self,
        tank_fields: List[ProtoField],
        result: DecodedData
    ) -> None:
        """Extract tank levels from Field 16 (repeated)."""
        for tank_field in tank_fields:
            if not tank_field.children:
                continue

            children = tank_field.children
            tank_id = None
            level = None
            status = None

            # Field 1 = Tank ID (varint)
            if TankFields.TANK_ID in children:
                f = children[TankFields.TANK_ID]
                if f.wire_type == WIRE_VARINT:
                    tank_id = f.value

            # Field 2 = Status (varint)
            if TankFields.STATUS in children:
                f = children[TankFields.STATUS]
                if f.wire_type == WIRE_VARINT:
                    status = f.value

            # Field 3 = Tank Level percentage (float)
            if TankFields.LEVEL_PCT in children:
                f = children[TankFields.LEVEL_PCT]
                if f.wire_type == WIRE_FIXED32:
                    val = ProtobufParser.decode_float(f.value)
                    if val is not None:
                        if ValidationRanges.TANK_MIN <= val <= ValidationRanges.TANK_MAX:
                            level = val

            # If we have a level but no tank_id, try to infer it
            if tank_id is None and level is not None:
                if status == TANK_STATUS_WASTE:
                    # Black/gray water tank
                    tank_id = 100
                elif status is None:
                    # Port Fuel is the ONLY tank with neither ID nor status
                    tank_id = 2

            if tank_id is not None and level is not None:
                result.tanks[tank_id] = level
                if self.verbose:
                    print(f"Tank {tank_id}: {level:.1f}%")

    def _extract_batteries(
        self,
        battery_fields: List[ProtoField],
        result: DecodedData
    ) -> None:
        """Extract battery voltages from Field 20 (repeated)."""
        for battery_field in battery_fields:
            if not battery_field.children:
                continue

            children = battery_field.children
            battery_id = None
            voltage = None

            # Field 1 = Battery ID (varint)
            if BatteryFields.BATTERY_ID in children:
                f = children[BatteryFields.BATTERY_ID]
                if f.wire_type == WIRE_VARINT:
                    battery_id = f.value

            # Field 3 = Voltage (float)
            if BatteryFields.VOLTAGE in children:
                f = children[BatteryFields.VOLTAGE]
                if f.wire_type == WIRE_FIXED32:
                    val = ProtobufParser.decode_float(f.value)
                    if val is not None:
                        if ValidationRanges.VOLTAGE_MIN <= val <= ValidationRanges.VOLTAGE_MAX:
                            voltage = val

            if battery_id is not None and voltage is not None:
                result.batteries[battery_id] = voltage
                if self.verbose:
                    print(f"Battery {battery_id}: {voltage:.2f}V")

    def _extract_engine_batteries(
        self,
        engine_fields: List[ProtoField],
        result: DecodedData
    ) -> None:
        """Extract engine battery voltages from Field 14 (repeated).

        Engine data structure:
            Field 14.1 (varint): Engine ID (0=Port, 1=Starboard)
            Field 14.3 (message): Engine sensor data
                Field 14.3.4 (float): Battery voltage
        """
        for engine_field in engine_fields:
            if not engine_field.children:
                continue

            children = engine_field.children

            # Field 1 = Engine ID (varint), default 0 (Port) if not present
            engine_id = 0
            if EngineFields.ENGINE_ID in children:
                f = children[EngineFields.ENGINE_ID]
                if f.wire_type == WIRE_VARINT:
                    engine_id = f.value

            # Field 3 = Engine sensor data (nested message)
            if EngineFields.SENSOR_DATA in children:
                f = children[EngineFields.SENSOR_DATA]
                if f.wire_type == WIRE_LENGTH:
                    sensor_data = f.value
                    # Parse the nested message to get Field 4 (voltage)
                    sensor_parser = ProtobufParser(sensor_data)
                    sensor_fields = sensor_parser.parse_message()

                    if EngineFields.BATTERY_VOLTAGE in sensor_fields:
                        vf = sensor_fields[EngineFields.BATTERY_VOLTAGE]
                        if vf.wire_type == WIRE_FIXED32:
                            val = ProtobufParser.decode_float(vf.value)
                            if val is not None:
                                if ValidationRanges.VOLTAGE_MIN <= val <= ValidationRanges.VOLTAGE_MAX:
                                    # Use battery_id = 1000 + engine_id
                                    battery_id = 1000 + engine_id
                                    result.batteries[battery_id] = val
                                    if self.verbose:
                                        print(f"Engine {engine_id} Battery: {val:.2f}V")