import serial
import matplotlib.pyplot as plt
import numpy as np

ser = serial.Serial(port='/dev/ttyACM1', timeout = 1)
ser.flush()
ser.reset_input_buffer()

gyros = []
accs = []

fig, axs = plt.subplots(2)
i = 0

while(1):
    x = [int(e, 16) for e in ser.readline()[1:-1].split(b" ")]
    imu_reading_type = x[0] >> 3
    imu_reading_tag_cnt = (x[0] >> 1) & 3
    #print(imu_reading_type, imu_reading_tag_cnt)
    reading_xyz = [int.from_bytes(x[1:3], byteorder = 'little', signed = True),
                   int.from_bytes(x[3:5], byteorder = 'little', signed = True),
                   int.from_bytes(x[5:7], byteorder = 'little', signed = True)]
    i += 1
    if imu_reading_type == 1:
        gyros.append([250 * e / (1 << 16) for e in reading_xyz])
    elif imu_reading_type == 2:
        accs.append([4 * e / (1<<16) for e in reading_xyz])

        if len(gyros) > 50 and (i % 10) == 0:
            gyros = gyros[-100:]
            accs = accs[-100:]
            a = np.array(accs)
            g = np.array(gyros)
            a -= np.mean(a, axis = 0).reshape(1, 3)
            g -= np.mean(g, axis = 0).reshape(1, 3)
            axs[0].cla()
            axs[0].plot(g[:,0])
            axs[0].plot(g[:,1])
            axs[0].plot(g[:,2])
            axs[1].cla()
            axs[1].plot(a[:,0])
            axs[1].plot(a[:,1])
            axs[1].plot(a[:,2])
            plt.pause(0.0001)