"""test_esp32_cmd_node.py — Unit tests for Stm32CmdNode with mock serial port.

Tests:
  - Serial open/close lifecycle
  - Twist→SPEED_STEER frame encoding and clamping
  - Heartbeat cadence (mock timer)
  - Watchdog fires after 500ms silence and sends zero-speed
  - ARM service request → ARM frame on serial
  - PID_UPDATE topic → PID_UPDATE frame on serial
  - Telemetry receive path: mock serial → publisher callbacks
  - Auto-reconnect on serial disconnect
  - Zero-speed sent on node shutdown
  - CRC errors counted correctly

Run with: pytest test/test_esp32_cmd_node.py -v
No ROS2 runtime required — uses mock Node infrastructure.
"""

from __future__ import annotations

import io
import struct
import threading
import time
import sys
import os
from unittest.mock import MagicMock, patch, call
import pytest

sys.path.insert(0, os.path.join(os.path.dirname(__file__), ".."))

from saltybot_bridge.esp32_protocol import (
    STX, ETX, CmdType, TelType,
    encode_speed_steer, encode_heartbeat, encode_arm, encode_pid_update,
    _build_frame, _crc16_ccitt,
    ImuFrame, BatteryFrame, ArmStateFrame, MotorRpmFrame, ErrorFrame,
)


# ── Mock serial ───────────────────────────────────────────────────────────────

class MockSerial:
    """In-memory serial port mock for testing TX/RX paths without hardware."""

    def __init__(self, rx_data: bytes = b""):
        self._tx_buf  = io.BytesIO()
        self._rx_buf  = io.BytesIO(rx_data)
        self.is_open  = True
        self._lock    = threading.Lock()
        self.write_calls  = 0
        self.raise_on_write: Exception | None = None

    def write(self, data: bytes) -> int:
        if self.raise_on_write:
            raise self.raise_on_write
        with self._lock:
            self._tx_buf.write(data)
            self.write_calls += 1
        return len(data)

    @property
    def in_waiting(self) -> int:
        pos = self._rx_buf.tell()
        end = self._rx_buf.seek(0, 2)
        self._rx_buf.seek(pos)
        return end - pos

    def read(self, n: int) -> bytes:
        return self._rx_buf.read(n)

    def reset_input_buffer(self) -> None:
        pass

    def close(self) -> None:
        self.is_open = False

    def get_written(self) -> bytes:
        with self._lock:
            return self._tx_buf.getvalue()

    def clear_written(self) -> None:
        with self._lock:
            self._tx_buf = io.BytesIO()


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

def _imu_frame_bytes(pitch=100, roll=0, yaw=0, ax=0, ay=0, az=981) -> bytes:
    payload = struct.pack(">hhhhhh", pitch, roll, yaw, ax, ay, az)
    return _build_frame(TelType.IMU, payload)


def _battery_frame_bytes(v_mv=11100, i_ma=500, soc=80) -> bytes:
    payload = struct.pack(">HhB", v_mv, i_ma, soc)
    return _build_frame(TelType.BATTERY, payload)


def _arm_state_frame_bytes(state=1, flags=0) -> bytes:
    payload = struct.pack("BB", state, flags)
    return _build_frame(TelType.ARM_STATE, payload)


def _motor_rpm_frame_bytes(left=1200, right=1200) -> bytes:
    payload = struct.pack(">hh", left, right)
    return _build_frame(TelType.MOTOR_RPM, payload)


def _error_frame_bytes(code=0x01, sub=0x00) -> bytes:
    payload = struct.pack("BB", code, sub)
    return _build_frame(TelType.ERROR, payload)


# ═══════════════════════════════════════════════════════════════════
# Protocol logic tests (no ROS2)
# ═══════════════════════════════════════════════════════════════════

