#!/usr/bin/env bash
# 本地数据输入回归测试。
#
# 验证链路：
# dataset_index.yaml -> site_data_input.yaml -> external -> chassis -> control -> sensor_intrinsic -> report
#
# 这个脚本不启动 Isaac，不依赖 GPU；用于快速检查算法模板和数据输入注入合同。

set -Eeuo pipefail

WORKSPACE_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
ROS_LOG_DIR="${ROS_LOG_DIR:-/tmp/roslog}"
RUN_LOG_ROOT="${RUN_LOG_ROOT:-${WORKSPACE_DIR}/log/local_data_input_smoke_$(date +%Y%m%d_%H%M%S)}"

SITE_PROFILE="src/deployment/profiles/site_template.yaml"
SESSION_ID="session_001"
SESSION_DIR="/tmp/agv_calib_local_data_input_smoke/session_001"
DATASET_INDEX_FILE=""
DATA_INPUT_PARAMS_FILE=""
TASKS="external,chassis,control,sensor_intrinsic"
KEEP_RUNNING=0
SKIP_GENERATE=0
STRICT_INDEX=1

PIDS=()
NAMES=()
CLEANED=0

usage() {
  cat <<'EOF'
用法:
  ./run_local_data_input_smoke.sh [选项]

用途:
  不启动 Isaac，只启动本地 ROS 标定服务和总控，验证数据输入参数注入与 report 链路。

选项:
  --site-profile PATH          现场 profile，默认 src/deployment/profiles/site_template.yaml。
  --session-id ID              自动生成样例数据集时使用的 session_id，默认 session_001。
  --session-dir PATH           自动生成样例数据集时使用的会话目录。
  --data-input-params-file PATH
                              使用已有 site_data_input.yaml，跳过自动生成。
  --dataset-index PATH        使用已有 site_data_input.yaml 时，对应的 dataset_index.yaml。
  --tasks LIST                 逗号分隔任务列表，默认 external,chassis,control,sensor_intrinsic。
  --skip-generate              不生成 dataset_index/site_data_input，只使用 --data-input-params-file。
  --no-strict-index            关闭 dataset_index 严格任务输入校验。
  --keep-running               smoke 结束后保持 launch 进程运行。
  -h, --help                   显示帮助。

示例:
  ./run_local_data_input_smoke.sh
  ./run_local_data_input_smoke.sh --data-input-params-file /data/agv_calib/session_001/site_data_input.yaml
EOF
}

log() {
  echo "[INFO] $*"
}

warn() {
  echo "[WARN] $*" >&2
}

die() {
  echo "[ERROR] $*" >&2
  exit 1
}

