"""Parse RoboSense H32 DIFOP channel calibration angles."""

from __future__ import annotations

from dataclasses import dataclass

import numpy as np

CHANNELS = 32
VERTICAL_START = 468
HORIZONTAL_START = 564


@dataclass(frozen=True)
class DifopAngles:
    vertical_deg: np.ndarray  # (32,)
    horizontal_deg: np.ndarray  # (32,)


def _read_u16_be(packet: bytes, index: int) -> int:
    return (packet[index] << 8) | packet[index + 1]


def signed_angle_deg(packet: bytes, index: int) -> float:
    """Match RSLidarH32 plugin SignedAngle: sign byte + BE u16 * 0.01 deg."""

    sign = -1.0 if packet[index] > 0 else 1.0
    return sign * _read_u16_be(packet, index + 1) * 0.01


def parse_difop_angles(packet: bytes) -> DifopAngles:
    needed = HORIZONTAL_START + CHANNELS * 3
    if len(packet) < needed:
        raise ValueError(f"DIFOP packet too short: {len(packet)} < {needed}")
    vertical = np.empty(CHANNELS, dtype=np.float64)
    horizontal = np.empty(CHANNELS, dtype=np.float64)
    for channel in range(CHANNELS):
        vertical[channel] = signed_angle_deg(packet, VERTICAL_START + channel * 3)
        horizontal[channel] = signed_angle_deg(packet, HORIZONTAL_START + channel * 3)
    return DifopAngles(vertical_deg=vertical, horizontal_deg=horizontal)