/// Simulation Bridge Node
///
/// Translates between Redis pub/sub (framework) and Godot RL Agents protocol (simulation).
use mecha10_core::prelude::*;
use mecha10_core::topics::Topic;
use mecha10_diagnostics::prelude::{GodotCollector, StreamingCollector};
use std::sync::atomic::{AtomicU64, Ordering};
use std::sync::Arc;
use tokio::sync::Mutex;
use tokio::time::Duration;

use crate::config::{CameraConfig, SignalingMode, SimulationBridgeConfig};
use crate::godot_client::GodotClient;
use crate::topics::{RedisMessage, Twist, *};
use mecha10_video::publisher::CameraPublisher;
use mecha10_video::signaling::start_signaling_server;
use mecha10_video::signaling_relay::{start_signaling_relay_client, SignalingRelayConfig};
use mecha10_video::webrtc::WebRTCServer;
use tokio::sync::mpsc;

/// Sensor data from Godot (camera or IMU)
enum SensorData {
    Camera((String, Vec<u8>, crate::godot_client::ImageFormat, u32, u32, u64)),
    Imu(crate::topics::ImuData),
}

/// Simulation Bridge Node
pub struct SimulationBridge {
    config: SimulationBridgeConfig,
    godot: Arc<Mutex<Option<GodotClient>>>,
    godot_camera: Arc<Mutex<Option<GodotClient>>>,
    message_count: u64,
    camera_frames_received: Arc<AtomicU64>,
    /// Camera publisher for dual-path streaming (WebRTC + Redis)
    camera_publisher: Option<CameraPublisher>,
    /// Receiver for Redis publisher task (taken in run())
    #[allow(clippy::type_complexity)]
    redis_publisher_rx: Option<mpsc::Receiver<(String, Arc<Vec<u8>>, crate::godot_client::ImageFormat, u32, u32, u64)>>,
    /// Diagnostics: Streaming metrics collector
    streaming_diagnostics: Arc<StreamingCollector>,
    /// Diagnostics: Godot connection health collector
    godot_diagnostics: Arc<GodotCollector>,
    /// Action throttling: timestamp of last action sent to Godot
    last_action_time: Arc<Mutex<std::time::Instant>>,
    /// Action throttling: minimum interval between actions (prevents overwhelming Godot)
    min_action_interval: Duration,
    /// Counter for dropped actions (throttled)
    actions_dropped: Arc<AtomicU64>,
}

impl std::fmt::Debug for SimulationBridge {
    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
        f.debug_struct("SimulationBridge")
            .field("config", &self.config)
            .field("message_count", &self.message_count)
            .finish()
    }
}

#[async_trait]
impl NodeImpl for SimulationBridge {
    type Config = SimulationBridgeConfig;

