from machine import Pin
import time
import math
from buzzer import Buzzer
from stepper import Stepper
from hcsr04 import HCSR04

# Initialize LED, button, and buzzer
led = Pin(12, Pin.OUT)
button = Pin(27, Pin.IN, Pin.PULL_UP)
buzzer = Buzzer(15)
hcsr04 = HCSR04(trigger_pin = 5, echo_pin = 18)

# Initialize two stepper motors
stepper_right = Stepper(step_pin=19)
stepper_left = Stepper(step_pin=13)

def wait_button_press():
    """Blocking function that waits until the button is pressed."""
    while button.value() == 1:
        time.sleep(0.1)  

def get_steps_from_distance(distance_cm):
    cm_per_step = (2 * math.pi * 3) / 200
    steps = distance_cm / cm_per_step
    return int(round(steps))



while True:
    # Check if the button is pressed
    if button.value() == 0:
        led.value(1)  # Turn on the LED
        buzzer.beep_once()  # Beep the buzzer once
        print(hcsr04.distance_cm())
        
        # Move both steppers while the button is pressed
        stepper_right.move_one_step()
        stepper_left.move_one_step()
        
        # time to avoid excessive stepping speed
        time.sleep(0.01)  
    else:
        led.value(0)  # Turn off the LED

    # Use the wait_button_press function to block until the button is pressed
    wait_button_press()  # This will block until the button is pressed

    # After the button is pressed
    led.value(1)  # Turn ON the LED
    buzzer.beep_once()  # Beep the buzzer once