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

import roslib, time
roslib.load_manifest('pr2_controller_manager')

import rospy, sys
from pr2_controller_manager import pr2_controller_manager_interface
from pr2_mechanism_msgs.msg import MechanismStatistics


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

    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-joints          - List joints and actuators
    list-types           - List controller Types
    reload-libraries     - Reloads all plugin controller libraries'''

    sys.exit(exit_code)

if __name__ == '__main__':
    args = rospy.myargv()
    if len(args) < 2:
        print_usage()
    if args[1] == 'lt' or args[1] == 'list-types':
        pr2_controller_manager_interface.list_controller_types()
    elif args[1] == 'lc' or args[1] == 'list':
        pr2_controller_manager_interface.list_controllers()
    elif args[1] == 'reload-libraries':
        print "ARGS", args, args[2:]
        if '--restore' in args[2:]:
            print "RE1"
            pr2_controller_manager_interface.reload_libraries(True, restore = True)
        else:
            pr2_controller_manager_interface.reload_libraries(True)
    elif args[1] == 'load':
        for c in args[2:]:
            pr2_controller_manager_interface.load_controller(c)
    elif args[1] == 'unload':
        for c in args[2:]:
            pr2_controller_manager_interface.unload_controller(c)
    elif args[1] == 'start':
        for c in args[2:]:
            pr2_controller_manager_interface.start_controller(c)
    elif args[1] == 'stop':
        for c in args[2:]:
            pr2_controller_manager_interface.stop_controller(c)
    elif args[1] == 'sp' or args[1] == 'spawn':
        for c in args[2:]:
            pr2_controller_manager_interface.load_controller(c)
        for c in args[2:]:
            pr2_controller_manager_interface.start_controller(c)
    elif args[1] == 'kl' or args[1] == 'kill':
        for c in args[2:]:
            pr2_controller_manager_interface.stop_controller(c)
        for c in args[2:]:
            pr2_controller_manager_interface.unload_controller(c)
    elif args[1] == 'lj' or args[1] == 'list-joints':
        track_mech = Tracker('/mechanism_statistics', MechanismStatistics)
        print "Waiting for mechanism statistics message..."
        rospy.init_node('mech', anonymous=True)
        while not track_mech.msg:
            time.sleep(0.01)
            if rospy.is_shutdown():
                sys.exit(0)
        msg = track_mech.msg
        print 'Actuators:'
        for i in range(len(msg.actuator_statistics)):
            print "  %3d %s" % (i, msg.actuator_statistics[i].name)
        print 'Joints:'
        for i in range(len(msg.joint_statistics)):
            print "  %3d %s" % (i, msg.joint_statistics[i].name)
    else:
        print_usage(1)
