from machine import Pin, PWM
import time

# ---------- CONFIGURACIÓN ----------
# Servo para puerta
servo = PWM(Pin(0))
servo.freq(50)

# Sensor ultrasónico 1 (entrada)
trigger1 = Pin(2, Pin.OUT)
echo1 = Pin(3, Pin.IN)

# Sensor ultrasónico 2 (salida)
trigger2 = Pin(4, Pin.OUT)
echo2 = Pin(5, Pin.IN)

# ---------- FUNCIONES ----------
def medird(trigger, echo):
    trigger.low()
    time.sleep_us(2)
    trigger.high()
    time.sleep_us(10)
    trigger.low()

    start = time.ticks_us()
    timeout = 30000  # 30 ms

    while echo.value() == 0:
        if time.ticks_diff(time.ticks_us(), start) > timeout:
            return 999  # Sin lectura válida
    pulsoi = time.ticks_us()

    while echo.value() == 1:
        if time.ticks_diff(time.ticks_us(), pulsoi) > timeout:
            return 999
    pulsof = time.ticks_us()

    dur = time.ticks_diff(pulsof, pulsoi)
    distancia = (dur / 2) / 29.1
    return distancia

def abrir_p():
    print("Clave correcta. Abriendo puerta...")
    servo.duty_u16(8000)  # Abrir puerta
    time.sleep(3)
    servo.duty_u16(2000)  # Cerrar puerta
    print("Puerta cerrada.")

# ---------- PROGRAMA PRINCIPAL ----------
clave_correcta = "134340"

try:
    while True:
        d1 = medird(trigger1, echo1)
        if d1 < 999:
            print("Sensor 1 (entrada): {:.1f} cm".format(d1))
        else:
            print("Sensor 1: Sin lectura válida.")
            time.sleep(1)
            continue

        if d1 < 15:  # Carro detectado en el sensor 1
            try:
                clave = input("Ingrese la clave: ")
            except Exception as e:
                print("Error al leer la clave:", e)
                continue

            if clave == clave_correcta:
                abrir_p()

                print("Esperando que el carro pase...")
                while True:
                    d2 = medird(trigger2, echo2)
                    if d2 < 999:
                        print("Sensor 2 (salida): {:.1f} cm".format(d2))
                        if d2 < 15:
                            print("Carro ha pasado.")
                            break
                    else:
                        print("Sensor 2: sin lectura válida.")
                    time.sleep(0.5)
            else:
                print("Clave incorrecta.")

        time.sleep(1)

except KeyboardInterrupt:
    print("Programa detenido manualmente.")