#!/bin/bash

############################################################################
# This script should be used to start the robocup stack for the simulator. #
# In the docker container, it is copied to /usr/local/bin/start. Therefore #
# it can be started using, for example, "start goalie 0" as the dockerCmd. #
############################################################################

#########
# Usage #
#########

function print_usage() {
    echo "Usage: $0 role position_number [--no-bag] [--team ID]"
    echo "       role: [offense | defense | goalie]"
    echo "       position_number: [0 | 1 | 2]"
}


if [[ -z $1 || -z $2 ]]; then
    print_usage
    exit 1
fi

#################
# Configuration #
#################

TEAM_ID=7

##########
# Checks #
##########

if [[ -z "$ROBOCUP_ROBOT_ID" ]]; then
    echo "ROBOCUP_ROBOT_ID is not set! Exiting."
    exit 2
fi

if [[ -z "$ROBOCUP_TEAM_COLOR" ]]; then
    echo "ROBOCUP_TEAM_COLOR is not set! Exiting."
    exit 2
fi

if [[ -z "$ROBOCUP_SIMULATOR_ADDR" ]]; then
    echo "ROBOCUP_SIMULATOR_ADDR is not set! Exiting."
    exit 2
fi

if [[ -z "$ROBOCUP_MIRROR_SERVER_IP" ]]; then
    echo "ROBOCUP_MIRROR_SERVER_IP is not set! Exiting."
    exit 2
fi

PARAM_DIR=$COLCON_PREFIX_PATH/bitbots_parameter_blackboard/share/bitbots_parameter_blackboard

if [[ ! -d "$PARAM_DIR" ]]; then
    echo "Could not find bitbots_parameter_blackboard! Did you source ROS?"
    exit 2
fi

TEAM_COMM_DIR=$COLCON_PREFIX_PATH/bitbots_team_communication/share/bitbots_team_communication

if [[ ! -d "$TEAM_COMM_DIR" ]]; then
    echo "Could not find bitbots_team_communication!"
    exit 2
fi

if [[ "$ROBOCUP_TEAM_COLOR" == "blue" ]]; then
    TEAM_COLOR_ID=0
elif [[ "$ROBOCUP_TEAM_COLOR" == "red" ]]; then
    TEAM_COLOR_ID=1
else
    echo "Invalid team color: \"$ROBOCUP_TEAM_COLOR\". Use \"red\" or \"blue\". Exiting."
    exit 2
fi

###########################
# Read command line flags #
###########################

ROLE=$1
shift

POSITION=$1
shift

RECORD=true

while test $# -gt 0; do
    case "$1" in
        -h|--help)
            print_usage
            exit 0
            ;;
        --no-bag)
            shift
            RECORD=false
            ;;
        --team)
            shift
            if test $# -gt 0; then
                if [[ "$1" =~ ^[0-9]+$ ]]; then
                    TEAM_ID=$1
                else
                    echo "Invalid team id: $1"
                    exit 1
                fi
            else
                echo "No team id for --team specified"
                exit 1
            fi
            shift
            ;;
        *)
            break
            ;;
    esac
done

#############################
# Write configuration files #
#############################

cat > $PARAM_DIR/config/game_settings.yaml << EOF
parameter_blackboard:
  ros__parameters:
    bot_id: $ROBOCUP_ROBOT_ID
    position_number: $POSITION
    role: $ROLE
    team_color: $TEAM_COLOR_ID
    team_id: $TEAM_ID
EOF

sed -i "/^    target_ip:/s/^.*$/    target_ip: $ROBOCUP_MIRROR_SERVER_IP/" $TEAM_COMM_DIR/config/team_communication_config.yaml

#############
# Start ROS #
#############

exec ros2 launch bitbots_robocup_api robocup_teamplayer.launch record:=$RECORD
