#!/usr/bin/env bash

# Safer bash defaults for this script
set -Eeuo pipefail

# Workspace directory defaults to the current working directory when the script is run
WS_DIR="${WS_DIR:-$PWD}"
SCRIPT_DIR="$(cd -- "$(dirname -- "${BASH_SOURCE[0]}")" >/dev/null 2>&1 && pwd)"
CONFIG_FILE="${CONFIG_FILE:-${SCRIPT_DIR}/launch.conf}"

# title|mode|package|target|args
# mode: run => ros2 run <pkg> <executable>
#       launch => ros2 launch <pkg> <launch_file.py>
if [[ -f "${CONFIG_FILE}" ]]; then
    mapfile -t COMPONENTS < <(grep -vE '^\s*#' "${CONFIG_FILE}" | sed '/^\s*$/d')
else
    echo "no configuration file found at ${CONFIG_FILE}, exiting." >&2
    exit 1
fi

compose_terminal_script() {
    local title="$1" cmds_block="$2"
    cat <<EOF
echo Launching Robot ${title}
cd "${WS_DIR}"
set +u; source install/setup.bash || { echo "Workspace not built (missing install/setup.bash). Run: colcon build" >&2; }; set -u;
# Freeze terminal title so it won't be changed by shell rc/profile
printf '\033]0;%s\007' "${title}"
${cmds_block}
echo "All processes launched for: Launching Robot ${title}"
exec bash --noprofile --norc
EOF
}

launch_with_gnome_terminal() {
    # Build unique title list
    local unique_titles=()
    for entry in "${COMPONENTS[@]}"; do
        IFS='|' read -r title _ _ _ _ <<<"$entry"
        local found=0
        for t in "${unique_titles[@]:-}"; do
            if [[ "$t" == "$title" ]]; then found=1; break; fi
        done
        if [[ $found -eq 0 ]]; then unique_titles+=("$title"); fi
    done

    # For each title, gather commands and open a terminal
    for title in "${unique_titles[@]}"; do
        local cmds_block=""
        for entry in "${COMPONENTS[@]}"; do
            IFS='|' read -r etitle mode pkg target args <<<"$entry"
            [[ "$etitle" != "$title" ]] && continue
            args="${args:-}"
            local ros_cmd
            if [[ "$mode" == "launch" ]]; then
                ros_cmd="ros2 launch ${pkg} ${target} ${args}"
            else
                ros_cmd="ros2 run ${pkg} ${target} ${args}"
            fi
            cmds_block+="${ros_cmd} "$'\n'
            if [[${pkg} == "ethercat_control"]]; then
                cmds_block="taskset -c 7 ./cpu7.sh"
            fi
        done
        # Compose the final script for this terminal
        local cmd
        cmd=$(compose_terminal_script "$title" "$cmds_block")
        # gnome-terminal --title="${title}" -- bash -lc "$cmd" &
        bash -c "$cmd" &
        sleep 3
    done
}
launch_with_xterm() {
    local unique_titles=()
    for entry in "${COMPONENTS[@]}"; do
        IFS='|' read -r title _ _ _ _ <<<"$entry"
        local found=0
        for t in "${unique_titles[@]:-}"; do
            if [[ "$t" == "$title" ]]; then found=1; break; fi
        done
        if [[ $found -eq 0 ]]; then unique_titles+=("$title"); fi
    done

    for title in "${unique_titles[@]}"; do
        local cmds_block=""
        for entry in "${COMPONENTS[@]}"; do
            IFS='|' read -r etitle mode pkg target args <<<"$entry"
            [[ "$etitle" != "$title" ]] && continue
            args="${args:-}"
            local ros_cmd
            if [[ "$mode" == "launch" ]]; then
                ros_cmd="ros2 launch ${pkg} ${target} ${args}"
            else
                ros_cmd="ros2 run ${pkg} ${target} ${args}"
            fi
            cmds_block+="${ros_cmd} &"$'\n'
        done
        local cmd
        cmd=$(compose_terminal_script "$title" "$cmds_block")
        xterm -T "${title}" -hold -e bash -lc "$cmd" &
    done
}
main() {
    echo "Starting robot components..."
    if command -v gnome-terminal >/dev/null 2>&1; then
        echo "Using gnome-terminal"
        launch_with_gnome_terminal
    elif command -v xterm >/dev/null 2>&1; then
        echo "Using xterm"
        launch_with_xterm
    else
        echo "No terminal emulator found (gnome-terminal or xterm). Please install one and retry." >&2
        exit 1
    fi
}

main "$@"