from machine import Pin,ADC,PWM
import utime
import time

#Keypad Setup
col=[4,5,6,7]
row=[0,1,2,3]
servo_pin1 = Pin(17)  
servo_pin2 = Pin(15)
servo_pwm1 = PWM(servo_pin1)
servo_pwm2 = PWM(servo_pin2)
servo_pwm1.freq(50)
servo_pwm2.freq(50)

# Servo positions
LEFT = 1638
CENTER = 4915
RIGHT = 8192

UP = 1638
DOWN = 8192

#Set rows pin as output
for x in range(0,4):
    row[x]=Pin(row[x],Pin.OUT)
    row[x].value(1)

#Set cols pin as output
for x in range(0,4):
    col[x]=Pin(col[x],Pin.IN,Pin.PULL_UP)

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

def Keypad4x4Read(cols, rows):
  for r in rows:                # Loop through each row
    r.value(0)                  # Set the current row to LOW (0)
    result = [cols[0].value(),  # Read the values of the columns
              cols[1].value(),
              cols[2].value(),
              cols[3].value()]
    if min(result) == 0:        # Check if any column is LOW (indicating a button press)
      key = key_map[int(rows.index(r))][int(result.index(0))]  # Find the corresponding key
      r.value(1)                # Set the row back to HIGH (1) after detecting the key press
      return key                # Return the pressed key
    r.value(1)                  # Set the row back to HIGH (1) if no key is pressed

flag=0;

print("--- Ready to get user inputs ---")
def keypressing():
    password=""
    global flag
    while True:
        key = Keypad4x4Read(col, row)  
        if key != None:                         # If a key is pressed
            print("Pressed button: " + key)     
            utime.sleep(0.3)                
            password+=key
            if(len(password)==4):
                if(password=="0612"):
                    print("Password Matched")
                    flag=1
                    break
                else:
                    print("Incorrect Password")
                    keypressing()

keypressing()

#Joystick Setup
xAxis=ADC(Pin(27))
yAxis=ADC(Pin(28))
button=Pin(26,Pin.IN,Pin.PULL_UP)

def set_servo_angle(servo, angle):
    duty = int(1638 + (angle / 180) * (8192 - 1638))
    servo.duty_u16(duty)


def moveServo(servo, direction):
    if direction == "left" or direction == "up":
        servo.duty_u16(LEFT)
    elif direction == "right" or direction == "down":
        servo.duty_u16(RIGHT)
    else:
        servo.duty_u16(CENTER)
    time.sleep(0.5)



if flag == 1:
    print("🔓 Access Granted. Use joystick to control servos.")
    while True:
        xValue = xAxis.read_u16()
        yValue = yAxis.read_u16()
        buttonValue = button.value()

        # Determine direction from joystick
        xDirection = "center"
        yDirection = "center"

        if xValue < 15000:
            xDirection = "right"
        elif xValue > 50000:
            xDirection = "left"

        if yValue < 15000:
            yDirection = "down"
        elif yValue > 50000:
            yDirection = "up"

        # Set servo angles based on direction
        if xDirection == "left":
            set_servo_angle(servo_pwm1, 0)
        elif xDirection == "right":
            set_servo_angle(servo_pwm1, 170)
        else:
            set_servo_angle(servo_pwm1, 90)  # center

        if yDirection == "up":
            set_servo_angle(servo_pwm2, 0)
        elif yDirection == "down":
            set_servo_angle(servo_pwm2, 170)
        else:
            set_servo_angle(servo_pwm2, 90)  # center

        # Print debug info
        print(f"X: {xDirection} ({xValue}), Y: {yDirection} ({yValue}), Button: {'Pressed' if buttonValue == 0 else 'Not pressed'}")
        utime.sleep(0.1)