venus/axiom-nmea/debug/watch_field.py

#!/usr/bin/env python3
"""
Single Field Monitor - Watch a specific field across packets.

Usage:
    python3 watch_field.py -i 192.168.1.100 --field 7.1
    python3 watch_field.py --pcap capture.pcap --field 7.1
    python3 watch_field.py --pcap capture.pcap --field 13.4  # TWD
"""

import struct
import socket
import time
import argparse
import threading
from datetime import datetime
from typing import Dict, Any, Optional, List

WIRE_VARINT = 0
WIRE_FIXED64 = 1
WIRE_LENGTH = 2
WIRE_FIXED32 = 5

HEADER_SIZE = 20

MULTICAST_GROUPS = [
    ("226.192.206.102", 2565),
]


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

    def read_varint(self) -> int:
        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
        return result

    def parse(self) -> Dict[int, Any]:
        fields = {}
        while self.pos < len(self.data):
            try:
                tag = self.read_varint()
                field_num = tag >> 3
                wire_type = tag & 0x07
                if field_num == 0 or field_num > 1000:
                    break

                if wire_type == WIRE_VARINT:
                    value = self.read_varint()
                    children = None
                elif wire_type == WIRE_FIXED64:
                    value = self.data[self.pos:self.pos + 8]
                    self.pos += 8
                    children = None
                elif wire_type == WIRE_LENGTH:
                    length = self.read_varint()
                    value = self.data[self.pos:self.pos + length]
                    self.pos += length
                    try:
                        nested = ProtobufParser(value)
                        children = nested.parse()
                        if nested.pos < len(value) * 0.5:
                            children = None
                    except:
                        children = None
                elif wire_type == WIRE_FIXED32:
                    value = self.data[self.pos:self.pos + 4]
                    self.pos += 4
                    children = None
                else:
                    break

                fields[field_num] = (wire_type, value, children)
            except:
                break
        return fields


def get_field(fields: Dict, path: List[int]):
    """Navigate to a specific field path like [7, 1] for field 7.1"""
    current = fields
    for i, field_num in enumerate(path):
        if field_num not in current:
            return None, None, None
        wire_type, value, children = current[field_num]
        if i == len(path) - 1:
            return wire_type, value, children
        if children is None:
            return None, None, None
        current = children
    return None, None, None


def format_value(wire_type: int, value: Any) -> str:
    """Format value with multiple interpretations."""
    results = []

    if wire_type == WIRE_VARINT:
        results.append(f"int: {value}")

    elif wire_type == WIRE_FIXED64:
        try:
            d = struct.unpack('<d', value)[0]
            if d == d:  # Not NaN
                results.append(f"double: {d:.6f}")
                # Could be depth in feet
                if 0 < d < 1000:
                    results.append(f"  -> {d:.1f} ft = {d * 0.3048:.1f} m")
        except:
            pass
        results.append(f"hex: {value.hex()}")

    elif wire_type == WIRE_FIXED32:
        try:
            f = struct.unpack('<f', value)[0]
            if f == f:  # Not NaN
                results.append(f"float: {f:.4f}")
                if 0 <= f <= 6.5:
                    results.append(f"  -> as angle: {f * 57.2958:.1f}°")
                if 0 < f < 100:
                    results.append(f"  -> as m/s: {f * 1.94384:.1f} kts")
                if 0 < f < 1000:
                    results.append(f"  -> as depth: {f:.1f} ft = {f * 0.3048:.1f} m")
        except:
            pass
        results.append(f"hex: {value.hex()}")

    elif wire_type == WIRE_LENGTH:
        results.append(f"bytes[{len(value)}]: {value[:20].hex()}...")

    return " | ".join(results) if results else "?"


def read_pcap(filename: str):
    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((ts_sec + ts_usec / 1e6, pkt_data[payload_start:]))
    return packets


class LiveListener:
    def __init__(self, interface_ip: str):
        self.interface_ip = interface_ip
        self.running = False
        self.packets = []
        self.lock = threading.Lock()

    def _create_socket(self, group: str, port: int):
        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

    def _listen(self, sock):
        while self.running:
            try:
                data, _ = sock.recvfrom(65535)
                if len(data) >= 200:
                    with self.lock:
                        self.packets.append((time.time(), data))
                        # Keep last 100
                        if len(self.packets) > 100:
                            self.packets = self.packets[-100:]
            except socket.timeout:
                continue
            except:
                pass

    def start(self):
        self.running = True
        for group, port in MULTICAST_GROUPS:
            try:
                sock = self._create_socket(group, port)
                t = threading.Thread(target=self._listen, args=(sock,), daemon=True)
                t.start()
            except Exception as e:
                print(f"Error: {e}")

    def get_latest(self):
        with self.lock:
            if self.packets:
                return self.packets[-1]
        return None, None

    def stop(self):
        self.running = False


def main():
    parser = argparse.ArgumentParser(description="Watch a specific protobuf field")
    parser.add_argument('-i', '--interface', help='Interface IP for live capture')
    parser.add_argument('--pcap', help='Read from pcap file')
    parser.add_argument('-f', '--field', required=True, help='Field path like "7.1" or "13.4"')
    parser.add_argument('-n', '--count', type=int, default=20, help='Number of samples to show')
    parser.add_argument('-t', '--interval', type=float, default=1.0, help='Seconds between samples (live)')
    args = parser.parse_args()

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

    # Parse field path
    field_path = [int(x) for x in args.field.split('.')]
    field_str = '.'.join(str(x) for x in field_path)

    print(f"Watching Field {field_str}")
    print("=" * 80)

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

        count = 0
        for ts, pkt in packets:
            if len(pkt) < HEADER_SIZE + 20:
                continue

            proto_data = pkt[HEADER_SIZE:]
            parser = ProtobufParser(proto_data)
            fields = parser.parse()

            wire_type, value, children = get_field(fields, field_path)
            if wire_type is not None:
                timestamp = datetime.fromtimestamp(ts).strftime("%H:%M:%S.%f")[:-3]
                val_str = format_value(wire_type, value)
                print(f"[{timestamp}] {len(pkt):4d}B | Field {field_str}: {val_str}")
                count += 1
                if count >= args.count:
                    break

        if count == 0:
            print(f"Field {field_str} not found in any packets")

    else:
        listener = LiveListener(args.interface)
        listener.start()
        print(f"Listening... showing {args.count} samples\n")

        try:
            count = 0
            last_ts = 0
            while count < args.count:
                time.sleep(args.interval)
                ts, pkt = listener.get_latest()
                if pkt is None or ts == last_ts:
                    continue
                last_ts = ts

                proto_data = pkt[HEADER_SIZE:]
                parser = ProtobufParser(proto_data)
                fields = parser.parse()

                wire_type, value, children = get_field(fields, field_path)
                if wire_type is not None:
                    timestamp = datetime.fromtimestamp(ts).strftime("%H:%M:%S.%f")[:-3]
                    val_str = format_value(wire_type, value)
                    print(f"[{timestamp}] {len(pkt):4d}B | Field {field_str}: {val_str}")
                    count += 1
                else:
                    print(f"[{datetime.now().strftime('%H:%M:%S')}] Field {field_str} not found in {len(pkt)}B packet")

        except KeyboardInterrupt:
            print("\nStopped")
        finally:
            listener.stop()


if __name__ == "__main__":
    main()