#!/bin/bash
# record_social_test.sh — Record a rosbag for social-bot integration test replay.
#
# Records all topics required by the social-bot CI test suite.
# Output: bags/social_test_YYYYMMDD_HHMMSS/  (ROS2 bag format)
#
# Usage:
#   # Record 30s with live hardware:
#   ./bags/record_social_test.sh --duration 30
#
#   # Record from already-running mock sensor stack:
#   MOCK_ONLY=1 ./bags/record_social_test.sh --duration 15
#
# Requirements: ROS2 Humble sourced, social-bot stack running

set -euo pipefail
SCRIPT_DIR="$(cd "$(dirname "$0")" && pwd)"
BAGS_DIR="$SCRIPT_DIR"

# ── Args ──────────────────────────────────────────────────────────────────────
DURATION=30
BAG_NAME="social_test_$(date +%Y%m%d_%H%M%S)"
MOCK_ONLY="${MOCK_ONLY:-0}"

while [[ $# -gt 0 ]]; do
    case "$1" in
        --duration) DURATION="$2"; shift 2 ;;
        --name)     BAG_NAME="$2";  shift 2 ;;
        *) echo "Unknown arg: $1"; exit 1 ;;
    esac
done

BAG_PATH="$BAGS_DIR/$BAG_NAME"

# ── Source ROS2 ───────────────────────────────────────────────────────────────
source /opt/ros/humble/setup.bash 2>/dev/null || true
if [ -f /ros2_ws/install/setup.bash ]; then
    source /ros2_ws/install/setup.bash
fi

echo "[record] Target bag: $BAG_PATH"
echo "[record] Duration:   ${DURATION}s"

# ── Topics to record ──────────────────────────────────────────────────────────
TOPICS=(
    # Mock / hardware sensor topics
    "/camera/color/image_raw"
    "/camera/color/camera_info"
    "/camera/depth/image_rect_raw"
    "/uwb/target"
    "/scan"
    "/odom"
    "/map"
    # Social-bot output topics
    "/social/faces/detections"
    "/social/faces/embeddings"
    "/social/tracking/fused_target"
    "/social/persons"
    "/social/orchestrator/state"
    "/social/speech/vad_state"
    "/social/speech/transcript"
    "/social/conversation/response"
    "/social/nav/mode"
    "/social/nav/status"
    "/social/attention/target_id"
    "/cmd_vel"
    # TF for coordinate frames
    "/tf"
    "/tf_static"
)

# ── Start mock sensor pub if MOCK_ONLY ────────────────────────────────────────
MOCK_PID=""
if [[ "$MOCK_ONLY" == "1" ]]; then
    echo "[record] Starting mock sensor publisher..."
    ros2 run saltybot_social_tests mock_sensor_pub &
    MOCK_PID=$!
    sleep 2
fi

# ── Record ────────────────────────────────────────────────────────────────────
echo "[record] Recording..."
ros2 bag record \
    --output "$BAG_PATH" \
    --storage sqlite3 \
    --max-bag-duration "$DURATION" \
    "${TOPICS[@]}" &
BAG_PID=$!

sleep "$DURATION"
wait "$BAG_PID" 2>/dev/null || true

# ── Cleanup ───────────────────────────────────────────────────────────────────
if [[ -n "$MOCK_PID" ]]; then
    kill "$MOCK_PID" 2>/dev/null || true
fi

echo "[record] Done. Bag saved to: $BAG_PATH"
echo "[record] Contents:"
ros2 bag info "$BAG_PATH" 2>/dev/null || echo "(ros2 bag info unavailable)"
