venus/axiom-nmea/debug/protobuf_decoder.py

#!/usr/bin/env python3
"""
Raymarine Protobuf Decoder

Properly parses the protobuf structure by field numbers rather than fixed byte offsets.
This is more robust and handles different packet sizes correctly.

Field Mapping (discovered through analysis):
  Field 2:  GPS/Position Data
            ├─ Field 1 (double): Latitude
            └─ Field 2 (double): Longitude

  Field 3:  Heading Block
            └─ Field 2 (float): Heading (radians)

  Field 5:  SOG/COG Navigation Data
            ├─ Field 1 (float): COG - Course Over Ground (radians)
            └─ Field 3 (float): SOG - Speed Over Ground (m/s)

  Field 7:  Depth Block (only in larger packets 1472B+)
            └─ Field 1 (float): Depth (meters)

  Field 13: Wind/Navigation Data
            ├─ Field 4 (float): True Wind Direction (radians)
            ├─ Field 5 (float): Wind Speed (m/s)
            └─ Field 6 (float): Apparent Wind Speed (m/s)

  Field 14: Engine Data (repeated)
            ├─ Field 1 (varint): Engine ID (0=Port, 1=Starboard)
            └─ Field 3: Engine Sensor Data
                └─ Field 4 (float): Battery Voltage (volts)

  Field 15: Temperature Data
            ├─ Field 3 (float): Air Temperature (Kelvin)
            └─ Field 9 (float): Water Temperature (Kelvin)

  Field 16: Tank Data (repeated)
            ├─ Field 1 (varint): Tank ID
            ├─ Field 2 (varint): Status/Flag
            └─ Field 3 (float): Tank Level (percentage)

  Field 20: House Battery Data (repeated)
            ├─ Field 1 (varint): Battery ID (11=Aft House, 13=Stern House)
            └─ Field 3 (float): Voltage (volts)
"""

import struct
import socket
import time
import json
import threading
import argparse
from dataclasses import dataclass, field
from typing import Dict, List, Optional, Any, Tuple
from datetime import datetime

# Wire types
WIRE_VARINT = 0
WIRE_FIXED64 = 1
WIRE_LENGTH = 2
WIRE_FIXED32 = 5

# Conversion constants
RAD_TO_DEG = 57.2957795131
MS_TO_KTS = 1.94384449
FEET_TO_M = 0.3048
KELVIN_OFFSET = 273.15

# Tank ID to name mapping (id -> (name, capacity_gallons))
TANK_NAMES = {
    1: ("Stbd Fuel", 265),       # Starboard fuel tank, 265 gallons
    2: ("Port Fuel", 265),       # Port fuel tank, 265 gallons
    10: ("Fwd Water", 90),       # Forward water tank, 90 gallons
    11: ("Aft Water", 90),       # Rear water tank, 90 gallons
    # Inferred tanks (status-based)
    100: ("Black Water", 53),    # Waste tank (status=5), 53 gallons
}

# Tank status values
TANK_STATUS_WASTE = 5  # Black/gray water tanks use status=5

# Battery ID to name mapping (id -> name)
# House batteries from Field 20: IDs 11, 13
# Engine batteries from Field 14: IDs 1000+ (1000 + engine_id)
BATTERY_NAMES = {
    11: "Aft House",       # Aft house battery bank (Field 20)
    13: "Stern House",     # Stern house battery bank (Field 20)
    1000: "Port Engine",   # Port engine battery (Field 14, engine_id=0)
    1001: "Stbd Engine",   # Starboard engine battery (Field 14, engine_id=1)
}

# Raymarine multicast groups - ALL groups (original)
MULTICAST_GROUPS_ALL = [
    ("226.192.206.98", 2561),   # Navigation sensors (mostly zeros)
    ("226.192.206.99", 2562),   # Heartbeat/status
    ("226.192.206.100", 2563),  # Alternative data (low traffic)
    ("226.192.206.101", 2564),  # Alternative data (low traffic)
    ("226.192.206.102", 2565),  # PRIMARY sensor data
    ("226.192.219.0", 3221),    # Display sync (high traffic, no sensor data)
    ("239.2.1.1", 2154),        # May contain tank/engine data
]

