import machine
import time

# ==========================================
# PARTE 1: LIBRERIA lcd_api
# ==========================================
class LcdApi:
    LCD_CLR             = 0x01
    LCD_HOME            = 0x02
    LCD_ENTRY_MODE      = 0x04
    LCD_ENTRY_INC       = 0x02
    LCD_ENTRY_SHIFT     = 0x01
    LCD_ON_CTRL         = 0x08
    LCD_ON_DISPLAY      = 0x04
    LCD_ON_CURSOR       = 0x02
    LCD_ON_BLINK        = 0x01
    LCD_MOVE            = 0x10
    LCD_MOVE_DISP       = 0x08
    LCD_MOVE_RIGHT      = 0x04
    LCD_FUNCTION        = 0x20
    LCD_FUNCTION_8BIT   = 0x10
    LCD_FUNCTION_2LINES = 0x08
    LCD_FUNCTION_10DOTS = 0x04
    LCD_CGRAM           = 0x40
    LCD_DDRAM           = 0x80
    LCD_RS_CMD          = 0
    LCD_RS_DATA         = 1
    LCD_RW_WRITE        = 0
    LCD_RW_READ         = 1

    def __init__(self, num_lines, num_columns):
        self.num_lines = num_lines
        if self.num_lines > 4:
            self.num_lines = 4
        self.num_columns = num_columns
        if self.num_columns > 40:
            self.num_columns = 40
        self.cursor_x = 0
        self.cursor_y = 0
        self.implied_newline = False
        self.backlight = True
        self.display_off()
        self.backlight_on()
        self.clear()
        self.hal_write_command(self.LCD_ENTRY_MODE | self.LCD_ENTRY_INC)
        self.hide_cursor()
        self.display_on()

    def clear(self):
        self.hal_write_command(self.LCD_CLR)
        self.hal_write_command(self.LCD_HOME)
        self.cursor_x = 0
        self.cursor_y = 0

    def show_cursor(self):
        self.hal_write_command(self.LCD_ON_CTRL | self.LCD_ON_DISPLAY | self.LCD_ON_CURSOR)

    def hide_cursor(self):
        self.hal_write_command(self.LCD_ON_CTRL | self.LCD_ON_DISPLAY)

    def blink_cursor_on(self):
        self.hal_write_command(self.LCD_ON_CTRL | self.LCD_ON_DISPLAY | self.LCD_ON_CURSOR | self.LCD_ON_BLINK)

    def blink_cursor_off(self):
        self.hal_write_command(self.LCD_ON_CTRL | self.LCD_ON_DISPLAY | self.LCD_ON_CURSOR)

    def display_on(self):
        self.hal_write_command(self.LCD_ON_CTRL | self.LCD_ON_DISPLAY)

    def display_off(self):
        self.hal_write_command(self.LCD_ON_CTRL)

    def backlight_on(self):
        self.backlight = True
        self.hal_backlight_on()

    def backlight_off(self):
        self.backlight = False
        self.hal_backlight_off()

    def move_to(self, cursor_x, cursor_y):
        self.cursor_x = cursor_x
        self.cursor_y = cursor_y
        addr = cursor_x & 0x3f
        if cursor_y & 1:
            addr += 0x40
        if cursor_y & 2:
            addr += self.num_columns
        self.hal_write_command(self.LCD_DDRAM | addr)

    def putchar(self, char):
        if char == '\n':
            if self.implied_newline:
                pass
            else:
                self.cursor_x = self.num_columns
        else:
            self.hal_write_data(ord(char))
            self.cursor_x += 1
        if self.cursor_x >= self.num_columns:
            self.cursor_x = 0
            self.cursor_y += 1
            self.implied_newline = (char != '\n')
        if self.cursor_y >= self.num_lines:
            self.cursor_y = 0
        self.move_to(self.cursor_x, self.cursor_y)

    def putstr(self, string):
        for char in string:
            self.putchar(char)

    def custom_char(self, location, charmap):
        location &= 0x7
        self.hal_write_command(self.LCD_CGRAM | (location << 3))
        self.hal_sleep_us(40)
        for i in range(8):
            self.hal_write_data(charmap[i])
            self.hal_sleep_us(40)
        self.move_to(self.cursor_x, self.cursor_y)

    def hal_backlight_on(self):
        pass

    def hal_backlight_off(self):
        pass

    def hal_write_command(self, cmd):
        raise NotImplementedError

    def hal_write_data(self, data):
        raise NotImplementedError

    def hal_sleep_us(self, usecs):
        time.sleep_us(usecs)


