"""
test_servo.py — Pan-tilt servo driver tests (Issue #206)

Verifies:
  - PWM frequency: 50 Hz (20 ms period)
  - Pulse width: 500-2500 µs for 0-180°
  - Angle conversion: linear mapping
  - Smooth sweeping: animation timing and interpolation
  - Multi-servo coordination (pan + tilt independently)
"""

import pytest

# ── Constants ─────────────────────────────────────────────────────────────

SERVO_MIN_US = 500
SERVO_MAX_US = 2500
SERVO_CENTER_US = 1500

PWM_FREQ_HZ = 50
PERIOD_MS = 20

NUM_SERVOS = 2
SERVO_PAN = 0
SERVO_TILT = 1


# ── Servo Simulator ────────────────────────────────────────────────────────

class ServoSimulator:
    def __init__(self):
        self.current_angle_deg = [90, 90]  # Both centered
        self.pulse_us = [SERVO_CENTER_US, SERVO_CENTER_US]
        self.is_sweeping = [False, False]
        self.sweep_start_deg = [0, 0]
        self.sweep_end_deg = [0, 0]
        self.sweep_duration_ms = [0, 0]
        self.sweep_start_ms = [None, None]

    def angle_to_pulse(self, degrees):
        """Convert angle (0-180) to pulse width (500-2500 µs)."""
        if degrees < 0:
            degrees = 0
        if degrees > 180:
            degrees = 180
        return SERVO_MIN_US + (degrees * (SERVO_MAX_US - SERVO_MIN_US)) // 180

    def pulse_to_angle(self, pulse_us):
        """Convert pulse width to angle."""
        if pulse_us < SERVO_MIN_US:
            pulse_us = SERVO_MIN_US
        if pulse_us > SERVO_MAX_US:
            pulse_us = SERVO_MAX_US
        return (pulse_us - SERVO_MIN_US) * 180 // (SERVO_MAX_US - SERVO_MIN_US)

    def set_angle(self, channel, degrees):
        """Immediately set servo angle."""
        self.current_angle_deg[channel] = min(180, max(0, degrees))
        self.pulse_us[channel] = self.angle_to_pulse(self.current_angle_deg[channel])
        self.is_sweeping[channel] = False

    def get_angle(self, channel):
        """Get current servo angle."""
        return self.current_angle_deg[channel]

    def set_pulse_us(self, channel, pulse_us):
        """Set servo by pulse width."""
        if pulse_us < SERVO_MIN_US:
            pulse_us = SERVO_MIN_US
        if pulse_us > SERVO_MAX_US:
            pulse_us = SERVO_MAX_US
        self.pulse_us[channel] = pulse_us
        self.current_angle_deg[channel] = self.pulse_to_angle(pulse_us)
        self.is_sweeping[channel] = False

    def sweep(self, channel, start_deg, end_deg, duration_ms):
        """Start smooth sweep."""
        self.sweep_start_deg[channel] = start_deg
        self.sweep_end_deg[channel] = end_deg
        self.sweep_duration_ms[channel] = duration_ms
        self.sweep_start_ms[channel] = None
        self.is_sweeping[channel] = True

    def tick(self, now_ms):
        """Update sweep animations."""
        for ch in range(NUM_SERVOS):
            if not self.is_sweeping[ch]:
                continue

            # Initialize start time on first call
            if self.sweep_start_ms[ch] is None:
                self.sweep_start_ms[ch] = now_ms

            elapsed = now_ms - self.sweep_start_ms[ch]
            duration = self.sweep_duration_ms[ch]

            if elapsed >= duration:
                # Sweep complete
                self.is_sweeping[ch] = False
                self.current_angle_deg[ch] = self.sweep_end_deg[ch]
                self.pulse_us[ch] = self.angle_to_pulse(self.sweep_end_deg[ch])
            else:
                # Linear interpolation
                start = self.sweep_start_deg[ch]
                end = self.sweep_end_deg[ch]
                delta = end - start
                angle = start + (delta * elapsed) // duration
                self.current_angle_deg[ch] = angle
                self.pulse_us[ch] = self.angle_to_pulse(angle)

    def is_sweeping_any(self):
        """Check if any servo is sweeping."""
        return any(self.is_sweeping)


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