# PRIMARY only - single group with all sensor data
MULTICAST_GROUPS_PRIMARY = [
    ("226.192.206.102", 2565),  # PRIMARY sensor data (GPS, wind, depth, tanks, batteries)
]

# MINIMAL - primary + potential tank/engine backup
MULTICAST_GROUPS_MINIMAL = [
    ("226.192.206.102", 2565),  # PRIMARY sensor data
    ("239.2.1.1", 2154),        # Tank/engine data (backup)
]

# Group presets for CLI
GROUP_PRESETS = {
    "all": MULTICAST_GROUPS_ALL,
    "primary": MULTICAST_GROUPS_PRIMARY,
    "minimal": MULTICAST_GROUPS_MINIMAL,
}

# Default to all groups for backward compatibility
MULTICAST_GROUPS = MULTICAST_GROUPS_ALL

# Fixed header size
HEADER_SIZE = 20


@dataclass
class ProtoField:
    """A decoded protobuf field."""
    field_num: int
    wire_type: int
    value: Any
    children: Dict[int, 'ProtoField'] = field(default_factory=dict)


class ProtobufParser:
    """Parses protobuf wire format without a schema."""

    def __init__(self, data: bytes):
        self.data = data
        self.pos = 0

    def remaining(self) -> int:
        return len(self.data) - self.pos

    def read_varint(self) -> int:
        """Decode a varint."""
        result = 0
        shift = 0
        while self.pos < len(self.data):
            byte = self.data[self.pos]
            self.pos += 1
            result |= (byte & 0x7F) << shift
            if not (byte & 0x80):
                break
            shift += 7
            if shift > 63:
                break
        return result

    def read_fixed64(self) -> bytes:
        """Read 8 bytes."""
        value = self.data[self.pos:self.pos + 8]
        self.pos += 8
        return value

    def read_fixed32(self) -> bytes:
        """Read 4 bytes."""
        value = self.data[self.pos:self.pos + 4]
        self.pos += 4
        return value

    def read_length_delimited(self) -> bytes:
        """Read length-prefixed bytes."""
        length = self.read_varint()
        value = self.data[self.pos:self.pos + length]
        self.pos += length
        return value

    def parse_message(self, collect_repeated: set = None) -> Dict[int, Any]:
        """Parse all fields in a message, returning dict by field number.

        Args:
            collect_repeated: Set of field numbers to collect as lists (for repeated fields)
        """
        fields = {}
        if collect_repeated is None:
            collect_repeated = set()

        while self.pos < len(self.data):
            if self.remaining() < 1:
                break

            try:
                tag = self.read_varint()
                field_num = tag >> 3
                wire_type = tag & 0x07

                if field_num == 0 or field_num > 536870911:
                    break

                if wire_type == WIRE_VARINT:
                    value = self.read_varint()
                elif wire_type == WIRE_FIXED64:
                    value = self.read_fixed64()
                elif wire_type == WIRE_LENGTH:
                    value = self.read_length_delimited()
                elif wire_type == WIRE_FIXED32:
                    value = self.read_fixed32()
                else:
                    break  # Unknown wire type

                # For length-delimited, try to parse as nested message
                children = {}
                if wire_type == WIRE_LENGTH and len(value) >= 2:
                    try:
                        nested_parser = ProtobufParser(value)
                        children = nested_parser.parse_message()
                        # Only keep if we parsed most of the data
                        if nested_parser.pos < len(value) * 0.5:
                            children = {}
                    except:
                        children = {}

                pf = ProtoField(field_num, wire_type, value, children)

                # Handle repeated fields - collect as list
                if field_num in collect_repeated:
                    if field_num not in fields:
                        fields[field_num] = []
                    fields[field_num].append(pf)
                else:
                    # Keep last occurrence for non-repeated fields
                    fields[field_num] = pf

            except (IndexError, struct.error):
                break

        return fields


