#!/bin/bash
# =============================================================================
# ArduPilot Gazebo Simulation Launcher
# =============================================================================
# Launches Gazebo with the ArduPilot iris drone.
# START THIS FIRST, then run SITL in another terminal.
#
# Usage: 
#   ./scripts/run_ardupilot_sim.sh           # Default runway
#   ./scripts/run_ardupilot_sim.sh warehouse # Indoor warehouse
# =============================================================================

set -e

echo "=============================================="
echo "  Gazebo + ArduPilot Simulation"
echo "=============================================="
echo ""

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

# =============================================================================
# GPU DETECTION
# =============================================================================
echo "[INFO] Detecting GPU..."

export LIBGL_ALWAYS_SOFTWARE=0

if command -v nvidia-smi &> /dev/null && nvidia-smi &> /dev/null 2>&1; then
    GPU_NAME=$(nvidia-smi --query-gpu=name --format=csv,noheader 2>/dev/null | head -1)
    echo "[GPU] NVIDIA: $GPU_NAME"
    export __GLX_VENDOR_LIBRARY_NAME=nvidia
    export __NV_PRIME_RENDER_OFFLOAD=1
elif command -v glxinfo &> /dev/null; then
    GPU_NAME=$(glxinfo 2>/dev/null | grep "OpenGL renderer" | cut -d: -f2 | xargs)
    echo "[GPU] $GPU_NAME"
    export MESA_GL_VERSION_OVERRIDE=3.3
else
    echo "[GPU] Unknown - using Mesa defaults"
    export MESA_GL_VERSION_OVERRIDE=3.3
fi

# =============================================================================
# GAZEBO PATHS
# =============================================================================
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}"
export GZ_SIM_RESOURCE_PATH="${PROJECT_DIR}/gazebo/models:${GZ_SIM_RESOURCE_PATH}"

# =============================================================================
# CHECK INSTALLATION
# =============================================================================
if [ ! -d "${HOME}/ardupilot_gazebo" ]; then
    echo "[ERROR] ardupilot_gazebo directory not found!"
    echo ""
    echo "Install with: ./setup/install_ardupilot.sh"
    exit 1
fi

if [ ! -f "${HOME}/ardupilot_gazebo/build/libArduPilotPlugin.so" ]; then
    echo "[ERROR] ArduPilot Gazebo plugin not built!"
    echo ""
    echo "Build it:"
    echo "  cd ~/ardupilot_gazebo"
    echo "  mkdir -p build && cd build"
    echo "  cmake .. && make -j4"
    exit 1
fi

# =============================================================================
# SELECT WORLD
# =============================================================================
WORLD_ARG="${1:-runway}"

case "$WORLD_ARG" in
    runway|default)
        WORLD="${HOME}/ardupilot_gazebo/worlds/iris_runway.sdf"
        ;;
    warehouse)
        WORLD="${HOME}/ardupilot_gazebo/worlds/iris_warehouse.sdf"
        ;;
    *)
        if [ -f "$WORLD_ARG" ]; then
            WORLD="$WORLD_ARG"
        elif [ -f "${HOME}/ardupilot_gazebo/worlds/${WORLD_ARG}.sdf" ]; then
            WORLD="${HOME}/ardupilot_gazebo/worlds/${WORLD_ARG}.sdf"
        else
            echo "[ERROR] World not found: $WORLD_ARG"
            echo ""
            echo "Available worlds:"
            echo "  runway    - Outdoor runway (default)"
            echo "  warehouse - Indoor warehouse"
            exit 1
        fi
        ;;
esac

echo "[OK] World: $(basename "$WORLD")"
echo ""

# =============================================================================
# INSTRUCTIONS
# =============================================================================
echo "=============================================="
echo "  NEXT STEPS"
echo "=============================================="
echo ""
echo "After Gazebo loads, open a NEW terminal and run:"
echo ""
echo "  source ~/.ardupilot_env"
echo "  sim_vehicle.py -v ArduCopter -f gazebo-iris --model JSON --console"
echo ""
echo "Then open ANOTHER terminal to run the controller:"
echo ""
echo "  cd ~/RDC_Simulation"
echo "  source activate.sh"
echo "  python scripts/run_ardupilot.py --pattern square"
echo ""
echo "=============================================="
echo ""
echo "[INFO] Starting Gazebo..."

# =============================================================================
# RUN GAZEBO
# =============================================================================
gz sim -v4 -r "$WORLD"