from machine import Pin, PWM, time_pulse_us
import time


entry_trig = Pin(4, Pin.OUT)
entry_echo = Pin(18, Pin.IN)


exit_trig = Pin(19, Pin.OUT)
exit_echo = Pin(21, Pin.IN)


servo = PWM(Pin(13))
servo.freq(50)


green = Pin(25, Pin.OUT)
yellow = Pin(26, Pin.OUT)
red = Pin(27, Pin.OUT)


MAX_CARS = 15
cars = 0


def distance(trig, echo):
    trig.value(0)
    time.sleep_us(2)

    trig.value(1)
    time.sleep_us(10)

    trig.value(0)

    try:
        duration = time_pulse_us(echo, 1, 30000)
        return duration * 0.0343 / 2
    except:
        return 999


def open_gate():
    servo.duty(77)
    print("GATE OPEN")


def close_gate():
    servo.duty(26)
    print("GATE CLOSED")


def update_leds():

    green.value(0)
    yellow.value(0)
    red.value(0)

    if cars >= 15:
        red.value(1)
    elif cars >= 10:
        yellow.value(1)
    else:
        green.value(1)



close_gate()
update_leds()

print("SMART GARAGE STARTED")
print("Cars:", cars)


while True:

    entry = distance(entry_trig, entry_echo)
    exit = distance(exit_trig, exit_echo)


    if entry < 10:

        if cars < MAX_CARS:

            print("CAR ENTERING")

            open_gate()
            time.sleep(2)

            cars += 1

            print("Cars:", cars)

            close_gate()
            update_leds()

            # Wait until car moves away
            while distance(entry_trig, entry_echo) < 20:
                time.sleep(0.1)

        else:

            print("GARAGE FULL")

            red.value(1)



    if exit < 10:

        if cars > 0:

            print("CAR EXITING")

            open_gate()
            time.sleep(2)

            cars -= 1

            print("Cars:", cars)

            close_gate()
            update_leds()

            while distance(exit_trig, exit_echo) < 20:
                time.sleep(0.1)

        else:

            print("GARAGE EMPTY")


    time.sleep(0.2)