@dataclass
class SensorData:
    """Current sensor readings."""
    # 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_ft: Optional[float] = None

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

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

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

    # Timestamps
    gps_time: float = 0
    heading_time: float = 0
    wind_time: float = 0
    depth_time: float = 0
    tank_time: float = 0
    battery_time: float = 0

    # Stats
    packet_count: int = 0
    decode_count: int = 0
    start_time: float = field(default_factory=time.time)

    # Per-group statistics: group -> {packets, decoded, last_decode_fields}
    group_stats: Dict[str, Dict[str, Any]] = field(default_factory=dict)

    # Thread safety
    _lock: threading.Lock = field(default_factory=threading.Lock)

    def to_dict(self) -> Dict[str, Any]:
        with self._lock:
            return {
                "timestamp": datetime.now().isoformat(),
                "position": {
                    "latitude": self.latitude,
                    "longitude": self.longitude,
                },
                "navigation": {
                    "heading_deg": round(self.heading_deg, 1) if self.heading_deg else None,
                    "cog_deg": round(self.cog_deg, 1) if self.cog_deg else None,
                    "sog_kts": round(self.sog_kts, 1) if self.sog_kts else None,
                },
                "wind": {
                    "true_direction_deg": round(self.twd_deg, 1) if self.twd_deg else None,
                    "true_speed_kts": round(self.tws_kts, 1) if self.tws_kts else None,
                    "apparent_angle_deg": round(self.awa_deg, 1) if self.awa_deg else None,
                    "apparent_speed_kts": round(self.aws_kts, 1) if self.aws_kts else None,
                },
                "depth": {
                    "feet": round(self.depth_ft, 1) if self.depth_ft else None,
                    "meters": round(self.depth_ft * FEET_TO_M, 1) if self.depth_ft else None,
                },
                "temperature": {
                    "water_c": round(self.water_temp_c, 1) if self.water_temp_c else None,
                    "air_c": round(self.air_temp_c, 1) if self.air_temp_c else None,
                },
                "tanks": {
                    str(tank_id): {
                        "name": TANK_NAMES.get(tank_id, (f"Tank#{tank_id}", None))[0],
                        "level_pct": round(level, 1),
                        "capacity_gal": TANK_NAMES.get(tank_id, (None, None))[1],
                    } for tank_id, level in self.tanks.items()
                },
                "batteries": {
                    str(battery_id): {
                        "name": BATTERY_NAMES.get(battery_id, f"Battery#{battery_id}"),
                        "voltage_v": round(voltage, 1),
                    } for battery_id, voltage in self.batteries.items()
                },
                "stats": {
                    "packets": self.packet_count,
                    "decoded": self.decode_count,
                    "uptime_s": round(time.time() - self.start_time, 1),
                }
            }


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

    def __init__(self, sensor_data: SensorData, verbose: bool = False):
        self.data = sensor_data
        self.verbose = verbose

    def decode_double(self, raw: bytes) -> Optional[float]:
        """Decode 8 bytes as little-endian double."""
        if len(raw) != 8:
            return None
        try:
            val = struct.unpack('<d', raw)[0]
            if val != val:  # NaN check
                return None
            return val
        except:
            return None

    def decode_float(self, raw: bytes) -> Optional[float]:
        """Decode 4 bytes as little-endian float."""
        if len(raw) != 4:
            return None
        try:
            val = struct.unpack('<f', raw)[0]
            if val != val:  # NaN check
                return None
            return val
        except:
            return None

    def decode_packet(self, packet: bytes, group: str = None) -> bool:
        """Decode a single Raymarine packet.

        Args:
            packet: Raw packet bytes
            group: Source multicast group (e.g., "226.192.206.102:2565")
        """
        with self.data._lock:
            self.data.packet_count += 1
            # Initialize group stats if needed
            if group and group not in self.data.group_stats:
                self.data.group_stats[group] = {
                    "packets": 0,
                    "decoded": 0,
                    "fields": set(),  # Which data types came from this group
                }
            if group:
                self.data.group_stats[group]["packets"] += 1

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

        # 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 False

        decoded = False

        # Track which fields were decoded from this packet
        decoded_fields = []

        # Extract GPS from Field 2
        if 2 in fields:
            gps_field = fields[2]
            if gps_field.children:
                if self._extract_gps(gps_field.children):
                    decoded = True
                    decoded_fields.append("gps")

        # Extract Heading from Field 3
        if 3 in fields:
            heading_field = fields[3]
            if heading_field.children:
                if self._extract_heading(heading_field.children):
                    decoded = True
                    decoded_fields.append("heading")

        # Extract COG/SOG from Field 5
        if 5 in fields:
            cog_field = fields[5]
            if cog_field.children:
                if self._extract_cog_sog(cog_field.children):
                    decoded = True
                    decoded_fields.append("cog_sog")

        # Extract Wind from Field 13
        if 13 in fields:
            wind_field = fields[13]
            if wind_field.children:
                if self._extract_wind(wind_field.children):
                    decoded = True
                    decoded_fields.append("wind")

        # Extract Depth from Field 7 (only in larger packets)
        if 7 in fields:
            depth_field = fields[7]
            if depth_field.children:
                if self._extract_depth(depth_field.children):
                    decoded = True
                    decoded_fields.append("depth")

        # Extract Temperature from Field 15
        if 15 in fields:
            temp_field = fields[15]
            if temp_field.children:
                if self._extract_temperature(temp_field.children):
                    decoded = True
                    decoded_fields.append("temp")

        # Extract Tank data from Field 16 (repeated)
        if 16 in fields:
            tank_fields = fields[16]  # This is a list
            if self._extract_tanks(tank_fields):
                decoded = True
                decoded_fields.append("tanks")

        # Extract Battery data from Field 20 (repeated) - house batteries
        if 20 in fields:
            battery_fields = fields[20]  # This is a list
            if self._extract_batteries(battery_fields):
                decoded = True
                decoded_fields.append("house_batt")

        # Extract Engine battery data from Field 14 (repeated)
        if 14 in fields:
            engine_fields = fields[14]  # This is a list
            if self._extract_engine_batteries(engine_fields):
                decoded = True
                decoded_fields.append("engine_batt")

        if decoded:
            with self.data._lock:
                self.data.decode_count += 1
                # Track which fields came from which group
                if group and group in self.data.group_stats:
                    self.data.group_stats[group]["decoded"] += 1
                    self.data.group_stats[group]["fields"].update(decoded_fields)

        return decoded

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

        # Field 1 = Latitude
        if 1 in fields and fields[1].wire_type == WIRE_FIXED64:
            lat = self.decode_double(fields[1].value)

        # Field 2 = Longitude
        if 2 in fields and fields[2].wire_type == WIRE_FIXED64:
            lon = self.decode_double(fields[2].value)

        # Validate lat/lon
        if lat is not None and lon is not None:
            if -90 <= lat <= 90 and -180 <= lon <= 180:
                if abs(lat) > 0.1 or abs(lon) > 0.1:  # Not at null island
                    with self.data._lock:
                        self.data.latitude = lat
                        self.data.longitude = lon
                        self.data.gps_time = time.time()
                    if self.verbose:
                        print(f"GPS: {lat:.6f}, {lon:.6f}")
                    return True
        return False

    def _extract_heading(self, fields: Dict[int, ProtoField]) -> bool:
        """Extract heading from Field 3's children."""
        # Field 2 = Heading in radians
        if 2 in fields and fields[2].wire_type == WIRE_FIXED32:
            val = self.decode_float(fields[2].value)
            if val is not None and 0 <= val <= 6.5:
                heading_deg = (val * RAD_TO_DEG) % 360
                with self.data._lock:
                    self.data.heading_deg = heading_deg
                    self.data.heading_time = time.time()
                if self.verbose:
                    print(f"Heading: {heading_deg:.1f}°")
                return True
        return False

    def _extract_cog_sog(self, fields: Dict[int, ProtoField]) -> bool:
        """Extract COG and SOG from Field 5's children."""
        decoded = False

        # Field 1 = COG (Course Over Ground) in radians - confirmed with real data
        if 1 in fields and fields[1].wire_type == WIRE_FIXED32:
            val = self.decode_float(fields[1].value)
            if val is not None and 0 <= val <= 6.5:
                cog_deg = (val * RAD_TO_DEG) % 360
                with self.data._lock:
                    self.data.cog_deg = cog_deg
                if self.verbose:
                    print(f"COG: {cog_deg:.1f}°")
                decoded = True

        # Field 3 = SOG (Speed Over Ground) in m/s - confirmed with real data
        if 3 in fields and fields[3].wire_type == WIRE_FIXED32:
            val = self.decode_float(fields[3].value)
            if val is not None and 0 <= val <= 100:
                sog_kts = val * MS_TO_KTS
                with self.data._lock:
                    self.data.sog_kts = sog_kts
                if self.verbose:
                    print(f"SOG: {sog_kts:.1f} kts")
                decoded = True

        return decoded

    def _extract_wind(self, fields: Dict[int, ProtoField]) -> bool:
        """Extract wind data from Field 13's children."""
        decoded = False

        # Field 4 = True Wind Direction (radians)
        if 4 in fields and fields[4].wire_type == WIRE_FIXED32:
            val = self.decode_float(fields[4].value)
            if val is not None and 0 <= val <= 6.5:
                twd_deg = (val * RAD_TO_DEG) % 360
                with self.data._lock:
                    self.data.twd_deg = twd_deg
                    self.data.wind_time = time.time()
                if self.verbose:
                    print(f"TWD: {twd_deg:.1f}°")
                decoded = True

        # Field 5 = Wind Speed (m/s)
        if 5 in fields and fields[5].wire_type == WIRE_FIXED32:
            val = self.decode_float(fields[5].value)
            if val is not None and 0 <= val <= 100:
                tws_kts = val * MS_TO_KTS
                with self.data._lock:
                    self.data.tws_kts = tws_kts
                if self.verbose:
                    print(f"TWS: {tws_kts:.1f} kts")
                decoded = True

        # Field 6 = Another wind speed (possibly apparent)
        if 6 in fields and fields[6].wire_type == WIRE_FIXED32:
            val = self.decode_float(fields[6].value)
            if val is not None and 0 <= val <= 100:
                aws_kts = val * MS_TO_KTS
                with self.data._lock:
                    self.data.aws_kts = aws_kts
                decoded = True

        return decoded

    def _extract_depth(self, fields: Dict[int, ProtoField]) -> bool:
        """Extract depth from Field 7's children."""
        # Field 1 = Depth in meters
        if 1 in fields and fields[1].wire_type == WIRE_FIXED32:
            val = self.decode_float(fields[1].value)
            # Validate: reasonable depth range (0-1000 meters)
            if val is not None and 0 < val <= 1000:
                depth_ft = val / FEET_TO_M  # Convert meters to feet
                with self.data._lock:
                    self.data.depth_ft = depth_ft
                    self.data.depth_time = time.time()
                if self.verbose:
                    print(f"Depth: {depth_ft:.1f} ft ({val:.2f} m)")
                return True
        return False

    def _extract_temperature(self, fields: Dict[int, ProtoField]) -> bool:
        """Extract temperature from Field 15's children."""
        decoded = False

        # Field 3 = Air Temperature (Kelvin)
        if 3 in fields and fields[3].wire_type == WIRE_FIXED32:
            val = self.decode_float(fields[3].value)
            # Validate: reasonable temp range (200-350 K = -73 to 77°C)
            if val is not None and 200 <= val <= 350:
                temp_c = val - KELVIN_OFFSET
                with self.data._lock:
                    self.data.air_temp_c = temp_c
                if self.verbose:
                    print(f"Air Temp: {temp_c:.1f}°C ({val:.1f} K)")
                decoded = True

        # Field 9 = Water Temperature (Kelvin)
        if 9 in fields and fields[9].wire_type == WIRE_FIXED32:
            val = self.decode_float(fields[9].value)
            # Validate: reasonable water temp range (270-320 K = -3 to 47°C)
            if val is not None and 270 <= val <= 320:
                temp_c = val - KELVIN_OFFSET
                with self.data._lock:
                    self.data.water_temp_c = temp_c
                if self.verbose:
                    print(f"Water Temp: {temp_c:.1f}°C ({val:.1f} K)")
                decoded = True

        return decoded

    def _extract_tanks(self, tank_fields: List[ProtoField]) -> bool:
        """Extract tank levels from Field 16 (repeated)."""
        decoded = False
        unknown_fuel_idx = 0  # Counter for fuel tanks without IDs

        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 1 in children and children[1].wire_type == WIRE_VARINT:
                tank_id = children[1].value

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

            # Field 3 = Tank Level percentage (float)
            if 3 in children and children[3].wire_type == WIRE_FIXED32:
                val = self.decode_float(children[3].value)
                # Validate: 0-100% range
                if val is not None and 0 <= val <= 100:
                    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 - only tank with status=5
                    tank_id = 100
                elif status is None:
                    # Port Fuel is the ONLY tank with neither ID nor status
                    # (Stbd Fuel has ID=1, water tanks have status=1)
                    tank_id = 2  # Port Fuel

            if tank_id is not None and level is not None:
                with self.data._lock:
                    self.data.tanks[tank_id] = level
                    self.data.tank_time = time.time()
                if self.verbose:
                    print(f"Tank {tank_id}: {level:.1f}%")
                decoded = True

        return decoded

    def _extract_batteries(self, battery_fields: List[ProtoField]) -> bool:
        """Extract battery voltages from Field 20 (repeated)."""
        decoded = False

        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 1 in children and children[1].wire_type == WIRE_VARINT:
                battery_id = children[1].value

            # Field 3 = Voltage (float)
            if 3 in children and children[3].wire_type == WIRE_FIXED32:
                val = self.decode_float(children[3].value)
                # Validate: reasonable voltage range (10-60V covers 12V, 24V, 48V systems)
                if val is not None and 10 <= val <= 60:
                    voltage = val

            if battery_id is not None and voltage is not None:
                with self.data._lock:
                    self.data.batteries[battery_id] = voltage
                    self.data.battery_time = time.time()
                if self.verbose:
                    print(f"Battery {battery_id}: {voltage:.2f}V")
                decoded = True

        return decoded

    def _extract_engine_batteries(self, engine_fields: List[ProtoField]) -> bool:
        """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
        """
        decoded = False

        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 1 in children and children[1].wire_type == WIRE_VARINT:
                engine_id = children[1].value

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

                if 4 in sensor_fields and sensor_fields[4].wire_type == WIRE_FIXED32:
                    val = self.decode_float(sensor_fields[4].value)
                    # Validate: reasonable voltage range (10-60V)
                    if val is not None and 10 <= val <= 60:
                        # Use battery_id = 1000 + engine_id to distinguish from house batteries
                        battery_id = 1000 + engine_id
                        with self.data._lock:
                            self.data.batteries[battery_id] = val
                            self.data.battery_time = time.time()
                        if self.verbose:
                            print(f"Engine {engine_id} Battery: {val:.2f}V")
                        decoded = True

        return decoded


