"""test_gimbal.py — Unit tests for saltybot_gimbal (Issue #548).

Runs without hardware: all serial I/O is mocked.
"""

import math
import struct
import sys
import types
import unittest
from unittest.mock import MagicMock

# ── Stub ROS2 / serial so tests run offline ────────────────────────────────────

def _make_rclpy_stub():
    rclpy = types.ModuleType("rclpy")
    rclpy.init = lambda **_: None
    rclpy.spin = lambda _: None
    rclpy.shutdown = lambda: None

    node_mod = types.ModuleType("rclpy.node")
    class _Node:
        def __init__(self, *a, **kw): pass
        def declare_parameter(self, *a, **kw): pass
        def get_parameter(self, name):
            defaults = {
                "serial_port": "/dev/null", "baud_rate": 921600,
                "pan_limit_deg": 150.0, "tilt_limit_deg": 45.0,
                "home_pan_deg": 0.0, "home_tilt_deg": 0.0,
                "max_speed_deg_s": 90.0, "accel_deg_s2": 180.0,
                "update_rate_hz": 20.0, "state_publish_hz": 10.0,
                "reconnect_delay_s": 2.0,
                "camera_focal_length_px": 600.0,
                "image_width_px": 848, "image_height_px": 480,
            }
            m = MagicMock()
            m.value = defaults.get(name, 0)
            return m
        def get_logger(self): return MagicMock()
        def create_subscription(self, *a, **kw): pass
        def create_publisher(self, *a, **kw): return MagicMock()
        def create_service(self, *a, **kw): pass
        def create_timer(self, *a, **kw): pass
        def destroy_node(self): pass
    node_mod.Node = _Node
    rclpy.node = node_mod

    qos_mod = types.ModuleType("rclpy.qos")
    qos_mod.QoSProfile = MagicMock(return_value=None)
    qos_mod.HistoryPolicy = MagicMock()
    qos_mod.ReliabilityPolicy = MagicMock()
    rclpy.qos = qos_mod

    return rclpy


def _install_stubs():
    rclpy_stub = _make_rclpy_stub()
    sys.modules.setdefault("rclpy", rclpy_stub)
    sys.modules.setdefault("rclpy.node", rclpy_stub.node)
    sys.modules.setdefault("rclpy.qos", rclpy_stub.qos)

    geo = types.ModuleType("geometry_msgs")
    geo_msg = types.ModuleType("geometry_msgs.msg")
    class _Vector3:
        def __init__(self, x=0.0, y=0.0, z=0.0): self.x=x; self.y=y; self.z=z
    class _PointStamped:
        def __init__(self): self.point = _Vector3()
    geo_msg.Vector3 = _Vector3
    geo_msg.PointStamped = _PointStamped
    geo.msg = geo_msg
    sys.modules.setdefault("geometry_msgs", geo)
    sys.modules.setdefault("geometry_msgs.msg", geo_msg)

    std_msgs = types.ModuleType("std_msgs")
    std_msgs_msg = types.ModuleType("std_msgs.msg")
    class _String:
        def __init__(self, data=""): self.data = data
    std_msgs_msg.String = _String
    std_msgs.msg = std_msgs_msg
    sys.modules.setdefault("std_msgs", std_msgs)
    sys.modules.setdefault("std_msgs.msg", std_msgs_msg)

    std_srvs = types.ModuleType("std_srvs")
    std_srvs_srv = types.ModuleType("std_srvs.srv")
    class _Trigger:
        class Request: pass
        class Response:
            success = False
            message = ""
    std_srvs_srv.Trigger = _Trigger
    std_srvs.srv = std_srvs_srv
    sys.modules.setdefault("std_srvs", std_srvs)
    sys.modules.setdefault("std_srvs.srv", std_srvs_srv)
    sys.modules.setdefault("serial", MagicMock())


_install_stubs()

from saltybot_gimbal.jlink_gimbal import (  # noqa: E402
    encode_gimbal_pos, encode_gimbal_home, decode_gimbal_state,
    GimbalStateFrame, CMD_GIMBAL_POS, TLM_GIMBAL_STATE,
    STX, ETX, _crc16_ccitt, _build_frame,
    degs_to_speed_reg, speed_reg_to_degs,
)
from saltybot_gimbal.gimbal_node import MotionAxis, _clamp  # noqa: E402


