main.py

"""
SYNAPSCAPE FSD Simulator - Main Orchestrator
=============================================

"Mapping the Neural Road" - Central orchestration module for the SYNAPSCAPE
Full Self-Driving Simulator. Manages all 13 autonomous agents, game loop,
training mode, and system coordination.

This module provides:
- SynapscapeOrchestrator: Central management class for all simulator components
- Game loop with delta time calculation and event handling
- Keyboard controls for manual and autonomous driving
- Training mode with PPO updates
- Performance monitoring and graceful shutdown

Author: SYNAPSCAPE Team
Version: 2.0.0
License: MIT
Python: >=3.11
PyTorch: >=2.0
PyGame: >=2.0
"""

import sys
import time
import argparse
import logging
from typing import Optional, Dict, Any, List, Tuple, Callable
from dataclasses import dataclass, field
from enum import Enum, auto
from pathlib import Path
import signal
import threading
from collections import deque

import numpy as np
import pygame
from pygame.locals import *

# Configure logging before other imports
logging.basicConfig(
    level=logging.INFO,
    format='%(asctime)s - %(name)s - %(levelname)s - %(message)s',
    handlers=[
        logging.StreamHandler(sys.stdout),
        logging.FileHandler('logs/synapscape.log', mode='a')
    ]
)
logger = logging.getLogger("SYNAPSCAPE")

# Import configuration
try:
    from config import (
        PROJECT_NAME, PROJECT_VERSION, PROJECT_TAGLINE, VERSION_INFO,
        SCREEN_WIDTH, SCREEN_HEIGHT, SCREEN_SIZE, TARGET_FPS, TARGET_DT,
        DisplayMode, DEFAULT_COLORS, DEFAULT_DEBUG, Paths, DEFAULT_RANDOM_SEED,
        KM_H_TO_M_S, M_S_TO_KM_H, DEG_TO_RAD, RAD_TO_DEG
    )
except ImportError as e:
    logger.error(f"Failed to import configuration: {e}")
    raise


# =============================================================================
# TYPE DEFINITIONS
# =============================================================================

class SimulationState(Enum):
    """Simulation runtime states."""
    INITIALIZING = auto()
    READY = auto()
    RUNNING = auto()
    PAUSED = auto()
    TRAINING = auto()
    SHUTTING_DOWN = auto()
    ERROR = auto()


class ControlMode(Enum):
    """Vehicle control modes."""
    MANUAL = auto()      # Human driver
    AUTONOMOUS = auto()  # AI driver
    TRAINING = auto()    # Training mode with PPO


class CameraView(Enum):
    """Available camera views."""
    DRIVER = auto()      # Driver's perspective
    CHASE = auto()       # Chase camera behind vehicle
    TOP_DOWN = auto()    # Top-down view
    FRONT = auto()       # Front bumper camera
    BIRD_EYE = auto()    # Bird's eye view
    SENSOR = auto()      # Sensor visualization view


@dataclass
class PerformanceMetrics:
    """Real-time performance metrics."""
    fps: float = 0.0
    frame_time_ms: float = 0.0
    physics_time_ms: float = 0.0
    render_time_ms: float = 0.0
    ai_time_ms: float = 0.0
    memory_mb: float = 0.0
    step_count: int = 0
    episode_count: int = 0
    total_steps: int = 0
    
    # History for averaging
    _fps_history: deque = field(default_factory=lambda: deque(maxlen=60))
    _frame_time_history: deque = field(default_factory=lambda: deque(maxlen=60))
    
    def update(self, fps: float, frame_time: float):
        """Update metrics with new values."""
        self._fps_history.append(fps)
        self._frame_time_history.append(frame_time)
        self.fps = np.mean(self._fps_history)
        self.frame_time_ms = np.mean(self._frame_time_history) * 1000
        self.step_count += 1
        self.total_steps += 1


@dataclass
class InputState:
    """Current input state."""
    throttle: float = 0.0  # -1.0 to 1.0
    steering: float = 0.0  # -1.0 to 1.0 (left to right)
    brake: float = 0.0     # 0.0 to 1.0
    handbrake: bool = False
    
    # Action flags
    reset_requested: bool = False
    camera_cycle: bool = False
    inspector_toggle: bool = False
    emergency_brake: bool = False
    training_toggle: bool = False
    pause_toggle: bool = False


# =============================================================================
# AGENT INTERFACES (Stubs for compilation)
# =============================================================================

class AgentInterface:
    """Base interface for all simulator agents."""
    
    def __init__(self, name: str, config: Optional[Dict[str, Any]] = None):
        self.name = name
        self.config = config or {}
        self.enabled = True
        self.initialized = False
        self.logger = logging.getLogger(f"SYNAPSCAPE.{name}")
    
    def initialize(self) -> bool:
        """Initialize the agent. Returns True on success."""
        self.initialized = True
        return True
    
    def update(self, dt: float, **kwargs) -> Dict[str, Any]:
        """Update agent state. Returns output data."""
        return {}
    
    def shutdown(self):
        """Clean shutdown of the agent."""
        self.initialized = False
    
    def reset(self):
        """Reset agent state."""
        pass


