route_gen.py

import math
import networkx as nx
import carla
from typing import List, Tuple


def get_nearest_node(graph: nx.DiGraph, loc: carla.Location) -> str:
    """Find the nearest graph node to a given location"""
    best, best_dist = None, float('inf')
    for node_id, data in graph.nodes(data=True):
        l2 = data['location']
        d = math.hypot(l2.x - loc.x, l2.y - loc.y)
        if d < best_dist:
            best, best_dist = node_id, d
    return best


def calculate_route_diversity(route1: List[str], route2: List[str]) -> float:
    """Calculate diversity ratio between two routes (0.0 = identical, 1.0 = completely different)"""
    if not route1 or not route2:
        return 0.0
    
    edges1 = set(zip(route1, route1[1:]))
    edges2 = set(zip(route2, route2[1:]))
    
    if len(edges1) == 0:
        return 0.0
    
    overlap = len(edges1 & edges2)
    return 1 - (overlap / len(edges1))


def calculate_route_distance(graph, route_nodes):
    """Calculate total distance of a route in meters"""
    total_distance = 0.0
    for i in range(len(route_nodes) - 1):
        current_loc = graph.nodes[route_nodes[i]]["location"]
        next_loc = graph.nodes[route_nodes[i + 1]]["location"]
        total_distance += current_loc.distance(next_loc)
    return total_distance


