//! UI module for sim2d control panel

// Bevy systems commonly have many arguments
#![allow(clippy::too_many_arguments)]

use crate::{recorder::Recorder, scenario::Scenario, AppConfig};
use bevy::prelude::*;
use bevy_egui::{egui, EguiContexts};
use std::path::{Path, PathBuf};

/// Helper function to safely get file name from a path
fn get_file_name_str(path: &Path) -> String {
    path.file_name()
        .map(|name| name.to_string_lossy().to_string())
        .unwrap_or_else(|| "Unknown".to_string())
}

/// UI state resource
#[derive(Resource)]
pub struct UiState {
    pub robot_config_path: Option<PathBuf>,
    pub world_config_path: Option<PathBuf>,
    pub scenario_path: Option<PathBuf>,
    pub active_topics: Vec<String>,
    pub show_file_dialog: FileDialogType,
    pub status_message: String,
    pub topic_input: String,
    pub paused: bool,
    pub simulation_speed: f32,
    pub show_world_section: bool,
    pub show_robot_section: bool,
    pub show_topics_section: bool,
    pub show_camera_section: bool,
    pub show_visual_section: bool,
    pub show_telemetry_section: bool,
    pub show_scenario_section: bool,
    pub show_recording_section: bool,
    pub recording_name: String,
    pub recording_description: String,
    pub recording_path: Option<PathBuf>,
    pub reset_simulation: bool,
    /// Pending topic prefix update: (robot_name, new_prefix)
    pub pending_topic_update: Option<(String, String)>,
    pub show_help: bool,
    pub show_editor_section: bool,
    pub editor_selected_color: [f32; 3],
    pub show_metrics_section: bool,
    pub metrics_export_path: Option<PathBuf>,
    // Articulated robot UI state
    pub show_articulated_section: bool,
    pub show_joint_markers: bool,
    pub selected_articulated_robot: Option<String>,
    pub joint_control_mode: crate::joint::JointControlMode,
    pub joint_sliders: std::collections::HashMap<String, f32>,
}

/// Visual preferences for the simulator
#[derive(Resource)]
pub struct VisualPreferences {
    pub show_grid: bool,
    pub grid_spacing: f32,
    pub grid_color: [f32; 3],
    pub obstacle_color: [f32; 3],
    pub wall_color: [f32; 3],
    pub background_color: [f32; 3],
    pub show_velocity_arrows: bool,
    pub show_lidar_rays: bool,
    pub show_trajectory: bool,
    pub trajectory_length: usize,
}

impl Default for VisualPreferences {
    fn default() -> Self {
        Self {
            show_grid: true,
            grid_spacing: 1.0, // 1 meter grid
            grid_color: [0.2, 0.2, 0.2],
            obstacle_color: [0.6, 0.4, 0.2],   // Brown
            wall_color: [0.3, 0.3, 0.3],       // Gray
            background_color: [0.1, 0.1, 0.1], // Dark gray
            show_velocity_arrows: false,
            show_lidar_rays: false,
            show_trajectory: false,
            trajectory_length: 100,
        }
    }
}

/// Camera controller for zoom and pan
#[derive(Resource)]
pub struct CameraController {
    pub zoom: f32,
    pub pan_x: f32,
    pub pan_y: f32,
}

impl Default for CameraController {
    fn default() -> Self {
        Self {
            zoom: 1.0,
            pan_x: 0.0,
            pan_y: 0.0,
        }
    }
}

/// Robot telemetry data
#[derive(Resource, Default)]
pub struct RobotTelemetry {
    pub position: (f32, f32),
    pub velocity: (f32, f32),
    pub heading: f32,
    pub angular_velocity: f32,
}

/// Frame timing metrics for UI display
#[derive(Resource)]
pub struct FrameMetrics {
    pub fps: f32,
    pub frame_time: f32,
    pub physics_time: f32,
    last_update: std::time::Instant,
    frame_count: u32,
}

impl Default for FrameMetrics {
    fn default() -> Self {
        Self {
            fps: 0.0,
            frame_time: 0.0,
            physics_time: 0.0,
            last_update: std::time::Instant::now(),
            frame_count: 0,
        }
    }
}

impl FrameMetrics {
    pub fn update(&mut self) {
        self.frame_count += 1;
        let elapsed = self.last_update.elapsed();

        if elapsed.as_secs_f32() >= 0.5 {
            self.fps = self.frame_count as f32 / elapsed.as_secs_f32();
            self.frame_time = 1000.0 / self.fps;
            self.frame_count = 0;
            self.last_update = std::time::Instant::now();
        }
    }
}

#[derive(PartialEq)]
pub enum FileDialogType {
    None,
    RobotConfig,
    WorldConfig,
    ScenarioSave,
    ScenarioLoad,
    RecordingLoad,
    ExportCSV,
    ExportVideo,
}

impl Default for UiState {
    fn default() -> Self {
        Self {
            robot_config_path: None,
            world_config_path: None,
            scenario_path: None,
            active_topics: vec![],
            show_file_dialog: FileDialogType::None,
            status_message: "Ready".to_string(),
            topic_input: String::new(),
            paused: false,
            simulation_speed: 1.0,
            show_world_section: true,
            show_robot_section: true,
            show_topics_section: false,
            show_camera_section: false,
            show_visual_section: false,
            show_telemetry_section: true,
            show_scenario_section: false,
            show_recording_section: false,
            recording_name: "My Recording".to_string(),
            recording_description: String::new(),
            recording_path: None,
            reset_simulation: false,
            pending_topic_update: None,
            show_help: false,
            show_editor_section: false,
            editor_selected_color: [0.6, 0.6, 0.6],
            show_metrics_section: false,
            metrics_export_path: None,
            // Articulated robot defaults
            show_articulated_section: false,
            show_joint_markers: true,
            selected_articulated_robot: None,
            joint_control_mode: crate::joint::JointControlMode::Position,
            joint_sliders: std::collections::HashMap::new(),
        }
    }
}

