from machine import Pin, I2C, PWM
import time
import struct


# 1
# =====================================
# Initialize I2C communication with MPU6050
i2c = I2C(0, scl=Pin(22), sda=Pin(21))
MPU_ADDR = 0x68

# 2
# Wake up the MPU6050 (disable sleep mode)
i2c.writeto_mem(MPU_ADDR, 0x6B, b'\x00')


# =====================================
# Configure Output Devices
# =====================================
led_green = Pin(2, Pin.OUT)    # Normal operation indicator
led_red = Pin(12, Pin.OUT)     # Fault indicator

buzzer = PWM(Pin(14))
buzzer.freq(2000)
buzzer.duty(0)                 # Buzzer OFF initially

# =====================================
# Vibration Detection Threshold (g)
# Increase or decrease this value
# depending on the simulation sensitivity.
# =====================================
THRESHOLD = 0.15

# =====================================
# Store Previous Accelerometer Readings
# Used to calculate the change (Delta)
# between consecutive measurements.
# =====================================
prev_ax = 0
prev_ay = 0
prev_az = 0

# 3
# =====================================
# Read Raw Accelerometer Data
# Returns X, Y, and Z values
# h represent Signed Short Integer
# =====================================
def read_accel():
    data = i2c.readfrom_mem(MPU_ADDR, 0x3B, 6)
    ax, ay, az = struct.unpack(">hhh", data)
    return ax, ay, az

# 4
# =====================================
# Main Program Loop
# =====================================
while True:

    # Read raw accelerometer measurements
    x, y, z = read_accel()

    # Convert raw readings to acceleration (g)
    #  $-32768  <==> +32767$. -> 16 bit
    # rang : 2g
    # 16384 : represent -> Sensitivity Scale Factor
    x /= 16384
    y /= 16384
    z /= 16384

    # Remove the Earth's gravity from the Z-axis
    z -= 1

    # =====================================
    # Calculate the change (Delta)
    # between the current and previous readings.
    # A large Delta indicates sudden vibration.
    # =====================================
    dx = abs(x - prev_ax)
    dy = abs(y - prev_ay)
    dz = abs(z - prev_az)

    # Save the current readings
    # for the next iteration
    prev_ax = x
    prev_ay = y
    prev_az = z

    # Display Delta values
    print("Change in X: ", dx)
    print("Change in Y: ", dy)
    print("Change in Z: ", dz)

    # =====================================
    # Machine Health Monitoring
    # If the vibration on any axis exceeds
    # the threshold, a fault is detected.
    # =====================================
    if dx > THRESHOLD or dy > THRESHOLD or dz > THRESHOLD:
        led_red.on()
        led_green.off()
        buzzer.duty(512)
        print("Be careful!")
    else:
        led_red.off()
        led_green.on()
        buzzer.duty(0)
        print("safe")
    
    print("----------------------------------------------------")

    # Wait before taking the next sample
    time.sleep(0.1)