from machine import Pin, I2C
import time
from mpu6050 import MPU6050  # Works for MPU6500 too!

# --- MPU6050/MPU6500 Setup ---
i2c = I2C(0, scl=Pin(21), sda=Pin(20))
mpu = MPU6050(i2c)

# --- Posture Classification ---
def classify_posture(ax, ay, az):
    threshold_fall = 15.0
    magnitude = (ax**2 + ay**2 + az**2)**0.5

    if magnitude > threshold_fall:
        return 3
    if abs(az) > 9 and abs(ax) < 2 and abs(ay) < 2:
        return 0
    if abs(az) < 6 and (abs(ax) > 3 or abs(ay) > 3):
        return 1
    if abs(az) < 3 and (abs(ax) > 6 or abs(ay) > 6):
        return 2
    return 1

# --- Main Loop ---
while True:
    accel = mpu.get_accel_data()
    gyro = mpu.get_gyro_data()

    ax = accel['x']
    ay = accel['y']
    az = accel['z']

    gx = gyro['x']
    gy = gyro['y']
    gz = gyro['z']

    print(f"Accel: X={ax:.2f}, Y={ay:.2f}, Z={az:.2f}")
    print(f"Gyro: X={gx:.2f}, Y={gy:.2f}, Z={gz:.2f}")

    posture_value = classify_posture(ax, ay, az)
    print(f"Posture Value: {posture_value}")

    time.sleep(1)