venus/axiom-nmea/debug/packet_debug.py

#!/usr/bin/env python3
"""
Raymarine Packet Debug Tool

Dumps raw protobuf field structure from Raymarine multicast packets.
Use this to discover field locations for COG, SOG, and other data.

Usage:
    python packet_debug.py -i 198.18.5.5

The tool shows:
- All top-level protobuf fields and their wire types
- Nested field structures with decoded values
- Float/double interpretations for potential navigation data
"""

import argparse
import logging
import os
import signal
import socket
import struct
import sys
import time
from datetime import datetime
from typing import Dict, Any, Optional, List

# Add parent directory to path for library import
sys.path.insert(0, os.path.dirname(os.path.dirname(os.path.abspath(__file__))))

from raymarine_nmea.protocol.parser import ProtobufParser, ProtoField
from raymarine_nmea.protocol.constants import (
    WIRE_VARINT, WIRE_FIXED64, WIRE_LENGTH, WIRE_FIXED32,
    HEADER_SIZE, RAD_TO_DEG, MS_TO_KTS,
)
from raymarine_nmea.sensors import MULTICAST_GROUPS

# Known field names for reference
FIELD_NAMES = {
    1: "DEVICE_INFO",
    2: "GPS_POSITION",
    3: "HEADING",
    7: "DEPTH",
    13: "WIND_NAVIGATION",
    14: "ENGINE_DATA",
    15: "TEMPERATURE",
    16: "TANK_DATA",
    20: "HOUSE_BATTERY",
}

WIRE_TYPE_NAMES = {
    0: "varint",
    1: "fixed64",
    2: "length",
    5: "fixed32",
}

running = True


def signal_handler(signum, frame):
    global running
    running = False


def decode_as_float(raw: bytes) -> Optional[float]:
    """Try to decode bytes as float."""
    if len(raw) == 4:
        try:
            val = struct.unpack('<f', raw)[0]
            if val == val:  # NaN check
                return val
        except struct.error:
            pass
    return None


def decode_as_double(raw: bytes) -> Optional[float]:
    """Try to decode bytes as double."""
    if len(raw) == 8:
        try:
            val = struct.unpack('<d', raw)[0]
            if val == val:  # NaN check
                return val
        except struct.error:
            pass
    return None


def format_value(pf: ProtoField, indent: int = 0) -> List[str]:
    """Format a protobuf field for display."""
    lines = []
    prefix = "  " * indent
    wire_name = WIRE_TYPE_NAMES.get(pf.wire_type, f"wire{pf.wire_type}")
    field_name = FIELD_NAMES.get(pf.field_num, "")
    if field_name:
        field_name = f" ({field_name})"

    if pf.wire_type == WIRE_VARINT:
        lines.append(f"{prefix}Field {pf.field_num}{field_name}: {pf.value} [{wire_name}]")

    elif pf.wire_type == WIRE_FIXED32:
        fval = decode_as_float(pf.value)
        hex_str = pf.value.hex()
        if fval is not None:
            # Show various interpretations
            deg = fval * RAD_TO_DEG if 0 <= fval <= 6.5 else None
            kts = fval * MS_TO_KTS if 0 <= fval <= 50 else None
            interp = []
            if deg is not None and 0 <= deg <= 360:
                interp.append(f"{deg:.1f}°")
            if kts is not None and 0 <= kts <= 100:
                interp.append(f"{kts:.1f}kts")
            interp_str = f" -> {', '.join(interp)}" if interp else ""
            lines.append(f"{prefix}Field {pf.field_num}{field_name}: {fval:.6f} [{wire_name}] 0x{hex_str}{interp_str}")
        else:
            lines.append(f"{prefix}Field {pf.field_num}{field_name}: 0x{hex_str} [{wire_name}]")

    elif pf.wire_type == WIRE_FIXED64:
        dval = decode_as_double(pf.value)
        hex_str = pf.value.hex()
        if dval is not None:
            lines.append(f"{prefix}Field {pf.field_num}{field_name}: {dval:.8f} [{wire_name}]")
        else:
            lines.append(f"{prefix}Field {pf.field_num}{field_name}: 0x{hex_str} [{wire_name}]")

    elif pf.wire_type == WIRE_LENGTH:
        data_len = len(pf.value) if isinstance(pf.value, bytes) else 0
        lines.append(f"{prefix}Field {pf.field_num}{field_name}: [{wire_name}, {data_len} bytes]")
        if pf.children:
            for child_num in sorted(pf.children.keys()):
                child = pf.children[child_num]
                lines.extend(format_value(child, indent + 1))
        elif data_len <= 32:
            lines.append(f"{prefix}  Raw: 0x{pf.value.hex()}")

    return lines


