#!/bin/bash
#===============================================================================
# 加减速映射表标定启动脚本
# 位置: /home/nvidia/FaRui/v3.1/calibrate_accel_brake_map.sh
#===============================================================================

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

# 颜色定义
RED='\033[0;31m'
GREEN='\033[0;32m'
YELLOW='\033[1;33m'
BLUE='\033[0;34m'
NC='\033[0m' # No Color

# 默认参数
VEHICLE_MODEL="ecar_vehicle"
USE_SIM_TIME="false"
RVIZ="true"
RECORD_BAG="false"
UPDATE_METHOD="update_offset_four_cell_around"

# 帮助信息
show_help() {
    echo "============================================================"
    echo "        加减速映射表标定工具"
    echo "============================================================"
    echo ""
    echo "用法: ./calibrate_accel_brake_map.sh [选项]"
    echo ""
    echo "选项:"
    echo "  -m, --model <车型>       车辆模型名称 (默认: ecar404)"
    echo "  -s, --sim-time          使用仿真时间 (rosbag回放时使用)"
    echo "  -n, --no-rviz           不启动RViz"
    echo "  -r, --record            录制数据到rosbag"
    echo "  -c, --cell              使用逐网格标定算法 (默认: 四邻域)"
    echo "  -h, --help              显示此帮助信息"
    echo ""
    echo "标定模式:"
    echo "  1. 实车标定:     ./calibrate_accel_brake_map.sh"
    echo "  2. rosbag标定:   ./calibrate_accel_brake_map.sh -s"
    echo "  3. 高精度标定:   ./calibrate_accel_brake_map.sh -c"
    echo ""
    echo "============================================================"
}

# 解析参数
while [[ $# -gt 0 ]]; do
    case $1 in
        -m|--model)
            VEHICLE_MODEL="$2"
            shift 2
            ;;
        -s|--sim-time)
            USE_SIM_TIME="true"
            shift
            ;;
        -n|--no-rviz)
            RVIZ="false"
            shift
            ;;
        -r|--record)
            RECORD_BAG="true"
            shift
            ;;
        -c|--cell)
            UPDATE_METHOD="update_offset_each_cell"
            shift
            ;;
        -h|--help)
            show_help
            exit 0
            ;;
        *)
            echo -e "${RED}错误: 未知参数 $1${NC}"
            show_help
            exit 1
            ;;
    esac
done

# 获取脚本所在目录
SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
cd "$SCRIPT_DIR"

echo "============================================================"
echo -e "${BLUE}        加减速映射表标定工具${NC}"
echo "============================================================"
echo ""

# 检查工作空间
if [ ! -f "install/setup.bash" ]; then
    echo -e "${RED}错误: 未找到 install/setup.bash${NC}"
    echo "请先编译工作空间: colcon build"
    exit 1
fi

# Source 环境
echo -e "${BLUE}[1/6] 加载 ROS2 环境...${NC}"
source /opt/ros/humble/setup.bash
source install/setup.bash

# 检查 launch 文件是否存在
LAUNCH_FILE="src/Launcher/farui_launch/launch/calibration/accel_brake_map_calibration.launch.xml"
if [ ! -f "$LAUNCH_FILE" ]; then
    echo -e "${RED}错误: 未找到标定 launch 文件${NC}"
    echo "路径: $LAUNCH_FILE"
    exit 1
fi

# 创建标定结果目录
CALIBRATION_DIR="$SCRIPT_DIR/src/Launcher/farui_launch/config/vehicle/calibrated_maps"
mkdir -p "$CALIBRATION_DIR"

echo -e "${GREEN}✓ 环境加载完成${NC}"
echo ""

# 显示配置信息
echo -e "${BLUE}[2/6] 标定配置信息:${NC}"
echo "  车辆模型:     $VEHICLE_MODEL"
echo "  仿真时间:     $USE_SIM_TIME"
echo "  RViz可视化:   $RVIZ"
echo "  录制数据:     $RECORD_BAG"
echo "  标定算法:     $UPDATE_METHOD"
echo "  结果保存路径: $CALIBRATION_DIR"
echo ""

# 检查车辆模型是否存在
VEHICLE_PKG="${VEHICLE_MODEL}_description"
if ! ros2 pkg list | grep "^$VEHICLE_PKG$" > /dev/null 2>&1; then
    echo -e "${YELLOW}警告: 未找到车辆模型包 $VEHICLE_PKG${NC}"
    echo "可用的车辆模型:"
    ros2 pkg list | grep "_description$" | sed 's/_description$//' | head -10
    echo ""
    read -p "是否继续? (y/n) " -n 1 -r
    echo
    if [[ ! $REPLY =~ ^[Yy]$ ]]; then
        exit 1
    fi
fi

# 如果录制数据，创建bag目录
if [ "$RECORD_BAG" = "true" ]; then
    BAG_DIR="$SCRIPT_DIR/calibration_bags"
    mkdir -p "$BAG_DIR"
    BAG_NAME="calibration_$(date +%Y%m%d_%H%M%S)"
fi

