#!/bin/bash
set -e

echo "=========================================="
echo "UAV-UGV Simulation Setup"
echo "Ubuntu 22.04 + Python 3.10 + ROS 2 Humble"
echo "GPS-Denied Navigation with Geofencing"
echo "=========================================="
echo ""

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

echo "[1/7] Updating system packages..."
sudo apt-get update

echo "[2/7] Installing system dependencies..."
sudo apt-get install -y \
    python3-pip \
    python3-venv \
    python3-opencv \
    libopencv-dev \
    ros-humble-mavros \
    ros-humble-mavros-extras \
    ros-humble-cv-bridge \
    ros-humble-image-transport \
    ros-humble-gazebo-ros-pkgs \
    ros-humble-tf2 \
    ros-humble-tf2-ros \
    ros-humble-tf2-geometry-msgs

echo "[3/7] Installing MAVROS GeographicLib datasets..."
if [ ! -f /usr/share/GeographicLib/geoids/egm96-5.pgm ]; then
    sudo /opt/ros/humble/lib/mavros/install_geographiclib_datasets.sh || true
fi

echo "[4/7] Creating Python virtual environment..."
if [ ! -d "venv" ]; then
    python3 -m venv venv
fi
source venv/bin/activate

echo "[5/7] Installing Python dependencies..."
pip install --upgrade pip
pip install -r requirements.txt

echo "[6/7] Building ROS 2 package..."
source /opt/ros/humble/setup.bash

cd ..
if [ -d "$(dirname "$SCRIPT_DIR")/src" ]; then
    cd "$(dirname "$SCRIPT_DIR")"
fi

if [ -f "$SCRIPT_DIR/package.xml" ]; then
    colcon build --packages-select uav_ugv_simulation --symlink-install 2>/dev/null || \
    echo "Colcon build skipped - run from ROS workspace"
fi

cd "$SCRIPT_DIR"

echo "[7/7] Making scripts executable..."
chmod +x scripts/*.sh

cat > activate_venv.sh << 'EOF'
#!/bin/bash
SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
source "$SCRIPT_DIR/venv/bin/activate"
source /opt/ros/humble/setup.bash
if [ -f "$SCRIPT_DIR/install/setup.bash" ]; then
    source "$SCRIPT_DIR/install/setup.bash"
fi
export GAZEBO_MODEL_PATH="$SCRIPT_DIR/models:$GAZEBO_MODEL_PATH"
export GAZEBO_RESOURCE_PATH="$SCRIPT_DIR/worlds:$GAZEBO_RESOURCE_PATH"
echo "Environment activated."
echo "Run: bash scripts/run_simulation.sh"
EOF
chmod +x activate_venv.sh

echo ""
echo "=========================================="
echo "Setup Complete!"
echo "=========================================="
echo ""
echo "Next steps:"
echo "  1. source ~/.bashrc"
echo "  2. source activate_venv.sh"
echo "  3. bash scripts/run_simulation.sh"
echo ""
echo "GPS-Denied Navigation:"
echo "  - All navigation uses LOCAL coordinates"
echo "  - GPS is ONLY used for geofencing"
echo ""