class MulticastListener:
    """Listens on Raymarine multicast groups."""

    def __init__(self, decoder: RaymarineDecoder, interface_ip: str,
                 groups: List[Tuple[str, int]] = None):
        """Initialize listener.

        Args:
            decoder: RaymarineDecoder instance
            interface_ip: IP address of network interface
            groups: List of (group, port) tuples. Defaults to MULTICAST_GROUPS.
        """
        self.decoder = decoder
        self.interface_ip = interface_ip
        self.groups = groups if groups is not None else MULTICAST_GROUPS
        self.running = False
        self.sockets = []
        self.threads = []

    def _create_socket(self, group: str, port: int) -> Optional[socket.socket]:
        try:
            sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP)
            sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
            if hasattr(socket, 'SO_REUSEPORT'):
                sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEPORT, 1)
            sock.bind(('', port))
            mreq = struct.pack("4s4s", socket.inet_aton(group), socket.inet_aton(self.interface_ip))
            sock.setsockopt(socket.IPPROTO_IP, socket.IP_ADD_MEMBERSHIP, mreq)
            sock.settimeout(1.0)
            return sock
        except Exception as e:
            print(f"Error creating socket for {group}:{port}: {e}")
            return None

    def _listen(self, sock: socket.socket, group: str, port: int):
        group_key = f"{group}:{port}"
        while self.running:
            try:
                data, addr = sock.recvfrom(65535)
                self.decoder.decode_packet(data, group=group_key)
            except socket.timeout:
                continue
            except Exception as e:
                if self.running:
                    print(f"Error on {group}:{port}: {e}")

    def start(self):
        self.running = True
        for group, port in self.groups:
            sock = self._create_socket(group, port)
            if sock:
                self.sockets.append(sock)
                t = threading.Thread(target=self._listen, args=(sock, group, port), daemon=True)
                t.start()
                self.threads.append(t)
                print(f"Listening on {group}:{port}")
        print(f"Active groups: {len(self.groups)} (threads: {len(self.threads)})")

    def stop(self):
        self.running = False
        for t in self.threads:
            t.join(timeout=2)
        for s in self.sockets:
            try:
                s.close()
            except:
                pass


