import time
import sys
from adafruit_servokit import ServoKit

# Number of channels on PCA9685 (usually 16)
kit = ServoKit(channels=16)

# Servo configuration
SERVO_CHANNEL = 0  # PCA9685 channel where servo is connected
LOCKED_ANGLE = 0   # Servo angle for locked position
ALARM_ANGLE = 90   # Servo angle for alarm/unlocked position

def set_servo_angle(angle):
    """Safely set servo angle with validation."""
    try:
        if not (0 <= angle <= 180):
            raise ValueError("Angle must be between 0 and 180 degrees.")
        kit.servo[SERVO_CHANNEL].angle = angle
        time.sleep(0.5)  # Allow servo to move
    except Exception as e:
        print(f"[ERROR] Failed to set servo angle: {e}")

def alarm_triggered():
    """Simulate alarm trigger (e.g., motion detected)."""
    print("[ALARM] Motion detected! Unlocking...")
    set_servo_angle(ALARM_ANGLE)
    time.sleep(3)  # Keep alarm position for 3 seconds
    print("[ALARM] Resetting to locked position.")
    set_servo_angle(LOCKED_ANGLE)

def main():
    print("=== Servo Motor Alarm System ===")
    print("Press Ctrl+C to exit.")
    
    # Initialize servo to locked position
    set_servo_angle(LOCKED_ANGLE)

    try:
        while True:
            # Simulate sensor input (replace with actual GPIO input)
            user_input = input("Trigger alarm? (y/n): ").strip().lower()
            if user_input == 'y':
                alarm_triggered()
            elif user_input == 'n':
                print("[INFO] System idle.")
            else:
                print("[WARN] Invalid input. Use 'y' or 'n'.")
    except KeyboardInterrupt:
        print("\n[EXIT] Shutting down system.")
        set_servo_angle(LOCKED_ANGLE)
        sys.exit(0)

if __name__ == "__main__":
    main()