#!/bin/bash

usage()
{
    echo -e "Usage: goby_launch"    
    echo -e "\t$0 [options] <launchfile>"
    echo -e ""
    echo -e "Example:"
    echo -e "\t$0 -p vehicle1 -s /opt/goby/vehicle1.launch"
    echo -e ""
    echo -e "<launchfile>:"
    echo -e "\tThe <launchfile> is a newline separate list of applications to launch, in the order given. For example, this <launchfile> would launch gobyd followed by goby_logger:"
    echo -e "----------------"
    echo -e "gobyd"
    echo -e "goby_logger --log_dir /media/data/logs"   
    echo -e "----------------"
    echo -e "" 
    echo -e "Optionally, the <launchfile> can contain directives (separated by commas) before each launch line that modify the behavior of that line. The current directives are [kill=SIGNAL], [name=foo], and [env=ENVIRONMENTAL_VARIABLE=value], for example:"
    echo -e "----------------"
    echo -e "[kill=SIGTERM] MOOSDB /tmp/auv.moos"
    echo -e "[env=LD_LIBRARY_PATH=\${LD_LIBRARY_PATH}:/home/toby/goby3/build/lib] goby_liaison liaison.pb.cfg"
    echo -e "[env=FOO=bar, kill=SIGINT] some_app"
    echo -e "[name=some_app1] some_app 1.cfg"
    echo -e "[name=some_app2] some_app 2.cfg"
    echo -e "----------------"
    echo -e ""
    echo -e "If a file called preseed.goby exists in the working directory, goby_launch will 'source' it before continuing with the rest of the launch"
    echo -e "" 
    echo -e "Options:"
    echo -e "\t-h\t\tShow help."
    echo -e "\t-r\t\tLaunch applications all in the current terminal)"
    echo -e "\t-s\t\tLaunch applications in a detached GNU 'screen' session (requires 'apt install screen')"
    echo -e "\t-L\t\tWrite screen debug -Logfiles to ${screen_log_prefix}*"
#    echo -e "\t-t\t\tLaunch applications in their own gnome-terminal tab (implies -b)" 
    echo -e "\t-n\t\tLaunch applications detached using 'nohup' (implies -b)"
    echo -e "\t-x\t\tLaunch applications in their own xterm"
    echo -e "\t-d 10\t\tDelay in milliseconds between launching applications"
    echo -e "\t-b\t\tDo not wait for processes to exit before exiting this script"
    echo -e "\t-p vehicle1\tPlatform name"
    echo -e "\t-m 10\t\tDelay in seconds after sending goby_terminate before escalating to SIGTERM"
    echo -e "\t-k 10\t\tDelay in seconds after sending SIGTERM before escalating to SIGKILL"
    echo -e "\t-P\t\tExecute kill commands in parallel"
}

num_running_proc()
{
    pid_list=$(echo ${spid[@]} | sed 's/ /,/g')
    num_proc=$(ps --pid "$pid_list" -o pid= | wc -l)
    echo $num_proc
    return $num_proc
}

launch_fail()
{
    echo "Failed to launch $@"
    exit 1;
}

check_launch_type_unset()
{
    [ "$launch" != screen ] && echo "Error: -r, -n, -t, -x, and -s are mutually exclusive. For help use -h." && exit 1
}

do_killone()
{
    p=$1
    cmd=$2
    sig=$3
    gobyd_interprocess=$4
    sigterm_delay=$5
    sigkill_delay=$6
    parallel=$7

    
    # don't kill anything that's already dead
    kill -0 $p >& /dev/null || return

    
    if [ "$sig" = "goby_terminate" ]; then

        if [ "$parallel" = true ]; then
            # don't kill gobyd yet
            echo $cmd | grep -q "^ *[/a-zA-Z0-9_]*gobyd" && return
        fi
        
        # run goby_terminate to cleanly shutdown processes
        echo -n "goby_terminate: $p (was $cmd)"
        if goby_terminate --response_timeout ${sigterm_delay} --target_pid $p <(echo ${gobyd_interprocess});
        then
            echo -e " .... success"
            return 0
        fi
        
        sig=SIGTERM
        # if goby_terminate failed, use SIGTERM, then SIGKILL
        echo -e "\n=== WARNING ===: Process $p did not respond to goby_terminate, sending $sig"        
    fi
    
    echo -n "$sig: $p (was $cmd)"
    kill -$sig $p
    
    elapsed_ms=0
    while $(kill -0 $p 2>/dev/null); do 
        sleep 0.1
        elapsed_ms=$(($elapsed_ms + 100))
        # if necessary, escalate to SIGKILL
        if (($elapsed_ms > $sigkill_delay)); then
            echo -e "\n====== WARNING ======: Process $p did not respond to SIGTERM after $sigkill_delay ms, sending SIGKILL"
            echo -n "SIGKILL: $p (was $cmd)"
            kill -SIGKILL $p
            break;
        fi
    done
    echo -e " .... success"
}

