#!/bin/bash
# 检查到未定义的变量则停止执行并报错
set -u
source /home/cicv/work/pji_desktop/config.sh

# 检查参数数量
if [ "$#" -ne 7 ]; then
  echo "Error: Incorrect number of arguments"
  echo "Usage: $0 RANDOM_FLAG COUNT OBSTACLE_FLAG DEFAULT_START_FLAG DEFAULT_END_FLAG START_POINT END_POINT"
  exit 1
fi

RANDOM_FLAG=$1
COUNT=$2
OBSTACLE_FLAG=$3
DEFAULT_START_FLAG=$4
DEFAULT_END_FLAG=$5
START_POINT=$(echo "$6" | tr ',' ' ')
END_POINT=$(echo "$7" | tr ',' ' ')
MAP_BAG_NAME="origin_map.bag"
#SIMULATION_PATH="/home/cicv/work/pji_desktop/simulation"
SIMULATION_RANDOM_LOG_PATH="$SIMULATION_PATH/logs/simulation_random.log"
SIMULATION_CUSTOM_LOG_PATH="$SIMULATION_PATH/logs/simulation_custom.log"
RECORD_BAG_LOG_PATH="$SIMULATION_PATH/logs/record_bag.log"
RECORD_BAG_FINISH_FLAG="killing the process"
EVALUATION_FINISH_FLAG="Evaluation finished"

# 自定义起终点，数量重置为1
if [ "$RANDOM_FLAG" == "false" ]; then
  COUNT=1
fi

# 激活conda环境
source activate simulation

