#!/usr/bin/env python3
"""分析航向角误差"""
import math

# 从log中提取的数据
pos_x, pos_y = 1.89, -2.81  # 当前位置
tgt_x, tgt_y = 5.40, -7.17   # 目标位置
current_hdg = 308.13         # 当前航向角(度)

# 计算期望航向（数学坐标系：东=0°，逆时针为正）
dx = tgt_x - pos_x
dy = tgt_y - pos_y
dist = math.sqrt(dx**2 + dy**2)

# 数学坐标系的角度
math_angle_rad = math.atan2(dy, dx)
math_angle_deg = math.degrees(math_angle_rad)

# 转换为罗盘角度（北=0°，顺时针为正）
compass_angle_deg = 90.0 - math_angle_deg
if compass_angle_deg < 0:
    compass_angle_deg += 360
if compass_angle_deg >= 360:
    compass_angle_deg -= 360

print(f"当前位置: ({pos_x:.2f}, {pos_y:.2f})")
print(f"目标位置: ({tgt_x:.2f}, {tgt_y:.2f})")
print(f"位移向量: dx={dx:.2f}m, dy={dy:.2f}m, dist={dist:.2f}m")
print(f"\n期望航向角(数学坐标系，东=0°): {math_angle_deg:.1f}°")
print(f"期望航向角(罗盘坐标系，北=0°): {compass_angle_deg:.1f}°")
print(f"当前航向角(IM23A报告): {current_hdg:.1f}°")
print(f"\n航向误差: {current_hdg - compass_angle_deg:.1f}°")
print(f"如果航向反了180°，实际车头方向: {(current_hdg + 180) % 360:.1f}°")
print(f"那么航向误差应该是: {((current_hdg + 180) % 360) - compass_angle_deg:.1f}°")