from machine import Pin, PWM
import time

# === Continuous Servo setup ===
servo = PWM(Pin(18), freq=50)

# 90° (duty=77) is STOP, <77 = rotate CW, >77 = rotate CCW
def rotate_servo(direction="stop"):
    if direction == "cw":
        servo.duty(40)  # Rotate clockwise
    elif direction == "ccw":
        servo.duty(115)  # Rotate counter-clockwise
    else:
        servo.duty(77)  # Stop (neutral)

# === Keypad setup ===
ROWS = [Pin(pin, Pin.OUT) for pin in (13, 12, 14, 27)]
COLS = [Pin(pin, Pin.IN, Pin.PULL_DOWN) for pin in (26, 25, 33, 32)]

keys = [
    ['1', '2', '3', 'A'],
    ['4', '5', '6', 'B'],
    ['7', '8', '9', 'C'],
    ['*', '0', '#', 'D']
]

# === Password logic ===
password = "1234"
input_password = ""

def read_keypad():
    for row_num, row_pin in enumerate(ROWS):
        row_pin.on()
        for col_num, col_pin in enumerate(COLS):
            if col_pin.value() == 1:
                row_pin.off()
                return keys[row_num][col_num]
        row_pin.off()
    return None

print("🔐 Enter password using keypad:")

while True:
    key = read_keypad()
    if key:
        print("🔘 Key Pressed:", key)

        if key == '#':
            if input_password == password:
                print("✅ Correct password! Rotating servo motor...")
                rotate_servo("cw")
                time.sleep(5)  # Rotate for 5 seconds
                rotate_servo("stop")
                print("🛑 Servo stopped.")
            else:
                print("❌ Wrong password!")
            input_password = ""  # Reset input after checking

        elif key == '*':
            input_password = ""  # Clear input
            print("🔄 Input cleared.")

        else:
            input_password += key
            print("✏️ Entered so far:", input_password)

    time.sleep(0.2)