import board
import time
import busio
import digitalio
import lcd
import i2c_pcf8574_interface
import adafruit_hcsr04

sonar = adafruit_hcsr04.HCSR04(trigger_pin=board.GP7, echo_pin=board.GP6)

i2c = busio.I2C(scl=board.GP5, sda=board.GP4)
address = 0x27 #0x3F
i2c = i2c_pcf8574_interface.I2CPCF8574Interface(i2c, address)
display = lcd.LCD(i2c, num_rows=2, num_cols=16)
display.set_backlight(True)
display.set_display_enabled(True)

display.clear()
display.print("Ultrasonic Dist\nMeasurement")
time.sleep(2)

while True:
    # Read and Check Sensor
    try:
        ultra = sonar.distance
        #print(sonar.distance)
    except RuntimeError:
        print("Retrying!")
        pass
    
    # Display
    display.clear()
    display.set_cursor_pos(0, 0)
    display.print("Dist(cm): {:0.2f}".format(ultra))
    time.sleep(2)