"""Unit tests for H32 Medulla dlog → V1 export helpers."""

from __future__ import annotations

import struct
from pathlib import Path

import numpy as np

from tools.h32_dlog.difop import CHANNELS, HORIZONTAL_START, VERTICAL_START, parse_difop_angles
from tools.h32_dlog.dobject import discover_records, iter_payloads, resolve_dlog_root
from tools.h32_dlog.load_session import load_h32_dlog_lidar
from tools.h32_dlog.payload_v1 import (
    MsopPacketItem,
    build_difop_payload,
    build_msop_batch_payload,
    parse_difop_payload,
    parse_msop_batch_payload,
)
from tools.rscap_v2.h32_msop import PACKET_LENGTH, iter_h32_frames_from_packets


def _make_msop_packet(*, seconds: int = 100, microseconds: int = 5000, az_deg: float = 10.0) -> bytes:
    packet = bytearray(PACKET_LENGTH)
    packet[17] = 1
    packet[20:26] = int(seconds).to_bytes(6, "big")
    packet[26:30] = int(microseconds).to_bytes(4, "big")
    az_raw = int(round(az_deg * 100))
    for block in range(12):
        offset = 42 + block * 100
        packet[offset] = 255
        packet[offset + 1] = 238
        packet[offset + 2] = (az_raw >> 8) & 0xFF
        packet[offset + 3] = az_raw & 0xFF
        idx = offset + 4
        for _ch in range(CHANNELS):
            packet[idx] = (1600 >> 8) & 0xFF
            packet[idx + 1] = 1600 & 0xFF
            packet[idx + 2] = 10
            idx += 3
    return bytes(packet)


def _write_signed_angle(buf: bytearray, index: int, degrees: float) -> None:
    sign = 1 if degrees < 0 else 0
    raw = int(round(abs(degrees) * 100))
    buf[index] = sign
    buf[index + 1] = (raw >> 8) & 0xFF
    buf[index + 2] = raw & 0xFF


def _make_difop_packet(*, vertical: list[float], horizontal: list[float] | None = None) -> bytes:
    packet = bytearray(1248)
    horiz = horizontal if horizontal is not None else [0.0] * CHANNELS
    for channel, angle in enumerate(vertical):
        _write_signed_angle(packet, VERTICAL_START + channel * 3, angle)
    for channel, angle in enumerate(horiz):
        _write_signed_angle(packet, HORIZONTAL_START + channel * 3, angle)
    return bytes(packet)


def _write_dorec_record(
    path: Path,
    *,
    object_name: str,
    ticks: int,
    record_id: str,
    payload: bytes,
    offset: int = 0,
) -> int:
    """Append one DObject record; return file offset of the record start."""

    path.parent.mkdir(parents=True, exist_ok=True)
    name_b = object_name.encode("ascii")
    id_b = record_id.encode("ascii")
    blob = (
        bytes([len(name_b)])
        + name_b
        + struct.pack("<q", ticks)
        + bytes([len(id_b)])
        + id_b
        + struct.pack("<i", len(payload))
        + payload
    )
    with path.open("ab" if path.exists() else "wb") as handle:
        if offset:
            handle.seek(offset)
        start = handle.tell()
        handle.write(blob)
    return start


def test_parse_msop_and_difop_payload_roundtrip():
    packet = _make_msop_packet(seconds=1700000000, microseconds=123456)
    item = MsopPacketItem(
        sequence=7,
        device_timestamp_us=1700000000 * 1_000_000 + 123456,
        device_timestamp_valid=True,
        host_receive_utc_ticks=111,
        host_receive_monotonic_ticks=222,
        raw=packet,
    )
    msop_payload = build_msop_batch_payload(packets=[item], session_id="sess-a")
    batch = parse_msop_batch_payload(msop_payload)
    assert batch.session_id == "sess-a"
    assert len(batch.packets) == 1
    assert batch.packets[0].sequence == 7
    assert batch.packets[0].device_timestamp_us == item.device_timestamp_us
    assert batch.packets[0].device_timestamp_valid is True
    assert batch.packets[0].raw == packet

    vertical = [-16.0 + i * (32.0 / 31) for i in range(CHANNELS)]
    difop_raw = _make_difop_packet(vertical=vertical, horizontal=[0.05] * CHANNELS)
    difop_payload = build_difop_payload(raw=difop_raw, sequence=3)
    difop = parse_difop_payload(difop_payload)
    assert difop.sequence == 3
    angles = parse_difop_angles(difop.raw)
    assert angles.vertical_deg.shape == (CHANNELS,)
    assert np.allclose(angles.vertical_deg, vertical, atol=1e-2)
    assert np.allclose(angles.horizontal_deg, 0.05, atol=1e-2)


def test_difop_signed_angle_negative():
    packet = _make_difop_packet(vertical=[-5.25] + [0.0] * 31)
    angles = parse_difop_angles(packet)
    assert abs(angles.vertical_deg[0] + 5.25) < 1e-9


def test_load_h32_dlog_lidar_mini_session(tmp_path: Path):
    dlog = tmp_path / "session" / "dlog"
    dorec_name = "raw.dorec"
    dorec_path = dlog / "dobject_recording" / dorec_name
    log_path = dlog / "dobject" / "rec.log"

    vertical = [-16.0 + i * (32.0 / 31) for i in range(CHANNELS)]
    difop_payload = build_difop_payload(raw=_make_difop_packet(vertical=vertical), sequence=1)
    msop_packet = _make_msop_packet(az_deg=15.0)
    msop_payload = build_msop_batch_payload(
        packets=[
            MsopPacketItem(
                sequence=1,
                device_timestamp_us=100_000_000,
                device_timestamp_valid=True,
                host_receive_utc_ticks=1,
                host_receive_monotonic_ticks=2,
                raw=msop_packet,
            )
        ]
    )

    off_difop = _write_dorec_record(
        dorec_path,
        object_name="frontlidar-difop-raw",
        ticks=1000,
        record_id="AA",
        payload=difop_payload,
    )
    off_msop = _write_dorec_record(
        dorec_path,
        object_name="frontlidar-msop-raw",
        ticks=1001,
        record_id="BB",
        payload=msop_payload,
    )

    log_path.parent.mkdir(parents=True, exist_ok=True)
    log_path.write_text(
        "\n".join(
            [
                f"[t] DObject `frontlidar-difop-raw` post len={len(difop_payload)}B, "
                f"id:AA, tic:1000, @{dorec_name}:{off_difop}",
                f"[t] DObject `frontlidar-msop-raw` post len={len(msop_payload)}B, "
                f"id:BB, tic:1001, @{dorec_name}:{off_msop}",
            ]
        )
        + "\n",
        encoding="utf-8",
    )

    root = resolve_dlog_root(tmp_path / "session")
    assert root == dlog
    assert len(discover_records(root, "frontlidar-msop-raw")) == 1
    payloads = list(iter_payloads(root, "frontlidar-msop-raw"))
    assert len(payloads) == 1

    session = load_h32_dlog_lidar(tmp_path / "session", require_difop=True)
    assert session.angle_source == "difop_channel_angles"
    assert len(session.msop_packets) == 1
    assert np.allclose(session.vertical_deg, vertical, atol=1e-2)

    frames = iter_h32_frames_from_packets(
        session.msop_packets,
        min_frame_points=1,
        vertical_deg=session.vertical_deg,
        horizontal_deg=session.horizontal_deg,
    )
    assert len(frames) == 1
    assert frames[0].points_xyz.shape[0] > 0