evodrive.py

"""
EVO Drive - A traffic simulation car sim
"""
import random
import time
import os
import math
import sys
import tty
import termios
import select
from datetime import datetime


class Snapshot:
    """
    Data structure to save a complete snapshot of the simulation state.
    """
    def __init__(self, carsim, iteration):
        self.iteration = iteration
        
        # Road grid state - full 2D array of current visible grid
        self.road_grid = [row[:] for row in carsim.road_grid.grid]
        
        # Lane speeds (absolute speeds 0-100)
        self.lane_speeds = carsim.lane_speeds[:]
        
        # Ego car state
        self.ego_car_speed = carsim.ego_car_speed
        self.ego_lane = carsim.ego_lane
        self.ego_display_col = carsim.ego_display_col
        
        # Current acceleration (if active command involves speed change)
        # Acceleration is reported as speed units per timestep
        self.ego_acceleration = None
        if carsim.active_command:
            cmd_type = carsim.active_command['type']
            if cmd_type == 'accelerate_by_n':
                # accelerate_by_n: value represents total speed change over 10 timesteps
                # Per-timestep rate = value / 10
                self.ego_acceleration = carsim.active_command.get('value', 0) / 10
            elif cmd_type == 'decelerate_by_n':
                # decelerate_by_n: value represents total speed change over 10 timesteps
                # Per-timestep rate = -value / 10 (negative for deceleration)
                self.ego_acceleration = -carsim.active_command.get('value', 0) / 10
            elif cmd_type == 'emergency_stop':
                # emergency_stop: decelerates by 1.0 per timestep until stopped
                self.ego_acceleration = -1.0
            elif cmd_type in ['overtake_on_right', 'overtake_on_left']:
                # Overtake commands have varying acceleration based on phase
                if carsim.overtake_phase == 'accelerate':
                    self.ego_acceleration = 0.3  # Per timestep during acceleration phase
                elif carsim.overtake_phase == 'passing':
                    self.ego_acceleration = 0.1  # Per timestep during passing phase
                else:
                    self.ego_acceleration = 0.0  # No acceleration during lane changes
            elif cmd_type == 'follow_safely':
                # follow_safely: accelerate/decelerate at 0.5 per timestep to target position
                if carsim.follow_safely_target_position is not None:
                    ego_pos = int(carsim.view_positions[carsim.ego_lane]) + carsim.ego_display_col
                    if ego_pos < carsim.follow_safely_target_position:
                        self.ego_acceleration = 0.5  # Accelerating to catch up
                    elif ego_pos > carsim.follow_safely_target_position:
                        self.ego_acceleration = -0.5  # Decelerating to slow down
                    else:
                        self.ego_acceleration = 0.0  # At target position
        
        # Last command executed
        self.last_command = None
        self.last_command_iteration = None
        self.command_in_progress = False
        self.command_stage = None
        
        if carsim.current_command:
            self.last_command = carsim.current_command['type']
            self.last_command_iteration = carsim.last_command_start_iteration  # Most recent command iteration
            
        # Check if any command is in progress
        if carsim.active_command:
            self.command_in_progress = True
            cmd_type = carsim.active_command['type']
            
            # Determine command stage
            if cmd_type in ['accelerate_by_n', 'decelerate_by_n']:
                timesteps_remaining = carsim.active_command_timesteps_remaining
                if timesteps_remaining > 0:
                    self.command_stage = f"executing ({10 - timesteps_remaining}/10 timesteps)"
                else:
                    self.command_stage = "complete"
            elif cmd_type == 'emergency_stop':
                if carsim.ego_car_speed > 0:
                    self.command_stage = f"stopping (speed: {carsim.ego_car_speed})"
                else:
                    self.command_stage = "complete"
            elif cmd_type in ['overtake_on_right', 'overtake_on_left']:
                self.command_stage = carsim.overtake_phase if carsim.overtake_phase else "initializing"
            elif cmd_type == 'follow_safely':
                self.command_stage = carsim.follow_safely_phase if carsim.follow_safely_phase else "initializing"
            elif cmd_type in ['move_lane_right', 'move_lane_left']:
                # Lane changes are instant but may trigger merging
                if carsim.merging:
                    self.command_stage = f"merging to lane speed {carsim.merge_target_speed}"
                elif carsim.collision:
                    self.command_stage = "collision - moved off road"
                else:
                    self.command_stage = f"changing lane from {carsim.ego_lane}"
        elif carsim.merging:
            # Merging still ongoing from previous lane change
            self.command_in_progress = True
            self.command_stage = f"merging to lane speed {carsim.merge_target_speed}"
        
        # Collision state
        self.collision = carsim.collision
        self.collision_cause = None
        if carsim.collision:
            if carsim.collision_lane is not None:
                if carsim.collision_lane == -1:
                    self.collision_cause = "went off road (top)"
                elif carsim.collision_lane == 6:
                    self.collision_cause = "went off road (bottom)"
                else:
                    self.collision_cause = "hit other vehicle"
            else:
                self.collision_cause = "hit other vehicle"
    
    def _get_command_acronym(self):
        """Generate 6-letter acronym for the last command."""
        if not self.last_command:
            return "NOCOMD"
        
        # Map command types to 6-letter acronyms
        acronym_map = {
            'accelerate_by_n': 'ACCRTE',
            'decelerate_by_n': 'DCCRTE',
            'emergency_stop': 'EMSTOP',
            'move_lane_right': 'MVLNRT',
            'move_lane_left': 'MVLNLT',
            'overtake_on_right': 'OVTKRT',
            'overtake_on_left': 'OVTKLT',
            'follow_safely': 'FOLSAF'
        }
        
        return acronym_map.get(self.last_command, 'UNKNWN')
    
    def save(self, folder="."):
        """Save snapshot to text file."""
        # Generate command acronym
        cmd_acronym = self._get_command_acronym()
        
        # Add collision prefix if this is a collision snapshot
        collision_prefix = "C_" if self.collision else ""
        
        # Get timestamp in DDMMYYYY format
        from datetime import datetime
        timestamp = datetime.now().strftime("%d%m%Y")
        
        filename = f"{collision_prefix}{cmd_acronym}_{self.iteration:04d}_{timestamp}.txt"
        filepath = os.path.join(folder, filename)
        
        with open(filepath, 'w') as f:
            # Write simulation description
            f.write("SIMULATION: EVO Drive - Traffic simulation with autonomous driving commands\n\n")
            f.write("This snapshot captures the complete state of the simulation at a specific iteration.\n")
            f.write("The road is displayed horizontally with 6 lanes (rows) and 100 columns.\n")
            f.write("Lane 0 is the top lane, Lane 5 is the bottom lane.\n")
            f.write("The ego car (autonomous vehicle) is fixed at column 50 in the display.\n")
            f.write("Traffic flows from left to right.\n\n")
            
            # Grid size and legend
            f.write("GRID SIZE: 6 rows × 100 columns\n")
            f.write("Each row is a lane for cars.\n")
            f.write("c = car position\n")
            f.write("e = ego car position\n\n")
            
            # List filled cells and collect car positions for spacing calculation
            lane_positions = {i: [] for i in range(6)}
            f.write("FILLED CELLS:\n")
            for lane in range(len(self.road_grid)):
                for col in range(len(self.road_grid[lane])):
                    cell = self.road_grid[lane][col]
                    if cell == 1:
                        f.write(f"c ({lane}, {col})\n")
                        lane_positions[lane].append(col)
                    elif cell == 2:
                        f.write(f"e ({lane}, {col})\n")
            f.write("\n")
            
            # Car spacings section
            f.write("CAR SPACINGS:\n")
            for lane in range(6):
                positions = sorted(lane_positions[lane])
                if len(positions) < 2:
                    spacings = [0]
                else:
                    spacings = [positions[i] - positions[i-1] for i in range(1, len(positions))]
                spacing_str = ", ".join(str(s) for s in spacings)
                f.write(f"Lane {lane}: {spacing_str}\n")
            f.write("\n")
            
            # Visual representation
            f.write("VISUAL REPRESENTATION:\n")
            for lane in range(len(self.road_grid)):
                f.write(f"Lane {lane}: ")
                for col in range(len(self.road_grid[lane])):
                    cell = self.road_grid[lane][col]
                    if cell == 2:
                        f.write("e")
                    elif cell == 1:
                        f.write("c")
                    else:
                        f.write(".")
                f.write("\n")
            f.write("\n")
            
            # Iteration number
            f.write(f"ITERATION: {self.iteration}\n\n")
            
            # Lane speeds
            f.write("LANE SPEEDS (0-100 units):\n")
            for i, speed in enumerate(self.lane_speeds):
                f.write(f"  Lane {i}: {speed}\n")
            f.write("\n")
            
            # Ego car state
            f.write("EGO CAR STATE:\n")
            f.write(f"  Speed: {self.ego_car_speed}\n")
            f.write(f"  Lane: {self.ego_lane}\n")
            f.write(f"  Display Column: {self.ego_display_col}\n")
            if self.ego_acceleration is not None:
                f.write(f"  Acceleration: {self.ego_acceleration:.2f} speed units per timestep\n")
            else:
                f.write(f"  Acceleration: None (not accelerating)\n")
            f.write("\n")
            
            # Command state
            f.write("COMMAND STATE:\n")
            if self.last_command:
                f.write(f"  Last Command: {self.last_command}\n")
                f.write(f"  Last Command Iteration: {self.last_command_iteration}\n")
                f.write(f"  Command In Progress: {self.command_in_progress}\n")
                if self.command_stage:
                    f.write(f"  Command Stage: {self.command_stage}\n")
            else:
                f.write("  No command executed yet\n")
            f.write("\n")
            
            # Collision state
            f.write("COLLISION STATE:\n")
            f.write(f"  Collision Occurred: {self.collision}\n")
            if self.collision and self.collision_cause:
                f.write(f"  Collision Cause: {self.collision_cause}\n")
            f.write("\n")
        
        return filepath


