# see:
# https://randomnerdtutorials.com/raspberry-pi-pico-hc-sr04-micropython/

from machine import Pin, I2C
import ssd1306
import time
from hcsr04 import HCSR04

time.sleep(0.1) # Wait for USB to become ready

# I2C setup: using I2C1 with GP2 (SDA) and GP3 (SCL)
i2c = I2C(1, scl=Pin(3), sda=Pin(2), freq=400000)

# Scan for devices
devices = i2c.scan()
if devices:
    print("I2C devices found:", [hex(dev) for dev in devices])
else:
    print("No I2C devices found")

# Initialize display (most SSD1306 are 128x64, some are 128x32)
oled_width = 128
oled_height = 64
oled = ssd1306.SSD1306_I2C(oled_width, oled_height, i2c)

# Initialize the HC-SR04 sensor with trigger on GPIO 27 and echo on GPIO 28
sensor = HCSR04(trigger_pin=17, echo_pin=16, echo_timeout_us=30000)

# Clear the display
oled.fill(0)

# Show some text
oled.text('Distance:', 0, 0)

# Update the display
oled.show()

while True:
    try:
        # Measure distance in centimeters
        distance_cm = sensor.distance_cm()
        # Convert distance from centimeters to inches
        #distance_inch = distance_cm * 0.393701
        #print('Distance: {:.2f} cm'.format(distance_cm))
        # Measure distance in millimeters
        #distance_mm = sensor.distance_mm()
        #print('Distance: {} mm'.format(distance_mm))

        oled.fill(0)
        oled.text("Distance:", 0, 10)
        oled.text("{:.1f} cm".format(distance_cm), 0, 30)
        oled.show()

    except OSError as e:
        print('Error:', e)
    
    # Wait 1 second before the next measurement
    time.sleep(1)