# import necessary libraries/modules
import machine
from machine import Pin, SoftI2C
from time import sleep
from buzzer import Buzzer
from hcsr04 import HCSR04
from oled import I2C
from mpu6050 import accel

# setup
led = Pin(12, Pin.OUT)
button = Pin(27, Pin.IN, Pin.PULL_UP)
buzzer = Buzzer(15)
step_pin_first = Pin(13, Pin.OUT)
dir_pin_first = Pin(33, Pin.OUT)
step_pin_second = Pin(19, Pin.OUT)
dir_pin_second = Pin(17, Pin.OUT)
sensor = HCSR04(trigger_pin=5, echo_pin=18)
i2c = SoftI2C(scl=Pin(22), sda=Pin(21))
oled = I2C(128, 64, i2c)
accel_sensor = accel(i2c)

# move first motor forward
def move_one_step_first(step_pin, dir_pin):
    dir_pin.value(1)
    step_pin.value(1)
    sleep(0.001)
    step_pin.value(0)
    sleep(0.001)

# move second motor forward
def move_one_step_second(step_pin, dir_pin):
    dir_pin.value(0)
    step_pin.value(1)
    sleep(0.001)
    step_pin.value(0)
    sleep(0.001)

# convert distance to steps
def get_steps_from_distance(d):
    return int((d / 18.85) * 200)

# display message on oled
def msg_on_oled():
    oled.fill(0)
    oled.text("press the button", 0, 0)
    oled.text("to start", 0, 2)
    oled.show()

# wait for button press
def wait_button_press():
    while button.value() == 1:
        pass

# main loop
while True:
    led.value(0)
    msg_on_oled()
    wait_button_press()
    buzzer.beep_once()
    values = accel_sensor.get_values()
    print("AcY:", values["AcY"])
    d = sensor.distance_cm()
    print("distance:", d, "cm")
    oled.fill(0)
    oled.text("distance:", 0, 0)
    oled.text("{:.2f} cm".format(d), 0, 2)
    oled.show()
    steps = get_steps_from_distance(d)
    reached = True
    for _ in range(steps):
        move_one_step_first(step_pin_first, dir_pin_first)
        move_one_step_second(step_pin_second, dir_pin_second)
        values = accel_sensor.get_values()
        acy = values["AcY"]
        if acy > 12000 or acy < -12000:
            reached = False
            break
    if reached:
        oled.fill(0)
        oled.text("REACHED", 0, 0)
        oled.show()
        buzzer.beep_once()
    else:
        led.value(1)
        oled.fill(0)
        oled.text("TILTED", 0, 0)
        oled.show()
        for _ in range(3):
            buzzer.beep_once()
            sleep(0.3)
    sleep(3)