# ── Helper ─────────────────────────────────────────────────────────────────────

def _parse_frame(raw: bytes):
    """Parse a JLINK frame; return (cmd, payload) or raise on error."""
    assert raw[0] == STX, f"STX expected, got 0x{raw[0]:02x}"
    cmd = raw[1]
    length = raw[2]
    payload = raw[3:3 + length]
    crc_hi = raw[3 + length]
    crc_lo = raw[4 + length]
    crc_rcvd = (crc_hi << 8) | crc_lo
    assert raw[-1] == ETX, f"ETX expected, got 0x{raw[-1]:02x}"
    crc_data = bytes([cmd, length]) + payload
    crc_calc = _crc16_ccitt(crc_data)
    assert crc_rcvd == crc_calc, (
        f"CRC mismatch: got {crc_rcvd:#06x}, expected {crc_calc:#06x}"
    )
    return cmd, payload


# ── CRC16 Tests ────────────────────────────────────────────────────────────────

class TestCRC16(unittest.TestCase):

    def test_empty_data(self):
        self.assertEqual(_crc16_ccitt(b""), 0xFFFF)

    def test_known_value_123456789(self):
        # Standard CCITT-FFFF check value for "123456789"
        self.assertEqual(_crc16_ccitt(b"123456789"), 0x29B1)

    def test_single_byte_is_int(self):
        v = _crc16_ccitt(b"\x0B")
        self.assertIsInstance(v, int)
        self.assertLessEqual(v, 0xFFFF)

    def test_different_data_different_crc(self):
        self.assertNotEqual(_crc16_ccitt(b"\x00\x01"), _crc16_ccitt(b"\x00\x02"))


# ── Speed Conversion Tests ─────────────────────────────────────────────────────

class TestSpeedConversion(unittest.TestCase):

    def test_zero_speed_is_max(self):
        self.assertEqual(degs_to_speed_reg(0.0), 0)

    def test_negative_speed_is_max(self):
        self.assertEqual(degs_to_speed_reg(-10.0), 0)

    def test_360_degs_maps_to_zero(self):
        # 360°/s → ratio=1 → reg=0 (max)
        self.assertEqual(degs_to_speed_reg(360.0), 0)

    def test_mid_speed(self):
        reg = degs_to_speed_reg(90.0)
        self.assertGreater(reg, 0)
        self.assertLess(reg, 4095)

    def test_round_trip_approx(self):
        for deg_s in (30.0, 60.0, 120.0, 200.0):
            reg = degs_to_speed_reg(deg_s)
            back = speed_reg_to_degs(reg)
            self.assertAlmostEqual(back, deg_s, delta=5.0)

    def test_reg_zero_is_max_speed(self):
        self.assertAlmostEqual(speed_reg_to_degs(0), 360.0)


# ── Encode Tests ───────────────────────────────────────────────────────────────

class TestEncodeGimbalPos(unittest.TestCase):

    def test_zero_pose(self):
        raw = encode_gimbal_pos(0.0, 0.0, 90.0)
        cmd, payload = _parse_frame(raw)
        self.assertEqual(cmd, CMD_GIMBAL_POS)  # 0x0B
        pan_x10, tilt_x10, speed = struct.unpack(">hhH", payload)
        self.assertEqual(pan_x10, 0)
        self.assertEqual(tilt_x10, 0)

    def test_positive_pan_tilt(self):
        raw = encode_gimbal_pos(30.0, 15.0, 45.0)
        _, payload = _parse_frame(raw)
        pan_x10, tilt_x10, _ = struct.unpack(">hhH", payload)
        self.assertEqual(pan_x10, 300)   # 30.0 * 10
        self.assertEqual(tilt_x10, 150)  # 15.0 * 10

    def test_negative_angles(self):
        raw = encode_gimbal_pos(-90.0, -20.0, 60.0)
        _, payload = _parse_frame(raw)
        pan_x10, tilt_x10, _ = struct.unpack(">hhH", payload)
        self.assertEqual(pan_x10, -900)
        self.assertEqual(tilt_x10, -200)

    def test_pan_limit_150_deg(self):
        raw = encode_gimbal_pos(150.0, 0.0, 30.0)
        _, payload = _parse_frame(raw)
        pan_x10, _, _ = struct.unpack(">hhH", payload)
        self.assertEqual(pan_x10, 1500)

    def test_tilt_limit_45_deg(self):
        raw = encode_gimbal_pos(0.0, 45.0, 30.0)
        _, payload = _parse_frame(raw)
        _, tilt_x10, _ = struct.unpack(">hhH", payload)
        self.assertEqual(tilt_x10, 450)

    def test_extreme_clamped(self):
        raw = encode_gimbal_pos(9999.0, -9999.0, 0.0)
        _, payload = _parse_frame(raw)
        pan_x10, tilt_x10, _ = struct.unpack(">hhH", payload)
        self.assertEqual(pan_x10, 32767)
        self.assertEqual(tilt_x10, -32767)

    def test_frame_byte_layout(self):
        raw = encode_gimbal_pos(10.0, -5.0, 30.0)
        self.assertEqual(raw[0], STX)
        self.assertEqual(raw[-1], ETX)
        self.assertEqual(raw[1], CMD_GIMBAL_POS)  # 0x0B
        self.assertEqual(raw[2], 6)               # 6-byte payload

    def test_crc_valid(self):
        # _parse_frame raises if CRC is bad
        _parse_frame(encode_gimbal_pos(45.0, -10.0, 120.0))


