#!/bin/bash
# =============================================================================
# Record Gazebo Simulation to Video
# =============================================================================
# Records the Gazebo simulation window to a video file.
#
# Usage:
#   ./scripts/record_simulation.sh                    # Default: record for 60s
#   ./scripts/record_simulation.sh --duration 120     # Record for 120 seconds
#   ./scripts/record_simulation.sh --output my_video  # Custom output name
#   ./scripts/record_simulation.sh --method gazebo    # Use Gazebo's recorder
#
# Prerequisites:
#   - Gazebo must be running
#   - For ffmpeg method: sudo apt install ffmpeg
# =============================================================================

set -e

# Default values
DURATION=60
OUTPUT_NAME="drone_simulation_$(date +%Y%m%d_%H%M%S)"
OUTPUT_DIR="./recordings"
METHOD="ffmpeg"  # ffmpeg or gazebo
FPS=30
QUALITY="medium"  # low, medium, high

# Parse arguments
while [[ $# -gt 0 ]]; do
    case $1 in
        -d|--duration)
            DURATION="$2"
            shift 2
            ;;
        -o|--output)
            OUTPUT_NAME="$2"
            shift 2
            ;;
        --dir)
            OUTPUT_DIR="$2"
            shift 2
            ;;
        -m|--method)
            METHOD="$2"
            shift 2
            ;;
        --fps)
            FPS="$2"
            shift 2
            ;;
        -q|--quality)
            QUALITY="$2"
            shift 2
            ;;
        -h|--help)
            echo "Usage: $0 [OPTIONS]"
            echo ""
            echo "Options:"
            echo "  -d, --duration SEC   Recording duration in seconds (default: 60)"
            echo "  -o, --output NAME    Output filename without extension"
            echo "  --dir PATH           Output directory (default: ./recordings)"
            echo "  -m, --method METHOD  Recording method: ffmpeg or gazebo"
            echo "  --fps FPS            Frames per second (default: 30)"
            echo "  -q, --quality QUAL   Quality: low, medium, high (default: medium)"
            echo "  -h, --help           Show this help"
            exit 0
            ;;
        *)
            echo "Unknown option: $1"
            exit 1
            ;;
    esac
done

# Create output directory
mkdir -p "$OUTPUT_DIR"

# Quality presets
case $QUALITY in
    low)
        CRF=28
        PRESET="faster"
        ;;
    medium)
        CRF=23
        PRESET="medium"
        ;;
    high)
        CRF=18
        PRESET="slow"
        ;;
esac

echo "=============================================="
echo "  Gazebo Simulation Recorder"
echo "=============================================="
echo "  Duration: ${DURATION}s"
echo "  Output: ${OUTPUT_DIR}/${OUTPUT_NAME}.mp4"
echo "  Method: $METHOD"
echo "  FPS: $FPS"
echo "  Quality: $QUALITY"
echo "=============================================="
echo ""

if [ "$METHOD" = "gazebo" ]; then
    # ==========================================================================
    # METHOD 1: Gazebo's built-in video recording
    # ==========================================================================
    # Gazebo Harmonic can record video via the GUI or command line
    
    OUTPUT_FILE="${OUTPUT_DIR}/${OUTPUT_NAME}.mp4"
    
    echo "[INFO] Using Gazebo's built-in recorder..."
    echo "[INFO] In Gazebo GUI:"
    echo "       1. Click the hamburger menu (☰) in top-left"
    echo "       2. Select 'Video Recorder'"
    echo "       3. Click 'Record' to start, 'Stop' to stop"
    echo "       4. Video saves to ~/.gz/sim/recordings/"
    echo ""
    echo "[INFO] Or use gz service to record programmatically..."
    
    # Check if Gazebo is running
    if ! pgrep -x "gz" > /dev/null && ! pgrep -x "ruby" > /dev/null; then
        echo "[ERROR] Gazebo doesn't appear to be running"
        echo "[TIP] Start Gazebo first: ./scripts/run_ardupilot_sim.sh runway"
        exit 1
    fi
    
    # Start recording via gz service (if available)
    echo "[INFO] Sending record command to Gazebo..."
    gz service -s /gui/record_video --reqtype gz.msgs.VideoRecord --reptype gz.msgs.Boolean \
        --timeout 5000 --req "start: true, format: \"mp4\", save_filename: \"${OUTPUT_FILE}\"" 2>/dev/null || {
        echo "[WARN] Could not start recording via service"
        echo "[TIP] Use the Gazebo GUI to record manually"
    }
    
    echo "[INFO] Recording for ${DURATION} seconds..."
    sleep "$DURATION"
    
    # Stop recording
    gz service -s /gui/record_video --reqtype gz.msgs.VideoRecord --reptype gz.msgs.Boolean \
        --timeout 5000 --req "start: false" 2>/dev/null || true
    
    echo "[OK] Recording stopped"
    echo "[INFO] Check ~/.gz/sim/recordings/ for the video"

