import board
import digitalio
import time

SPEED_OF_SOUND = 343  # m/s
TRIGGER_DURATION = 0.00001  # 10 µs

# Internal cache to track history per echo pin
sensor_history = {}

def measure_distance(trigger_pin, echo_pin, sample_window=5):
    global sensor_history

    with digitalio.DigitalInOut(trigger_pin) as trigger, digitalio.DigitalInOut(echo_pin) as echo:
        trigger.direction = digitalio.Direction.OUTPUT
        echo.direction = digitalio.Direction.INPUT

        # Send trigger pulse
        trigger.value = False
        time.sleep(0.0001)
        trigger.value = True
        time.sleep(TRIGGER_DURATION)
        trigger.value = False

        # Timeout setup
        start_time = time.monotonic_ns()
        timeout = start_time + 3_000_000  # 3 ms

        while not echo.value:
            if time.monotonic_ns() > timeout:
                break
        pulse_start = time.monotonic_ns()

        while echo.value:
            if time.monotonic_ns() > timeout:
                return None
        pulse_end = time.monotonic_ns()

        # Calculate raw distance
        pulse_duration = (pulse_end - pulse_start) / 1_000_000_000
        distance_cm = (pulse_duration * SPEED_OF_SOUND * 100) / 2

    # Store reading in history buffer
    key = str(echo_pin)  # Use pin identity as key
    if key not in sensor_history:
        sensor_history[key] = []
    history = sensor_history[key]
    history.append(distance_cm)
    if len(history) > sample_window:
        history.pop(0)

    smoothed = sum(history) / len(history)
    return smoothed