def test_initialization():
    """Servos should initialize centered at 90°."""
    sim = ServoSimulator()
    assert sim.get_angle(SERVO_PAN) == 90
    assert sim.get_angle(SERVO_TILT) == 90
    assert sim.pulse_us[SERVO_PAN] == SERVO_CENTER_US
    assert sim.pulse_us[SERVO_TILT] == SERVO_CENTER_US


def test_angle_to_pulse_conversion():
    """Angle to pulse conversion should be linear."""
    sim = ServoSimulator()

    assert sim.angle_to_pulse(0) == SERVO_MIN_US      # 500 µs
    assert sim.angle_to_pulse(90) == SERVO_CENTER_US  # 1500 µs
    assert sim.angle_to_pulse(180) == SERVO_MAX_US    # 2500 µs

    # Intermediate angles
    assert sim.angle_to_pulse(45) == 1000   # 0.5 way: 500 + 500 = 1000
    assert sim.angle_to_pulse(135) == 2000  # 0.75 way: 500 + 1500 = 2000


def test_pulse_to_angle_conversion():
    """Pulse to angle conversion should invert angle_to_pulse."""
    sim = ServoSimulator()

    assert sim.pulse_to_angle(SERVO_MIN_US) == 0
    assert sim.pulse_to_angle(SERVO_CENTER_US) == 90
    assert sim.pulse_to_angle(SERVO_MAX_US) == 180

    # Intermediate pulses
    assert sim.pulse_to_angle(1000) == 45
    assert sim.pulse_to_angle(2000) == 135


def test_set_angle_pan():
    """Pan servo should update angle immediately."""
    sim = ServoSimulator()

    sim.set_angle(SERVO_PAN, 0)
    assert sim.get_angle(SERVO_PAN) == 0
    assert sim.pulse_us[SERVO_PAN] == SERVO_MIN_US

    sim.set_angle(SERVO_PAN, 90)
    assert sim.get_angle(SERVO_PAN) == 90
    assert sim.pulse_us[SERVO_PAN] == SERVO_CENTER_US

    sim.set_angle(SERVO_PAN, 180)
    assert sim.get_angle(SERVO_PAN) == 180
    assert sim.pulse_us[SERVO_PAN] == SERVO_MAX_US


def test_set_angle_tilt():
    """Tilt servo should work independently."""
    sim = ServoSimulator()

    sim.set_angle(SERVO_TILT, 45)
    assert sim.get_angle(SERVO_TILT) == 45
    assert sim.get_angle(SERVO_PAN) == 90  # Pan unchanged


def test_set_pulse_us():
    """Pulse width setter should update angle correctly."""
    sim = ServoSimulator()

    sim.set_pulse_us(SERVO_PAN, SERVO_MIN_US)
    assert sim.get_angle(SERVO_PAN) == 0

    sim.set_pulse_us(SERVO_PAN, SERVO_CENTER_US)
    assert sim.get_angle(SERVO_PAN) == 90

    sim.set_pulse_us(SERVO_PAN, SERVO_MAX_US)
    assert sim.get_angle(SERVO_PAN) == 180


def test_sweep_timing():
    """Sweep should complete in specified duration."""
    sim = ServoSimulator()

    # Pan from 0° to 180° over 2 seconds
    sim.sweep(SERVO_PAN, 0, 180, 2000)

    # Initial tick
    sim.tick(0)
    assert sim.get_angle(SERVO_PAN) == 0

    # Halfway through sweep (t=1000ms)
    sim.tick(1000)
    assert sim.get_angle(SERVO_PAN) == 90  # Linear interpolation

    # End of sweep (t=2000ms)
    sim.tick(2000)
    assert sim.get_angle(SERVO_PAN) == 180
    assert not sim.is_sweeping[SERVO_PAN]


def test_sweep_interpolation():
    """Sweep should interpolate smoothly."""
    sim = ServoSimulator()

    # Sweep from 0° to 180° in 1000ms
    sim.sweep(SERVO_PAN, 0, 180, 1000)

    angles = []
    for t in range(0, 1001, 100):
        sim.tick(t)
        angles.append(sim.get_angle(SERVO_PAN))

    # Expected: [0, 18, 36, 54, 72, 90, 108, 126, 144, 162, 180]
    expected = [i * 18 for i in range(11)]
    assert angles == expected, f"Got {angles}, expected {expected}"


