"""Check path segment headings"""
from math import atan2, pi

approach_points = [
    (-1.5, -1.0),
    (-1.262, -0.749),
    (-1.045, -0.514),
    (-0.829, -0.309),
    (-0.596, -0.146),
    (-0.326, -0.039),
    (0, 0)
]

print("Approach path segment headings:")
for i in range(len(approach_points)-1):
    p1, p2 = approach_points[i], approach_points[i+1]
    heading = atan2(p2[1]-p1[1], p2[0]-p1[0])
    print(f"Segment {i}->{i+1}: heading={heading:.3f} rad = {heading*180/pi:.1f} deg")

print("\nWork path segment headings (first few):")
work_points = [(0, 0), (10, 0), (10, 1), (0, 1)]
for i in range(len(work_points)-1):
    p1, p2 = work_points[i], work_points[i+1]
    heading = atan2(p2[1]-p1[1], p2[0]-p1[0])
    print(f"Segment {i}->{i+1}: heading={heading:.3f} rad = {heading*180/pi:.1f} deg")