setup_environment

#! /bin/bash

REPO_ROOT=$( cd -- "$( dirname -- "${BASH_SOURCE[0]}" )" &> /dev/null && pwd )

# Kill all ROS processes
kill_ros() {
    GREP_PATTERN="ros|gz|ros-args"
    GREP_IGNORE_PATTERN="grep|rqt|rviz|teleop|ros2 topic|foxglove|kill_ros"

    PIDS=$(ps -afx | grep -E "$GREP_PATTERN" | grep -v -E "$GREP_IGNORE_PATTERN" | awk '{print $1}')
    if [ -z "$PIDS" ]; then
        echo "No ROS processes found"
        return
    fi
    while [ -n "$PIDS" ]; do
        for PID in $PIDS; do
            echo "$PID $(ps -p $PID -o comm=)"
            kill -9 $PID > /dev/null 2>&1
        done

        sleep 1
        PIDS=$(ps -afx | grep $GREP_PATTERN | grep -v -e grep | awk '{print $1}')
    done

    pkill telegraf
}
export -f kill_ros

## Source workspace specific preferences
export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
export GZ_SIM_RESOURCE_PATH="$REPO_ROOT/gazebo_models:$REPO_ROOT/gazebo_models/ycb:$REPO_ROOT/workspace/install/robotiq_description/share"
export USER="ubuntu"
export IP=`hostname -I | cut -d ' ' -f 1`
export RCUTILS_COLORIZED_OUTPUT=1
export LD_LIBRARY_PATH=/usr/local/lib:$LD_LIBRARY_PATH


if [[ -f "${REPO_ROOT}/.env" ]]; then
    set -a
    source $REPO_ROOT/.env
    set +a
fi


## Source ROS setup scripts
if [ -f "/opt/ros/${ROS_DISTRO}/setup.bash" ]; then
    source "/opt/ros/${ROS_DISTRO}/setup.bash"
fi

if [ -f "${REPO_ROOT}/workspace/install/setup.bash" ]; then
    source "${REPO_ROOT}/workspace/install/setup.bash"
fi

if [ -f "/usr/share/colcon_argcomplete/hook/colcon-argcomplete.bash" ]; then
    source "/usr/share/colcon_argcomplete/hook/colcon-argcomplete.bash"
fi

## Init NodeStrategy to avoid autocomplete hang on first tab
# python3 -c "from ros2cli.node.strategy import NodeStrategy; NodeStrategy(None)"

## Set default name (emoji) and color
if [ -z "$MY_EMOJI" ]; then export MY_EMOJI="♻️ "; fi
if [ -z "$MY_COLOR" ]; then export MY_COLOR=$(shuf -i 30-37 -n 1); fi

## Set PS1
if [ -z "$PS" ]; then
    export PS=">> "
fi
if [ "$DISABLE_BLESH" != "true" ] && [ "$DISABLE_BLESH" != "True" ]; then
    export PS1="\[\e[1;${MY_COLOR}m\]$PS\[\e[m\]";
else
    export PS1="${MY_EMOJI} \[\e[1;${MY_COLOR}m\]\W\[\e[m\] ";
fi

# Enable completion
if [ -f $HOME/git-completion.bash ]; then
    source $HOME/git-completion.bash
fi

if [ -f /etc/bash_completion ]; then
    source /etc/bash_completion
fi

## Set aliases
alias xop='xdg-open'
alias brcs='source ~/.bashrc'
alias brcv='vim ~/.bashrc'
alias g='git status'
alias gf='git fetch'
alias gv='git log --graph --abbrev-commit --all --decorate --format=format:"%C(bold blue)%h%C(reset) - %C(bold green)(%ar)%C(reset) %C(white)%s%C(reset) %C(dim white)- %an%C(reset)%C(auto)%d%C(reset)"'
alias gd="git diff"
alias gds="git diff --staged"
alias supd="git submodule update --init --recursive"
alias rviz="rviz2 -d $HOME/manipulation/rviz_config.rviz"
alias teleop='ros2 run teleop_twist_keyboard teleop_twist_keyboard'
alias build="cd ~/manipulation/workspace && colcon build --cmake-args ' -DCMAKE_EXPORT_COMPILE_COMMANDS=ON' --packages-select "

exec "$@"