def test_reverse_sweep():
    """Sweep from higher angle to lower angle."""
    sim = ServoSimulator()

    sim.sweep(SERVO_TILT, 180, 0, 1000)

    sim.tick(0)
    assert sim.get_angle(SERVO_TILT) == 180

    sim.tick(500)
    assert sim.get_angle(SERVO_TILT) == 90

    sim.tick(1000)
    assert sim.get_angle(SERVO_TILT) == 0
    assert not sim.is_sweeping[SERVO_TILT]


def test_sweep_stops_on_immediate_set():
    """Setting angle immediately should stop sweep."""
    sim = ServoSimulator()

    sim.sweep(SERVO_PAN, 0, 180, 2000)
    sim.tick(500)

    # Stop sweep by setting angle
    sim.set_angle(SERVO_PAN, 45)
    assert not sim.is_sweeping[SERVO_PAN]
    assert sim.get_angle(SERVO_PAN) == 45


def test_independent_servos():
    """Pan and tilt servos should sweep independently."""
    sim = ServoSimulator()

    sim.sweep(SERVO_PAN, 0, 180, 1000)
    sim.sweep(SERVO_TILT, 180, 0, 2000)

    # Initialize sweep timing
    sim.tick(0)

    # After 1 second
    sim.tick(1000)
    assert sim.get_angle(SERVO_PAN) == 180
    assert not sim.is_sweeping[SERVO_PAN]
    assert sim.get_angle(SERVO_TILT) == 90  # Halfway through
    assert sim.is_sweeping[SERVO_TILT]

    # After 2 seconds
    sim.tick(2000)
    assert not sim.is_sweeping[SERVO_PAN]
    assert sim.get_angle(SERVO_TILT) == 0
    assert not sim.is_sweeping[SERVO_TILT]
    assert not sim.is_sweeping_any()


def test_fast_sweep():
    """Very short sweep should work."""
    sim = ServoSimulator()

    sim.sweep(SERVO_PAN, 45, 135, 100)  # 90° in 100ms

    sim.tick(0)
    assert sim.get_angle(SERVO_PAN) == 45

    sim.tick(50)
    assert sim.get_angle(SERVO_PAN) == 90

    sim.tick(100)
    assert sim.get_angle(SERVO_PAN) == 135
    assert not sim.is_sweeping[SERVO_PAN]


def test_multiple_sweeps():
    """Multiple sequential sweeps should work."""
    sim = ServoSimulator()

    # First sweep (0° to 90° in 500ms)
    sim.sweep(SERVO_PAN, 0, 90, 500)
    sim.tick(0)
    sim.tick(500)
    assert sim.get_angle(SERVO_PAN) == 90
    assert not sim.is_sweeping[SERVO_PAN]

    # Second sweep (90° to 0° in 500ms, starting at t=500)
    sim.sweep(SERVO_PAN, 90, 0, 500)
    sim.tick(500)  # Initialize second sweep
    sim.tick(1000)  # After 500ms of second sweep
    assert sim.get_angle(SERVO_PAN) == 0
    assert not sim.is_sweeping[SERVO_PAN]


def test_boundary_angles():
    """Angles > 180° should clamp to 180°."""
    sim = ServoSimulator()

    sim.set_angle(SERVO_PAN, 200)
    assert sim.get_angle(SERVO_PAN) == 180

    sim.set_angle(SERVO_PAN, -10)
    assert sim.get_angle(SERVO_PAN) == 0


def test_pulse_clamping():
    """Pulse widths outside 500-2500 µs should clamp."""
    sim = ServoSimulator()

    sim.set_pulse_us(SERVO_PAN, 100)  # Too low
    assert sim.pulse_us[SERVO_PAN] == SERVO_MIN_US

    sim.set_pulse_us(SERVO_PAN, 3000)  # Too high
    assert sim.pulse_us[SERVO_PAN] == SERVO_MAX_US


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