#!/usr/bin/env bash

sig_int_handler() {
    pkill play_private
    third_party/ros/ros_comm/tools/rosnode/rosnode kill --all
    kill $obsbag_pid
    kill $roscore_pid
}

# On Ctrl-C, run the above function.
trap 'sig_int_handler' 2

log_name="${1}"
wait_duration_seconds="${2}"

# Ensure the ROS tools we need are available.
bazel build //ros_utilities/cli:roscore //ros_utilities/cli:rosparam //ros_utilities/cli:rosnode

# Start a ROS core in case one isn't already up.
third_party/ros/roslaunch/roscore &
roscore_pid=$!

# Ensure use_sim_time is set.
third_party/ros/ros_comm/tools/rosparam/rosparam set use_sim_time true

# Keep trying to set it until rosparam server is up.
while [[ $? != 0 ]]; do
    third_party/ros/ros_comm/tools/rosparam/rosparam set use_sim_time true
done

# Start obsbag run with a 100x clock slowdown.
bazel_run //sim/obstacle_bag:obsbag2 -- \
    run "${log_name}" run_planner:=false time_dilation:=100 &
obsbag_pid=$!

# Start the planner executable under valgrind.
bazel_run //vehicle/planner/node:planner_node_private \
    --run_under='valgrind --track-origins=yes' -- \
    ${ZOOX_WORKSPACE_ROOT}/vehicle/planner/node/planner_configuration.pbtxt &

if [[ ${wait_duration_seconds} != "" ]]; then
    # Shutdown after so many seconds.
    sleep ${wait_duration_seconds}
else
    # Shutdown after the above processes are killed.
    wait
fi

# Shutdown all processes started by this script.
sig_int_handler
