import sys
import time
from adafruit_servokit import ServoKit

def main():
    if len(sys.argv) != 3:
        print("Usage: python servo.py <servo_number> <servo_angle>")
        sys.exit(1)

    try:
        servo_num = int(sys.argv[1])
        servo_angle = float(sys.argv[2])
    except ValueError:
        print("Error: servo_number must be int, servo_angle must be int/float")
        sys.exit(1)

    # Initialize PCA9685 (16 channels, default address 0x40)
    kit = ServoKit(channels=16)

    if not (0 <= servo_num <= 15):
        print("Error: servo_number must be between 0 and 15")
        sys.exit(1)

    if not (0 <= servo_angle <= 180):
        print("Error: servo_angle must be between 0 and 180")
        sys.exit(1)

    # Send command
    print(f"Moving servo {servo_num} to {servo_angle}° ...")
    kit.servo[servo_num].angle = servo_angle
    time.sleep(0.5)
    print("Done.")

if __name__ == "__main__":
    main()