    async fn init(config: Self::Config) -> Result<Self> {
        info!("🎮 Initializing Simulation Bridge Node");
        info!("Godot control URL: {}", config.godot_url);
        info!("Godot camera URL: {}", config.camera_url);
        info!("Signaling mode: {:?}", config.signaling_mode);
        if config.signaling_mode == SignalingMode::Server {
            info!("WebRTC signaling port: {}", config.signaling_port);
        } else if let Some(relay) = &config.signaling_relay {
            info!("WebRTC signaling relay URL: {}", relay.url);
        }
        info!("Sensor publish rate: {} Hz", config.publish_rate_hz);
        info!(
            "📷 Camera config from JSON: {}x{} @ {} FPS",
            config.camera.width, config.camera.height, config.camera.fps
        );
        info!("📷 IMPORTANT: This config will be sent to Godot to configure streaming");

        // Validate camera configuration
        config.camera.validate()?;

        let godot = Arc::new(Mutex::new(None));
        let godot_camera = Arc::new(Mutex::new(None));

        // Initialize diagnostics collectors (early, before connection loops)
        let streaming_diagnostics = Arc::new(StreamingCollector::new("simulation-bridge"));
        let godot_diagnostics = Arc::new(GodotCollector::new(
            "simulation-bridge",
            config.godot_url.clone(),
            config.camera_url.clone(),
        ));

        // Start Godot control connection in background
        let godot_clone = Arc::clone(&godot);
        let godot_url = config.godot_url.clone();
        let godot_diag = Arc::clone(&godot_diagnostics);
        let camera_config = config.camera.clone();
        tokio::spawn(async move {
            Self::godot_connection_loop(godot_clone, godot_url, camera_config, godot_diag).await;
        });

        // Start Godot camera connection in background
        let godot_camera_clone = Arc::clone(&godot_camera);
        let camera_url = config.camera_url.clone();
        let godot_diag_cam = Arc::clone(&godot_diagnostics);
        tokio::spawn(async move {
            Self::godot_camera_connection_loop(godot_camera_clone, camera_url, godot_diag_cam).await;
        });

        // Initialize WebRTC server for low-latency streaming
        // Small buffer (3 frames) to handle timing jitter while maintaining low latency
        // At 10 FPS, 3 frames = 300ms max buffering (acceptable latency)
        let (webrtc_frame_tx, webrtc_frame_rx) = mpsc::channel(3);

        // Background Redis publisher (off hot path)
        let (redis_publisher_tx, redis_publisher_rx) =
            mpsc::channel::<(String, Arc<Vec<u8>>, crate::godot_client::ImageFormat, u32, u32, u64)>(50);

        let camera_publisher = match WebRTCServer::new(webrtc_frame_rx, Arc::clone(&streaming_diagnostics)).await {
            Ok(server) => {
                let server = Arc::new(server);
                info!("✅ WebRTC server initialized for low-latency camera streaming");

                // Note: Streaming loops are now created per-connection in the signaling server

                // Start WebRTC signaling based on mode
                match config.signaling_mode {
                    SignalingMode::Server => {
                        // Server mode: run local signaling server
                        let signaling_port = config.signaling_port;
                        info!("🌐 Starting WebRTC signaling server on port {}", signaling_port);
                        tokio::spawn(async move {
                            if let Err(e) = start_signaling_server(signaling_port, server).await {
                                error!("❌ WebRTC signaling server error: {}", e);
                            }
                        });
                    }
                    SignalingMode::Client => {
                        // Client mode: connect to remote relay service
                        if let Some(relay_config) = &config.signaling_relay {
                            let relay_config = SignalingRelayConfig {
                                url: relay_config.url.clone(),
                                robot_id: relay_config.robot_id.clone(),
                                api_key: relay_config.api_key.clone(),
                                reconnect_interval_ms: relay_config.reconnect_interval_ms,
                            };
                            info!("🌐 Starting WebRTC signaling relay client");
                            tokio::spawn(async move {
                                if let Err(e) = start_signaling_relay_client(relay_config, server).await {
                                    error!("❌ WebRTC signaling relay client error: {}", e);
                                }
                            });
                        } else {
                            error!("❌ Client mode requires signaling_relay configuration");
                        }
                    }
                }

                // Create camera publisher for dual-path streaming (WebRTC + Redis)
                let camera_publisher = CameraPublisher::new(webrtc_frame_tx, Arc::clone(&streaming_diagnostics))
                    .with_secondary_publisher(redis_publisher_tx);

                Some(camera_publisher)
            }
            Err(e) => {
                warn!(
                    "⚠️  Failed to initialize WebRTC server: {}. Continuing without WebRTC.",
                    e
                );
                None
            }
        };

        info!("📊 Diagnostics collectors initialized");

        // Action throttling: max 15 actions per second to Godot (66ms between actions)
        // This prevents overwhelming Godot when the dashboard sends commands too fast
        let min_action_interval = Duration::from_millis(66);
        info!("🎮 Action throttling: max {} Hz to Godot", 1000 / 66);

        Ok(Self {
            config,
            godot,
            godot_camera,
            message_count: 0,
            camera_frames_received: Arc::new(AtomicU64::new(0)),
            camera_publisher,
            redis_publisher_rx: Some(redis_publisher_rx),
            streaming_diagnostics,
            godot_diagnostics,
            last_action_time: Arc::new(Mutex::new(std::time::Instant::now())),
            min_action_interval,
            actions_dropped: Arc::new(AtomicU64::new(0)),
        })
    }

