from machine import Pin, SoftI2C
from time import sleep
from stepper import Stepper
from hcsr04 import HCSR04
from ssd1306 import SSD1306_I2C
from mpu6050 import accel

i2c = SoftI2C(scl=15, sda=2, freq=400000)
sensor = HCSR04(23, 22)
motor = Stepper(17, 16)
oled = SSD1306_I2C(128, 64, i2c)
mpu = accel(i2c)

PARKING_DISTANCE = 15
ENGINE_THRESHOLD = 1.2
ENGINE_STATE = False

def updateOled(distance) -> None:
    oled.fill(0)
    if ENGINE_STATE:
        oled.text("ES: RUNNING", 0, 0)
    else:
        oled.text("ES: N/A", 0, 0)
    oled.text("Distance: " + str(distance), 0, 10)
    oled.show()

def updateEverything():
    global ENGINE_STATE

    distance = sensor.distance_cm()
    accelerations = mpu.get_values()
    xAcel = accelerations["AcX"]/16384
    yAcel = accelerations["AcY"]/16384
    vibration = abs(xAcel) + abs(yAcel)

    updateOled(distance)
    
    if vibration > ENGINE_THRESHOLD:
        ENGINE_STATE = True
    else:
        ENGINE_STATE = False

    return distance

def main() -> None:
    while True:
        distance = updateEverything()
        
        if distance <= PARKING_DISTANCE and ENGINE_STATE == False:
            motor.abs_angle(90)
            while distance <= PARKING_DISTANCE:
                distance = updateEverything()
                sleep(0.05)
            motor.abs_angle(0)
        
        sleep(0.1)

if __name__ == "__main__":
    main()