diff --git a/scripts/run_simulation.sh b/scripts/run_simulation.sh
index 206ba28..72b3191 100755
--- a/scripts/run_simulation.sh
+++ b/scripts/run_simulation.sh
@@ -1,6 +1,14 @@
 #!/bin/bash
-# Full simulation launch script with ArduPilot SITL
-# This script launches Gazebo, ArduPilot SITL, and MAVROS
+# =============================================================================
+# UAV-UGV Simulation - Run Script
+# =============================================================================
+# Launches Gazebo with ArduPilot SITL for GPS-denied navigation testing
+#
+# Usage:
+#   ./scripts/run_simulation.sh                     # Default world
+#   ./scripts/run_simulation.sh --world runway      # Runway world
+#   ./scripts/run_simulation.sh --software-render   # For WSL/no GPU
+# =============================================================================
 
 set -e
 
@@ -28,51 +36,32 @@ fi
 
 # Detect ROS distro
 ROS_DISTRO=""
-for distro in humble jazzy iron galactic; do
+for distro in jazzy humble iron galactic; do
     if [ -d "/opt/ros/$distro" ]; then
         ROS_DISTRO="$distro"
         break
     fi
 done
 
-if [ -z "$ROS_DISTRO" ]; then
-    echo -e "${RED}ERROR: No ROS 2 installation found!${NC}"
-    echo "Please install ROS 2 first. See docs/setup_guide.md"
-    exit 1
+if [ -n "$ROS_DISTRO" ]; then
+    source /opt/ros/${ROS_DISTRO}/setup.bash
+    echo -e "${GREEN}ROS 2: $ROS_DISTRO${NC}"
 fi
 
-echo -e "${GREEN}Using ROS 2: $ROS_DISTRO${NC}"
-
-# Check ArduPilot installation
+# ArduPilot paths
 ARDUPILOT_HOME="${ARDUPILOT_HOME:-$HOME/ardupilot}"
-if [ ! -d "$ARDUPILOT_HOME" ]; then
-    echo -e "${RED}ERROR: ArduPilot not found at $ARDUPILOT_HOME${NC}"
-    echo ""
-    echo "Please install ArduPilot first:"
-    echo "  bash scripts/install_ardupilot.sh"
-    echo ""
-    exit 1
-fi
-
-# Source environments
-source /opt/ros/${ROS_DISTRO}/setup.bash
-
-if [ -f /usr/share/gazebo/setup.bash ]; then
-    source /usr/share/gazebo/setup.bash
-elif [ -f /usr/share/gazebo-11/setup.bash ]; then
-    source /usr/share/gazebo-11/setup.bash
-fi
+ARDUPILOT_GZ="${ARDUPILOT_GZ:-$HOME/ardupilot_gazebo}"
 
 # Parse arguments
-WORLD="$PROJECT_DIR/worlds/iris_runway.world"
-VEHICLE="copter"
+WORLD_NAME="iris_runway"
+VEHICLE="ArduCopter"
 USE_SOFTWARE_RENDER=false
 INSTANCE=0
 
 while [[ $# -gt 0 ]]; do
     case $1 in
         --world)
-            WORLD="$2"
+            WORLD_NAME="$2"
             shift 2
             ;;
         --vehicle)