    async fn run(&mut self, ctx: &Context) -> Result<()> {
        info!("✅ Simulation Bridge initialized");
        info!("📡 Subscribing to /motor/cmd_vel...");
        info!("🔌 Waiting for Godot connection...");

        // Subscribe to motor commands
        let mut cmd_vel_rx = ctx.subscribe(Topic::<Twist>::new(TOPIC_MOTOR_COMMAND)).await?;

        // Create channel for event-driven camera frame delivery
        // Reduced buffer (30 frames) to prevent long freeze times
        // At 10 FPS, 30 frames = 3 seconds max buffer (acceptable for streaming)
        let (camera_tx, mut camera_rx) =
            mpsc::channel::<(String, Vec<u8>, crate::godot_client::ImageFormat, u32, u32, u64)>(30);

        // Track channel depth for monitoring
        let camera_channel_depth = Arc::new(AtomicU64::new(0));

        // Create channel for IMU data delivery
        // Small buffer (50) - IMU data is tiny (~50 bytes per message)
        let (imu_tx, mut imu_rx) = mpsc::channel::<crate::topics::ImuData>(50);

        // Spawn background Redis publisher task (off hot path)
        if let Some(mut redis_rx) = self.redis_publisher_rx.take() {
            let ctx_clone = ctx.clone();
            tokio::spawn(async move {
                info!("📡 Starting background Redis publisher task");
                let mut published_count = 0u64;

                while let Some((camera_id, image_bytes_arc, format, width, height, timestamp)) = redis_rx.recv().await {
                    published_count += 1;

                    // Construct Redis message (serialization happens here, off hot path!)
                    let camera_msg = crate::topics::CameraImage {
                        camera_id,
                        width,
                        height,
                        timestamp,
                        image_bytes: (*image_bytes_arc).clone(), // Clone Arc contents for Redis serialization
                        format: match format {
                            crate::godot_client::ImageFormat::Jpeg => "jpeg".to_string(),
                            crate::godot_client::ImageFormat::Rgb => "rgb".to_string(),
                        },
                    };

                    let redis_msg = RedisMessage {
                        timestamp,
                        publisher: "simulation-bridge".to_string(),
                        payload: camera_msg,
                    };

                    // Publish to Redis (blocking I/O is fine here - we're in a dedicated task)
                    let topic = Topic::<RedisMessage<crate::topics::CameraImage>>::new(crate::topics::TOPIC_CAMERA_RGB);
                    if let Err(e) = ctx_clone.publish_to(topic, &redis_msg).await {
                        debug!("⚠️  Failed to publish camera data to Redis: {}", e);
                    } else if published_count % 100 == 0 {
                        debug!("📊 Redis publisher: {} frames published", published_count);
                    }
                }

                info!("🛑 Redis publisher task ended");
            });
        }

        // Spawn diagnostics publishing task
        let streaming_diag = Arc::clone(&self.streaming_diagnostics);
        let godot_diag = Arc::clone(&self.godot_diagnostics);
        let ctx_diag = ctx.clone();
        let depth_monitor = Arc::clone(&camera_channel_depth);
        tokio::spawn(async move {
            let mut interval = tokio::time::interval(Duration::from_secs(5));
            info!("📊 Starting diagnostics publishing task (every 5 seconds)");

            loop {
                interval.tick().await;

                // Log channel depth for monitoring
                let current_depth = depth_monitor.load(Ordering::Relaxed);
                info!(
                    "📊 Camera channel depth: {} / 30 frames ({:.1}% full)",
                    current_depth,
                    (current_depth as f64 / 30.0) * 100.0
                );

                // Publish streaming diagnostics
                if let Err(e) = streaming_diag.publish_all(&ctx_diag, 400_000).await {
                    debug!("Failed to publish streaming diagnostics: {}", e);
                }

                // Publish Godot connection diagnostics
                if let Err(e) = godot_diag.publish_connection_metrics(&ctx_diag).await {
                    debug!("Failed to publish Godot diagnostics: {}", e);
                }
            }
        });

        // Spawn dedicated task for camera/IMU data reading (event-driven, no polling!)
        let godot_camera = Arc::clone(&self.godot_camera);
        let godot_diag_cam = Arc::clone(&self.godot_diagnostics);
        let depth_counter = Arc::clone(&camera_channel_depth);
        tokio::spawn(async move {
            loop {
                let mut should_clear = false;
                if let Some(client) = godot_camera.lock().await.as_mut() {
                    // Continuously drain WebSocket messages (event-driven await)
                    match Self::read_sensor_data_blocking(client).await {
                        Ok(Some(SensorData::Camera((camera_id, image_bytes, format, width, height, timestamp)))) => {
                            // Record camera frame received (diagnostics)
                            godot_diag_cam.record_camera_frame(timestamp);

                            // Check current channel depth (without incrementing yet)
                            let current_depth = depth_counter.load(Ordering::Relaxed);

                            // SMART FRAME DROPPING: Drop frames earlier to prevent freeze
                            // With 30-frame buffer at 10 FPS, this gives 3 seconds max latency
                            if current_depth > 20 {
                                // Drop 2 out of 3 frames when over 67% capacity (20/30)
                                static FRAME_DROP_COUNTER: AtomicU64 = AtomicU64::new(0);
                                let drop_count = FRAME_DROP_COUNTER.fetch_add(1, Ordering::Relaxed);

                                if drop_count % 3 != 0 {
                                    // Drop this frame
                                    if drop_count % 30 == 0 {
                                        warn!("⚠️  Camera channel overloaded: {} / 30 frames buffered, dropping 67% of frames", current_depth);
                                    }
                                    continue; // Skip this frame
                                }
                            } else if current_depth > 10 {
                                // Drop 1 out of 2 frames when over 33% capacity (10/30)
                                static FRAME_DROP_COUNTER_MODERATE: AtomicU64 = AtomicU64::new(0);
                                let drop_count = FRAME_DROP_COUNTER_MODERATE.fetch_add(1, Ordering::Relaxed);

                                if drop_count % 2 != 0 {
                                    // Drop this frame
                                    if drop_count % 30 == 0 {
                                        warn!("⚠️  Camera channel filling: {} / 30 frames buffered, dropping 50% of frames", current_depth);
                                    }
                                    continue; // Skip this frame
                                }
                            }

                            // Send to main loop via channel (TRULY non-blocking with try_send)
                            match camera_tx.try_send((camera_id, image_bytes, format, width, height, timestamp)) {
                                Ok(_) => {
                                    // Successfully sent - track channel depth
                                    depth_counter.fetch_add(1, Ordering::Relaxed);
                                }
                                Err(mpsc::error::TrySendError::Full(_)) => {
                                    // Channel full - drop frame to prevent blocking WebSocket reader
                                    // This should rarely happen due to smart dropping above, but protects against edge cases
                                    warn!(
                                        "⚠️  Camera channel full ({}/30), dropping frame to prevent freeze",
                                        current_depth
                                    );
                                }
                                Err(mpsc::error::TrySendError::Closed(_)) => {
                                    warn!("📷 Camera frame channel closed, stopping sensor reader");
                                    break;
                                }
                            }
                        }
                        Ok(Some(SensorData::Imu(imu_data))) => {
                            // Send IMU data to main loop via channel (truly non-blocking)
                            match imu_tx.try_send(imu_data) {
                                Ok(_) => {
                                    // Successfully sent
                                }
                                Err(mpsc::error::TrySendError::Full(_)) => {
                                    // IMU channel full - drop this reading (IMU data is high-frequency)
                                    debug!("⚠️  IMU channel full, dropping reading");
                                }
                                Err(mpsc::error::TrySendError::Closed(_)) => {
                                    warn!("📡 IMU data channel closed, stopping sensor reader");
                                    break;
                                }
                            }
                        }
                        Ok(None) => {
                            // No sensor data available (timeout/ping/pong) - continue reading
                        }
                        Err(e) => {
                            error!("❌ Sensor WebSocket error: {}", e);
                            // Explicitly close WebSocket before clearing
                            let _ = client.close().await;
                            should_clear = true;
                        }
                    }
                } else {
                    // No connection - wait before checking again
                    tokio::time::sleep(Duration::from_millis(100)).await;
                }

                if should_clear {
                    warn!("🔌 Clearing failed sensor connection to trigger reconnect");
                    *godot_camera.lock().await = None;
                }
            }
        });

        info!("🚀 Simulation Bridge running");
        info!("📷 Event-driven camera streaming (no polling!)");

        // Clone depth counter for main loop
        let depth_counter_main = Arc::clone(&camera_channel_depth);

        loop {
            tokio::select! {
                // Handle motor commands from Redis
                Some(msg) = cmd_vel_rx.recv() => {
                    self.handle_motor_command(ctx, msg).await;
                    self.message_count += 1;
                }

                // Event-driven camera frame delivery (replaces 100Hz polling)
                Some((camera_id, image_bytes, format, width, height, timestamp)) = camera_rx.recv() => {
                    // Decrement channel depth (frame consumed)
                    depth_counter_main.fetch_sub(1, Ordering::Relaxed);

                    // Publish to both WebRTC (low-latency streaming) and Redis (telemetry/recording)
                    self.publish_camera_data_from_binary(ctx, camera_id, image_bytes, format, width, height, timestamp).await;
                }

                // Event-driven IMU data delivery
                Some(imu_data) = imu_rx.recv() => {
                    // Publish IMU data to Redis
                    self.publish_imu_data(ctx, imu_data).await;
                }
            }
        }
    }

