import serial
import time
import struct

def configure_module(ser):
    # Comando para configurar o módulo para enviar todos os pacotes de dados
    comando_configurar = bytes([0xFF, 0xAA, 0x02, 0x1F, 0x00])  # Habilita pacotes 0x50, 0x51, 0x52, 0x53, 0x54, 0x59
    ser.write(comando_configurar)
    time.sleep(0.1)  # Pequena pausa para garantir que o comando seja processado

def request_data(ser):
    # Comando para requisitar dados
    comando_requisitar = bytes([0xFF, 0xAA, 0x02, 0x1F, 0x00])
    ser.write(comando_requisitar)

def read_data(ser):
    while True:
        request_data(ser)
        time.sleep(1)
        if ser.in_waiting > 0:
            data = ser.read(ser.in_waiting)
            process_data(data)

def process_data(data):
    index = 0
    while index < len(data):
        header = data[index]
        if header == 0x55:  # Supondo que 0x55 é o cabeçalho
            packet_type = data[index + 1]
            if packet_type == 0x50:  # Pacote de tempo
                time_data = struct.unpack('<BBBBBBBBB', data[index + 2:index + 11])
                ms = (time_data[7] << 8) | time_data[8]
                print(f"Time: 20{time_data[0]:02}-{time_data[1]:02}-{time_data[2]:02} {time_data[3]:02}:{time_data[4]:02}:{time_data[5]:02}.{ms:03}")
            elif packet_type == 0x51:  # Pacote de aceleração
                accel = struct.unpack('<hhhH', data[index + 2:index + 10])
                ax = accel[0] / 32768.0 * 16
                ay = accel[1] / 32768.0 * 16
                az = accel[2] / 32768.0 * 16
                temp = accel[3] / 100.0
                print(f"Acceleration: X={ax:.2f}g, Y={ay:.2f}g, Z={az:.2f}g, Temp={temp:.2f}°C")
            elif packet_type == 0x52:  # Pacote de velocidade angular
                gyro = struct.unpack('<hhhH', data[index + 2:index + 10])
                wx = gyro[0] / 32768.0 * 2000
                wy = gyro[1] / 32768.0 * 2000
                wz = gyro[2] / 32768.0 * 2000
                voltage = gyro[3] / 100.0
                print(f"Gyro: X={wx:.2f}°/s, Y={wy:.2f}°/s, Z={wz:.2f}°/s, Voltage={voltage:.2f}V")
            elif packet_type == 0x53:  # Pacote de ângulo
                angle = struct.unpack('<hhhH', data[index + 2:index + 10])
                roll = angle[0] / 32768.0 * 180
                pitch = angle[1] / 32768.0 * 180
                yaw = angle[2] / 32768.0 * 180
                version = angle[3]
                print(f"Angle: Roll={roll:.2f}°, Pitch={pitch:.2f}°, Yaw={yaw:.2f}°, Version={version}")
            elif packet_type == 0x54:  # Pacote magnético
                magnetic = struct.unpack('<hhhH', data[index + 2:index + 10])
                hx = magnetic[0]
                hy = magnetic[1]
                hz = magnetic[2]
                temp = magnetic[3] / 100.0
                print(f"Magnetic: X={hx}, Y={hy}, Z={hz}, Temp={temp:.2f}°C")
            elif packet_type == 0x59:  # Pacote de quaternions
                quaternion = struct.unpack('<hhhh', data[index + 2:index + 10])
                q0 = quaternion[0] / 32768.0
                q1 = quaternion[1] / 32768.0
                q2 = quaternion[2] / 32768.0
                q3 = quaternion[3] / 32768.0
                print(f"Quaternion: Q0={q0:.2f}, Q1={q1:.2f}, Q2={q2:.2f}, Q3={q3:.2f}")
            index += 11  # Avançar para o próximo pacote
        else:
            index += 1

if __name__ == "__main__":
    ser = serial.Serial('COM3', 115200, timeout=1)
    configure_module(ser)
    read_data(ser)