/// System to render the left control panel
pub fn ui_system(
    mut contexts: EguiContexts,
    mut ui_state: ResMut<UiState>,
    mut app_config: ResMut<AppConfig>,
    mut camera_controller: ResMut<CameraController>,
    mut visual_prefs: ResMut<VisualPreferences>,
    telemetry: Option<Res<RobotTelemetry>>,
    mut metrics: ResMut<FrameMetrics>,
    mut recorder: ResMut<Recorder>,
    mut editor: ResMut<crate::editor::WorldEditor>,
    mut perf_metrics: ResMut<crate::metrics::PerformanceMetrics>,
    articulated_robots: Query<&crate::joint::ArticulatedRobot>,
    mut physics_world: ResMut<crate::PhysicsWorld>,
) {
    // Update performance metrics
    metrics.update();

    // Handle keyboard shortcuts
    let ctx = contexts.ctx_mut();
    if ctx.input(|i| i.modifiers.ctrl && i.key_pressed(egui::Key::S)) {
        ui_state.show_file_dialog = FileDialogType::ScenarioSave;
    }
    if ctx.input(|i| i.modifiers.ctrl && i.key_pressed(egui::Key::O)) {
        ui_state.show_file_dialog = FileDialogType::ScenarioLoad;
    }

    // Editor keyboard shortcuts
    if editor.enabled {
        if ctx.input(|i| i.modifiers.ctrl && i.key_pressed(egui::Key::Z)) && editor.undo().is_some()
        {
            ui_state.status_message = "Undone".to_string();
        }
        if ctx.input(|i| i.modifiers.ctrl && i.key_pressed(egui::Key::Y)) && editor.redo().is_some()
        {
            ui_state.status_message = "Redone".to_string();
        }
        if ctx.input(|i| i.key_pressed(egui::Key::Escape)) && !editor.selected_obstacles.is_empty()
        {
            editor.clear_selection();
            ui_state.status_message = "Selection cleared".to_string();
        }
    }

    egui::SidePanel::left("control_panel")
        .min_width(340.0)
        .max_width(400.0)
        .resizable(true)
        .show(contexts.ctx_mut(), |ui| {
            // Header with improved styling
            ui.vertical_centered(|ui| {
                ui.add_space(5.0);
                ui.heading(egui::RichText::new("sim2d").size(28.0).strong());
                ui.label(
                    egui::RichText::new("2D Robotics Simulator")
                        .size(11.0)
                        .color(egui::Color32::from_rgb(150, 150, 150)),
                );
                ui.add_space(3.0);
            });
            ui.separator();
            ui.add_space(5.0);

            // Simulation state indicator
            let state_color = if recorder.is_playing() {
                egui::Color32::from_rgb(100, 149, 237) // Cornflower blue for playback
            } else if recorder.is_recording() {
                egui::Color32::from_rgb(220, 60, 60) // Red for recording
            } else if ui_state.paused {
                egui::Color32::from_rgb(180, 180, 60) // Yellow for paused
            } else {
                egui::Color32::from_rgb(60, 179, 113) // Green for running
            };

            let state_text = if recorder.is_playing() {
                "PLAYBACK"
            } else if recorder.is_recording() {
                "RECORDING"
            } else if ui_state.paused {
                "PAUSED"
            } else {
                "RUNNING"
            };

            // State indicator bar
            ui.horizontal(|ui| {
                ui.add(egui::Label::new(
                    egui::RichText::new(state_text)
                        .size(11.0)
                        .color(state_color)
                        .strong(),
                ));
                ui.with_layout(egui::Layout::right_to_left(egui::Align::Center), |ui| {
                    ui.label(
                        egui::RichText::new(format!("{:.0} FPS", metrics.fps))
                            .size(10.0)
                            .color(egui::Color32::from_rgb(140, 140, 140)),
                    );
                });
            });
            ui.add_space(3.0);

            // Simulation Controls
            ui.group(|ui| {
                ui.set_min_width(ui.available_width());
                ui.vertical(|ui| {
                    ui.horizontal(|ui| {
                        // Play/Pause button
                        let (play_text, play_color) = if ui_state.paused {
                            ("Play", egui::Color32::from_rgb(60, 179, 113))
                        } else {
                            ("Pause", egui::Color32::from_rgb(180, 180, 60))
                        };

                        let play_btn = egui::Button::new(
                            egui::RichText::new(play_text).size(13.0).color(play_color),
                        )
                        .min_size(egui::vec2(70.0, 28.0));

                        if ui.add(play_btn).on_hover_text("Space to toggle").clicked() {
                            ui_state.paused = !ui_state.paused;
                            ui_state.status_message = if ui_state.paused {
                                "Paused".to_string()
                            } else {
                                "Running".to_string()
                            };
                        }

                        // Reset button
                        let reset_btn = egui::Button::new(egui::RichText::new("Reset").size(13.0))
                            .min_size(egui::vec2(70.0, 28.0));
                        if ui
                            .add(reset_btn)
                            .on_hover_text("Reset simulation to initial state")
                            .clicked()
                        {
                            ui_state.reset_simulation = true;
                            ui_state.status_message = "Resetting...".to_string();
                        }

                        // Help button
                        let help_btn = egui::Button::new(egui::RichText::new("Help").size(13.0))
                            .min_size(egui::vec2(50.0, 28.0));
                        if ui.add(help_btn).clicked() {
                            ui_state.show_help = !ui_state.show_help;
                        }
                    });
                });
            });

            ui.horizontal(|ui| {
                ui.label("Speed:");
                if ui
                    .add(egui::Slider::new(&mut ui_state.simulation_speed, 0.1..=5.0).text("x"))
                    .changed()
                {
                    ui_state.status_message = format!("Speed: {:.1}x", ui_state.simulation_speed);
                }
            });

            ui.separator();

            // Scrollable area for sections
            egui::ScrollArea::vertical().show(ui, |ui| {
                // Telemetry Section
                egui::CollapsingHeader::new(egui::RichText::new("Telemetry").size(14.0))
                    .default_open(ui_state.show_telemetry_section)
                    .show(ui, |ui| {
                        ui_state.show_telemetry_section = true;

                        if let Some(telem) = telemetry.as_ref() {
                            egui::Grid::new("telemetry_grid")
                                .num_columns(2)
                                .spacing([20.0, 4.0])
                                .show(ui, |ui| {
                                    ui.label(
                                        egui::RichText::new("Position")
                                            .size(11.0)
                                            .color(egui::Color32::from_rgb(160, 160, 160)),
                                    );
                                    ui.label(format!(
                                        "({:.2}, {:.2}) m",
                                        telem.position.0, telem.position.1
                                    ));
                                    ui.end_row();

                                    ui.label(
                                        egui::RichText::new("Velocity")
                                            .size(11.0)
                                            .color(egui::Color32::from_rgb(160, 160, 160)),
                                    );
                                    ui.label(format!(
                                        "({:.2}, {:.2}) m/s",
                                        telem.velocity.0, telem.velocity.1
                                    ));
                                    ui.end_row();

                                    ui.label(
                                        egui::RichText::new("Heading")
                                            .size(11.0)
                                            .color(egui::Color32::from_rgb(160, 160, 160)),
                                    );
                                    ui.label(format!("{:.1} deg", telem.heading.to_degrees()));
                                    ui.end_row();

                                    ui.label(
                                        egui::RichText::new("Angular")
                                            .size(11.0)
                                            .color(egui::Color32::from_rgb(160, 160, 160)),
                                    );
                                    ui.label(format!("{:.2} rad/s", telem.angular_velocity));
                                    ui.end_row();
                                });
                        } else {
                            ui.label(
                                egui::RichText::new("No telemetry data").color(egui::Color32::GRAY),
                            );
                        }
                    });

                // Scenarios Section
                egui::CollapsingHeader::new(egui::RichText::new("Scenarios").size(14.0))
                    .default_open(ui_state.show_scenario_section)
                    .show(ui, |ui| {
                        ui_state.show_scenario_section = true;

                        ui.label(
                            egui::RichText::new("Save/Load complete simulation states")
                                .size(11.0)
                                .color(egui::Color32::from_rgb(150, 150, 150)),
                        );
                        ui.add_space(5.0);

                        ui.horizontal(|ui| {
                            let save_button =
                                egui::Button::new(egui::RichText::new("Save Scenario").size(13.0))
                                    .min_size(egui::vec2(140.0, 28.0));

                            if ui.add(save_button).clicked() {
                                ui_state.show_file_dialog = FileDialogType::ScenarioSave;
                            }

                            let load_button =
                                egui::Button::new(egui::RichText::new("Load Scenario").size(13.0))
                                    .min_size(egui::vec2(140.0, 28.0));

                            if ui.add(load_button).clicked() {
                                ui_state.show_file_dialog = FileDialogType::ScenarioLoad;
                            }
                        });

                        if let Some(path) = &ui_state.scenario_path {
                            ui.add_space(5.0);
                            ui.label(
                                egui::RichText::new(format!(
                                    "Current: {}",
                                    get_file_name_str(path)
                                ))
                                .color(egui::Color32::LIGHT_GREEN),
                            );
                        } else {
                            ui.add_space(5.0);
                            ui.label(
                                egui::RichText::new("No scenario loaded")
                                    .color(egui::Color32::GRAY),
                            );
                        }

                        ui.add_space(5.0);
                        ui.label(
                            egui::RichText::new("Keyboard shortcuts:")
                                .size(11.0)
                                .strong(),
                        );
                        ui.label(
                            egui::RichText::new("  Ctrl+S - Quick save")
                                .size(10.0)
                                .color(egui::Color32::from_rgb(180, 180, 180)),
                        );
                        ui.label(
                            egui::RichText::new("  Ctrl+O - Open scenario")
                                .size(10.0)
                                .color(egui::Color32::from_rgb(180, 180, 180)),
                        );
                    });

                // Recording Section
                egui::CollapsingHeader::new(egui::RichText::new("Recording").size(14.0))
                    .default_open(ui_state.show_recording_section)
                    .show(ui, |ui| {
                        ui_state.show_recording_section = true;

                        ui.label(
                            egui::RichText::new("Record and export simulation runs")
                                .size(11.0)
                                .color(egui::Color32::from_rgb(150, 150, 150)),
                        );
                        ui.add_space(5.0);

                        // Recording name input
                        ui.horizontal(|ui| {
                            ui.label("Name:");
                            ui.text_edit_singleline(&mut ui_state.recording_name);
                        });

                        // Recording description input
                        ui.horizontal(|ui| {
                            ui.label("Desc:");
                            ui.text_edit_singleline(&mut ui_state.recording_description);
                        });

                        ui.add_space(5.0);

                        // Recording controls
                        if recorder.is_recording() {
                            // Show recording status
                            let metadata = recorder.get_metadata().unwrap();
                            ui.label(
                                egui::RichText::new(format!("Recording: {}", metadata.name))
                                    .color(egui::Color32::RED)
                                    .strong(),
                            );
                            ui.label(format!("Frames: {}", metadata.frame_count));
                            ui.label(format!("Duration: {:.1}s", metadata.duration));

                            ui.add_space(5.0);

                            // Stop button
                            let stop_button = egui::Button::new(
                                egui::RichText::new("Stop Recording")
                                    .size(13.0)
                                    .color(egui::Color32::RED),
                            )
                            .min_size(egui::vec2(200.0, 28.0));

                            if ui.add(stop_button).clicked() {
                                if let Some(recording) = recorder.stop_recording() {
                                    // Save dialog
                                    if let Some(path) = rfd::FileDialog::new()
                                        .add_filter("Recording Files", &["yaml", "yml"])
                                        .set_file_name("recording.yaml")
                                        .save_file()
                                    {
                                        match recording.save_to_file(&path) {
                                            Ok(()) => {
                                                ui_state.recording_path = Some(path.clone());
                                                ui_state.status_message = format!(
                                                    "Recording saved: {}",
                                                    get_file_name_str(&path)
                                                );
                                            }
                                            Err(e) => {
                                                ui_state.status_message =
                                                    format!("Error saving recording: {}", e);
                                            }
                                        }
                                    }
                                }
                            }
                        } else {
                            // Start button
                            let start_button = egui::Button::new(
                                egui::RichText::new("Start Recording")
                                    .size(13.0)
                                    .color(egui::Color32::GREEN),
                            )
                            .min_size(egui::vec2(200.0, 28.0));

                            if ui.add(start_button).clicked() {
                                recorder.start_recording(
                                    ui_state.recording_name.clone(),
                                    ui_state.recording_description.clone(),
                                    app_config.robots.clone(),
                                );
                                ui_state.status_message = "Recording started".to_string();
                            }
                        }

                        // Show last saved recording path
                        if let Some(path) = &ui_state.recording_path {
                            ui.add_space(3.0);
                            ui.label(
                                egui::RichText::new(format!("Last: {}", get_file_name_str(path)))
                                    .size(10.0)
                                    .color(egui::Color32::from_rgb(120, 120, 120)),
                            );
                        }

                        ui.add_space(5.0);
                        ui.separator();

                        // Playback section
                        ui.label(egui::RichText::new("Playback").size(11.0).strong());

                        if recorder.playback.recording.is_some() {
                            let total_frames = recorder.playback.total_frames();
                            let current_frame = recorder.playback.current_frame;
                            let progress = recorder.playback.progress();

                            // Progress bar
                            ui.add(
                                egui::ProgressBar::new(progress)
                                    .text(format!("Frame {} / {}", current_frame, total_frames)),
                            );

                            ui.add_space(3.0);

                            // Playback controls
                            ui.horizontal(|ui| {
                                // Play/Pause
                                let (pb_text, pb_color) = if recorder.playback.is_playing {
                                    ("Pause", egui::Color32::from_rgb(180, 180, 60))
                                } else {
                                    ("Play", egui::Color32::from_rgb(60, 179, 113))
                                };

                                if ui
                                    .button(egui::RichText::new(pb_text).color(pb_color))
                                    .clicked()
                                {
                                    if recorder.playback.is_playing {
                                        recorder.playback.pause();
                                    } else {
                                        recorder.playback.resume();
                                    }
                                }

                                // Stop
                                if ui.button("Stop").clicked() {
                                    recorder.playback.stop();
                                    recorder.playback.seek(0);
                                    ui_state.status_message = "Playback stopped".to_string();
                                }

                                // Step buttons
                                if ui
                                    .button("< Prev")
                                    .on_hover_text("Previous frame")
                                    .clicked()
                                {
                                    let new_frame = current_frame.saturating_sub(1);
                                    recorder.playback.seek(new_frame);
                                }

                                if ui.button("Next >").on_hover_text("Next frame").clicked() {
                                    recorder.playback.seek(current_frame + 1);
                                }
                            });

                            ui.add_space(3.0);

                            // Speed and loop controls
                            ui.horizontal(|ui| {
                                ui.label("Speed:");
                                ui.add(
                                    egui::Slider::new(&mut recorder.playback.speed, 0.1..=4.0)
                                        .text("x"),
                                );
                            });

                            ui.checkbox(&mut recorder.playback.loop_playback, "Loop playback");

                            ui.add_space(3.0);
                            if ui.button("Unload Recording").clicked() {
                                recorder.playback.recording = None;
                                ui_state.recording_path = None;
                                ui_state.status_message = "Recording unloaded".to_string();
                            }
                        } else if ui.button("Load Recording").clicked() {
                            ui_state.show_file_dialog = FileDialogType::RecordingLoad;
                        }

                        ui.add_space(5.0);
                        ui.separator();

                        // Export options
                        ui.label(egui::RichText::new("Export").size(11.0).strong());
                        if ui.button("Export to CSV").clicked() {
                            ui_state.show_file_dialog = FileDialogType::ExportCSV;
                        }
                        if ui.button("Export to Video (MP4)").clicked() {
                            ui_state.show_file_dialog = FileDialogType::ExportVideo;
                        }
                    });

                // World Configuration Section
                egui::CollapsingHeader::new(egui::RichText::new("World").size(14.0))
                    .default_open(ui_state.show_world_section)
                    .show(ui, |ui| {
                        ui_state.show_world_section = true;

                        if ui.button("Load World File").clicked() {
                            ui_state.show_file_dialog = FileDialogType::WorldConfig;
                        }

                        if let Some(path) = &ui_state.world_config_path {
                            ui.label(
                                egui::RichText::new(format!("File: {}", get_file_name_str(path)))
                                    .color(egui::Color32::LIGHT_GREEN),
                            );
                        } else {
                            ui.label(
                                egui::RichText::new("Using default config")
                                    .color(egui::Color32::GRAY),
                            );
                        }

                        ui.add_space(5.0);
                        ui.label(format!(
                            "Size: {:.1}m × {:.1}m",
                            app_config.world_config.width, app_config.world_config.height
                        ));
                        ui.label(format!(
                            "Obstacles: {}",
                            app_config.world_config.obstacles.len()
                        ));
                    });

                // Robot Configuration Section
                egui::CollapsingHeader::new(egui::RichText::new("Robot").size(14.0))
                    .default_open(ui_state.show_robot_section)
                    .show(ui, |ui| {
                        ui_state.show_robot_section = true;

                        if ui.button("Load Robot File").clicked() {
                            ui_state.show_file_dialog = FileDialogType::RobotConfig;
                        }

                        if let Some(path) = &ui_state.robot_config_path {
                            ui.label(
                                egui::RichText::new(format!("File: {}", get_file_name_str(path)))
                                    .color(egui::Color32::LIGHT_GREEN),
                            );
                        } else {
                            ui.label(
                                egui::RichText::new("Using default config")
                                    .color(egui::Color32::GRAY),
                            );
                        }

                        ui.add_space(5.0);
                        ui.label(egui::RichText::new("Robot Parameters").strong());

                        // Edit first robot's parameters (if any robots exist)
                        if let Some(robot_config) = app_config.robots.get_mut(0) {
                            ui.horizontal(|ui| {
                                ui.label("Max Speed:");
                                if ui
                                    .add(
                                        egui::Slider::new(&mut robot_config.max_speed, 0.1..=10.0)
                                            .suffix(" m/s"),
                                    )
                                    .changed()
                                {
                                    ui_state.status_message = format!(
                                        "Max speed: {:.1} m/s (applied)",
                                        robot_config.max_speed
                                    );
                                    // Max speed applies live through kinematics, no restart needed
                                }
                            });

                            ui.horizontal(|ui| {
                                ui.label("Length:");
                                if ui
                                    .add(
                                        egui::Slider::new(&mut robot_config.length, 0.1..=3.0)
                                            .suffix(" m"),
                                    )
                                    .changed()
                                {
                                    ui_state.reset_simulation = true;
                                    ui_state.status_message =
                                        "Robot size changed - restarting...".to_string();
                                }
                            });

                            ui.horizontal(|ui| {
                                ui.label("Width:");
                                if ui
                                    .add(
                                        egui::Slider::new(&mut robot_config.width, 0.1..=3.0)
                                            .suffix(" m"),
                                    )
                                    .changed()
                                {
                                    ui_state.reset_simulation = true;
                                    ui_state.status_message =
                                        "Robot size changed - restarting...".to_string();
                                }
                            });

                            ui.horizontal(|ui| {
                                ui.label("Color:");
                                let mut color = egui::Color32::from_rgb(
                                    (robot_config.color[0] * 255.0) as u8,
                                    (robot_config.color[1] * 255.0) as u8,
                                    (robot_config.color[2] * 255.0) as u8,
                                );
                                if ui.color_edit_button_srgba(&mut color).changed() {
                                    robot_config.color = [
                                        color.r() as f32 / 255.0,
                                        color.g() as f32 / 255.0,
                                        color.b() as f32 / 255.0,
                                    ];
                                    // Color updates live - no restart needed
                                    ui_state.status_message = "Robot color updated".to_string();
                                }
                            });
                        } else {
                            ui.label("No robots configured");
                        }
                    });

                // Topics Section
                egui::CollapsingHeader::new(egui::RichText::new("Topics").size(14.0))
                    .default_open(ui_state.show_topics_section)
                    .show(ui, |ui| {
                        ui_state.show_topics_section = true;

                        ui.horizontal(|ui| {
                            ui.label("Prefix:");
                            if ui_state.topic_input.is_empty() {
                                ui_state.topic_input = app_config.args.topic.clone();
                            }
                            let response = ui.text_edit_singleline(&mut ui_state.topic_input);

                            // Check if topic changed from current
                            let topic_changed = ui_state.topic_input != app_config.args.topic;

                            if topic_changed
                                && (ui.button("Apply").clicked()
                                    || response.lost_focus()
                                        && ui.input(|i| i.key_pressed(egui::Key::Enter)))
                            {
                                // Update the topic dynamically (no restart needed)
                                app_config.args.topic = ui_state.topic_input.clone();
                                // Update robot topic prefixes too
                                if let Some(robot) = app_config.robots.get_mut(0) {
                                    let robot_name = robot.name.clone();
                                    let new_prefix = ui_state.topic_input.clone();
                                    robot.topic_prefix = new_prefix.clone();
                                    // Queue the hub update for the system to process
                                    ui_state.pending_topic_update = Some((robot_name, new_prefix));
                                }
                                ui_state.active_topics = vec![ui_state.topic_input.clone()];
                                ui_state.status_message = format!(
                                    "Topic changed to {} - updating hubs...",
                                    ui_state.topic_input
                                );
                            }
                        });

                        // Show hint if topic is different
                        if ui_state.topic_input != app_config.args.topic {
                            ui.label(
                                egui::RichText::new("Press Apply or Enter to change topic")
                                    .color(egui::Color32::YELLOW)
                                    .size(10.0),
                            );
                        }

                        ui.add_space(5.0);
                        ui.label(egui::RichText::new("Active Topics:").strong());
                        if ui_state.active_topics.is_empty() {
                            ui_state.active_topics.push(app_config.args.topic.clone());
                        }
                        for topic in &ui_state.active_topics {
                            ui.label(format!("  - {}.cmd_vel", topic));
                            ui.label(format!("  - {}.odom", topic));
                            ui.label(format!("  - {}.scan", topic));
                        }
                    });

                // Camera Controls Section
                egui::CollapsingHeader::new(egui::RichText::new("Camera").size(14.0))
                    .default_open(ui_state.show_camera_section)
                    .show(ui, |ui| {
                        ui_state.show_camera_section = true;

                        ui.horizontal(|ui| {
                            ui.label("Zoom:");
                            if ui
                                .add(
                                    egui::Slider::new(&mut camera_controller.zoom, 0.1..=5.0)
                                        .text("x"),
                                )
                                .changed()
                            {
                                ui_state.status_message =
                                    format!("Zoom: {:.1}x", camera_controller.zoom);
                            }
                        });

                        ui.horizontal(|ui| {
                            ui.label("Pan X:");
                            ui.add(
                                egui::Slider::new(&mut camera_controller.pan_x, -1000.0..=1000.0)
                                    .text("px"),
                            );
                        });

                        ui.horizontal(|ui| {
                            ui.label("Pan Y:");
                            ui.add(
                                egui::Slider::new(&mut camera_controller.pan_y, -1000.0..=1000.0)
                                    .text("px"),
                            );
                        });

                        if ui.button("Reset Camera").clicked() {
                            camera_controller.zoom = 1.0;
                            camera_controller.pan_x = 0.0;
                            camera_controller.pan_y = 0.0;
                            ui_state.status_message = "Camera reset".to_string();
                        }
                    });

                // Visual Customization Section
                egui::CollapsingHeader::new(egui::RichText::new("Visuals").size(14.0))
                    .default_open(ui_state.show_visual_section)
                    .show(ui, |ui| {
                        ui_state.show_visual_section = true;

                        ui.checkbox(&mut visual_prefs.show_grid, "Show Grid");

                        if visual_prefs.show_grid {
                            ui.horizontal(|ui| {
                                ui.label("  Spacing:");
                                ui.add(
                                    egui::Slider::new(&mut visual_prefs.grid_spacing, 0.5..=5.0)
                                        .suffix(" m"),
                                );
                            });

                            ui.horizontal(|ui| {
                                ui.label("  Color:");
                                let mut color = egui::Color32::from_rgb(
                                    (visual_prefs.grid_color[0] * 255.0) as u8,
                                    (visual_prefs.grid_color[1] * 255.0) as u8,
                                    (visual_prefs.grid_color[2] * 255.0) as u8,
                                );
                                if ui.color_edit_button_srgba(&mut color).changed() {
                                    visual_prefs.grid_color = [
                                        color.r() as f32 / 255.0,
                                        color.g() as f32 / 255.0,
                                        color.b() as f32 / 255.0,
                                    ];
                                }
                            });
                        }

                        ui.add_space(5.0);
                        ui.horizontal(|ui| {
                            ui.label("Obstacle:");
                            let mut color = egui::Color32::from_rgb(
                                (visual_prefs.obstacle_color[0] * 255.0) as u8,
                                (visual_prefs.obstacle_color[1] * 255.0) as u8,
                                (visual_prefs.obstacle_color[2] * 255.0) as u8,
                            );
                            if ui.color_edit_button_srgba(&mut color).changed() {
                                visual_prefs.obstacle_color = [
                                    color.r() as f32 / 255.0,
                                    color.g() as f32 / 255.0,
                                    color.b() as f32 / 255.0,
                                ];
                            }
                        });

                        ui.horizontal(|ui| {
                            ui.label("Wall:");
                            let mut color = egui::Color32::from_rgb(
                                (visual_prefs.wall_color[0] * 255.0) as u8,
                                (visual_prefs.wall_color[1] * 255.0) as u8,
                                (visual_prefs.wall_color[2] * 255.0) as u8,
                            );
                            if ui.color_edit_button_srgba(&mut color).changed() {
                                visual_prefs.wall_color = [
                                    color.r() as f32 / 255.0,
                                    color.g() as f32 / 255.0,
                                    color.b() as f32 / 255.0,
                                ];
                            }
                        });

                        ui.add_space(5.0);
                        ui.checkbox(
                            &mut visual_prefs.show_velocity_arrows,
                            "Show Velocity Arrows",
                        );
                        ui.checkbox(&mut visual_prefs.show_lidar_rays, "Show LIDAR Rays");
                        ui.checkbox(&mut visual_prefs.show_trajectory, "Show Trajectory Trail");

                        if visual_prefs.show_trajectory {
                            ui.horizontal(|ui| {
                                ui.label("  Trail Length:");
                                ui.add(egui::Slider::new(
                                    &mut visual_prefs.trajectory_length,
                                    10..=500,
                                ));
                            });
                        }
                    });

                // World Editor Section
                egui::CollapsingHeader::new(egui::RichText::new("World Editor").size(14.0))
                    .default_open(ui_state.show_editor_section)
                    .show(ui, |ui| {
                        ui_state.show_editor_section = true;

                        ui.label(
                            egui::RichText::new("Create and modify obstacles interactively")
                                .size(11.0)
                                .color(egui::Color32::from_rgb(150, 150, 150)),
                        );
                        ui.add_space(5.0);

                        // Enable/Disable editor
                        ui.horizontal(|ui| {
                            let enable_text = if editor.enabled {
                                "Disable Editor"
                            } else {
                                "Enable Editor"
                            };
                            let enable_color = if editor.enabled {
                                egui::Color32::RED
                            } else {
                                egui::Color32::GREEN
                            };

                            let button = egui::Button::new(
                                egui::RichText::new(enable_text)
                                    .size(13.0)
                                    .color(enable_color),
                            )
                            .min_size(egui::vec2(200.0, 28.0));

                            if ui.add(button).clicked() {
                                editor.enabled = !editor.enabled;
                                ui_state.status_message = if editor.enabled {
                                    "Editor enabled".to_string()
                                } else {
                                    "Editor disabled".to_string()
                                };
                            }
                        });

                        if editor.enabled {
                            ui.add_space(5.0);
                            ui.label(egui::RichText::new("Tools").strong());

                            // Tool selection buttons
                            ui.horizontal(|ui| {
                                use crate::editor::EditorTool;

                                let select_color = if editor.active_tool == EditorTool::Select {
                                    egui::Color32::LIGHT_BLUE
                                } else {
                                    egui::Color32::GRAY
                                };

                                if ui
                                    .button(egui::RichText::new("Select").color(select_color))
                                    .clicked()
                                {
                                    editor.active_tool = EditorTool::Select;
                                    ui_state.status_message = "Tool: Select".to_string();
                                }

                                let rect_color = if editor.active_tool == EditorTool::Rectangle {
                                    egui::Color32::LIGHT_BLUE
                                } else {
                                    egui::Color32::GRAY
                                };

                                if ui
                                    .button(egui::RichText::new("Rectangle").color(rect_color))
                                    .clicked()
                                {
                                    editor.active_tool = EditorTool::Rectangle;
                                    ui_state.status_message = "Tool: Rectangle".to_string();
                                }
                            });

                            ui.horizontal(|ui| {
                                use crate::editor::EditorTool;

                                let circle_color = if editor.active_tool == EditorTool::Circle {
                                    egui::Color32::LIGHT_BLUE
                                } else {
                                    egui::Color32::GRAY
                                };

                                if ui
                                    .button(egui::RichText::new("Circle").color(circle_color))
                                    .clicked()
                                {
                                    editor.active_tool = EditorTool::Circle;
                                    ui_state.status_message = "Tool: Circle".to_string();
                                }

                                let delete_color = if editor.active_tool == EditorTool::Delete {
                                    egui::Color32::RED
                                } else {
                                    egui::Color32::GRAY
                                };

                                if ui
                                    .button(egui::RichText::new("Delete").color(delete_color))
                                    .clicked()
                                {
                                    editor.active_tool = EditorTool::Delete;
                                    ui_state.status_message = "Tool: Delete".to_string();
                                }
                            });

                            ui.add_space(5.0);
                            ui.separator();

                            // Grid settings
                            ui.label(egui::RichText::new("Grid Settings").strong());
                            ui.checkbox(&mut editor.grid_snap, "Snap to Grid");

                            if editor.grid_snap {
                                ui.horizontal(|ui| {
                                    ui.label("  Grid Size:");
                                    if ui
                                        .add(
                                            egui::Slider::new(&mut editor.grid_size, 0.1..=2.0)
                                                .suffix(" m"),
                                        )
                                        .changed()
                                    {
                                        ui_state.status_message =
                                            format!("Grid size: {:.1}m", editor.grid_size);
                                    }
                                });
                            }

                            ui.add_space(5.0);
                            ui.separator();

                            // Obstacle properties (for new obstacles)
                            ui.label(egui::RichText::new("New Obstacle Color").strong());
                            ui.horizontal(|ui| {
                                let mut color = egui::Color32::from_rgb(
                                    (ui_state.editor_selected_color[0] * 255.0) as u8,
                                    (ui_state.editor_selected_color[1] * 255.0) as u8,
                                    (ui_state.editor_selected_color[2] * 255.0) as u8,
                                );
                                if ui.color_edit_button_srgba(&mut color).changed() {
                                    ui_state.editor_selected_color = [
                                        color.r() as f32 / 255.0,
                                        color.g() as f32 / 255.0,
                                        color.b() as f32 / 255.0,
                                    ];
                                    // Sync color to editor for obstacle creation
                                    editor.selected_color = ui_state.editor_selected_color;
                                }
                            });
                            // Always keep editor color in sync with UI
                            editor.selected_color = ui_state.editor_selected_color;

                            ui.add_space(5.0);
                            ui.separator();

                            // Undo/Redo
                            ui.label(egui::RichText::new("History").strong());
                            ui.horizontal(|ui| {
                                let undo_enabled = !editor.undo_stack.is_empty();
                                let redo_enabled = !editor.redo_stack.is_empty();

                                if ui
                                    .add_enabled(undo_enabled, egui::Button::new("Undo"))
                                    .clicked()
                                    && editor.undo().is_some()
                                {
                                    ui_state.status_message = "Undone".to_string();
                                }

                                if ui
                                    .add_enabled(redo_enabled, egui::Button::new("Redo"))
                                    .clicked()
                                    && editor.redo().is_some()
                                {
                                    ui_state.status_message = "Redone".to_string();
                                }
                            });

                            ui.add_space(5.0);

                            // Selection info
                            if !editor.selected_obstacles.is_empty() {
                                ui.separator();
                                ui.label(
                                    egui::RichText::new(format!(
                                        "Selected: {} obstacle(s)",
                                        editor.selected_obstacles.len()
                                    ))
                                    .color(egui::Color32::LIGHT_GREEN),
                                );

                                if ui.button("Clear Selection").clicked() {
                                    editor.clear_selection();
                                    ui_state.status_message = "Selection cleared".to_string();
                                }
                            }

                            ui.add_space(5.0);
                            ui.label(
                                egui::RichText::new("Keyboard shortcuts:")
                                    .size(11.0)
                                    .strong(),
                            );
                            ui.label(
                                egui::RichText::new("  Ctrl+Z - Undo")
                                    .size(10.0)
                                    .color(egui::Color32::from_rgb(180, 180, 180)),
                            );
                            ui.label(
                                egui::RichText::new("  Ctrl+Y - Redo")
                                    .size(10.0)
                                    .color(egui::Color32::from_rgb(180, 180, 180)),
                            );
                            ui.label(
                                egui::RichText::new("  Escape - Clear selection")
                                    .size(10.0)
                                    .color(egui::Color32::from_rgb(180, 180, 180)),
                            );
                        }
                    });

                // Performance Metrics Section
                egui::CollapsingHeader::new(egui::RichText::new("Performance Metrics").size(14.0))
                    .default_open(ui_state.show_metrics_section)
                    .show(ui, |ui| {
                        ui_state.show_metrics_section = true;

                        ui.label(
                            egui::RichText::new("Real-time performance tracking and analysis")
                                .size(11.0)
                                .color(egui::Color32::from_rgb(150, 150, 150)),
                        );
                        ui.add_space(5.0);

                        // Goal Status
                        if perf_metrics.goal_reached {
                            ui.label(
                                egui::RichText::new("Goal Reached!")
                                    .size(14.0)
                                    .color(egui::Color32::GREEN)
                                    .strong(),
                            );
                            if let Some(time) = perf_metrics.time_to_goal {
                                ui.label(format!("Time: {:.2}s", time));
                            }
                        } else if perf_metrics.goal_position.is_some() {
                            ui.label(
                                egui::RichText::new("In Progress...")
                                    .size(14.0)
                                    .color(egui::Color32::YELLOW)
                                    .strong(),
                            );
                            ui.label(format!(
                                "Distance to Goal: {:.2}m",
                                perf_metrics.distance_to_goal
                            ));
                        } else {
                            ui.label(
                                egui::RichText::new("No goal set")
                                    .size(12.0)
                                    .color(egui::Color32::GRAY),
                            );
                        }

                        ui.add_space(5.0);
                        ui.separator();

                        // Path Metrics
                        egui::CollapsingHeader::new(egui::RichText::new("Path Metrics").strong())
                            .default_open(true)
                            .show(ui, |ui| {
                                ui.label(format!("Path Length: {:.2}m", perf_metrics.path_length));
                                ui.label(format!("Avg Speed: {:.2} m/s", perf_metrics.avg_speed));
                                ui.label(format!("Max Speed: {:.2} m/s", perf_metrics.max_speed));

                                ui.add_space(3.0);
                                ui.label("Smoothness:");
                                ui.add(
                                    egui::widgets::ProgressBar::new(perf_metrics.path_smoothness)
                                        .text(format!("{:.2}", perf_metrics.path_smoothness)),
                                );
                            });

                        // Safety Metrics
                        egui::CollapsingHeader::new(egui::RichText::new("Safety Metrics").strong())
                            .default_open(true)
                            .show(ui, |ui| {
                                ui.label(format!("Collisions: {}", perf_metrics.collision_count));
                                ui.label(format!("Near Misses: {}", perf_metrics.near_miss_count));

                                ui.add_space(3.0);
                                let safety = perf_metrics.safety_score();
                                let safety_color = if safety > 0.8 {
                                    egui::Color32::GREEN
                                } else if safety > 0.5 {
                                    egui::Color32::YELLOW
                                } else {
                                    egui::Color32::RED
                                };

                                ui.colored_label(safety_color, "Safety Score:");
                                ui.add(
                                    egui::widgets::ProgressBar::new(safety)
                                        .text(format!("{:.2}", safety))
                                        .fill(safety_color),
                                );
                            });

                        // Resource Usage
                        egui::CollapsingHeader::new(egui::RichText::new("Resource Usage").strong())
                            .default_open(true)
                            .show(ui, |ui| {
                                ui.label(format!(
                                    "Elapsed Time: {:.2}s",
                                    perf_metrics.elapsed_time
                                ));
                                ui.label(format!("Energy: {:.2} J", perf_metrics.energy_consumed));
                            });

                        // Overall Scores
                        ui.add_space(5.0);
                        ui.separator();
                        ui.label(egui::RichText::new("Overall Scores").strong());

                        let efficiency = perf_metrics.efficiency_score();
                        ui.label("Efficiency:");
                        ui.add(
                            egui::widgets::ProgressBar::new(efficiency)
                                .text(format!("{:.2}", efficiency)),
                        );

                        ui.add_space(3.0);
                        let overall = perf_metrics.overall_score();
                        let overall_color = if overall > 0.8 {
                            egui::Color32::GREEN
                        } else if overall > 0.5 {
                            egui::Color32::YELLOW
                        } else {
                            egui::Color32::LIGHT_RED
                        };

                        ui.colored_label(overall_color, "Overall:");
                        ui.add(
                            egui::widgets::ProgressBar::new(overall)
                                .text(format!("{:.2}", overall))
                                .fill(overall_color),
                        );

                        // Export Options
                        ui.add_space(5.0);
                        ui.separator();
                        ui.label(egui::RichText::new("Export").strong());

                        ui.horizontal(|ui| {
                            if ui.button("Export CSV").clicked() {
                                if let Some(path) = rfd::FileDialog::new()
                                    .add_filter("CSV Files", &["csv"])
                                    .set_file_name("metrics.csv")
                                    .save_file()
                                {
                                    match perf_metrics.export_to_csv(&path) {
                                        Ok(()) => {
                                            ui_state.metrics_export_path = Some(path.clone());
                                            ui_state.status_message = format!(
                                                "Metrics exported: {}",
                                                get_file_name_str(&path)
                                            );
                                        }
                                        Err(e) => {
                                            ui_state.status_message =
                                                format!("Error exporting metrics: {}", e);
                                        }
                                    }
                                }
                            }

                            if ui.button("Export JSON").clicked() {
                                if let Some(path) = rfd::FileDialog::new()
                                    .add_filter("JSON Files", &["json"])
                                    .set_file_name("metrics.json")
                                    .save_file()
                                {
                                    match perf_metrics.export_to_json(&path) {
                                        Ok(()) => {
                                            ui_state.metrics_export_path = Some(path.clone());
                                            ui_state.status_message = format!(
                                                "Metrics exported: {}",
                                                get_file_name_str(&path)
                                            );
                                        }
                                        Err(e) => {
                                            ui_state.status_message =
                                                format!("Error exporting metrics: {}", e);
                                        }
                                    }
                                }
                            }
                        });

                        if let Some(path) = &ui_state.metrics_export_path {
                            ui.add_space(3.0);
                            ui.label(
                                egui::RichText::new(format!("Last: {}", get_file_name_str(path)))
                                    .size(10.0)
                                    .color(egui::Color32::from_rgb(120, 120, 120)),
                            );
                        }

                        // Reset button
                        ui.add_space(5.0);
                        if ui.button("Reset Metrics").clicked() {
                            perf_metrics.reset();
                            ui_state.status_message = "Metrics reset".to_string();
                        }
                    });

                // ==================== ARTICULATED ROBOTS ====================
                // Only show if there are articulated robots
                let has_articulated = !articulated_robots.is_empty();
                if has_articulated {
                    ui.add_space(10.0);
                    egui::CollapsingHeader::new(
                        egui::RichText::new("Articulated Robots").size(14.0),
                    )
                    .default_open(ui_state.show_articulated_section)
                    .show(ui, |ui| {
                        ui_state.show_articulated_section = true;

                        // Robot selector (if multiple robots exist)
                        let robot_names: Vec<String> = articulated_robots.iter().map(|r| r.name.clone()).collect();
                        if robot_names.len() > 1 {
                            ui.horizontal(|ui| {
                                ui.label("Select Robot:");
                                egui::ComboBox::from_id_salt("robot_selector")
                                    .selected_text(
                                        ui_state.selected_articulated_robot
                                            .as_ref()
                                            .unwrap_or(&robot_names[0])
                                    )
                                    .show_ui(ui, |ui| {
                                        for name in &robot_names {
                                            ui.selectable_value(
                                                &mut ui_state.selected_articulated_robot,
                                                Some(name.clone()),
                                                name,
                                            );
                                        }
                                    });
                            });
                            ui.add_space(5.0);
                        }

                        // Joint Control Mode selector
                        ui.horizontal(|ui| {
                            ui.label("Control Mode:");
                            egui::ComboBox::from_id_salt("joint_control_mode")
                                .selected_text(format!("{:?}", ui_state.joint_control_mode))
                                .show_ui(ui, |ui| {
                                    ui.selectable_value(
                                        &mut ui_state.joint_control_mode,
                                        crate::joint::JointControlMode::Position,
                                        "Position",
                                    );
                                    ui.selectable_value(
                                        &mut ui_state.joint_control_mode,
                                        crate::joint::JointControlMode::Velocity,
                                        "Velocity",
                                    );
                                    ui.selectable_value(
                                        &mut ui_state.joint_control_mode,
                                        crate::joint::JointControlMode::Effort,
                                        "Effort",
                                    );
                                });
                        });

                        // Visual options
                        ui.horizontal(|ui| {
                            ui.checkbox(&mut ui_state.show_joint_markers, "Show Joint Markers");
                        });

                        ui.add_space(5.0);
                        ui.separator();

                        // Filter to selected robot or show all
                        let selected_robot_name = ui_state.selected_articulated_robot.clone();

                        // List each articulated robot
                        for robot in articulated_robots.iter() {
                            // Skip if not the selected robot (when multiple exist)
                            if robot_names.len() > 1 {
                                if let Some(ref selected) = selected_robot_name {
                                    if &robot.name != selected {
                                        continue;
                                    }
                                }
                            }
                            ui.add_space(5.0);
                            ui.label(
                                egui::RichText::new(&robot.name)
                                    .strong()
                                    .color(egui::Color32::from_rgb(100, 180, 255)),
                            );
                            ui.label(
                                egui::RichText::new(format!(
                                    "{} links, {} joints",
                                    robot.config.links.len(),
                                    robot.config.joints.len()
                                ))
                                .size(11.0)
                                .color(egui::Color32::from_rgb(150, 150, 150)),
                            );

                            // Joint sliders - behavior depends on control mode
                            let control_mode = ui_state.joint_control_mode;
                            let (slider_unit, slider_range_default) = match control_mode {
                                crate::joint::JointControlMode::Position => ("rad", (-std::f32::consts::PI, std::f32::consts::PI)),
                                crate::joint::JointControlMode::Velocity => ("rad/s", (-5.0, 5.0)),
                                crate::joint::JointControlMode::Effort => ("Nm", (-50.0, 50.0)),
                            };

                            for joint_cfg in &robot.config.joints {
                                let joint_name = &joint_cfg.name;
                                let current_pos = robot
                                    .joint_positions
                                    .get(joint_name)
                                    .copied()
                                    .unwrap_or(0.0);

                                // Get limits from joint config (for position mode)
                                let [pos_min, pos_max] = match &joint_cfg.joint_type {
                                    crate::joint::Joint2DType::Revolute { limits, .. } => limits
                                        .unwrap_or([-std::f32::consts::PI, std::f32::consts::PI]),
                                    crate::joint::Joint2DType::Prismatic { limits, .. } => {
                                        limits.unwrap_or([-1.0, 1.0])
                                    }
                                    crate::joint::Joint2DType::Fixed => [0.0, 0.0],
                                };

                                // Use appropriate range based on control mode
                                let (min, max) = match control_mode {
                                    crate::joint::JointControlMode::Position => (pos_min, pos_max),
                                    _ => slider_range_default,
                                };

                                // Only show slider for non-fixed joints
                                if !matches!(joint_cfg.joint_type, crate::joint::Joint2DType::Fixed)
                                {
                                    ui.horizontal(|ui| {
                                        ui.label(
                                            egui::RichText::new(joint_name)
                                                .size(11.0)
                                                .color(egui::Color32::from_rgb(180, 180, 180)),
                                        );

                                        // Get or initialize slider value based on mode
                                        let slider_key = format!("{}:{}:{:?}", robot.name, joint_name, control_mode);
                                        let default_val = match control_mode {
                                            crate::joint::JointControlMode::Position => current_pos,
                                            _ => 0.0,
                                        };
                                        let slider_val = ui_state
                                            .joint_sliders
                                            .entry(slider_key)
                                            .or_insert(default_val);

                                        let response = ui.add(
                                            egui::Slider::new(slider_val, min..=max)
                                                .text(slider_unit)
                                                .step_by(0.01),
                                        );

                                        // If slider changed, update motor based on control mode
                                        if response.changed() {
                                            if let Some(joint_handle) =
                                                robot.joint_handles.get(joint_name)
                                            {
                                                if let Some(joint) = physics_world
                                                    .impulse_joint_set
                                                    .get_mut(*joint_handle)
                                                {
                                                    match control_mode {
                                                        crate::joint::JointControlMode::Position => {
                                                            // Position control with PD controller
                                                            joint.data.set_motor_position(
                                                                rapier2d::dynamics::JointAxis::AngX,
                                                                *slider_val,
                                                                100.0, // stiffness
                                                                10.0,  // damping
                                                            );
                                                        }
                                                        crate::joint::JointControlMode::Velocity => {
                                                            // Velocity control
                                                            joint.data.set_motor_velocity(
                                                                rapier2d::dynamics::JointAxis::AngX,
                                                                *slider_val,
                                                                10.0, // damping factor
                                                            );
                                                        }
                                                        crate::joint::JointControlMode::Effort => {
                                                            // Direct effort/torque control
                                                            joint.data.set_motor_max_force(
                                                                rapier2d::dynamics::JointAxis::AngX,
                                                                slider_val.abs(),
                                                            );
                                                            // Set velocity direction based on sign
                                                            let dir = if *slider_val >= 0.0 { 1000.0 } else { -1000.0 };
                                                            joint.data.set_motor_velocity(
                                                                rapier2d::dynamics::JointAxis::AngX,
                                                                dir,
                                                                0.0,
                                                            );
                                                        }
                                                    }
                                                }
                                            }
                                        }
                                    });
                                }
                            }

                            // Joint state display
                            ui.add_space(3.0);
                            egui::CollapsingHeader::new(
                                egui::RichText::new("Joint States").size(11.0),
                            )
                            .default_open(false)
                            .show(ui, |ui| {
                                for (joint_name, pos) in &robot.joint_positions {
                                    let vel = robot
                                        .joint_velocities
                                        .get(joint_name)
                                        .copied()
                                        .unwrap_or(0.0);
                                    ui.label(
                                        egui::RichText::new(format!(
                                            "{}: {:.2} rad ({:.2} rad/s)",
                                            joint_name, pos, vel
                                        ))
                                        .size(10.0)
                                        .color(egui::Color32::from_rgb(140, 140, 140)),
                                    );
                                }
                            });

                            ui.separator();
                        }

                        // Control buttons
                        ui.add_space(5.0);
                        ui.horizontal(|ui| {
                            if ui.button("Zero All").clicked() {
                                // Reset all slider values to 0
                                for key in
                                    ui_state.joint_sliders.keys().cloned().collect::<Vec<_>>()
                                {
                                    ui_state.joint_sliders.insert(key, 0.0);
                                }
                                // Apply to physics
                                for robot in articulated_robots.iter() {
                                    for joint_handle in robot.joint_handles.values() {
                                        if let Some(joint) =
                                            physics_world.impulse_joint_set.get_mut(*joint_handle)
                                        {
                                            joint.data.set_motor_position(
                                                rapier2d::dynamics::JointAxis::AngX,
                                                0.0,
                                                100.0,
                                                10.0,
                                            );
                                        }
                                    }
                                }
                                ui_state.status_message = "All joints zeroed".to_string();
                            }

                            if ui.button("Limp").clicked() {
                                // Set all motor forces to 0 (disable motors)
                                for robot in articulated_robots.iter() {
                                    for joint_handle in robot.joint_handles.values() {
                                        if let Some(joint) =
                                            physics_world.impulse_joint_set.get_mut(*joint_handle)
                                        {
                                            joint.data.set_motor_max_force(
                                                rapier2d::dynamics::JointAxis::AngX,
                                                0.0,
                                            );
                                        }
                                    }
                                }
                                ui_state.status_message = "Motors disabled (limp mode)".to_string();
                            }
                        });
                    });
                }
            });

            // Status Bar - Outside scroll area
            ui.add_space(5.0);
            ui.separator();

            // Enhanced status bar with simulation info
            ui.horizontal(|ui| {
                // Status message
                let status_color = if ui_state.status_message.contains("Error") {
                    egui::Color32::from_rgb(220, 80, 80)
                } else if ui_state.status_message.contains("saved")
                    || ui_state.status_message.contains("loaded")
                {
                    egui::Color32::from_rgb(80, 180, 80)
                } else {
                    egui::Color32::from_rgb(180, 180, 180)
                };
                ui.label(
                    egui::RichText::new(&ui_state.status_message)
                        .size(11.0)
                        .color(status_color),
                );
            });

            ui.add_space(2.0);

            // Quick info line
            ui.horizontal(|ui| {
                ui.spacing_mut().item_spacing.x = 15.0;
                ui.label(
                    egui::RichText::new(format!(
                        "World: {:.0}x{:.0}m",
                        app_config.world_config.width, app_config.world_config.height
                    ))
                    .size(10.0)
                    .color(egui::Color32::from_rgb(120, 120, 120)),
                );
                ui.label(
                    egui::RichText::new(format!(
                        "Obstacles: {}",
                        app_config.world_config.obstacles.len()
                    ))
                    .size(10.0)
                    .color(egui::Color32::from_rgb(120, 120, 120)),
                );
                ui.label(
                    egui::RichText::new(format!("Robots: {}", app_config.robots.len()))
                        .size(10.0)
                        .color(egui::Color32::from_rgb(120, 120, 120)),
                );
            });
        });

    // Help Dialog Window
    if ui_state.show_help {
        egui::Window::new("Help & Controls")
            .collapsible(false)
            .resizable(false)
            .default_width(400.0)
            .show(contexts.ctx_mut(), |ui| {
                ui.heading("Keyboard Shortcuts");
                ui.add_space(10.0);

                ui.label(egui::RichText::new("Camera Controls").strong());
                ui.label("  Middle Mouse + Drag - Pan camera");
                ui.label("  Scroll Wheel - Zoom in/out");
                ui.add_space(5.0);

                ui.label(egui::RichText::new("Simulation").strong());
                ui.label("  Space - Toggle pause/play");
                ui.label("  R - Reset simulation");
                ui.add_space(5.0);

                ui.label(egui::RichText::new("Scenarios").strong());
                ui.label("  Ctrl+S - Save scenario");
                ui.label("  Ctrl+O - Open/load scenario");
                ui.add_space(5.0);

                ui.label(egui::RichText::new("Topics & Commands").strong());
                ui.label(format!("  Listening on: {}", app_config.args.topic));
                ui.label("  Send CmdVel messages to control robot");
                ui.add_space(5.0);

                ui.label(egui::RichText::new("Tips").strong());
                ui.label("  - Adjust simulation speed with slider");
                ui.label("  - Load custom robot/world configs");
                ui.label("  - Toggle visual elements in Visuals panel");
                ui.add_space(10.0);

                ui.separator();
                ui.horizontal(|ui| {
                    if ui.button("Close").clicked() {
                        ui_state.show_help = false;
                    }
                });
            });
    }
}