    async fn health_check(&self) -> HealthStatus {
        let has_godot = self.godot.lock().await.is_some();
        let has_camera = self.godot_camera.lock().await.is_some();

        HealthStatus {
            healthy: has_godot && has_camera,
            last_check: now_micros(),
            failure_count: if has_godot && has_camera { 0 } else { 1 },
            message: Some(format!(
                "Simulation bridge: {} messages processed, Control: {}, Camera: {}",
                self.message_count,
                if has_godot { "connected" } else { "disconnected" },
                if has_camera { "connected" } else { "disconnected" }
            )),
        }
    }
}

impl SimulationBridge {
    /// Background task to maintain Godot connection
    async fn godot_connection_loop(
        godot: Arc<Mutex<Option<GodotClient>>>,
        godot_url: String,
        camera_config: CameraConfig,
        diagnostics: Arc<GodotCollector>,
    ) {
        loop {
            match GodotClient::connect(&godot_url).await {
                Ok(mut client) => {
                    info!("🎮 Connected to Godot, performing handshake...");

                    match client.handshake().await {
                        Ok(_) => {
                            info!("✅ Godot handshake complete");

                            // Send camera configuration to Godot
                            if let Err(e) = client
                                .send_camera_config(camera_config.width, camera_config.height, camera_config.fps)
                                .await
                            {
                                error!("❌ Failed to send camera config: {}", e);
                                // Continue anyway - Godot will use defaults
                            }

                            *godot.lock().await = Some(client);

                            // Track connection established
                            diagnostics.set_control_connected(true);

                            // Connection is established - the connection will be used by handle_motor_command
                            // When it fails, we'll detect it and reconnect
                            // For now, just wait and let the motor command handler use it
                            // We'll monitor by checking if the connection gets cleared
                            loop {
                                tokio::time::sleep(Duration::from_secs(1)).await;

                                // Check if connection was cleared (due to error)
                                if godot.lock().await.is_none() {
                                    warn!("⚠️  Godot connection was lost, will reconnect...");
                                    diagnostics.set_control_connected(false);
                                    break;
                                }
                            }
                        }
                        Err(e) => {
                            error!("❌ Handshake failed: {}", e);
                            // Explicitly close WebSocket before dropping
                            let _ = client.close().await;
                            *godot.lock().await = None;
                            diagnostics.set_control_connected(false);
                        }
                    }
                }
                Err(e) => {
                    warn!("⚠️  Failed to connect to Godot: {}", e);
                    *godot.lock().await = None;
                    diagnostics.set_control_connected(false);
                }
            }

            // Retry after delay
            tokio::time::sleep(Duration::from_secs(2)).await;
        }
    }