class RoadGrid:
    """
    Represents the visible road area (6 lanes high x 100 cells wide).
    Each cell represents a car space: 4m long x 2m wide.
    Road is displayed horizontally with traffic flowing left to right.
    """
    def __init__(self, height=6, width=100):
        self.height = height  # Number of lanes
        self.width = width  # Length of road
        self.grid = [[0 for _ in range(width)] for _ in range(height)]
        self.tree_array = None  # Will be set by carsim
        self.tree_view_position = 0.0  # Scrolls with ego speed
    
    def clear(self):
        """Clear the road grid."""
        self.grid = [[0 for _ in range(self.width)] for _ in range(self.height)]
    
    def set_cell(self, row, col, value):
        """Set a cell value in the grid."""
        if 0 <= row < self.height and 0 <= col < self.width:
            self.grid[row][col] = value
    
    def get_cell(self, row, col):
        """Get a cell value from the grid."""
        if 0 <= row < self.height and 0 <= col < self.width:
            return self.grid[row][col]
        return 0
    
    def display(self, collision=False, collision_lane=None, collision_col=None, lane_speeds=None, ego_lane=None):
        """Display the full road grid in terminal (horizontal layout).
        
        Args:
            collision: Whether a collision occurred
            collision_lane: Lane where collision occurred (can be -1 or 6 for off-road)
            collision_col: Column where collision occurred
            lane_speeds: List of 6 lane speeds to display on the left
            ego_lane: Current ego car lane (for highlighting speed)
        """
        # Print top trees
        print(" " * 6, end="")  # Left padding to match lane speed formatting
        for col_idx in range(self.width):
            tree_col = int(self.tree_view_position) + col_idx
            tree_col = tree_col % self.tree_array.width
            if self.tree_array.get_tree(0, tree_col) == 1:
                print("\033[30m^\033[0m", end="")
            else:
                print(" ", end="")
        print()
        
        # Print top border (with embedded car if collision off top)
        print(" " * 6, end="")  # Left padding to match lane speed formatting
        if collision and collision_lane == -1:
            # Show car embedded in top border
            for col_idx in range(self.width):
                if col_idx == collision_col:
                    print("\033[91m▀\033[0m", end="")  # Red ego car crashed in border
                else:
                    print("=", end="")
        else:
            print("=" * self.width, end="")
        print()
        
        # Print each lane horizontally with speed on the left
        for lane_idx in range(self.height):
            # Print lane speed on the left
            speed = lane_speeds[lane_idx]
            # Highlight ego lane speed
            if ego_lane == lane_idx:
                print(f"\033[94m{speed:3d}\033[0m ", end="")  # Blue for ego lane
            elif speed == 0:
                print(f"\033[91m{speed:3d}\033[0m ", end="")  # Red for stopped
            elif speed < 15:
                print(f"\033[93m{speed:3d}\033[0m ", end="")  # Yellow for very slow
            else:
                print(f"{speed:3d} ", end="")
            print("| ", end="")  # Separator between speed and road
            
            # Print road cells
            for col_idx in range(self.width):
                cell = self.grid[lane_idx][col_idx]
                # Check if this is collision position - show red car
                if collision and lane_idx == collision_lane and col_idx == collision_col:
                    print("\033[91m▀\033[0m", end="")  # Red ego car (collision)
                # Only show green ego car if this is NOT the collision position
                elif cell == 2 and not (collision and lane_idx == collision_lane and col_idx == collision_col):
                    print("\033[32m▀\033[0m", end="")  # Dark green ego car
                elif cell == 1:
                    print("▀", end="")  # Regular car
                else:
                    print("˙", end="")  # Empty road
            print()  # New line after each lane
        
        # Print bottom border (with embedded car if collision off bottom)
        print(" " * 6, end="")  # Left padding to match lane speed formatting
        if collision and collision_lane == self.height:
            # Show car embedded in bottom border
            for col_idx in range(self.width):
                if col_idx == collision_col:
                    print("\033[91m▀\033[0m", end="")  # Red ego car crashed in border
                else:
                    print("=", end="")
        else:
            print("=" * self.width, end="")
        print()
        
        # Print bottom trees
        print(" " * 6, end="")  # Left padding to match lane speed formatting
        for col_idx in range(self.width):
            tree_col = int(self.tree_view_position) + col_idx
            tree_col = tree_col % self.tree_array.width
            if self.tree_array.get_tree(1, tree_col) == 1:
                print("\033[30m^\033[0m", end="")
            else:
                print(" ", end="")
        print()
        
        # Show collision message
        if collision:
            print(" " * 10 + "\033[91m*** COLLISION ***\033[0m")