do_kill()
{
    # nothing to kill
    [ ${#spid[@]} = "0" ] && exit 0;
    
    # clear out ^C symbol
    echo ""
    echo "Cleaning up..."

    gobyd_interprocess_1line=$(echo "${gobyd_interprocess}" | tr '\n' ' ')
    if [ "$parallel_shutdown" = true ]; then
        echo "Executing parallel shutdown, but delaying quitting gobyd (if any)"
        export -f do_killone
        parallel --link do_killone ::: ${spid[@]} ::: "${spid_cmd[@]}" ::: ${spid_sig[@]} ::: "${gobyd_interprocess_1line}" ::: ${sigterm_delay} ::: ${sigkill_delay} ::: true
    fi

    # kill everything in non-parallel mode, or gobyd if left over in parallel mode
    for ((i=$((${#spid[@]}-1)); i>=0; --i)); do
        do_killone ${spid[i]} "${spid_cmd[i]}" ${spid_sig[i]} "${gobyd_interprocess_1line}" ${sigterm_delay} ${sigkill_delay} false
    done       
    
    
    spid=()

    rm $launchfile;

    echo "[${platform}]: All processes exited"

    exit 0;
}

# clean up from last launch
screen_log_prefix=/tmp/goby_launch_screen
rm -f ${screen_log_prefix}*

launch_delay=10
launch=screen
screen_debug=false
launch_time=$(date -u +%Y%m%dT%H%M%S)
platform=$launch_time
background=false
# in ms
sigkill_delay=10000
# in s
sigterm_delay=10
parallel_shutdown=false

while getopts ":rhsnxPLd:bp:k:m:" opt; do
    case $opt in
        h) usage; exit 1 ;;
        r) check_launch_type_unset; launch=normal ;;
        s) check_launch_type_unset; launch=screen ;;
        n) check_launch_type_unset; launch=nohup; background=true ;;
#        t) check_launch_type_unset; launch=gnometerm; background=true ;;
        x) check_launch_type_unset; launch=xterm ;;        
        d) launch_delay=$OPTARG ;;
        p) platform=$OPTARG ;;
        b) background=true ;;
        k) sigkill_delay=$(($OPTARG*1000)) ;;
        m) sigterm_delay=$OPTARG ;;
        P) parallel_shutdown=true ;;
        L) screen_debug=true ;;
        \?)
            echo "Invalid option: -$OPTARG. For help use -h."
            exit  1
            ;;
        :)
            echo "Option -$OPTARG requires an argument. For help use -h."
            exit 1
    esac
done

if [ "$background" = false ]; then
    # send SIGTERM to all background process when we get signaled
    trap "trap - SIGTERM && do_kill" SIGTERM SIGINT EXIT
fi

# parent PIDs
ppid=()
# PIDs to signal (child of screen)
spid=()
# spid to command
spid_cmd=()
# initial signal to use
spid_sig=()

# copy launchfile to allow multiple reads
origlaunchfile=${@:$OPTIND:1}
if [ ! -e "$origlaunchfile" ]; then
    echo "Error: must provide a valid launchfile: \"$origlaunchfile\" does not exist. For help use -h."
    exit 1
fi

preseed="$(dirname $origlaunchfile)/preseed.goby"
if [ -e "$preseed" ]; then
    echo "source $preseed"
    source $preseed;
fi

launchfile="/tmp/goby_$$.launch"
# echo adds a newline at the end which is required in case one doesn't exist
echo | cat ${origlaunchfile} - > ${launchfile}


