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

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

OBSTACLE_FLAG=$1
DEFAULT_START_FLAG=$2
DEFAULT_END_FLAG=$3
START_POINT=$(echo "$4" | tr ',' ' ')
END_POINT=$(echo "$5" | tr ',' ' ')

BUILD_MAP_NAME="build_map.bag"
SIMULATION_PATH="/home/cicv/work/pji_desktop/simulation"

# 是否加载障碍物
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; exec bash"
  nohup bash -c "$command" >/dev/null 2>&1 &
#  gnome-terminal --tab -e 'bash -c '\""$command"\"
  sleep 0.5
  echo 'bash -c '"$command"
  echo "Default obstacle loaded"
fi

# 起点设置
if [ "$DEFAULT_START_FLAG" == "true" ]; then
  # 默认起点
  echo "Use default start point"
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 5

  ## 发布自定义起点 - 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 && source devel/setup.bash && rosrun goal_publish pub_node $SIMULATION_PATH/data/build_map_bag/$BUILD_MAP_NAME $SIMULATION_PATH/evaluation_bag/example_0821_4.bag; exec bash"
  ## 录包
  ### 激活conda环境
  source activate simulation
  nohup python "$SIMULATION_PATH"/rosbag_record.py "$SIMULATION_PATH"/data/record_bag/test.bag >/dev/null 2>&1 &
  ## 清除环境
  pkill -f "goal_publish/goal" # 下发自定义/默认终点前必须先清除goal，不能只清除pub_node
  pkill -f "goal_publish/pub_node"
  sleep 1
  echo 'bash -c '\""$command"\"
  ## 设置终点并执行
  nohup bash -c "$command" >/dev/null 2>&1 &
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"
  ## 录包
  ### 激活conda环境
  source activate simulation
  nohup python "$SIMULATION_PATH"/rosbag_record.py "$SIMULATION_PATH"/data/record_bag/test.bag >/dev/null 2>&1 &
  ## 清除环境
  pkill -f "goal_publish/goal"
  pkill -f "goal_publish/pub_node"
  sleep 1
  echo 'bash -c '\""$command"\"
  nohup bash -c "$command" >/dev/null 2>&1 &
  ## 设置终点并执行
  echo "End point: $END_POINT has been set"
fi