class TestEncodeGimbalHome(unittest.TestCase):

    def test_homes_to_zero(self):
        raw = encode_gimbal_home()
        cmd, payload = _parse_frame(raw)
        self.assertEqual(cmd, CMD_GIMBAL_POS)
        pan_x10, tilt_x10, _ = struct.unpack(">hhH", payload)
        self.assertEqual(pan_x10, 0)
        self.assertEqual(tilt_x10, 0)

    def test_crc_valid(self):
        _parse_frame(encode_gimbal_home())


# ── Decode Tests ───────────────────────────────────────────────────────────────

class TestDecodeGimbalState(unittest.TestCase):

    def _make_payload(self, pan_x10, tilt_x10, pan_spd, tilt_spd,
                      torque_en, rx_err_pct) -> bytes:
        return struct.pack(">hhHHBB",
                           pan_x10, tilt_x10,
                           pan_spd, tilt_spd,
                           torque_en, rx_err_pct)

    def test_zero_state(self):
        state = decode_gimbal_state(self._make_payload(0, 0, 0, 0, 0, 0))
        self.assertIsNotNone(state)
        self.assertAlmostEqual(state.pan_deg, 0.0)
        self.assertAlmostEqual(state.tilt_deg, 0.0)
        self.assertFalse(state.torque_en)
        self.assertEqual(state.rx_err_pct, 0)

    def test_angles_decode(self):
        state = decode_gimbal_state(self._make_payload(500, -250, 100, 200, 1, 5))
        self.assertAlmostEqual(state.pan_deg,  50.0)
        self.assertAlmostEqual(state.tilt_deg, -25.0)

    def test_torque_en(self):
        state = decode_gimbal_state(self._make_payload(0, 0, 0, 0, 1, 0))
        self.assertTrue(state.torque_en)

    def test_torque_off(self):
        state = decode_gimbal_state(self._make_payload(0, 0, 0, 0, 0, 0))
        self.assertFalse(state.torque_en)

    def test_speed_raw(self):
        state = decode_gimbal_state(self._make_payload(0, 0, 1234, 5678, 0, 0))
        self.assertEqual(state.pan_speed_raw, 1234)
        self.assertEqual(state.tilt_speed_raw, 5678)

    def test_rx_err_pct(self):
        state = decode_gimbal_state(self._make_payload(0, 0, 0, 0, 0, 42))
        self.assertEqual(state.rx_err_pct, 42)

    def test_short_payload_returns_none(self):
        self.assertIsNone(decode_gimbal_state(b"\x00\x01\x02"))

    def test_empty_payload_returns_none(self):
        self.assertIsNone(decode_gimbal_state(b""))

    def test_exactly_10_bytes_ok(self):
        payload = self._make_payload(100, -50, 512, 512, 1, 3)
        state = decode_gimbal_state(payload)
        self.assertIsNotNone(state)


# ── MotionAxis Tests ───────────────────────────────────────────────────────────