class TestCommandEncoding:
    """Verify the TX path logic without spinning up ROS2."""

    def test_zero_twist_sends_zero_frame(self):
        frame = encode_speed_steer(0, 0)
        speed, steer = struct.unpack(">hh", frame[3:7])
        assert speed == 0
        assert steer == 0

    def test_forward_1ms_scales_to_1000(self):
        """linear.x = 1.0 m/s with speed_scale=1000 → speed=1000."""
        speed_scale = 1000.0
        speed = int(max(-1000, min(1000, 1.0 * speed_scale)))
        frame = encode_speed_steer(speed, 0)
        s, _ = struct.unpack(">hh", frame[3:7])
        assert s == 1000

    def test_left_turn_scales_correctly(self):
        """angular.z = 1.0 with steer_scale=-500 → steer=-500."""
        steer_scale = -500.0
        steer = int(max(-1000, min(1000, 1.0 * steer_scale)))
        frame = encode_speed_steer(0, steer)
        _, st = struct.unpack(">hh", frame[3:7])
        assert st == -500

    def test_heartbeat_frame_is_minimal(self):
        frame = encode_heartbeat()
        assert frame[1] == CmdType.HEARTBEAT
        assert frame[2] == 0    # zero payload

    def test_arm_frame_payload(self):
        frame_arm   = encode_arm(True)
        frame_disarm = encode_arm(False)
        assert frame_arm[3]   == 1
        assert frame_disarm[3] == 0

    def test_pid_frame_all_zeros(self):
        frame = encode_pid_update(0.0, 0.0, 0.0)
        kp, ki, kd = struct.unpack(">fff", frame[3:15])
        assert kp == pytest.approx(0.0)
        assert ki == pytest.approx(0.0)
        assert kd == pytest.approx(0.0)

    def test_pid_frame_typical_values(self):
        frame = encode_pid_update(3.0, 0.02, 0.12)
        kp, ki, kd = struct.unpack(">fff", frame[3:15])
        assert kp == pytest.approx(3.0, rel=1e-5)
        assert ki == pytest.approx(0.02, rel=1e-5)
        assert kd == pytest.approx(0.12, rel=1e-5)


class TestMockSerialTX:
    """Test TX path using MockSerial — verifies bytes sent over the wire."""

    def _write_and_get(self, frames: list[bytes]) -> bytes:
        """Write multiple frames to a MockSerial and return all written bytes."""
        ms = MockSerial()
        for f in frames:
            ms.write(f)
        return ms.get_written()

    def test_speed_steer_bytes_on_wire(self):
        frame = encode_speed_steer(300, -150)
        ms = MockSerial()
        ms.write(frame)
        written = ms.get_written()
        assert written[0] == STX
        assert written[1] == CmdType.SPEED_STEER
        assert written[-1] == ETX

    def test_heartbeat_bytes_on_wire(self):
        frame = encode_heartbeat()
        ms = MockSerial()
        ms.write(frame)
        written = ms.get_written()
        assert written[0] == STX
        assert written[1] == CmdType.HEARTBEAT

    def test_write_error_detected(self):
        import serial
        ms = MockSerial()
        ms.raise_on_write = serial.SerialException("test error")
        with pytest.raises(serial.SerialException):
            ms.write(b"\x00")

    def test_multiple_frames_written_sequentially(self):
        ms = MockSerial()
        ms.write(encode_heartbeat())
        ms.write(encode_speed_steer(100, 0))
        ms.write(encode_arm(True))
        written = ms.get_written()
        # All three frames should be in the buffer
        assert written.count(STX) == 3
        assert written.count(ETX) == 3

    def test_write_call_count_tracked(self):
        ms = MockSerial()
        ms.write(encode_heartbeat())
        ms.write(encode_heartbeat())
        assert ms.write_calls == 2


