#!/usr/bin/env python
# 检查轨迹详细数据

with open('controller_log.csv', 'r') as f:
    lines = f.readlines()

# 找到 reached_start
reached_idx = None
for i, line in enumerate(lines):
    if 'reached_start' in line:
        reached_idx = i
        break

if reached_idx:
    print(f"Found reached_start at line {reached_idx}")
    
    # 前20帧的详细数据
    print("\nFirst 20 frames after reached_start:")
    count = 0
    for i in range(reached_idx + 1, min(reached_idx + 100, len(lines))):
        if 'running' in lines[i]:
            parts = lines[i].strip().split(',')
            if len(parts) >= 11:
                t, x, y, heading, tx, ty, tdist, tidx, xte, herr = parts[:10]
                print(f"t={t:>6s} pos=({x:>6s},{y:>6s}) h={heading:>5s} target=({tx:>6s},{ty:>6s}) idx={tidx:>3s} xte={xte:>6s} h_err={herr:>6s}")
                count += 1
                if count >= 20:
                    break
    
    # 多个时间点的位置
    print("\n\nPosition every 10 seconds:")
    target_times = [10, 20, 30, 40, 50, 60, 70, 80, 90, 100, 110, 120]
    for target_t in target_times:
        for i in range(reached_idx + 1, len(lines)):
            if 'running' in lines[i]:
                parts = lines[i].strip().split(',')
                if len(parts) >= 3:
                    t = float(parts[0])
                    if t >= target_t:
                        x, y, heading = parts[1], parts[2], parts[3]
                        print(f"t={t:>6.1f}s: pos=({x:>7s},{y:>7s}) heading={heading:>6s}")
                        break