from machine import Pin, PWM
from time import sleep, sleep_us, ticks_us

# ============================
#    PINS DISPLAY 7 SEG
# ============================
a = Pin(23, Pin.OUT)
b = Pin(22, Pin.OUT)
c = Pin(21, Pin.OUT)
d = Pin(19, Pin.OUT)
e = Pin(18, Pin.OUT)
f = Pin(5, Pin.OUT)
g = Pin(4, Pin.OUT)

segmentos = [a,b,c,d,e,f,g]

numeros = [
    [1,1,1,1,1,1,0], #0
    [0,1,1,0,0,0,0], #1
    [1,1,0,1,1,0,1], #2
    [1,1,1,1,0,0,1], #3
    [0,1,1,0,0,1,1], #4
    [1,0,1,1,0,1,1], #5
    [1,0,1,1,1,1,1], #6
    [1,1,1,0,0,0,0], #7
    [1,1,1,1,1,1,1], #8
    [1,1,1,1,0,1,1]  #9
]

def mostrar_numero(n):
    if n < 0: n = 0
    if n > 9: n = 9
    for i in range(7):
        segmentos[i].value(numeros[n][i])


# ============================
#  ULTRASONIDOS ENTRADA/SALIDA
# ============================
def crear_ultra(trig_pin, echo_pin):
    return (Pin(trig_pin, Pin.OUT), Pin(echo_pin, Pin.IN))

trig_in, echo_in = crear_ultra(12, 14)     # Entrada
trig_out, echo_out = crear_ultra(26, 25)   # Salida

def distancia_cm(trig, echo):
    trig.value(0)
    sleep_us(2)
    trig.value(1)
    sleep_us(10)
    trig.value(0)

    while echo.value() == 0:
        pass
    t1 = ticks_us()

    while echo.value() == 1:
        pass
    t2 = ticks_us()

    duracion = t2 - t1
    distancia = (duracion * 0.0343) / 2
    return distancia


# ============================
#     SERVOS + LEDS
# ============================
servo_in = PWM(Pin(13), freq=50)
servo_out = PWM(Pin(27), freq=50)

# LEDs
led_in = Pin(32, Pin.OUT)
led_out = Pin(33, Pin.OUT)

def angle_to_duty(angle):
    return int(((angle/180)*102) + 26)

def abrir_entrada():
    servo_in.duty(angle_to_duty(90))
    led_in.value(1)     # LED entrada ON

def cerrar_entrada():
    servo_in.duty(angle_to_duty(0))
    led_in.value(0)     # LED entrada OFF

def abrir_salida():
    servo_out.duty(angle_to_duty(90))
    led_out.value(1)    # LED salida ON

def cerrar_salida():
    servo_out.duty(angle_to_duty(0))
    led_out.value(0)    # LED salida OFF


# ============================
#      VARIABLES DEL SISTEMA
# ============================
espacios_totales = 9
ocupados = 0

cerrar_entrada()
cerrar_salida()
mostrar_numero(espacios_totales)

# ============================
#        LOOP PRINCIPAL
# ============================
while True:

    # ---------- SENSOR DE ENTRADA ----------
    dist_in = distancia_cm(trig_in, echo_in)

    if dist_in < 20 and ocupados < espacios_totales:
        abrir_entrada()
        sleep(1)
        cerrar_entrada()

        ocupados += 1
        mostrar_numero(espacios_totales - ocupados)
        sleep(2)   # anti-rebote


    # ---------- SENSOR DE SALIDA ----------
    dist_out = distancia_cm(trig_out, echo_out)

    if dist_out < 20 and ocupados > 0:
        abrir_salida()
        sleep(1)
        cerrar_salida()

        ocupados -= 1
        mostrar_numero(espacios_totales - ocupados)
        sleep(2)

    sleep(0.1)