from machine import Pin, I2C, time_pulse_us
from time import sleep

# Configuración de pines para sensores ultrasónicos
TRIG_ENTRADA = Pin(2, Pin.OUT)
ECHO_ENTRADA = Pin(3, Pin.IN)

TRIG_SALIDA = Pin(4, Pin.OUT)
ECHO_SALIDA = Pin(5, Pin.IN)

# Configuración de la pantalla LCD I2C
I2C_ADDR = 0x27  # Dirección de la pantalla LCD 
i2c = I2C(0, sda=Pin(0), scl=Pin(1), freq=400000)

# Variables globales
contador_personas = 0

# Comandos básicos para la pantalla LCD
LCD_CLR = 0x01  # Limpiar pantalla
LCD_HOME = 0x02  # Cursor al inicio
LCD_LINE1 = 0x80  # Primera línea
LCD_LINE2 = 0xC0  # Segunda línea

# Inicialización de la pantalla LCD
def lcd_init():
    lcd_send(0x33, 0)
    lcd_send(0x32, 0)
    lcd_send(0x28, 0)
    lcd_send(0x0C, 0)
    lcd_send(0x06, 0)
    lcd_send(LCD_CLR, 0)
    sleep(0.2)

def lcd_send(cmd, is_data):
    # Enviar datos o comandos a la pantalla
    high = (cmd & 0xF0) | 0x08 | (0x01 if is_data else 0)
    low = ((cmd << 4) & 0xF0) | 0x08 | (0x01 if is_data else 0)
    i2c.writeto(I2C_ADDR, bytes([high, high | 0x04, high, low, low | 0x04, low]))
    sleep(0.002)

def lcd_clear():
    lcd_send(LCD_CLR, 0)

def lcd_putstr(text):
    for char in text:
        lcd_send(ord(char), 1)

def lcd_update():
    lcd_send(LCD_LINE1, 0)
    lcd_putstr("Personas: ")
    lcd_putstr(str(contador_personas))

# Función para medir la distancia con los sensores ultrasónicos
def medir_distancia(TRIG, ECHO):
    TRIG.low()
    sleep(0.02)
    TRIG.high()
    sleep(0.00001)
    TRIG.low()

    duracion = time_pulse_us(ECHO, 1)
    distancia = (duracion * 0.0343) / 2
    return distancia

# Programa principal
def main():
    global contador_personas

    lcd_init()
    lcd_update()
    print("Sistema iniciado")
    print("Esperando detección de personas...")

    while True:
        # Sensor de entrada
        distancia_entrada = medir_distancia(TRIG_ENTRADA, ECHO_ENTRADA)
        if 0 < distancia_entrada < 100:
            contador_personas += 1
            print(">>> Persona detectada entrando")
            print(f"Contador actual: {contador_personas}")
            lcd_update()
            sleep(1)

        # Sensor de salida
        distancia_salida = medir_distancia(TRIG_SALIDA, ECHO_SALIDA)
        if 0 < distancia_salida < 100:
            if contador_personas > 0:
                contador_personas -= 1
                print("<<< Persona detectada saliendo")
                print(f"Contador actual: {contador_personas}")
                lcd_update()
            else:
                print("!!! Advertencia: No hay más personas, pero el sensor de salida detectó algo.")
            sleep(1)

        sleep(0.1)

if __name__ == "__main__":
    main()