calibration/tools/h32_dlog/payload_v1.py

"""Parse RSLidarH32_3D_DLogCaptureNet48 raw MSOP/DIFOP DObject payloads."""

from __future__ import annotations

import io
import struct
from dataclasses import dataclass

from .dotnet_bin import read_bool, read_dotnet_string, read_i32, read_i64

MSOP_MAGIC = "RSLIDAR_H32_MSOP_DLOG_V1"
DIFOP_MAGIC = "RSLIDAR_H32_DIFOP_DLOG_V1"


@dataclass(frozen=True)
class MsopPacketItem:
    sequence: int
    device_timestamp_us: int
    device_timestamp_valid: bool
    host_receive_utc_ticks: int
    host_receive_monotonic_ticks: int
    raw: bytes


@dataclass(frozen=True)
class MsopBatch:
    version: int
    session_id: str
    session_start_utc_ticks: int
    session_start_monotonic_ticks: int
    monotonic_frequency: int
    lidar_ip: str
    msop_port: int
    packets: list[MsopPacketItem]


@dataclass(frozen=True)
class DifopRecord:
    version: int
    session_id: str
    session_start_utc_ticks: int
    session_start_monotonic_ticks: int
    monotonic_frequency: int
    lidar_ip: str
    difop_port: int
    sequence: int
    host_receive_utc_ticks: int
    host_receive_monotonic_ticks: int
    raw: bytes


def _read_bytes(stream: io.BytesIO, length: int) -> bytes:
    if length < 0 or length > 64 * 1024 * 1024:
        raise ValueError(f"invalid byte length: {length}")
    raw = stream.read(length)
    if len(raw) != length:
        raise EOFError(f"expected {length} bytes, got {len(raw)}")
    return raw


def parse_msop_batch_payload(payload: bytes) -> MsopBatch:
    stream = io.BytesIO(payload)
    magic = read_dotnet_string(stream)
    if magic != MSOP_MAGIC:
        raise ValueError(f"unexpected MSOP payload magic: {magic!r}")
    version = read_i32(stream)
    session_id = read_dotnet_string(stream)
    session_start_utc_ticks = read_i64(stream)
    session_start_monotonic_ticks = read_i64(stream)
    monotonic_frequency = read_i64(stream)
    lidar_ip = read_dotnet_string(stream)
    msop_port = read_i32(stream)
    packet_count = read_i32(stream)
    if packet_count < 0 or packet_count > 100_000:
        raise ValueError(f"invalid MSOP packet count: {packet_count}")
    packets: list[MsopPacketItem] = []
    for _ in range(packet_count):
        packets.append(
            MsopPacketItem(
                sequence=read_i64(stream),
                device_timestamp_us=read_i64(stream),
                device_timestamp_valid=read_bool(stream),
                host_receive_utc_ticks=read_i64(stream),
                host_receive_monotonic_ticks=read_i64(stream),
                raw=_read_bytes(stream, read_i32(stream)),
            )
        )
    return MsopBatch(
        version=version,
        session_id=session_id,
        session_start_utc_ticks=session_start_utc_ticks,
        session_start_monotonic_ticks=session_start_monotonic_ticks,
        monotonic_frequency=monotonic_frequency,
        lidar_ip=lidar_ip,
        msop_port=msop_port,
        packets=packets,
    )


def parse_difop_payload(payload: bytes) -> DifopRecord:
    stream = io.BytesIO(payload)
    magic = read_dotnet_string(stream)
    if magic != DIFOP_MAGIC:
        raise ValueError(f"unexpected DIFOP payload magic: {magic!r}")
    version = read_i32(stream)
    session_id = read_dotnet_string(stream)
    session_start_utc_ticks = read_i64(stream)
    session_start_monotonic_ticks = read_i64(stream)
    monotonic_frequency = read_i64(stream)
    lidar_ip = read_dotnet_string(stream)
    difop_port = read_i32(stream)
    sequence = read_i64(stream)
    host_receive_utc_ticks = read_i64(stream)
    host_receive_monotonic_ticks = read_i64(stream)
    raw = _read_bytes(stream, read_i32(stream))
    return DifopRecord(
        version=version,
        session_id=session_id,
        session_start_utc_ticks=session_start_utc_ticks,
        session_start_monotonic_ticks=session_start_monotonic_ticks,
        monotonic_frequency=monotonic_frequency,
        lidar_ip=lidar_ip,
        difop_port=difop_port,
        sequence=sequence,
        host_receive_utc_ticks=host_receive_utc_ticks,
        host_receive_monotonic_ticks=host_receive_monotonic_ticks,
        raw=raw,
    )


def build_msop_batch_payload(
    *,
    version: int = 1,
    session_id: str = "test",
    session_start_utc_ticks: int = 0,
    session_start_monotonic_ticks: int = 0,
    monotonic_frequency: int = 10_000_000,
    lidar_ip: str = "192.168.1.200",
    msop_port: int = 6699,
    packets: list[MsopPacketItem],
) -> bytes:
    """Test helper: write an MSOP batch matching the C# BinaryWriter layout."""

    from .dotnet_bin import write_bool, write_dotnet_string

    stream = io.BytesIO()
    write_dotnet_string(stream, MSOP_MAGIC)
    stream.write(struct.pack("<i", version))
    write_dotnet_string(stream, session_id)
    stream.write(struct.pack("<qqq", session_start_utc_ticks, session_start_monotonic_ticks, monotonic_frequency))
    write_dotnet_string(stream, lidar_ip)
    stream.write(struct.pack("<i", msop_port))
    stream.write(struct.pack("<i", len(packets)))
    for item in packets:
        stream.write(struct.pack("<qq", item.sequence, item.device_timestamp_us))
        write_bool(stream, item.device_timestamp_valid)
        stream.write(
            struct.pack(
                "<qqi",
                item.host_receive_utc_ticks,
                item.host_receive_monotonic_ticks,
                len(item.raw),
            )
        )
        stream.write(item.raw)
    return stream.getvalue()


def build_difop_payload(
    *,
    version: int = 1,
    session_id: str = "test",
    session_start_utc_ticks: int = 0,
    session_start_monotonic_ticks: int = 0,
    monotonic_frequency: int = 10_000_000,
    lidar_ip: str = "192.168.1.200",
    difop_port: int = 7788,
    sequence: int = 1,
    host_receive_utc_ticks: int = 0,
    host_receive_monotonic_ticks: int = 0,
    raw: bytes,
) -> bytes:
    """Test helper: write a DIFOP record matching the C# BinaryWriter layout."""

    from .dotnet_bin import write_dotnet_string

    stream = io.BytesIO()
    write_dotnet_string(stream, DIFOP_MAGIC)
    stream.write(struct.pack("<i", version))
    write_dotnet_string(stream, session_id)
    stream.write(struct.pack("<qqq", session_start_utc_ticks, session_start_monotonic_ticks, monotonic_frequency))
    write_dotnet_string(stream, lidar_ip)
    stream.write(struct.pack("<i", difop_port))
    stream.write(
        struct.pack(
            "<qqqi",
            sequence,
            host_receive_utc_ticks,
            host_receive_monotonic_ticks,
            len(raw),
        )
    )
    stream.write(raw)
    return stream.getvalue()