#!/bin/bash
# 检查到未定义的变量则停止执行并报错
set -u

# 检查参数数量
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"

# 激活conda环境
source activate simulation

# 清除历史文件
## 删除录制的rosbag
safe-rm -f "${SIMULATION_PATH:?}"/data/record_bag/* # ${var:?}避免删除其他位置的文件，当变量不存在时会报错，而不是误删除根目录的文件

# 判断是否为随机起终点
if [ "$RANDOM_FLAG" == "true" ]; then # 随机起终点
  command="cd $SIMULATION_PATH/random_install && source install/setup.bash && rosrun random_point random_point $SIMULATION_PATH/data/mapBuf $COUNT"
  # 清除环境
  pkill -f "rosbag_record"
  nohup script -q -c bash -c "$command" > "$SIMULATION_RANDOM_LOG_PATH" 2>&1 &
  # 录包
  nohup python "$SIMULATION_PATH/rosbag_record.py" "$SIMULATION_PATH/data/record_bag/" "$COUNT" > "$RECORD_BAG_LOG_PATH" 2>&1 &
elif [ "$RANDOM_FLAG" == "false" ]; then # 给定起终点
  # 起点设置
  if [ "$DEFAULT_START_FLAG" == "true" ]; then
    # 默认起点
    echo "Use default start point"
    ## 配置默认起点
    command="cd $SIMULATION_PATH/catkin_ws && catkin_make && source devel/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 1
    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 1
    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 && safe-rm -r build devel && catkin_make && source devel/setup.bash && rosrun goal_publish pub_node $SIMULATION_PATH/data/map_bag/$MAP_BAG_NAME $SIMULATION_PATH/data/old_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 &
    ## 设置终点并执行
    nohup bash -c "$command" > "$SIMULATION_CUSTOM_LOG_PATH" 2>&1 &
  elif [ "$DEFAULT_END_FLAG" == "false" ]; then
    # 自定义终点
    echo "Setting end point: $END_POINT"
    ## 发布自定义终点
    command="cd $SIMULATION_PATH/catkin_ws && safe-rm -r build devel && 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 &
    ## 设置终点并执行
    nohup bash -c "$command" > "$SIMULATION_CUSTOM_LOG_PATH" 2>&1 &
    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 && source devel/setup.bash && rosrun test demo $SIMULATION_PATH/merge_obstacles_data/merged_obstacles_new_0517_1.csv"
  nohup bash -c "$command" >/dev/null 2>&1 &
  sleep 0.5
  echo 'bash -c '"$command"
  echo "Default obstacle loaded"
fi