"""Analyze path_heading from log"""
from math import atan2, pi

with open('controller_log.csv') as f:
    lines = [l.strip() for l in f.readlines()[1:]]

filtered = [l.split(',') for l in lines if len(l.split(',')) >= 9]

print("Analysis at key time points:")
for target_t in [10.0, 20.0, 30.0]:
    rows = [p for p in filtered if abs(float(p[0]) - target_t) < 0.05]
    if rows:
        r = rows[0]
        t = float(r[0])
        x, y = float(r[1]), float(r[2])
        heading = float(r[3])
        tx, ty = float(r[4]), float(r[5])
        xte = float(r[8])
        heading_err = float(r[9])
        
        path_heading = atan2(ty-y, tx-x)
        
        print(f"\nt={t:.1f}s:")
        print(f"  pos=({x:.3f}, {y:.3f}), heading={heading:.3f} rad = {heading*180/pi:.1f} deg")
        print(f"  target=({tx:.3f}, {ty:.3f})")
        print(f"  path_heading={path_heading:.3f} rad = {path_heading*180/pi:.1f} deg")
        print(f"  xte={xte:.3f}, heading_err={heading_err:.3f}")
        print(f"  => desired_heading = path_heading + xte_correction")
        print(f"             = {path_heading:.3f} + (-1.2 * {xte:.3f})")
        print(f"             = {path_heading - 1.2*xte:.3f} rad = {(path_heading - 1.2*xte)*180/pi:.1f} deg")