"""Debug desired_heading calculation"""
from math import atan2, pi

def wrap_angle(a):
    while a > pi:
        a -= 2 * pi
    while a < -pi:
        a += 2 * pi
    return a

# From log at t=10s
x, y = -0.282, -0.638
heading = -0.470
tx, ty = 0.000, 0.000
xte = 0.573
heading_err_log = -0.095

# Calculate path_heading
path_heading = atan2(ty - y, tx - x)
print(f"path_heading = atan2({ty} - {y}, {tx} - {x}) = atan2({ty-y:.3f}, {tx-x:.3f}) = {path_heading:.3f}")

# Calculate xte_correction
k_xte = 1.2
max_xte = 1.0
xte_gain_scale = 1.0
xte_normalized = max(-1.0, min(1.0, xte / max_xte))
xte_correction = -k_xte * xte_normalized * xte_gain_scale
print(f"xte_correction = -k_xte * xte_normalized * xte_gain_scale")
print(f"               = -{k_xte} * {xte_normalized:.3f} * {xte_gain_scale}")
print(f"               = {xte_correction:.3f}")

# Calculate desired_heading
desired_heading_raw = path_heading + xte_correction
desired_heading = wrap_angle(desired_heading_raw)
print(f"desired_heading = wrap_angle({path_heading:.3f} + {xte_correction:.3f})")
print(f"                = wrap_angle({desired_heading_raw:.3f})")
print(f"                = {desired_heading:.3f}")

# Calculate heading_err
heading_err_calc = wrap_angle(desired_heading - heading)
print(f"\nheading_err = wrap_angle({desired_heading:.3f} - {heading:.3f})")
print(f"            = wrap_angle({desired_heading - heading:.3f})")
print(f"            = {heading_err_calc:.3f}")

print(f"\n heading_err from log: {heading_err_log:.3f}")
print(f"heading_err calculated: {heading_err_calc:.3f}")
print(f"           Difference: {abs(heading_err_log - heading_err_calc):.3f}")

if abs(heading_err_log - heading_err_calc) > 0.1:
    print("\n⚠️ WARNING: Significant difference detected!")
    print("Either:")
    print("1. xte_correction is not being applied")
    print("2. Different target point is being used")
    print("3. Log data is from a different code path")