from machine import Pin, Timer
import utime

# Ultrasonic Sensor Setup
TRIG_PIN = 2
ECHO_PIN = 3
trig = Pin(TRIG_PIN, Pin.OUT)
echo = Pin(ECHO_PIN, Pin.IN)

# 7-Segment Display Setup
# Segment pins (A-G, DP)
segment_pins = [Pin(i, Pin.OUT) for i in range(4, 12)]
# Digit control pins (D1-D4)
digit_pins = [Pin(i, Pin.OUT) for i in range(12, 16)]

# Segment configurations for digits 0-9 (for common cathode displays)
digits = [
    [1, 1, 1, 1, 1, 1, 0],  # 0
    [0, 1, 1, 0, 0, 0, 0],  # 1
    [1, 1, 0, 1, 1, 0, 1],  # 2
    [1, 1, 1, 1, 0, 0, 1],  # 3
    [0, 1, 1, 0, 0, 1, 1],  # 4
    [1, 0, 1, 1, 0, 1, 1],  # 5
    [1, 0, 1, 1, 1, 1, 1],  # 6
    [1, 1, 1, 0, 0, 0, 0],  # 7
    [1, 1, 1, 1, 1, 1, 1],  # 8
    [1, 1, 1, 1, 0, 1, 1]   # 9
]

def measure_distance():
    trig.low()
    utime.sleep_us(2)
    trig.high()
    utime.sleep_us(10)
    trig.low()
    while echo.value() == 0:
        signal_off = utime.ticks_us()
    while echo.value() == 1:
        signal_on = utime.ticks_us()
    time_passed = utime.ticks_diff(signal_on, signal_off)
    distance = (time_passed * 0.0343) / 2
    return int(distance)  # Convert to integer for display

def display_digit(digit, value):
    # Turn off all digits
    for d in digit_pins:
        d.low()
    # Set segments for the value
    for i in range(7):
        segment_pins[i].value(digits[value][i])
    # Turn on the appropriate digit
    digit_pins[digit].high()
    utime.sleep_ms(5)

def display_number(number):
    # Convert number to a zero-padded 4-digit string
    str_num = "{:04}".format(number)
    for i, digit in enumerate(str_num):
        display_digit(i, int(digit))

# Main loop to update display and measure distance
while True:
    distance = measure_distance()
    display_number(distance)
    utime.sleep(0.1)