# Import necessary libraries
from machine import Pin, PWM, SoftI2C
from time import sleep
from stepper import Stepper
from buzzer import Buzzer
from hcsr04 import HCSR04
import mpu6050
import oled

# Initialize LED on pin 12
led = Pin(12, Pin.OUT)

# Initialize button on pin 27 with pull-up resistor (active low)
button = Pin(27, Pin.IN, Pin.PULL_UP)

# Initialize buzzer on pin 15
buzzer = Buzzer(15)

# Initialize two stepper motors (right and left) on pins 19 and 13
stepper_right = Stepper(19)
stepper_left = Stepper(13)

# Initialize ultrasonic distance sensor (trigger on pin 5, echo on pin 18)
ultrasonic = HCSR04(trigger_pin=5, echo_pin=18)

# Initialize I2C for OLED and accelerometer (SDA=21, SCL=22)
i2c = SoftI2C(scl=Pin(22), sda=Pin(21))

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

# Initialize MPU6050 accelerometer
mpu = mpu6050.accel(i2c)

# Tire and stepper motor parameters
TIRE_RADIUS_CM = 3  # Radius of tire in cm
TIRE_CIRCUMFERENCE_CM = 2 * 3.1416 * TIRE_RADIUS_CM  # Circumference calculation
STEPS_PER_REVOLUTION = 200  # Stepper motor full revolution steps
STEPS_PER_CM = STEPS_PER_REVOLUTION / TIRE_CIRCUMFERENCE_CM  # Steps needed to move 1 cm

# Function to convert distance in cm to equivalent stepper motor steps
def get_steps_from_distance(distance_cm):
    return int(distance_cm * STEPS_PER_CM)

# Function to wait until the button is pressed (active low)
def wait_button_press():
    while button.value() == 1:
        sleep(0.05)  # Poll every 50ms to reduce CPU usage

# Main program loop
while True:
    # Show initial message on OLED to prompt user to press the button
    led.value(0)  # Turn off LED
    oled.fill(0)
    oled.text("Press button", 0, 0)
    oled.text("to start", 1, 1)
    oled.show()

    # Wait for user to press the button
    wait_button_press()

    # Button pressed: play a short beep and turn LED on briefly
    buzzer.beep_once()
    led.value(1)
    sleep(0.05)
    led.value(0)

    # Measure distance using ultrasonic sensor
    try:
        distance_cm = ultrasonic.distance_cm()
    except OSError as e:
        distance_cm = 0  # If error occurs, set distance to 0

    # Calculate number of steps to move based on distance
    steps_to_move = get_steps_from_distance(distance_cm)

    # Display measured distance and calculated steps on OLED
    oled.fill(0)
    oled.text("Distance: {:.2f}cm".format(distance_cm), 0, 0)
    oled.text("Steps: {}".format(steps_to_move), 1, 2)
    oled.show()

    reached = True  # Flag to track if target reached without tilting

    # Move the stepper motors step by step
    for _ in range(steps_to_move):
        stepper_right.move_one_step()
        stepper_left.move_one_step()
        sleep(0.005)  # Small delay for motor stability

        # Read accelerometer Y-axis value to detect tilting
        values = mpu.get_values()
        acy = values["AcY"]
        # If Y-axis acceleration exceeds threshold, consider it tilted
        if acy > 12000 or acy < -12000:
            reached = False
            break  # Stop further steps

    # If target reached successfully
    if reached:
        oled.fill(0)
        oled.text("REACHED", 0, 0)
        oled.show()
        buzzer.beep_once()
    else:
        # If tilted detected
        led.value(1)  # Turn on LED
        oled.fill(0)
        oled.text("TILTED", 0, 0)
        oled.show()
        # Beep three times to alert tilt
        for _ in range(3):
            buzzer.beep_once()
            sleep(0.3)

    # Pause for 3 seconds to show the final message on OLED
    sleep(3)