"""
Simple path planner to generate approach path from mower start position to planned path start.
"""
import numpy as np
from math import hypot, atan2, cos, sin, pi


def plan_approach_path(start_pos, start_heading, target_pos, target_heading, method='line'):
    """
    Generate approach path from mower start to planned path start.
    
    Args:
        start_pos: (x, y) mower starting position
        start_heading: mower starting heading in radians
        target_pos: (x, y) planned path starting position
        target_heading: planned path starting heading in radians
        method: 'line' | 'dubins' | 'smooth'
    
    Returns:
        List of (x, y) waypoints forming the approach path
    """
    sx, sy = start_pos
    tx, ty = target_pos
    dist = hypot(tx - sx, ty - sy)
    
    if dist < 0.05:  # Already at start, no approach needed
        return [start_pos, target_pos]
    
    if method == 'line':
        # Simple straight line with waypoints every 0.5m
        num_points = max(3, int(dist / 0.5) + 1)
        path = []
        for i in range(num_points):
            t = i / (num_points - 1)
            x = sx + t * (tx - sx)
            y = sy + t * (ty - sy)
            path.append((x, y))
        return path
    
    elif method == 'smooth':
        # Smooth curve considering start and target headings
        # Use cubic bezier curve
        # Control points based on headings
        ctrl_dist = min(dist * 0.4, 2.0)  # Control point distance
        
        # Start control point along start heading
        c1x = sx + ctrl_dist * cos(start_heading)
        c1y = sy + ctrl_dist * sin(start_heading)
        
        # End control point along target heading (backwards)
        c2x = tx - ctrl_dist * cos(target_heading)
        c2y = ty - ctrl_dist * sin(target_heading)
        
        # Generate bezier curve points
        num_points = max(5, int(dist / 0.3) + 1)
        path = []
        for i in range(num_points):
            t = i / (num_points - 1)
            # Cubic bezier formula
            x = (1-t)**3 * sx + 3*(1-t)**2*t * c1x + 3*(1-t)*t**2 * c2x + t**3 * tx
            y = (1-t)**3 * sy + 3*(1-t)**2*t * c1y + 3*(1-t)*t**2 * c2y + t**3 * ty
            path.append((x, y))
        return path
    
    else:
        raise ValueError(f"Unknown method: {method}")


def compute_path_heading(path, idx=0):
    """Compute heading of path at given index."""
    if idx >= len(path) - 1:
        idx = len(path) - 2
    if idx < 0:
        idx = 0
    
    dx = path[idx + 1][0] - path[idx][0]
    dy = path[idx + 1][1] - path[idx][1]
    return atan2(dy, dx)


def combine_paths(approach_path, work_path):
    """
    Combine approach path and work path into single path with metadata.
    
    Returns:
        combined_path: List of (x, y) waypoints
        approach_end_idx: Index where approach path ends (work path starts)
    """
    # Remove duplicate point at junction if exists
    if len(approach_path) > 0 and len(work_path) > 0:
        if hypot(approach_path[-1][0] - work_path[0][0], 
                 approach_path[-1][1] - work_path[0][1]) < 0.01:
            combined = approach_path[:-1] + work_path
            approach_end_idx = len(approach_path) - 1
        else:
            combined = approach_path + work_path
            approach_end_idx = len(approach_path)
    else:
        combined = approach_path + work_path
        approach_end_idx = len(approach_path)
    
    return combined, approach_end_idx


if __name__ == '__main__':
    # Test path planning
    import json
    import matplotlib.pyplot as plt
    
    # Load planned path
    with open('example_path.json', 'r') as f:
        work_path = json.load(f)
    
    # Mower starts 2m away from path start
    mower_start = (work_path[0][0] - 1.5, work_path[0][1] - 1.5)
    mower_heading = pi / 4  # 45 degrees
    
    # Plan path heading
    plan_heading = compute_path_heading(work_path, 0)
    
    # Generate approach paths with different methods
    approach_line = plan_approach_path(mower_start, mower_heading, 
                                       work_path[0], plan_heading, 'line')
    approach_smooth = plan_approach_path(mower_start, mower_heading,
                                        work_path[0], plan_heading, 'smooth')
    
    # Plot comparison
    fig, (ax1, ax2) = plt.subplots(1, 2, figsize=(14, 6))
    
    # Line method
    ax1.plot([p[0] for p in work_path], [p[1] for p in work_path], 
             'b-', linewidth=2, label='Work Path')
    ax1.plot([p[0] for p in approach_line], [p[1] for p in approach_line],
             'r--', linewidth=2, marker='o', markersize=4, label='Approach (Line)')
    ax1.plot(mower_start[0], mower_start[1], 'go', markersize=10, label='Mower Start')
    ax1.arrow(mower_start[0], mower_start[1], 0.3*cos(mower_heading), 0.3*sin(mower_heading),
              head_width=0.15, head_length=0.1, fc='g', ec='g')
    ax1.set_xlabel('X (m)')
    ax1.set_ylabel('Y (m)')
    ax1.set_title('Line Approach')
    ax1.legend()
    ax1.grid(True)
    ax1.axis('equal')
    
    # Smooth method
    ax2.plot([p[0] for p in work_path], [p[1] for p in work_path],
             'b-', linewidth=2, label='Work Path')
    ax2.plot([p[0] for p in approach_smooth], [p[1] for p in approach_smooth],
             'r--', linewidth=2, marker='o', markersize=4, label='Approach (Smooth)')
    ax2.plot(mower_start[0], mower_start[1], 'go', markersize=10, label='Mower Start')
    ax2.arrow(mower_start[0], mower_start[1], 0.3*cos(mower_heading), 0.3*sin(mower_heading),
              head_width=0.15, head_length=0.1, fc='g', ec='g')
    ax2.set_xlabel('X (m)')
    ax2.set_ylabel('Y (m)')
    ax2.set_title('Smooth Approach')
    ax2.legend()
    ax2.grid(True)
    ax2.axis('equal')
    
    plt.tight_layout()
    plt.savefig('approach_path_test.png', dpi=150)
    print("Saved approach_path_test.png")
    print(f"Line approach: {len(approach_line)} points")
    print(f"Smooth approach: {len(approach_smooth)} points")