# allow importing from the repository root
import sys, os
sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__))))

from integration.master import Master

import json
apc_order = json.load(open(os.path.join(os.path.dirname(__file__), 'all_bins_order.json')))

master = Master(apc_order)
from time import sleep

for request in master.order:
    print '  {}: {}'.format(request['bin'], request['item'])

task = master.manager.control.update()
while not task.done: sleep(0.1)

master._move_initial()

while master.order:
    request = master.order.pop(0)

    master._set_target(request['bin'], request['item'])
    master._move_vantage_point(master.knowledge_base.bin_vantage_points[request['bin']][0])
    master._localize_object(master.knowledge_base.bin_vantage_points[request['bin']][0])

    task = master.manager.control.update()
    while not task.done: sleep(0.1)
        args = parts[1:]

        if cmd == 'active' and len(args) == 1:
            master.knowledge_base.active_limb = args[0]

        elif cmd == 'target':
            if len(args) == 2:
                print master._set_target(args[0], args[1])
            elif len(args) == 1:
                print master._set_target(args[0], filter(lambda o: o['bin'] == args[0], master.order)[0]['item'])

        elif cmd == 'localize' and len(args) >= 1:
            if args[0] in [ 'shelf', 'order_bin', 'both' ]:
                print master._localize_shelf_and_order_bin()
            elif args[0] == 'object':
                print master._localize_object('unused')

        elif cmd == 'robot' and len(args) == 1:
            if args[0] == 'enable':
                os.system('rosrun baxter_tools enable_robot.py -e')
            elif args[0] == 'disable':
                os.system('rosrun baxter_tools enable_robot.py -d')

        elif cmd == 'reset' and len(args) == 1:
            if args[0] == 'control':
                print master.manager._restart_control()
            elif args[0] == 'order':
                print master._load_apc_order(apc_order)
            elif args[0] == 'kb':
                print master._create_knowledge_base()
                print master._load_apc_order(apc_order)