print("This Program will measure the thickness of an object using two ultrasonic sensors!")

from hcsr04 import HCSR04
from machine import SoftI2C, Pin
from ssd1306 import SSD1306_I2C
from utime import sleep

# Initialize ultrasonic sensors (top and bottom)
sensor_top = HCSR04(trigger_pin=13, echo_pin=12)
sensor_bottom = HCSR04(trigger_pin=27, echo_pin=26)

# Initialize I2C for OLED
i2c_oled = SoftI2C(scl=Pin(22), sda=Pin(21))
oled = SSD1306_I2C(128, 64, i2c_oled)

while True:
    try:
        # Measure distances (in cm)
        distance_top_cm = sensor_top.distance_cm()
        distance_bottom_cm = sensor_bottom.distance_cm()

        # Convert distances to mm
        distance_top_mm = distance_top_cm * 10
        distance_bottom_mm = distance_bottom_cm * 10

        # Calculate thickness in mm
        thickness_mm = abs(distance_top_mm - distance_bottom_mm)

        # Display on OLED
        oled.fill(0)
        oled.text("THICKNESS DETECT", 0, 0, 1)
        oled.text(f"Top: {distance_top_mm:.1f} mm", 0, 10, 1)
        oled.text(f"Bottom: {distance_bottom_mm:.1f} mm", 0, 20, 1)
        oled.text(f"Thickness: {thickness_mm:.1f} mm", 0, 30, 1)
        oled.show()

        # Print to Serial Monitor
        print(f"Top Distance: {distance_top_mm:.1f} mm")
        print(f"Bottom Distance: {distance_bottom_mm:.1f} mm")
        print(f"Object Thickness: {thickness_mm:.1f} mm")
        print("-----------------------------")

        sleep(1)

    except Exception as e:
        print("Error:", e)
        sleep(1)