# Import necessary modules
from machine import Pin, SoftI2C     # For GPIO and I2C communication
from time import sleep               # To add delays
from buzzer import Buzzer            # Custom buzzer control
from stepper import Stepper          # Custom stepper motor control
from hcsr04 import HCSR04            # Ultrasonic sensor module
from oled import I2C as oled_driver  # OLED screen driver
from mpu6050 import accel            # Accelerometer module

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

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

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

# Initialize stepper motors
stepper_left = Stepper(13)
stepper_right = Stepper(19)

# Initialize ultrasonic sensor (trigger pin = 5, echo pin = 18)
ultrasonic = HCSR04(5, 18)

# Initialize software I2C for OLED and accelerometer (MPU6050)
i2c = SoftI2C(scl=Pin(22), sda=Pin(21), freq=400000)

# Initialize OLED display with size 128x64 using detected I2C device
oled = oled_driver(128, 64, i2c)

# Initialize accelerometer object
mpu = accel(i2c)

# Function to wait for the button press (blocking code until pressed)
def wait_button_press():
  while btn.value() == 1:
      sleep(0.05)

# Convert distance in cm to number of steps for the stepper motors
def get_steps_from_distance(dist):
  circumference = 18.5  # Circumference of wheel in cm (2πr = 2π×3)
  steps_per_cm = 200 / circumference  # Steps per cm based on 200 steps/rev motor
  no_steps = dist * steps_per_cm
  return int(no_steps)  # Return whole number of steps

# Main loop
while True:
  # LED off and OLED prompts user to press button
  led.off()
  oled.clear()
  oled.text("Press button", 10, 2)
  oled.text("to start", 10, 3)
  oled.show()

  # Wait for button press
  wait_button_press()

  # Beep once and measure distance
  bzzr.beep_once()
  distance = ultrasonic.distance_cm()
  steps = get_steps_from_distance(distance)

  # Display measured distance and calculated steps
  oled.clear()
  oled.text(f"distance:{str(distance)}", 10, 3)
  oled.text(f"steps:{str(steps)}", 10, 4)
  oled.show()

  # Movement loop
  reached = True
  for i in range(steps):
      stepper_left.move_one_step()
      stepper_right.move_one_step()

      # Read Y-axis acceleration to detect tilting
      values = mpu.get_values()
      acy = values["AcY"]

      # If tilted significantly on Y-axis, break loop
      if acy > 12000 or acy < -12000:
          reached = False
          break

  # Seeing if the robot was tilted or not
  if reached:
      oled.clear()
      oled.text("REACHED", 10, 2)
      oled.show()
      bzzr.beep_once()
  else:
      led.on()
      oled.clear()
      oled.text("TILTED", 10, 2)
      oled.show()
      for i in range(3):
          bzzr.beep_once()
          sleep(0.5)

  #Leaving time for user to read the message on OLED
  sleep(3)