from machine import Pin, I2C
from time import sleep, sleep_ms
from hcsr04 import HCSR04
from stepper import Stepper
from buzzer import Buzzer
from ssd1306 import SSD1306
from mpu6050 import MPU6050

#ultrasonic sensor
ultra = HCSR04(trigger_pin=5, echo_pin=18)

#stepper motors 
left_stepper = Stepper(Pin(13, Pin.OUT), Pin(14, Pin.OUT))  
right_stepper = Stepper(Pin(19, Pin.OUT), Pin(17, Pin.OUT)) 

#button,buzzer,led 
button = Pin(27, Pin.IN, Pin.PULL_UP)
buzzer = Buzzer(15)
led = Pin(12, Pin.OUT)
#i2c,oled,Accelerometer
i2c = I2C(0, scl=Pin(22), sda=Pin(21))
oled = SSD1306(128, 64, i2c)
mpu = MPU6050(i2c)

def wait_button_press():
    while button.value() == 1:
        sleep_ms(50)
    sleep_ms(200)
    while button.value() == 0:
        sleep_ms(50)

def get_steps_from_distance(distance_cm):
    steps_per_rev = 200
    radius_cm = 3
    circumference = 2 * 3.1416 * radius_cm
    steps_per_cm = steps_per_rev / circumference
    return int(distance_cm * steps_per_cm)

print("program started")

while True:
    led.off()
    oled.fill(0)
    oled.text("Press button to",0,0)
    oled.text("start",0,15)  
    oled.show()


    wait_button_press()
    buzzer.beep_once()

    distance = ultra.distance_cm()
    steps = get_steps_from_distance(distance)

    oled.fill(0)
    oled.text("Distance: {} cm".format(distance), 0, 0)
    oled.text("Steps: {}".format(steps), 0, 15)
    oled.show()

    reached = True

    left_stepper.set_direction(forward=False) 
    right_stepper.set_direction(forward=False)

    for _ in range(steps):
        left_stepper.move_one_step()
        right_stepper.move_one_step()

        acc = mpu.get_values()
        y_val = acc["AcY"]

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

    oled.fill(0)
    if reached:
        oled.text("REACHED", 30, 30)
        buzzer.beep_once()
    else:
        led.on()
        oled.text("TILTED", 40, 30)
        for _ in range(3):
            buzzer.beep_once()
            sleep(0.3)
        led.off()

    oled.show()
    sleep(3)