else
    # ==========================================================================
    # METHOD 2: FFmpeg screen recording
    # ==========================================================================
    # Records the Gazebo window using ffmpeg
    
    OUTPUT_FILE="${OUTPUT_DIR}/${OUTPUT_NAME}.mp4"
    
    # Check for ffmpeg
    if ! command -v ffmpeg &> /dev/null; then
        echo "[ERROR] ffmpeg not found"
        echo "[TIP] Install: sudo apt install ffmpeg"
        exit 1
    fi
    
    # Find the Gazebo window
    echo "[INFO] Looking for Gazebo window..."
    
    # Try to find the window ID
    WINDOW_ID=""
    
    # Try xdotool first
    if command -v xdotool &> /dev/null; then
        WINDOW_ID=$(xdotool search --name "Gazebo" 2>/dev/null | head -1) || true
    fi
    
    # Fallback to wmctrl
    if [ -z "$WINDOW_ID" ] && command -v wmctrl &> /dev/null; then
        WINDOW_ID=$(wmctrl -l | grep -i "gazebo" | awk '{print $1}' | head -1) || true
    fi
    
    if [ -n "$WINDOW_ID" ]; then
        # Get window geometry
        if command -v xdotool &> /dev/null; then
            eval $(xdotool getwindowgeometry --shell "$WINDOW_ID" 2>/dev/null) || true
        fi
        
        if [ -n "$WIDTH" ] && [ -n "$HEIGHT" ] && [ -n "$X" ] && [ -n "$Y" ]; then
            echo "[INFO] Found Gazebo window: ${WIDTH}x${HEIGHT} at +${X}+${Y}"
            GRAB_REGION="-video_size ${WIDTH}x${HEIGHT} -grab_x $X -grab_y $Y"
        else
            echo "[WARN] Could not get window geometry, recording full screen"
            GRAB_REGION=""
        fi
    else
        echo "[WARN] Could not find Gazebo window, recording full screen"
        GRAB_REGION=""
    fi
    
    echo "[INFO] Starting recording..."
    echo "[INFO] Press Ctrl+C to stop early"
    echo ""
    
    # Record using ffmpeg
    # Using x11grab to capture the screen
    ffmpeg -y \
        -f x11grab \
        -framerate "$FPS" \
        $GRAB_REGION \
        -i "${DISPLAY:-:0}" \
        -t "$DURATION" \
        -c:v libx264 \
        -preset "$PRESET" \
        -crf "$CRF" \
        -pix_fmt yuv420p \
        "$OUTPUT_FILE" \
        2>&1 | while IFS= read -r line; do
            # Show progress
            if [[ "$line" =~ time=([0-9:\.]+) ]]; then
                echo -ne "\r[Recording] Time: ${BASH_REMATCH[1]} / ${DURATION}s"
            fi
        done
    
    echo ""
    echo ""
    echo "[OK] Recording complete!"
    echo "[INFO] Output: $OUTPUT_FILE"
    
    # Get file info
    if [ -f "$OUTPUT_FILE" ]; then
        FILE_SIZE=$(du -h "$OUTPUT_FILE" | cut -f1)
        echo "[INFO] Size: $FILE_SIZE"
    fi
fi

echo ""
echo "=============================================="
echo "  Recording Complete"
echo "=============================================="