#!/bin/bash

# To be set by developer
NAV2_WS="${HOME}/nav2_ws"

AMENT_CMAKE_TEST_DIR=`ros2 pkg prefix ament_cmake_test`
RUN_TEST_PY="${AMENT_CMAKE_TEST_DIR}/share/ament_cmake_test/cmake/run_test.py"

RUN_LOG="run.log"

#export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
export RMW_IMPLEMENTATION=rmw_fastrtps_cpp

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

  PIDS=`ps -afx | grep $GREP_PATTERN |\
        grep -v -e grep | awk '{print $1}'`
  while [ -n "$PIDS" ]; do
    for PID in $PIDS; do
      kill -9 $PID > /dev/null 2>&1
    done

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

run_test() {
  timeout 20 /usr/bin/python3 "-u" \
    "${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" \
    > "$RUN_LOG" 2>&1
}

check_pattern() {
  if [ -z "$1" ]; then
    echo "Call: check_pattern <pattern>"
    exit -1
  fi

  grep -e "$1" "$RUN_LOG" > /dev/null 2>&1
  if [[ "$?" != "0" ]]; then
    echo "Failed to find $1 pattern"
    STATUS="FAIL"
  fi
}

# main()

STATUS="OK"
COUNTER=0

# Stopping all previous ROS processes, just for the case
kill_ros

while [[ "$STATUS" == "OK" ]]; do
  COUNTER=$((COUNTER+1))

  # Do actual test
  run_test

  # Stopping all remaining ROS processes
  kill_ros

  # Check that everything is configured
  check_pattern "amcl.*Configuring$"
  check_pattern "map_server.*Configuring$"
  check_pattern "controller_server.*Configuring$"
  check_pattern "planner_server.*Configuring$"

  # Check that everything is activated
  check_pattern "amcl.*Activating$"
  check_pattern "map_server.*Activating$"
  check_pattern "controller_server.*Activating$"
  check_pattern "planner_server.*Activating$"

  echo "[`date +%Y.%m.%d_%H:%M:%S`] counter $COUNTER ... $STATUS"
done

notify-send \
  -u critical \
  -i /usr/share/icons/elementary-xfce/status/64/dialog-error.png \
  "Test ${COUNTER} ... FAILED"