from machine import Pin, I2C
from time import sleep
from math import pi
from hcsr04 import HCSR04
from buzzer import Buzzer
from stepper import Stepper
from oled import I2C as OLED
from mpu6050 import accel as Accel

WHEEL_DIAMETER = 6 * pi

def wait_button_press(button_pin: Pin, activate_value=0):
    while button_pin() ^ activate_value:
        pass

def get_steps_from_distance(distance: int):
    return distance * 200 / WHEEL_DIAMETER

i2c = I2C(scl=Pin(22), sda=Pin(21))

DISTANCE_SENSOR = HCSR04(trigger_pin=5, echo_pin=18)
BUZZER = Buzzer(15)
MOTORS = (Stepper(13), Stepper(19))
DISPLAY = OLED(128, 64, i2c)
ACCEL = Accel(i2c)
LED = Pin(12, Pin.OUT)
BUTTON1 = Pin(27, Pin.IN, Pin.PULL_UP)

while True:
    LED(0)
    DISPLAY.fill(0)
    DISPLAY.text("Press  ", 0, 0)
    DISPLAY.text("button ", 0, 1)
    DISPLAY.text("to start", 0, 2)
    DISPLAY.show()

    wait_button_press(BUTTON1)
    BUZZER.beep_once()

    DISPLAY.fill(0)
    distance = DISTANCE_SENSOR.distance_cm()
    steps = round(get_steps_from_distance(distance))

    DISPLAY.text(f"{distance:>3} cm", 0, 0)
    DISPLAY.text(f"{steps:>4} steps", 0, 1)
    DISPLAY.show()

    reached = True
    for step in range(steps):
        for motor in MOTORS:
            motor.move_one_step()
        tilt = ACCEL.get_values()["AcY"]

        if tilt < -12000 or tilt > 12000:
            reached = False
            break

    DISPLAY.fill(0)
    if reached:
        DISPLAY.text("REACHED", 61, 0)
        DISPLAY.show()
        BUZZER.beep_once()
    else:
        DISPLAY.text("TILTED ", 61, 0)
        DISPLAY.show()
        LED(1)
        for _ in range(3):
            BUZZER.beep_once()
            sleep(0.1)

    sleep(2)