from machine import Pin, I2C
import time
from hcsr04 import HCSR04
from lcd_api import LcdApi
from i2c_lcd import I2cLcd

# Function to setup I2C communication
def setup_i2c(scl_pin, sda_pin, freq=400000):
    i2c = I2C(scl=Pin(scl_pin), sda=Pin(sda_pin), freq=freq)
    return i2c

# Function to initialize LCD
def setup_lcd(i2c, address, rows, cols):
    lcd = I2cLcd(i2c, address, rows, cols)
    return lcd

# Function to initialize ultrasonic sensor
def setup_ultrasonic(trigger_pin, echo_pin):
    sensor = HCSR04(trigger_pin=trigger_pin, echo_pin=echo_pin)
    return sensor

# Function to measure distance
def measure_distance(sensor):
    distance = sensor.distance_cm()
    return distance

# Function to display distance on LCD and print to serial
def display_distance(distance, lcd):
    lcd.clear()
    lcd.putstr("Distance:\n{} cm".format(distance))
    print('Distance:', distance, 'cm')

def main():
    # Setup I2C, LCD, and ultrasonic sensor
    i2c = setup_i2c(22, 21)
    lcd = setup_lcd(i2c, 0x27, 2, 16)
    sensor = setup_ultrasonic(25, 26)
    
    while True:
        # Measure distance
        distance = measure_distance(sensor)
        
        # Display distance
        display_distance(distance, lcd)
        
        # Wait for 5 seconds
        time.sleep(5)

if __name__ == "__main__":
    main()