import grovepi
from sensors.lock import safe_grove_access
from shared import config

ULTRASONIC_RANGER_PORT = 4

def read_ultrasonic_ranger(ultrasonic_ranger=ULTRASONIC_RANGER_PORT):
    with safe_grove_access(timeout=1.5) as acquired:
        if not acquired:
            return None
            
        try:
            return grovepi.ultrasonicRead(ultrasonic_ranger)
        except Exception as e:
            print(f"Ultrasonic read error: {e}")
            return None

def get_dish_height():
    """Returns the height of the dish in centimeters."""
    distance = read_ultrasonic_ranger()
    if distance is not None:
        dish_height = config.COOKING_COMPARTMENT_HEIGHT - distance
        return max(dish_height, 0)
    return None