class WorldBuilderAgent(AgentInterface):
    """Agent responsible for world/scene construction."""
    def __init__(self, config: Optional[Dict[str, Any]] = None):
        super().__init__("WorldBuilder", config)
        self.map_data: Optional[Any] = None
        self.road_network: Optional[Any] = None
    
    def load_map(self, map_path: str) -> bool:
        """Load a map from file."""
        self.logger.info(f"Loading map: {map_path}")
        return True
    
    def spawn_vehicle(self, position: Tuple[float, float, float]) -> Any:
        """Spawn ego vehicle at position."""
        return None


class PhysicsEngineAgent(AgentInterface):
    """Agent responsible for physics simulation."""
    def __init__(self, config: Optional[Dict[str, Any]] = None):
        super().__init__("PhysicsEngine", config)
        self.vehicle_state: Optional[Dict[str, Any]] = None
        self.gravity: float = 9.81
    
    def step(self, dt: float, controls: InputState) -> Dict[str, Any]:
        """Execute physics step."""
        return {"position": (0.0, 0.0, 0.0), "velocity": (0.0, 0.0, 0.0)}
    
    def get_vehicle_state(self) -> Dict[str, Any]:
        """Get current vehicle state."""
        return self.vehicle_state or {}


class SensorFusionAgent(AgentInterface):
    """Agent responsible for sensor data fusion."""
    def __init__(self, config: Optional[Dict[str, Any]] = None):
        super().__init__("SensorFusion", config)
        self.camera_data: List[np.ndarray] = []
        self.lidar_data: Optional[np.ndarray] = None
        self.radar_data: Optional[np.ndarray] = None
        self.imu_data: Optional[Dict[str, float]] = None
    
    def get_fused_perception(self) -> Dict[str, Any]:
        """Get fused sensor perception output."""
        return {
            "camera": self.camera_data,
            "lidar": self.lidar_data,
            "radar": self.radar_data,
            "imu": self.imu_data
        }


class PerceptionAgent(AgentInterface):
    """Agent responsible for neural perception."""
    def __init__(self, config: Optional[Dict[str, Any]] = None):
        super().__init__("Perception", config)
        self.detected_objects: List[Dict[str, Any]] = []
        self.lane_detection: Optional[Dict[str, Any]] = None
        self.traffic_signs: List[Dict[str, Any]] = []
    
    def process(self, sensor_data: Dict[str, Any]) -> Dict[str, Any]:
        """Process sensor data through neural networks."""
        return {
            "objects": self.detected_objects,
            "lanes": self.lane_detection,
            "signs": self.traffic_signs
        }


class PredictionAgent(AgentInterface):
    """Agent responsible for trajectory prediction."""
    def __init__(self, config: Optional[Dict[str, Any]] = None):
        super().__init__("Prediction", config)
        self.predicted_trajectories: Dict[int, List[Tuple[float, float]]] = {}
        self.prediction_horizon: float = 3.0  # seconds
    
    def predict(self, detected_objects: List[Dict[str, Any]], 
                dt: float) -> Dict[int, List[Tuple[float, float]]]:
        """Predict future trajectories for detected objects."""
        return self.predicted_trajectories


class DecisionAgent(AgentInterface):
    """Agent responsible for high-level decision making."""
    def __init__(self, config: Optional[Dict[str, Any]] = None):
        super().__init__("Decision", config)
        self.current_action: str = "cruise"
        self.target_speed: float = 0.0
        self.target_lane: int = 0
    
    def decide(self, perception: Dict[str, Any], 
               predictions: Dict[int, Any]) -> Dict[str, Any]:
        """Make driving decisions based on perception and predictions."""
        return {
            "action": self.current_action,
            "target_speed": self.target_speed,
            "target_lane": self.target_lane
        }


class ADASAgent(AgentInterface):
    """Agent responsible for ADAS features."""
    def __init__(self, config: Optional[Dict[str, Any]] = None):
        super().__init__("ADAS", config)
        self.tacc_enabled: bool = False
        self.aeb_enabled: bool = True
        self.lka_enabled: bool = True
        self.ttc: float = float('inf')
    
    def update_adas(self, vehicle_state: Dict[str, Any], 
                    perception: Dict[str, Any]) -> Dict[str, Any]:
        """Update ADAS systems."""
        interventions = {}
        if self.aeb_enabled and self.ttc < 1.0:
            interventions["emergency_brake"] = True
        return interventions


