"""
packet.py — ESP-NOW packet parser for SaltyBot UWB tag relay (Issue #618)

The tag firmware broadcasts 20-byte ESP-NOW packets over the air.
An ESP32 receiver (acting as a WiFi sniffer or paired ESP-NOW node)
forwards each received packet verbatim over USB serial as a SLIP-framed
or length-prefixed binary blob.

Serial framing (receiver firmware convention)
─────────────────────────────────────────────
  STX  0x02        (1 byte, start of frame)
  LEN  0x14 (=20)  (1 byte, payload length — always 20 for this protocol)
  DATA [20 bytes]  (raw EspNowPacket)
  CRC  0x00        (1 byte, XOR of DATA bytes, simple integrity check)

  → Total wire bytes per frame: 23

Packet layout (EspNowPacket, 20 bytes, little-endian)
──────────────────────────────────────────────────────
  [0-1]  magic       0x5B 0x01
  [2]    tag_id      uint8
  [3]    msg_type    0x10=RANGE, 0x20=ESTOP, 0x30=HEARTBEAT
  [4]    anchor_id   uint8
  [5-8]  range_mm    int32 LE
  [9-12] rssi_dbm    float32 LE
  [13-16] timestamp_ms uint32 LE  (ESP32 millis())
  [17]   battery_pct uint8  (0-100, or 0xFF = unknown)
  [18]   flags       uint8  bit0 = estop_active
  [19]   seq_num     uint8  (rolling)
"""

from __future__ import annotations

import struct
from dataclasses import dataclass
from typing import Optional

MAGIC_0 = 0x5B
MAGIC_1 = 0x01

MSG_RANGE     = 0x10
MSG_ESTOP     = 0x20
MSG_HEARTBEAT = 0x30

PACKET_LEN = 20

_FRAME_STX = 0x02
_FRAME_LEN_BYTE = PACKET_LEN   # always 20

FLAG_ESTOP_ACTIVE = 0x01

_FMT = "<2BBI f I BBB x"  # x = 1-byte pad (matches struct alignment)
# Breakdown:
#   2B  = magic[2]
#   B   = tag_id
#   B   = msg_type
#   B   = anchor_id  ← wait, need to account for actual layout carefully

# Use explicit offset-based unpacking to be safe:
#   [0]   magic[0]      B
#   [1]   magic[1]      B
#   [2]   tag_id        B
#   [3]   msg_type      B
#   [4]   anchor_id     B
#   [5-8] range_mm      i  (signed 32-bit LE)
#   [9-12]rssi_dbm      f  (float32 LE)
#   [13-16]timestamp_ms I  (uint32 LE)
#   [17]  battery_pct   B
#   [18]  flags         B
#   [19]  seq_num       B

_STRUCT = struct.Struct("<BBBBBifIBBB")  # 1+1+1+1+1+4+4+4+1+1+1 = 20 ✓
assert _STRUCT.size == 20, f"struct size mismatch: {_STRUCT.size}"


@dataclass
class EspNowPacket:
    tag_id:       int
    msg_type:     int        # MSG_RANGE, MSG_ESTOP, MSG_HEARTBEAT
    anchor_id:    int
    range_mm:     int        # signed, valid only for MSG_RANGE
    rssi_dbm:     float      # valid for MSG_RANGE
    timestamp_ms: int        # ESP32 millis() at send time
    battery_pct:  int        # 0-100 or 255 (unknown)
    flags:        int        # bit0 = estop_active
    seq_num:      int

    @property
    def estop_active(self) -> bool:
        return bool(self.flags & FLAG_ESTOP_ACTIVE)

    @staticmethod
    def from_bytes(data: bytes) -> "EspNowPacket":
        if len(data) != PACKET_LEN:
            raise ValueError(f"Expected {PACKET_LEN} bytes, got {len(data)}")
        m0, m1, tag_id, msg_type, anchor_id, range_mm, rssi_dbm, \
            timestamp_ms, battery_pct, flags, seq_num = _STRUCT.unpack(data)
        if m0 != MAGIC_0 or m1 != MAGIC_1:
            raise ValueError(
                f"Bad magic: 0x{m0:02X} 0x{m1:02X} (expected 0x5B 0x01)"
            )
        return EspNowPacket(
            tag_id=tag_id,
            msg_type=msg_type,
            anchor_id=anchor_id,
            range_mm=range_mm,
            rssi_dbm=rssi_dbm,
            timestamp_ms=timestamp_ms,
            battery_pct=battery_pct,
            flags=flags,
            seq_num=seq_num,
        )


class FrameReader:
    """
    Stateful framing decoder for the serial stream from the ESP32 receiver.

    Framing: STX(0x02) + LEN(0x14) + DATA(20 bytes) + XOR-CRC(1 byte)
    Yields EspNowPacket objects for each valid, CRC-passing frame.
    Invalid bytes before STX are silently discarded (sync recovery).
    """

    _STATE_HUNT  = 0
    _STATE_LEN   = 1
    _STATE_DATA  = 2
    _STATE_CRC   = 3

    def __init__(self) -> None:
        self._state = self._STATE_HUNT
        self._buf: bytearray = bytearray()

    def feed(self, data: bytes) -> list[EspNowPacket]:
        """Feed raw bytes; return list of parsed packets (may be empty)."""
        packets: list[EspNowPacket] = []
        for byte in data:
            pkt = self._step(byte)
            if pkt is not None:
                packets.append(pkt)
        return packets

    def _step(self, byte: int) -> Optional[EspNowPacket]:
        if self._state == self._STATE_HUNT:
            if byte == _FRAME_STX:
                self._state = self._STATE_LEN
            return None

        if self._state == self._STATE_LEN:
            if byte == _FRAME_LEN_BYTE:
                self._buf = bytearray()
                self._state = self._STATE_DATA
            else:
                self._state = self._STATE_HUNT   # unexpected length — re-hunt
            return None

        if self._state == self._STATE_DATA:
            self._buf.append(byte)
            if len(self._buf) == PACKET_LEN:
                self._state = self._STATE_CRC
            return None

        if self._state == self._STATE_CRC:
            self._state = self._STATE_HUNT
            crc = 0
            for b in self._buf:
                crc ^= b
            if crc != byte:
                return None   # CRC fail — drop frame
            try:
                return EspNowPacket.from_bytes(bytes(self._buf))
            except ValueError:
                return None

        return None  # unreachable