@@ -91,40 +80,58 @@ while [[ $# -gt 0 ]]; do
             echo "Usage: $0 [OPTIONS]"
             echo ""
             echo "Options:"
-            echo "  --world FILE        World file (default: iris_runway.world)"
-            echo "  --vehicle TYPE      Vehicle type: copter, rover (default: copter)"
+            echo "  --world NAME        World name (default: iris_runway)"
+            echo "  --vehicle TYPE      ArduCopter or Rover (default: ArduCopter)"
             echo "  --software-render   Force software rendering (for WSL)"
-            echo "  --instance N        Vehicle instance number (default: 0)"
+            echo "  --instance N        SITL instance number (default: 0)"
             echo "  --help              Show this help"
+            echo ""
+            echo "Available worlds in ardupilot_gazebo:"
+            ls "$ARDUPILOT_GZ/worlds/"*.sdf 2>/dev/null | xargs -I {} basename {} .sdf || echo "  (none found)"
             exit 0
             ;;
         *)
-            if [ -f "$1" ]; then
-                WORLD="$1"
-            elif [ -f "$PROJECT_DIR/worlds/$1" ]; then
-                WORLD="$PROJECT_DIR/worlds/$1"
-            fi
+            WORLD_NAME="$1"
             shift
             ;;
     esac
 done
 
 # Setup Gazebo paths
-export GAZEBO_MODEL_PATH="$PROJECT_DIR/models:${GAZEBO_MODEL_PATH:-}"
-export GAZEBO_RESOURCE_PATH="$PROJECT_DIR/worlds:${GAZEBO_RESOURCE_PATH:-}"
+export GZ_SIM_RESOURCE_PATH="$PROJECT_DIR/models:$PROJECT_DIR/worlds:${GZ_SIM_RESOURCE_PATH:-}"
 
-if [ -d "$HOME/ardupilot_gazebo" ]; then
-    export GAZEBO_MODEL_PATH="$HOME/ardupilot_gazebo/models:$GAZEBO_MODEL_PATH"
-    export GAZEBO_RESOURCE_PATH="$HOME/ardupilot_gazebo/worlds:$GAZEBO_RESOURCE_PATH"
+if [ -d "$ARDUPILOT_GZ" ]; then
+    export GZ_SIM_SYSTEM_PLUGIN_PATH="$ARDUPILOT_GZ/build:${GZ_SIM_SYSTEM_PLUGIN_PATH:-}"
+    export GZ_SIM_RESOURCE_PATH="$ARDUPILOT_GZ/models:$ARDUPILOT_GZ/worlds:$GZ_SIM_RESOURCE_PATH"
+fi
+
+# Find world file
+WORLD_FILE=""
+if [ -f "$PROJECT_DIR/worlds/${WORLD_NAME}.sdf" ]; then
+    WORLD_FILE="$PROJECT_DIR/worlds/${WORLD_NAME}.sdf"
+elif [ -f "$ARDUPILOT_GZ/worlds/${WORLD_NAME}.sdf" ]; then
+    WORLD_FILE="$ARDUPILOT_GZ/worlds/${WORLD_NAME}.sdf"
+elif [ -f "$WORLD_NAME" ]; then
+    WORLD_FILE="$WORLD_NAME"
+else
+    echo -e "${RED}ERROR: World file not found: $WORLD_NAME${NC}"
+    echo ""
+    echo "Available worlds:"
+    echo "  Project worlds:"
+    ls "$PROJECT_DIR/worlds/"*.sdf 2>/dev/null | xargs -I {} basename {} .sdf || echo "    (none)"
+    echo "  ArduPilot Gazebo worlds:"
+    ls "$ARDUPILOT_GZ/worlds/"*.sdf 2>/dev/null | xargs -I {} basename {} .sdf || echo "    (none)"
+    exit 1
 fi
 
 # WSL/Display setup
 if $IS_WSL; then
-    if [ -f "$PROJECT_DIR/wsl_env.sh" ]; then
-        source "$PROJECT_DIR/wsl_env.sh"
-    fi
     if [ -z "$DISPLAY" ]; then
-        export DISPLAY=:0
+        if [ -d "/mnt/wslg" ]; then
+            export DISPLAY=:0
+        else
+            export DISPLAY=$(cat /etc/resolv.conf | grep nameserver | awk '{print $2}'):0
+        fi
     fi
     echo -e "${BLUE}DISPLAY: $DISPLAY${NC}"
 fi
@@ -140,15 +147,12 @@ cleanup() {
     echo ""
     echo -e "${YELLOW}Shutting down simulation...${NC}"
     
-    # Kill processes
     pkill -f "sim_vehicle.py" 2>/dev/null || true
     pkill -f "mavproxy" 2>/dev/null || true
-    pkill -f "ArduCopter" 2>/dev/null || true
-    pkill -f "ArduRover" 2>/dev/null || true
-    pkill -f "gzserver" 2>/dev/null || true
-    pkill -f "gzclient" 2>/dev/null || true
-    pkill -f "gazebo" 2>/dev/null || true
-    pkill -f "mavros" 2>/dev/null || true
+    pkill -f "arducopter" 2>/dev/null || true
+    pkill -f "ardurover" 2>/dev/null || true
+    pkill -f "ruby" 2>/dev/null || true  # gz sim uses ruby
+    pkill -f "gz sim" 2>/dev/null || true
     
     sleep 2
     echo -e "${GREEN}Cleanup complete.${NC}"
@@ -156,125 +160,78 @@ cleanup() {
 
 trap cleanup EXIT INT TERM
 
-# Activate virtual environment
-if [ -f "$PROJECT_DIR/venv/bin/activate" ]; then
-    source "$PROJECT_DIR/venv/bin/activate"
+# Check ArduPilot installation
+if [ ! -d "$ARDUPILOT_HOME" ]; then
+    echo -e "${RED}ERROR: ArduPilot not found at $ARDUPILOT_HOME${NC}"
+    echo "Run setup.sh first to install ArduPilot"
+    exit 1
 fi
 
-# Use ardupilot_gazebo iris world if no custom world specified
-if [ ! -f "$WORLD" ]; then
-    if [ -f "$HOME/ardupilot_gazebo/worlds/iris_arducopter_runway.world" ]; then
-        WORLD="$HOME/ardupilot_gazebo/worlds/iris_arducopter_runway.world"
-    else
-        echo -e "${RED}ERROR: World file not found: $WORLD${NC}"
-        exit 1
-    fi
+# Check Gazebo plugin
+if [ ! -f "$ARDUPILOT_GZ/build/libArduPilotPlugin.so" ]; then
+    echo -e "${RED}ERROR: ArduPilot Gazebo plugin not found${NC}"
+    echo "Run setup.sh first to build the plugin"
+    exit 1
 fi
 
 echo ""
 echo -e "${GREEN}Configuration:${NC}"
 echo "  Vehicle: $VEHICLE"
-echo "  World: $(basename $WORLD)"
+echo "  World: $(basename $WORLD_FILE .sdf)"
 echo "  Instance: $INSTANCE"
 echo ""
 
-# Calculate ports
-MAVLINK_PORT=$((14550 + INSTANCE * 10))
-SITL_PORT=$((5760 + INSTANCE * 10))
-
 # Start Gazebo in background
 echo -e "${GREEN}Starting Gazebo...${NC}"
-gazebo --verbose "$WORLD" &
-GAZEBO_PID=$!
+gz sim -v4 -r "$WORLD_FILE" &
+GZ_PID=$!
 
 sleep 5
 
 # Check if Gazebo started
-if ! kill -0 $GAZEBO_PID 2>/dev/null; then
+if ! kill -0 $GZ_PID 2>/dev/null; then
     echo -e "${RED}ERROR: Gazebo failed to start${NC}"
     exit 1
 fi
 
-echo -e "${GREEN}Gazebo running (PID: $GAZEBO_PID)${NC}"
+echo -e "${GREEN}Gazebo running (PID: $GZ_PID)${NC}"
 
 # Start ArduPilot SITL
 echo -e "${GREEN}Starting ArduPilot SITL...${NC}"
 cd "$ARDUPILOT_HOME"
 
-if [ "$VEHICLE" = "copter" ]; then
-    VEHICLE_DIR="ArduCopter"
-    FRAME="gazebo-iris"
-elif [ "$VEHICLE" = "rover" ]; then
-    VEHICLE_DIR="Rover"
-    FRAME="gazebo-rover"
-else
-    VEHICLE_DIR="ArduCopter"
-    FRAME="gazebo-iris"
-fi
+export PATH=$PATH:$ARDUPILOT_HOME/Tools/autotest:$HOME/.local/bin
 
-cd "$ARDUPILOT_HOME/$VEHICLE_DIR"
-
-# Start sim_vehicle.py in background
-python3 "$ARDUPILOT_HOME/Tools/autotest/sim_vehicle.py" \
-    -v "$VEHICLE_DIR" \
-    -f "$FRAME" \
-    --no-mavproxy \
-    -I "$INSTANCE" \
-    --no-rebuild \
+sim_vehicle.py \
+    -v $VEHICLE \
+    -f gazebo-iris \
+    --model JSON \
+    -I $INSTANCE \
+    --console \
     &
 SITL_PID=$!
 
 sleep 10
 
-# Check if SITL started
-if ! kill -0 $SITL_PID 2>/dev/null; then
-    echo -e "${RED}ERROR: ArduPilot SITL failed to start${NC}"
-    exit 1
-fi
-
-echo -e "${GREEN}ArduPilot SITL running (PID: $SITL_PID)${NC}"
-
-# Start MAVROS
-echo -e "${GREEN}Starting MAVROS...${NC}"
-ros2 run mavros mavros_node --ros-args \
-    -p fcu_url:="udp://127.0.0.1:${MAVLINK_PORT}@" \
-    -p gcs_url:="" \
-    -p target_system_id:=1 \
-    -p target_component_id:=1 \
-    &>/dev/null &
-MAVROS_PID=$!
-
-sleep 5
-
 echo ""
 echo -e "${GREEN}==========================================${NC}"
 echo -e "${GREEN}  Simulation Running${NC}"
 echo -e "${GREEN}==========================================${NC}"
 echo ""
 echo -e "${BLUE}Components:${NC}"
-echo "  - Gazebo: PID $GAZEBO_PID"
+echo "  - Gazebo: PID $GZ_PID"
 echo "  - ArduPilot SITL: PID $SITL_PID"
-echo "  - MAVROS: PID $MAVROS_PID"
 echo ""
-echo -e "${BLUE}MAVROS Topics:${NC}"
-echo "  ros2 topic list | grep mavros"
-echo ""
-echo -e "${BLUE}Control UAV:${NC}"
-echo "  # Arm"
-echo "  ros2 service call /mavros/cmd/arming mavros_msgs/srv/CommandBool \"{value: true}\""
-echo ""
-echo "  # Set mode to GUIDED"
-echo "  ros2 service call /mavros/set_mode mavros_msgs/srv/SetMode \"{base_mode: 0, custom_mode: 'GUIDED'}\""
-echo ""
-echo "  # Takeoff to 5 meters"
-echo "  ros2 service call /mavros/cmd/takeoff mavros_msgs/srv/CommandTOL \"{altitude: 5}\""
-echo ""
-echo "  # Send position setpoint (x, y, z in meters)"
-echo "  ros2 topic pub /mavros/setpoint_position/local geometry_msgs/PoseStamped \\"
-echo "    \"{header: {frame_id: 'map'}, pose: {position: {x: 10, y: 5, z: 5}}}\""
+echo -e "${BLUE}MAVProxy Commands (in console):${NC}"
+echo "  mode guided      # Switch to GUIDED mode"
+echo "  arm throttle     # Arm the drone"
+echo "  takeoff 5        # Takeoff to 5 meters"
+echo "  guided 10 5 -10  # Fly to position (N, E, D)"
+echo "  rtl              # Return to launch"
+echo "  land             # Land"
 echo ""
 echo -e "${YELLOW}Press Ctrl+C to stop simulation${NC}"
 echo ""
 
-# Wait for all processes
+# Wait for processes
 wait
diff --git a/setup.sh b/setup.sh
index e4cfd8b..45554ce 100755
--- a/setup.sh
+++ b/setup.sh
@@ -1,7 +1,14 @@
 #!/bin/bash
+# =============================================================================
 # UAV-UGV Simulation - Complete Installation Script
+# =============================================================================
 # Installs everything needed for GPS-denied navigation simulation
 # Compatible with Ubuntu 22.04/24.04 and WSL2
+#
+# Usage:
+#   ./setup.sh                    # Full installation
+#   ./setup.sh --skip-ardupilot   # Skip ArduPilot (Gazebo only)
+# =============================================================================
 
 set -e
 
@@ -36,6 +43,14 @@ print_error() {
     echo -e "${RED}ERROR: $1${NC}"
 }
 
+# Get script directory
+SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
+cd "$SCRIPT_DIR"
+
+# ArduPilot directories
+ARDUPILOT_HOME="$HOME/ardupilot"
+ARDUPILOT_GZ="$HOME/ardupilot_gazebo"
+
 # Detect environment
 detect_environment() {
     IS_WSL=false
@@ -43,7 +58,7 @@ detect_environment() {
     
     if grep -qEi "(microsoft|wsl)" /proc/version 2>/dev/null; then
         IS_WSL=true
-        if [ -f /run/WSL ] || grep -qi "wsl2" /proc/version 2>/dev/null; then
+        if grep -qi "wsl2" /proc/version 2>/dev/null || [ -f /run/WSL ]; then
             IS_WSL2=true
         fi
     fi
@@ -53,47 +68,65 @@ detect_environment() {
         . /etc/os-release
         UBUNTU_VERSION="$VERSION_ID"
         UBUNTU_CODENAME="$VERSION_CODENAME"
+    else
+        UBUNTU_VERSION="22.04"
+        UBUNTU_CODENAME="jammy"
     fi
     
-    # Determine ROS distro
-    case "$UBUNTU_VERSION" in
-        "22.04") ROS_DISTRO="humble" ;;
-        "24.04") ROS_DISTRO="jazzy" ;;
-        "20.04") ROS_DISTRO="galactic" ;;
-        *) ROS_DISTRO="humble" ;;
-    esac
+    # Determine ROS distro and compatible repository codename
+    if [ "$UBUNTU_VERSION" = "24.04" ]; then
+        ROS_DISTRO="jazzy"
+        ROS_UBUNTU_CODENAME="noble"
+    elif [ "$UBUNTU_VERSION" = "22.04" ]; then
+        ROS_DISTRO="humble"
+        ROS_UBUNTU_CODENAME="jammy"
+    else
+        # For newer versions, use latest available
+        UBUNTU_MAJOR=$(echo "$UBUNTU_VERSION" | cut -d. -f1)
+        if [ "$UBUNTU_MAJOR" -ge 24 ] 2>/dev/null; then
+            ROS_DISTRO="jazzy"
+            ROS_UBUNTU_CODENAME="noble"
+        else
+            ROS_DISTRO="humble"
+            ROS_UBUNTU_CODENAME="jammy"
+        fi
+    fi
 }
 
-SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
-cd "$SCRIPT_DIR"
+# Parse arguments
+INSTALL_ARDUPILOT=true
+for arg in "$@"; do
+    if [ "$arg" = "--skip-ardupilot" ]; then
+        INSTALL_ARDUPILOT=false
+    fi
+done
+
+detect_environment
 
 print_header "UAV-UGV Simulation - Complete Setup"
 echo "GPS-Denied Navigation with Geofencing"
 echo ""
 
-detect_environment
-
 echo -e "${CYAN}Detected Environment:${NC}"
 echo "  Ubuntu: $UBUNTU_VERSION ($UBUNTU_CODENAME)"
 echo "  WSL: $IS_WSL | WSL2: $IS_WSL2"
 echo "  ROS 2 Target: $ROS_DISTRO"
+echo "  ArduPilot: $INSTALL_ARDUPILOT"
 echo ""
 
-TOTAL_STEPS=10
+if [ "$INSTALL_ARDUPILOT" = true ]; then
+    TOTAL_STEPS=10
+else
+    TOTAL_STEPS=7
+fi
 STEP=1
 
 # ============================================================================
-# STEP 1: System Update
+# STEP 1: System Update & Dependencies
 # ============================================================================
-print_step $STEP "Updating system packages"
-sudo apt-get update
-sudo apt-get upgrade -y
-((STEP++))
+print_step $STEP "Installing system dependencies"
 
-# ============================================================================
-# STEP 2: Install Base Dependencies
-# ============================================================================
-print_step $STEP "Installing base dependencies"
+sudo apt-get update
 sudo apt-get install -y \
     curl \
     gnupg \
@@ -116,9 +149,15 @@ sudo apt-get install -y \
     libopencv-dev \
     libxml2-dev \
     libxslt1-dev \
+    libgstreamer1.0-dev \
+    libgstreamer-plugins-base1.0-dev \
+    gstreamer1.0-plugins-bad \
+    gstreamer1.0-libav \
+    gstreamer1.0-gl \
     ccache \
     gawk \
-    libtool-bin
+    libtool-bin \
+    netcat-openbsd
 
 # WSL-specific packages
 if $IS_WSL; then
@@ -133,143 +172,194 @@ fi
 ((STEP++))
 
 # ============================================================================
-# STEP 3: Install ROS 2
+# STEP 2: Install ROS 2
 # ============================================================================
 print_step $STEP "Installing ROS 2 $ROS_DISTRO"
 
-if [ ! -d "/opt/ros/$ROS_DISTRO" ]; then
-    # Add ROS 2 repository
-    if [ ! -f /usr/share/keyrings/ros-archive-keyring.gpg ]; then
-        sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key \
-            -o /usr/share/keyrings/ros-archive-keyring.gpg
-    fi
-    
-    echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] \
-        http://packages.ros.org/ros2/ubuntu $(lsb_release -cs) main" | \
-        sudo tee /etc/apt/sources.list.d/ros2.list > /dev/null
-    
-    sudo apt-get update
-    sudo apt-get install -y ros-${ROS_DISTRO}-desktop python3-colcon-common-extensions
-else
-    print_info "ROS 2 $ROS_DISTRO already installed"
-fi
-((STEP++))
+# Add ROS 2 GPG key
+sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key \
+    -o /usr/share/keyrings/ros-archive-keyring.gpg
 