class SafetyMonitorAgent(AgentInterface):
    """Agent responsible for safety monitoring."""
    def __init__(self, config: Optional[Dict[str, Any]] = None):
        super().__init__("SafetyMonitor", config)
        self.safety_score: float = 100.0
        self.violations: List[Dict[str, Any]] = []
        self.is_safe: bool = True
    
    def check_safety(self, state: Dict[str, Any], 
                     controls: InputState) -> Dict[str, Any]:
        """Check safety conditions."""
        return {
            "safe": self.is_safe,
            "score": self.safety_score,
            "violations": self.violations
        }


class RewardShapingAgent(AgentInterface):
    """Agent responsible for reward shaping in RL."""
    def __init__(self, config: Optional[Dict[str, Any]] = None):
        super().__init__("RewardShaping", config)
        self.current_reward: float = 0.0
        self.episode_reward: float = 0.0
        self.reward_history: deque = deque(maxlen=100)
    
    def calculate_reward(self, state: Dict[str, Any], 
                         action: InputState,
                         next_state: Dict[str, Any]) -> float:
        """Calculate reward for RL."""
        reward = 0.0
        self.current_reward = reward
        self.episode_reward += reward
        return reward
    
    def get_episode_reward(self) -> float:
        """Get total episode reward."""
        return self.episode_reward
    
    def reset_episode(self):
        """Reset episode reward tracking."""
        self.reward_history.append(self.episode_reward)
        self.episode_reward = 0.0


class LearningOrchestratorAgent(AgentInterface):
    """Agent responsible for RL training orchestration."""
    def __init__(self, config: Optional[Dict[str, Any]] = None):
        super().__init__("LearningOrchestrator", config)
        self.is_training: bool = False
        self.episode_count: int = 0
        self.step_count: int = 0
        self.model_path: str = Paths.CHECKPOINTS_DIR
    
    def start_training(self):
        """Start training mode."""
        self.is_training = True
        self.logger.info("Training mode activated")
    
    def stop_training(self):
        """Stop training mode."""
        self.is_training = False
        self.logger.info("Training mode deactivated")
    
    def update_policy(self, batch_data: Dict[str, Any]) -> Dict[str, float]:
        """Update policy with PPO."""
        return {"loss": 0.0, "value_loss": 0.0, "entropy": 0.0}
    
    def save_model(self, path: str) -> bool:
        """Save model checkpoint."""
        self.logger.info(f"Saving model to {path}")
        return True
    
    def load_model(self, path: str) -> bool:
        """Load model checkpoint."""
        self.logger.info(f"Loading model from {path}")
        return True


class VisualizerAgent(AgentInterface):
    """Agent responsible for visualization and rendering."""
    def __init__(self, config: Optional[Dict[str, Any]] = None):
        super().__init__("Visualizer", config)
        self.screen: Optional[pygame.Surface] = None
        self.camera_view: CameraView = CameraView.DRIVER
        self.show_hud: bool = True
        self.show_sensor_overlay: bool = False
    
    def initialize_display(self, width: int, height: int) -> bool:
        """Initialize display surface."""
        pygame.display.init()
        self.screen = pygame.display.set_mode(
            (width, height),
            pygame.DOUBLEBUF | pygame.HWSURFACE
        )
        pygame.display.set_caption(VERSION_INFO)
        return True
    
    def render(self, world_state: Dict[str, Any], 
               perception: Dict[str, Any],
               metrics: PerformanceMetrics) -> pygame.Surface:
        """Render the scene."""
        if self.screen:
            self.screen.fill(DEFAULT_COLORS.background_dark)
        return self.screen
    
    def cycle_camera(self):
        """Cycle through camera views."""
        views = list(CameraView)
        current_idx = views.index(self.camera_view)
        self.camera_view = views[(current_idx + 1) % len(views)]
        self.logger.info(f"Camera view: {self.camera_view.name}")


class InspectorConsoleAgent(AgentInterface):
    """Agent responsible for debug inspector console."""
    def __init__(self, config: Optional[Dict[str, Any]] = None):
        super().__init__("InspectorConsole", config)
        self.visible: bool = False
        self.width: int = 400
        self.log_messages: deque = deque(maxlen=100)
        self.metrics_display: Dict[str, Any] = {}
    
    def toggle(self):
        """Toggle inspector visibility."""
        self.visible = not self.visible
    
    def log(self, message: str, level: str = "INFO"):
        """Log message to inspector."""
        self.log_messages.append({"time": time.time(), "level": level, "msg": message})
    
    def render(self, screen: pygame.Surface, 
               metrics: PerformanceMetrics,
               agents: Dict[str, AgentInterface]) -> pygame.Surface:
        """Render inspector overlay."""
        if not self.visible:
            return screen
        # Render inspector panel
        return screen


# =============================================================================
# MAIN ORCHESTRATOR
# =============================================================================

