#!/bin/bash
set -e

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

echo "=========================================="
echo "UAV-UGV Simulation - Run Script"
echo "GPS-Denied Navigation with Geofencing"
echo "=========================================="

WORLD="${1:-$PROJECT_DIR/worlds/empty_custom.world}"

if [ ! -f "$WORLD" ]; then
    echo "World file not found: $WORLD"
    echo "Using default world..."
    WORLD="$PROJECT_DIR/worlds/empty_custom.world"
fi

export GAZEBO_MODEL_PATH="$PROJECT_DIR/models:$GAZEBO_MODEL_PATH"
export GAZEBO_RESOURCE_PATH="$PROJECT_DIR/worlds:$GAZEBO_RESOURCE_PATH"

if command -v nvidia-smi &> /dev/null; then
    echo "NVIDIA GPU detected, enabling acceleration..."
    export __NV_PRIME_RENDER_OFFLOAD=1
    export __GLX_VENDOR_LIBRARY_NAME=nvidia
fi

cleanup() {
    echo ""
    echo "Shutting down simulation..."
    pkill -f "gazebo" 2>/dev/null || true
    pkill -f "sim_vehicle.py" 2>/dev/null || true
    pkill -f "mavros" 2>/dev/null || true
    echo "Cleanup complete."
}

trap cleanup EXIT

echo "Starting simulation with world: $WORLD"
echo ""

if [ -f "$PROJECT_DIR/venv/bin/activate" ]; then
    source "$PROJECT_DIR/venv/bin/activate"
fi

source /opt/ros/humble/setup.bash 2>/dev/null || true

if [ -f "$PROJECT_DIR/install/setup.bash" ]; then
    source "$PROJECT_DIR/install/setup.bash"
fi

ros2 launch uav_ugv_simulation full_simulation.launch.py world:="$WORLD"