from machine import Pin, I2C
from i2c_lcd import I2cLcd
import time

# Define the GPIO pins for TRIG and ECHO
TRIG_PIN = 3
ECHO_PIN = 2

# Set up the TRIG and ECHO pins
trig = Pin(TRIG_PIN, Pin.OUT)
echo = Pin(ECHO_PIN, Pin.IN)

# Define I2C pins and initialize I2C
I2C_SDA_PIN = 20
I2C_SCL_PIN = 21
i2c = I2C(0, sda=Pin(I2C_SDA_PIN), scl=Pin(I2C_SCL_PIN), freq=400000)

# Define the LCD address (usually 0x27 or 0x3F)
I2C_ADDR = 0x27

# Initialize the LCD
lcd = I2cLcd(i2c, I2C_ADDR, 2, 16)

def sleep_us(us):
    start = time.ticks_us()
    while time.ticks_diff(time.ticks_us(), start) < us:
        pass

def get_distance():
    # Ensure the trigger pin is low
    trig.low()
    sleep_us(2)
    
    # Send a 10us pulse to trigger the measurement
    trig.high()
    sleep_us(10)
    trig.low()
    
    # Measure the duration of the echo pulse
    duration = time_pulse_us(echo, 1, 30000)  # Timeout of 30000 us (30 ms)
    
    if duration < 0:
        # If we have a timeout or error in reading, return None
        return None
    
    # Calculate distance in cm
    distance_cm = (duration / 2) / 29.1
    
    return distance_cm

last_measurement = time.ticks_ms()

while True:
    current_time = time.ticks_ms()
    
    if time.ticks_diff(current_time, last_measurement) >= 1000:  # 1000 ms = 1 second
        distance = get_distance()
        if distance is None:
            lcd.clear()
            lcd.puts("Out of range")
        else:
            lcd.clear()
            lcd.puts("Distance: {:.2f} cm".format(distance))
        
        last_measurement = current_time
    
    # Other code can run here without being blocked