# 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)
        
        cmd = parts[0]
        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 == 'move' and len(args) >= 1:
            if args[0] == 'initial':
                print master._move_initial()
                sleep(2)
            elif args[0] == 'vantage' and len(args) >= 2:
                print master._move_vantage_point(args[1])
                sleep(2)
            elif args[0] == 'withdraw' and len(args) >= 1:
                name = args[1] if len(args) >= 2 else master.knowledge_base.target_bin
                limb = args[2] if len(args) >= 3 else master.knowledge_base.active_limb
                print master.manager.moveToXform(se3.mul(master.knowledge_base.shelf_xform, master.knowledge_base.withdraw_point_xforms[name]), limb)
                sleep(2)
            elif args[0] == 'pickTray':
                print master.manager.PickUpOrderTray(True)
            elif args[0] == 'placeTray':
                print master.manager.PickUpOrderTray(False)
            elif args[0] == 'moveTray' and len(args) > 1:
                if(args[1].startswith('bin')):