"""
Example for Pi Pico. Blink An External LED (GP1)
Push the Button at pin GP0 (Active High)
"""
import time
import board
import digitalio
import pwmio
import os

#Initialize LEDs
ledRed = digitalio.DigitalInOut(board.GP3)
ledRed.direction = digitalio.Direction.OUTPUT

ledGreen = digitalio.DigitalInOut(board.GP4)
ledGreen.direction = digitalio.Direction.OUTPUT

ledOrange = digitalio.DigitalInOut(board.GP5)
ledOrange.direction = digitalio.Direction.OUTPUT

#Initialize Push Buttons
pb1 = digitalio.DigitalInOut(board.GP0)
pb1.direction = digitalio.Direction.INPUT

pb2 = digitalio.DigitalInOut(board.GP1)
pb2.direction = digitalio.Direction.INPUT

pb3 = digitalio.DigitalInOut(board.GP2)
pb3.direction = digitalio.Direction.INPUT

#PIR SENSOR Initialization
PIRsen = digitalio.DigitalInOut(board.GP26)
PIRsen.direction = digitalio.Direction.INPUT

#SERVO
SERVO_PIN = board.GP15
SERVO_MIN_PULSE_WIDTH = 500
SERVO_MAX_PULSE_WIDTH = 2500
SERVO_FREQ = 50
pwm = pwmio.PWMOut(SERVO_PIN, frequency=SERVO_FREQ, duty_cycle=0)

def angle_to_pulse_width(angle):
    pulse_width = (SERVO_MAX_PULSE_WIDTH - SERVO_MIN_PULSE_WIDTH) * (angle / 180) + SERVO_MIN_PULSE_WIDTH
    return int(pulse_width)

def move_servo(angle):  
    pulse_width = angle_to_pulse_width(angle)
    pwm.duty_cycle = int(pulse_width / (1000000 / SERVO_FREQ / 65535))
    time.sleep(0.5)

def operateGate(led, angle):
    shift = 0
    while not (pb2.value or pb3.value or PIRsen.value):
        led.value = True
        time.sleep(0.5)
        led.value = False
        time.sleep(0.5)
        if shift != angle:
            move_servo(shift)
            shift += 10

def allLedOff():
    ledRed.value = False
    ledGreen.value = False
    ledOrange.value = False

gateStatus = False

def gate():
    global gateStatus
    gateStatus = not gateStatus
    allLedOff()
    if gateStatus:
        operateGate(ledGreen, 90)
    else:
        operateGate(ledRed, 90)
    
firstTime = True
while True:
    print(os.listdir('/'))
    print(PIRsen.value)