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.
button = Pin(27, Pin.IN, Pin.PULL_UP)
buzz = Buzzer(15)

# Initilaize stepper instances.
l_stepper = Stepper(13, 14)

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

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

# Initialize accelerometer instance.
accelerometer = accel(i2c) 

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

# Main loop.
while True:
    distance = ultra.distance_cm()

    values = (accelerometer.get_values())
    acy = values["AcY"]
    if 12000 <= acy or -12000 >= acy:
        tilted = "Yes"
    else:
        tilted = "No"

    direction = "CW" if l_stepper.know_dir() == 1 else "CCW"
    
    # Puts text on the OLED.
    my_oled.clear()
    my_oled.text("Distance: ", 10, 1)
    my_oled.text(f"{distance} cm.", 10, 2)
    my_oled.text(f"Tilted: {tilted}.", 10, 3)
    my_oled.text(f"Direction: {direction}.", 10, 4)
    my_oled.show()


    if button.value() == 0:
        if tilted == "No":
            for i in range(200):
                l_stepper.move_one_step()
                sleep(0.0001)
            l_stepper.change_dir()
            buzz.beep_once()
        else:
            buzz.beep_once()
            buzz.beep_once()
            buzz.beep_once()
    if distance < 30:
        if tilted == "No":
            if l_stepper.know_dir() == 1:
                for i in range(200):
                    l_stepper.move_one_step()
                    sleep(0.0001)
                l_stepper.change_dir()
                buzz.beep_once()

    sleep(0.001)