#!/usr/bin/env python3
"""
检查IM23A航向角的定义
通过分析位置变化方向和航向角的关系，推断正确的坐标系
"""
import sys
import math

def parse_cal_line(line):
    """解析$CAL数据行"""
    if not line.startswith('$CAL'):
        return None
    parts = line.strip().split(',')
    if len(parts) < 10:
        return None
    try:
        return {
            'seq': int(parts[1]),
            'time': int(parts[2]),
            'state': parts[3],
            'x': float(parts[6]),      # ENU X (东)
            'y': float(parts[7]),      # ENU Y (北)
            'z': float(parts[8]),      # ENU Z (天)
            'heading': float(parts[9]),  # 航向角(弧度)
        }
    except (ValueError, IndexError):
        return None

def main():
    if len(sys.argv) < 2:
        print("用法: python check_heading_def.py <log_file>")
        sys.exit(1)
    
    logfile = sys.argv[1]
    
    data_points = []
    with open(logfile, 'r', encoding='utf-8', errors='ignore') as f:
        for line in f:
            point = parse_cal_line(line)
            if point:
                data_points.append(point)
    
    print(f"共读取 {len(data_points)} 条数据")
    
    # 找几个有明显位置变化的片段
    print("\n=== 分析位置变化和航向角的关系 ===\n")
    
    for i in range(50, min(len(data_points)-20, 200), 20):
        p1 = data_points[i]
        p2 = data_points[i+10]
        
        dx = p2['x'] - p1['x']
        dy = p2['y'] - p1['y']
        dist = math.sqrt(dx**2 + dy**2)
        
        if dist > 0.5:  # 只看位移大于0.5m的
            # 计算位置变化的方向（数学坐标系：东=0°，北=90°）
            move_angle_math = math.atan2(dy, dx)  # 弧度
            move_angle_deg = math.degrees(move_angle_math)
            
            # 转换为罗盘角度（北=0°，东=90°）
            move_angle_compass_deg = 90.0 - move_angle_deg
            if move_angle_compass_deg < 0:
                move_angle_compass_deg += 360
            if move_angle_compass_deg >= 360:
                move_angle_compass_deg -= 360
            
            # IM23A的航向角
            heading_rad = p2['heading']
            heading_deg = math.degrees(heading_rad)
            
            # 转换为0-360范围
            heading_compass = heading_deg
            if heading_compass < 0:
                heading_compass += 360
            
            print(f"序列 {p1['seq']}-{p2['seq']} ({p1['state']})")
            print(f"  位移: dx={dx:6.2f}m, dy={dy:6.2f}m, dist={dist:5.2f}m")
            print(f"  移动方向(数学): {move_angle_deg:6.1f}°")
            print(f"  移动方向(罗盘): {move_angle_compass_deg:6.1f}°")
            print(f"  IM23A航向(原始): {heading_rad:6.2f} rad = {heading_deg:6.1f}°")
            print(f"  IM23A航向(0-360): {heading_compass:6.1f}°")
            print(f"  差值(罗盘-IM23A): {move_angle_compass_deg - heading_compass:6.1f}°")
            print()

if __name__ == '__main__':
    main()