def read_pcap(filename: str):
    """Read packets from pcap file."""
    packets = []
    with open(filename, 'rb') as f:
        header = f.read(24)
        magic = struct.unpack('<I', header[0:4])[0]
        swapped = magic == 0xd4c3b2a1
        endian = '>' if swapped else '<'

        while True:
            pkt_header = f.read(16)
            if len(pkt_header) < 16:
                break
            ts_sec, ts_usec, incl_len, orig_len = struct.unpack(f'{endian}IIII', pkt_header)
            pkt_data = f.read(incl_len)
            if len(pkt_data) < incl_len:
                break

            if len(pkt_data) > 42 and pkt_data[12:14] == b'\x08\x00':
                ip_header_len = (pkt_data[14] & 0x0F) * 4
                payload_start = 14 + ip_header_len + 8
                if payload_start < len(pkt_data):
                    packets.append(pkt_data[payload_start:])
    return packets


def display_dashboard(data: SensorData):
    """Display live dashboard."""
    print("\033[2J\033[H", end="")  # Clear screen

    d = data.to_dict()
    ts = datetime.now().strftime("%H:%M:%S")

    print("=" * 60)
    print(f"  RAYMARINE DECODER (Protobuf)                    {ts}")
    print("=" * 60)

    pos = d["position"]
    if pos["latitude"] and pos["longitude"]:
        print(f"  GPS:     {pos['latitude']:.6f}, {pos['longitude']:.6f}")
    else:
        print("  GPS:     No data")

    nav = d["navigation"]
    if nav["heading_deg"] is not None or nav["cog_deg"] is not None or nav["sog_kts"] is not None:
        hdg = f"{nav['heading_deg']}°" if nav['heading_deg'] is not None else "---"
        cog = f"{nav['cog_deg']}°" if nav['cog_deg'] is not None else "---"
        sog = f"{nav['sog_kts']} kts" if nav['sog_kts'] is not None else "---"
        print(f"  Heading: {hdg}  COG: {cog}  SOG: {sog}")
    else:
        print("  Nav:     No data")

    wind = d["wind"]
    if wind["true_direction_deg"] or wind["true_speed_kts"]:
        twd = f"{wind['true_direction_deg']}°" if wind['true_direction_deg'] else "---"
        tws = f"{wind['true_speed_kts']} kts" if wind['true_speed_kts'] else "---"
        print(f"  Wind:    {tws} @ {twd} (true)")
    else:
        print("  Wind:    No data")

    depth = d["depth"]
    if depth["feet"]:
        print(f"  Depth:   {depth['feet']} ft ({depth['meters']} m)")
    else:
        print("  Depth:   No data")

    temp = d["temperature"]
    if temp["air_c"] is not None or temp["water_c"] is not None:
        if temp['air_c'] is not None:
            air_f = temp['air_c'] * 9/5 + 32
            air = f"{temp['air_c']}°C / {air_f:.1f}°F"
        else:
            air = "---"
        if temp['water_c'] is not None:
            water_f = temp['water_c'] * 9/5 + 32
            water = f"{temp['water_c']}°C / {water_f:.1f}°F"
        else:
            water = "---"
        print(f"  Temp:    Air {air}, Water {water}")
    else:
        print("  Temp:    No data")

    tanks = d["tanks"]
    if tanks:
        tank_parts = []
        for tid, tank_info in sorted(tanks.items(), key=lambda x: int(x[0])):
            name = tank_info["name"]
            lvl = tank_info["level_pct"]
            capacity = tank_info["capacity_gal"]
            if capacity:
                gallons = capacity * lvl / 100
                tank_parts.append(f"{name}: {lvl}% ({gallons:.0f}gal)")
            else:
                tank_parts.append(f"{name}: {lvl}%")
        print(f"  Tanks:   {', '.join(tank_parts)}")
    else:
        print("  Tanks:   No data")

    batteries = d["batteries"]
    if batteries:
        battery_parts = []
        for bid, battery_info in sorted(batteries.items(), key=lambda x: int(x[0])):
            name = battery_info["name"]
            voltage = battery_info["voltage_v"]
            battery_parts.append(f"{name}: {voltage}V")
        print(f"  Batts:   {', '.join(battery_parts)}")
    else:
        print("  Batts:   No data")

    print("-" * 60)
    stats = d["stats"]
    print(f"  Packets: {stats['packets']}  Decoded: {stats['decoded']}  Uptime: {stats['uptime_s']}s")

    # Show per-group statistics
    with data._lock:
        if data.group_stats:
            print("-" * 60)
            print("  GROUP STATISTICS:")
            for group_key, gstats in sorted(data.group_stats.items()):
                fields_str = ", ".join(sorted(gstats["fields"])) if gstats["fields"] else "none"
                pct = (gstats["decoded"] / gstats["packets"] * 100) if gstats["packets"] > 0 else 0
                print(f"    {group_key}: {gstats['packets']} pkts, {gstats['decoded']} decoded ({pct:.0f}%)")
                print(f"      Fields: {fields_str}")
    print("=" * 60)


