#!/bin/bash
# =============================================================================
# ArduPilot GPS-Denied Simulation Launcher
# =============================================================================
# Launches Gazebo with GPU acceleration and camera support
# Automatically detects GPU: NVIDIA > Intel/AMD integrated > Software
#
# Usage: 
#   ./scripts/run_ardupilot_sim.sh           # Default iris_runway
#   ./scripts/run_ardupilot_sim.sh camera    # With camera world
# =============================================================================

set -e

echo "=============================================="
echo "  ArduPilot GPS-Denied Simulation"
echo "=============================================="

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

# Disable software rendering by default
export LIBGL_ALWAYS_SOFTWARE=0

# Check for NVIDIA dedicated GPU
if command -v nvidia-smi &> /dev/null && nvidia-smi &> /dev/null; then
    GPU_NAME=$(nvidia-smi --query-gpu=name --format=csv,noheader 2>/dev/null | head -1)
    echo "[GPU] NVIDIA (dedicated): $GPU_NAME"
    export __GLX_VENDOR_LIBRARY_NAME=nvidia
    export __NV_PRIME_RENDER_OFFLOAD=1
    export __VK_LAYER_NV_optimus=NVIDIA_only
    GPU_TYPE="nvidia"

# Check for Intel integrated GPU
elif [ -d "/sys/class/drm/card0" ] && grep -qi "intel" /sys/class/drm/card0/device/vendor 2>/dev/null; then
    echo "[GPU] Intel (integrated)"
    export MESA_GL_VERSION_OVERRIDE=3.3
    export LIBVA_DRIVER_NAME=iHD  # Intel media driver
    GPU_TYPE="intel"

# Check via glxinfo
elif command -v glxinfo &> /dev/null; then
    GPU_NAME=$(glxinfo 2>/dev/null | grep "OpenGL renderer" | cut -d: -f2 | xargs)
    
    if [[ "$GPU_NAME" == *"NVIDIA"* ]]; then
        echo "[GPU] NVIDIA: $GPU_NAME"
        export __GLX_VENDOR_LIBRARY_NAME=nvidia
        GPU_TYPE="nvidia"
    elif [[ "$GPU_NAME" == *"Intel"* ]]; then
        echo "[GPU] Intel: $GPU_NAME"
        export MESA_GL_VERSION_OVERRIDE=3.3
        GPU_TYPE="intel"
    elif [[ "$GPU_NAME" == *"AMD"* ]] || [[ "$GPU_NAME" == *"Radeon"* ]]; then
        echo "[GPU] AMD: $GPU_NAME"
        export MESA_GL_VERSION_OVERRIDE=3.3
        GPU_TYPE="amd"
    elif [[ "$GPU_NAME" == *"llvmpipe"* ]] || [[ "$GPU_NAME" == *"software"* ]]; then
        echo "[GPU] Software rendering (llvmpipe) - SLOW!"
        echo "[WARN] Install GPU drivers for better performance"
        GPU_TYPE="software"
    else
        echo "[GPU] $GPU_NAME"
        export MESA_GL_VERSION_OVERRIDE=3.3
        GPU_TYPE="other"
    fi

# Fallback to Mesa defaults
else
    echo "[GPU] Unknown - using Mesa defaults"
    export MESA_GL_VERSION_OVERRIDE=3.3
    GPU_TYPE="unknown"
fi

# For integrated graphics, ensure Mesa works well
if [[ "$GPU_TYPE" == "intel" ]] || [[ "$GPU_TYPE" == "amd" ]] || [[ "$GPU_TYPE" == "other" ]]; then
    export MESA_LOADER_DRIVER_OVERRIDE=""
    export DRI_PRIME=0  # Use primary GPU (integrated)
fi

echo "[INFO] GPU type: $GPU_TYPE"

# =============================================================================
# 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}"

# Add local models
SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
PROJECT_DIR="$(dirname "$SCRIPT_DIR")"
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 not found!"
    echo "Run: ./setup/install_ardupilot.sh"
    exit 1
fi

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

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

case "$WORLD_ARG" in
    runway|iris|default)
        WORLD="${HOME}/ardupilot_gazebo/worlds/iris_runway.sdf"
        ;;
    warehouse)
        WORLD="${HOME}/ardupilot_gazebo/worlds/iris_warehouse.sdf"
        ;;
    gimbal)
        WORLD="${HOME}/ardupilot_gazebo/worlds/gimbal.sdf"
        ;;
    zephyr|plane)
        WORLD="${HOME}/ardupilot_gazebo/worlds/zephyr_runway.sdf"
        ;;
    parachute)
        WORLD="${HOME}/ardupilot_gazebo/worlds/zephyr_parachute.sdf"
        ;;
    custom)
        WORLD="${PROJECT_DIR}/gazebo/worlds/custom_landing.sdf"
        ;;
    *)
        if [ -f "$WORLD_ARG" ]; then
            WORLD="$WORLD_ARG"
        elif [ -f "${PROJECT_DIR}/gazebo/worlds/${WORLD_ARG}" ]; then
            WORLD="${PROJECT_DIR}/gazebo/worlds/${WORLD_ARG}"
        elif [ -f "${PROJECT_DIR}/gazebo/worlds/${WORLD_ARG}.sdf" ]; then
            WORLD="${PROJECT_DIR}/gazebo/worlds/${WORLD_ARG}.sdf"
        elif [ -f "${HOME}/ardupilot_gazebo/worlds/${WORLD_ARG}" ]; then
            WORLD="${HOME}/ardupilot_gazebo/worlds/${WORLD_ARG}"
        elif [ -f "${HOME}/ardupilot_gazebo/worlds/${WORLD_ARG}.sdf" ]; then
            WORLD="${HOME}/ardupilot_gazebo/worlds/${WORLD_ARG}.sdf"
        else
            WORLD=""
        fi
        ;;
esac

if [ -z "$WORLD" ] || [ ! -f "$WORLD" ]; then
    echo "[ERROR] World not found: $WORLD_ARG"
    echo ""
    echo "Built-in worlds:"
    echo "  runway     - Iris on runway (default)"
    echo "  warehouse  - Iris in warehouse"
    echo "  custom     - Custom landing pad"
    echo ""
    echo "Local worlds in gazebo/worlds/:"
    ls -1 "${PROJECT_DIR}/gazebo/worlds/"*.sdf 2>/dev/null | xargs -n1 basename || echo "  (none)"
    echo ""
    echo "Or specify full path to .sdf file"
    exit 1
fi

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

# =============================================================================
# INSTRUCTIONS
# =============================================================================
echo ""
echo "=============================================="
echo "  Starting Gazebo"
echo "=============================================="
echo ""
echo "World: $WORLD"
echo ""
echo "After Gazebo starts, in another terminal run:"
echo ""
echo "  sim_vehicle.py -v ArduCopter -f gazebo-iris --model JSON --console"
echo ""
echo "For GPS-DENIED mode, in MAVProxy console:"
echo ""
echo "  param set ARMING_CHECK 0"
echo "  mode stabilize"
echo "  arm throttle force"
echo ""
echo "=============================================="
echo ""

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