#! /usr/bin/env python
# Provides quick access to the services exposed by controller_manager_msgs

from __future__ import print_function
import rospy, sys
from controller_manager import controller_manager_interface


class Tracker:
    def __init__(self, topic, Msg):
        self.sub = rospy.Subscriber(topic, Msg, self.callback)
        self.msg = None

    def callback(self, msg):
        self.msg = msg

def print_usage(exit_code = 0):
    print('''Commands:
    load <name>          - Load the controller named <name>
    unload <name>        - Unload the controller named <name>
    start <name>         - Start the controller named <name>
    stop <name>          - Stop the controller named <name>
    spawn <name>         - Load and start the controller named <name>
    kill <name>          - Stop and unload the controller named <name>
    list                 - List active controllers
    list-types           - List controller Types
    reload-libraries     - Reloads all plugin controller libraries''')

    sys.exit(exit_code)


if __name__ == '__main__':
    args = rospy.myargv()
    rospy.init_node('controller_manager', anonymous=True)
    if len(args) < 2:
        print_usage()
    if args[1] == 'lt' or args[1] == 'list-types':
        controller_manager_interface.list_controller_types()
    elif args[1] == 'lc' or args[1] == 'list':
        controller_manager_interface.list_controllers()
    elif args[1] == 'reload-libraries':
        if '--restore' in args[2:]:
            controller_manager_interface.reload_libraries(True, restore = True)
        else:
            controller_manager_interface.reload_libraries(True)
    elif args[1] == 'load':
        for c in args[2:]:
            controller_manager_interface.load_controller(c)
    elif args[1] == 'unload':
        for c in args[2:]:
            controller_manager_interface.unload_controller(c)
    elif args[1] == 'start':
        for c in args[2:]:
            controller_manager_interface.start_controller(c)
    elif args[1] == 'stop':
        for c in args[2:]:
            controller_manager_interface.stop_controller(c)
    elif args[1] == 'sp' or args[1] == 'spawn':
        for c in args[2:]:
            controller_manager_interface.load_controller(c)
        for c in args[2:]:
            controller_manager_interface.start_controller(c)
    elif args[1] == 'kl' or args[1] == 'kill':
        for c in args[2:]:
            controller_manager_interface.stop_controller(c)
        for c in args[2:]:
            controller_manager_interface.unload_controller(c)
    else:
        print_usage(1)
