import csv, json, math

with open('example_path.json') as f:
    path = json.load(f)
start = tuple(path[0])

rows = []
with open('controller_log.csv') as f:
    reader = csv.DictReader(f)
    for r in reader:
        rows.append(r)

first_follow_idx = None
for i, r in enumerate(rows):
    if r.get('stage','') == 'follow_path':
        first_follow_idx = i
        break

if first_follow_idx is not None:
    print('Found follow_path at row', first_follow_idx, 't=', rows[first_follow_idx]['t'])
    for j in range(max(0, first_follow_idx-5), first_follow_idx+5):
        r = rows[j]
        x = float(r['x']); y = float(r['y']); heading = float(r['heading'])
        dx = start[0]-x; dy = start[1]-y
        desired = math.atan2(dy, dx)
        h_err = (desired - heading + math.pi) % (2*math.pi) - math.pi
        print(j, r['t'], r['stage'], r['status'], f"pos=({x:.3f},{y:.3f})", f"dist={(math.hypot(dx,dy)):.3f}", f"h_err={h_err:.3f}")
else:
    print('No follow_path in log. Scanning for closest approach to start...')
    min_dist = float('inf')
    min_idx = None
    for i, r in enumerate(rows):
        x = float(r['x']); y = float(r['y']);
        d = math.hypot(start[0]-x, start[1]-y)
        if d < min_dist:
            min_dist = d; min_idx = i
    print('min dist to start =', min_dist, 'at row', min_idx, 't=', rows[min_idx]['t'])
    print('Nearby log lines:')
    for j in range(max(0, min_idx-10), min_idx+11):
        r = rows[j]
        x = float(r['x']); y = float(r['y']); heading = float(r['heading'])
        dx = start[0]-x; dy = start[1]-y
        desired = math.atan2(dy, dx)
        h_err = (desired - heading + math.pi) % (2*math.pi) - math.pi
        print(j, r['t'], r['stage'], r['status'], f"pos=({x:.3f},{y:.3f})", f"dist={(math.hypot(dx,dy)):.3f}", f"h_err={h_err:.3f}")

# also print counts of key statuses
from collections import Counter
cnt = Counter((r['stage'], r['status']) for r in rows)
print('\nStage/Status counts (top 20):')
for k,v in cnt.most_common(20):
    print(k, v)