class SynapscapeOrchestrator:
    """
    Central orchestrator for the SYNAPSCAPE FSD Simulator.
    
    Manages all 13 autonomous agents, coordinates the game loop,
    handles input, training mode, and system lifecycle.
    
    Attributes:
        state: Current simulation state
        control_mode: Current vehicle control mode
        agents: Dictionary of all managed agents
        metrics: Performance metrics tracking
        running: Flag indicating if main loop is running
    """
    
    def __init__(self, config: Optional[Dict[str, Any]] = None):
        """
        Initialize the orchestrator.
        
        Args:
            config: Optional configuration dictionary
        """
        self.config = config or {}
        self.state = SimulationState.INITIALIZING
        self.control_mode = ControlMode.MANUAL
        self.camera_view = CameraView.DRIVER
        
        # Performance tracking
        self.metrics = PerformanceMetrics()
        self.input_state = InputState()
        
        # Timing
        self.clock = pygame.time.Clock()
        self.last_frame_time = time.time()
        self.delta_time = TARGET_DT
        self.time_scale = 1.0
        
        # Agent registry
        self.agents: Dict[str, AgentInterface] = {}
        
        # Shutdown flag
        self._shutdown_requested = False
        self._shutdown_complete = False
        
        # Training state
        self.training_enabled = False
        self.episode_in_progress = False
        self.episode_start_time = 0.0
        
        # Setup signal handlers
        self._setup_signal_handlers()
        
        logger.info(f"{PROJECT_NAME} Orchestrator initialized")
    
    def _setup_signal_handlers(self):
        """Setup OS signal handlers for graceful shutdown."""
        def signal_handler(signum, frame):
            logger.info(f"Received signal {signum}, initiating shutdown...")
            self.request_shutdown()
        
        signal.signal(signal.SIGINT, signal_handler)
        signal.signal(signal.SIGTERM, signal_handler)
        
        # Windows doesn't have SIGQUIT
        if hasattr(signal, 'SIGQUIT'):
            signal.signal(signal.SIGQUIT, signal_handler)
    
    # =====================================================================
    # AGENT INITIALIZATION
    # =====================================================================
    
    def initialize_agents(self) -> bool:
        """
        Initialize all 13 agents in the correct sequence.
        
        Initialization order:
        1. WorldBuilder - Scene construction
        2. PhysicsEngine - Physics simulation
        3. SensorFusion - Sensor data integration
        4. Perception - Neural perception
        5. Prediction - Trajectory prediction
        6. Decision - High-level decisions
        7. ADAS - Advanced driver assistance
        8. SafetyMonitor - Safety monitoring
        9. RewardShaping - RL reward calculation
        10. LearningOrchestrator - Training management
        11. Visualizer - Rendering
        12. InspectorConsole - Debug console
        
        Returns:
            True if all agents initialized successfully
        """
        logger.info("Initializing agents...")
        
        try:
            # 1. WorldBuilder - Scene construction
            self.agents["world_builder"] = WorldBuilderAgent(
                self.config.get("world_builder")
            )
            
            # 2. PhysicsEngine - Physics simulation
            self.agents["physics_engine"] = PhysicsEngineAgent(
                self.config.get("physics_engine")
            )
            
            # 3. SensorFusion - Sensor data integration
            self.agents["sensor_fusion"] = SensorFusionAgent(
                self.config.get("sensor_fusion")
            )
            
            # 4. Perception - Neural perception
            self.agents["perception"] = PerceptionAgent(
                self.config.get("perception")
            )
            
            # 5. Prediction - Trajectory prediction
            self.agents["prediction"] = PredictionAgent(
                self.config.get("prediction")
            )
            
            # 6. Decision - High-level decisions
            self.agents["decision"] = DecisionAgent(
                self.config.get("decision")
            )
            
            # 7. ADAS - Advanced driver assistance
            self.agents["adas"] = ADASAgent(
                self.config.get("adas")
            )
            
            # 8. SafetyMonitor - Safety monitoring
            self.agents["safety_monitor"] = SafetyMonitorAgent(
                self.config.get("safety_monitor")
            )
            
            # 9. RewardShaping - RL reward calculation
            self.agents["reward_shaping"] = RewardShapingAgent(
                self.config.get("reward_shaping")
            )
            
            # 10. LearningOrchestrator - Training management
            self.agents["learning_orchestrator"] = LearningOrchestratorAgent(
                self.config.get("learning_orchestrator")
            )
            
            # 11. Visualizer - Rendering
            self.agents["visualizer"] = VisualizerAgent(
                self.config.get("visualizer")
            )
            
            # 12. InspectorConsole - Debug console
            self.agents["inspector_console"] = InspectorConsoleAgent(
                self.config.get("inspector_console")
            )
            
            # Initialize each agent in sequence
            for name, agent in self.agents.items():
                logger.info(f"Initializing {name}...")
                if not agent.initialize():
                    logger.error(f"Failed to initialize {name}")
                    return False
                logger.info(f"{name} initialized successfully")
            
            # Initialize display
            visualizer = self.agents.get("visualizer")
            if visualizer:
                visualizer.initialize_display(SCREEN_WIDTH, SCREEN_HEIGHT)
            
            self.state = SimulationState.READY
            logger.info("All agents initialized successfully")
            return True
            
        except Exception as e:
            logger.error(f"Agent initialization failed: {e}")
            self.state = SimulationState.ERROR
            return False
    
    # =====================================================================
    # GAME LOOP
    # =====================================================================
    
    def run(self):
        """Main game loop."""
        if self.state != SimulationState.READY:
            logger.error("Orchestrator not ready. Call initialize_agents() first.")
            return
        
        logger.info("Starting main loop...")
        self.state = SimulationState.RUNNING
        self.last_frame_time = time.time()
        
        try:
            while not self._shutdown_requested:
                # Calculate delta time
                current_time = time.time()
                self.delta_time = min(current_time - self.last_frame_time, 0.1)  # Cap at 100ms
                self.last_frame_time = current_time
                
                # Handle events
                self._handle_events()
                
                # Update simulation
                if self.state == SimulationState.RUNNING:
                    self._update(self.delta_time)
                
                # Render
                self._render()
                
                # Update metrics
                fps = self.clock.get_fps()
                self.metrics.update(fps, self.delta_time)
                
                # Cap frame rate
                self.clock.tick(TARGET_FPS)
                
        except Exception as e:
            logger.error(f"Main loop error: {e}")
            self.state = SimulationState.ERROR
            raise
        finally:
            self.shutdown()
    
    def _handle_events(self):
        """Process pygame events and keyboard input."""
        # Reset per-frame input flags
        self.input_state.reset_requested = False
        self.input_state.camera_cycle = False
        self.input_state.inspector_toggle = False
        self.input_state.emergency_brake = False
        self.input_state.training_toggle = False
        self.input_state.pause_toggle = False
        
        for event in pygame.event.get():
            if event.type == pygame.QUIT:
                self.request_shutdown()
                return
            
            elif event.type == pygame.KEYDOWN:
                # Training mode toggle (T)
                if event.key == pygame.K_t:
                    self.input_state.training_toggle = True
                    self._toggle_training_mode()
                
                # Reset (R)
                elif event.key == pygame.K_r:
                    self.input_state.reset_requested = True
                    self._reset_simulation()
                
                # Camera cycle (C)
                elif event.key == pygame.K_c:
                    self.input_state.camera_cycle = True
                    self._cycle_camera()
                
                # Inspector toggle (I)
                elif event.key == pygame.K_i:
                    self.input_state.inspector_toggle = True
                    self._toggle_inspector()
                
                # Emergency brake (SPACE)
                elif event.key == pygame.K_SPACE:
                    self.input_state.emergency_brake = True
                    self._emergency_brake()
                
                # Pause (P)
                elif event.key == pygame.K_p:
                    self.input_state.pause_toggle = True
                    self._toggle_pause()
                
                # Escape to quit
                elif event.key == pygame.K_ESCAPE:
                    self.request_shutdown()
                    return
        
        # Get continuous key states for driving controls
        keys = pygame.key.get_pressed()
        
        # WASD / Arrow key controls
        self.input_state.throttle = 0.0
        self.input_state.brake = 0.0
        self.input_state.steering = 0.0
        
        # Throttle (W or UP)
        if keys[pygame.K_w] or keys[pygame.K_UP]:
            self.input_state.throttle = 1.0
        
        # Brake (S or DOWN)
        if keys[pygame.K_s] or keys[pygame.K_DOWN]:
            self.input_state.brake = 1.0
        
        # Steering (A/D or LEFT/RIGHT)
        if keys[pygame.K_a] or keys[pygame.K_LEFT]:
            self.input_state.steering = -1.0
        elif keys[pygame.K_d] or keys[pygame.K_RIGHT]:
            self.input_state.steering = 1.0
        
        # Handbrake (LSHIFT)
        self.input_state.handbrake = keys[pygame.K_LSHIFT]
    
    def _update(self, dt: float):
        """
        Update simulation state.
        
        Args:
            dt: Delta time in seconds
        """
        update_start = time.time()
        
        # Apply time scale
        scaled_dt = dt * self.time_scale
        
        # Get agent references for efficiency
        physics = self.agents.get("physics_engine")
        sensor_fusion = self.agents.get("sensor_fusion")
        perception = self.agents.get("perception")
        prediction = self.agents.get("prediction")
        decision = self.agents.get("decision")
        adas = self.agents.get("adas")
        safety = self.agents.get("safety_monitor")
        reward_shaping = self.agents.get("reward_shaping")
        learning = self.agents.get("learning_orchestrator")
        
        # 1. Update physics
        physics_start = time.time()
        if physics:
            vehicle_state = physics.step(scaled_dt, self.input_state)
        else:
            vehicle_state = {}
        self.metrics.physics_time_ms = (time.time() - physics_start) * 1000
        
        # 2. Update sensors
        if sensor_fusion:
            sensor_data = sensor_fusion.get_fused_perception()
        else:
            sensor_data = {}
        
        # 3. Run perception
        ai_start = time.time()
        if perception:
            perception_output = perception.process(sensor_data)
        else:
            perception_output = {}
        
        # 4. Run prediction
        if prediction:
            predicted_trajectories = prediction.predict(
                perception_output.get("objects", []), 
                scaled_dt
            )
        else:
            predicted_trajectories = {}
        
        # 5. Make decisions
        if decision:
            decision_output = decision.decide(perception_output, predicted_trajectories)
        else:
            decision_output = {}
        
        # 6. Update ADAS
        if adas:
            adas_interventions = adas.update_adas(vehicle_state, perception_output)
            # Apply ADAS interventions
            if adas_interventions.get("emergency_brake"):
                self.input_state.brake = 1.0
        
        # 7. Check safety
        if safety:
            safety_status = safety.check_safety(vehicle_state, self.input_state)
        
        # 8. Calculate rewards (if training)
        if self.training_enabled and reward_shaping:
            reward = reward_shaping.calculate_reward(
                vehicle_state, 
                self.input_state,
                vehicle_state  # Next state (same for now)
            )
        
        # 9. Update learning (if training)
        if self.training_enabled and learning and learning.is_training:
            if self.metrics.step_count % 2048 == 0:
                learning.update_policy({})
        
        self.metrics.ai_time_ms = (time.time() - ai_start) * 1000
        
        # Update all agents
        for agent in self.agents.values():
            if agent.enabled and agent.initialized:
                agent.update(scaled_dt)
    
    def _render(self):
        """Render the simulation."""
        render_start = time.time()
        
        visualizer = self.agents.get("visualizer")
        inspector = self.agents.get("inspector_console")
        
        if visualizer:
            # Get data for rendering
            perception = self.agents.get("perception")
            perception_data = perception.process({}) if perception else {}
            
            # Render main view
            screen = visualizer.render(
                {},  # world_state
                perception_data,
                self.metrics
            )
            
            # Render inspector if visible
            if inspector and inspector.visible:
                inspector.render(screen, self.metrics, self.agents)
            
            # Update display
            pygame.display.flip()
        
        self.metrics.render_time_ms = (time.time() - render_start) * 1000
    
    # =====================================================================
    # CONTROL ACTIONS
    # =====================================================================
    
    def _toggle_training_mode(self):
        """Toggle training mode on/off."""
        learning = self.agents.get("learning_orchestrator")
        
        if not self.training_enabled:
            self.training_enabled = True
            self.control_mode = ControlMode.TRAINING
            self.state = SimulationState.TRAINING
            if learning:
                learning.start_training()
            logger.info("Training mode ENABLED")
        else:
            self.training_enabled = False
            self.control_mode = ControlMode.AUTONOMOUS
            self.state = SimulationState.RUNNING
            if learning:
                learning.stop_training()
            logger.info("Training mode DISABLED")
    
    def _reset_simulation(self):
        """Reset the simulation state."""
        logger.info("Resetting simulation...")
        
        # Reset all agents
        for agent in self.agents.values():
            agent.reset()
        
        # Reset metrics
        reward_shaping = self.agents.get("reward_shaping")
        if reward_shaping:
            reward_shaping.reset_episode()
        
        # Increment episode count
        self.metrics.episode_count += 1
        self.episode_start_time = time.time()
        
        logger.info("Simulation reset complete")
    
    def _cycle_camera(self):
        """Cycle through camera views."""
        visualizer = self.agents.get("visualizer")
        if visualizer:
            visualizer.cycle_camera()
    
    def _toggle_inspector(self):
        """Toggle inspector console visibility."""
        inspector = self.agents.get("inspector_console")
        if inspector:
            inspector.toggle()
            logger.info(f"Inspector {'visible' if inspector.visible else 'hidden'}")
    
    def _emergency_brake(self):
        """Trigger emergency brake."""
        logger.warning("EMERGENCY BRAKE ACTIVATED")
        self.input_state.brake = 1.0
        self.input_state.throttle = 0.0
    
    def _toggle_pause(self):
        """Toggle simulation pause."""
        if self.state == SimulationState.RUNNING:
            self.state = SimulationState.PAUSED
            logger.info("Simulation PAUSED")
        elif self.state == SimulationState.PAUSED:
            self.state = SimulationState.RUNNING
            logger.info("Simulation RESUMED")
    
    # =====================================================================
    # SHUTDOWN
    # =====================================================================
    
    def request_shutdown(self):
        """Request graceful shutdown."""
        logger.info("Shutdown requested")
        self._shutdown_requested = True
        self.state = SimulationState.SHUTTING_DOWN
    
    def shutdown(self):
        """Perform graceful shutdown."""
        if self._shutdown_complete:
            return
        
        logger.info("Shutting down...")
        self.state = SimulationState.SHUTTING_DOWN
        
        # Save models if training
        if self.training_enabled:
            learning = self.agents.get("learning_orchestrator")
            if learning:
                timestamp = time.strftime("%Y%m%d_%H%M%S")
                save_path = f"{Paths.CHECKPOINTS_DIR}/model_{timestamp}.pth"
                learning.save_model(save_path)
        
        # Shutdown all agents in reverse order
        agent_order = [
            "inspector_console",
            "visualizer",
            "learning_orchestrator",
            "reward_shaping",
            "safety_monitor",
            "adas",
            "decision",
            "prediction",
            "perception",
            "sensor_fusion",
            "physics_engine",
            "world_builder",
        ]
        
        for name in agent_order:
            agent = self.agents.get(name)
            if agent:
                logger.info(f"Shutting down {name}...")
                try:
                    agent.shutdown()
                except Exception as e:
                    logger.error(f"Error shutting down {name}: {e}")
        
        # Quit pygame
        pygame.quit()
        
        self._shutdown_complete = True
        logger.info("Shutdown complete")
    
    # =====================================================================
    # UTILITY METHODS
    # =====================================================================
    
    def get_agent(self, name: str) -> Optional[AgentInterface]:
        """
        Get an agent by name.
        
        Args:
            name: Agent name
            
        Returns:
            Agent instance or None
        """
        return self.agents.get(name)
    
    def get_all_agents(self) -> Dict[str, AgentInterface]:
        """
        Get all registered agents.
        
        Returns:
            Dictionary of agent name to agent instance
        """
        return self.agents.copy()
    
    def set_time_scale(self, scale: float):
        """
        Set simulation time scale.
        
        Args:
            scale: Time multiplier (1.0 = normal, 0.5 = half speed, etc.)
        """
        self.time_scale = max(0.1, min(scale, 10.0))
        logger.info(f"Time scale set to {self.time_scale}x")
    
    def get_performance_report(self) -> Dict[str, Any]:
        """
        Get performance metrics report.
        
        Returns:
            Dictionary of performance metrics
        """
        return {
            "fps": self.metrics.fps,
            "frame_time_ms": self.metrics.frame_time_ms,
            "physics_time_ms": self.metrics.physics_time_ms,
            "render_time_ms": self.metrics.render_time_ms,
            "ai_time_ms": self.metrics.ai_time_ms,
            "total_steps": self.metrics.total_steps,
            "episodes": self.metrics.episode_count,
            "state": self.state.name,
            "control_mode": self.control_mode.name,
        }


