import serial
import struct
from datetime import datetime, timedelta

# ================= CONFIG =================
PORT = "COM17"       # <-- cambia
BAUD = 2000000
OUT_FILE = "imu_log.txt"

FRAME_FMT = "<H I 6f 6f H"
# FRAME_FMT = "<H I 6h 6h H"
FRAME_SIZE = struct.calcsize(FRAME_FMT)
MAGIC = 0xABCD

# ================= CRC ====================
def crc16_xor(data: bytes) -> int:
    c = 0
    for b in data:
        c ^= b
    return c & 0xFFFF

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

start_wall_time = None
start_ts_us = None

print("Listening...")

with open(OUT_FILE, "w", encoding="utf-8") as f:
    while True:
        buffer += ser.read(1024)

        while len(buffer) >= FRAME_SIZE:
            # ricerca MAGIC (0xABCD little-endian)
            if buffer[0] != 0xCD or buffer[1] != 0xAB:
                buffer.pop(0)
                continue

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

            magic, ts_us, *imu, crc = struct.unpack(FRAME_FMT, frame)

            if crc16_xor(frame[:-2]) != crc:
                continue

            # tempo base
            if start_wall_time is None:
                start_wall_time = datetime.now()
                start_ts_us = ts_us

            delta_us = ts_us - start_ts_us
            ts = start_wall_time + timedelta(microseconds=delta_us)

            imu1 = imu[:6]
            imu2 = imu[6:]

            line = (
                f"{ts.strftime('%Y-%m-%d %H:%M:%S.%f')[:-3]}"
                f"|1:{','.join(map(str, imu1))}"
                f"|2:{','.join(map(str, imu2))}\n"
            )

            f.write(line)
            f.flush()   # rimuovi se vuoi più performance

            print(line.strip())