#!/bin/bash
# Activate virtual environment and setup for UAV-UGV Simulation

SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"

# Detect ROS distro
ROS_DISTRO=""
for distro in humble jazzy iron galactic; do
    if [ -d "/opt/ros/$distro" ]; then
        ROS_DISTRO="$distro"
        break
    fi
done

# Source ROS 2
if [ -n "$ROS_DISTRO" ] && [ -f "/opt/ros/${ROS_DISTRO}/setup.bash" ]; then
    source /opt/ros/${ROS_DISTRO}/setup.bash
fi

# Source Gazebo setup (critical for shader/resource paths)
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

# Activate Python virtual environment
if [ -f "$SCRIPT_DIR/venv/bin/activate" ]; then
    source "$SCRIPT_DIR/venv/bin/activate"
fi

# Source ROS workspace if built
if [ -f "$SCRIPT_DIR/../install/setup.bash" ]; then
    source "$SCRIPT_DIR/../install/setup.bash"
elif [ -f "$SCRIPT_DIR/install/setup.bash" ]; then
    source "$SCRIPT_DIR/install/setup.bash"
fi

# Gazebo model and resource paths
export GAZEBO_MODEL_PATH="$SCRIPT_DIR/models:${GAZEBO_MODEL_PATH:-}"
export GAZEBO_RESOURCE_PATH="$SCRIPT_DIR/worlds:${GAZEBO_RESOURCE_PATH:-}"

# ArduPilot Gazebo (if installed)
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"
fi

# WSL environment (if applicable)
if grep -qEi "(microsoft|wsl)" /proc/version 2>/dev/null; then
    if [ -f "$SCRIPT_DIR/wsl_env.sh" ]; then
        source "$SCRIPT_DIR/wsl_env.sh"
    fi
fi

echo -e "\033[0;32mEnvironment activated (ROS 2 ${ROS_DISTRO:-not found})\033[0m"
echo "Run: bash scripts/run_simulation.sh"
echo ""
echo "Options:"
echo "  bash scripts/run_simulation.sh                    # Default world"
echo "  bash scripts/run_simulation.sh --software-render  # For WSL/rendering issues"