/// System to handle file dialog actions
pub fn file_dialog_system(
    mut ui_state: ResMut<UiState>,
    mut app_config: ResMut<AppConfig>,
    mut recorder: ResMut<Recorder>,
) {
    match ui_state.show_file_dialog {
        FileDialogType::RobotConfig => {
            let mut dialog = rfd::FileDialog::new()
                .add_filter("Config Files", &["yaml", "yml", "toml"])
                .add_filter("YAML Files", &["yaml", "yml"])
                .add_filter("TOML Files", &["toml"])
                .set_title("Load Robot Configuration (YAML/TOML only)");

            // Try to set default directory to configs folder
            if let Ok(exe_path) = std::env::current_exe() {
                if let Some(exe_dir) = exe_path.parent() {
                    let configs_path = exe_dir.join("../../../horus_library/tools/sim2d/configs");
                    if configs_path.exists() {
                        dialog = dialog.set_directory(&configs_path);
                    }
                }
            }

            if let Some(path) = dialog.pick_file() {
                match AppConfig::load_robot_config(path.to_str().unwrap()) {
                    Ok(config) => {
                        // Update first robot or create new robots list with this config
                        if app_config.robots.is_empty() {
                            app_config.robots = vec![config];
                        } else {
                            app_config.robots[0] = config;
                        }
                        ui_state.robot_config_path = Some(path);
                        ui_state.status_message =
                            "Robot config loaded! Changes applied.".to_string();
                        info!(" Loaded robot config - changes applied live");
                    }
                    Err(e) => {
                        ui_state.status_message = format!("Error: {}", e);
                        warn!(" Failed to load robot config: {}", e);
                    }
                }
            }
            ui_state.show_file_dialog = FileDialogType::None;
        }
        FileDialogType::WorldConfig => {
            let mut dialog = rfd::FileDialog::new()
                .add_filter(
                    "All Supported Files",
                    &["yaml", "yml", "toml", "png", "jpg", "jpeg", "pgm"],
                )
                .add_filter(
                    "Image Files (PNG, JPG, PGM)",
                    &["png", "jpg", "jpeg", "pgm"],
                )
                .add_filter("Config Files (YAML, TOML)", &["yaml", "yml", "toml"])
                .set_title("Load World Configuration or Image");

            // Try to set default directory to configs folder
            if let Ok(exe_path) = std::env::current_exe() {
                if let Some(exe_dir) = exe_path.parent() {
                    let configs_path = exe_dir.join("../../../horus_library/tools/sim2d/configs");
                    if configs_path.exists() {
                        dialog = dialog.set_directory(&configs_path);
                    }
                }
            }

            if let Some(path) = dialog.pick_file() {
                let path_str = path.to_str().unwrap();

                // Check if it's an image file
                if path_str.ends_with(".png")
                    || path_str.ends_with(".jpg")
                    || path_str.ends_with(".jpeg")
                    || path_str.ends_with(".pgm")
                {
                    // Load from image with default resolution and threshold
                    match AppConfig::load_world_from_image(path_str, 0.05, 128) {
                        Ok(config) => {
                            app_config.world_config = config;
                            ui_state.world_config_path = Some(path);
                            ui_state.status_message =
                                "World loaded from image! Reloading...".to_string();
                            info!(" Loaded world from image - reloading world");
                        }
                        Err(e) => {
                            ui_state.status_message = format!("Error: {}", e);
                            warn!(" Failed to load world from image: {}", e);
                        }
                    }
                } else {
                    // Load from config file
                    match AppConfig::load_world_config(path_str) {
                        Ok(config) => {
                            app_config.world_config = config;
                            ui_state.world_config_path = Some(path);
                            ui_state.status_message =
                                "World config loaded! Reloading...".to_string();
                            info!(" Loaded world config - reloading world");
                        }
                        Err(e) => {
                            ui_state.status_message = format!("Error: {}", e);
                            warn!(" Failed to load world config: {}", e);
                        }
                    }
                }
            }
            ui_state.show_file_dialog = FileDialogType::None;
        }
        FileDialogType::ScenarioSave => {
            let mut dialog = rfd::FileDialog::new()
                .add_filter("Scenario Files", &["yaml", "yml"])
                .set_title("Save Scenario");

            // Try to set default directory to scenarios folder
            if let Ok(exe_path) = std::env::current_exe() {
                if let Some(exe_dir) = exe_path.parent() {
                    let scenarios_path =
                        exe_dir.join("../../../horus_library/tools/sim2d/scenarios");
                    if scenarios_path.exists() {
                        dialog = dialog.set_directory(&scenarios_path);
                    }
                }
            }

            // Set default filename if we have a current scenario
            if let Some(current_path) = &ui_state.scenario_path {
                if let Some(filename) = current_path.file_name() {
                    dialog = dialog.set_file_name(filename.to_string_lossy().as_ref());
                }
            } else {
                dialog = dialog.set_file_name("my_scenario.yaml");
            }

            if let Some(path) = dialog.save_file() {
                // Create scenario from current state
                let scenario = Scenario::from_current_state(
                    path.file_stem()
                        .unwrap_or_default()
                        .to_string_lossy()
                        .to_string(),
                    "Saved from sim2d".to_string(),
                    &app_config.world_config,
                    &app_config.robots,
                    0.0, // Current time - would need to be tracked
                );

                match scenario.save_to_file(&path) {
                    Ok(()) => {
                        ui_state.scenario_path = Some(path.clone());
                        ui_state.status_message =
                            format!("Scenario saved: {}", get_file_name_str(&path));
                        info!("Saved scenario to {:?}", path);
                    }
                    Err(e) => {
                        ui_state.status_message = format!("Error saving scenario: {}", e);
                        warn!("Failed to save scenario: {}", e);
                    }
                }
            }
            ui_state.show_file_dialog = FileDialogType::None;
        }
        FileDialogType::ScenarioLoad => {
            let mut dialog = rfd::FileDialog::new()
                .add_filter("Scenario Files", &["yaml", "yml"])
                .set_title("Load Scenario");

            // Try to set default directory to scenarios folder
            if let Ok(exe_path) = std::env::current_exe() {
                if let Some(exe_dir) = exe_path.parent() {
                    let scenarios_path =
                        exe_dir.join("../../../horus_library/tools/sim2d/scenarios");
                    if scenarios_path.exists() {
                        dialog = dialog.set_directory(&scenarios_path);
                    }
                }
            }

            if let Some(path) = dialog.pick_file() {
                match Scenario::load_from_file(&path) {
                    Ok(scenario) => {
                        // Apply scenario to current configuration
                        app_config.world_config = scenario.to_world_config();
                        app_config.robots = scenario.to_robot_configs();

                        ui_state.scenario_path = Some(path.clone());
                        ui_state.status_message =
                            format!("Scenario loaded: {}", get_file_name_str(&path));
                        info!("Loaded scenario from {:?}", path);
                    }
                    Err(e) => {
                        ui_state.status_message = format!("Error loading scenario: {}", e);
                        warn!("Failed to load scenario: {}", e);
                    }
                }
            }
            ui_state.show_file_dialog = FileDialogType::None;
        }
        FileDialogType::RecordingLoad => {
            let dialog = rfd::FileDialog::new()
                .add_filter("Recording Files", &["yaml", "yml"])
                .set_title("Load Recording");

            if let Some(path) = dialog.pick_file() {
                match crate::recorder::Recording::load_from_file(&path) {
                    Ok(recording) => {
                        let frame_count = recording.metadata.frame_count;
                        ui_state.recording_path = Some(path.clone());
                        ui_state.status_message = format!(
                            "Recording loaded: {} ({} frames) - Press Play to start",
                            get_file_name_str(&path),
                            frame_count
                        );
                        info!("Loaded recording from {:?}", path);
                        // Start playback
                        recorder.start_playback(recording);
                        recorder.playback.pause(); // Start paused so user can press play
                    }
                    Err(e) => {
                        ui_state.status_message = format!("Error loading recording: {}", e);
                        warn!("Failed to load recording: {}", e);
                    }
                }
            }
            ui_state.show_file_dialog = FileDialogType::None;
        }
        FileDialogType::ExportCSV => {
            // Check if there's a recording to export
            if let Some(recording) = recorder.get_recording() {
                let dialog = rfd::FileDialog::new()
                    .add_filter("CSV Files", &["csv"])
                    .set_title("Export Recording to CSV")
                    .set_file_name("recording.csv");

                if let Some(path) = dialog.save_file() {
                    match recording.export_to_csv(&path) {
                        Ok(()) => {
                            ui_state.status_message =
                                format!("Exported to CSV: {}", get_file_name_str(&path));
                            info!("Exported recording to CSV: {:?}", path);
                        }
                        Err(e) => {
                            ui_state.status_message = format!("Error exporting to CSV: {}", e);
                            warn!("Failed to export to CSV: {}", e);
                        }
                    }
                }
            } else {
                ui_state.status_message = "No recording to export".to_string();
            }
            ui_state.show_file_dialog = FileDialogType::None;
        }
        FileDialogType::ExportVideo => {
            // Check if there's a recording to export
            if let Some(recording) = recorder.get_recording() {
                let dialog = rfd::FileDialog::new()
                    .add_filter("MP4 Video", &["mp4"])
                    .set_title("Export Recording to Video")
                    .set_file_name("recording.mp4");

                if let Some(path) = dialog.save_file() {
                    // Use a temp directory for frame images
                    match std::env::temp_dir().canonicalize() {
                        Ok(temp_dir) => {
                            ui_state.status_message =
                                "Exporting video (this may take a while)...".to_string();
                            match recording.export_to_video(&path, &temp_dir) {
                                Ok(()) => {
                                    ui_state.status_message =
                                        format!("Exported to video: {}", get_file_name_str(&path));
                                    info!("Exported recording to video: {:?}", path);
                                }
                                Err(e) => {
                                    ui_state.status_message =
                                        format!("Error exporting to video: {}", e);
                                    warn!("Failed to export to video: {}", e);
                                }
                            }
                        }
                        Err(e) => {
                            ui_state.status_message =
                                format!("Error accessing temp directory: {}", e);
                        }
                    }
                }
            } else {
                ui_state.status_message = "No recording to export".to_string();
            }
            ui_state.show_file_dialog = FileDialogType::None;
        }
        FileDialogType::None => {}
    }
}