-# ============================================================================
-# STEP 4: Install ROS 2 Packages
-# ============================================================================
-print_step $STEP "Installing ROS 2 packages"
+# Add repository using compatible Ubuntu codename
+echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] \
+    http://packages.ros.org/ros2/ubuntu $ROS_UBUNTU_CODENAME main" | \
+    sudo tee /etc/apt/sources.list.d/ros2.list > /dev/null
+
+sudo apt-get update
+
+# Install ROS 2
 sudo apt-get install -y \
-    ros-${ROS_DISTRO}-mavros \
-    ros-${ROS_DISTRO}-mavros-extras \
-    ros-${ROS_DISTRO}-cv-bridge \
-    ros-${ROS_DISTRO}-image-transport \
-    ros-${ROS_DISTRO}-tf2 \
-    ros-${ROS_DISTRO}-tf2-ros \
-    ros-${ROS_DISTRO}-tf2-geometry-msgs \
-    ros-${ROS_DISTRO}-gazebo-ros-pkgs 2>/dev/null || \
-    print_warning "Some ROS packages not available for $ROS_DISTRO"
+    ros-${ROS_DISTRO}-ros-base \
+    ros-${ROS_DISTRO}-geometry-msgs \
+    ros-${ROS_DISTRO}-std-msgs \
+    ros-${ROS_DISTRO}-nav-msgs \
+    ros-${ROS_DISTRO}-sensor-msgs \
+    python3-colcon-common-extensions || {
+    print_error "Failed to install ROS 2 $ROS_DISTRO"
+    exit 1
+}
 
