"""
Check trajectory with approach path visualization
"""
import json

# Load controller log
lines = []
with open('controller_log.csv', 'r') as f:
    for line in f:
        lines.append(line.strip())

# Find reached_start marker
reached_start_idx = None
for i, line in enumerate(lines):
    if 'reached_start' in line:
        reached_start_idx = i
        print(f"Found reached_start at line {i+1}")
        break

if reached_start_idx is None:
    print("No reached_start found")
    exit(1)

# Parse and display trajectory
print("\n=== TRAJECTORY ANALYSIS WITH APPROACH PATH ===\n")

# Show first 20 frames after reached_start
print("First 20 frames after reached_start (now following combined path):")
for i in range(reached_start_idx + 1, min(reached_start_idx + 21, len(lines))):
    parts = lines[i].split(',')
    if len(parts) >= 8:
        t = float(parts[0])
        x, y = float(parts[1]), float(parts[2])
        heading = float(parts[3])
        target_x, target_y = float(parts[4]), float(parts[5])
        target_idx = int(parts[7])
        xte = float(parts[8]) if len(parts) > 8 else 0.0
        h_err = float(parts[9]) if len(parts) > 9 else 0.0
        print(f"t={t:6.3f} pos=({x:6.3f},{y:6.3f}) h={heading:5.3f} target=({target_x:6.3f},{target_y:6.3f}) idx={target_idx:3d} xte={xte:6.3f} h_err={h_err:6.3f}")

# Sample every 10 seconds
print("\n\nPosition every 10 seconds:")
for target_t in [10, 20, 30, 40, 50, 60, 70, 80, 90, 100, 110]:
    for line in lines[reached_start_idx+1:]:
        parts = line.split(',')
        if len(parts) >= 8:
            t = float(parts[0])
            if abs(t - target_t) < 0.1:  # within 0.1s
                x, y = float(parts[1]), float(parts[2])
                heading = float(parts[3])
                print(f"t={t:6.1f}s: pos=({x:7.3f},{y:7.3f}) heading={heading:6.3f}")
                break

# Check if completed
print("\n\nCompletion status:")
last_line = lines[-1]
parts = last_line.split(',')
if len(parts) >= 14:
    t = float(parts[0])
    x, y = float(parts[1]), float(parts[2])
    stage = parts[13]
    print(f"Last frame: t={t:.1f}s, pos=({x:.3f},{y:.3f}), stage={stage}")
    if t >= 119.0:
        print("⚠️ TIMEOUT - did not complete in 120s")
    else:
        print("✓ Completed successfully")

# Load paths and check indices
print("\n\n=== PATH INFORMATION ===")
with open('example_path.json', 'r') as f:
    work_path = json.load(f)

print(f"Work path: {len(work_path)} points ({len(work_path)-1} segments)")
print(f"Work path range: x=[{min(p[0] for p in work_path):.1f}, {max(p[0] for p in work_path):.1f}], "
      f"y=[{min(p[1] for p in work_path):.1f}, {max(p[1] for p in work_path):.1f}]")

# Estimate approach path length
print(f"\nNote: Combined path includes approach path + work path")
print(f"Check simulation_results.png to see both paths visualized")