    /// Background task to maintain Godot camera WebSocket connection
    async fn godot_camera_connection_loop(
        godot_camera: Arc<Mutex<Option<GodotClient>>>,
        camera_url: String,
        diagnostics: Arc<GodotCollector>,
    ) {
        loop {
            match GodotClient::connect_no_handshake(&camera_url).await {
                Ok(client) => {
                    info!("📷 Connected to Godot camera server on {}", camera_url);
                    *godot_camera.lock().await = Some(client);

                    // Track connection established
                    diagnostics.set_camera_connected(true);

                    // Connection is established - camera data will be polled by check_for_camera_data
                    // Monitor connection health
                    loop {
                        tokio::time::sleep(Duration::from_secs(1)).await;

                        // Check if connection was cleared (due to error)
                        if godot_camera.lock().await.is_none() {
                            warn!("⚠️  Godot camera connection was lost, will reconnect...");
                            diagnostics.set_camera_connected(false);
                            break;
                        }
                    }
                }
                Err(e) => {
                    warn!("⚠️  Failed to connect to Godot camera server: {}", e);
                    *godot_camera.lock().await = None;
                    diagnostics.set_camera_connected(false);
                }
            }

            // Retry after delay
            tokio::time::sleep(Duration::from_secs(2)).await;
        }
    }

