#!/bin/bash
set -eo pipefail

cleanup() {
	for pid_var in HTTP_PID ROSMASTER_PID ROSBRIDGE_PID ROSAPI_PID ROBOT_PID MOTOR_PID JOYSTICK_PID JOY_PID DELAY_PID; do
		if [[ -n "${!pid_var:-}" ]]; then
			kill "${!pid_var}" 2>/dev/null || true
		fi
	done
	wait || true
}

if [[ $1 == "config" ]]
then
	cd /root/exomy_ws/src/exomy/scripts
	bash
elif [[ $1 == "autostart" ]]
then
	trap cleanup EXIT INT TERM
	source /opt/ros/melodic/setup.bash
	cd /root/exomy_ws
	catkin_make
	export ROS_MASTER_URI=http://localhost:11311
	source devel/setup.bash

	cd src/exomy/gui
	python -m SimpleHTTPServer 8000 &
	HTTP_PID=$!
	cd /root/exomy_ws

	rosmaster --core -p 11311 -w 3 > /tmp/rosmaster.log 2>&1 &
	ROSMASTER_PID=$!
	sleep 3

	rosparam load /root/exomy_ws/src/exomy/config/exomy.yaml
	rosparam set /controller logitech-F710
	rosparam set /delay_seconds 0.0
	echo 0.0 > /tmp/exomy_delay.txt

	/opt/ros/melodic/lib/rosbridge_server/rosbridge_websocket > /tmp/rosbridge.log 2>&1 &
	ROSBRIDGE_PID=$!
	/opt/ros/melodic/lib/rosapi/rosapi_node > /tmp/rosapi.log 2>&1 &
	ROSAPI_PID=$!
	python /root/exomy_ws/src/exomy/src/delay_node.py > /tmp/delay_node.log 2>&1 &
	DELAY_PID=$!
	python /root/exomy_ws/src/exomy/src/f710_joy_node.py > /tmp/joy_node.log 2>&1 &
	JOY_PID=$!
	python /root/exomy_ws/src/exomy/src/robot_node.py > /tmp/robot_node.log 2>&1 &
	ROBOT_PID=$!
	python /root/exomy_ws/src/exomy/src/motor_node.py > /tmp/motor_node.log 2>&1 &
	MOTOR_PID=$!
	python /root/exomy_ws/src/exomy/src/joystick_parser_node.py > /tmp/joystick_parser.log 2>&1 &
	JOYSTICK_PID=$!

	wait -n "$HTTP_PID" "$ROSMASTER_PID" "$ROSBRIDGE_PID" "$ROSAPI_PID" "$ROBOT_PID" "$MOTOR_PID" "$JOYSTICK_PID" "$JOY_PID" "$DELAY_PID"
	exit 1
elif [[ $1 == "devel" ]]
then
	cd /root/exomy_ws
	source /opt/ros/melodic/setup.bash
	# catkin_make
	# source devel/setup.bash
	bash
else
	bash
fi