#!/usr/bin/env python3
"""
调试航向控制的符号定义
根据实际IMU定义：GyroZ正值 = 顺时针(CW) = 右转 = 罗盘角增加
"""
import math

# 从log中提取的数据
pos_x, pos_y = 1.89, -2.81  # ENU坐标
tgt_x, tgt_y = 5.40, -7.17
current_hdg_compass = 308.13  # 罗盘角度
turn_output = 0.80  # log中显示的turn值（正值）

print("=" * 60)
print("实际测试数据分析")
print("=" * 60)
print(f"当前位置(ENU): ({pos_x:.2f}, {pos_y:.2f})")
print(f"目标位置(ENU): ({tgt_x:.2f}, {tgt_y:.2f})")
print(f"当前航向(罗盘): {current_hdg_compass:.1f}°")
print(f"Log显示turn: {turn_output:.2f} (正值)")
print()

# 1. 计算目标航向
dx = tgt_x - pos_x  # 东方向
dy = tgt_y - pos_y  # 北方向
dist = math.sqrt(dx**2 + dy**2)

target_math_rad = math.atan2(dy, dx)
target_math_deg = math.degrees(target_math_rad)

# 转换为罗盘角度
target_compass_deg = 90.0 - target_math_deg
if target_compass_deg < 0:
    target_compass_deg += 360
if target_compass_deg >= 360:
    target_compass_deg -= 360

print("=" * 60)
print("步骤1: 计算目标航向")
print("=" * 60)
print(f"位移向量: dx={dx:.2f}m(东), dy={dy:.2f}m(北), dist={dist:.2f}m")
print(f"目标航向(数学坐标系): {target_math_deg:.1f}° = {target_math_rad:.3f} rad")
print(f"目标航向(罗盘坐标系): {target_compass_deg:.1f}°")
print()

# 2. 模拟motion_control.c中的转换
# mc_compass_deg_to_math_rad: math_deg = 90.0f - heading_deg
current_math_deg = 90.0 - current_hdg_compass
current_math_rad = math.radians(current_math_deg)

# 归一化到-π~π
def wrap_angle(angle_rad):
    while angle_rad > math.pi:
        angle_rad -= 2 * math.pi
    while angle_rad < -math.pi:
        angle_rad += 2 * math.pi
    return angle_rad

current_math_rad = wrap_angle(current_math_rad)
target_math_rad_wrapped = wrap_angle(target_math_rad)

print("=" * 60)
print("步骤2: 罗盘角→数学坐标系（motion_control.c第114行）")
print("=" * 60)
print(f"math_deg = 90.0 - heading_deg = 90.0 - {current_hdg_compass:.1f} = {current_math_deg:.1f}°")
print(f"current_math_rad = {current_math_rad:.3f} rad = {math.degrees(current_math_rad):.1f}°")
print(f"target_math_rad  = {target_math_rad_wrapped:.3f} rad = {math.degrees(target_math_rad_wrapped):.1f}°")
print()

# 3. 计算航向误差（数学坐标系）
heading_err_rad = target_math_rad_wrapped - current_math_rad
heading_err_rad = wrap_angle(heading_err_rad)
heading_err_deg = math.degrees(heading_err_rad)

print("=" * 60)
print("步骤3: 计算航向误差（motion_control.c第43行）")
print("=" * 60)
print(f"heading_err = target - current")
print(f"            = {target_math_rad_wrapped:.3f} - {current_math_rad:.3f}")
print(f"            = {heading_err_rad:.3f} rad")
print(f"            = {heading_err_deg:.1f}°")
if heading_err_rad > 0:
    print("误差为正 → 数学坐标系中需要逆时针转(CCW)")
    print("           → 罗盘坐标系中需要顺时针转(从西北转向东南)")
else:
    print("误差为负 → 数学坐标系中需要顺时针转(CW)")
    print("           → 罗盘坐标系中需要逆时针转")
print()

# 4. 计算yaw_rate_cmd（假设Kp=1，简化）
Kp = 1.0
max_turn_rate = 0.8
yaw_rate_cmd = Kp * heading_err_rad
yaw_rate_cmd = max(-max_turn_rate, min(max_turn_rate, yaw_rate_cmd))

print("=" * 60)
print("步骤4: 计算yaw_rate_cmd（motion_control.c第247行）")
print("=" * 60)
print(f"yaw_rate_cmd = Kp × heading_err")
print(f"             = {Kp:.1f} × {heading_err_rad:.3f}")
print(f"             = {yaw_rate_cmd:.3f} rad/s")
print(f"限制到±{max_turn_rate}: yaw_rate_cmd = {yaw_rate_cmd:.3f} rad/s")
print()

# 5. motion_control.c第548行：取反
turn_rate_before_invert = yaw_rate_cmd
turn_rate_after_invert = -yaw_rate_cmd

print("=" * 60)
print("步骤5: 第548行取反（motion_control.c）")
print("=" * 60)
print(f"_out->turn_rate = -yaw_rate_cmd")
print(f"                = -{yaw_rate_cmd:.3f}")
print(f"                = {turn_rate_after_invert:.3f} rad/s")
print()

# 6. 对比log中的值
print("=" * 60)
print("步骤6: 对比实际log输出")
print("=" * 60)
print(f"计算的turn_rate: {turn_rate_after_invert:.3f} rad/s")
print(f"Log显示turn:     {turn_output:.2f} rad/s")
print()

if (turn_rate_after_invert > 0 and turn_output > 0) or (turn_rate_after_invert < 0 and turn_output < 0):
    print("✓ 符号一致")
else:
    print("✗ 符号不一致！需要检查代码")
print()

# 7. PWM映射分析
print("=" * 60)
print("步骤7: PWM映射（motion_control_task.c第469-487行）")
print("=" * 60)
print("注释说：正值=左转，负值=右转")
print(f"turn_rate = {turn_rate_after_invert:.3f} rad/s")

if turn_rate_after_invert > 0:
    print("→ 正值 → 应该左转（逆时针，罗盘角减少）")
    print("→ PWM < 1500")
elif turn_rate_after_invert < 0:
    print("→ 负值 → 应该右转（顺时针，罗盘角增加）")
    print("→ PWM > 1500")
print()

# 8. 实际车辆行为
print("=" * 60)
print("步骤8: 实际车辆行为")
print("=" * 60)
print(f"Log显示航向从308°增加到344° → 顺时针转（右转）")
print(f"目标航向141°")
print(f"正确的转向：308° → 逆时针 → 141°（转167°）")
print(f"实际转向：顺时针转")
print()
print("结论：转向方向反了！")
print()

# 9. 问题诊断
print("=" * 60)
print("问题诊断")
print("=" * 60)
print("可能的原因：")
print("1. 第548行的取反导致符号错误")
print("2. PWM映射的符号定义与注释不符")
print("3. 航向误差计算有问题")
print()
print("建议：")
print("1. 去掉motion_control.c第548行的取反")
print("2. 或者修改motion_control_task.c的PWM映射符号")
print("3. 或者检查mc_compass_deg_to_math_rad转换是否正确")