"""Debug Pure Pursuit logic"""
import json
from math import pi, hypot, atan2, cos, sin
from path_planner import plan_approach_path, compute_path_heading, combine_paths
from mower_controller import MowerController

# Load work path
work_path = json.load(open('example_path.json'))
mower_start_pos = (work_path[0][0] - 1.5, work_path[0][1] - 1.0)
mower_start_heading = pi / 4

# Generate approach path
work_path_heading = compute_path_heading(work_path, 0)
approach_path = plan_approach_path(mower_start_pos, mower_start_heading,
                                   work_path[0], work_path_heading, 
                                   method='smooth')

# Combine paths
combined_path, approach_end_idx = combine_paths(approach_path, work_path)

print(f"Combined path: {len(combined_path)} points")
print(f"First 10 points:")
for i in range(min(10, len(combined_path))):
    print(f"  {i}: {combined_path[i]}")

# Initialize controller
ctrl = MowerController(combined_path)
print(f"\nDensified path: {len(ctrl.path)} points")
print(f"First 20 densified points:")
for i in range(min(20, len(ctrl.path))):
    print(f"  {i}: {ctrl.path[i]}")

# Simulate a few steps
ctrl.update_gps(pos_xy=mower_start_pos, heading=mower_start_heading)
ctrl.stage = ctrl.STAGE_FOLLOW_PATH  # Skip goto_start

print("\n=== Testing Pure Pursuit Logic ===")
for step in range(5):
    result = ctrl.compute_control(timestamp=step * 0.014)
    x, y = ctrl.pos
    target_idx = ctrl.current_target_idx if hasattr(ctrl, 'current_target_idx') else -1
    if target_idx >= 0 and target_idx < len(ctrl.path):
        tx, ty = ctrl.path[target_idx]
        target_heading = atan2(ty - y, tx - x)
        print(f"\nStep {step}:")
        print(f"  Pos: ({x:.3f}, {y:.3f}), heading: {ctrl.heading:.3f}")
        print(f"  Target idx: {target_idx}, target: ({tx:.3f}, {ty:.3f})")
        print(f"  Target heading: {target_heading:.3f}")
        print(f"  Command: forward={result['forward']}, turn={result['turn']}")
    
    # Simulate motion (simplified kinematic model)
    dt = 0.014
    forward_mps = result['forward'] / 100.0 * ctrl.max_forward_mps
    yawrate = result['turn'] / 100.0 * ctrl.max_yawrate
    
    # Update heading
    new_heading = ctrl.heading + yawrate * dt
    
    # Update position
    vx = forward_mps * cos(new_heading)
    vy = forward_mps * sin(new_heading)
    new_x = x + vx * dt
    new_y = y + vy * dt
    
    ctrl.update_gps(pos_xy=(new_x, new_y), heading=new_heading)