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

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


D = np.zeros((0,3))

fig, axs = plt.subplots(3)

counter = 0
start = time.time()
while(1):
    ser.write(b"R")
    read_data = ser.read(180)
    counter += 1
    p = np.array([int.from_bytes(read_data[3 * i : 3 * i + 3], byteorder = 'big') for i in range(60)]).reshape(20,3)
    D = np.concatenate((D, p))

    if counter == 30:
        print(15 * 30 / (time.time() - start))
        start = time.time()
        counter = 0

    if D.shape[0] > 400:
        D = D[-400:,:]
    
    axs[0].cla()
    axs[0].plot(D[:,0])
    axs[1].cla()
    axs[1].plot(D[:,1])
    axs[2].cla()
    axs[2].plot(D[:,2])
    plt.pause(0.00001)