from __future__ import print_function

import threading
from collections import defaultdict

import roslib
roslib.load_manifest('joint_dependency_ros')
from std_msgs.msg import UInt8, Bool, Float64
import rospy

__author__ = 'johannes'


def create_ros_lockbox():
    print("create lockbox")
    rospy.init_node("joint_dependency")
    joints = []
    for i in range(5):
        joints.append(RosJoint("lock_{}".format(i), i+1, [0, 180]))
    joints[0].locked = False
    world = RosWorld(joints)
    return world


class FakeService(object):
    def __init__(self, request, response, request_t, response_t):
        self.publisher = rospy.Publisher(request, request_t, queue_size=10)
        self.subscriber = rospy.Subscriber(response, response_t, self.callback)
        self.response_received = threading.Event()
        self.response = None

    def callback(self, msg):
        self.response = msg
        self.response_received.set()


    def __call__(self, request):
        self.publisher.publish(request)
        self.response_received.wait()
        self.response_received.clear()
        return self.response


class RosJoint(object):
    def __init__(self, name, index, limits):
        self.name = name
        self.index = index
        self.min_limit = limits[0]
        self.max_limit = limits[1]
        self.locked = True
        self.get_q_srv = FakeService('/lockbox/joint_position',
                                     '/lockbox/joint_position_result',
                                     UInt8, Float64)

    def get_q(self, time=None):
        print("get_q {}".format(self.index))
        q = float(self.get_q_srv(self.index).data)
        print(q)
        return q

    def is_locked(self):
        return self.locked


class RosWorld(object):
    def __init__(self, joints):
        self.joints = joints


class RosActionMachine(object):
    def __init__(self, world):
        self.world = world
        self.locking_state = defaultdict(bool)  # returns False on empty bucket
        self.run_action_srv = FakeService('/lockbox/test_joint',
                                          '/lockbox/test_joint_result',
                                          UInt8, Bool)

    def run_action(self, pos, joint_idx):
        joint = self.world.joints[joint_idx]
        print("run_action: {}".format(joint))
        joint.locked = not bool(self.run_action_srv(joint.index).data)
        print(joint.is_locked())

    def check_state(self, joint_idx):
        return self.world.joints[joint_idx].is_locked()


@@ -562,24 +562,30 @@
 elf, request
+_topic
 , response, 
@@ -578,24 +578,30 @@
 ic, response
+_topic
 , request_t,
@@ -662,16 +662,22 @@
 (request
+_topic
 , reques
@@ -684,213 +684,95 @@
 t_t,
- queue_size=10)%0A        self.subscriber = rospy.Subscriber(response, response_t, self.callback)%0A        self.response_received = threading.Event()%0A        self.response = None%0A%0A    def callback(self, msg):
+%0A                queue_size=1, latch=True)%0A        self.response_topic = response_topic
 %0A   
@@ -793,52 +793,23 @@
 onse
+_t
  = 
-msg%0A        self.response_received.set()%0A
+response_t
 %0A%0A  
@@ -891,91 +891,58 @@
     
-self.response_received.wait()%0A        self.response_received.clear()%0A        return
+return rospy.wait_for_message(self.response_topic,
  sel
@@ -951,16 +951,19 @@
 response
+_t)
 %0A%0A%0Aclass

use wait_for_message instead of own synch and latch publisher