def dump_packet(packet: bytes, packet_num: int, filter_fields: Optional[set] = None):
    """Dump a single packet's protobuf structure."""
    if len(packet) < HEADER_SIZE + 10:
        return

    proto_data = packet[HEADER_SIZE:]
    parser = ProtobufParser(proto_data)

    # Collect repeated fields that we know about
    fields = parser.parse_message(collect_repeated={14, 16, 20})

    if not fields:
        return

    # Filter if requested
    if filter_fields:
        fields = {k: v for k, v in fields.items() if k in filter_fields}
        if not fields:
            return

    timestamp = datetime.now().strftime('%H:%M:%S.%f')[:-3]
    print(f"\n{'='*70}")
    print(f"Packet #{packet_num} at {timestamp} ({len(packet)} bytes)")
    print(f"{'='*70}")

    for field_num in sorted(fields.keys()):
        val = fields[field_num]
        # Handle repeated fields (stored as list)
        if isinstance(val, list):
            for i, pf in enumerate(val):
                lines = format_value(pf)
                for line in lines:
                    # Add index to first line for repeated fields
                    if lines.index(line) == 0:
                        print(f"{line} [{i}]")
                    else:
                        print(line)
        else:
            for line in format_value(val):
                print(line)


def main():
    global running

    parser = argparse.ArgumentParser(description="Debug Raymarine packet structure")
    parser.add_argument('-i', '--interface', required=True,
                        help='Interface IP for Raymarine multicast (e.g., 198.18.5.5)')
    parser.add_argument('-n', '--count', type=int, default=0,
                        help='Number of packets to capture (0 = unlimited)')
    parser.add_argument('-f', '--fields', type=str, default='',
                        help='Comma-separated field numbers to show (empty = all)')
    parser.add_argument('--interval', type=float, default=0.0,
                        help='Minimum interval between packets shown (seconds)')
    parser.add_argument('--nav-only', action='store_true',
                        help='Show only navigation-related fields (2,3,13)')

    args = parser.parse_args()

    # Parse field filter
    filter_fields = None
    if args.nav_only:
        filter_fields = {2, 3, 13}  # GPS, Heading, Wind/Nav
    elif args.fields:
        try:
            filter_fields = set(int(x.strip()) for x in args.fields.split(','))
        except ValueError:
            print("Error: --fields must be comma-separated integers")
            sys.exit(1)

    signal.signal(signal.SIGINT, signal_handler)
    signal.signal(signal.SIGTERM, signal_handler)

    # Create sockets for all multicast groups
    sockets = []
    for group, port in MULTICAST_GROUPS:
        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))

            # Join multicast group using struct.pack like MulticastListener does
            mreq = struct.pack("4s4s",
                socket.inet_aton(group),
                socket.inet_aton(args.interface)
            )
            sock.setsockopt(socket.IPPROTO_IP, socket.IP_ADD_MEMBERSHIP, mreq)
            sock.setblocking(False)
            sockets.append((sock, group, port))
            print(f"Joined {group}:{port}")
        except Exception as e:
            print(f"Error joining {group}:{port}: {e}")

    if not sockets:
        print("Error: Could not join any multicast groups")
        sys.exit(1)

    print(f"\nRaymarine Packet Debug Tool")
    print(f"Listening on {args.interface}")
    if filter_fields:
        print(f"Showing fields: {sorted(filter_fields)}")
    print(f"Press Ctrl+C to stop\n")

    print("Known field numbers:")
    for num, name in sorted(FIELD_NAMES.items()):
        print(f"  {num}: {name}")
    print()

    packet_count = 0
    last_dump = 0

    try:
        while running:
            # Poll all sockets
            for sock, group, port in sockets:
                try:
                    data, addr = sock.recvfrom(65535)
                    packet_count += 1

                    # Rate limiting
                    now = time.time()
                    if args.interval > 0 and (now - last_dump) < args.interval:
                        continue

                    dump_packet(data, packet_count, filter_fields)
                    last_dump = now

                    # Count limit
                    if args.count > 0 and packet_count >= args.count:
                        running = False
                        break

                except BlockingIOError:
                    continue
                except Exception as e:
                    if running:
                        print(f"Error on {group}:{port}: {e}")

            time.sleep(0.01)  # Small sleep to avoid busy-waiting

    finally:
        for sock, _, _ in sockets:
            sock.close()
        print(f"\n\nCaptured {packet_count} packets")


if __name__ == "__main__":
    main()