from machine import Pin, PWM, I2C
from time import sleep_ms
from mpu6050 import MPU6050

# Set up the servo and button pins
servo_pin = Pin(1)
button_pin = Pin(4, Pin.IN, Pin.PULL_UP)

# Initialize PWM for the servo
pwm = PWM(servo_pin)
pwm.freq(50)

# Initialize servo position (start at 90 degrees)
pwm.duty_u16(8000)

# Initialize MPU6050
i2c = I2C(scl=Pin(22), sda=Pin(21), freq=100000)
mpu = MPU6050(i2c)

# Track button state
button_pressed = False

while True:
    # Check if the button is pressed
    if not button_pin.value():
        # Toggle servo position
        if button_pressed:
            # Move to 0 degrees
            pwm.duty_u16(1500)
        else:
            # Move to 90 degrees
            pwm.duty_u16(8000)
        # Update button state
        button_pressed = not button_pressed
        # Debounce delay
        sleep_ms(200)
        # Add a 5-second delay after button press
        sleep_ms(5000)

    # Read accelerometer data
    accel = mpu.read_accel_data()
    x_accel, y_accel, z_accel = accel["x"], accel["y"], accel["z"]

    # Check for orientation change (30-degree threshold)
    if abs(x_accel) > 30 or abs(y_accel) > 30:
        # Move servo to 90 degrees
        pwm.duty_u16(8000)
        sleep_ms(500)  # Add a delay to prevent rapid toggling
        # Move servo back to 0 degrees
        pwm.duty_u16(1500)
        sleep_ms(500)  # Add another delay