def main():
    parser = argparse.ArgumentParser(
        description="Raymarine Protobuf Decoder",
        formatter_class=argparse.RawDescriptionHelpFormatter,
        epilog="""
Group presets:
  primary  - Only 226.192.206.102:2565 (all sensor data from Data Master)
  minimal  - Primary + 239.2.1.1:2154 (backup for tank/engine data)
  all      - All 7 multicast groups (original behavior, high CPU)

Examples:
  %(prog)s -i 198.18.5.5 --groups primary    # Test with single group
  %(prog)s -i 198.18.5.5 --groups minimal    # Test with 2 groups
  %(prog)s -i 198.18.5.5 --groups all        # Use all groups (default)
"""
    )
    parser.add_argument('-i', '--interface', help='Interface IP for multicast')
    parser.add_argument('--pcap', help='Read from pcap file')
    parser.add_argument('--json', action='store_true', help='JSON output')
    parser.add_argument('-v', '--verbose', action='store_true', help='Verbose output')
    parser.add_argument('-g', '--groups', choices=['primary', 'minimal', 'all'],
                        default='all', help='Multicast group preset (default: all)')
    args = parser.parse_args()

    if not args.pcap and not args.interface:
        parser.error("Either --interface or --pcap is required")

    # Get selected multicast groups
    selected_groups = GROUP_PRESETS[args.groups]
    print(f"Using group preset: {args.groups} ({len(selected_groups)} groups)")

    sensor_data = SensorData()
    decoder = RaymarineDecoder(sensor_data, verbose=args.verbose)

    if args.pcap:
        print(f"Reading {args.pcap}...")
        packets = read_pcap(args.pcap)
        print(f"Processing {len(packets)} packets...\n")

        for pkt in packets:
            decoder.decode_packet(pkt)

        print("Final state:")
        print(json.dumps(sensor_data.to_dict(), indent=2))

    else:
        listener = MulticastListener(decoder, args.interface, groups=selected_groups)
        try:
            listener.start()
            while True:
                if args.json:
                    print(json.dumps(sensor_data.to_dict()))
                else:
                    display_dashboard(sensor_data)
                time.sleep(0.5)
        except KeyboardInterrupt:
            print("\nStopping...")
        finally:
            listener.stop()
            print("\nFinal state:")
            print(json.dumps(sensor_data.to_dict(), indent=2))

            # Print final group statistics summary
            print("\n" + "=" * 60)
            print("GROUP STATISTICS SUMMARY")
            print("=" * 60)
            with sensor_data._lock:
                for group_key, gstats in sorted(sensor_data.group_stats.items()):
                    fields_str = ", ".join(sorted(gstats["fields"])) if gstats["fields"] else "none"
                    print(f"\n{group_key}:")
                    print(f"  Packets received: {gstats['packets']}")
                    print(f"  Packets decoded:  {gstats['decoded']}")
                    print(f"  Data types found: {fields_str}")


if __name__ == "__main__":
    main()