#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
测试GPS/IMU数据解析 - 使用实际数据验证
"""

import struct

# 从你提供的数据中提取的完整GPS帧
gps_frame_hex = """
AA 55 01 2C 00 68 70 21 B0 6E EA 43 40 E9 11 40
BD DB 11 5D 40 7F C3 65 43 F4 FD 54 BC 58 39 34
BC A6 9B 44 BB 77 3E A5 42 EC 4C 9E 00 01 1B 00
00 6A 14 0D 0A
"""

# 完整IMU帧
imu_frame_hex = """
AA 55 02 20 00 58 39 34 BC BA 49 8C BE 4E 62 80
BF 7F 6A BC 3D D1 22 5B BE 00 00 00 80 00 00 F0
41 F1 00 2F 06 A3 0D 0D 0A
"""

def test_gps_parsing():
    """测试GPS数据解析"""
    print("="*60)
    print("测试GPS数据包解析")
    print("="*60)
    
    # 转换为字节
    hex_clean = gps_frame_hex.replace(' ', '').replace('\n', '')
    frame = bytes.fromhex(hex_clean)
    
    print(f"完整帧长度: {len(frame)} 字节")
    print(f"帧头: {frame[0]:02X} {frame[1]:02X}")
    print(f"类型: {frame[2]:02X}")
    
    data_len = struct.unpack('<H', frame[3:5])[0]
    print(f"数据长度: {data_len} 字节")
    
    payload = frame[5:5+data_len]
    print(f"负载长度: {len(payload)} 字节")
    
    checksum = struct.unpack('<H', frame[5+data_len:5+data_len+2])[0]
    calc_checksum = sum(frame[0:5+data_len]) & 0xFFFF
    print(f"校验和: 0x{checksum:04X} (计算: 0x{calc_checksum:04X}) {'✓' if checksum==calc_checksum else '✗'}")
    
    # 尝试不同的解析格式
    print("\n尝试解析 (格式: <dd5fIBB2x):")
    try:
        gps = struct.unpack('<dd5fIBB2x', payload)
        print(f"  [0] 纬度:     {gps[0]:.8f}°")
        print(f"  [1] 经度:     {gps[1]:.8f}°")
        print(f"  [2] 航向角:   {gps[2]:.2f}°")
        print(f"  [3] 东速度:   {gps[3]:.3f} m/s")
        print(f"  [4] 北速度:   {gps[4]:.3f} m/s")
        print(f"  [5] 天速度:   {gps[5]:.3f} m/s")
        print(f"  [6] 高程:     {gps[6]:.2f} m")
        print(f"  [7] UTC时间:  {gps[7]}")
        print(f"  [8] 定位质量: {gps[8]}")
        print(f"  [9] 卫星数:   {gps[9]}")
        print("✓ 解析成功!")
    except Exception as e:
        print(f"✗ 解析失败: {e}")
    
    # 十六进制显示负载
    print(f"\n负载十六进制 ({len(payload)}字节):")
    hex_str = payload.hex(' ').upper()
    for i in range(0, len(hex_str), 48):
        print(f"  {hex_str[i:i+48]}")


def test_imu_parsing():
    """测试IMU数据解析"""
    print("\n" + "="*60)
    print("测试IMU数据包解析")
    print("="*60)
    
    # 转换为字节
    hex_clean = imu_frame_hex.replace(' ', '').replace('\n', '')
    frame = bytes.fromhex(hex_clean)
    
    print(f"完整帧长度: {len(frame)} 字节")
    print(f"帧头: {frame[0]:02X} {frame[1]:02X}")
    print(f"类型: {frame[2]:02X}")
    
    data_len = struct.unpack('<H', frame[3:5])[0]
    print(f"数据长度: {data_len} 字节")
    
    payload = frame[5:5+data_len]
    print(f"负载长度: {len(payload)} 字节")
    
    checksum = struct.unpack('<H', frame[5+data_len:5+data_len+2])[0]
    calc_checksum = sum(frame[0:5+data_len]) & 0xFFFF
    print(f"校验和: 0x{checksum:04X} (计算: 0x{calc_checksum:04X}) {'✓' if checksum==calc_checksum else '✗'}")
    
    # 解析IMU数据
    print("\n尝试解析 (格式: <7fI):")
    try:
        imu = struct.unpack('<7fI', payload)
        print(f"  [0] 加速度X:  {imu[0]:.3f} g")
        print(f"  [1] 加速度Y:  {imu[1]:.3f} g")
        print(f"  [2] 加速度Z:  {imu[2]:.3f} g")
        print(f"  [3] 角速度X:  {imu[3]:.2f} °/s")
        print(f"  [4] 角速度Y:  {imu[4]:.2f} °/s")
        print(f"  [5] 角速度Z:  {imu[5]:.2f} °/s")
        print(f"  [6] 温度:     {imu[6]:.1f} ℃")
        print(f"  [7] UTC时间:  {imu[7]} ({imu[7]/1000:.3f}s)")
        print("✓ 解析成功!")
    except Exception as e:
        print(f"✗ 解析失败: {e}")
    
    # 十六进制显示负载
    print(f"\n负载十六进制 ({len(payload)}字节):")
    hex_str = payload.hex(' ').upper()
    for i in range(0, len(hex_str), 48):
        print(f"  {hex_str[i:i+48]}")


if __name__ == "__main__":
    test_gps_parsing()
    test_imu_parsing()
    
    print("\n" + "="*60)
    print("结论:")
    print("="*60)
    print("✓ GPS数据包: 44字节，格式 <dd5fIBB2x")
    print("✓ IMU数据包: 32字节，格式 <7fI")
    print("✓ 校验和: 16位累加和")
    print("✓ 帧格式: AA 55 TYPE LEN(2) PAYLOAD CHECKSUM(2) 0D 0A")