"""Unit tests for pan/tilt servo control."""

import unittest
import math
from unittest.mock import Mock, MagicMock, patch
from saltybot_pan_tilt.pan_tilt_node import SCSServoBus


class TestSCSServoBus(unittest.TestCase):
    """Test SCS/STS serial bus protocol handler."""

    def test_checksum(self):
        """Test XOR checksum calculation."""
        bus = SCSServoBus("/dev/null")

        # Test with known data
        data = bytes([1, 5, 0x03, 46, 100, 0, 0, 0x10])
        expected = 1 ^ 5 ^ 0x03 ^ 46 ^ 100 ^ 0 ^ 0 ^ 0x10
        self.assertEqual(bus._checksum(data), expected)

    def test_position_clamping(self):
        """Test position clamping to valid range."""
        bus = SCSServoBus("/dev/null")

        # Mock serial port
        bus.serial = MagicMock()

        # Test clamping below minimum
        bus.set_position(1, -100, 100)
        call_args = bus.serial.write.call_args_list[-1][0][0]
        position_lo = call_args[6]
        position_hi = call_args[7]
        position = position_lo | (position_hi << 8)
        self.assertEqual(position, 0)

        # Test clamping above maximum
        bus.serial.reset_mock()
        bus.set_position(1, 5000, 100)
        call_args = bus.serial.write.call_args_list[-1][0][0]
        position_lo = call_args[6]
        position_hi = call_args[7]
        position = position_lo | (position_hi << 8)
        self.assertEqual(position, 4095)

    def test_speed_clamping(self):
        """Test speed clamping to valid range."""
        bus = SCSServoBus("/dev/null")

        # Mock serial port
        bus.serial = MagicMock()

        # Test clamping above maximum
        bus.set_position(1, 2048, 2000)
        call_args = bus.serial.write.call_args_list[-1][0][0]
        speed_lo = call_args[4]
        speed_hi = call_args[5]
        speed = speed_lo | (speed_hi << 8)
        self.assertEqual(speed, 1000)


class TestPanTiltControl(unittest.TestCase):
    """Test pan/tilt control algorithms."""

    def test_pixel_to_angle_conversion(self):
        """Test pixel error to angle error conversion."""
        image_width = 848
        image_height = 480
        fov_h = 87.0
        fov_v = 58.0

        # Calculate pixels per degree
        pixels_per_deg_h = image_width / fov_h
        pixels_per_deg_v = image_height / fov_v

        # Test 10 pixels horizontal error
        error_x = 10
        angle_error_h = error_x / pixels_per_deg_h
        self.assertAlmostEqual(angle_error_h, 10 * 87.0 / image_width, places=2)

        # Test 5 pixels vertical error
        error_y = 5
        angle_error_v = error_y / pixels_per_deg_v
        self.assertAlmostEqual(angle_error_v, 5 * 58.0 / image_height, places=2)

    def test_position_error_calculation(self):
        """Test servo position error from angle error."""
        # 0.24 degrees per position unit
        angle_error = 2.4
        pos_error = angle_error / 0.24
        self.assertAlmostEqual(pos_error, 10.0, places=1)

    def test_servo_position_bounds(self):
        """Test servo position stays within bounds."""
        pan_min = 0
        pan_max = 4095
        pan_center = 2048

        # Test position below minimum
        pos_error = 3000
        pan_pos = max(pan_min, min(pan_max, pan_center - pos_error))
        self.assertEqual(pan_pos, pan_min)

        # Test position above maximum
        pos_error = -3000
        pan_pos = max(pan_min, min(pan_max, pan_center - pos_error))
        self.assertEqual(pan_pos, pan_max)

        # Test position within bounds
        pos_error = 100
        pan_pos = max(pan_min, min(pan_max, pan_center - pos_error))
        self.assertEqual(pan_pos, pan_center - pos_error)


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