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

echo_pin = Pin(2, Pin.IN)
trig_pin = Pin(5, Pin.OUT)

i2c = I2C(0, scl=Pin(22), sda=Pin(21), freq=400000)  

LCD_ADDR = 0x27
LCD_WIDTH = 16  
LCD_CHR = 1
LCD_CMD = 0
LCD_LINE_1 = 0x80  
LCD_LINE_2 = 0xC0  

def lcd_init():
    lcd_byte(0x33, LCD_CMD)  
    lcd_byte(0x32, LCD_CMD)  
    lcd_byte(0x06, LCD_CMD)  
    lcd_byte(0x0C, LCD_CMD)  
    lcd_byte(0x28, LCD_CMD)  
    lcd_byte(0x01, LCD_CMD)  
    sleep_us(500)

def lcd_byte(bits, mode):
    bits_high = mode | (bits & 0xF0) | 0x08  
    bits_low = mode | ((bits << 4) & 0xF0) | 0x08  

    i2c.writeto(LCD_ADDR, bytes([bits_high]))
    lcd_toggle_enable(bits_high)

    i2c.writeto(LCD_ADDR, bytes([bits_low]))
    lcd_toggle_enable(bits_low)

def lcd_toggle_enable(bits):
    sleep_us(500)
    i2c.writeto(LCD_ADDR, bytes([bits | 0x04]))
    sleep_us(500)
    i2c.writeto(LCD_ADDR, bytes([bits & ~0x04]))
    sleep_us(500)

def lcd_clear():
    lcd_byte(0x01, LCD_CMD)

def lcd_string(message, line):
    message = message[:LCD_WIDTH]  
    message = message + ' ' * (LCD_WIDTH - len(message))  
    lcd_byte(line, LCD_CMD)
    for i in range(LCD_WIDTH):
        lcd_byte(ord(message[i]), LCD_CHR)

def measure_distance():
    trig_pin.value(0)
    sleep_us(2)
    trig_pin.value(1)
    sleep_us(10)
    trig_pin.value(0)
    
    x = machine.time_pulse_us(echo_pin, 1)
    distance = (250.0 * x) / 2  
    return distance

def main():
    lcd_init()
    while True:
        distance = measure_distance()
        lcd_clear()
        lcd_string("Distance: {} cm".format(distance), LCD_LINE_1)
        sleep(1)

if __name__ == "__main__":
    main()