"""
Unit tests for saltybot_uwb_espnow_relay.packet (Issue #618).
No ROS2 or hardware required.
"""

import struct
import sys
import os

import pytest

sys.path.insert(0, os.path.join(os.path.dirname(__file__), ".."))
from saltybot_uwb_espnow_relay.packet import (
    EspNowPacket, FrameReader,
    MAGIC_0, MAGIC_1, MSG_RANGE, MSG_ESTOP, MSG_HEARTBEAT,
    PACKET_LEN, _STRUCT,
)


# ── Helpers ────────────────────────────────────────────────────────────────

def _make_raw(
    tag_id=1, msg_type=MSG_RANGE, anchor_id=0,
    range_mm=1500, rssi=-75.0, timestamp_ms=12345,
    battery_pct=80, flags=0, seq_num=42
) -> bytes:
    """Build a valid 20-byte raw EspNowPacket."""
    return _STRUCT.pack(
        MAGIC_0, MAGIC_1,
        tag_id, msg_type, anchor_id,
        range_mm, rssi, timestamp_ms,
        battery_pct, flags, seq_num,
    )


def _frame(raw: bytes) -> bytes:
    """Wrap raw packet in STX+LEN+DATA+CRC framing."""
    crc = 0
    for b in raw:
        crc ^= b
    return bytes([0x02, len(raw)]) + raw + bytes([crc])


# ── EspNowPacket.from_bytes ────────────────────────────────────────────────

class TestFromBytes:
    def test_range_packet_parsed(self):
        raw = _make_raw(tag_id=1, msg_type=MSG_RANGE, anchor_id=0,
                        range_mm=2345, rssi=-68.5)
        pkt = EspNowPacket.from_bytes(raw)
        assert pkt.tag_id == 1
        assert pkt.msg_type == MSG_RANGE
        assert pkt.anchor_id == 0
        assert pkt.range_mm == 2345
        assert abs(pkt.rssi_dbm - (-68.5)) < 0.01

    def test_estop_active_flag(self):
        raw = _make_raw(msg_type=MSG_ESTOP, flags=0x01)
        pkt = EspNowPacket.from_bytes(raw)
        assert pkt.msg_type == MSG_ESTOP
        assert pkt.estop_active is True

    def test_estop_cleared_flag(self):
        raw = _make_raw(msg_type=MSG_ESTOP, flags=0x00)
        pkt = EspNowPacket.from_bytes(raw)
        assert pkt.estop_active is False

    def test_heartbeat_parsed(self):
        raw = _make_raw(msg_type=MSG_HEARTBEAT, battery_pct=55, seq_num=7)
        pkt = EspNowPacket.from_bytes(raw)
        assert pkt.msg_type == MSG_HEARTBEAT
        assert pkt.battery_pct == 55
        assert pkt.seq_num == 7

    def test_bad_magic_raises(self):
        raw = bytearray(_make_raw())
        raw[0] = 0xAA
        with pytest.raises(ValueError, match="magic"):
            EspNowPacket.from_bytes(bytes(raw))

    def test_wrong_length_raises(self):
        with pytest.raises(ValueError):
            EspNowPacket.from_bytes(b"\x00" * 10)

    def test_negative_range_mm(self):
        raw = _make_raw(range_mm=-50)
        pkt = EspNowPacket.from_bytes(raw)
        assert pkt.range_mm == -50

    def test_battery_unknown(self):
        raw = _make_raw(battery_pct=0xFF)
        pkt = EspNowPacket.from_bytes(raw)
        assert pkt.battery_pct == 255


# ── FrameReader ────────────────────────────────────────────────────────────

class TestFrameReader:
    def _make_frame(self, **kwargs) -> bytes:
        return _frame(_make_raw(**kwargs))

    def test_single_frame_decoded(self):
        r = FrameReader()
        frame = self._make_frame(range_mm=3000, anchor_id=1)
        pkts = r.feed(frame)
        assert len(pkts) == 1
        assert pkts[0].range_mm == 3000
        assert pkts[0].anchor_id == 1

    def test_two_consecutive_frames(self):
        r = FrameReader()
        f1 = self._make_frame(seq_num=1)
        f2 = self._make_frame(seq_num=2)
        pkts = r.feed(f1 + f2)
        assert len(pkts) == 2
        assert pkts[0].seq_num == 1
        assert pkts[1].seq_num == 2

    def test_garbage_before_stx_skipped(self):
        r = FrameReader()
        junk = bytes([0xDE, 0xAD, 0xBE, 0xEF, 0x99])
        frame = self._make_frame()
        pkts = r.feed(junk + frame)
        assert len(pkts) == 1

    def test_bad_crc_dropped(self):
        r = FrameReader()
        raw = _make_raw()
        crc = 0
        for b in raw:
            crc ^= b
        frame = bytes([0x02, 0x14]) + raw + bytes([crc ^ 0xFF])  # corrupt CRC
        pkts = r.feed(frame)
        assert len(pkts) == 0

    def test_wrong_len_byte_dropped(self):
        r = FrameReader()
        raw = _make_raw()
        frame = bytes([0x02, 0x10]) + raw  # wrong LEN
        pkts = r.feed(frame)
        assert len(pkts) == 0

    def test_split_feed_reassembles(self):
        """Feed the frame in two chunks — should still parse."""
        r = FrameReader()
        frame = self._make_frame(seq_num=99)
        half = len(frame) // 2
        pkts1 = r.feed(frame[:half])
        pkts2 = r.feed(frame[half:])
        assert len(pkts1) == 0
        assert len(pkts2) == 1
        assert pkts2[0].seq_num == 99

    def test_byte_by_byte_feed(self):
        """Feed one byte at a time."""
        r = FrameReader()
        frame = self._make_frame(tag_id=3)
        all_pkts = []
        for b in frame:
            all_pkts.extend(r.feed(bytes([b])))
        assert len(all_pkts) == 1
        assert all_pkts[0].tag_id == 3

    def test_recovery_after_truncated_frame(self):
        """Truncated frame followed by valid frame — valid one should parse."""
        r = FrameReader()
        truncated = bytes([0x02, 0x14]) + b"\x00" * 10   # no CRC
        good = self._make_frame(seq_num=77)
        pkts = r.feed(truncated + good)
        # Might or might not get 77 depending on CRC collision, but must not crash
        assert isinstance(pkts, list)


if __name__ == "__main__":
    pytest.main([__file__, "-v"])