def find_k_shortest_paths_fast(graph: nx.DiGraph, start: str, end: str, k: int = 8) -> List[List[str]]:
    """
    Fast K-shortest paths using multiple strategies
    Returns more diverse routes by trying different approaches
    """
    paths = []
        
    try:
        # Path 1: Standard shortest path
        shortest = nx.shortest_path(graph, start, end, weight='length')
        paths.append(shortest)
        
        # Strategy 1: Remove edges from different sections of shortest path
        for section_start in [0.1, 0.3, 0.5, 0.7]:  # Try more sections
            for remove_count in [1, 2, 3, 4]:  # Try different removal amounts
                graph_copy = graph.copy()
                
                start_idx = int(len(shortest) * section_start)
                end_idx = min(start_idx + remove_count, len(shortest) - 1)
                
                # Remove consecutive edges
                removed = 0
                for j in range(start_idx, end_idx):
                    if j < len(shortest) - 1:
                        node1, node2 = shortest[j], shortest[j + 1]
                        if graph_copy.has_edge(node1, node2):
                            graph_copy.remove_edge(node1, node2)
                            removed += 1
                
                if removed > 0:
                    try:
                        alternative = nx.shortest_path(graph_copy, start, end, weight='length')
                        if len(alternative) <= len(shortest) * 3:  # Allow longer alternatives
                            if alternative not in paths:  # Avoid duplicates
                                paths.append(alternative)
                                distance = calculate_route_distance(graph, alternative)
                                diversity = calculate_route_diversity(shortest, alternative)
                                if len(paths) >= k:
                                    return paths
                    except nx.NetworkXNoPath:
                        continue
        
        # Strategy 2: Remove random edges pattern
        import random
        for seed in [42, 123, 456, 789, 101112]:  # More random attempts
            random.seed(seed)
            graph_copy = graph.copy()
            
            # Randomly select edges to remove
            edges_to_remove = max(1, len(shortest) // 6)  # Remove fewer edges
            indices = random.sample(range(len(shortest) - 1), min(edges_to_remove, len(shortest) - 1))
            
            removed = 0
            for j in indices:
                node1, node2 = shortest[j], shortest[j + 1]
                if graph_copy.has_edge(node1, node2):
                    graph_copy.remove_edge(node1, node2)
                    removed += 1
            
            if removed > 0:
                try:
                    alternative = nx.shortest_path(graph_copy, start, end, weight='length')
                    if len(alternative) <= len(shortest) * 3:
                        if alternative not in paths:  # Avoid duplicates
                            paths.append(alternative)
                            distance = calculate_route_distance(graph, alternative)
                            diversity = calculate_route_diversity(shortest, alternative)
                            if len(paths) >= k:
                                return paths
                except nx.NetworkXNoPath:
                    continue
        
        # Strategy 3: Intermediate waypoints for very different routes
        if len(paths) < k:
            try:
                # Find nodes that are not on the shortest path
                shortest_set = set(shortest)
                all_nodes = list(graph.nodes())
                
                import random
                random.seed(42)
                potential_waypoints = [node for node in all_nodes if node not in shortest_set]
                
                if len(potential_waypoints) > 10:
                    waypoints = random.sample(potential_waypoints, min(10, len(potential_waypoints)))
                    
                    for waypoint in waypoints:
                        try:
                            # Route through waypoint
                            part1 = nx.shortest_path(graph, start, waypoint, weight='length')
                            part2 = nx.shortest_path(graph, waypoint, end, weight='length')
                            
                            # Combine parts (remove duplicate waypoint)
                            waypoint_route = part1 + part2[1:]
                            
                            if len(waypoint_route) <= len(shortest) * 4:  # Allow much longer waypoint routes
                                if waypoint_route not in paths:  # Avoid duplicates
                                    paths.append(waypoint_route)
                                    distance = calculate_route_distance(graph, waypoint_route)
                                    diversity = calculate_route_diversity(shortest, waypoint_route)
                                    if len(paths) >= k:
                                        return paths
                        except nx.NetworkXNoPath:
                            continue
            except Exception:
                pass
                
    except nx.NetworkXNoPath:
        print("❌ No path found between start and end nodes")
        return []
    
    print(f"📊 Total routes generated: {len(paths)}")
    return paths


def pick_diverse_routes(candidate_paths: List[List[str]], threshold: float = 0.2, num_routes: int = 3) -> List[List[str]]:
    """
    From candidate_paths, pick diverse routes with at least 'threshold' difference
    Enhanced with detailed logging
    """
    if not candidate_paths:
        return []
    
    print(f"🎯 Applying diversity filter (threshold: {threshold:.1%}, max routes: {num_routes})")
    
    selected = []
    
    # Always take the first (shortest) path
    selected.append(candidate_paths[0])
    
    # Add additional paths that meet diversity threshold
    for i, candidate in enumerate(candidate_paths[1:], 1):
        if len(selected) >= num_routes:
            break
            
        # Check diversity against all previously selected paths
        is_diverse_enough = True
        min_diversity = float('inf')
        
        for j, selected_path in enumerate(selected):
            diversity = calculate_route_diversity(selected_path, candidate)
            min_diversity = min(min_diversity, diversity)
            
            if diversity < threshold:
                is_diverse_enough = False
                break
        
        if is_diverse_enough:
            selected.append(candidate)
    
    print(f"📊 Final selection: {len(selected)} diverse routes from {len(candidate_paths)} candidates")
    return selected


def plan_diverse_routes(graph: nx.DiGraph, loc_a: carla.Location, loc_b: carla.Location,
                       threshold: float = 0.2, max_routes: int = 3) -> List[Tuple[str, List[str]]]:
    """
    Fast diverse route planning using edge removal strategy
    
    Args:
        graph: Road network graph
        loc_a: Start location
        loc_b: End location  
        threshold: Minimum diversity required (0.2 = 20% different)
        max_routes: Maximum number of routes
    
    Returns:
        List of (route_name, route_nodes) tuples
    """
    start_node = get_nearest_node(graph, loc_a)
    end_node = get_nearest_node(graph, loc_b)
    
    if not start_node or not end_node:
        return []
    
    # Generate candidate paths using fast method
    candidates = find_k_shortest_paths_fast(graph, start_node, end_node, k=8)  # Generate more candidates
    
    if not candidates:
        return []
    
    print(f"Generated {len(candidates)} candidate routes")
    
    # Pick diverse routes from candidates
    diverse_routes = pick_diverse_routes(candidates, threshold, max_routes)
    
    # Convert to named tuples
    route_names = ["Shortest", "Alternative-1", "Alternative-2", "Alternative-3"]
    results = []
    
    for i, route in enumerate(diverse_routes):
        name = route_names[i] if i < len(route_names) else f"Alternative-{i}"
        results.append((name, route))
        
        # Show diversity info
        if i > 0:
            diversity = calculate_route_diversity(diverse_routes[0], route)
            print(f"Route {i+1} ({name}): {len(route)} nodes, {diversity:.1%} different from shortest")
        else:
            print(f"Route 1 (Shortest): {len(route)} nodes")
    
    return results