# Path: commproxy.py
# class CallbackSocket(object):
#     """Listen on a socket and call back when a message comes in."""
#     name = "commproxy"
# 
#     def __init__(self, port, msg_sep="\n", local=False):
#         self.msg_sep = msg_sep
#         self.callbacks = []
#         self.queue = Queue()
#         self._sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
#         self._sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
#         self._sock.bind(('' if not local else 'localhost', port))
#         self._sock.listen(5)
#         self._callback_lock = threading.Lock()
# 
#         # Make a thread that accepts new connections
#         accept_thread = threading.Thread(target=self._accept)
#         accept_thread.daemon = True
#         accept_thread.start()
# 
#         # Start the callback thread for synchronously handling requests
#         callback_thread = threading.Thread(target=self._pumpmsg)
#         callback_thread.daemon = True
#         callback_thread.start()
# 
#     def _accept(self):
#         """Accept connections on the listening port."""
#         timed_out = False  # Use this flag to avoid printing when we timeout
#         while True:
#             if not timed_out:
#                 print "%s: Waiting for connection..." % self.name
#             try:
#                 conn, addr = self._sock.accept()
#                 print "%s: Connected to %s" % (self.name, str(addr))
#                 # TODO: Add support for unique client names
#                 client = ClientHandler(conn, addr, self)
#                 client.daemon = True
#                 client.start()
# 
#                 timed_out = False
#             except timeout:
#                 timed_out = True
#                 continue
#             except:   # Because any error can occur here during shutdown
#                 break
# 
#     def shutdown(self):
#         """Shutdown the socket."""
#         print "%s: Shutting down socket." % self.name
#         try:
#             self._sock.shutdown(socket.SHUT_RDWR)
#         except error:
#             pass
#         self._sock.close()
#         print "%s: Socket closed." % self.name
# 
#     def register_callback(self, func):
#         """Add a callback to the set of callbacks."""
#         with self._callback_lock:
#             self.callbacks.append(func)
# 
#     def _pumpmsg(self):
#         """Read messages and synchronously call back listeners."""
#         while True:
#             msg, client = self.queue.get()
#             # Lock just in case callbacks change while we are pumping
#             with self._callback_lock:
#                 for func in self.callbacks:
#                     func(msg, client)


import json
import rospy
from threading import Thread, Lock
from time import sleep
from commproxy import CallbackSocket
from worldmap import get_topo_map, get_occupancy_grid
from subtle_msgs.msg import Fiducial
from tf import TransformListener
from tf import ExtrapolationException
from geometry_msgs.msg import *
    @param obj_name String name of map object
    @param obj_type String either "region" or "icon"
    @param location [[x1,y1], [x2,y2] ...] - should only be one "point" for icon'''
    validObjTypes = ['region', 'icon-bomb', 'icon-hostage', 'icon-badguy',
                     'icon-user1', 'icon-user2', 'icon-robot']
    validObjTypes.index(obj_type)
    new_obj = dict()
    new_obj['type'] = "mapupdate"
    new_obj['data'] = dict()
    new_obj['data']['event'] = "addobj"
    new_obj['data']['name'] = obj_name
    new_obj['data']['params']  =  dict()
    new_obj['data']['params']['type'] = obj_type
    new_obj['data']['params']['location'] = location
    return json.dumps(new_obj)


def SerializeRemObj(obj_name, obj_type):
    ''' Serialize Rem Obj Request 
    @param obj_name String name of map object'''
    new_obj = dict()
    new_obj['type'] = "mapupdate"
    new_obj['data'] = dict()
    new_obj['data']['event'] = "remobj"
    new_obj['data']['name'] = obj_name
    new_obj['data']['params'] = dict()
    new_obj['data']['params']['type'] = obj_type
    return json.dumps(new_obj)



# Path: commproxy.py
# class CallbackSocket(object):
#     """Listen on a socket and call back when a message comes in."""
#     name = "commproxy"
# 
#     def __init__(self, port, msg_sep="\n", local=False):
#         self.msg_sep = msg_sep
#         self.callbacks = []
#         self.queue = Queue()
#         self._sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
#         self._sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
#         self._sock.bind(('' if not local else 'localhost', port))
#         self._sock.listen(5)
#         self._callback_lock = threading.Lock()
# 
#         # Make a thread that accepts new connections
#         accept_thread = threading.Thread(target=self._accept)
#         accept_thread.daemon = True
#         accept_thread.start()
# 
#         # Start the callback thread for synchronously handling requests
#         callback_thread = threading.Thread(target=self._pumpmsg)
#         callback_thread.daemon = True
#         callback_thread.start()
# 
#     def _accept(self):
#         """Accept connections on the listening port."""
#         timed_out = False  # Use this flag to avoid printing when we timeout
#         while True:
#             if not timed_out:
#                 print "%s: Waiting for connection..." % self.name
#             try:
#                 conn, addr = self._sock.accept()
#                 print "%s: Connected to %s" % (self.name, str(addr))
#                 # TODO: Add support for unique client names
#                 client = ClientHandler(conn, addr, self)
#                 client.daemon = True
#                 client.start()
# 
#                 timed_out = False
#             except timeout:
#                 timed_out = True
#                 continue
#             except:   # Because any error can occur here during shutdown
#                 break
# 
#     def shutdown(self):
#         """Shutdown the socket."""
#         print "%s: Shutting down socket." % self.name
#         try:
#             self._sock.shutdown(socket.SHUT_RDWR)
#         except error:
#             pass
#         self._sock.close()
#         print "%s: Socket closed." % self.name
# 
#     def register_callback(self, func):
#         """Add a callback to the set of callbacks."""
#         with self._callback_lock:
#             self.callbacks.append(func)
# 
#     def _pumpmsg(self):
#         """Read messages and synchronously call back listeners."""
#         while True:
#             msg, client = self.queue.get()
#             # Lock just in case callbacks change while we are pumping
#             with self._callback_lock:
#                 for func in self.callbacks:
#                     func(msg, client)


# Path: jr/mapcontrol.py
import json
import rospy
from threading import Thread, Lock
from time import sleep
from commproxy import CallbackSocket
from worldmap import get_topo_map, get_occupancy_grid
from subtle_msgs.msg import Fiducial
from tf import TransformListener
from tf import ExtrapolationException
from geometry_msgs.msg import *

    @param obj_name String name of map object
    @param obj_type String either "region" or "icon"
    @param location [[x1,y1], [x2,y2] ...] - should only be one "point" for icon'''
    validObjTypes = ['region', 'icon-bomb', 'icon-hostage', 'icon-badguy',
                     'icon-user1', 'icon-user2', 'icon-robot']
    validObjTypes.index(obj_type)
    new_obj = dict()
    new_obj['type'] = "mapupdate"
    new_obj['data'] = dict()
    new_obj['data']['event'] = "addobj"
    new_obj['data']['name'] = obj_name
    new_obj['data']['params']  =  dict()
    new_obj['data']['params']['type'] = obj_type
    new_obj['data']['params']['location'] = location
    return json.dumps(new_obj)


def SerializeRemObj(obj_name, obj_type):
    ''' Serialize Rem Obj Request 
    @param obj_name String name of map object'''
    new_obj = dict()
    new_obj['type'] = "mapupdate"
    new_obj['data'] = dict()
    new_obj['data']['event'] = "remobj"
    new_obj['data']['name'] = obj_name
    new_obj['data']['params'] = dict()
    new_obj['data']['params']['type'] = obj_type
    return json.dumps(new_obj)



class iPadConnection(CallbackSocket):