-# Install GeographicLib datasets for MAVROS
-GEOGRAPHICLIB_SCRIPT="/opt/ros/${ROS_DISTRO}/lib/mavros/install_geographiclib_datasets.sh"
-if [ -f "$GEOGRAPHICLIB_SCRIPT" ]; then
-    if [ ! -f /usr/share/GeographicLib/geoids/egm96-5.pgm ]; then
-        print_info "Installing GeographicLib datasets (this may take a minute)"
-        sudo "$GEOGRAPHICLIB_SCRIPT" || print_warning "GeographicLib installation failed"
-    fi
-fi
+# Install ros-gz bridge
+sudo apt-get install -y ros-${ROS_DISTRO}-ros-gz || \
+    print_warning "Could not install ros-gz bridge"
+
+print_info "ROS 2 $ROS_DISTRO installed"
 ((STEP++))
 
 # ============================================================================
-# STEP 5: Install Gazebo
+# STEP 3: Install Gazebo (Harmonic or Garden)
 # ============================================================================
 print_step $STEP "Installing Gazebo"
-sudo apt-get install -y gazebo libgazebo-dev || \
-sudo apt-get install -y gazebo11 libgazebo11-dev || \
-print_warning "Gazebo installation may require manual setup"
-((STEP++))
 
-# ============================================================================
-# STEP 6: Install ArduPilot SITL
-# ============================================================================
-print_step $STEP "Installing ArduPilot SITL"
+# Add Gazebo repository
+GZ_UBUNTU_CODENAME="$ROS_UBUNTU_CODENAME"
+sudo wget -q https://packages.osrfoundation.org/gazebo.gpg \
+    -O /usr/share/keyrings/pkgs-osrf-archive-keyring.gpg 2>/dev/null || true
 