# =============================================================================
# COMMAND LINE INTERFACE
# =============================================================================

def create_argument_parser() -> argparse.ArgumentParser:
    """Create command line argument parser."""
    parser = argparse.ArgumentParser(
        prog="synapscape",
        description=f"{PROJECT_NAME} FSD Simulator - {PROJECT_TAGLINE}",
        formatter_class=argparse.RawDescriptionHelpFormatter,
        epilog="""
Controls:
  WASD / Arrow Keys  : Drive vehicle
  SPACE              : Emergency brake
  T                  : Toggle training mode
  R                  : Reset simulation
  C                  : Cycle camera view
  I                  : Toggle inspector console
  P                  : Pause simulation
  ESC                : Quit

Examples:
  python main.py                          # Run with default settings
  python main.py --training               # Start in training mode
  python main.py --map city_map.json      # Load specific map
  python main.py --headless               # Run without visualization
        """
    )
    
    # Display options
    display_group = parser.add_argument_group("Display Options")
    display_group.add_argument(
        "--fullscreen", "-f",
        action="store_true",
        help="Run in fullscreen mode"
    )
    display_group.add_argument(
        "--width", "-w",
        type=int,
        default=SCREEN_WIDTH,
        help=f"Screen width (default: {SCREEN_WIDTH})"
    )
    display_group.add_argument(
        "--height", "-h",
        type=int,
        default=SCREEN_HEIGHT,
        help=f"Screen height (default: {SCREEN_HEIGHT})"
    )
    display_group.add_argument(
        "--fps",
        type=int,
        default=TARGET_FPS,
        help=f"Target FPS (default: {TARGET_FPS})"
    )
    display_group.add_argument(
        "--headless",
        action="store_true",
        help="Run without visualization (for training)"
    )
    
    # Simulation options
    sim_group = parser.add_argument_group("Simulation Options")
    sim_group.add_argument(
        "--map", "-m",
        type=str,
        default=None,
        help="Path to map file to load"
    )
    sim_group.add_argument(
        "--time-scale",
        type=float,
        default=1.0,
        help="Simulation time scale multiplier (default: 1.0)"
    )
    sim_group.add_argument(
        "--seed",
        type=int,
        default=DEFAULT_RANDOM_SEED,
        help=f"Random seed (default: {DEFAULT_RANDOM_SEED})"
    )
    
    # Training options
    train_group = parser.add_argument_group("Training Options")
    train_group.add_argument(
        "--training", "-t",
        action="store_true",
        help="Start in training mode"
    )
    train_group.add_argument(
        "--model",
        type=str,
        default=None,
        help="Path to model checkpoint to load"
    )
    train_group.add_argument(
        "--save-interval",
        type=int,
        default=1000,
        help="Episode interval for saving checkpoints (default: 1000)"
    )
    
    # Debug options
    debug_group = parser.add_argument_group("Debug Options")
    debug_group.add_argument(
        "--debug",
        action="store_true",
        help="Enable debug mode"
    )
    debug_group.add_argument(
        "--verbose", "-v",
        action="store_true",
        help="Enable verbose logging"
    )
    debug_group.add_argument(
        "--profile",
        action="store_true",
        help="Enable performance profiling"
    )
    
    # Version
    parser.add_argument(
        "--version", "-V",
        action="version",
        version=f"%(prog)s {PROJECT_VERSION}"
    )
    
    return parser


