#!/bin/bash
# =============================================================================
# ArduPilot Controller Launcher
# =============================================================================
# Starts ArduPilot SITL and your drone controller together.
#
# Usage:
#   Terminal 1: ./scripts/run_ardupilot_sim.sh runway
#   Terminal 2: ./scripts/run_ardupilot_controller.sh
# =============================================================================

set -e

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

# Deactivate any existing virtual environment to avoid conflicts
if [ -n "$VIRTUAL_ENV" ]; then
    deactivate 2>/dev/null || true
fi

# Source ArduPilot environment
if [ -f "$HOME/.ardupilot_env" ]; then
    source "$HOME/.ardupilot_env"
fi

# Activate ArduPilot venv (has empy and other dependencies)
# This MUST be the active venv for sim_vehicle.py to work
if [ -f "$HOME/venv-ardupilot/bin/activate" ]; then
    source "$HOME/venv-ardupilot/bin/activate"
fi

echo "=============================================="
echo "  ArduPilot Controller"
echo "=============================================="
echo ""

# Check dependencies
if ! command -v sim_vehicle.py &> /dev/null; then
    echo "[ERROR] sim_vehicle.py not found"
    echo "Add to PATH: export PATH=\$PATH:~/ardupilot/Tools/autotest"
    exit 1
fi

# Verify correct Python environment (ArduPilot venv should have empy)
if ! python3 -c "import em" 2>/dev/null; then
    echo "[WARN] empy not found in current Python environment"
    echo "[INFO] Attempting to use ArduPilot venv..."
    if [ -f "$HOME/venv-ardupilot/bin/activate" ]; then
        source "$HOME/venv-ardupilot/bin/activate"
    fi
fi

# Start SITL in background
echo "[INFO] Starting ArduPilot SITL..."
sim_vehicle.py -v ArduCopter -f gazebo-iris --model JSON --no-mavproxy &
SITL_PID=$!

# Wait for SITL to start
sleep 5

# Check if SITL started
if ! kill -0 $SITL_PID 2>/dev/null; then
    echo "[ERROR] SITL failed to start"
    exit 1
fi

echo "[OK] SITL started (PID: $SITL_PID)"
echo ""

# Trap to kill SITL on exit
trap "echo ''; echo '[INFO] Stopping SITL...'; kill $SITL_PID 2>/dev/null; exit 0" INT TERM

# Run controller
echo "[INFO] Starting drone controller..."
echo ""
cd "$PROJECT_DIR"
python scripts/run_ardupilot.py "$@"

# Cleanup
kill $SITL_PID 2>/dev/null