from machine import SoftI2C, Pin, time_pulse_us
from utime import sleep

from led import LED
from button import Button
from buzzer import Buzzer
from stepper import Stepper
from hcsr04 import HCSR04
import oled
import mpu6050

led        = LED(12)        # D12
button     = Button(27)     # D27
buzzer     = Buzzer(15)     # D15
leftWheel  = Stepper(13)    # Step: D13
rightWheel = Stepper(19)    # Step: D19
ultrasonic = HCSR04(5, 18)  # Trigger: D5, Echo: 18

i2c = SoftI2C(scl=Pin(22), sda=Pin(21))
oled = oled.I2C(128, 64, i2c)
mpu = mpu6050.accel(i2c)

# 1 cm -> 11 steps
# 10 cm -> 106 steps
# 49 cm -> 520 steps
def get_steps_from_distance(distance):
    wheel_diameter = 6  # centimeters
    wheel_circumference = wheel_diameter * 3.14159265
    steps_per_revolution = 200
    return int(
        distance * steps_per_revolution / wheel_circumference
    )

# ------------------------- Part 1: Test -------------------------

# while True:
#     value = button.pin.value()
#     print(value)
#     if button.is_pressed():
#         led.on()
#         buzzer.beep_once()
#     else:
#         led.off()
#     sleep(0.5)

# while True:
#     led.off()
#     # Program pauses here until the button is pressed
#     button.wait_button_press()
#     led.on()
#     buzzer.beep_once()
#     # Wait until the button is released before restarting
#     while button.is_pressed():
#         sleep(0.01)

# ------------------------- Part 2: Test -------------------------

# while True:
#     if button.is_pressed():
#         # Opposite directions make the two robot wheels move forward
#         leftWheel.move_one_step()
#         rightWheel.move_one_step()
#     else:
#         sleep(0.01)

# ------------------------- Part 3: Test -------------------------

# while True:
#     try:
#         distance = ultrasonic.distance_cm()
#         print("Distance:", distance, "cm")
#     except OSError as error:
#         print("Ultrasonic error:", error)
#     sleep(0.5)

# test_distances = (10, 20, 30, 50, 100)
# for distance in test_distances:
#     steps = get_steps_from_distance(distance)
#     print(distance, "cm =", steps, "steps")

# while True:
#     led.off()
#     print("Press the button to measure and move")
#     button.wait_button_press()
#     try:
#         distance = ultrasonic.distance_cm()
#         target_steps = get_steps_from_distance(distance)
#         print("Distance:", distance, "cm")
#         print("Target steps:", target_steps)
#         led.on()
#         for step in range(target_steps):
#             leftWheel.move_one_step()
#             rightWheel.move_one_step()
#         buzzer.beep_once()
#         print("Movement complete")
#     except OSError as error:
#         print("Ultrasonic error:", error)
#     # Prevent another measurement while the button remains held
#     while button.is_pressed():
#         sleep(0.01)

# ------------------------- Part 4: Test -------------------------

# print("I2C devices:", [hex(address) for address in i2c.scan()])
# I2C devices: ['0x3c', '0x68']

# OLED test
# while True:
#     try:
#         distance = ultrasonic.distance_cm()
#         print("Distance:", distance, "cm")
#         oled.clear()
#         oled.text("Distance:", 0, 1)
#         oled.text(str(distance) + " cm", 0, 2)
#         oled.show()
#     except OSError as error:
#         print("Ultrasonic error:", error)
#         oled.clear()
#         oled.text("Sensor error", 0, 2)
#         oled.show()
#     sleep(0.5)

# MPU6050 test
# while True:
#     values = mpu.get_values()
#     ac_y = values["AcY"]
#     print("AcY:", ac_y)
#     oled.clear()
#     oled.text("Accelerometer", 0, 1)
#     oled.text("AcY:", 0, 2)
#     oled.text(str(ac_y), 0, 3)
#     oled.show()
#     sleep(0.25)

# Combined test
# while True:
#     try:
#         distance = ultrasonic.distance_cm()
#         values = mpu.get_values()
#         ac_y = values["AcY"]
#         print("Distance:", distance, "cm")
#         print("AcY:", ac_y)
#         oled.clear()
#         oled.text("Distance: " + str(distance), 0, 1)
#         oled.text("cm", 0, 2)
#         oled.text("AcY: " + str(ac_y), 0, 4)
#         oled.show()
#     except OSError as error:
#         print("Ultrasonic error:", error)
#     sleep(0.5)

# ------------------------- Final Project -------------------------

while True:

    # Set LED to off, # IDLE state
    led.off()

    oled.clear()
    oled.text("Press button", 0, 2)
    oled.text("To Start", 0, 3)
    oled.show()

    # Wait here until the button is pressed
    button.wait_button_press()
    buzzer.beep_once()

    # Read the ultrasonic distance
    try:
        distance = ultrasonic.distance_cm()
    except OSError:
        display.clear()
        display.text("Sensor error", 0, 3)
        display.show()
        sleep(3)
        # Require the button to be released
        while button.is_pressed():
            sleep(0.01)
        continue

    target_steps = get_steps_from_distance(distance)
    
    # Display the measured distance and steps
    oled.clear()
    oled.text("Distance: " + str(distance), 0, 2)
    oled.text("Steps: " + str(target_steps), 0, 3)
    oled.show()

    reached = True

    # Drive the required number of steps
    for _ in range(target_steps):
        leftWheel.move_one_step()
        rightWheel.move_one_step()

        # Check the accelerometer after each step
        values = mpu.get_values()
        ac_y = values["AcY"]

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

    oled.clear()

    if reached:
        oled.text("REACHED", 0, 3)
        oled.show()
        buzzer.beep_once()
    else:
        led.on()
        oled.text("TILTED", 0, 3)
        oled.show()
        # Beep three times
        for beep in range(3):
            buzzer.beep_once()
            if beep < 2:
                sleep(0.2)

    # Keep the result visible
    sleep(3)

    # Require a new button press for the next run
    while button.is_pressed():
        sleep(0.01)