def main():
    """Main entry point."""
    # Parse arguments
    parser = create_argument_parser()
    args = parser.parse_args()
    
    # Setup logging level
    if args.verbose:
        logging.getLogger().setLevel(logging.DEBUG)
        logger.debug("Verbose logging enabled")
    
    # Create log directory
    Path("logs").mkdir(exist_ok=True)
    Path("checkpoints").mkdir(exist_ok=True)
    
    # Log startup info
    logger.info("=" * 60)
    logger.info(VERSION_INFO)
    logger.info("=" * 60)
    logger.info(f"Python: {sys.version}")
    logger.info(f"PyGame: {pygame.version.ver}")
    logger.info(f"NumPy: {np.__version__}")
    
    # Initialize pygame
    pygame.init()
    pygame.display.set_caption(VERSION_INFO)
    
    # Create orchestrator configuration
    config = {
        "display": {
            "fullscreen": args.fullscreen,
            "width": args.width,
            "height": args.height,
            "fps": args.fps,
            "headless": args.headless,
        },
        "simulation": {
            "map_path": args.map,
            "time_scale": args.time_scale,
            "seed": args.seed,
        },
        "training": {
            "enabled": args.training,
            "model_path": args.model,
            "save_interval": args.save_interval,
        },
        "debug": {
            "enabled": args.debug,
            "profile": args.profile,
        }
    }
    
    # Create and run orchestrator
    orchestrator = SynapscapeOrchestrator(config)
    
    # Initialize agents
    if not orchestrator.initialize_agents():
        logger.error("Failed to initialize agents. Exiting.")
        sys.exit(1)
    
    # Load model if specified
    if args.model:
        learning = orchestrator.get_agent("learning_orchestrator")
        if learning:
            learning.load_model(args.model)
    
    # Enable training mode if requested
    if args.training:
        orchestrator._toggle_training_mode()
    
    # Set time scale
    orchestrator.set_time_scale(args.time_scale)
    
    # Run main loop
    try:
        orchestrator.run()
    except KeyboardInterrupt:
        logger.info("Interrupted by user")
    except Exception as e:
        logger.exception("Fatal error in main loop")
        sys.exit(1)
    finally:
        orchestrator.shutdown()
    
    logger.info("Exiting normally")
    sys.exit(0)


# =============================================================================
# EXPORTS
# =============================================================================

__all__ = [
    # Main class
    "SynapscapeOrchestrator",
    
    # Enums
    "SimulationState",
    "ControlMode",
    "CameraView",
    
    # Data classes
    "PerformanceMetrics",
    "InputState",
    
    # Agent interfaces
    "AgentInterface",
    "WorldBuilderAgent",
    "PhysicsEngineAgent",
    "SensorFusionAgent",
    "PerceptionAgent",
    "PredictionAgent",
    "DecisionAgent",
    "ADASAgent",
    "SafetyMonitorAgent",
    "RewardShapingAgent",
    "LearningOrchestratorAgent",
    "VisualizerAgent",
    "InspectorConsoleAgent",
    
    # Functions
    "create_argument_parser",
    "main",
]


if __name__ == "__main__":
    main()