class TestMockSerialRX:
    """Test RX parsing path using MockSerial with pre-loaded telemetry data."""

    from saltybot_bridge.esp32_protocol import FrameParser

    def test_rx_imu_frame(self):
        from saltybot_bridge.esp32_protocol import FrameParser, ImuFrame
        raw = _imu_frame_bytes(pitch=500, roll=-200, yaw=100, ax=0, ay=0, az=981)
        ms  = MockSerial(rx_data=raw)
        parser = FrameParser()
        results = []
        while ms.in_waiting:
            chunk = ms.read(ms.in_waiting)
            for b in chunk:
                r = parser.feed(b)
                if r is not None:
                    results.append(r)
        assert len(results) == 1
        f = results[0]
        assert isinstance(f, ImuFrame)
        assert f.pitch_deg == pytest.approx(5.0)
        assert f.roll_deg  == pytest.approx(-2.0)
        assert f.accel_z   == pytest.approx(9.81)

    def test_rx_battery_frame(self):
        from saltybot_bridge.esp32_protocol import FrameParser, BatteryFrame
        raw = _battery_frame_bytes(v_mv=10500, i_ma=1200, soc=45)
        ms  = MockSerial(rx_data=raw)
        parser = FrameParser()
        results = []
        while ms.in_waiting:
            for b in ms.read(ms.in_waiting):
                r = parser.feed(b)
                if r is not None:
                    results.append(r)
        f = results[0]
        assert isinstance(f, BatteryFrame)
        assert f.voltage_mv == 10500
        assert f.soc_pct    == 45

    def test_rx_multiple_frames_in_one_read(self):
        from saltybot_bridge.esp32_protocol import FrameParser
        raw = (_imu_frame_bytes() + _arm_state_frame_bytes() + _battery_frame_bytes())
        ms  = MockSerial(rx_data=raw)
        parser = FrameParser()
        results = []
        data = ms.read(len(raw))
        for b in data:
            r = parser.feed(b)
            if r is not None:
                results.append(r)
        assert len(results) == 3
        assert parser.frames_error == 0

    def test_rx_bad_crc_counted_as_error(self):
        from saltybot_bridge.esp32_protocol import FrameParser
        raw = bytearray(_arm_state_frame_bytes(state=1))
        raw[-3] ^= 0xFF    # corrupt CRC
        ms = MockSerial(rx_data=bytes(raw))
        parser = FrameParser()
        for b in ms.read(len(raw)):
            parser.feed(b)
        assert parser.frames_ok    == 0
        assert parser.frames_error == 1

    def test_rx_resync_after_corrupt_byte(self):
        from saltybot_bridge.esp32_protocol import FrameParser, ArmStateFrame
        garbage = b"\xDE\xAD\x00\x00"
        valid   = _arm_state_frame_bytes(state=1)
        ms = MockSerial(rx_data=garbage + valid)
        parser = FrameParser()
        results = []
        while ms.in_waiting:
            for b in ms.read(ms.in_waiting):
                r = parser.feed(b)
                if r is not None:
                    results.append(r)
        assert len(results) == 1
        assert isinstance(results[0], ArmStateFrame)


# ═══════════════════════════════════════════════════════════════════
# Watchdog logic tests
# ═══════════════════════════════════════════════════════════════════

class TestWatchdog:
    """Test the watchdog logic in isolation without a full ROS2 node."""

    def test_watchdog_fires_after_timeout(self):
        """Simulate watchdog: record time, wait >500ms, verify zero-speed sent."""
        ms = MockSerial()
        last_cmd_t = time.monotonic() - 0.6    # 600ms ago
        wd_timeout = 0.5

        # Watchdog check logic (mirrors _watchdog_cb)
        if time.monotonic() - last_cmd_t >= wd_timeout:
            ms.write(encode_speed_steer(0, 0))

        written = ms.get_written()
        speed, steer = struct.unpack(">hh", written[3:7])
        assert speed == 0
        assert steer == 0

    def test_watchdog_does_not_fire_before_timeout(self):
        """Watchdog must not fire if /cmd_vel was recent."""
        ms = MockSerial()
        last_cmd_t = time.monotonic() - 0.2    # 200ms ago — within timeout
        wd_timeout = 0.5

        if time.monotonic() - last_cmd_t >= wd_timeout:
            ms.write(encode_speed_steer(0, 0))

        assert ms.get_written() == b""

    def test_zero_speed_frame_structure(self):
        """Zero-speed frame must have correct structure."""
        frame = encode_speed_steer(0, 0)
        assert frame[0] == STX
        assert frame[1] == CmdType.SPEED_STEER
        assert frame[-1] == ETX
        speed, steer = struct.unpack(">hh", frame[3:7])
        assert speed == 0
        assert steer == 0