    /// Handle motor command from Redis and send to Godot
    async fn handle_motor_command(&mut self, _ctx: &Context, msg: Twist) {
        let action = vec![msg.linear, msg.angular];
        let timestamp = now_micros();

        // Throttle check: don't send actions faster than min_action_interval
        // This prevents overwhelming Godot when commands come in too fast
        {
            let mut last_time = self.last_action_time.lock().await;
            let elapsed = last_time.elapsed();
            if elapsed < self.min_action_interval {
                // Too soon - drop this action (keep the latest velocity value for next send)
                let dropped = self.actions_dropped.fetch_add(1, Ordering::Relaxed) + 1;
                if dropped % 50 == 0 {
                    debug!(
                        "🔄 Throttled {} actions (last sent {}ms ago, min interval {}ms)",
                        dropped,
                        elapsed.as_millis(),
                        self.min_action_interval.as_millis()
                    );
                }
                return;
            }
            *last_time = std::time::Instant::now();
        }

        info!(
            "🎮 Received motor command: linear={}, angular={} (timestamp: {}µs)",
            msg.linear, msg.angular, timestamp
        );

        // Record control message (diagnostics)
        self.godot_diagnostics.record_control_message(timestamp);

        if let Some(client) = self.godot.lock().await.as_mut() {
            let step_start = std::time::Instant::now();
            // Send action with timestamp for command expiration validation
            match client.step_with_timestamp(action, Some(timestamp)).await {
                Ok(step_result) => {
                    let step_elapsed = step_start.elapsed();
                    info!("✅ Action sent to Godot successfully in {:?}", step_elapsed);
                    debug!("Observation size: {}", step_result.observation.len());
                    debug!("Reward: {}, Done: {}", step_result.reward, step_result.done);

                    // Camera data now comes from dedicated camera WebSocket (port 11009)
                    // No longer processed here
                }
                Err(e) => {
                    // For live streaming, timeouts are OK - Godot may drop responses under load
                    // Don't disconnect, just log and continue
                    warn!("⚠️  Step timeout/error (continuing): {}", e);
                }
            }
        } else {
            warn!("⚠️  No Godot connection, dropping motor command");
        }
    }

