from machine import Pin, SoftI2C
from time import sleep
from buzzer import Buzzer
from stepper import Stepper
from hcsr04 import HCSR04
from oled import I2C as OLED_I2C  
from mpu6050 import accel

#Components 
led = Pin(12, Pin.OUT)
button = Pin(27, Pin.IN, Pin.PULL_UP)
buzzer = Buzzer(15)
right_stepper = Stepper(19)
left_stepper = Stepper(13)
ultrasonic = HCSR04(5, 18)
# i2c  for OLEd and mpu
i2c = SoftI2C(scl=Pin(22), sda=Pin(21), freq=100000)
oled = OLED_I2C(128, 64, i2c)
mpu = accel(i2c)


#constants
steps_per_revolution = 200
tire_diameter_cm = 6
circumference_cm = 3.1416 * tire_diameter_cm
steps_per_cm = steps_per_revolution / circumference_cm
tilt_threshold = 1200




def move_motor(distance_cm):
    steps = get_steps_from_distance(distance_cm)
    for i in range(steps):
        left_stepper.move_one_step()
        right_stepper.move_one_step()
        acy = mpu.get_values()["AcY"]
        if abs(acy) > tilt_threshold:
            return False # tilted
    return True # reached

def get_steps_from_distance(distance_cm):
    return int (distance_cm * steps_per_cm)

def display_idle():
    oled.fill(0)
    oled.text("press button to", 0, 0)
    oled.text("start", 0, 2)
    oled.show()

def display_reached():
    oled.fill(0)
    oled.text("REACHED", 4, 2)
    oled.show()

def display_tilted():
    oled.fill(0)
    oled.text("TILTED", 0, 0)
    oled.show()

def display_distance_and_steps(distance,steps):
    oled.fill(0)
    oled.text("Distance: {:.2f}".format(distance), 0, 0)
    oled.text("Steps: {}".format(steps), 0, 2)
    oled.show()

def beep(times, delay = 0.2):
    for i in range(times):
        buzzer.beep_once()
        sleep(delay)




# blocking function
def wait_button_press():
    while button.value(): # in case button is not pressed
        sleep(0.1) 
    sleep(0.2) # for debounce
    return # exit function in case that the button is pressed
    

while True:
    led.off()
    display_idle()
    # print("displaying idle message") # Debugging print
    wait_button_press() 

   # in case that the button is pressed(after press)
    beep(1)  
    sleep(0.2)

    # read distance from ultrasonic
    distance = ultrasonic.distance_cm()
    steps = get_steps_from_distance(distance)
    # print(steps) # debugging print
    display_distance_and_steps(distance, steps)

    # save the status of motor motion in var to track either the motor is reached or tilted
    reached = move_motor(distance)

    # during motion 
    if reached :
        display_reached()
        beep(1)
    else:
        led.on()
        display_tilted()
        beep(3)


    sleep(3)