#!/bin/bash

PYTHON="/usr/bin/python3.10"
ROS2_ROLLING="/opt/ros/rolling"
NAV2_WS="/opt/overlay_ws"

kill_ros() {
  GREP_PATTERN="-e ros -e gazebo -e gzserver -e gzclient -e tester_node.py"

  PIDS=`ps -afx | grep $GREP_PATTERN |\
        grep -v grep | awk '{print $1}'`
  while [ -n "$PIDS" ]; do
    echo "Attempt to stop ROS2 processes"
    for PID in $PIDS; do
      echo "stopping $PID"
      kill -9 $PID
    done

    sleep 1
    PIDS=`ps -afx | grep $GREP_PATTERN |\
          grep -v grep | awk '{print $1}'`
  done

  echo "All ROS2 processes stopped"
}

run_keepout() {
  # test_keepout_filter
  echo "Running test_keepout_filter" >> run.log
  /usr/bin/timeout 300 \
  ${PYTHON} "-u" "${ROS2_ROLLING}/share/ament_cmake_test/cmake/run_test.py" \
    "${NAV2_WS}/build/nav2_system_tests/test_results/nav2_system_tests/test_keepout_filter.xml" \
    "--package-name" "nav2_system_tests" "--generate-result-on-success" \
    "--env" \
      "TEST_DIR=${NAV2_WS}/src/navigation2/nav2_system_tests/src/costmap_filters" \
      "TEST_MAP=${NAV2_WS}/src/navigation2/nav2_system_tests/maps/map_circular.yaml" \
      "TEST_MASK=${NAV2_WS}/src/navigation2/nav2_system_tests/maps/keepout_mask.yaml" \
      "TEST_WORLD=${NAV2_WS}/src/navigation2/nav2_system_tests/worlds/turtlebot3_ros2_demo.world" \
      "PARAMS_FILE=${NAV2_WS}/src/navigation2/nav2_system_tests/src/costmap_filters/keepout_params.yaml" \
      "GAZEBO_MODEL_PATH=${NAV2_WS}/src/navigation2/nav2_system_tests/models" \
      "BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml" \
      "ASTAR=False" \
    "--command" \
      "${NAV2_WS}/src/navigation2/nav2_system_tests/src/costmap_filters/test_keepout_launch.py" \
  2>&1 | tee -a run.log
}

run_speed_global() {
  # test_speed_filter_global
  echo "Running test_speed_filter_global" >> run.log
  /usr/bin/timeout 300 \
  ${PYTHON} "-u" "${ROS2_ROLLING}/share/ament_cmake_test/cmake/run_test.py" \
    "${NAV2_WS}/build/nav2_system_tests/test_results/nav2_system_tests/test_speed_filter_global.xml" \
    "--package-name" "nav2_system_tests" "--generate-result-on-success" \
    "--env" \
      "TEST_DIR=${NAV2_WS}/src/navigation2/nav2_system_tests/src/costmap_filters" \
      "TEST_MAP=${NAV2_WS}/src/navigation2/nav2_system_tests/maps/map_circular.yaml" \
      "TEST_MASK=${NAV2_WS}/src/navigation2/nav2_system_tests/maps/speed_mask.yaml" \
      "TEST_WORLD=${NAV2_WS}/src/navigation2/nav2_system_tests/worlds/turtlebot3_ros2_demo.world" \
      "PARAMS_FILE=${NAV2_WS}/src/navigation2/nav2_system_tests/src/costmap_filters/speed_global_params.yaml" \
      "GAZEBO_MODEL_PATH=${NAV2_WS}/src/navigation2/nav2_system_tests/models" \
      "BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml" \
      "ASTAR=False" \
    "--command" \
      "${NAV2_WS}/src/navigation2/nav2_system_tests/src/costmap_filters/test_speed_launch.py" \
  2>&1 | tee -a run.log
}

run_speed_local() {
  # test_speed_filter_local
  echo "Running test_speed_filter_local" >> run.log
  /usr/bin/timeout 300 \
  ${PYTHON} "-u" "${ROS2_ROLLING}/share/ament_cmake_test/cmake/run_test.py" \
    "${NAV2_WS}/build/nav2_system_tests/test_results/nav2_system_tests/test_speed_filter_local.xml" \
    "--package-name" "nav2_system_tests" "--generate-result-on-success" \
    "--env" \
      "TEST_DIR=${NAV2_WS}/src/navigation2/nav2_system_tests/src/costmap_filters" \
      "TEST_MAP=${NAV2_WS}/src/navigation2/nav2_system_tests/maps/map_circular.yaml" \
      "TEST_MASK=${NAV2_WS}/src/navigation2/nav2_system_tests/maps/speed_mask.yaml" \
      "TEST_WORLD=${NAV2_WS}/src/navigation2/nav2_system_tests/worlds/turtlebot3_ros2_demo.world" \
      "PARAMS_FILE=${NAV2_WS}/src/navigation2/nav2_system_tests/src/costmap_filters/speed_local_params.yaml" \
      "GAZEBO_MODEL_PATH=${NAV2_WS}/src/navigation2/nav2_system_tests/models" \
      "BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml" \
      "ASTAR=False" \
    "--command" \
      "${NAV2_WS}/src/navigation2/nav2_system_tests/src/costmap_filters/test_speed_launch.py" \
  2>&1 | tee -a run.log
}

# Arguments
# $1 - iteration number
# $2 - test selection
run_test() {
  echo "Running $1 test iteration" > run.log

  if [[ "$2" == "1" ]]; then
    run_keepout
  elif [[ "$2" == "2" ]]; then
    run_speed_global
  else
    run_speed_local
  fi

  # Remove all related processes for next run
  kill_ros

  # Check whether the testcase was failed
  grep "run_test.py: return code 0" run.log
  if [[ "$?" != "0" ]]; then
    echo "Test failed on $iter iteration"
    exit 1
  fi
}

# main()

iter=0
max_iter=30

while [[ "$iter" -lt "$max_iter" ]]; do
  iter=$((iter+1))

  run_test $iter 1
  run_test $iter 2
  run_test $iter 3
done