import grovepi
from sensors.lock import grove_lock

# Connect the Grove Ultrasonic Ranger to digital port D4
# SIG,NC,VCC,GND
ULTRASONIC_RANGER_PORT = 4

def read_ultrasonic_ranger(ultrasonic_ranger=ULTRASONIC_RANGER_PORT):
    if not grove_lock.acquire(timeout=1.0):
        print("Ultrasonic: Lock acquisition timed out")
        return None
        
    try:
        return grovepi.ultrasonicRead(ultrasonic_ranger)
    except Exception as e:
        print(f"Error: {e}")
        return None
    finally:
        grove_lock.release()
    
def get_dish_height():
    """Returns the height of the dish in centimeters."""
    distance = read_ultrasonic_ranger()
    if distance is not None:
        # Assuming the ultrasonic sensor is mounted at a fixed height above the dish
        # and pointing downwards, we can calculate the height of the dish.
        # For example, if the sensor is 30 cm above the dish when it's empty:
        SENSOR_HEIGHT = 30  # cm
        dish_height = SENSOR_HEIGHT - distance
        return max(dish_height, 0)  # Ensure height is not negative
    else:
        return None