from machine import Pin, I2C
from time import sleep
from sys import exit
from buzzer import Buzzer
from a4988 import Stepper
from hcsr04 import HCSR04
import oled
from mpu6050 import accel

# Calculates steps per cm.
steps_per_revolution = 200
stepper_radius_cm = 3
pi = 3.141592653589793
circumference = 2*stepper_radius_cm*pi
steps_per_cm = steps_per_revolution/circumference

# Initialize i2c (to use OLED and accelerometer).
i2c = I2C(0,scl=Pin(22), sda=Pin(21), freq=10000)

# Initialize LED, pushbutton and buzzer.
led = Pin(12, Pin.OUT)
button = Pin(27, Pin.IN, Pin.PULL_UP)
buzz = Buzzer(15)

# Initilaize stepper instances.
l_stepper = Stepper(13)
r_stepper = Stepper(19)

# Intalize ultrasonic sensor instance.
ultra = HCSR04(5, 18)

# Initialize OLED instance.
my_oled = oled.I2C(128, 64, i2c)

# Initialize accelerometer instance.
accelerometer = accel(i2c) 

# Pauses program until button is clicked.
def wait_button_press():
    while button.value() != 0:
        sleep(0.1)

# Moves the stepper motors based on the distance the ultrasonic sensor recieves.
# Also detects the Y-axis state in the accelemotor sensor each step to ensure
# the stopping of the stepper motors if the Y-axis is changed.
def get_steps_from_distance():
    global steps
    global reached_state

    for i in range(steps):

        values = (accelerometer.get_values())
        acy = values["AcY"]
        if 12000 <= acy or -12000 >= acy:
            reached_state = False
            return

        l_stepper.move_one_step()
        r_stepper.move_one_step()

        reached_state = True
        sleep(0.01)


print("Program started.")
sleep(0.5)

# Main loop.
while True:
    led.off()
    
    # Puts text on the OLED.
    my_oled.clear()
    my_oled.text("Press button", 10, 2)
    my_oled.text("to start.", 10, 3)
    my_oled.show()

    # Pauses program until button is clicked.
    wait_button_press()

    # Measures distance then steps.
    distance = ultra.distance_cm()
    steps = round(distance*steps_per_cm)

    # Prints distance and steps.
    my_oled.clear()
    my_oled.text("Distance:", 10, 2)
    my_oled.text(f"{distance} cm.", 10, 3)
    my_oled.text(f"Steps: {steps}.", 10, 4)
    my_oled.show()

    led.on()
    buzz.beep_once()
    led.off()
    sleep(0.1)

    # Moves steppers and checks Y-axis state.
    get_steps_from_distance()

    # Checks if the steppers finished or were stopped.
    if reached_state == True:
        buzz.beep_once()

        my_oled.clear()
        my_oled.text("Reached.", 10, 2)
        my_oled.show()
    
    elif reached_state == False:

        my_oled.clear()
        my_oled.text("Tilted.", 10, 3)
        my_oled.show()

        led.on()
        for i in range(3):
            buzz.beep_once()
            sleep(0.3)

    # Sleep then program resets
    sleep(5)