from machine import Pin, I2C
from time import sleep, ticks_us, ticks_diff
from pico_i2c_lcd import I2cLcd

# Setup I2C LCD
i2c = I2C(0, sda=Pin(0), scl=Pin(1), freq=400000)
I2C_ADDR = i2c.scan()[0]
lcd = I2cLcd(i2c, I2C_ADDR, 2, 16)

# Ultrasonic pins (match with diagram)
trigger = Pin(22, Pin.OUT)
echo = Pin(21, Pin.IN)

def get_distance():
    # Send pulse
    trigger.low()
    sleep(0.002)
    trigger.high()
    sleep(0.00001)
    trigger.low()

    # Wait for echo to go high with timeout
    timeout = 30000
    start_time = ticks_us()
    while echo.value() == 0:
        if ticks_diff(ticks_us(), start_time) > timeout:
            return -1  # timeout
    start = ticks_us()

    # Wait for echo to go low with timeout
    while echo.value() == 1:
        if ticks_diff(ticks_us(), start) > timeout:
            return -1  # timeout
    end = ticks_us()

    duration = ticks_diff(end, start)
    distance_cm = (duration * 0.0343) / 2
    return distance_cm

# Main loop
while True:
    dist = get_distance()
    lcd.clear()
    lcd.putstr("Distance:")
    lcd.move_to(0, 1)

    if dist == -1:
        lcd.putstr("No Echo")
    else:
        lcd.putstr("{:.2f} cm".format(dist))
    
    sleep(1)