from machine import Pin, I2C
from imu import MPU6050
import time
import math

i2c = I2C(1, sda=Pin(21), scl=Pin(22), freq=400000)
mpu = MPU6050(i2c)

VIBRATION_THRESHOLD = 1.5  # Threshold for significant vibration (in g)
WINDOW_SIZE = 5  # Moving average window size for smoothing

def calculate_vibration():
    ax = mpu.accel.x
    ay = mpu.accel.y
    az = mpu.accel.z - 1.0  # Subtract 1g from Z-axis to remove gravity
    # Calculate magnitude of acceleration vector
    vibration = math.sqrt(ax * ax + ay * ay + az * az)
    return vibration

vibration_avg = 0.0
print("Vibration Testing Started...")
while True:
    try:
        vibration = calculate_vibration()

        print("-" * 50)
        print("Accel X: {:.3f}, Y: {:.3f}, Z: {:.3f} g".format(mpu.accel.x, mpu.accel.y, mpu.accel.z))
        print("Gyro X: {:.3f}, Y: {:.3f}, Z: {:.3f} deg/s".format(mpu.gyro.x, mpu.gyro.y, mpu.gyro.z))
        print("Temperature: {:.2f} °C".format(mpu.temperature))
        print("Vibration: {:.3f} g (smoothed)".format(vibration_avg))
        print(vibration)
  
        if vibration > VIBRATION_THRESHOLD:
            print("⚠ ALERT: Significant vibration detected!")
        else:
            print("Vibration within normal range.")
    except Exception as e:
        print("Error:", e)
    time.sleep(0.5)