#!/bin/bash
set -e

# Colors
RED='\033[0;31m'
GREEN='\033[0;32m'
YELLOW='\033[1;33m'
BLUE='\033[0;34m'
NC='\033[0m'

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

echo -e "${BLUE}==========================================${NC}"
echo -e "${BLUE}  UAV-UGV Simulation${NC}"
echo -e "${BLUE}  GPS-Denied Navigation with Geofencing${NC}"
echo -e "${BLUE}==========================================${NC}"
echo ""

# Detect WSL
IS_WSL=false
if grep -qEi "(microsoft|wsl)" /proc/version 2>/dev/null; then
    IS_WSL=true
    echo -e "${YELLOW}Running in WSL environment${NC}"
fi

# 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

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 or docs/wsl_setup_guide.md"
    exit 1
fi

echo -e "${GREEN}Using ROS 2: $ROS_DISTRO${NC}"

# Parse arguments
WORLD="${1:-$PROJECT_DIR/worlds/empty_custom.world}"
USE_NVIDIA=true
HEADLESS=false

while [[ $# -gt 0 ]]; do
    case $1 in
        --world)
            WORLD="$2"
            shift 2
            ;;
        --headless)
            HEADLESS=true
            shift
            ;;
        --no-gpu)
            USE_NVIDIA=false
            shift
            ;;
        --software-render)
            export LIBGL_ALWAYS_SOFTWARE=1
            shift
            ;;
        --help)
            echo "Usage: $0 [OPTIONS] [WORLD_FILE]"
            echo ""
            echo "Options:"
            echo "  --world FILE       Specify world file (default: empty_custom.world)"
            echo "  --headless         Run without GUI"
            echo "  --no-gpu           Disable GPU acceleration"
            echo "  --software-render  Force software rendering (useful in WSL)"
            echo "  --help             Show this help"
            echo ""
            echo "Examples:"
            echo "  $0                                    # Default world"
            echo "  $0 worlds/indoor_warehouse.world     # Indoor environment"
            echo "  $0 --software-render                 # For WSL without GPU"
            exit 0
            ;;
        *)
            if [ -f "$1" ]; then
                WORLD="$1"
            fi
            shift
            ;;
    esac
done

# Validate world file
if [ ! -f "$WORLD" ]; then
    echo -e "${YELLOW}World file not found: $WORLD${NC}"
    WORLD="$PROJECT_DIR/worlds/empty_custom.world"
    echo "Using default: $WORLD"
fi

# Setup environment
export GAZEBO_MODEL_PATH="$PROJECT_DIR/models:$GAZEBO_MODEL_PATH"
export GAZEBO_RESOURCE_PATH="$PROJECT_DIR/worlds:$GAZEBO_RESOURCE_PATH"

# ArduPilot Gazebo paths
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-specific setup
if $IS_WSL; then
    # Source WSL environment if exists
    if [ -f "$PROJECT_DIR/wsl_env.sh" ]; then
        source "$PROJECT_DIR/wsl_env.sh"
    fi
    
    # Check DISPLAY
    if [ -z "$DISPLAY" ]; then
        # Try to set DISPLAY for WSL
        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 set to: $DISPLAY${NC}"
    
    # Performance hint for WSL
    if [ -z "$LIBGL_ALWAYS_SOFTWARE" ]; then
        echo -e "${YELLOW}TIP: If Gazebo is slow, run with --software-render flag${NC}"
    fi
fi

# GPU setup for native Linux
if $USE_NVIDIA && ! $IS_WSL; then
    if command -v nvidia-smi &> /dev/null; then
        echo -e "${GREEN}NVIDIA GPU detected${NC}"
        export __NV_PRIME_RENDER_OFFLOAD=1
        export __GLX_VENDOR_LIBRARY_NAME=nvidia
    fi
fi

# Cleanup function
cleanup() {
    echo ""
    echo -e "${YELLOW}Shutting down simulation...${NC}"
    pkill -f "gazebo" 2>/dev/null || true
    pkill -f "gzserver" 2>/dev/null || true
    pkill -f "gzclient" 2>/dev/null || true
    pkill -f "sim_vehicle.py" 2>/dev/null || true
    pkill -f "mavros" 2>/dev/null || true
    pkill -f "ArduCopter" 2>/dev/null || true
    sleep 1
    echo -e "${GREEN}Cleanup complete.${NC}"
}

trap cleanup EXIT INT TERM

# Source ROS and workspace
source /opt/ros/${ROS_DISTRO}/setup.bash

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

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

echo ""
echo -e "${GREEN}Starting simulation with world: $(basename $WORLD)${NC}"
echo ""

# Launch based on ROS distro and Gazebo version
if [ "$ROS_DISTRO" = "jazzy" ]; then
    # Jazzy uses Gazebo Harmonic (gz-sim)
    echo -e "${YELLOW}Note: Jazzy uses Gazebo Sim (Harmonic). World files may need conversion.${NC}"
    ros2 launch uav_ugv_simulation full_simulation.launch.py world:="$WORLD" 2>&1 || {
        echo -e "${YELLOW}Launch file failed. Trying direct Gazebo launch...${NC}"
        gz sim "$WORLD"
    }
else
    # Humble and earlier use Gazebo Classic
    if $HEADLESS; then
        GAZEBO_CMD="gzserver --verbose"
    else
        GAZEBO_CMD="gazebo --verbose"
    fi
    
    ros2 launch uav_ugv_simulation full_simulation.launch.py world:="$WORLD" 2>&1 || {
        echo -e "${YELLOW}Launch file failed. Trying direct Gazebo launch...${NC}"
        $GAZEBO_CMD "$WORLD"
    }
fi