-ARDUPILOT_HOME="$HOME/ardupilot"
-if [ ! -d "$ARDUPILOT_HOME" ]; then
-    print_info "Cloning ArduPilot repository..."
-    cd "$HOME"
-    git clone --recurse-submodules https://github.com/ArduPilot/ardupilot.git
-    cd ardupilot
-    git submodule update --init --recursive
+echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/pkgs-osrf-archive-keyring.gpg] \
+    http://packages.osrfoundation.org/gazebo/ubuntu-stable $GZ_UBUNTU_CODENAME main" | \
+    sudo tee /etc/apt/sources.list.d/gazebo-stable.list > /dev/null
+
+sudo apt-get update
+
+# Try Gazebo Harmonic first, then Garden
+GZ_VERSION=""
+if sudo apt-get install -y gz-harmonic 2>/dev/null; then
+    GZ_VERSION="harmonic"
+    # Install Harmonic development packages for building plugins
+    sudo apt-get install -y \
+        libgz-cmake3-dev \
+        libgz-sim8-dev \
+        libgz-plugin2-dev \
+        libgz-common5-dev \
+        libgz-physics7-dev \
+        libgz-sensors8-dev \
+        libgz-rendering8-dev \
+        libgz-transport13-dev \
+        libgz-msgs10-dev \
+        rapidjson-dev \
+        2>/dev/null || print_warning "Some Gazebo dev packages may be missing"
+elif sudo apt-get install -y gz-garden 2>/dev/null; then
+    GZ_VERSION="garden"
+    # Install Garden development packages
+    sudo apt-get install -y \
+        libgz-cmake3-dev \
+        libgz-sim7-dev \
+        libgz-plugin2-dev \
+        libgz-common5-dev \
+        libgz-physics6-dev \
+        libgz-sensors7-dev \
+        libgz-rendering7-dev \
+        libgz-transport12-dev \
+        libgz-msgs9-dev \
+        rapidjson-dev \
+        2>/dev/null || print_warning "Some Gazebo dev packages may be missing"
 else
