import socket
import select
import struct
import code
import threading
import Queue
import time
import os
import sys
import subprocess
import rospy

class listen(threading.Thread):
    # listener class for getting data from robot
    def __init__(self, q, client):
        threading.Thread.__init__(self)
        self.q = q
        self.client = client
        self.callback = None
        self.setDaemon(True)

    def addMsgCallback(self, callbackFcn):
        # adds a function that is called whenever a message is recived by the queue
        self.callback = callbackFcn

    def run(self):
        # runs a listener thread, reading in data and placing it in a queue
        try:
            while True:
                # Wait for incoming data from the RCP or for the listener
                # pipe to close, indicating that we are
                (r, w, x) = select.select(
                    (self.client.s, self.client.listener_pipe[0]), (), ()
                )
                if self.client.listener_pipe[0] in r:
                    # listener pipe signaled the thread to exit
                    os.close(self.client.listener_pipe[0])
                    return
                if self.client.s in r:
                    (t, l, v) = self.client.receive_next_message()
                    if not self.client.do_async_message(t, l, v):
                        self.q.put((t, l, v))
                        try:
                            if self.callback == None:
                                raise NameError("No Callback has been defined")
                            self.callback(t, v)
                        except NameError:
                            pass
                    # time.sleep(0.1)
        except Exception as e:
            rospy.logwarn(e)
            raise e