import time
import board
import pwmio
import digitalio
import pulseio

TRIG = digitalio.DigitalInOut(board.GP3)
TRIG.direction = digitalio.Direction.OUTPUT
ECHO = board.GP4

LED = digitalio.DigitalInOut(board.GP14)
LED.direction = digitalio.Direction.OUTPUT

DISTANCE_THRESHOLD = 100
AUTO_OFF_TIMEOUT = 5

led_on_time = None
led_state = False

servo_pwm = pwmio.PWMOut(board.GP2, frequency=50)

def set_servo_angle(pwm, angle):

    duty_cycle = int(65535 * (0.05 + (0.1 * angle / 180)))
    pwm.duty_cycle = duty_cycle

def led_alert(LED, distance):
    global led_on_time, led_state
    if distance <= DISTANCE_THRESHOLD:
        if not led_state:
            print("Person Detected. LED is ON.")
            print("Opening the door...")
        LED.value = True
        set_servo_angle(servo_pwm, 90)
        led_state = True
        led_on_time = time.monotonic()
    else:
        if led_on_time and (time.monotonic() - led_on_time >= AUTO_OFF_TIMEOUT):
            if led_state:
                print(f"No person detected for {AUTO_OFF_TIMEOUT} seconds. LED is OFF.")
                print("Closing the door...")
            set_servo_angle(servo_pwm, 0)
            LED.value = False
            led_state = False
            led_on_time = None


def get_distance():
    TRIG.value = False
    time.sleep(0.002)  
    TRIG.value = True
    time.sleep(0.00001)
    TRIG.value = False

    with pulseio.PulseIn(ECHO, maxlen = 1, idle_state = False) as pulse:
        start_time = time.monotonic()
        while len(pulse) == 0:
            if time.monotonic() - start_time > 0.1:
                return None
            
        pulse_width = pulse[0]
    
    distance = pulse_width * 0.034 / 2
    return distance

while True:
    distance = get_distance()

    if distance is not None:
        print(f"Distance: {distance:.2f} cm.")
        led_alert(LED, distance)
    else:
        print("No echo received from the sensor")
        if led_on_time and (time.monotonic() - led_on_time > AUTO_OFF_TIMEOUT):
            if led_state:
                print(f"No person detected for {AUTO_OFF_TIMEOUT} seconds. LED is OFF")
            LED.value = False
            led_state = False
            led_on_time = None

    time.sleep(1)