import serial
import struct
import datetime
import time

# ---------------- CONFIG ----------------
PORT = 'COM3'       # aggiorna con la tua COM
BAUD = 921600

fmt = '<I6h6h'      # ts, imu1(6h), imu2(6h)
sample_size = struct.calcsize(fmt)

# ---------------- INIT ------------------
ser = serial.Serial(PORT, BAUD, timeout=1)
print(f"Listening on {PORT}...")

start_ts = None
start_time = None  # tempo assoluto all’inizio

try:
    while True:
        data = ser.read(sample_size)
        if len(data) != sample_size:
            continue  # pacchetto incompleto

        # ---- unpack del pacchetto ----
        s = struct.unpack(fmt, data)
        ts = s[0]          # ts dell'ESP32 in microsecondi
        imu1 = s[1:7]
        imu2 = s[7:13]

        # ---- inizializza riferimento tempo assoluto ----
        if start_ts is None:
            start_ts = ts
            start_time = datetime.datetime.now()

        # ---- calcola tempo assoluto basato sul ts progressivo ----
        delta_us = ts - start_ts
        abs_time = start_time + datetime.timedelta(microseconds=delta_us)

        # ---- stampa dati con timestamp leggibile ----
        print(f"{abs_time.strftime('%Y-%m-%d %H:%M:%S.%f')[:-3]} | 1:{imu1} | 2:{imu2}")

except KeyboardInterrupt:
    ser.close()
    print("Closed")