import serial
import struct
import csv
from datetime import datetime, timedelta

# ================= CONFIG =================
PORT = "COM3"          # <-- cambia
BAUD = 921600
OUT_CSV = "imu_log.csv"

FRAME_FMT = "<H I 6h 6h H"   # little-endian
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...")

try:
    with open(OUT_CSV, "w", newline="") as f:
        writer = csv.writer(f)
        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"
        ])

        while True:
            buffer += ser.read(1024)

            while len(buffer) >= FRAME_SIZE:
                # cerca MAGIC
                if buffer[0] != 0xCD or buffer[1] != 0xAB:
                    buffer.pop(0)
                    continue

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

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

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

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

                # timestamp assoluto coerente con ESP
                delta_us = ts_us - start_ts_us
                ts = start_wall_time + timedelta(microseconds=delta_us)

                writer.writerow([
                    ts.strftime("%Y-%m-%d %H:%M:%S.%f")[:-3],
                    *imu
                ])

                # logging si può escludere
                print(
                    ts.strftime("%H:%M:%S.%f")[:-3],
                    "| 1:", tuple(imu[:6]),
                    "| 2:", tuple(imu[6:])
                )
except KeyboardInterrupt:
    ser.close()
    print("Stopped")