class TestMotionAxis(unittest.TestCase):

    def test_starts_at_initial_pos(self):
        ax = MotionAxis(10.0, 90.0, 180.0)
        self.assertAlmostEqual(ax.pos, 10.0)

    def test_not_moving_at_rest(self):
        ax = MotionAxis(0.0, 90.0, 180.0)
        self.assertFalse(ax.is_moving)

    def test_moves_toward_target(self):
        ax = MotionAxis(0.0, 90.0, 180.0)
        ax.set_target(90.0)
        for _ in range(5):
            ax.tick(0.05)
        self.assertGreater(ax.pos, 0.0)

    def test_reaches_target(self):
        ax = MotionAxis(0.0, 90.0, 180.0)
        ax.set_target(10.0)
        for _ in range(200):
            ax.tick(0.05)
        self.assertAlmostEqual(ax.pos, 10.0, places=1)
        self.assertFalse(ax.is_moving)

    def test_negative_target(self):
        ax = MotionAxis(0.0, 90.0, 180.0)
        ax.set_target(-20.0)
        for _ in range(200):
            ax.tick(0.05)
        self.assertAlmostEqual(ax.pos, -20.0, places=1)

    def test_faster_speed_reaches_sooner(self):
        ax_fast = MotionAxis(0.0, 180.0, 360.0)
        ax_slow = MotionAxis(0.0,  30.0, 360.0)
        ax_fast.set_target(30.0)
        ax_slow.set_target(30.0)
        for _ in range(5):
            ax_fast.tick(0.05)
            ax_slow.tick(0.05)
        self.assertGreater(ax_fast.pos, ax_slow.pos)

    def test_is_moving_while_en_route(self):
        ax = MotionAxis(0.0, 90.0, 180.0)
        ax.set_target(60.0)
        ax.tick(0.05)
        self.assertTrue(ax.is_moving)

    def test_no_overshoot(self):
        ax = MotionAxis(0.0, 90.0, 180.0)
        ax.set_target(5.0)
        for _ in range(500):
            ax.tick(0.05)
        self.assertLessEqual(ax.pos, 5.01)

    def test_set_target_updates_speed(self):
        ax = MotionAxis(0.0, 90.0, 180.0)
        ax.set_target(10.0, speed=45.0)
        self.assertAlmostEqual(ax.max_speed, 45.0)


# ── Clamp Tests ────────────────────────────────────────────────────────────────

class TestClamp(unittest.TestCase):

    def test_within_range(self):
        self.assertEqual(_clamp(5.0, 0.0, 10.0), 5.0)

    def test_below_min(self):
        self.assertEqual(_clamp(-5.0, 0.0, 10.0), 0.0)

    def test_above_max(self):
        self.assertEqual(_clamp(15.0, 0.0, 10.0), 10.0)

    def test_at_lower_boundary(self):
        self.assertEqual(_clamp(0.0, 0.0, 10.0), 0.0)

    def test_at_upper_boundary(self):
        self.assertEqual(_clamp(10.0, 0.0, 10.0), 10.0)


# ── Look-at Projection Tests ───────────────────────────────────────────────────

class TestLookAtProjection(unittest.TestCase):
    """Test the look-at 3D → pan/tilt math (mirrors gimbal_node logic)."""

    def _project(self, x, y, z):
        pan_deg  =  math.degrees(math.atan2(x, z))
        tilt_deg = -math.degrees(math.atan2(y, z))
        return pan_deg, tilt_deg

    def test_straight_ahead(self):
        pan, tilt = self._project(0.0, 0.0, 1.0)
        self.assertAlmostEqual(pan,  0.0, places=5)
        self.assertAlmostEqual(tilt, 0.0, places=5)

    def test_right_target(self):
        pan, tilt = self._project(1.0, 0.0, 1.0)
        self.assertAlmostEqual(pan,  45.0, places=4)
        self.assertAlmostEqual(tilt,  0.0, places=4)

    def test_up_target(self):
        # camera frame: negative y = above centre → tilt up (positive)
        pan, tilt = self._project(0.0, -1.0, 1.0)
        self.assertAlmostEqual(pan,   0.0, places=4)
        self.assertAlmostEqual(tilt, 45.0, places=4)

    def test_left_down_target(self):
        pan, tilt = self._project(-1.0, 1.0, 2.0)
        self.assertLess(pan,  0.0)   # left → negative pan
        self.assertLess(tilt, 0.0)   # below → negative tilt

    def test_90_degree_right(self):
        pan, _ = self._project(1.0, 0.0, 0.0)
        self.assertAlmostEqual(pan, 90.0, places=4)


if __name__ == "__main__":
    unittest.main()
