import serial
import struct
import json
import matplotlib.pyplot as plt

# Open the serial port
ser = serial.Serial('/dev/ttyACM0', 48000)

if ser.is_open:
    print("Serial port is open")

plt.ion();
fig, ax = plt.subplots(2);
x_data_a, y_data_a, z_data_a = [], [], []
x_data_b, y_data_b, z_data_b = [], [], []

ax[0].set_xlabel('Time');
ax[0].set_ylabel('X,Y and Z values in ASCII');

ax[1].set_xlabel('Time');
ax[1].set_ylabel('X,Y and Z values in raw binary data');
running = True

try:
    while running:
        first_byte = ser.read(1)

        if first_byte == b'{':
            remaining_data = ser.read_until(b'\n\r')
            full_data = first_byte + remaining_data

            ascii_data = json.loads(full_data.decode('utf-8'))
            print(f"ASCII Data: {ascii_data}")

            x_data_a.append(ascii_data['X']);
            y_data_a.append(ascii_data['Y']);
            z_data_a.append(ascii_data['Z']);
        
            ax[0].clear();
            ax[0].plot(x_data_a, label='X')
            ax[0].plot(y_data_a, label='Y')
            ax[0].plot(z_data_a, label='Z')
            ax[0].legend()
            plt.draw()
            plt.pause(0.01) 
        else:

            # Read a packet of size equal to GyroPacket
            remaining_data = ser.read(11)  # Assuming GyroPacket is 12 bytes long

            full_data = first_byte + remaining_data
        
            # Unpack the packet using struct
            packet = struct.unpack('<hhhhhh', full_data)

            x_data_b.append(packet[2])
            y_data_b.append(packet[3])
            z_data_b.append(packet[4])

            ax[1].clear()
            ax[1].plot(x_data_b, label='X')
            ax[1].plot(y_data_b, label='Y')
            ax[1].plot(z_data_b, label='Z')
            ax[1].legend()
            plt.draw()
            plt.pause(0.01)
        
            # Print the unpacked data
            print(f"Header: {packet[0]:04X}, Packet Number: {packet[1]}, "
                f"X: {packet[2]}, Y: {packet[3]}, Z: {packet[4]}, "
                f"Temperature: {packet[5]}")

except KeyboardInterrupt:
    # Close the serial port when interrupted
    running = False
    ser.close()
    print("Serial port closed.")