#!/bin/bash
# =============================================================================
# ArduPilot SITL + Controller Launcher
# =============================================================================
# Starts SITL and the drone controller together.
# Run Gazebo FIRST in another terminal!
#
# Usage:
#   Terminal 1: ./scripts/run_ardupilot_sim.sh runway
#   Terminal 2: ./scripts/run_ardupilot_controller.sh --pattern square
# =============================================================================

set -e

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

# =============================================================================
# ENVIRONMENT SETUP
# =============================================================================

# Deactivate any existing venv
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

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

echo "=============================================="
echo "  ArduPilot SITL + Controller"
echo "=============================================="
echo ""

# =============================================================================
# CHECKS
# =============================================================================

if ! command -v sim_vehicle.py &> /dev/null; then
    echo "[ERROR] sim_vehicle.py not found"
    echo ""
    echo "Fix: source ~/.ardupilot_env"
    echo " Or: export PATH=\$PATH:~/ardupilot/Tools/autotest"
    exit 1
fi

if ! python3 -c "import em" 2>/dev/null; then
    echo "[ERROR] empy not found"
    echo ""
    echo "Fix: pip install empy==3.3.4"
    exit 1
fi

echo "[OK] Environment ready"
echo "[INFO] Python: $(which python3)"
echo ""

# =============================================================================
# START SITL
# =============================================================================

echo "[INFO] Starting ArduCopter SITL..."
echo "[INFO] Make sure Gazebo is running first!"
echo ""

cd ~/ardupilot
sim_vehicle.py -v ArduCopter -f gazebo-iris --model JSON --console &
SITL_PID=$!

# Wait for SITL to be ready
echo "[INFO] Waiting for SITL (TCP 5760)..."
TRIES=0
while ! nc -z 127.0.0.1 5760 2>/dev/null; do
    sleep 1
    TRIES=$((TRIES + 1))
    if [ $TRIES -ge 90 ]; then
        echo ""
        echo "[ERROR] SITL timeout (90s)"
        kill $SITL_PID 2>/dev/null || true
        exit 1
    fi
    if ! kill -0 $SITL_PID 2>/dev/null; then
        echo ""
        echo "[ERROR] SITL process died"
        echo "[TIP] Is Gazebo running? Start it first with:"
        echo "      ./scripts/run_ardupilot_sim.sh runway"
        exit 1
    fi
    echo -n "."
done
echo ""
echo "[OK] SITL ready on port 5760"
echo ""

# =============================================================================
# CLEANUP
# =============================================================================

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

# Wait for SITL to stabilize
sleep 3

# =============================================================================
# RUN CONTROLLER
# =============================================================================

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

# Cleanup
kill $SITL_PID 2>/dev/null || true