class TrafficGrid:
    """
    Represents a single lane traffic pattern (10000 high x 1 wide).
    Cars are randomly placed with varying density using a sinusoidal pattern.
    """
    def __init__(self, height=10000, lane_number=0):
        self.height = height
        self.lane_number = lane_number  # 0-5, where 5 is rightmost (fastest)
        self.grid = [0 for _ in range(height)]
        self._generate_traffic()
    
    def _generate_density_sine(self):
        """
        Create a sinusoidal density function that repeats 8-20 times over the grid height.
        Higher lane numbers (rightmost/faster lanes) have fewer cycles (less congestion).
        Each lane starts at a random phase offset in the sine wave.
        Returns a function that takes a position and returns a density value (0.0 to 1.0).
        """
        # Rightmost lanes have lower max cycles (less congested)
        # Lane 0 (left): 10-20 cycles (most congested)
        # Lane 5 (right): 5-10 cycles (least congested)
        max_cycles = 20 - (self.lane_number * 2)  # 20, 18, 16, 14, 12, 10
        min_cycles = 10 - self.lane_number  # 10, 9, 8, 7, 6, 5
        num_cycles = random.randint(min_cycles, max_cycles)
        frequency = (2 * math.pi * num_cycles) / self.height
        
        # Random phase offset (0 to 2π) - each lane starts at different point in wave
        phase_offset = random.uniform(0, 2 * math.pi)
        
        def density_sine(position):
            # Returns value between 0.0 (minimum) and 1.0 (maximum)
            # sin wave oscillates between -1 and 1, so we normalize to 0-1
            return (math.sin(frequency * position + phase_offset) + 1) / 2
        
        return density_sine
    
    def _generate_traffic(self):
        """
        Generate random traffic pattern using sinusoidal density variation.
        - High density (sine at max): cars placed one after another (gap of 1)
        - Low density (sine at min): cars placed far apart (gap of 20)
        - Medium density (sine at middle): medium spacing (gap of ~10)
        """
        # Create the density sine wave for this lane
        density_sine = self._generate_density_sine()
        
        row = 0
        while row < self.height:
            # Get density at current position (0.0 to 1.0)
            density = density_sine(row)
            
            # Place a car
            self.grid[row] = 1
            
            # Calculate gap based on density
            # density = 1.0 (max): gap = 1 (very packed)
            # density = 0.5 (mid): gap = ~10
            # density = 0.0 (min): gap = 20 (sparse)
            min_gap = 1
            max_gap = 20
            gap = int(min_gap + (1 - density) * (max_gap - min_gap))
            
            row += gap
    
    def get_cell(self, row):
        """
        Get a cell value from the grid.
        Returns 0 if out of bounds.
        """
        if 0 <= row < self.height:
            return self.grid[row]
        return 0
    
    def find_empty_slot(self, start_position):
        """
        Find the nearest empty slot starting from start_position.
        Searches forward first, then backward if needed.
        
        Args:
            start_position: Starting position to search from
        
        Returns:
            Position of nearest empty slot
        """
        # Check if start position is already empty
        if self.get_cell(start_position) == 0:
            return start_position
        
        # Search forward and backward simultaneously
        max_search = 100  # Don't search too far
        for offset in range(1, max_search):
            # Search forward
            forward_pos = start_position + offset
            if forward_pos < self.height and self.get_cell(forward_pos) == 0:
                return forward_pos
            
            # Search backward
            backward_pos = start_position - offset
            if backward_pos >= 0 and self.get_cell(backward_pos) == 0:
                return backward_pos
        
        # If no empty slot found, return start position anyway
        return start_position


class TreeArray:
    """
    Represents tree placement along the roadside (2 rows x 1000 wide).
    Row 0 = top side trees, Row 1 = bottom side trees.
    1 = tree present, 0 = no tree.
    """
    def __init__(self, width=1000):
        self.width = width
        self.array = [[0 for _ in range(width)] for _ in range(2)]
        self._generate_trees()
    
    def _generate_trees(self):
        """Generate random tree placement with ~10% probability."""
        for col in range(self.width):
            # Top side (row 0)
            if random.random() < 0.1:
                self.array[0][col] = 1
            # Bottom side (row 1)
            if random.random() < 0.1:
                self.array[1][col] = 1
    
    def get_tree(self, side, col):
        """
        Get tree presence at a specific column and side.
        
        Args:
            side: 0 for top, 1 for bottom
            col: Position in tree array (0-999)
        
        Returns:
            1 if tree present, 0 otherwise
        """
        if side in [0, 1] and 0 <= col < self.width:
            return self.array[side][col]
        return 0


class AutoDrive:
    """
    Holds driving commands for manual control or scheduled execution.
    """
    def __init__(self):
        self.commands = []
        self.command_queue = []  # List of (iteration, command) tuples
    
    def add_command(self, cmd_type, value=None, trigger_iteration=None):
        """Add a command to the queue.
        
        Args:
            cmd_type: Type of command
            value: Command value (if applicable)
            trigger_iteration: If specified, command will be queued for this iteration
        """
        command = {'type': cmd_type, 'value': value}
        
        if trigger_iteration is not None:
            # Add to scheduled queue
            self.command_queue.append((trigger_iteration, command))
            # Sort by iteration
            self.command_queue.sort(key=lambda x: x[0])
        else:
            # Add to immediate queue
            self.commands.append(command)
    
    def get_next_command(self, current_iteration=None):
        """Get and remove the next command from the queue.
        
        Args:
            current_iteration: Current iteration number (for checking scheduled commands)
        
        Returns:
            Command dict or None
        """
        # Check scheduled commands first if iteration provided
        if current_iteration is not None and self.command_queue:
            if self.command_queue[0][0] <= current_iteration:
                iteration, command = self.command_queue.pop(0)
                return command
        
        # Otherwise return from immediate queue
        if self.commands:
            return self.commands.pop(0)
        
        return None
    
    def has_commands(self):
        """Check if there are commands in the queue."""
        return len(self.commands) > 0 or len(self.command_queue) > 0


class SpeedArray:
    """
    Represents changing lane speeds over time (10000 high x 6 wide).
    Each column represents a lane's speed over time.
    Speeds are based on traffic density from TrafficGrids.
    """
    def __init__(self, height=10000, num_lanes=6, traffic_grids=None):
        self.height = height
        self.num_lanes = num_lanes
        self.traffic_grids = traffic_grids
        self.array = [[0 for _ in range(num_lanes)] for _ in range(height)]
        self._generate_speeds()
    
    def _generate_speeds(self):
        """
        Generate speed patterns for all lanes over time.
        - Speeds are based on traffic density in each lane's TrafficGrid
        - More cars in a 20-cell window = slower speed
        - Fewer cars = faster speed
        - Each lane's speed is independent
        """
        for lane in range(self.num_lanes):
            for time_step in range(self.height):
                # Calculate traffic density in a 20-cell window centered on time_step
                window_start = max(0, time_step - 10)
                window_end = min(self.traffic_grids[lane].height, time_step + 10)
                window_size = window_end - window_start
                
                # Count cars in this window
                car_count = 0
                for pos in range(window_start, window_end):
                    car_count += self.traffic_grids[lane].get_cell(pos)
                
                # Calculate density (0.0 to 1.0)
                density = car_count / window_size if window_size > 0 else 0
                
                # Speed is a smooth linear inverse function of density
                # density = 0.0 (no cars): speed = 70
                # density = 1.0 (max cars): speed = 0
                max_speed = 70
                speed = max_speed * (1 - density)
                
                self.array[time_step][lane] = max(0, min(100, int(speed)))
    
    def get_speed_at_position(self, lane, position):
        """
        Get the speed for a specific lane at a specific TrafficGrid position.
        
        Args:
            lane: Lane number (0-5)
            position: Position in the TrafficGrid (0-9999)
        
        Returns:
            Speed value (0-100)
        """
        if 0 <= position < self.height and 0 <= lane < self.num_lanes:
            return self.array[position][lane]
        # Return neutral speed if out of bounds
        return 50


