#!/bin/bash
set -ex

URI=${1:-chum://20181112T222349-pri_001@1542063160.99+30.0}

bazel build --config=cuda //vehicle/sensors/cameras/cammux:cammux_main //bin/...

export CUDA_VISIBLE_DEVICES=0
export ZOOX_ROBOT_NAME=pri_001

bazel_run --config=cuda //bin:roscore &

until bazel_run //bin:rosparam -- list 2>&1 >/dev/null
do
  echo "Waiting for roscore"
  sleep 1
done

sudo mv /etc/robot_name /etc/robot_name.bak || true
function cleanup {
  sudo mv /etc/robot_name.bak /etc/robot_name || true
}
trap cleanup EXIT

bazel_run --config=cuda //vehicle/launch:vehicle_params_launch

# Signify that cammux / vision / etc should use the sim clock for timing purposes
bazel_run --config=cuda //bin:rosparam -- set /use_sim_time true

# Chum play needs to begin in another terminal
echo "Please run the following command in another terminal:"
echo "brun //bin:chum_play -- -publish_clock -uri=$URI -topics=/vehicle/measurements -pause_topics=/vehicle/measurements"
echo
echo "Playback will start in a paused state. Press <space> in the other terminal once cammux has finished loading."
read -n 1 -s -r -p "Press any key to continue..."


VEHICLE_CONFIG=/etc/zoox/calibration/param/${ZOOX_ROBOT_NAME}.json
PROCESS_MAPPING_CONFIG="vehicle/sensors/cameras/cammux/config/cam_mapping_c3_1_c1r.json"
CAMERAS_PROTO="$(bazel_run //experimental/rafe:get_camera_proto_for_chum_uri --  --process_mapping=${PROCESS_MAPPING_CONFIG} --process_index=1 $URI)"

# bazel_run --config=cuda //bin:chum_play -- -uri=$URI -topics=/system_monitor/pcu/system_metrics -publish_clock &

bazel_run --config=cuda -- //vehicle/sensors/cameras/cammux:cammux_main \
    --alsologtostderr=1 \
    --enable_rtprio=false \
    --inherit_rtprio=false \
    --wait_for_mode=false \
    --process_index=1 --vision_name=main-ai \
    --image_source_count=1 \
    --vision_platform=VEHICLE_COMPUTE_PLATFORM_31 \
    --camera_layout=ursula \
    --process_mapping_config="${PROCESS_MAPPING_CONFIG}" \
    --initial_vehicle_config="${VEHICLE_CONFIG}" \
    --cameras_proto "${CAMERAS_PROTO}" \
    --ros_publish=true \
    --rate_limit=90.0 \
    --launch_vision_immediately