resolve_workspace_path() {
  local path="$1"
  case "${path}" in
    /*)
      echo "${path}"
      ;;
    *)
      echo "${WORKSPACE_DIR}/${path}"
      ;;
  esac
}

source_ros_env() {
  set +u
  if [[ -f /opt/ros/humble/setup.bash ]]; then
    source /opt/ros/humble/setup.bash
  fi
  source "${WORKSPACE_DIR}/install/setup.bash"
  set -u
}

cleanup() {
  if [[ "${CLEANED}" -eq 1 ]]; then
    return
  fi
  CLEANED=1

  local i pid name
  for ((i=${#PIDS[@]}-1; i>=0; i--)); do
    pid="${PIDS[$i]}"
    name="${NAMES[$i]}"
    if kill -0 "${pid}" >/dev/null 2>&1; then
      log "停止 ${name}: pid=${pid}"
      kill -INT "${pid}" >/dev/null 2>&1 || true
    fi
  done
  sleep 2
  for ((i=${#PIDS[@]}-1; i>=0; i--)); do
    pid="${PIDS[$i]}"
    name="${NAMES[$i]}"
    if kill -0 "${pid}" >/dev/null 2>&1; then
      warn "${name} 未退出，发送 SIGTERM: pid=${pid}"
      kill -TERM "${pid}" >/dev/null 2>&1 || true
    fi
  done
}

trap cleanup EXIT

while [[ $# -gt 0 ]]; do
  case "$1" in
    --site-profile)
      [[ $# -ge 2 ]] || die "--site-profile 需要参数"
      SITE_PROFILE="$2"
      shift 2
      ;;
    --session-id)
      [[ $# -ge 2 ]] || die "--session-id 需要参数"
      SESSION_ID="$2"
      shift 2
      ;;
    --session-dir)
      [[ $# -ge 2 ]] || die "--session-dir 需要参数"
      SESSION_DIR="$2"
      shift 2
      ;;
    --data-input-params-file)
      [[ $# -ge 2 ]] || die "--data-input-params-file 需要参数"
      DATA_INPUT_PARAMS_FILE="$2"
      SKIP_GENERATE=1
      shift 2
      ;;
    --dataset-index)
      [[ $# -ge 2 ]] || die "--dataset-index 需要参数"
      DATASET_INDEX_FILE="$2"
      shift 2
      ;;
    --tasks)
      [[ $# -ge 2 ]] || die "--tasks 需要参数"
      TASKS="$2"
      shift 2
      ;;
    --skip-generate)
      SKIP_GENERATE=1
      shift
      ;;
    --no-strict-index)
      STRICT_INDEX=0
      shift
      ;;
    --keep-running)
      KEEP_RUNNING=1
      shift
      ;;
    -h|--help)
      usage
      exit 0
      ;;
    *)
      die "未知参数: $1"
      ;;
  esac
done

SITE_PROFILE="$(resolve_workspace_path "${SITE_PROFILE}")"
SESSION_DIR="$(resolve_workspace_path "${SESSION_DIR}")"
if [[ -n "${DATA_INPUT_PARAMS_FILE}" ]]; then
  DATA_INPUT_PARAMS_FILE="$(resolve_workspace_path "${DATA_INPUT_PARAMS_FILE}")"
fi
if [[ -n "${DATASET_INDEX_FILE}" ]]; then
  DATASET_INDEX_FILE="$(resolve_workspace_path "${DATASET_INDEX_FILE}")"
fi

[[ -f "${SITE_PROFILE}" ]] || die "找不到 site profile: ${SITE_PROFILE}"
[[ -f "${WORKSPACE_DIR}/install/setup.bash" ]] || die "找不到 install/setup.bash，请先执行 colcon build"

mkdir -p "${ROS_LOG_DIR}" "${RUN_LOG_ROOT}"

source_ros_env

if [[ "${SKIP_GENERATE}" -eq 0 ]]; then
  log "生成本地样例数据集索引和数据输入参数文件"
  python3 "${WORKSPACE_DIR}/src/site_deployment/workshop_sensor_ingest_real/site_session_capture.py" \
    --site-profile "${SITE_PROFILE}" \
    --session-dir "${SESSION_DIR}" \
    --session-id "${SESSION_ID}" \
    --site-id local_smoke \
    --vehicle-id demo_agv_001 \
    --start-us 1770000000000000 \
    --end-us 1770000120000000 \
    --overwrite \
    --file external.external_observation_files=external/external_observations.csv \
    --file chassis.chassis_motion_data_files=chassis/chassis_motion.csv \
    --file control.control_evaluation_data_files=control/control_eval.csv \
    --file sensor.image_sample_files=sensor/front_camera/images.yaml \
    --file sensor.target_detection_files=sensor/front_camera/charuco_detections.json \
    --finalize
  DATA_INPUT_PARAMS_FILE="${SESSION_DIR}/site_data_input.yaml"
  DATASET_INDEX_FILE="${SESSION_DIR}/dataset_index.yaml"
elif [[ -z "${DATASET_INDEX_FILE}" ]]; then
  DATASET_INDEX_FILE="$(dirname "${DATA_INPUT_PARAMS_FILE}")/dataset_index.yaml"
fi

[[ -f "${DATA_INPUT_PARAMS_FILE}" ]] || die "找不到数据输入参数文件: ${DATA_INPUT_PARAMS_FILE}"

if [[ "${STRICT_INDEX}" -eq 1 ]]; then
  if [[ -f "${DATASET_INDEX_FILE}" ]]; then
    log "校验数据集索引: ${DATASET_INDEX_FILE}"
    python3 "${WORKSPACE_DIR}/src/deployment/tools/validate_dataset_index.py" \
      "${DATASET_INDEX_FILE}" \
      --tasks "${TASKS}"
  else
    warn "未找到 dataset_index.yaml，跳过索引校验: ${DATASET_INDEX_FILE}"
  fi
fi

LAUNCH_LOG="${RUN_LOG_ROOT}/launch.log"
log "启动本地标定服务和总控，日志: ${LAUNCH_LOG}"
ros2 launch win_ubuntu_bridge minimal_workshop_demo.launch.py \
  use_gateway:=false \
  data_input_params_file:="${DATA_INPUT_PARAMS_FILE}" \
  dataset_index_file:="${DATASET_INDEX_FILE}" \
  sensor_registry:=demo_front_camera,demo_down_camera,demo_lidar_3d,demo_lidar_2d,demo_imu \
  >"${LAUNCH_LOG}" 2>&1 &
PIDS+=("$!")
NAMES+=("local-workshop-demo")

log "等待 /workshop_v2/create_session"
for _ in $(seq 1 60); do
  if ros2 service list 2>/dev/null | grep -qx /workshop_v2/create_session; then
    break
  fi
  sleep 1
done
ros2 service list 2>/dev/null | grep -qx /workshop_v2/create_session || {
  tail -n 120 "${LAUNCH_LOG}" >&2 || true
  die "等待 /workshop_v2/create_session 超时"
}

log "确认四个算法服务已读取 data_input 参数"
ros2 param get /external_localization_service data_input.external_observation_files
ros2 param get /chassis_calibration_service data_input.chassis_motion_data_files
ros2 param get /control_calibration_service data_input.control_evaluation_data_files
ros2 param get /sensor_calibration_service data_input.synchronized_dataset_files
ros2 param get /workshop_orchestrator_v2 data_input_params_file
ros2 param get /workshop_orchestrator_v2 dataset_index_file

SMOKE_LOG="${RUN_LOG_ROOT}/smoke.log"
log "执行本地四阶段 smoke，日志: ${SMOKE_LOG}"
python3 "${WORKSPACE_DIR}/src/simulation/tools/smoke_test_workshop_orchestrator.py" \
  --tasks "${TASKS}" \
  --verbose-feedback \
  --disable-wifi6-precheck \
  2>&1 | tee "${SMOKE_LOG}"

grep -q "site_data_input.yaml" "${SMOKE_LOG}" || die "报告中没有记录 site_data_input.yaml"
if [[ -f "${DATASET_INDEX_FILE}" ]]; then
  grep -q "dataset_index.yaml" "${SMOKE_LOG}" || die "报告中没有记录 dataset_index.yaml"
fi

if [[ "${KEEP_RUNNING}" -eq 1 ]]; then
  log "本地服务保持运行中。按 Ctrl+C 停止。"
  while true; do
    sleep 3600
  done
fi

log "本地数据输入回归测试通过"
