#!/bin/bash
# ECar Mapping Script - 在线建图启动脚本
# 使用方法: ./mapping.sh [选项]

set -e  # 遇到错误立即退出

# ============ 配置 ============
WORKSPACE_DIR="/home/nvidia/FaRui/v3.1"
MAP_SAVE_PATH="${WORKSPACE_DIR}/maps/$(date +%Y%m%d_%H%M%S)"
SLAM_METHOD="liorf"  # 可选: fast_lio2 或 liorf
RECORD_BAG="false"
LAUNCH_RVIZ="true"

# ============ 帮助信息 ============
function show_help() {
    echo "ECar Mapping Script - 在线建图启动脚本"
    echo ""
    echo "用法: $0 [选项]"
    echo ""
    echo "选项:"
    echo "  -h, --help              显示帮助信息"
    echo "  -m, --method METHOD     选择SLAM算法: fast_lio2 或 liorf (默认: ${SLAM_METHOD})"
    echo "  -s, --save PATH         地图保存路径 (默认: ${MAP_SAVE_PATH})"
    echo "  -r, --record            录制rosbag备份"
    echo "  --no-rviz               不启动RViz"
    echo "  -b, --bag-path PATH     指定rosbag录制路径"
    echo ""
    echo "示例:"
    echo "  $0                      # 默认使用liorf建图"
    echo "  $0 -m fast_lio2         # 使用FAST_LIO2建图"
    echo "  $0 -r -s /home/nvidia/my_maps   # 建图并录制bag到指定目录"
    echo "  $0 --no-rviz            # 建图但不启动RViz"
    exit 0
}

# ============ 解析参数 ============
while [[ $# -gt 0 ]]; do
    case $1 in
        -h|--help)
            show_help
            ;;
        -m|--method)
            SLAM_METHOD="$2"
            shift 2
            ;;
        -s|--save)
            MAP_SAVE_PATH="$2"
            shift 2
            ;;
        -r|--record)
            RECORD_BAG="true"
            shift
            ;;
        --no-rviz)
            LAUNCH_RVIZ="false"
            shift
            ;;
        -b|--bag-path)
            BAG_SAVE_PATH="$2"
            shift 2
            ;;
        *)
            echo "未知参数: $1"
            show_help
            ;;
    esac
done

# ============ 验证参数 ============
if [[ "${SLAM_METHOD}" != "fast_lio2" && "${SLAM_METHOD}" != "liorf" ]]; then
    echo "错误: 不支持的SLAM算法 '${SLAM_METHOD}'"
    echo "支持的算法: fast_lio2, liorf"
    exit 1
fi

# 创建地图保存目录
mkdir -p "${MAP_SAVE_PATH}"
echo "地图将保存到: ${MAP_SAVE_PATH}"

# ============ 环境设置 ============
echo "================================"
echo "  ECar 在线建图启动"
echo "================================"
echo "工作目录: ${WORKSPACE_DIR}"
echo "SLAM算法: ${SLAM_METHOD}"
echo "RViz: ${LAUNCH_RVIZ}"
echo "录制bag: ${RECORD_BAG}"
echo "================================"

# 切换到工作目录
cd "${WORKSPACE_DIR}"

# 加载ROS环境
echo "[1/3] 加载ROS2环境..."
if [ -f "install/setup.bash" ]; then
    source install/setup.bash
    echo "    已加载本地工作空间"
else
    echo "    警告: 未找到本地工作空间,使用系统ROS2"
    source /opt/ros/humble/setup.bash
fi

# 检查必要的包是否存在
echo "[2/3] 检查必要组件..."
REQUIRED_PKGS=("farui_launch" "timoo_ros2_driver" "wheeltec_gps_driver" "hipnuc_imu")
for pkg in "${REQUIRED_PKGS[@]}"; do
    if ros2 pkg list | grep -q "^${pkg}$"; then
        echo "    ✓ ${pkg}"
    else
        echo "    ✗ ${pkg} 未找到!"
        exit 1
    fi
done

# 启动建图
echo "[3/3] 启动建图节点..."
echo ""

# 构建启动参数
LAUNCH_ARGS=""
LAUNCH_ARGS+=" slam_method:=${SLAM_METHOD}"
LAUNCH_ARGS+=" map_save_path:=${MAP_SAVE_PATH}"
LAUNCH_ARGS+=" record_bag:=${RECORD_BAG}"
LAUNCH_ARGS+=" rviz:=${LAUNCH_RVIZ}"

if [ -n "${BAG_SAVE_PATH}" ]; then
    LAUNCH_ARGS+=" map_save_path:=${BAG_SAVE_PATH}"
fi

echo "启动参数: ${LAUNCH_ARGS}"
echo ""

# 启动 launch 文件
ros2 launch farui_launch ecar_mapping.launch.xml ${LAUNCH_ARGS}

# 建图结束后提示
echo ""
echo "================================"
echo "  建图已结束"
echo "================================"
if [ -d "${MAP_SAVE_PATH}" ]; then
    echo "地图保存位置: ${MAP_SAVE_PATH}"
    ls -lh "${MAP_SAVE_PATH}" 2>/dev/null || true
fi
echo "================================"