#!/bin/bash
if "true" : '''\'
then
python${ROS_PYTHON_VERSION:-} "${BASH_SOURCE[0]}" $*
exit
fi
'''
# flake8: noqa
import rospy
import sys
import getopt

from flexbe_widget.behavior_launcher import BehaviorLauncher
from flexbe_msgs.msg import BehaviorRequest


def usage():
    print("Soon...")


if __name__ == '__main__':
    rospy.init_node('flexbe_widget')

    try:
        opts, args = getopt.getopt(sys.argv[1:], "hb:a:", ["help", "behavior=", "autonomy="])
    except getopt.GetoptError:
        usage()
        sys.exit(2)

    behavior = ""
    autonomy = 255

    for opt, arg in opts:
        if opt in ("-h", "--help"):
            usage()
            sys.exit()
        elif opt in ("-b", "--behavior"):
            behavior = arg
        elif opt in ("-a", "--autonomy"):
            autonomy = int(arg)
    ignore_args = ['__name', '__log']  # auto-set by roslaunch

    launcher = BehaviorLauncher()

    if behavior != "":
        request = BehaviorRequest()
        request.behavior_name = behavior
        request.autonomy_level = autonomy
        for arg in args:
            if ':=' not in arg:
                continue
            k, v = arg.split(':=', 1)
            if k in ignore_args:
                continue
            request.arg_keys.append('/'+k)
            request.arg_values.append(v)
        rospy.sleep(0.2)  # wait for publishers...
        launcher._callback(request)

    # Wait for ctrl-c to stop the application
    rospy.spin()