# 清除历史文件
## 删除录制的rosbag
safe-rm -f "${SIMULATION_PATH:?}"/data/record_bag/*  # ${var:?}避免删除其他位置的文件，当变量不存在时会报错，而不是误删除根目录的文件
## 删除历史日志
if [ -f "$RECORD_BAG_LOG_PATH" ]; then
  safe-rm "$RECORD_BAG_LOG_PATH"
fi
if [ -f "$SIMULATION_RANDOM_LOG_PATH" ]; then
  safe-rm "$SIMULATION_RANDOM_LOG_PATH"
fi
if [ -f "$SIMULATION_CUSTOM_LOG_PATH" ]; then
  safe-rm "$SIMULATION_CUSTOM_LOG_PATH"
fi
# 判断是否为随机起终点
if [ "$RANDOM_FLAG" == "true" ]; then # 随机起终点
  echo "Use random start/end point"
  command="cd $SIMULATION_PATH/random_install && source install/setup.bash && rosrun random_point random_point $SIMULATION_PATH/data/mapBuf $COUNT"
  # 清除环境
  pkill -f "rosbag_record"
  pkill -f "random_point"
  sleep 0.5
  # 执行命令
  nohup script -q -c bash -c "$command" > "$SIMULATION_RANDOM_LOG_PATH" 2>&1 &
  disown
  # 录包
  command="python $SIMULATION_PATH/rosbag_record.py $SIMULATION_PATH/data/record_bag $COUNT"
  nohup script -q -c bash -c "$command" > "$RECORD_BAG_LOG_PATH" 2>&1 &
  disown
elif [ "$RANDOM_FLAG" == "false" ]; then # 给定起终点
  # 起点设置
  if [ "$DEFAULT_START_FLAG" == "true" ]; then
    # 默认起点
    echo "Use default start point"
    ## 配置默认起点
    command="cd $SIMULATION_PATH/custom_point && source install/setup.bash && rosrun goal_publish custom $SIMULATION_PATH/data/map_bag/$MAP_BAG_NAME"
    ### 清除环境
    pkill -f "goal_publish/initialPose"
    pkill -f "goal_publish/custom"
    sleep 1
    echo 'bash -c '\""$command"\"
    nohup bash -c "$command" >/dev/null 2>&1 &
    sleep 1
  elif [ "$DEFAULT_START_FLAG" == "false" ]; then
    # 自定义起点
    echo "Setting start point: $START_POINT"
    ## 发布自定义起点 - gazebo
    command="cd $SIMULATION_PATH/catkin_ws && catkin_make && source devel/setup.bash && rosrun goal_publish initialPose $START_POINT"
    ### 清除环境
    pkill -f "goal_publish/initialPose"
    sleep 0.5
    echo 'bash -c '\""$command"\"
    nohup bash -c "$command" >/dev/null 2>&1 &
    sleep 1

    ## 发布自定义起点 - rviz
    command="cd $SIMULATION_PATH/catkin_ws && catkin_make && source devel/setup.bash && rosrun goal_publish initialPose_rviz $START_POINT"
    ### 清除环境
    pkill -f "goal_publish/initialPose_rviz"
    sleep 0.5
    echo 'bash -c '\""$command"\"
    nohup bash -c "$command" >/dev/null 2>&1 &
    ### 这里必须设置不少于5s的延迟时间，否则rviz中的起点设置会出现问题
    sleep 5
    echo "Start point: $START_POINT has been set"
  fi

  # 终点设置
  if [ "$DEFAULT_END_FLAG" == "true" ]; then
    # 默认终点
    echo "Use default end point"
    ## 发布默认终点
    command="cd $SIMULATION_PATH/catkin_ws && catkin_make && source devel/setup.bash && rosrun goal_publish pub_node $SIMULATION_PATH/data/map_bag/$MAP_BAG_NAME $SIMULATION_PATH/data/map_bag/example.bag"
    ## 清除环境
    pkill -f "goal_publish/goal" # 下发自定义/默认终点前必须先清除goal，不能只清除pub_node
    pkill -f "goal_publish/pub_node"
    pkill -f "rosbag record"
    sleep 1
    echo 'bash -c '\""$command"\"
    ## 录包
    nohup python "$SIMULATION_PATH/rosbag_record.py" "$SIMULATION_PATH/data/record_bag/" 1 > "$RECORD_BAG_LOG_PATH" 2>&1 &
    disown
    ## 设置终点并执行
    nohup bash -c "$command" > "$SIMULATION_CUSTOM_LOG_PATH" 2>&1 &
    disown
  elif [ "$DEFAULT_END_FLAG" == "false" ]; then
    # 自定义终点
    echo "Setting end point: $END_POINT"
    ## 发布自定义终点
    command="cd $SIMULATION_PATH/catkin_ws && catkin_make && source devel/setup.bash && rosrun goal_publish goal $END_POINT"
    ## 清除环境
    pkill -f "goal_publish/goal"
    pkill -f "goal_publish/pub_node"
    pkill -f "rosbag record"
    sleep 1
    echo 'bash -c '\""$command"\"
    ## 录包
    nohup python "$SIMULATION_PATH/rosbag_record.py" "$SIMULATION_PATH/data/record_bag/" 1 > "$RECORD_BAG_LOG_PATH" 2>&1 &
    disown
    ## 设置终点并执行
    nohup bash -c "$command" > "$SIMULATION_CUSTOM_LOG_PATH" 2>&1 &
    disown
    echo "End point: $END_POINT has been set"
  fi
fi

# 是否加载障碍物
if [ "$OBSTACLE_FLAG" == "true" ]; then # 加载
  echo "Loading default obstacle..."
  # 动态加载障碍物
  command="cd $SIMULATION_PATH/pji_work && safe-rm -r build devel && catkin_make && source devel/setup.bash && rosrun test demo $SIMULATION_PATH/merge_obstacles_data/merged_obstacles_new.csv"
  nohup bash -c "$command" >/dev/null 2>&1 &
  sleep 0.5
  echo 'bash -c '"$command"
  echo "Default obstacle loaded"
fi

## 检查录包是否完成 - 执行算法评价
tail -F "$RECORD_BAG_LOG_PATH" | while read LINE; do
  echo "$LINE"
  # 检查录包完成标志
  if echo "$LINE" | grep -q "$RECORD_BAG_FINISH_FLAG"; then # 已完成
    # 启动算法评价
    ## 清理环境
#    killall -9 roscore # 如果提示未找到进程不用管，继续执行下一条
#    killall -9 rosmaster

    ## 清理历史文件
    safe-rm -rf "$SIMULATION_PATH"/data/evaluation/* # *必须在外面才代表通配符
    ## 配置环境
    cd "$SIMULATION_PATH"/track_space
    sudo safe-rm -rf "$SIMULATION_PATH/track_space/build"  "$SIMULATION_PATH/track_space/devel"
    catkin_make
    source devel/setup.bash
    ## 运行roscore
    nohup roscore >/dev/null 2>&1 &
    sleep 2
    ## 循环评价录制的rosbag
    for ((i = 1; i <= COUNT; i++)); do
      echo "Evaluating test-$i.bag..."
      save_folder="$SIMULATION_PATH/data/evaluation/$i"
      if [ ! -d "$save_folder" ]; then
        echo "Creating folder: $save_folder..."
        mkdir -p "$save_folder"
        mkdir -p "$save_folder/trackPicture"
        mkdir -p "$save_folder/result"
        echo "Folder: $save_folder created successfully"
      fi
      ## 生成轨迹图片
      echo "Creating the track picture..."
      rosrun trajectory demo_node "$SIMULATION_PATH/data/map_bag/map.bag" \
            "$SIMULATION_PATH/data/record_bag/" \
            "$SIMULATION_PATH/data/evaluation/$i/trackPicture/test.png"
      echo "The track picture: $SIMULATION_PATH/data/evaluation/$i/trackPicture/test.png created successfully"
      ## 执行算法评价
      cd "$SIMULATION_PATH"/pji_single/run

      ./pji_single "$SIMULATION_PATH/data/record_bag/test-$i.bag" \
            "$SIMULATION_PATH/data/evaluation/$i/result"  \
            "$SIMULATION_PATH/data/evaluation/$i/trackPicture/test.png"
      echo "Rosbag test-$i.bag evaluation results generated successfully"
    done
    echo "$EVALUATION_FINISH_FLAG"
    break
  fi
done