from machine import Pin, time_pulse_us
from time import sleep, sleep_us, sleep_ms

botonOnOFF = Pin(34, Pin.IN) #Amarillo

#Sensor1
trig1 = Pin(23, Pin.OUT) #Verde
echo1 = Pin(22, Pin.IN) #Azul
vibrar1 = Pin(32, Pin.OUT) #Café

#Sensor2
trig2 = Pin(21, Pin.OUT)
echo2 = Pin(19, Pin.IN)
vibrar2 = Pin(33, Pin.OUT)

#Sensor3
trig3 = Pin(18, Pin.OUT)
echo3 = Pin(5, Pin.IN)
vibrar3 = Pin(25, Pin.OUT)

sistemaActivo = 0 

def medirDistancia(trig, echo):
    trig.off()
    sleep_us(2)
    trig.on()
    sleep_us(10)
    trig.off()
    duracion = time_pulse_us(echo, 1, 30000)
    if duracion < 0:  
        return None
    return duracion * 0.0343 / 2

def frecuencia_vibracion(distancia):
    if distancia < 30:
        return 80
    elif distancia < 50:
        return 150
    elif distancia < 80:
        return 300
    else:
        return None

def manejar_sensor(trig, echo, vibrar, nombre="S"):
    distancia = medirDistancia(trig, echo)
    if distancia is None:
        vibrar.off()
        return
    print(nombre, ":", distancia, "cm")
    intervalo = frecuencia_vibracion(distancia)
    if intervalo:
        vibrar.on()
        sleep_ms(intervalo // 2)
        vibrar.off()
        sleep_ms(intervalo // 2)
    else:
        vibrar.off()
        
while True:
    if botonOnOFF.value() == 1 and sistemaActivo == 0:
        while botonOnOFF.value() == 1:
            pass
        sistemaActivo = 1
        print("Sistema Activo")

    if sistemaActivo == 1 and botonOnOFF.value() == 0:
        manejar_sensor(trig1, echo1, vibrar1, "S1")
        manejar_sensor(trig2, echo2, vibrar2, "S2")
        manejar_sensor(trig3, echo3, vibrar3, "S3")

    elif sistemaActivo == 1 and botonOnOFF.value() == 1:
        while botonOnOFF.value() == 1:
            pass
        sistemaActivo = 0
        print("Sistema Inactivo")
        vibrar1.off()
        vibrar2.off()
        vibrar3.off()

    sleep(0.05)

"""
botonOnOFF = Pin(34, Pin.IN)
botonEncontrar = Pin(16, Pin.IN)
trig = Pin(25, Pin.OUT)
echo = Pin(33, Pin.IN)
vibrar = Pin(17, Pin.OUT)
aviso = Pin(18, Pin.OUT)
encontrar = Pin(5, Pin.OUT)
sistemaActivo = 0
ultimo_bip = 0

def medirDistancia():
    trig.off()
    sleep_us(2)
    trig.on()
    sleep_us(10)
    trig.off()
    duracion = time_pulse_us(echo, 1, 30000)
    distancia = duracion * 0.0343 / 2
    return distancia

def frecuencia_vibracion(distancia):
    if distancia < 50:
        return 80
    elif distancia < 100:
        return 150
    elif distancia < 150:
        return 300
    elif distancia < 200:
        return 500
    else:
        return None

def checar_buzzer(distancia):
    global ultimo_bip
    ahora = ticks_ms()
    if (195 <= distancia <= 205 or
        145 <= distancia <= 155 or
        95 <= distancia <= 105 or
        45 <= distancia <= 55):
        if ticks_diff(ahora, ultimo_bip) > 1000:
            aviso.on()
            sleep(0.1)
            aviso.off()
            ultimo_bip = ahora

while True:
    if botonEncontrar.value() == 1:
        encontrar.on()
        sistemaActivo = 0
    else:
        encontrar.off()
        if sistemaActivo == 0 and botonOnOFF.value() == 1:
            while botonOnOFF.value() == 1:
                pass
            sistemaActivo = 1
            print("Sistema Activo")

        if sistemaActivo == 1 and botonOnOFF.value() == 0:
            distancia = medirDistancia()
            print("Distancia:", distancia, "cm")

            checar_buzzer(distancia)

            intervalo = frecuencia_vibracion(distancia)
            if intervalo:
                vibrar.on()
                sleep_ms(intervalo // 2)
                vibrar.off()
                sleep_ms(intervalo // 2)
            else:
                vibrar.off()

        elif sistemaActivo == 1 and botonOnOFF.value() == 1:
            while botonOnOFF.value() == 1:
                pass
            sistemaActivo = 0
            print("Sistema Inactivo")
            aviso.off()
            vibrar.off()
    sleep(0.05)
    """

"""
#con boton de emergencia
botonOnOFF= Pin(34, Pin.IN)
botonEncontrar= Pin(16, Pin.IN)
trig= Pin(25, Pin.OUT)
echo= Pin(33, Pin.IN)
vibrar= Pin(17, Pin.OUT)
aviso= Pin(18, Pin.OUT)
encontrar= Pin(5, Pin.OUT)
sistemaActivo=0
def medirDistancia():
    trig.off()
    sleep_us(2)
    trig.on()
    sleep_us(10)
    trig.off()

    duracion = time_pulse_us(echo, 1, 30000)
    distancia = duracion * 0.0343 / 2
    return distancia
while True:
    if botonEncontrar.value()==1:
        encontrar.on()
        sistemaActivo=0
    else:
        encontrar.off()
        if sistemaActivo==0 and botonOnOFF.value()==1:
            while botonOnOFF.value()==1:
                pass
            sistemaActivo=1
            print("sistema Activo")
        if sistemaActivo==1 and botonOnOFF.value()==0:
            distancia = medirDistancia()
            print("Distancia:", distancia, "cm")
            if distancia <= 50:
                if distancia<=50 and distancia>=45:
                    aviso.on()
                elif distancia<=30 and distancia>=25:
                    aviso.on()
                elif distancia<=10:
                    aviso.on()
                else:
                    aviso.off()
                    vibrar.on()
            else:
                vibrar.off()
                aviso.off()
        elif sistemaActivo==1 and botonOnOFF.value()==1:
            while botonOnOFF.value()==1:
                pass
            sistemaActivo=0
            print("sistema Inactivo")
            aviso.off()
            vibrar.off()
        sleep(0.1)
""""""
#sin boton de encontar
botonOnOFF= Pin(34, Pin.IN)
trig= Pin(25, Pin.OUT)
echo= Pin(33, Pin.IN)
vibrar= Pin(17, Pin.OUT)
aviso= Pin(18, Pin.OUT)
sistemaActivo=0
def medirDistancia():
    trig.off()
    sleep_us(2)
    trig.on()
    sleep_us(10)
    trig.off()

    duracion = time_pulse_us(echo, 1, 30000)
    distancia = duracion * 0.0343 / 2
    return distancia
while True:
    if sistemaActivo==0 and botonOnOFF.value()==1:
        while botonOnOFF.value()==1:
            pass
        sistemaActivo=1
        print("sistema Activo")
    if sistemaActivo==1 and botonOnOFF.value()==0:
        distancia = medirDistancia()
        print("Distancia:", distancia, "cm")
        if distancia <= 50:
            if distancia<=50 and distancia>=45:
                aviso.on()
            elif distancia<=30 and distancia>=25:
                aviso.on()
            elif distancia<=10:
                aviso.on()
            else:
                aviso.off()
                vibrar.on()
        else:
            vibrar.off()
            aviso.off()
    elif sistemaActivo==1 and botonOnOFF.value()==1:
        while botonOnOFF.value()==1:
            pass
        sistemaActivo=0
        print("sistema Inactivo")
        aviso.off()
        vibrar.off()
    sleep(0.1)
    """