feature_extractor.py

import carla
import math
import json
import os
from datetime import datetime
from typing import List, Tuple, Dict, Any
import sys

sys.path.append(r"C:\Users\eliav\Desktop\Uni\Workshop\CARLA_0.9.11\WindowsNoEditor\PythonAPI\carla")
from agents.navigation.local_planner import RoadOption

class RouteFeatureExtractor:
    def __init__(self, world):
        self.world = world
        self.map = world.get_map()
        
    def extract_all_features(self, route: List[Tuple[carla.Waypoint, RoadOption]]) -> Dict[str, Any]:
        """
        Extract ETA-relevant features from a route
        
        Args:
            route: List of (carla.Waypoint, RoadOption) tuples
            
        Returns:
            Dictionary containing all extracted features for ETA prediction
        """
        if not route:
            return {}
        
        features = {}
        
        # Core ETA features
        features.update(self._extract_distance_features(route))
        features.update(self._extract_speed_features(route))
        features.update(self._extract_traffic_features(route))
        features.update(self._extract_junction_features(route))
        features.update(self._extract_road_complexity_features(route))
        features.update(self._extract_environmental_features())
        
        # NEW: Additional advanced features
        features.update(self._extract_traffic_density_features(route))
        features.update(self._extract_road_type_features(route))
        features.update(self._extract_maneuver_sequence_features(route))
        features.update(self._extract_advanced_complexity_features(route, features))
        
        # Add basic counts
        features['total_waypoints'] = len(route)
        
        return features
    
    def _extract_distance_features(self, route: List[Tuple[carla.Waypoint, RoadOption]]) -> Dict[str, float]:
        """Extract distance-related features"""
        features = {}
        
        total_distance = 0.0
        segment_distances = []
        
        for i in range(len(route) - 1):
            current_wp = route[i][0]
            next_wp = route[i + 1][0]
            
            segment_dist = current_wp.transform.location.distance(next_wp.transform.location)
            segment_distances.append(segment_dist)
            total_distance += segment_dist
        
        features['total_distance'] = total_distance
        features['avg_segment_distance'] = sum(segment_distances) / len(segment_distances) if segment_distances else 0
        features['max_segment_distance'] = max(segment_distances) if segment_distances else 0
        features['min_segment_distance'] = min(segment_distances) if segment_distances else 0
        
        return features
    
    def _extract_speed_features(self, route: List[Tuple[carla.Waypoint, RoadOption]]) -> Dict[str, float]:
        """Extract speed-related features for ETA prediction"""
        features = {}
        
        speed_limits = []
        for waypoint, _ in route:
            try:
                # Get actual speed limit from landmark
                landmarks = waypoint.get_landmarks(distance=50.0)
                speed_limit = None
                for landmark in landmarks:
                    if 'speed' in landmark.name.lower() or landmark.type == '274':  # Speed limit sign
                        try:
                            speed_limit = float(landmark.name.split('_')[-1])
                            break
                        except:
                            continue
                
                if speed_limit is None:
                    # Fallback: estimate based on road type - defaulting to 30 km/h for urban
                    speed_limit = 30.0  # Default urban speed
                        
                speed_limits.append(speed_limit)
            except:
                speed_limits.append(30.0)  # Default fallback to 30
        
        if speed_limits:
            features['avg_speed_limit'] = sum(speed_limits) / len(speed_limits)
            features['min_speed_limit'] = min(speed_limits)
            features['max_speed_limit'] = max(speed_limits)
        else:
            features.update({'avg_speed_limit': 30.0, 'min_speed_limit': 30.0, 'max_speed_limit': 30.0})
        
        return features
    
    def _extract_environmental_features(self) -> Dict[str, Any]:
        """Extract time of day and weather features"""
        features = {}
        
        # Get weather conditions
        weather = self.world.get_weather()
        features['precipitation'] = weather.precipitation  # 0-100 (rain intensity)
        features['cloudiness'] = weather.cloudiness  # 0-100
        features['wind_intensity'] = weather.wind_intensity  # 0-100
        features['fog_density'] = weather.fog_density  # 0-100
        features['wetness'] = weather.wetness  # 0-100 (road wetness)
        
        # Calculate visibility score (higher = better visibility)
        features['visibility_score'] = 100 - (weather.fog_density * 0.5 + weather.cloudiness * 0.3 + weather.precipitation * 0.2)
        
        # Get time of day
        # CARLA uses sun_altitude_angle: -90 (midnight) to +90 (noon)
        sun_angle = weather.sun_altitude_angle
        
        # Convert to hour of day (approximate)
        # -90 to +90 maps to 0 to 24 hours
        hour_of_day = ((sun_angle + 90) / 180) * 24
        features['hour_of_day'] = max(0, min(24, hour_of_day))
        
        # Time categories for ETA prediction
        if 6 <= hour_of_day <= 9 or 17 <= hour_of_day <= 19:
            features['is_rush_hour'] = 1
        else:
            features['is_rush_hour'] = 0
            
        if 22 <= hour_of_day or hour_of_day <= 6:
            features['is_night'] = 1
        else:
            features['is_night'] = 0
            
        # Weather impact on driving (0-1 scale, higher = worse conditions)
        weather_impact = (weather.precipitation * 0.4 + weather.fog_density * 0.3 + 
                         weather.wetness * 0.2 + weather.wind_intensity * 0.1) / 100
        features['weather_impact_score'] = min(1.0, weather_impact)
        
        return features
    
    def _extract_junction_features(self, route: List[Tuple[carla.Waypoint, RoadOption]]) -> Dict[str, Any]:
        """Extract junction-related features for ETA prediction"""
        features = {}
        
        junction_waypoints = 0
        unique_junctions = set()
        
        for waypoint, _ in route:
            if waypoint.is_junction:
                junction_waypoints += 1
                if waypoint.junction_id is not None:
                    unique_junctions.add(waypoint.junction_id)
        
        features['junctions'] = len(unique_junctions)
        features['junction_waypoints'] = junction_waypoints
        features['junction_ratio'] = junction_waypoints / len(route) if route else 0
        
        return features
    
    def _extract_maneuver_features(self, route: List[Tuple[carla.Waypoint, RoadOption]]) -> Dict[str, Any]:
        """Extract maneuver features relevant for ETA prediction"""
        features = {}
        
        maneuver_counts = {
            'LANEFOLLOW': 0,
            'LEFT': 0,
            'RIGHT': 0,
            'STRAIGHT': 0,
            'CHANGELANERIGHT': 0,
            'CHANGELANELEFT': 0
        }
        
        for _, road_option in route:
            option_name = road_option.name
            if option_name in maneuver_counts:
                maneuver_counts[option_name] += 1
        
        # Add individual counts
        features.update(maneuver_counts)
        
        # Calculate ratios (more useful for ML)
        total_waypoints = len(route)
        if total_waypoints > 0:
            features['lanefollow_ratio'] = maneuver_counts['LANEFOLLOW'] / total_waypoints
            features['turn_ratio'] = (maneuver_counts['LEFT'] + maneuver_counts['RIGHT']) / total_waypoints
            features['lanechange_ratio'] = (maneuver_counts['CHANGELANERIGHT'] + maneuver_counts['CHANGELANELEFT']) / total_waypoints
        
        return features
    
    def _extract_traffic_features(self, route: List[Tuple[carla.Waypoint, RoadOption]]) -> Dict[str, int]:
        """Extract traffic control features for ETA prediction with proper deduplication"""
        features = {}
        
        # Get all traffic lights
        traffic_lights = self.world.get_actors().filter('traffic.traffic_light')
        
        unique_traffic_lights = set()
        stop_sign_junctions = set()  # Track junctions with stop signs
        
        
        for i, (waypoint, _) in enumerate(route):
            wp_location = waypoint.transform.location
            
            # Check for traffic lights (more generous radius, better deduplication)
            for traffic_light in traffic_lights:
                tl_location = traffic_light.get_location()
                distance = wp_location.distance(tl_location)
                
                # If waypoint is within 12 meters of a traffic light
                if distance < 12.0:
                    # Use rounded location with larger grid to catch all related lights
                    tl_key = (round(tl_location.x / 10) * 10, round(tl_location.y / 10) * 10)
                    if tl_key not in unique_traffic_lights:
                        unique_traffic_lights.add(tl_key)
            
            # Check for stop signs - group by junction to avoid duplicates
            try:
                landmarks = waypoint.get_landmarks(distance=15.0)
                for landmark in landmarks:
                    if 'stop' in landmark.name.lower() or landmark.type == '206':  # Stop sign
                        # If in a junction, use junction ID to avoid duplicates
                        if waypoint.is_junction and waypoint.junction_id is not None:
                            junction_key = waypoint.junction_id
                            if junction_key not in stop_sign_junctions:
                                stop_sign_junctions.add(junction_key)
                        else:
                            # Not in junction, use location-based deduplication
                            landmark_loc = landmark.transform.location
                            stop_key = (round(landmark_loc.x / 5) * 5, round(landmark_loc.y / 5) * 5)
                            if stop_key not in stop_sign_junctions:
                                stop_sign_junctions.add(stop_key)
                        break  # Only count one stop sign per waypoint
            except Exception as e:
                pass
             
        features['traffic_lights'] = len(unique_traffic_lights)
        features['stop_signs'] = len(stop_sign_junctions)
        features['traffic_controls_total'] = len(unique_traffic_lights) + len(stop_sign_junctions)
        
        return features
    
    def _extract_road_complexity_features(self, route: List[Tuple[carla.Waypoint, RoadOption]]) -> Dict[str, float]:
        """Extract road complexity features that affect driving time"""
        features = {}
        
        # Count actual turns and lane changes from RoadOption
        left_turns = sum(1 for _, road_option in route if road_option.name == 'LEFT')
        right_turns = sum(1 for _, road_option in route if road_option.name == 'RIGHT')
        lane_changes = sum(1 for _, road_option in route if road_option.name in ['CHANGELANERIGHT', 'CHANGELANELEFT'])
        
        total_turns = left_turns + right_turns
                
        features['left_turns'] = left_turns
        features['right_turns'] = right_turns
        features['total_turns'] = total_turns
        features['lane_changes'] = lane_changes
        features['complex_maneuvers'] = total_turns + lane_changes
        
        # Calculate route straightness (how direct the route is)
        if len(route) >= 2:
            start_loc = route[0][0].transform.location
            end_loc = route[-1][0].transform.location
            straight_distance = start_loc.distance(end_loc)
            route_distance = self._extract_distance_features(route)['total_distance']
            features['route_directness'] = straight_distance / route_distance if route_distance > 0 else 0
            
        else:
            features['route_directness'] = 1.0
        
        return features
    
    def _extract_traffic_density_features(self, route: List[Tuple[carla.Waypoint, RoadOption]]) -> Dict[str, float]:
        """Extract traffic density features"""
        features = {}
        
        # Get all vehicles in the world
        vehicles = self.world.get_actors().filter('vehicle.*')
        
        nearby_vehicles_total = 0
        vehicles_per_segment = []
        
        for waypoint, _ in route:
            wp_location = waypoint.transform.location
            nearby_count = 0
            
            # Count vehicles within 50m radius of each waypoint
            for vehicle in vehicles:
                try:
                    vehicle_location = vehicle.get_location()
                    distance = wp_location.distance(vehicle_location)
                    if distance <= 50.0:
                        nearby_count += 1
                except:
                    continue
            
            vehicles_per_segment.append(nearby_count)
            nearby_vehicles_total += nearby_count
        
        # Calculate total route distance for density calculation
        total_distance = 0.0
        for i in range(len(route) - 1):
            current_wp = route[i][0]
            next_wp = route[i + 1][0]
            total_distance += current_wp.transform.location.distance(next_wp.transform.location)
        
        route_length_km = total_distance / 1000.0
        
        features['avg_nearby_vehicles'] = sum(vehicles_per_segment) / len(vehicles_per_segment) if vehicles_per_segment else 0
        features['max_nearby_vehicles'] = max(vehicles_per_segment) if vehicles_per_segment else 0
        features['vehicle_density_per_km'] = nearby_vehicles_total / route_length_km if route_length_km > 0 else 0
        features['congestion_score'] = min(1.0, features['avg_nearby_vehicles'] / 10.0)  # Normalized 0-1
        
        return features

    def _extract_road_type_features(self, route: List[Tuple[carla.Waypoint, RoadOption]]) -> Dict[str, float]:
        """Extract road type and infrastructure features"""
        features = {}
        
        highway_waypoints = 0
        urban_waypoints = 0
        lane_width_sum = 0
        unique_road_ids = set()
        elevation_changes = []
        
        for i, (waypoint, _) in enumerate(route):
            # Collect unique road IDs for diversity
            unique_road_ids.add(waypoint.road_id)
            
            # Lane width for road type classification
            lane_width = waypoint.lane_width
            lane_width_sum += lane_width
            
            # Classify by lane width and speed (rough heuristic)
            if lane_width > 4.0:  # Wider lanes suggest highways
                highway_waypoints += 1
            else:
                urban_waypoints += 1
            
            # Track elevation changes
            if i > 0:
                prev_elevation = route[i-1][0].transform.location.z
                current_elevation = waypoint.transform.location.z
                elevation_change = abs(current_elevation - prev_elevation)
                elevation_changes.append(elevation_change)
        
        total_waypoints = len(route)
        features['highway_ratio'] = highway_waypoints / total_waypoints if total_waypoints > 0 else 0
        features['urban_ratio'] = urban_waypoints / total_waypoints if total_waypoints > 0 else 0
        features['avg_lane_width'] = lane_width_sum / total_waypoints if total_waypoints > 0 else 0
        features['road_diversity'] = len(unique_road_ids)
        features['avg_elevation_change'] = sum(elevation_changes) / len(elevation_changes) if elevation_changes else 0
        features['max_elevation_change'] = max(elevation_changes) if elevation_changes else 0
        features['total_elevation_change'] = sum(elevation_changes)
        
        return features

    def _extract_maneuver_sequence_features(self, route: List[Tuple[carla.Waypoint, RoadOption]]) -> Dict[str, float]:
        """Extract maneuver sequence and pattern features"""
        features = {}
        
        # Analyze turn sequences
        max_consecutive_turns = 0
        current_turn_streak = 0
        
        # Analyze straight segments
        straight_segments = []
        current_straight_length = 0
        
        # Traffic control gaps
        traffic_control_gaps = []
        distance_since_last_control = 0
        
        for i, (waypoint, road_option) in enumerate(route):
            # Turn sequence analysis
            if road_option.name in ['LEFT', 'RIGHT']:
                current_turn_streak += 1
                max_consecutive_turns = max(max_consecutive_turns, current_turn_streak)
            else:
                current_turn_streak = 0
            
            # Straight segment analysis
            if road_option.name == 'LANEFOLLOW':
                current_straight_length += 1
            else:
                if current_straight_length > 0:
                    straight_segments.append(current_straight_length)
                current_straight_length = 0
            
            # Traffic control gap analysis (simplified - check for junctions as proxy)
            if waypoint.is_junction:
                if distance_since_last_control > 0:
                    traffic_control_gaps.append(distance_since_last_control)
                distance_since_last_control = 0
            else:
                distance_since_last_control += 1
        
        # Finalize segments
        if current_straight_length > 0:
            straight_segments.append(current_straight_length)
        
        features['max_consecutive_turns'] = max_consecutive_turns
        features['turn_clusters'] = len([s for s in straight_segments if s > 10])  # Significant straight segments
        features['avg_straight_segment_length'] = sum(straight_segments) / len(straight_segments) if straight_segments else 0
        features['max_straight_segment_length'] = max(straight_segments) if straight_segments else 0
        features['straight_segments_count'] = len(straight_segments)
        features['avg_traffic_control_gap'] = sum(traffic_control_gaps) / len(traffic_control_gaps) if traffic_control_gaps else 0
        features['max_traffic_control_gap'] = max(traffic_control_gaps) if traffic_control_gaps else 0
        
        return features

    def _extract_advanced_complexity_features(self, route: List[Tuple[carla.Waypoint, RoadOption]], existing_features: Dict[str, Any]) -> Dict[str, float]:
        """Extract advanced route complexity features"""
        features = {}
        
        # Calculate total distance for density calculations
        total_distance = 0.0
        segment_lengths = []
        for i in range(len(route) - 1):
            current_wp = route[i][0]
            next_wp = route[i + 1][0]
            segment_dist = current_wp.transform.location.distance(next_wp.transform.location)
            total_distance += segment_dist
            segment_lengths.append(segment_dist)
        
        # Calculate traffic control density
        traffic_controls = existing_features.get('traffic_controls_total', 0)
        features['traffic_control_density'] = (traffic_controls / (total_distance / 1000.0)) if total_distance > 0 else 0
        
        # Calculate complexity index (weighted combination of factors)
        junction_weight = existing_features.get('junction_ratio', 0) * 0.3
        turn_weight = (existing_features.get('total_turns', 0) / len(route)) * 0.2 if route else 0
        traffic_weight = min(1.0, traffic_controls / 20.0) * 0.3  # Normalize to reasonable range
        directness_weight = (1.0 - existing_features.get('route_directness', 1.0)) * 0.2
        
        features['route_complexity_index'] = junction_weight + turn_weight + traffic_weight + directness_weight
        
        # Route predictability (how uniform vs varied the route is)
        if segment_lengths:
            avg_segment = sum(segment_lengths) / len(segment_lengths)
            variance = sum((x - avg_segment) ** 2 for x in segment_lengths) / len(segment_lengths)
            features['route_uniformity'] = 1.0 / (1.0 + variance)  # Higher = more uniform
        else:
            features['route_uniformity'] = 1.0
        
        return features
    
    def _extract_curvature_features(self, route: List[Tuple[carla.Waypoint, RoadOption]]) -> Dict[str, float]:
        """Extract curvature and direction change features"""
        features = {}
        
        direction_changes = []
        sharp_turns = 0
        
        for i in range(1, len(route) - 1):
            prev_wp = route[i - 1][0]
            curr_wp = route[i][0]
            next_wp = route[i + 1][0]
            
            # Calculate vectors
            vec1 = curr_wp.transform.location - prev_wp.transform.location
            vec2 = next_wp.transform.location - curr_wp.transform.location
            
            # Calculate angle between vectors
            if vec1.length() > 0 and vec2.length() > 0:
                # Normalize vectors
                vec1_norm = vec1 / vec1.length()
                vec2_norm = vec2 / vec2.length()
                
                # Calculate dot product and angle
                dot_product = vec1_norm.x * vec2_norm.x + vec1_norm.y * vec2_norm.y
                dot_product = max(-1, min(1, dot_product))  # Clamp to [-1, 1]
                
                angle = math.acos(dot_product)
                direction_changes.append(math.degrees(angle))
                
                # Count sharp turns (> 30 degrees)
                if math.degrees(angle) > 30:
                    sharp_turns += 1
        
        features['avg_direction_change'] = sum(direction_changes) / len(direction_changes) if direction_changes else 0
        features['max_direction_change'] = max(direction_changes) if direction_changes else 0
        features['sharp_turns'] = sharp_turns
        features['curvature_score'] = sum(direction_changes) / len(route) if route else 0
        
        return features
    
    def print_features(self, features: Dict[str, Any]) -> None:
        """Print ETA-relevant features in a readable format"""
        print("\n=== ETA PREDICTION FEATURES ===")
        
        print("\n--- Basic Route Info ---")
        print(f"Total Distance: {features.get('total_distance', 0):.2f} meters")
        print(f"Total Waypoints: {features.get('total_waypoints', 0)}")
        print(f"Route Directness: {features.get('route_directness', 0):.3f} (1.0 = perfectly straight)")
        
        print("\n--- Speed Limits ---")
        print(f"Average Speed Limit: {features.get('avg_speed_limit', 0):.1f} km/h")
        print(f"Speed Limit Range: {features.get('min_speed_limit', 0):.1f} - {features.get('max_speed_limit', 0):.1f} km/h")
        
        print("\n--- Environmental Conditions ---")
        print(f"Hour of Day: {features.get('hour_of_day', 0):.1f}")
        print(f"Rush Hour: {'Yes' if features.get('is_rush_hour', 0) else 'No'}")
        print(f"Night Time: {'Yes' if features.get('is_night', 0) else 'No'}")
        print(f"Precipitation: {features.get('precipitation', 0):.1f}%")
        print(f"Weather Impact Score: {features.get('weather_impact_score', 0):.3f} (0=good, 1=bad)")
        
        print("\n--- Traffic Controls ---")
        print(f"Traffic Lights: {features.get('traffic_lights', 0)}")
        print(f"Stop Signs: {features.get('stop_signs', 0)}")
        print(f"Total Traffic Controls: {features.get('traffic_controls_total', 0)}")
        print(f"Traffic Control Density: {features.get('traffic_control_density', 0):.1f} per km")
        
        print("\n--- Junctions & Maneuvers ---")
        print(f"Junctions: {features.get('junctions', 0)}")
        print(f"Junction Ratio: {features.get('junction_ratio', 0):.3f}")
        print(f"Left Turns: {features.get('left_turns', 0)}")
        print(f"Right Turns: {features.get('right_turns', 0)}")
        print(f"Total Turns: {features.get('total_turns', 0)}")
        print(f"Lane Changes: {features.get('lane_changes', 0)}")
        print(f"Complex Maneuvers: {features.get('complex_maneuvers', 0)}")
        
        print("\n--- NEW: Traffic Density ---")
        print(f"Avg Nearby Vehicles: {features.get('avg_nearby_vehicles', 0):.1f}")
        print(f"Vehicle Density: {features.get('vehicle_density_per_km', 0):.1f} per km")
        print(f"Congestion Score: {features.get('congestion_score', 0):.3f}")
        
        print("\n--- NEW: Road Type ---")
        print(f"Highway Ratio: {features.get('highway_ratio', 0):.3f}")
        print(f"Urban Ratio: {features.get('urban_ratio', 0):.3f}")
        print(f"Avg Lane Width: {features.get('avg_lane_width', 0):.1f}m")
        print(f"Road Diversity: {features.get('road_diversity', 0)} roads")
        
        print("\n--- NEW: Complexity ---")
        print(f"Route Complexity Index: {features.get('route_complexity_index', 0):.3f}")
        print(f"Route Uniformity: {features.get('route_uniformity', 0):.3f}")
        print(f"Max Consecutive Turns: {features.get('max_consecutive_turns', 0)}")

    def save_features_to_json(self, features: Dict[str, Any], route_id: str = None) -> str:
        """Save features to JSON file with placeholder for ride time"""
        if route_id is None:
            route_id = f"route_{datetime.now().strftime('%Y%m%d_%H%M%S')}"
        
        # Add placeholder for actual ride time
        features['route_id'] = route_id
        features['actual_ride_time'] = None  # Placeholder for ML model
        features['timestamp'] = datetime.now().isoformat()
        
        # Create features directory if it doesn't exist
        features_dir = "route_features"
        if not os.path.exists(features_dir):
            os.makedirs(features_dir)
        
        # Save to JSON file
        filename = f"{route_id}_features.json"
        filepath = os.path.join(features_dir, filename)
        
        try:
            with open(filepath, 'w') as f:
                json.dump(features, f, indent=2)
            print(f"Features saved to: {filepath}")
            return filepath
        except Exception as e:
            print(f"Error saving features: {e}")
            return None