# 启动标定
echo -e "${BLUE}[3/6] 启动标定节点...${NC}"
echo ""
echo -e "${YELLOW}提示: 标定过程中请进行以下操作:${NC}"
echo "  1. 确保车辆在安全开阔场地"
echo "  2. 使用遥控器控制车辆"
echo "  3. 平稳加速、减速，覆盖各种速度"
echo "  4. 观察RViz中单元格变红表示数据充足"
echo "  5. 重点关注 0-6m/s 低速区间"
echo ""
read -p "按回车键开始标定，或按 Ctrl+C 取消..."
echo ""

# 构建 launch 参数
LAUNCH_ARGS="vehicle_model:=$VEHICLE_MODEL"
LAUNCH_ARGS="$LAUNCH_ARGS use_sim_time:=$USE_SIM_TIME"
LAUNCH_ARGS="$LAUNCH_ARGS rviz:=$RVIZ"
LAUNCH_ARGS="$LAUNCH_ARGS update_method:=$UPDATE_METHOD"
LAUNCH_ARGS="$LAUNCH_ARGS progress_file_output:=true"
LAUNCH_ARGS="$LAUNCH_ARGS pedal_accel_graph_output:=true"

# 启动 rosbag 录制（后台）
if [ "$RECORD_BAG" = "true" ]; then
    echo -e "${BLUE}[4/6] 启动数据录制...${NC}"
    ros2 bag record -o "$BAG_DIR/$BAG_NAME" \
        /vehicle/status/velocity_status \
        /vehicle/status/steering_status \
        /vehicle/status/actuation_status \
        /control/command/actuation_cmd \
        /tf \
        /tf_static \
        &
    BAG_PID=$!
    echo -e "${GREEN}✓ 数据录制已启动，PID: $BAG_PID${NC}"
    echo "  保存路径: $BAG_DIR/$BAG_NAME"
    echo ""
fi

# 启动标定
echo -e "${BLUE}[5/6] 启动标定器...${NC}"
echo "  命令: ros2 launch farui_launch accel_brake_map_calibration.launch.xml $LAUNCH_ARGS"
echo ""

# 捕获 Ctrl+C 信号
cleanup() {
    echo ""
    echo -e "${YELLOW}接收到中断信号，正在清理...${NC}"
    if [ "$RECORD_BAG" = "true" ] && [ -n "$BAG_PID" ]; then
        echo "停止数据录制..."
        kill $BAG_PID 2>/dev/null || true
        wait $BAG_PID 2>/dev/null || true
    fi
    echo -e "${GREEN}✓ 已清理${NC}"
    exit 0
}
trap cleanup SIGINT SIGTERM

# 启动标定节点
ros2 launch farui_launch accel_brake_map_calibration.launch.xml $LAUNCH_ARGS &
LAUNCH_PID=$!

# 等待标定节点启动
sleep 3

# 检查节点是否正常运行
if ! ps -p $LAUNCH_PID > /dev/null; then
    echo -e "${RED}错误: 标定节点启动失败${NC}"
    exit 1
fi

echo -e "${GREEN}✓ 标定节点已启动${NC}"
echo ""

# 显示操作提示
echo "============================================================"
echo -e "${BLUE}                    标定操作指南${NC}"
echo "============================================================"
echo ""
echo "1. ${YELLOW}驾驶车辆${NC}: 使用遥控器控制车辆"
echo "   - 平稳加速、减速"
echo "   - 覆盖 0-最大车速 范围"
echo "   - 覆盖 0-最大踏板开度 范围"
echo ""
echo "2. ${YELLOW}观察进度${NC}: 查看RViz可视化"
echo "   - 灰色: 无数据"
echo "   - 蓝色→红色: 数据逐渐充足"
echo "   - 目标: 所有单元格变红"
echo ""
echo "3. ${YELLOW}保存结果${NC}: 新开终端执行"
echo "   ros2 service call /accel_brake_map_calibrator/update_map_dir \\"
echo "     tier4_vehicle_msgs/srv/UpdateAccelBrakeMap \\"
echo "     \"path: '$CALIBRATION_DIR'\""
echo ""
echo "4. ${YELLOW}查看误差${NC}:"
echo "   ros2 topic echo /accel_brake_map_calibrator/output/map_error_ratio"
echo "   (比值 < 0.7 表示标定有效)"
echo ""
echo "5. ${YELLOW}结束标定${NC}: 在此窗口按 Ctrl+C"
echo ""
echo "============================================================"
echo ""

# 等待用户结束
wait $LAUNCH_PID

# 标定结束后的提示
echo ""
echo "============================================================"
echo -e "${BLUE}                    标定已结束${NC}"
echo "============================================================"
echo ""
echo -e "${GREEN}标定结果保存在:${NC}"
echo "  $CALIBRATION_DIR/"
echo ""
echo "生成的文件:"
ls -lh "$CALIBRATION_DIR/" 2>/dev/null || echo "  (目录为空，请检查是否已调用保存服务)"
echo ""
echo "下一步:"
echo "  1. 检查 accel_map.csv 和 brake_map.csv 是否生成"
echo "  2. 修改 raw_vehicle_cmd_converter 配置使用新表"
echo "  3. 重启 Autoware 验证效果"
echo ""
echo "============================================================"


# ros2 service call /accel_brake_map_calibrator/input/update_map_dir   tier4_vehicle_msgs/srv/UpdateAccelBrakeMap   "path: '/home/nvidia/FaRui-test/v3.2/src/Launcher/farui_launch/config/vehicle/calibrated_maps'"