#!/usr/bin/env bash
set -euo pipefail

current_directory="$(pwd)"

cleanup() {
    cd ${current_directory}
    echo -e "${yellow}Recieved EXIT. Shutting down! ${reset_color}"
    kill_processes
}

trap cleanup EXIT
# cd "$(dirname "$0")"

bright_red='\u001b[31;1m'
bright_green='\u001b[32;1m'
yellow='\u001b[33;1m'
reset_color='\033[0m'

LOG_DIR="/tmp/firefly_logs"
ROS_CORE_LOG="${LOG_DIR}/roscore-log-$$.log"
BRIDGE_LOG="${LOG_DIR}/bridge-log-$$.log"
PRODUCER_LOG="${LOG_DIR}/producer-log-$$.log"

main(){
    mkdir --parents ${LOG_DIR}
    launch_ros_core
    launch_bridge
    launch_producer
    wait_for_exit
}

launch_ros_core(){
    ./bin/roscore &> "${ROS_CORE_LOG}" &
    ROS_CORE_PID=$!
    echo -e "${bright_green}Logging roscore to ${ROS_CORE_LOG}${reset_color}"
}

launch_bridge(){
    ./spectrum/apps/firefly/bridge/main --offline_dds &> "${BRIDGE_LOG}" &
    BRIDGE_PID=$!
    echo -e "${bright_green}Logging bridge to ${BRIDGE_LOG}${reset_color}"
}

launch_producer(){
    ./spectrum/apps/firefly/producer/main &> "${PRODUCER_LOG}" &
    PRODUCER_PID=$!
    echo -e "${bright_green}Logging producer to ${PRODUCER_LOG}${reset_color}"
}

wait_for_exit(){
    wait < <(jobs -p)
}

kill_processes(){
    echo -e "${yellow}Killing running processes${reset_color}"
    killall ${ROS_CORE_PID} --signal SIGINT &> /dev/null || true
    killall ${BRIDGE_PID} --signal SIGINT &> /dev/null|| true
    killall ${PRODUCER_PID} --signal SIGINT &> /dev/null || true
}

main