class EvoDrive:
    """
    def __init__(self):
        self.road_grid = RoadGrid(height=100, width=6)
        # Create 6 separate traffic grids, one for each lane
        # Pass lane number so rightmost lanes have less congestion
        self.traffic_grids = [TrafficGrid(height=10000, lane_number=i) for i in range(6)]
    Ego car is always visible in center of display, colored blue.
    All movement is relative to ego car speed.
    """
    def __init__(self):
        self.road_grid = RoadGrid(height=6, width=100)
        # Create 6 separate traffic grids, one for each lane
        self.traffic_grids = [TrafficGrid(height=10000) for _ in range(6)]
        
        # Create tree array and link to road grid
        self.tree_array = TreeArray(width=1000)
        self.road_grid.tree_array = self.tree_array
        
        # Ego car setup
        self.ego_lane = 0  # First lane (top)
        self.ego_display_col = self.road_grid.width // 2  # Center of visible display - FIXED position
        
        # Each lane has its own view position
        # All lanes start at 10% of TrafficGrid length
        # Find an empty spot near 10% to avoid starting on top of a car
        self.view_positions = [0.0 for _ in range(6)]
        start_position = int(self.traffic_grids[0].height * 0.1)
        
        # Use the TrafficGrid's find_empty_slot method to find nearest empty spot in lane 0
        ego_position_in_grid = self.traffic_grids[0].find_empty_slot(start_position)
        
        # Set initial view positions so ego car appears at the empty spot found
        initial_position = ego_position_in_grid - self.ego_display_col
        for i in range(6):
            self.view_positions[i] = initial_position
        
        # Create dynamic speed array with traffic density awareness
        self.speed_array = SpeedArray(height=10000, num_lanes=6, traffic_grids=self.traffic_grids)
        
        # Initialize road grid with current view to calculate initial speeds
        self._update_road_grid_from_traffic()
        
        # Calculate initial lane speeds based on actual visible car density
        self.lane_speeds = self._calculate_lane_speeds()
        self.ego_lane_speed = self.lane_speeds[self.ego_lane]  # Traffic speed in ego lane
        self.ego_car_speed = self.ego_lane_speed  # Actual car speed (can vary independently)
        
        # AutoDrive setup
        self.autodrive = AutoDrive()
        self.timestep = 0
        self.current_command = None
        self.last_command_start_iteration = None  # Track iteration of most recent command initiated
        self.command_display_counter = 0
        self.error_message = None
        self.error_message_counter = 0
        
        # Active command execution tracking
        self.active_command = None
        self.active_command_timesteps_remaining = 0
        self.speed_accumulator = 0.0  # For fractional speed changes
        
        # Lane merging tracking
        self.merging = False
        self.merge_target_speed = None
        
        # Overtake on right tracking
        self.overtaking = False
        self.overtake_phase = None  # 'accelerate', 'change_right', 'passing', 'change_left', 'merge'
        self.overtake_original_lane = None
        self.overtake_car_position = None  # Position in TrafficGrid of car being overtaken
        
        # Follow safely tracking
        self.follow_safely_target_position = None  # Target position in TrafficGrid to reach
        self.follow_safely_phase = None  # 'accelerating' or 'merging'
        
        # Keyboard input setup
        self.old_settings = None
        
        # Snapshot folder - created on first run
        self.snapshot_folder = None
        
        # Data generation mode
        self.data_generation_mode = False
        
        # Lane shift highlighting
        self.lane_shift_highlight = {}  # Dictionary: {lane_idx: counter}
        
        # Ego lane shift tracking
        self.ego_lane_iterations = 0  # How many iterations in current lane
        self.ego_lane_shift_count = 0  # How many shifts have been performed
        self.ego_lane_target_shifts = random.randint(1, 5)  # Random number of shifts to perform (1-5)
        self.ego_lane_last_shift_iteration = 0  # When the last ego lane shift occurred
        
        self.collision = False
        self.collision_lane = None  # Track which lane collision occurred in (can be -1 or 6 for off-road)
        self.running = False
    
    def _get_scroll_speed(self, lane_speed):
        """
        Convert lane speed (0-100) to actual scroll speed in cells/frame.
        0 = 0.0 cells/frame (stationary)
        100 = 3.0 cells/frame (rapid)
        """
        return (lane_speed / 100.0) * 3.0
    
    def _calculate_lane_speeds(self):
        """
        Calculate lane speeds based on actual visible car density in the road grid.
        Returns list of 6 lane speeds.
        """
        speeds = []
        for lane in range(6):
            # Count cars currently visible in this lane
            car_count = sum(1 for col in range(self.road_grid.width) if self.road_grid.grid[lane][col] == 1)
            # Calculate density (0.0 to 1.0)
            density = car_count / self.road_grid.width
            # Speed is inverse function of density
            max_speed = 70
            speed = max_speed * (1 - density)
            speeds.append(max(0, min(100, int(speed))))
        return speeds
    
    def _update_road_grid_from_traffic(self):
        """
        Update the road grid view from traffic grids based on current view_positions.
        """
        self.road_grid.clear()
        for lane in range(6):
            position = int(self.view_positions[lane])
            for col in range(self.road_grid.width):
                cell_value = self.traffic_grids[lane].get_cell(position + col)
                self.road_grid.set_cell(lane, col, cell_value)
    
    def _find_car_pair_distance(self, lane_idx, start_pos, end_pos):
        """
        Find the first two consecutive cars in a lane within a range and return their distance.
        
        Args:
            lane_idx: Lane number to search
            start_pos: Start position in traffic grid
            end_pos: End position in traffic grid
            
        Returns:
            Distance between first and second car, or None if not found
        """
        first_car_pos = None
        second_car_pos = None
        
        for pos in range(start_pos, end_pos):
            if pos >= 0 and pos < self.traffic_grids[lane_idx].height:
                if self.traffic_grids[lane_idx].grid[pos] == 1:
                    if first_car_pos is None:
                        first_car_pos = pos
                    elif second_car_pos is None:
                        second_car_pos = pos
                        return second_car_pos - first_car_pos
        
        return None
    
    def _find_next_car_ahead(self, lane, search_distance=50):
        """
        Find the next car ahead in the specified lane.
        
        Args:
            lane: Lane number to search
            search_distance: How far ahead to search in cells
        
        Returns:
            Tuple of (distance_to_car, position_in_traffic_grid) or (None, None) if no car found
        """
        ego_position = int(self.view_positions[lane]) + self.ego_display_col
        
        for distance in range(1, search_distance + 1):
            check_position = ego_position + distance
            if self.traffic_grids[lane].get_cell(check_position) == 1:
                return (distance, check_position)
        
        return (None, None)
    
    def _reset_ego_lane_shift_counters(self):
        """Reset all ego lane shift tracking counters."""
        self.ego_lane_iterations = 0
        self.ego_lane_shift_count = 0
        self.ego_lane_last_shift_iteration = 0
    
    def _initiate_follow_safely(self):
        """Initiate follow_safely command by finding cars ahead/behind and setting target position."""
        ego_position = int(self.view_positions[self.ego_lane]) + self.ego_display_col
        
        # Find car ahead
        car_ahead_pos = None
        for pos in range(ego_position + 1, min(ego_position + 50, self.traffic_grids[self.ego_lane].height)):
            if self.traffic_grids[self.ego_lane].get_cell(pos) == 1:
                car_ahead_pos = pos
                break
        
        # Find car behind
        car_behind_pos = None
        for pos in range(ego_position - 1, max(ego_position - 50, 0), -1):
            if self.traffic_grids[self.ego_lane].get_cell(pos) == 1:
                car_behind_pos = pos
                break
        
        # Set target position to midpoint between cars
        self.follow_safely_target_position = (car_ahead_pos + car_behind_pos) // 2
        self.follow_safely_phase = 'accelerating'
        self.active_command = {'type': 'follow_safely', 'value': None}
        self.active_command_timesteps_remaining = -1
        self.speed_accumulator = 0.0
        
        # Save snapshot when follow_safely is auto-initiated
        self._save_snapshot(reason="follow_safely_auto_initiated")
    
    def _process_new_command(self):
        """Process new command from queue and initiate execution."""
        next_cmd = self.autodrive.get_next_command(self.timestep)
        if not next_cmd:
            return
        
        self.current_command = next_cmd
        self.command_display_counter = 30
        
        # Start executing accelerate/decelerate commands
        if self.current_command['type'] in ['accelerate_by_n', 'decelerate_by_n']:
            self._reset_ego_lane_shift_counters()
            self.active_command = self.current_command
            self.active_command_timesteps_remaining = 10
            self.speed_accumulator = 0.0
            self.last_command_start_iteration = self.timestep  # Track when command becomes active
            # Save snapshot after command is set up
            self._save_snapshot(reason="command_initiated")
        elif self.current_command['type'] == 'emergency_stop':
            self._reset_ego_lane_shift_counters()
            self.active_command = self.current_command
            self.active_command_timesteps_remaining = -1
            self.speed_accumulator = 0.0
            self.last_command_start_iteration = self.timestep  # Track when command becomes active
            # Save snapshot after command is set up
            self._save_snapshot(reason="command_initiated")
        elif self.current_command['type'] == 'move_lane_right':
            # Save snapshot before lane change in case it causes immediate collision
            self.active_command = self.current_command  # Mark as active for snapshot
            self.last_command_start_iteration = self.timestep  # Track when command becomes active
            self._save_snapshot(reason="command_initiated")
            new_lane = self.ego_lane + 1
            if new_lane >= 6:
                self.collision = True
                self.collision_lane = 6  # Off-road below bottom lane
            else:
                self.ego_lane = new_lane
                # Reset ego lane shift counters on lane change
                self._reset_ego_lane_shift_counters()
                # Start merging to match new lane speed
                self.merging = True
                self.merge_target_speed = self.lane_speeds[new_lane]
        elif self.current_command['type'] == 'move_lane_left':
            # Save snapshot before lane change in case it causes immediate collision
            self.active_command = self.current_command  # Mark as active for snapshot
            self.last_command_start_iteration = self.timestep  # Track when command becomes active
            self._save_snapshot(reason="command_initiated")
            new_lane = self.ego_lane - 1
            if new_lane < 0:
                self.collision = True
                self.collision_lane = -1  # Off-road above top lane
            else:
                self.ego_lane = new_lane
                # Reset ego lane shift counters on lane change
                self._reset_ego_lane_shift_counters()
                # Start merging to match new lane speed
                self.merging = True
                self.merge_target_speed = self.lane_speeds[new_lane]
        elif self.current_command['type'] == 'overtake_on_right':
            # Check if there's a lane on the right
            if self.ego_lane >= 5:
                # No lane on right, can't overtake
                self.error_message = "Cannot overtake on right - no lane available"
                self.error_message_counter = 30
            else:
                # Find car ahead in current lane
                distance, car_pos = self._find_next_car_ahead(self.ego_lane)
                if distance is not None:
                    # Start overtake maneuver
                    self._reset_ego_lane_shift_counters()
                    self.overtaking = True
                    self.overtake_phase = 'accelerate'
                    self.overtake_original_lane = self.ego_lane
                    self.overtake_car_position = car_pos
                    self.active_command = self.current_command
                    self.active_command_timesteps_remaining = -1
                    self.speed_accumulator = 0.0
                    self.last_command_start_iteration = self.timestep  # Track when command becomes active
                    # Save snapshot after command is set up
                    self._save_snapshot(reason="command_initiated")
        elif self.current_command['type'] == 'overtake_on_left':
            # Check if there's a lane on the left
            if self.ego_lane <= 0:
                # No lane on left, can't overtake
                self.error_message = "Cannot overtake on left - no lane available"
                self.error_message_counter = 30
            else:
                # Find car ahead in current lane
                distance, car_pos = self._find_next_car_ahead(self.ego_lane)
                if distance is not None:
                    # Start overtake maneuver
                    self._reset_ego_lane_shift_counters()
                    self.overtaking = True
                    self.overtake_phase = 'accelerate'
                    self.overtake_original_lane = self.ego_lane
                    self.overtake_car_position = car_pos
                    self.active_command = self.current_command
                    self.active_command_timesteps_remaining = -1
                    self.speed_accumulator = 0.0
                    self.last_command_start_iteration = self.timestep  # Track when command becomes active
                    # Save snapshot after command is set up
                    self._save_snapshot(reason="command_initiated")
        elif self.current_command['type'] == 'follow_safely':
            self._reset_ego_lane_shift_counters()
            self._initiate_follow_safely()
            # Snapshot is saved inside _initiate_follow_safely
    
    def _execute_active_command(self):
        """Execute the currently active command."""
        if not self.active_command or self.active_command_timesteps_remaining == 0:
            return
        
        cmd_type = self.active_command['type']
        
        if cmd_type == 'emergency_stop':
            # Emergency stop: decelerate by 1 per timestep until speed is 0
            self.ego_car_speed = max(0, self.ego_car_speed - 1)
            
            # Clear active command when speed reaches 0
            if self.ego_car_speed == 0:
                self.active_command = None
        elif cmd_type == 'overtake_on_right' or cmd_type == 'overtake_on_left':
            # Handle overtake maneuver (right or left)
            lane_change_direction = 1 if cmd_type == 'overtake_on_right' else -1
            
            if self.overtake_phase == 'accelerate':
                # Phase 1: Accelerate by 0.3 per timestep until 2 cells from car ahead
                distance, _ = self._find_next_car_ahead(self.ego_lane)
                if distance is not None and distance <= 2:
                    # Close enough, move to adjacent lane
                    self.overtake_phase = 'change_lane'
                    self.ego_lane += lane_change_direction
                else:
                    # Keep accelerating
                    self.speed_accumulator += 0.3
                    speed_change = int(self.speed_accumulator)
                    if speed_change != 0:
                        self.ego_car_speed = min(100, self.ego_car_speed + speed_change)
                        self.speed_accumulator -= speed_change
            
            elif self.overtake_phase == 'change_lane':
                # Phase 2: Just changed to adjacent lane, start passing phase
                self.overtake_phase = 'passing'
            
            elif self.overtake_phase == 'passing':
                # Phase 3: Continue accelerating by 0.1 per timestep while passing
                self.speed_accumulator += 0.1
                speed_change = int(self.speed_accumulator)
                if speed_change != 0:
                    self.ego_car_speed = min(100, self.ego_car_speed + speed_change)
                    self.speed_accumulator -= speed_change
                
                # Check if we've passed the car by 2 cells
                # Calculate relative positions
                ego_pos_in_original_lane = int(self.view_positions[self.overtake_original_lane]) + self.ego_display_col
                if ego_pos_in_original_lane >= self.overtake_car_position + 2:
                    # We've passed the car, change back to original lane
                    self.overtake_phase = 'change_back'
                    self.ego_lane = self.overtake_original_lane
            
            elif self.overtake_phase == 'change_back':
                # Phase 4: Just changed back to original lane, start merging
                self.overtake_phase = 'merge'
                self.merging = True
                self.merge_target_speed = self.lane_speeds[self.ego_lane]
            
            elif self.overtake_phase == 'merge':
                # Phase 5: Merging to match lane speed (handled by merging logic below)
                if not self.merging:
                    # Merge complete, overtake finished
                    self.overtaking = False
                    self.overtake_phase = None
                    self.overtake_original_lane = None
                    self.overtake_car_position = None
                    self.active_command = None
                    self.speed_accumulator = 0.0
        elif cmd_type == 'follow_safely':
            # Handle follow_safely command
            if self.follow_safely_phase == 'accelerating':
                # Get current ego position
                ego_position = int(self.view_positions[self.ego_lane]) + self.ego_display_col
                
                # Check if we've reached target position
                if abs(ego_position - self.follow_safely_target_position) <= 1:
                    # Reached target, switch to merging phase
                    self.follow_safely_phase = 'merging'
                    self.merging = True
                    self.merge_target_speed = self.lane_speeds[self.ego_lane]
                else:
                    # Continue accelerating/decelerating toward target at 0.3 per timestep
                    if ego_position < self.follow_safely_target_position:
                        # Need to accelerate (go faster to catch up)
                        self.speed_accumulator += 0.3
                    else:
                        # Need to decelerate (go slower to fall back)
                        self.speed_accumulator -= 0.3
                    
                    # Apply speed change
                    speed_change = int(self.speed_accumulator)
                    if speed_change != 0:
                        self.ego_car_speed = max(0, min(100, self.ego_car_speed + speed_change))
                        self.speed_accumulator -= speed_change
            
            elif self.follow_safely_phase == 'merging':
                # Merging is handled by the main merging logic
                if not self.merging:
                    # Merge complete, follow_safely finished
                    self.follow_safely_phase = None
                    self.follow_safely_target_position = None
                    self.active_command = None
                    self.speed_accumulator = 0.0
        else:
            # accelerate_by_n or decelerate_by_n
            n_value = self.active_command.get('value', 0)
            if n_value is None:
                n_value = 0
            
            # Accumulate speed change: n represents speed change per timestep
            # n=0.1 means +1 speed every 10 timesteps, n=1 means +1 every timestep
            self.speed_accumulator += n_value
            
            # Apply integer part of accumulated speed change
            speed_change = int(self.speed_accumulator)
            if speed_change != 0:
                if cmd_type == 'accelerate_by_n':
                    self.ego_car_speed = min(100, self.ego_car_speed + speed_change)
                elif cmd_type == 'decelerate_by_n':
                    self.ego_car_speed = max(0, self.ego_car_speed - speed_change)
                
                # Keep fractional remainder
                self.speed_accumulator -= speed_change
            
            # Decrement timesteps remaining
            self.active_command_timesteps_remaining -= 1
            
            # Clear active command when complete
            if self.active_command_timesteps_remaining == 0:
                self.active_command = None
                self.speed_accumulator = 0.0
    
    def _handle_lane_merging(self):
        """Handle lane merging (matching speed to new lane)."""
        if self.merging and self.merge_target_speed is not None:
            # Update target speed to current lane's traffic speed
            self.merge_target_speed = self.lane_speeds[self.ego_lane]
            
            speed_diff = self.merge_target_speed - self.ego_car_speed
            
            # Check if we've reached target speed (within 1 unit)
            if abs(speed_diff) <= 1:
                self.ego_car_speed = self.merge_target_speed
                self.merging = False
                self.merge_target_speed = None
                # Clear active command if it was a simple lane change (not part of overtake)
                if self.active_command and self.active_command['type'] in ['move_lane_right', 'move_lane_left']:
                    self.active_command = None
            else:
                # Accelerate or decelerate at up to n=0.5 per timestep
                merge_rate = 0.5
                if speed_diff > 0:
                    # Need to speed up
                    speed_change = min(merge_rate, speed_diff)
                    self.ego_car_speed = min(100, self.ego_car_speed + speed_change)
                else:
                    # Need to slow down
                    speed_change = min(merge_rate, abs(speed_diff))
                    self.ego_car_speed = max(0, self.ego_car_speed - speed_change)
    
    def _shift_lane_forward(self, lane_idx):
        """
        Shift a lane forward by finding two consecutive cars near ego position
        and moving the lane so car B is where car A was.
        
        Args:
            lane_idx: Lane number to shift
        """
        center_pos = int(self.view_positions[lane_idx]) + self.ego_display_col
        
        # Find car A (closest car before or at center)
        car_A_pos = None
        for pos in range(center_pos, int(self.view_positions[lane_idx]) - 1, -1):
            if pos >= 0 and pos < self.traffic_grids[lane_idx].height:
                if self.traffic_grids[lane_idx].grid[pos] == 1:
                    car_A_pos = pos
                    break
        
        # Find car B (closest car after center)
        car_B_pos = None
        for pos in range(center_pos + 1, min(int(self.view_positions[lane_idx]) + self.road_grid.width, self.traffic_grids[lane_idx].height)):
            if self.traffic_grids[lane_idx].grid[pos] == 1:
                car_B_pos = pos
                break
        
        # Shift the lane so B moves to where A was
        distance = car_B_pos - car_A_pos
        self.view_positions[lane_idx] = (self.view_positions[lane_idx] + distance) % self.traffic_grids[lane_idx].height
        
        # Highlight lane position in red briefly
        if not self.data_generation_mode:
            self.lane_shift_highlight[lane_idx] = 30
    
    def _process_ego_lane_shift(self):
        """
        Process ego lane shift: if ego car has been in same lane for >100 iterations,
        shift the lane 1-5 times (random) every 20 iterations.
        """
        if self.ego_lane_iterations > 100:
            # Set new random target if starting a new shift cycle
            if self.ego_lane_shift_count == 0:
                self.ego_lane_target_shifts = random.randint(1, 5)
            
            if self.ego_lane_shift_count < self.ego_lane_target_shifts:
                # Check if it's time for the next shift (every 20 iterations after the 100th)
                iterations_since_threshold = self.ego_lane_iterations - 100
                if iterations_since_threshold % 20 == 0 and self.timestep > self.ego_lane_last_shift_iteration:
                    # Perform ego lane shift
                    self._shift_lane_forward(self.ego_lane)
                    # Track shift
                    self.ego_lane_shift_count += 1
                    self.ego_lane_last_shift_iteration = self.timestep
                    # Start follow_safely to position between cars after shift
                    self._initiate_follow_safely()
    
    def _process_other_lane_shifts(self):
        """
        Process shifts for lanes with similar speeds to ego lane.
        Lanes within 2 speed units have probabilistic chance to shift.
        """
        ego_lane_speed = self.lane_speeds[self.ego_lane]
        for lane_idx in range(6):
            if lane_idx != self.ego_lane:
                speed_diff = abs(self.lane_speeds[lane_idx] - ego_lane_speed)
                
                # Only consider lanes within 2 speed units of ego lane
                if speed_diff <= 2:
                    # Calculate probability based on speed similarity
                    # speed_diff = 0: 1 in 10 chance (10%)
                    # speed_diff = 1: 1 in 20 chance (5%)
                    # speed_diff = 2: 1 in 40 chance (2.5%)
                    if speed_diff == 0:
                        threshold = 10
                    elif speed_diff == 1:
                        threshold = 20
                    else:  # speed_diff == 2
                        threshold = 40
                    
                    if random.randint(1, threshold) == 1:
                        # Shift this lane forward
                        self._shift_lane_forward(lane_idx)
    
    def _save_snapshot(self, reason="manual"):
        """Helper method to save a snapshot."""
        # Create snapshot folder if it doesn't exist
        if self.snapshot_folder is None:
            timestamp = datetime.now().strftime("%d%m%Y_%H%M%S")
            self.snapshot_folder = f"snapshots_{timestamp}"
            os.makedirs(self.snapshot_folder, exist_ok=True)
        
        snapshot = Snapshot(self, self.timestep)
        filepath = snapshot.save(self.snapshot_folder)
        
        # In data generation mode, print snapshot info with command details
        if self.data_generation_mode:
            cmd_info = ""
            if self.current_command:
                cmd_type = self.current_command['type']
                cmd_value = self.current_command.get('value')
                # Use "Last command" for collision snapshots, "Command" for others
                cmd_label = "Last command" if reason == "collision" else "Command"
                
                # Use the actual acceleration from the snapshot if available
                if snapshot.ego_acceleration is not None:
                    accel_sign = "+" if snapshot.ego_acceleration > 0 else ""
                    cmd_info = f" | {cmd_label}: {cmd_type}({accel_sign}{snapshot.ego_acceleration:.1f})"
                # Fall back to calculating from command value for acceleration commands
                elif cmd_type in ['accelerate_by_n', 'decelerate_by_n'] and cmd_value is not None:
                    per_iteration = cmd_value / 10
                    sign = "+" if cmd_type == 'accelerate_by_n' else "-"
                    cmd_info = f" | {cmd_label}: {cmd_type}({sign}{per_iteration:.1f})"
                elif cmd_type == 'emergency_stop':
                    cmd_info = f" | {cmd_label}: {cmd_type}(-1.0)"
                elif cmd_value is not None:
                    cmd_info = f" | {cmd_label}: {cmd_type}({cmd_value})"
                else:
                    cmd_info = f" | {cmd_label}: {cmd_type}"
            print(f"Snapshot saved: iteration {self.timestep:06d} - {reason}{cmd_info}")
        
        return filepath
    
    def update_view(self):
        """
        Update the road grid view based on current positions in all traffic grids.
        All lanes scroll based on their view_positions.
        Ego car is drawn as a fixed green car in center of display.
        """
        # Update all lanes from their traffic grids using view_positions
        self._update_road_grid_from_traffic()
        
        # Check for collision before drawing ego car
        ego_grid_position = int(self.view_positions[self.ego_lane]) + self.ego_display_col
        if self.traffic_grids[self.ego_lane].get_cell(ego_grid_position) == 1:
            self.collision = True
        
        # Draw ego car at fixed position in center of display (overwrite traffic)
        # Only draw if not off-road collision (collision_lane is None or within bounds)
        if self.collision_lane is None or (0 <= self.collision_lane < 6):
            self.road_grid.set_cell(self.ego_lane, self.ego_display_col, 2)
    
    def scroll_view(self):
        """
        Scroll lanes through their traffic grids relative to ego car.
        All lanes move relative to ego_car_speed.
        Ego lane scrolls based on difference between its traffic speed and ego car speed.
        Other lanes scroll at (their speed - ego car speed) to show relative motion.
        Trees scroll backwards at ego car speed to show forward movement.
        """
        # Increment timestep
        self.timestep += 1
        
        # Check for keyboard input
        self._handle_keyboard_input()
        
        # Get next command from queue if no active command
        if not self.active_command or self.active_command_timesteps_remaining == 0:
            self._process_new_command()
        
        # Decrement command display counter
        if self.command_display_counter > 0:
            self.command_display_counter -= 1
        
        # Decrement error message counter
        if self.error_message_counter > 0:
            self.error_message_counter -= 1
        
        # Execute active command
        self._execute_active_command()
        
        # Handle lane merging
        self._handle_lane_merging()
        
        # Update speeds based on actual visible car density in each lane
        self.lane_speeds = self._calculate_lane_speeds()
        
        # Track how long ego car has been in current lane
        self.ego_lane_iterations += 1
        
        # Process lane shifts only when no command is active
        if not self.active_command:
            self._process_ego_lane_shift()
            self._process_other_lane_shifts()
        
        # Update ego lane speed from traffic
        self.ego_lane_speed = self.lane_speeds[self.ego_lane]
        
        ego_car_actual_speed = self._get_scroll_speed(self.ego_car_speed)
        
        # Scroll trees forward (same direction as travel) at ego car speed
        self.road_grid.tree_view_position += ego_car_actual_speed
        # Loop tree array when reaching end
        if self.road_grid.tree_view_position >= self.tree_array.width:
            self.road_grid.tree_view_position = 0
        
        # Scroll ALL lanes relative to ego car speed
        for i in range(6):
            lane_actual_speed = self._get_scroll_speed(self.lane_speeds[i])
            
            # Relative speed = lane_speed - ego_car_speed
            # All lanes (including ego lane) move relative to ego car speed
            # If lane faster than ego car: negative (scrolls backward/left - they fall behind)
            # If lane slower than ego car: positive (scrolls forward/right - we overtake them)
            relative_speed = ego_car_actual_speed - lane_actual_speed
            self.view_positions[i] += relative_speed
        
        # Handle looping for all lanes (including ego lane now)
        for i in range(6):
            # Check if view position has exceeded grid boundaries
            if self.view_positions[i] >= self.traffic_grids[i].height:
                self.collision = True
                self.collision_lane = self.ego_lane
                self.collision_reason = f"Lane {i} view position exceeded grid end: {self.view_positions[i]} >= {self.traffic_grids[i].height}"
                return
            elif self.view_positions[i] < -self.road_grid.width:
                self.collision = True
                self.collision_lane = self.ego_lane
                self.collision_reason = f"Lane {i} view position exceeded grid start: {self.view_positions[i]} < {-self.road_grid.width}"
                return
            
            # Normal looping behavior
            if self.view_positions[i] >= self.traffic_grids[i].height - self.road_grid.width:
                self.view_positions[i] = 0
            elif self.view_positions[i] < 0:
                self.view_positions[i] = self.traffic_grids[i].height - self.road_grid.width - 1
    
    def _handle_keyboard_input(self):
        """Handle keyboard input for manual control."""
        if select.select([sys.stdin], [], [], 0)[0]:
            key = sys.stdin.read(1)
            
            # Handle arrow keys (escape sequences)
            if key == '\x1b':
                # Arrow keys send 3-character sequences: ESC [ A/B/C/D
                # Read the entire sequence at once
                next1 = sys.stdin.read(1)
                next2 = sys.stdin.read(1)
                
                if next1 == '[':
                    if next2 == 'A':  # Up arrow
                        self.autodrive.add_command('move_lane_left')
                    elif next2 == 'B':  # Down arrow
                        self.autodrive.add_command('move_lane_right')
                    elif next2 == 'C':  # Right arrow
                        if self.active_command and self.active_command['type'] == 'accelerate_by_n':
                            max_n = 0.2 if self.merging else 1.0
                            new_value = min(max_n, self.active_command['value'] + 0.1)
                            self.active_command['value'] = new_value
                        else:
                            self.autodrive.add_command('accelerate_by_n', 0.1)
                    elif next2 == 'D':  # Left arrow
                        if self.active_command and self.active_command['type'] == 'decelerate_by_n':
                            max_n = 0.2 if self.merging else 1.0
                            new_value = min(max_n, self.active_command['value'] + 0.1)
                            self.active_command['value'] = new_value
                        else:
                            self.autodrive.add_command('decelerate_by_n', 0.1)
            elif key == ' ':  # Spacebar
                self.autodrive.add_command('emergency_stop')
            elif key == 's':  # Save snapshot
                filepath = self._save_snapshot(reason="manual")
                self.error_message = f"Snapshot saved: {os.path.basename(filepath)}"
                self.error_message_counter = 30
            elif key == '[':
                self.autodrive.add_command('overtake_on_left')
            elif key == ']':  # Right bracket
                self.autodrive.add_command('overtake_on_right')
            elif key == 'f':  # Follow safely
                self.autodrive.add_command('follow_safely')
            elif key == 'q':  # Quit
                self.running = False
    
    def _setup_terminal(self):
        """Setup terminal for non-blocking input."""
        self.old_settings = termios.tcgetattr(sys.stdin)
        tty.setcbreak(sys.stdin.fileno())
    
    def _restore_terminal(self):
        """Restore terminal to original settings."""
        if self.old_settings:
            termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.old_settings)
    
    def run(self, duration=None, fps=10):
        """
        Run the simulation.
        
        Args:
            duration: How long to run in seconds (None = infinite)
            fps: Frames per second
        """
        # In data generation mode, skip terminal setup and display
        if not self.data_generation_mode:
            self._setup_terminal()
        
        self.running = True
        frame_delay = 1.0 / fps
        start_time = time.time()
        
        if self.data_generation_mode:
            print("EVO Drive - Data Generation Mode")
            print(f"Starting simulation with {len(self.autodrive.command_queue)} queued commands...\\n")
        else:
            print("EVO Drive - Traffic Simulation")
            print("Lane speeds vary based on traffic density.")
            print("Green car is your ego vehicle in top lane.")
            print("All movement is relative to ego car speed.")
            print("Controls: ← → speed, ↑ ↓ change lanes, spacebar emergency stop")
            print("          [ ] overtake left/right, 's' save snapshot, 'q' quit\\n")
            time.sleep(2)
        
        try:
            while self.running:
                # Clear screen only in interactive mode
                if not self.data_generation_mode:
                    print('\033[2J\033[3J\033[H', end='', flush=True)
                
                # Update view
                self.update_view()
                
                # Check for collision
                if self.collision:
                    # Save snapshot on collision
                    self._save_snapshot(reason="collision")
                    
                    if not self.data_generation_mode:
                        # Use collision_lane if set (for off-road collisions), otherwise use ego_lane
                        display_lane = self.collision_lane if self.collision_lane is not None else self.ego_lane
                        self.road_grid.display(collision=True, collision_lane=display_lane, collision_col=self.ego_display_col, lane_speeds=self.lane_speeds, ego_lane=self.ego_lane)
                    
                    self.running = False
                    break
                
                # Display only in interactive mode
                if not self.data_generation_mode:
                    self.road_grid.display(lane_speeds=self.lane_speeds, ego_lane=self.ego_lane)
                    
                    # Decrement lane shift highlight counters
                    lanes_to_remove = []
                    for lane_idx, counter in self.lane_shift_highlight.items():
                        self.lane_shift_highlight[lane_idx] -= 1
                        if self.lane_shift_highlight[lane_idx] <= 0:
                            lanes_to_remove.append(lane_idx)
                    for lane_idx in lanes_to_remove:
                        del self.lane_shift_highlight[lane_idx]
                    
                    # Show info
                    ego_position_in_grid = int(self.view_positions[self.ego_lane]) + self.ego_display_col
                    print(f"\033[94mEgo Car\033[0m - Lane: {self.ego_lane + 1}, Position: {ego_position_in_grid}, Car Speed: {self.ego_car_speed}")
                    print(f"Lane Positions: ", end="")
                    for i in range(6):
                        if i == self.ego_lane:
                            print(f"\033[94mL{i+1}:{int(self.view_positions[i])}\033[0m ", end="")
                        elif i in self.lane_shift_highlight:
                            print(f"\033[91mL{i+1}:{int(self.view_positions[i])}\033[0m ", end="")  # Red for lane shifting
                        else:
                            print(f"L{i+1}:{int(self.view_positions[i])} ", end="")
                    print(f"| FPS: {fps}")
                    
                    # Display current command if active
                    if self.command_display_counter > 0 and self.current_command:
                        cmd_type = self.current_command['type']
                        cmd_value = self.current_command['value']
                        if cmd_value is not None:
                            cmd_str = f"{cmd_type.replace('_', ' ').title()} ({cmd_value})"
                        else:
                            cmd_str = cmd_type.replace('_', ' ').title()
                        print(f"\n\033[30m>>> AutoDrive Command: {cmd_str} <<<\033[0m")
                    
                    # Display error message if active
                    if self.error_message_counter > 0 and self.error_message:
                        print(f"\n\033[91m>>> {self.error_message} <<<\033[0m")
                
                # Scroll
                self.scroll_view()
                
                # Check duration
                if duration and (time.time() - start_time) >= duration:
                    break
                
                # Frame rate control (skip in data generation mode for maximum speed)
                if not self.data_generation_mode:
                    time.sleep(frame_delay)
                
        except KeyboardInterrupt:
            if not self.data_generation_mode:
                print("\\n\\nSimulation stopped.")
            self.running = False
        finally:
            # Restore terminal settings only in interactive mode
            if not self.data_generation_mode:
                self._restore_terminal()
            
            # Print completion message in data generation mode
            if self.data_generation_mode:
                print(f"\nSimulation ended at iteration {self.timestep}")
                if self.collision:
                    print("Reason: Collision")
                print(f"Snapshots saved to folder: {self.snapshot_folder}")


def main():
    """Main entry point for the carsim."""
    # Check for data generation mode
    data_gen_mode = '--data-generate' in sys.argv or '-d' in sys.argv
    
    carsim = EvoDrive()
    
    if data_gen_mode:
        # Enable data generation mode
        carsim.data_generation_mode = True
        
        # Generate 20 random commands with 200 iteration pauses
        command_types = [
            ('accelerate_by_n', lambda: random.randint(5, 20)),
            ('decelerate_by_n', lambda: random.randint(5, 20)),
            ('move_lane_right', lambda: None),
            ('move_lane_left', lambda: None),
            ('overtake_on_right', lambda: None),
            ('overtake_on_left', lambda: None),
            ('emergency_stop', lambda: None),
            ('follow_safely', lambda: None)
        ]
        
        for i in range(20):
            iteration = (i + 1) * 200
            cmd_type, value_fn = random.choice(command_types)
            value = value_fn()
            carsim.autodrive.add_command(cmd_type, value, iteration)
        
        # Run without display, max speed
        carsim.run(fps=1000)  # High FPS but will run as fast as possible
    else:
        # Normal interactive mode
        carsim.run(fps=10)


if __name__ == "__main__":
    main()