    /// Publish camera data from raw binary bytes (JPEG or RGB, zero-copy optimized!)
    ///
    /// Publishes to both (using Arc for zero-copy sharing):
    /// - WebRTC server: Low-latency streaming (Arc shared with encoder)
    /// - Redis: Telemetry/recording (background task, Arc shared)
    ///
    /// Performance: Wraps image_bytes in Arc ONCE, shared across all consumers!
    #[allow(clippy::too_many_arguments)]
    async fn publish_camera_data_from_binary(
        &self,
        _ctx: &Context, // Not used anymore - Redis publishing moved to background task
        camera_id: String,
        image_bytes: Vec<u8>,
        format: crate::godot_client::ImageFormat,
        width: u32,
        height: u32,
        timestamp: u64,
    ) {
        self.camera_frames_received.fetch_add(1, Ordering::Relaxed);

        // Only process front camera for now (ignore overhead/debug cameras)
        if camera_id != "CameraFront" {
            return;
        }

        // Record frame received (diagnostics)
        self.streaming_diagnostics.record_frame_received();

        // Publish to both WebRTC and Redis via CameraPublisher
        if let Some(publisher) = &self.camera_publisher {
            match publisher.publish(camera_id, image_bytes, format, width, height, timestamp) {
                Ok(_stats) => {
                    // Successfully published (diagnostics tracking handled by publisher)
                }
                Err(e) => {
                    debug!("⚠️  Failed to publish camera frame: {}", e);
                }
            }
        }
    }

    /// Read a single camera frame from Godot WebSocket (event-driven)
    ///
    /// This method properly awaits on the WebSocket stream with no timeout or polling.
    /// It blocks until a message arrives, minimizing CPU usage and latency.
    ///
    /// Returns Some(sensor_data) if camera or IMU data received, None for other message types.
    async fn read_sensor_data_blocking(client: &mut crate::godot_client::GodotClient) -> Result<Option<SensorData>> {
        use crate::godot_client::GodotMessage;

        // Await on WebSocket for next message (event-driven, no polling!)
        match client.receive_message_async().await {
            Ok(Some(GodotMessage::CameraDataBinary {
                camera_id,
                image_bytes,
                format,
                width,
                height,
                timestamp,
            })) => {
                debug!("📷 Received camera frame: {}, {} bytes", camera_id, image_bytes.len());
                Ok(Some(SensorData::Camera((
                    camera_id,
                    image_bytes,
                    format,
                    width,
                    height,
                    timestamp,
                ))))
            }
            Ok(Some(GodotMessage::ImuDataBinary {
                imu_id,
                timestamp,
                roll,
                pitch,
                yaw,
                angular_velocity_x,
                angular_velocity_y,
                angular_velocity_z,
                linear_acceleration_x,
                linear_acceleration_y,
                linear_acceleration_z,
            })) => {
                debug!("📡 Received IMU data: {}", imu_id);
                Ok(Some(SensorData::Imu(crate::topics::ImuData {
                    imu_id,
                    timestamp,
                    roll,
                    pitch,
                    yaw,
                    angular_velocity_x,
                    angular_velocity_y,
                    angular_velocity_z,
                    linear_acceleration_x,
                    linear_acceleration_y,
                    linear_acceleration_z,
                })))
            }
            Ok(Some(_)) => {
                // Other message type (ping/pong/text) - not sensor data
                Ok(None)
            }
            Ok(None) => {
                // No message (shouldn't happen with blocking receive, but handle gracefully)
                Ok(None)
            }
            Err(e) => Err(e.into()),
        }
    }

    /// Publish IMU data to Redis
    async fn publish_imu_data(&self, ctx: &Context, imu_data: crate::topics::ImuData) {
        let redis_msg = RedisMessage {
            timestamp: imu_data.timestamp,
            publisher: "simulation-bridge".to_string(),
            payload: imu_data,
        };

        let topic = Topic::<RedisMessage<crate::topics::ImuData>>::new(TOPIC_IMU_DATA);
        if let Err(e) = ctx.publish_to(topic, &redis_msg).await {
            debug!("⚠️  Failed to publish IMU data to Redis: {}", e);
        }
    }
}
