"""Unit tests for rscap → V1 export helpers (no large real captures)."""

from __future__ import annotations

import struct

import numpy as np

from tools.rscap_v2.h32_msop import (
    CHANNELS,
    PACKET_LENGTH,
    decode_packet_points,
    default_vertical_deg,
    default_horizontal_deg,
    device_timestamp_ms,
    normalize_azimuth_deg,
)
from tools.rscap_v2.n300_imu import crc8_fdilink, crc16_fdilink, iter_n300_imu_samples
from tools.rscap_v2.capture_format_v2 import CaptureFile, CaptureHeader, RawChunk


def _make_msop_packet(*, seconds: int = 100, microseconds: int = 5000, az_deg: float = 10.0) -> bytes:
    packet = bytearray(PACKET_LENGTH)
    packet[17] = 1  # 2.5 mm unit
    sec = seconds.to_bytes(6, "big")
    packet[20:26] = sec
    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):
            # 4.0 m at 2.5 mm/unit => raw = 1600
            packet[idx] = (1600 >> 8) & 0xFF
            packet[idx + 1] = 1600 & 0xFF
            packet[idx + 2] = 10
            idx += 3
    return bytes(packet)


def test_h32_device_timestamp_and_points():
    packet = _make_msop_packet(seconds=1700000000, microseconds=123000)
    assert device_timestamp_ms(packet) == 1700000000 * 1000 + 123
    az_list, pts = decode_packet_points(
        packet,
        default_vertical_deg(),
        default_horizontal_deg(),
        min_range_m=0.1,
        max_range_m=50.0,
    )
    assert len(az_list) == 12
    assert pts.shape[0] == 12 * CHANNELS
    assert np.allclose(np.linalg.norm(pts, axis=1), 4.0, atol=1e-3)


def test_normalize_azimuth():
    assert abs(normalize_azimuth_deg(190.0) + 170.0) < 1e-9


def _n300_imu_frame(device_us: int = 123456) -> bytes:
    payload = bytearray(56)
    struct.pack_into("<3f", payload, 0, 0.1, -0.2, 0.3)
    struct.pack_into("<3f", payload, 12, 0.0, 0.0, 9.81)
    struct.pack_into("<q", payload, 48, device_us)
    header = bytearray([0xFC, 0x40, 56, 7])
    header.append(crc8_fdilink(header))
    crc = crc16_fdilink(payload)
    frame = bytes(header) + crc.to_bytes(2, "big") + bytes(payload) + b"\xFD"
    return frame


def test_n300_imu_sample_from_capture_chunks():
    frame = _n300_imu_frame(654321)
    header = CaptureHeader(
        sensor_kind="wheeltec-n300",
        session_id="test",
        session_start_utc_ticks=0,
        session_start_monotonic_ticks=0,
        monotonic_frequency=10_000_000,
        port="COM1",
        baud=921600,
        file_start_utc_ticks=0,
    )
    chunk = RawChunk(
        sequence=1,
        receive_utc_ticks=100,
        receive_monotonic_ticks=1,
        raw=frame,
        record_file_offset=0,
        raw_file_offset=0,
        record_crc32=0,
        crc_valid=True,
    )
    capture = CaptureFile(path="mem", header=header, chunks=[chunk], footer=None)
    samples = iter_n300_imu_samples(capture)
    assert len(samples) == 1
    assert samples[0].device_timestamp_us == 654321
    assert abs(samples[0].t_s - 654321e-6) < 1e-12
    assert abs(samples[0].gyro_rad_s[0] - 0.1) < 1e-6
    assert abs(samples[0].accel_m_s2[2] - 9.81) < 1e-5