# search for gobyd so we can use the same interprocess { } configuration for goby_terminate
gobyd_line=$(cat "$launchfile" | egrep "^ *(\[.*\])? *[/a-zA-Z0-9_]*gobyd")
num_gobyd=$(echo "$gobyd_line" | wc -l)

use_goby_terminate=true
if [[ "$num_gobyd" != "1" ]]; then
    echo "Note: multiple instances of 'gobyd' found - will use SIGTERM instead of goby_terminate to kill processes (unless otherwise marked with [kill=...])"
    use_goby_terminate=false
elif [[ "$gobyd_line"  == "" ]]; then
    echo "Note: no line found for launching 'gobyd' - will use SIGTERM instead of goby_terminate to kill processes (unless otherwise marked with [kill=...])"
    use_goby_terminate=false
else
    directive_str=
    echo "$gobyd_line" | egrep -q '\[.*\]'  &&
        directive_str=$(echo "$gobyd_line" | sed 's/^ *\[\(.*\)\].*$/\1/')    
    if [ ! -z "$directive_str" ]; then

        IFS=';, ' read -r -a directives <<< "$directive_str" 
        for ((i=0; i<${#directives[@]}; i++))
        do
            directive=${directives[$i]}
            key=$(echo "$directive" | cut -d "=" -f 1)
            value=$(echo "$directive" | cut -d "=" -f 2-)
            case $key in
                env)
                    eval "export $value"
                    ;;
                *)
                    ;;
            esac
        done               
    fi
    gobyd_line=$(echo "$gobyd_line"  | sed 's/^ *\[.*\]//' )
    gobyd_escline=$(echo "$gobyd_line" |  sed 's/"/\\"/g')
    gobyd_interprocess=$(bash -c "$gobyd_line --app 'debug_cfg: true'" | sed -n "/^ *interprocess/,/}/p;")    
    gobyd_platform=$(echo "$gobyd_interprocess" | grep platform | sed 's/.*platform: \"\(.*\)\".*/\1/')
    if [ ! -z "$gobyd_platform" ]; then
        platform="$gobyd_platform"
        echo "Setting platform to [$gobyd_platform] from gobyd interprocess configuration"       
    fi
fi

# actually launch the lines
gnterm=()
line_index=0
while IFS= read -r line
do
    cmd=$(echo "$line" | sed 's/^ *\[.*\]//')
    directive_str=
    echo "$line" | egrep -q '\[.*\]'  &&
        directive_str=$(echo "$line" | sed 's/^ *\[\(.*\)\].*$/\1/')

    kill_signal="goby_terminate"

    # if no gobyd, we default to SIGTERM rather than goby_terminate
    if [ "$use_goby_terminate" = false ]; then
        kill_signal="SIGTERM"
    fi   

    bin=$(echo $cmd | cut -d " " -f 1)

    if [ ! -z "$directive_str" ]; then

        IFS=';, ' read -r -a directives <<< "$directive_str" 
        for ((i=0; i<${#directives[@]}; i++))
        do
            directive=${directives[$i]}
            key=$(echo "$directive" | cut -d "=" -f 1)
            value=$(echo "$directive" | cut -d "=" -f 2-)
            case $key in
                kill)
                    kill_signal=$value
                    if ! echo $kill_signal | egrep -q '^SIGTERM$|^SIGKILL$|^SIGINT$|^goby_terminate$'; then
                        echo "Invalid kill=$kill_signal signal \"$kill_signal\": use \"goby_terminate\" (default), \"SIGTERM\", \"SIGINT\", or \"SIGKILL\"";
                        exit 1
                    fi
                    ;;
                env)
#                    echo "Exporting $value"
                    eval "export $value"
                    ;;
                name)
                    bin="$value"
                    ;;
                *)
                    echo "Invalid [$key=$value]: directive key: " $key
                    exit 1
                    ;;
            esac
        done               
    fi
    spid_sig+=($kill_signal)

    
    esccmd=$(echo $cmd | sed 's/"/\\"/g')
    name=$platform.${bin##*/}    
    extra=
    if [ ! -z "$directive_str" ]; then
        extra="[$directive_str]"
    fi
    case $launch in
        normal)            
            bash -c "$cmd"&
            pid=$!
            echo "Launched ($pid) $cmd $extra"
            ppid+=($pid)
            spid+=($pid)
            spid_cmd+=("$cmd")
            ;;
        xterm)
            setsid xterm  -T $name -e "bash -c \"$esccmd\""&
            # pid of xterm
            parent_pid=$!
            # pid of process in xterm
            child_pid=
            # wait until the child launched in screen
            while [[ -z "$child_pid" ]]; do
                child_pid=$(ps --ppid $parent_pid -o pid=)
                kill -0 $parent_pid >& /dev/null || launch_fail $cmd;
                sleep 0.01
            done
            echo "Launched (xterm: $parent_pid, child: $child_pid) $cmd $extra"
            ppid+=($parent_pid)
            spid+=($child_pid)
            spid_cmd+=("$cmd")
            ;;
        nohup)
            nohup bash -c "$esccmd"&
            pid=$!
            echo "Launched ($pid) $cmd"
            ppid+=($pid)
            spid+=($pid)
            spid_cmd+=("$cmd")
            ;;
        gnometerm)
            gnterm+=(--tab -e "bash -c \"echo -ne \\\"\\033]0;${name}\\007\\\"; $esccmd\"")
            ;;       
        screen)
            screen_log_cmd=
            if [ "$screen_debug" = true ]; then
                screen_log_cmd="-L -Logfile ${screen_log_prefix}_${name}_${line_index}"
            fi
            screen -DmS ${name} ${screen_log_cmd} /bin/bash -c "exec $cmd"&
            # pid of screen
            parent_pid=$!
            # pid of process in screen
            child_pid=
            # wait until the child launched in screen
            while [ -z "$child_pid" ]; do
                child_pid=$(ps --ppid $parent_pid -o pid=)
                kill -0 $parent_pid >& /dev/null || launch_fail $cmd;
                sleep 0.01
            done
               
            echo "Launched (screen: $parent_pid, child: $child_pid) $cmd $extra"
            ppid+=($parent_pid)
            spid+=($child_pid)
            spid_cmd+=("$cmd")
            ;;       
    esac
    if [ $launch != gnometerm ]; then
        i=$((${#ppid[@]}-1))

        # avoid association with this shell so we can kill them later without "Terminated" warning
        disown ${ppid[i]} >& /dev/null
        disown ${spid[i]} >& /dev/null
        sleep ${launch_delay}E-3
    fi

    line_index=$((line_index+1))
done < <(sed 's/#.*//' "$launchfile" | sed '/^\s*$/d')

# check that everything is still running 1 second after launch
sleep 1
for ((i=0; i<${#spid[@]}; ++i)); do 
    p=${spid[i]}
    cmd=${spid_cmd[i]}
    kill -0 $p >& /dev/null || launch_fail "$cmd";
done

if [ $launch = gnometerm ]; then 
    gnome-terminal --maximize "${gnterm[@]}"
    pid=$!
    ppid+=($pid)
    spid+=($pid)
fi

case $launch in
    screen)
        echo "Screen sessions (screen -ls): "
        screen -ls | cut -f 2 | cut -d "." -f 2-3 | head -n-1 | tail -n+2 | sort | xargs printf "\t%s\n"
        echo "Use 'screen -r <name>' to (re)attach session"
        if [ "$screen_debug" = true ]; then
            echo "Screen debug logs (-Logfile) are written to ${screen_log_prefix}*"
        fi
    ;;
esac

if [ "$background" = false ]; then    
    echo "Use <CTRL+C> to exit"    
    while (($(num_running_proc) > 0))
    do
        if (($(num_running_proc) == ${#spid[@]})); then
            echo "[${platform}] "`date +%T`": All processes running"
        else
            for ((i=0; i<${#spid[@]}; ++i)); do 
                p=${spid[i]}
                cmd=${spid_cmd[i]}
                [ $(ps --pid $p -o pid=) ] || echo "[${platform}] "`date +%T`": Missing PID: $p (was: $cmd)"
            done
        fi
        sleep 5            
    done
else
    if [ $launch = "gnometerm" ]; then 
        echo -e "To quit, close the gnome-terminal window(s)"
    else       
        echo -e "Backgrounding as requested. To kill use goby_terminate and/or\nkill ${spid[@]}"
    fi
fi

