"""
Servo Test Script
Tests pan (servo 1) and tilt (servo 2) via Kitronik Robotics Board
"""

from machine import Pin, I2C
import time
import utime

print("🤖 Servo Test Starting...")
print("=" * 50)

# Kitronik Robotics Board Control Class
class KitronikPicoRobotics:
    SRV_REG_BASE = 0x08
    REG_OFFSET = 4
    PRESCALE_VAL = b'\x79'
    CHIP_ADDRESS = 108

    def __init__(self, sda=8, scl=9):
        sda = Pin(sda)
        scl = Pin(scl)
        self.i2c = I2C(0, sda=sda, scl=scl, freq=100000)
        self.initPCA()

    def initPCA(self):
        self.i2c.writeto(0, b"\x06")
        self.i2c.writeto_mem(108, 0xfe, self.PRESCALE_VAL)
        self.i2c.writeto_mem(108, 0xfa, b"\x00")
        self.i2c.writeto_mem(108, 0xfb, b"\x00")
        self.i2c.writeto_mem(108, 0xfc, b"\x00")
        self.i2c.writeto_mem(108, 0xfd, b"\x00")
        self.i2c.writeto_mem(108, 0x00, b"\x01")
        utime.sleep_us(500)

    def servoWrite(self, servo, degrees):
        if degrees > 180:
            degrees = 180
        elif degrees < 0:
            degrees = 0
        if servo < 1 or servo > 8:
            raise Exception("INVALID SERVO NUMBER")
        
        calcServo = self.SRV_REG_BASE + ((servo - 1) * self.REG_OFFSET)
        PWMVal = int((degrees * 2.2755) + 102)
        lowByte = PWMVal & 0xFF
        highByte = (PWMVal >> 8) & 0x01
        
        self.i2c.writeto_mem(self.CHIP_ADDRESS, calcServo, bytes([lowByte]))
        self.i2c.writeto_mem(self.CHIP_ADDRESS, calcServo + 1, bytes([highByte]))

# Initialize
onboard_led = Pin("LED", Pin.OUT)
onboard_led.value(1)

print("\n🔧 Initializing Kitronik Robotics Board...")
try:
    board = KitronikPicoRobotics()
    print("✅ Kitronik board initialized")
except Exception as e:
    print(f"❌ Failed to initialize: {e}")
    print("   Check I2C connections: GP8 (SDA), GP9 (SCL)")
    exit()

# Test sequence
print("\n📋 Test Sequence:")
print("   1. Center both servos (90°)")
print("   2. Pan sweep: 0° → 90° → 180° → 90°")
print("   3. Tilt sweep: 50° → 90° → 135° → 90°")
print("   4. Repeat pan/tilt sweeps")
print("\n   Press Ctrl+C to stop\n")

time.sleep(2)

try:
    # Center both servos
    print("➡️  Centering servos...")
    board.servoWrite(1, 90)  # Pan center
    board.servoWrite(2, 90)  # Tilt center
    time.sleep(1)
    
    count = 0
    while True:
        count += 1
        print(f"\n🔄 Sweep cycle #{count}")
        
        # Pan sweep (full 180°)
        print("   Pan: 0° (full left)")
        board.servoWrite(1, 0)
        time.sleep(1)
        
        print("   Pan: 90° (center)")
        board.servoWrite(1, 90)
        time.sleep(1)
        
        print("   Pan: 180° (full right)")
        board.servoWrite(1, 180)
        time.sleep(1)
        
        print("   Pan: 90° (center)")
        board.servoWrite(1, 90)
        time.sleep(1)
        
        # Tilt sweep (50° to 135° to avoid base collision)
        print("   Tilt: 50° (down -40°)")
        board.servoWrite(2, 50)  # -40° in our angle system
        time.sleep(1)
        
        print("   Tilt: 90° (center 0°)")
        board.servoWrite(2, 90)
        time.sleep(1)
        
        print("   Tilt: 135° (up +45°)")
        board.servoWrite(2, 135)  # +45° in our angle system
        time.sleep(1)
        
        print("   Tilt: 90° (center 0°)")
        board.servoWrite(2, 90)
        time.sleep(1)
        
        # Blink LED
        onboard_led.value(0)
        time.sleep(0.1)
        onboard_led.value(1)

except KeyboardInterrupt:
    print("\n\n🛑 Test stopped")
    print("   Centering servos...")
    board.servoWrite(1, 90)
    board.servoWrite(2, 90)
    time.sleep(0.5)
    onboard_led.value(0)
    print("✅ Servos centered and stopped")

except Exception as e:
    print(f"\n❌ Error during test: {e}")
    onboard_led.value(0)