import serial
import struct
from datetime import datetime, timedelta

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

# Formato frame ESP32 (raw)
FRAME_FMT = "<H I 6h 6h H"   # magic, ts_us, imu1 6x int16, imu2 6x int16, crc
FRAME_SIZE = struct.calcsize(FRAME_FMT)
MAGIC_BYTES = b'\xCD\xAB'

# Conversione accelerometro e giroscopio
ACC_LSB_PER_G = 2048.0
G_CONST = 9.80665
GYRO_LSB_PER_DPS = 16.4
DEG2RAD = 0.017453292519943295

# ================= 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...")

def convert_acc(raw):
    return round(((raw / ACC_LSB_PER_G) * G_CONST)/10, 2)

def convert_gyro(raw):
    return round((raw / GYRO_LSB_PER_DPS) * DEG2RAD, 2)

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

        while True:
            idx = buffer.find(MAGIC_BYTES)
            if idx == -1:
                buffer.clear()
                break
            if idx > 0:
                buffer = buffer[idx:]
            if len(buffer) < FRAME_SIZE:
                break

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

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

            if crc16_xor(frame[:-2]) != crc:
                print("CRC error (resync)")
                continue

            # Base timestamp
            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)

            # Split raw dati
            imu1_raw = imu_raw[:6]
            imu2_raw = imu_raw[6:]

            # Conversione in m/s² e rad/s
            imu1 = [convert_acc(imu1_raw[0]), convert_acc(imu1_raw[1]), convert_acc(imu1_raw[2]),
                    convert_gyro(imu1_raw[3]), convert_gyro(imu1_raw[4]), convert_gyro(imu1_raw[5])]
            imu2 = [convert_acc(imu2_raw[0]), convert_acc(imu2_raw[1]), convert_acc(imu2_raw[2]),
                    convert_gyro(imu2_raw[3]), convert_gyro(imu2_raw[4]), convert_gyro(imu2_raw[5])]

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

            f.write(line)
            f.flush()
            print(line.strip())