#!/bin/bash
# =============================================================================
# ArduPilot Flight Demo with Camera Viewer
# =============================================================================
# Runs the drone controller and camera viewer together.
#
# Usage: ./scripts/run_flight_demo.sh [options]
#
# Options:
#   --pattern square|circle|hover    Flight pattern (default: square)
#   --altitude HEIGHT                Flight altitude in meters (default: 5)
#   --size SIZE                      Pattern size in meters (default: 5)
#   --no-camera                      Skip camera viewer
# =============================================================================

set -e

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

# Parse arguments
PATTERN="square"
ALTITUDE="5"
SIZE="5"
SHOW_CAMERA=true

while [[ $# -gt 0 ]]; do
    case $1 in
        --pattern|-p)
            PATTERN="$2"
            shift 2
            ;;
        --altitude|-a)
            ALTITUDE="$2"
            shift 2
            ;;
        --size|-s)
            SIZE="$2"
            shift 2
            ;;
        --no-camera)
            SHOW_CAMERA=false
            shift
            ;;
        *)
            shift
            ;;
    esac
done

# 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 Flight Demo"
echo "=============================================="
echo ""
echo "  Pattern:  $PATTERN"
echo "  Altitude: ${ALTITUDE}m"
echo "  Size:     ${SIZE}m"
echo "  Camera:   $SHOW_CAMERA"
echo ""
echo "=============================================="
echo ""

# Check if ROS 2 is available for camera viewer
ROS2_AVAILABLE=false
if command -v ros2 &> /dev/null; then
    ROS2_AVAILABLE=true
fi

# Start camera viewer in background if requested and ROS 2 is available
CAMERA_PID=""
if [ "$SHOW_CAMERA" = true ]; then
    if [ "$ROS2_AVAILABLE" = true ]; then
        echo "[INFO] Starting camera viewer..."
        source /opt/ros/humble/setup.bash 2>/dev/null || source /opt/ros/jazzy/setup.bash 2>/dev/null || true
        python "$PROJECT_DIR/src/camera_viewer.py" --topic /camera/image_raw &
        CAMERA_PID=$!
        sleep 2
    else
        echo "[WARN] ROS 2 not available - camera viewer disabled"
        echo "[INFO] To view camera, run Gazebo with camera in the world"
    fi
fi

# Cleanup function
cleanup() {
    echo ""
    echo "[INFO] Cleaning up..."
    if [ -n "$CAMERA_PID" ]; then
        kill $CAMERA_PID 2>/dev/null || true
    fi
}
trap cleanup EXIT INT TERM

# Run the flight controller
echo "[INFO] Starting flight controller..."
cd "$PROJECT_DIR"
python scripts/run_ardupilot.py --pattern "$PATTERN" --altitude "$ALTITUDE" --size "$SIZE"

echo ""
echo "[OK] Flight demo complete!"