"""jlink_gimbal.py — JLINK binary frame codec for gimbal commands (Issue #548).

<<<<<<< HEAD
Matches the JLINK protocol defined in include/jlink.h (Issue #547 ESP32 side).

Command type (Jetson → ESP32 BALANCE):
=======
Matches the JLINK protocol defined in include/jlink.h (Issue #547 ESP32-S3 side).

Command type (Jetson → ESP32-S3):
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
  0x0B  GIMBAL_POS  — int16 pan_x10 + int16 tilt_x10 + uint16 speed   (6 bytes)
                       pan_x10  = pan_deg  * 10  (±1500 for ±150°)
                       tilt_x10 = tilt_deg * 10  (±450  for ±45°)
                       speed    = servo speed register 0–4095 (0 = max)

<<<<<<< HEAD
Telemetry type (ESP32 BALANCE → Jetson):
=======
Telemetry type (ESP32-S3 → Jetson):
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
  0x84  GIMBAL_STATE — int16 pan_x10 + int16 tilt_x10 +
                        uint16 pan_speed_raw + uint16 tilt_speed_raw +
                        uint8 torque_en + uint8 rx_err_pct             (10 bytes)

Frame format (shared with esp32_protocol.py):
  [STX=0x02][CMD][LEN][PAYLOAD...][CRC16_hi][CRC16_lo][ETX=0x03]
  CRC16-CCITT: poly=0x1021, init=0xFFFF, covers CMD+LEN+PAYLOAD bytes.
"""

from __future__ import annotations

import struct
from dataclasses import dataclass
from typing import Optional

# ── Frame constants ────────────────────────────────────────────────────────────

STX = 0x02
ETX = 0x03

# ── Command / telemetry type codes ─────────────────────────────────────────────

<<<<<<< HEAD
CMD_GIMBAL_POS   = 0x0B   # Jetson → ESP32 BALANCE: set pan/tilt target
TLM_GIMBAL_STATE = 0x84   # ESP32 BALANCE → Jetson: measured state
=======
CMD_GIMBAL_POS   = 0x0B   # Jetson → ESP32-S3: set pan/tilt target
TLM_GIMBAL_STATE = 0x84   # ESP32-S3 → Jetson: measured state
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)

# Speed register: 0 = maximum servo speed; 4095 = slowest non-zero speed.
# Map deg/s to this register: speed_reg = max(0, 4095 - int(deg_s * 4095 / 360))
_MAX_SPEED_DEGS = 360.0   # degrees/sec at speed_reg=0


# ── Parsed telemetry ───────────────────────────────────────────────────────────

@dataclass
class GimbalStateFrame:
    pan_deg:        float   # measured pan position (degrees, + = right)
    tilt_deg:       float   # measured tilt position (degrees, + = up)
    pan_speed_raw:  int     # pan servo present-speed register value
    tilt_speed_raw: int     # tilt servo present-speed register value
    torque_en:      bool    # True = servo torque is on
    rx_err_pct:     int     # SCS bus error rate 0–100 %


# ── CRC16-CCITT ────────────────────────────────────────────────────────────────

def _crc16_ccitt(data: bytes, init: int = 0xFFFF) -> int:
    """CRC16-CCITT: poly=0x1021, init=0xFFFF, no final XOR."""
    crc = init
    for byte in data:
        crc ^= byte << 8
        for _ in range(8):
            crc = ((crc << 1) ^ 0x1021) if (crc & 0x8000) else (crc << 1)
        crc &= 0xFFFF
    return crc


# ── Frame builder ──────────────────────────────────────────────────────────────

def _build_frame(cmd: int, payload: bytes) -> bytes:
    length = len(payload)
    header = bytes([cmd, length])
    crc = _crc16_ccitt(header + payload)
    return bytes([STX, cmd, length]) + payload + struct.pack(">H", crc) + bytes([ETX])


# ── Speed conversion ───────────────────────────────────────────────────────────

def degs_to_speed_reg(deg_s: float) -> int:
    """Convert degrees/second to servo speed register value.

    speed_reg=0 is maximum speed; speed_reg=4095 is the slowest non-zero.
    Linear mapping: 0 deg/s → 0 (max), 360 deg/s → 0, clamped.
    """
    if deg_s <= 0.0:
        return 0  # max speed
    ratio = min(1.0, deg_s / _MAX_SPEED_DEGS)
    return max(0, min(4095, int((1.0 - ratio) * 4095)))


def speed_reg_to_degs(reg: int) -> float:
    """Convert speed register to approximate degrees/second."""
    if reg == 0:
        return _MAX_SPEED_DEGS
    return (1.0 - reg / 4095.0) * _MAX_SPEED_DEGS


# ── Encoder ────────────────────────────────────────────────────────────────────

def encode_gimbal_pos(pan_deg: float, tilt_deg: float, speed_deg_s: float) -> bytes:
    """Build GIMBAL_POS frame.

    Args:
        pan_deg:     Pan angle (degrees, clamped to ±327°).
        tilt_deg:    Tilt angle (degrees, clamped to ±327°).
        speed_deg_s: Motion speed (degrees/second; 0 = max).

    Returns:
        Complete JLINK binary frame (10 bytes total).
    """
    pan_x10  = max(-32767, min(32767, int(round(pan_deg  * 10))))
    tilt_x10 = max(-32767, min(32767, int(round(tilt_deg * 10))))
    speed    = degs_to_speed_reg(speed_deg_s)
    return _build_frame(CMD_GIMBAL_POS, struct.pack(">hhH", pan_x10, tilt_x10, speed))


def encode_gimbal_home() -> bytes:
    """Build GIMBAL_POS frame targeting (0°, 0°) at default speed."""
    return encode_gimbal_pos(0.0, 0.0, 0.0)  # speed=0 → max speed for homing


# ── Decoder ────────────────────────────────────────────────────────────────────

def decode_gimbal_state(payload: bytes) -> Optional[GimbalStateFrame]:
    """Decode GIMBAL_STATE telemetry payload (10 bytes).

    Returns:
        GimbalStateFrame on success, None if payload is too short.
    """
    if len(payload) < 10:
        return None
    pan_x10, tilt_x10, pan_spd, tilt_spd, torque_en, rx_err_pct = (
        struct.unpack_from(">hhHHBB", payload)
    )
    return GimbalStateFrame(
        pan_deg=pan_x10 / 10.0,
        tilt_deg=tilt_x10 / 10.0,
        pan_speed_raw=pan_spd,
        tilt_speed_raw=tilt_spd,
        torque_en=bool(torque_en),
        rx_err_pct=int(rx_err_pct),
    )
