import serial
import struct
import datetime
import csv

PORT = 'COM3'
BAUD = 921600

MAGIC = 0xABCD
FRAME_FMT = '<HI6h6hH'
FRAME_SIZE = struct.calcsize(FRAME_FMT)

def crc16(data: bytes) -> int:
    c = 0
    for b in data:
        c ^= b
    return c

ser = serial.Serial(PORT, BAUD, timeout=1)
buffer = bytearray()

start_ts = None
start_time = None

csv_file = open('imu_log.csv', 'w', newline='')
writer = csv.writer(csv_file)
writer.writerow([
    'timestamp',
    'imu1_ax','imu1_ay','imu1_az','imu1_gx','imu1_gy','imu1_gz',
    'imu2_ax','imu2_ay','imu2_az','imu2_gx','imu2_gy','imu2_gz'
])

print("Listening...")

try:
    while True:
        buffer += ser.read(512)

        while len(buffer) >= FRAME_SIZE:
            if buffer[0:2] != MAGIC.to_bytes(2, 'little'):
                buffer.pop(0)
                continue

            frame = buffer[:FRAME_SIZE]
            buffer = buffer[FRAME_SIZE:]

            unpacked = struct.unpack(FRAME_FMT, frame)
            crc_rx = unpacked[-1]
            crc_calc = crc16(frame[:-2])

            if crc_rx != crc_calc:
                continue  # frame corrotto

            ts_us = unpacked[1]

            if start_ts is None:
                start_ts = ts_us
                start_time = datetime.datetime.now()

            abs_time = start_time + datetime.timedelta(
                microseconds=(ts_us - start_ts)
            )

            writer.writerow([
                abs_time.strftime('%Y-%m-%d %H:%M:%S.%f')[:-3],
                *unpacked[2:14]
            ])

except KeyboardInterrupt:
    csv_file.close()
    ser.close()
    print("Stopped")