# ==========================================
# PARTE 2: LIBRERIA machine_i2c_lcd
# ==========================================
class I2cLcd(LcdApi):
    MASK_RS = 0x01
    MASK_RW = 0x02
    MASK_E  = 0x04
    SHIFT_BACKLIGHT = 3
    SHIFT_DATA      = 4

    def __init__(self, i2c, i2c_addr, num_lines, num_columns):
        self.i2c = i2c
        self.i2c_addr = i2c_addr
        self.i2c.writeto(self.i2c_addr, bytearray([0]))
        time.sleep_ms(20)
        self.hal_write_init_nibble(self.LCD_FUNCTION_8BIT)
        time.sleep_ms(5)
        self.hal_write_init_nibble(self.LCD_FUNCTION_8BIT)
        time.sleep_ms(1)
        self.hal_write_init_nibble(self.LCD_FUNCTION_8BIT)
        time.sleep_ms(1)
        self.hal_write_init_nibble(self.LCD_FUNCTION)
        time.sleep_ms(1)
        LcdApi.__init__(self, num_lines, num_columns)
        cmd = self.LCD_FUNCTION
        if num_lines > 1:
            cmd |= self.LCD_FUNCTION_2LINES
        self.hal_write_command(cmd)

    def hal_write_init_nibble(self, nibble):
        byte = ((nibble >> 4) & 0x0f) << self.SHIFT_DATA
        self.i2c.writeto(self.i2c_addr, bytearray([byte | self.MASK_E]))
        self.i2c.writeto(self.i2c_addr, bytearray([byte]))

    def hal_backlight_on(self):
        self.i2c.writeto(self.i2c_addr, bytearray([1 << self.SHIFT_BACKLIGHT]))

    def hal_backlight_off(self):
        self.i2c.writeto(self.i2c_addr, bytearray([0]))

    def hal_write_command(self, cmd):
        byte = ((self.backlight << self.SHIFT_BACKLIGHT) | (((cmd >> 4) & 0x0f) << self.SHIFT_DATA))
        self.i2c.writeto(self.i2c_addr, bytearray([byte | self.MASK_E]))
        self.i2c.writeto(self.i2c_addr, bytearray([byte]))
        byte = ((self.backlight << self.SHIFT_BACKLIGHT) | ((cmd & 0x0f) << self.SHIFT_DATA))
        self.i2c.writeto(self.i2c_addr, bytearray([byte | self.MASK_E]))
        self.i2c.writeto(self.i2c_addr, bytearray([byte]))
        if cmd <= 3:
            time.sleep_ms(5)

    def hal_write_data(self, data):
        byte = (self.MASK_RS | (self.backlight << self.SHIFT_BACKLIGHT) | (((data >> 4) & 0x0f) << self.SHIFT_DATA))
        self.i2c.writeto(self.i2c_addr, bytearray([byte | self.MASK_E]))
        self.i2c.writeto(self.i2c_addr, bytearray([byte]))
        byte = (self.MASK_RS | (self.backlight << self.SHIFT_BACKLIGHT) | ((data & 0x0f) << self.SHIFT_DATA))
        self.i2c.writeto(self.i2c_addr, bytearray([byte | self.MASK_E]))
        self.i2c.writeto(self.i2c_addr, bytearray([byte]))


# ==========================================
# PARTE 3: CÓDIGO PRINCIPAL DEL ULTRASÓNICO
# ==========================================

# --- Configuración I2C LCD ---
I2C_ADDR = 0x27
TOTAL_ROWS = 2
TOTAL_COLUMNS = 16
i2c = machine.I2C(0, sda=machine.Pin(0), scl=machine.Pin(1), freq=400000)
lcd = I2cLcd(i2c, I2C_ADDR, TOTAL_ROWS, TOTAL_COLUMNS)

# --- Configuración Sensor Ultrasónico ---
trig = machine.Pin(10, machine.Pin.OUT)
echo = machine.Pin(11, machine.Pin.IN)

def medir_distancia():
    """Envía un pulso y calcula la distancia en centímetros"""
    trig.value(0)
    time.sleep_us(5)
    trig.value(1)
    time.sleep_us(10)
    trig.value(0)
    
    try:
        # Mide el tiempo que tarda el pulso en volver (timeout de 30ms)
        duracion = machine.time_pulse_us(echo, 1, 30000)
        if duracion < 0:
            return -1.0 # Fuera de rango
        # Fórmula: (Tiempo / 2) / 29.1
        distancia = (duracion / 2) / 29.1
        return distancia
    except OSError:
        return -1.0

# --- Configuración de los 8 LEDs ---
led1 = machine.Pin(2, machine.Pin.OUT)
led2 = machine.Pin(3, machine.Pin.OUT)
led3 = machine.Pin(4, machine.Pin.OUT)
led4 = machine.Pin(5, machine.Pin.OUT)
led5 = machine.Pin(6, machine.Pin.OUT)
led6 = machine.Pin(7, machine.Pin.OUT)
led7 = machine.Pin(8, machine.Pin.OUT)
led8 = machine.Pin(9, machine.Pin.OUT)

def apagar_leds():
    """Apaga los 8 LEDs para reiniciar el estado antes de cada lectura"""
    led1.value(0)
    led2.value(0)
    led3.value(0)
    led4.value(0)
    led5.value(0)
    led6.value(0)
    led7.value(0)
    led8.value(0)

# Inicio del programa
lcd.clear()
lcd.putstr("Iniciando...")
time.sleep(1)

while True:
    dist = medir_distancia()

    # Visualizar en pantalla LCD
    lcd.clear()
    lcd.move_to(0, 0)
    lcd.putstr("Distancia:")
    
    lcd.move_to(0, 1)
    if dist < 0:
        lcd.putstr("Fuera de rango")
    else:
        lcd.putstr("{:.1f} cm".format(dist))

    # Lógica de encendido de LEDs (se evalúa en cascada)
    apagar_leds() # Siempre partimos con todos apagados
    
    if dist > 0: # Solo aplicamos lógica si la lectura es válida
        if dist > 19.8:
            led5.value(1)
            
        elif dist > 16.4:
            led6.value(1)
            led7.value(1)
            
        elif dist > 9.6:
            led1.value(1)
            led7.value(1)
            led8.value(1)
            
        elif dist > 8.0:
            led1.value(1)
            led3.value(1)
            led4.value(1)
            
        else: 
            # Menor o igual a 8 cm
            led2.value(1)

    # Pequeña pausa antes de la siguiente medición
    time.sleep(0.5)