"""
Unit tests for ESP32-S3 telemetry parsing logic.
Run with: pytest jetson/ros2_ws/src/saltybot_bridge/test/test_parse.py
"""

import json
import pytest


# ── Minimal stub so we can test parsing without a ROS2 runtime ───────────────

def parse_frame(raw: bytes):
    """Mirror of the parsing logic in serial_bridge_node._parse_and_publish."""
    text = raw.decode("ascii", errors="ignore").strip()
    if not text:
        return None
    data = json.loads(text)

    if "err" in data:
        return {"type": "fault", "errno": data["err"]}

    required = ("p", "r", "e", "ig", "m", "s", "y")
    if not all(k in data for k in required):
        raise ValueError(f"Incomplete frame: {data}")

    return {
        "type": "telemetry",
        "pitch_deg":     data["p"] / 10.0,
        "roll_deg":      data["r"] / 10.0,
        "yaw_deg":       data["y"] / 10.0,
        "pid_error_deg": data["e"] / 10.0,
        "integral":      data["ig"] / 10.0,
        "motor_cmd":     int(data["m"]),
        "state":         int(data["s"]),
    }


# ── Tests ─────────────────────────────────────────────────────────────────────

def test_normal_frame():
    raw = b'{"p":125,"r":-30,"e":50,"ig":20,"m":350,"s":1,"y":0}\n'
    result = parse_frame(raw)
    assert result["type"] == "telemetry"
    assert result["pitch_deg"] == pytest.approx(12.5)
    assert result["roll_deg"] == pytest.approx(-3.0)
    assert result["yaw_deg"] == pytest.approx(0.0)
    assert result["pid_error_deg"] == pytest.approx(5.0)
    assert result["integral"] == pytest.approx(2.0)
    assert result["motor_cmd"] == 350
    assert result["state"] == 1   # ARMED


def test_fault_frame():
    raw = b'{"err":-1}\n'
    result = parse_frame(raw)
    assert result["type"] == "fault"
    assert result["errno"] == -1


def test_zero_frame():
    raw = b'{"p":0,"r":0,"e":0,"ig":0,"m":0,"s":0,"y":0}\n'
    result = parse_frame(raw)
    assert result["type"] == "telemetry"
    assert result["pitch_deg"] == pytest.approx(0.0)
    assert result["state"] == 0   # DISARMED


def test_tilt_fault_state():
    raw = b'{"p":450,"r":0,"e":400,"ig":999,"m":1000,"s":2,"y":0}\n'
    result = parse_frame(raw)
    assert result["state"] == 2   # TILT_FAULT
    assert result["pitch_deg"] == pytest.approx(45.0)
    assert result["motor_cmd"] == 1000


def test_negative_motor_cmd():
    raw = b'{"p":-100,"r":0,"e":-100,"ig":-50,"m":-750,"s":1,"y":10}\n'
    result = parse_frame(raw)
    assert result["motor_cmd"] == -750
    assert result["pitch_deg"] == pytest.approx(-10.0)


def test_incomplete_frame_raises():
    raw = b'{"p":100,"r":0}\n'
    with pytest.raises(ValueError, match="Incomplete frame"):
        parse_frame(raw)


def test_empty_line_returns_none():
    assert parse_frame(b"\n") is None
    assert parse_frame(b"  \n") is None


def test_corrupt_frame_raises_json_error():
    raw = b'not_json\n'
    with pytest.raises(json.JSONDecodeError):
        parse_frame(raw)