import time
import machine
import buzzer
import stepper
import hcsr04
import oled
import mpu6050

# initialize the LED on pin 12 and set it to low.
LED = machine.Pin(12, machine.Pin.OUT)
LED.value(0)

# initialize the button and wire it to pin D27 (remember to enable pull_up).
BTN = machine.Pin(27, machine.Pin.IN, machine.Pin.PULL_UP)

# create a Buzzer class passing the pin number (15)
BUZ = buzzer.Buzzer(15)

# Add a Ultrasonic Sensor (HC-SR04) (trigger_pin=5, echo_pin=18)
DistSensor = hcsr04.HCSR04(5, 18) 

# Create I2C bus for the OLED and the MPU6050
AppI2C = machine.I2C(1, sda=machine.Pin(21), scl=machine.Pin(22), freq = 400000)

# Connect OLED to I2C Bus
Display = oled.I2C(128, 64, AppI2C)
Display.clear()


# Connect Accel to I2C Bus
Accel = mpu6050.accel(AppI2C)

# Connect Right Motor to Pin 19
MOT_R_Pin = machine.Pin(19, machine.Pin.OUT)
MOT_R = stepper.Stepper(MOT_R_Pin);

# Connect Right Motor to Pin 13
MOT_L_Pin = machine.Pin(13, machine.Pin.OUT)
MOT_L = stepper.Stepper(MOT_L_Pin);

def wait_button_press():
    while BTN.value():
        pass

TireCircunference = 2 * 3.14 * 3    # circunference in cm 
MotorSteps = 200                    # Motor steps in Full Mode
def get_steps_from_distance(dist):
    return int((dist / TireCircunference) * 200)


while True:
    # Display Initial Message
    Display.clear()
    Display.text("Press button to start", 0, 0,1)
    Display.show()

    # Wait until the Start Button is pressed
    wait_button_press()
    BUZ.beep_once()

    # Measure the distance ans steps
    Distance = DistSensor.distance_cm()
    Steps = get_steps_from_distance(Distance)
    reached = True

    # Display the measured Disatnce and steps
    Display.clear()
    Display.text("Distance= " + str(Distance), 0, 0,1)
    Display.text("Steps= " + str(Steps), 0, 3,1)
    Display.show()

    for step in range(Steps):
        # Move both Motors 
        MOT_R.step()
        MOT_L.step()
        # Checj if the robot is tilted
        Accel_Y = Accel.get_values()["AcY"]
        if ((Accel_Y > 12000) or (Accel_Y < -12000)):
            reached = False
            break

    if (reached):
        Display.clear()
        Display.text("REACHED", 0, 0,1)
        Display.show()
        BUZ.beep_once()
    else:
        Display.clear()
        Display.text("TILTED", 0, 0,1)
        Display.show()
        
        BUZ.beep_once()
        time.sleep(0.1)
        BUZ.beep_once()
        time.sleep(0.1)
        BUZ.beep_once()
        LED.value(1)

    time.sleep(3)