#!/usr/bin/env python
# -*- coding:utf-8 -*-
import os.path as osp
import sys
import json

import rospkg
import rospy
from jsk_2015_05_baxter_apc.msg import WorkOrder, WorkOrderArray

rp = rospkg.RosPack()
rp.get_path('jsk_2015_05_baxter_apc')

sys.path.insert(0, osp.join(rp.get_path('jsk_2015_05_baxter_apc'),
                            'node_scripts'))
from bin_contents import get_bin_contents


def get_work_order(json_file):
    with open(json_file, 'r') as f:
        data = json.load(f)['work_order']
    for order in data:
        bin_ = order['bin'].split('_')[1].lower()  # bin_A -> a
        target_object = order['item']
        yield (bin_, target_object)


def get_sorted_work_order(json_file):
    """Sort work order to maximize the score"""
    bin_contents = get_bin_contents(json_file=json_file)
    bins, objects = zip(*bin_contents)
    bin_n_contents = dict(zip(bins, map(len, objects)))
    sorted_work_order = []
    work_order = dict(get_work_order(json_file=json_file))
    for bin_, n_contents in sorted(bin_n_contents.items(), key=lambda x:x[1]):
        if n_contents > 5:  # Level3
            continue
        sorted_work_order.append((bin_, work_order[bin_]))
    return sorted_work_order


def get_work_order_msg(json_file):
    work_order = get_sorted_work_order(json_file=json_file)
    msg = dict(left=WorkOrderArray(), right=WorkOrderArray())
    abandon_objects = [
        'genuine_joe_plastic_stir_sticks',
        'cheezit_big_original',
        'rolodex_jumbo_pencil_cup',
        'champion_copper_plus_spark_plug',
        'oreo_mega_stuf',
    ]
    for bin_, target_object in work_order:
        if target_object in abandon_objects:
            continue
        if bin_ in 'abdegj':
            msg['left'].array.append(WorkOrder(bin=bin_, object=target_object))
        elif bin_ in 'cfhikl':
            msg['right'].array.append(WorkOrder(bin=bin_, object=target_object))
    return msg


def main():
    json_file = rospy.get_param('~json', None)
    if json_file is None:
        rospy.logerr('must set json file path to ~json')
        return

    msg = get_work_order_msg(json_file)

    pub_left = rospy.Publisher('~left_hand',
                               WorkOrderArray,
                               queue_size=1)
    pub_right = rospy.Publisher('~right_hand',
                                WorkOrderArray,
                                queue_size=1)
    rate = rospy.Rate(rospy.get_param('rate', 1))
    while not rospy.is_shutdown():
        pub_left.publish(msg['left'])
        pub_right.publish(msg['right'])
        rate.sleep()


if __name__ == "__main__":
    rospy.init_node('work_order')
    main()

@@ -1592,90 +1592,565 @@
     
-for bin_, target_object in work_order:%0A        if target_object in abandon_objects
+abandon_bin_objects = %5B%0A        'rolodex_jumbo_pencil_cup',%0A        'oreo_mega_stuf'%0A    %5D%0A    for bin_, target_object in work_order:%0A        if target_object in abandon_objects:%0A    bin_contents = dict(get_bin_contents(json_file=json_file))%0A    for bin_, target_object in work_order:%0A        if target_object in abandon_target_objects:%0A            continue%0A        bin_contents_bool = False%0A        for bin_object in bin_contents%5Bbin_%5D:%0A            if bin_object in abandon_bin_objects:%0A                bin_contents_bool = True%0A        if bin_contents_bool
 :%0A  

add blacklist for bin contents