-    print_info "ArduPilot already exists, updating..."
-    cd "$ARDUPILOT_HOME"
-    git fetch origin
-    git submodule update --init --recursive
+    print_warning "Could not install Gazebo Harmonic/Garden"
+    GZ_VERSION="none"
 fi
 
-# Install ArduPilot prerequisites
-print_info "Installing ArduPilot prerequisites..."
-cd "$ARDUPILOT_HOME"
-USER_NONINTERACTIVE=1 Tools/environment_install/install-prereqs-ubuntu.sh -y || true
-
-# Reload profile
-. ~/.profile 2>/dev/null || true
-
-# Build ArduPilot SITL
-print_info "Building ArduPilot SITL (this may take several minutes)..."
-cd "$ARDUPILOT_HOME"
-./waf configure --board sitl
-./waf copter
-./waf rover
-((STEP++))
-
-# ============================================================================
-# STEP 7: Install ardupilot_gazebo Plugin
-# ============================================================================
-print_step $STEP "Installing ardupilot_gazebo plugin"
-
-ARDUPILOT_GAZEBO_HOME="$HOME/ardupilot_gazebo"
-if [ ! -d "$ARDUPILOT_GAZEBO_HOME" ]; then
-    print_info "Cloning ardupilot_gazebo..."
-    cd "$HOME"
-    git clone https://github.com/ArduPilot/ardupilot_gazebo.git
+if command -v gz &> /dev/null; then
+    print_info "Gazebo $GZ_VERSION installed"
 else
-    print_info "ardupilot_gazebo already exists, updating..."
-    cd "$ARDUPILOT_GAZEBO_HOME"
-    git pull origin main || true
+    print_warning "Gazebo command not found"
 fi
-
-cd "$ARDUPILOT_GAZEBO_HOME"
-mkdir -p build && cd build
-cmake ..
-make -j$(nproc)
-sudo make install
 ((STEP++))
 
 # ============================================================================
-# STEP 8: Setup Python Virtual Environment
+# STEP 4: Python Virtual Environment
 # ============================================================================
 print_step $STEP "Setting up Python environment"
 
-cd "$SCRIPT_DIR"
-
-if [ ! -d "venv" ]; then
-    python3 -m venv venv
+if [ -d "$SCRIPT_DIR/venv" ]; then
+    rm -rf "$SCRIPT_DIR/venv"
 fi
 
-source venv/bin/activate
+python3 -m venv "$SCRIPT_DIR/venv"
+source "$SCRIPT_DIR/venv/bin/activate"
 pip install --upgrade pip
-pip install -r requirements.txt || print_warning "Some Python packages failed"
+
+if [ -f "$SCRIPT_DIR/requirements.txt" ]; then
+    pip install -r "$SCRIPT_DIR/requirements.txt" || print_warning "Some Python packages failed"
+else
+    pip install numpy opencv-python scipy shapely filterpy transforms3d pymavlink pexpect
+fi
+
 deactivate
 ((STEP++))
 
 # ============================================================================
-# STEP 9: Configure Environment
+# STEP 5: Create Activation Script
 # ============================================================================
