from machine import Pin, I2C
from time import sleep, ticks_ms, ticks_diff, ticks_us
import struct

i2c = I2C(0, scl=Pin(22), sda=Pin(21))
MPU_ADDR = 0x68
REG = 0x6B
ACCEL_REG = 0x3B
GYRO_REG = 0x43
TEMP_REG = 0x41
bytes_per_short = 2
accel_bytes, gyro_bytes, temp_bytes = 3, 3, 1
mpu_scale_factor = 1 / 340.0
mpu_offset = 36.53
accel_factor = 16384
gyro_factor = 131
g = 9.8
s = 1_000_000_000
prev_ax = 0
prev_ay = 0
prev_az = 0
jerk = 0
da = 0
dt = 0

i2c.writeto_mem(MPU_ADDR, REG, b'\x00') # Wake up MPU

def read_accel():
    data = i2c.readfrom_mem(MPU_ADDR, ACCEL_REG, accel_bytes * bytes_per_short)
    ax, ay, az = struct.unpack(">hhh", data)
    return ax, ay, az

def read_gyro():
    data = i2c.readfrom_mem(MPU_ADDR, GYRO_REG, gyro_bytes * bytes_per_short)
    gx, gy, gz = struct.unpack(">hhh", data)
    return gx, gy, gz

def read_temp():
    data = i2c.readfrom_mem(MPU_ADDR, TEMP_REG, temp_bytes * bytes_per_short)
    temp = struct.unpack(">h", data)
    return temp



while True:
    now = ticks_ms()
    ax, ay, az = read_accel()
    gx, gy, gz = read_gyro()
    temp = read_temp()
    celsius = (temp[0] * mpu_scale_factor) + mpu_offset


    ax /= accel_factor
    ay /= accel_factor
    az /= accel_factor
    az -= 1

    gx /= gyro_factor
    gy /= gyro_factor
    gz /= gyro_factor

    if ax != prev_ax:
        now2 = ticks_us()
        da = abs(ax - prev_ax)
        dt = ticks_diff(now2, now)
        jerk = da / dt
        print(f"Jerk: {jerk} m/s^3")
        prev_ax = ax
    elif ax != prev_ax:
        now2 = ticks_us()
        da = abs(ay - prev_ay)
        dt = ticks_diff(now2, now)
        jerk = da / dt
        print(f"Jerk: {jerk} m/s^3")
        prev_ay = ay
    elif ax != prev_ax:
        now2 = ticks_us()
        da = abs(az - prev_az)
        dt = ticks_diff(now2, now)
        jerk = da / dt
        print(f"Jerk: {jerk} m/s^3")
        prev_az = az
    sleep(0.2)