-print_step $STEP "Configuring environment"
+print_step $STEP "Creating activation script"
+
+cat > "$SCRIPT_DIR/activate_venv.sh" << 'ACTIVATE_EOF'
+#!/bin/bash
+# UAV-UGV Simulation - Environment Activation
+# Usage: source activate_venv.sh
+
+SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
+
+# ROS 2 Setup
+if [ -f "/opt/ros/jazzy/setup.bash" ]; then
+    source /opt/ros/jazzy/setup.bash
+    ROS_VER="jazzy"
+elif [ -f "/opt/ros/humble/setup.bash" ]; then
+    source /opt/ros/humble/setup.bash
+    ROS_VER="humble"
+else
+    echo "[WARN] ROS 2 not found in /opt/ros/"
+    ROS_VER="none"
+fi
+
+# ArduPilot Environment
+export ARDUPILOT_HOME="$HOME/ardupilot"
+export PATH="$ARDUPILOT_HOME/Tools/autotest:$PATH"
+export PATH="$HOME/.local/bin:$PATH"
+
+# Deactivate existing venv
+if [ -n "$VIRTUAL_ENV" ]; then
+    deactivate 2>/dev/null || true
+fi
+
+# Activate project venv
+if [ -f "$SCRIPT_DIR/venv/bin/activate" ]; then
+    source "$SCRIPT_DIR/venv/bin/activate"
+fi
+
+# Gazebo paths (new Gazebo - Ignition/Harmonic)
+export GZ_SIM_RESOURCE_PATH="$SCRIPT_DIR/models:$SCRIPT_DIR/worlds:${GZ_SIM_RESOURCE_PATH:-}"
+
+# ArduPilot Gazebo plugin
+if [ -d "$HOME/ardupilot_gazebo/build" ]; then
+    export GZ_SIM_SYSTEM_PLUGIN_PATH="$HOME/ardupilot_gazebo/build:${GZ_SIM_SYSTEM_PLUGIN_PATH:-}"
+    export GZ_SIM_RESOURCE_PATH="$HOME/ardupilot_gazebo/models:$HOME/ardupilot_gazebo/worlds:$GZ_SIM_RESOURCE_PATH"
+fi
+
+# WSL environment
+if grep -qEi "(microsoft|wsl)" /proc/version 2>/dev/null; then
+    if [ -d "/mnt/wslg" ]; then
+        export DISPLAY=:0
+    else
+        export DISPLAY=$(cat /etc/resolv.conf 2>/dev/null | grep nameserver | awk '{print $2}'):0
+    fi
+    export LIBGL_ALWAYS_INDIRECT=0
+    export MESA_GL_VERSION_OVERRIDE=3.3
+fi
+
+echo -e "\033[0;32mEnvironment activated (ROS 2 $ROS_VER)\033[0m"
+echo ""
+echo "Run simulation:  bash scripts/run_simulation.sh"
+ACTIVATE_EOF
+
+chmod +x "$SCRIPT_DIR/activate_venv.sh"
+((STEP++))
+
+# ============================================================================
+# STEP 6: Create WSL Environment (if applicable)
+# ============================================================================
+print_step $STEP "Configuring environment files"
 
-# Create WSL environment file
 if $IS_WSL; then
     cat > "$SCRIPT_DIR/wsl_env.sh" << 'WSLEOF'
 #!/bin/bash
@@ -285,77 +375,120 @@ export LIBGL_ALWAYS_INDIRECT=0
 export MESA_GL_VERSION_OVERRIDE=3.3
 export MESA_GLSL_VERSION_OVERRIDE=330
 export OGRE_RTT_MODE=Copy
-
-# Source Gazebo setup
-[ -f /usr/share/gazebo/setup.bash ] && source /usr/share/gazebo/setup.bash
-[ -f /usr/share/gazebo-11/setup.bash ] && source /usr/share/gazebo-11/setup.bash
 WSLEOF
     chmod +x "$SCRIPT_DIR/wsl_env.sh"
 fi
+((STEP++))
 
-# Create activation script
-cat > "$SCRIPT_DIR/activate_venv.sh" << EOF
-#!/bin/bash
-SCRIPT_DIR="\$(cd "\$(dirname "\${BASH_SOURCE[0]}")" && pwd)"
+# ============================================================================
+# ARDUPILOT INSTALLATION (if not skipped)
+# ============================================================================
+if [ "$INSTALL_ARDUPILOT" = true ]; then
 
-# ROS 2
-source /opt/ros/${ROS_DISTRO}/setup.bash
+    # ========================================================================
+    # STEP 7: Clone and Setup ArduPilot
+    # ========================================================================
+    print_step $STEP "Setting up ArduPilot SITL"
+    
+    if [ ! -d "$ARDUPILOT_HOME" ]; then
+        print_info "Cloning ArduPilot repository..."
+        git clone --recurse-submodules https://github.com/ArduPilot/ardupilot.git "$ARDUPILOT_HOME"
+    else
+        print_info "ArduPilot directory already exists"
+    fi
+    
+    cd "$ARDUPILOT_HOME"
+    git submodule update --init --recursive
+    
+    # Install ArduPilot prerequisites
+    print_info "Installing ArduPilot prerequisites (this may take a while)..."
+    Tools/environment_install/install-prereqs-ubuntu.sh -y || true
+    . ~/.profile 2>/dev/null || true
+    
+    # Source ArduPilot environment
+    [ -f "$HOME/.ardupilot_env" ] && source "$HOME/.ardupilot_env"
+    
+    print_info "ArduPilot prerequisites installed"
+    ((STEP++))
+    
+    # ========================================================================
+    # STEP 8: Build ArduCopter SITL
+    # ========================================================================
+    print_step $STEP "Building ArduPilot SITL (this takes several minutes)"
+    
+    cd "$ARDUPILOT_HOME"
+    ./waf configure --board sitl
+    ./waf copter
+    ./waf rover
+    
+    print_info "ArduPilot SITL built"
+    ((STEP++))
+    
+    # ========================================================================
+    # STEP 9: Build ArduPilot Gazebo Plugin
+    # ========================================================================
+    print_step $STEP "Building ArduPilot Gazebo plugin"
+    
+    if [ ! -d "$ARDUPILOT_GZ" ]; then
+        print_info "Cloning ardupilot_gazebo repository..."
+        git clone https://github.com/ArduPilot/ardupilot_gazebo.git "$ARDUPILOT_GZ"
+    else
+        print_info "ardupilot_gazebo directory already exists"
+        cd "$ARDUPILOT_GZ"
+        git pull origin main || true
+    fi
+    
+    cd "$ARDUPILOT_GZ"
+    mkdir -p build && cd build
+    cmake .. -DCMAKE_BUILD_TYPE=Release
+    make -j$(nproc)
+    
+    print_info "ArduPilot Gazebo plugin built"
+    
+    # Install MAVProxy
+    pip3 install --user pymavlink mavproxy pexpect
+    ((STEP++))
+    
+    # ========================================================================
+    # STEP 10: Verify Installation
+    # ========================================================================
+    print_step $STEP "Verifying installation"
+    
+    export PATH=$PATH:$ARDUPILOT_HOME/Tools/autotest:$HOME/.local/bin
+    
+    echo ""
+    command -v sim_vehicle.py &> /dev/null && echo "[OK] sim_vehicle.py" || echo "[WARN] sim_vehicle.py not found"
+    command -v gz &> /dev/null && echo "[OK] Gazebo (gz)" || echo "[WARN] Gazebo not found"
+    command -v mavproxy.py &> /dev/null && echo "[OK] MAVProxy" || echo "[WARN] MAVProxy not in PATH"
+    [ -f "$ARDUPILOT_GZ/build/libArduPilotPlugin.so" ] && echo "[OK] ArduPilot Gazebo plugin" || echo "[WARN] Plugin not built"
+    ((STEP++))
 
-# Gazebo
-[ -f /usr/share/gazebo/setup.bash ] && source /usr/share/gazebo/setup.bash
-[ -f /usr/share/gazebo-11/setup.bash ] && source /usr/share/gazebo-11/setup.bash
-
-# Python venv
-source "\$SCRIPT_DIR/venv/bin/activate"
-
-# Gazebo paths
-export GAZEBO_MODEL_PATH="\$SCRIPT_DIR/models:\$HOME/ardupilot_gazebo/models:\${GAZEBO_MODEL_PATH:-}"
-export GAZEBO_RESOURCE_PATH="\$SCRIPT_DIR/worlds:\$HOME/ardupilot_gazebo/worlds:\${GAZEBO_RESOURCE_PATH:-}"
-
-# ArduPilot
-export PATH="\$PATH:\$HOME/ardupilot/Tools/autotest"
-export ARDUPILOT_HOME="\$HOME/ardupilot"
-
-# WSL
-if grep -qEi "(microsoft|wsl)" /proc/version 2>/dev/null; then
-    [ -f "\$SCRIPT_DIR/wsl_env.sh" ] && source "\$SCRIPT_DIR/wsl_env.sh"
 fi
 
-echo -e "\033[0;32mEnvironment activated (ROS 2 ${ROS_DISTRO})\033[0m"
-echo ""
-echo "Run simulation:  bash scripts/run_simulation.sh"
-echo "With rendering:  bash scripts/run_simulation.sh --software-render"
-EOF
-chmod +x "$SCRIPT_DIR/activate_venv.sh"
-
-((STEP++))
-
 # ============================================================================
-# STEP 10: Make Scripts Executable
+# Make scripts executable
 # ============================================================================
-print_step $STEP "Finalizing installation"
 chmod +x "$SCRIPT_DIR/scripts/"*.sh 2>/dev/null || true
-chmod +x "$SCRIPT_DIR/activate_venv.sh" 2>/dev/null || true
-((STEP++))
 
 # ============================================================================
 # COMPLETE
 # ============================================================================
 print_header "Installation Complete!"
 
-echo -e "${GREEN}All components installed:${NC}"
+echo -e "${GREEN}Components installed:${NC}"
 echo "  - ROS 2 $ROS_DISTRO"
-echo "  - Gazebo"
-echo "  - ArduPilot SITL ($ARDUPILOT_HOME)"
-echo "  - ardupilot_gazebo ($ARDUPILOT_GAZEBO_HOME)"
-echo "  - MAVROS"
+echo "  - Gazebo $GZ_VERSION"
+if [ "$INSTALL_ARDUPILOT" = true ]; then
+    echo "  - ArduPilot SITL ($ARDUPILOT_HOME)"
+    echo "  - ardupilot_gazebo ($ARDUPILOT_GZ)"
+fi
 echo "  - Python dependencies"
 echo ""
 
 if $IS_WSL; then
-    echo -e "${YELLOW}WSL Setup Notes:${NC}"
-    echo "  - GUI apps require WSLg (Windows 11) or VcXsrv (Windows 10)"
-    echo "  - Use --software-render flag if graphics are slow"
+    echo -e "${YELLOW}WSL Notes:${NC}"
+    echo "  - GUI requires WSLg (Win11) or VcXsrv (Win10)"
+    echo "  - Use --software-render if graphics are slow"
     echo ""
 fi
 
@@ -364,10 +497,5 @@ echo ""
 echo "  cd $SCRIPT_DIR"
 echo "  source activate_venv.sh"
 echo "  bash scripts/run_simulation.sh"
-if $IS_WSL; then
-echo ""
-echo "  # Or with software rendering for WSL:"
-echo "  bash scripts/run_simulation.sh --software-render"
-fi
 echo ""
 echo -e "${GREEN}==========================================${NC}"