def get_maps():
    """
    Queries the database and returns details of the available topological maps.
    :return: A dictionary where each key is the name of a topological map and each
    item is a dictionary of details. Details are:
       "number_nodes" ; integer
        "edge_actions" : list of action server names used for traversal
        "last_modified" : datetime.datetime object for the last time a node was inserted
    """
    maps = dict()
    msg_store = MessageStoreProxy(collection='topological_maps')

    nodes = msg_store.query(TopologicalNode._type)

    for node in nodes:
        pointset = node[1]["pointset"]
        if not maps.has_key(pointset):
            maps[pointset] = {"number_nodes": 0, "edge_actions": set(), "last_modified": ""}
        maps[pointset]["number_nodes"] += 1
        if (maps[pointset]["last_modified"] == "" or
                    node[1]["inserted_at"] > maps[pointset]["last_modified"]):
            maps[pointset]["last_modified"] = node[1]["inserted_at"]
        for edge in node[0].edges:
            maps[pointset]["edge_actions"].add(edge.action)

    return maps

    def __init__(self, obj_map, obj_conf, roi_map, roi_conf, config_file=None):

        self._soma_obj_roi_ids = dict()
        self._soma_obj_type = dict()
        self._soma_obj_pose = dict()

        
        self.obj_map = obj_map
        self.obj_conf = obj_conf
        self.roi_map = roi_map
        self.roi_conf = roi_conf
        self._soma_roi = dict()
        self._obj_msg_store=MessageStoreProxy(collection="soma")
        self._roi_msg_store=MessageStoreProxy(collection="soma_roi")
        self._retrieve_objects()
        self._retrieve_rois()

        self._server = InteractiveMarkerServer("soma_vis")
        
        if config_file:
            self._config_file = config_file
        else:
            # default file
            rp = RosPack()
            path = rp.get_path('soma_objects') + '/config/'
            filename = 'default.json'
            self._config_file=path+filename

        self._init_types()

    def gen_blog_entry(self, roi_id, obj_id, objs):

        print 'Region: ' + self.get_roi_name(roi_id)
        time = dt.fromtimestamp(int(rospy.get_time()))
        body = '### CHANGE DETECTION REPORT\n\n'
        body += '- **Region:** ' + self.get_roi_name(roi_id) + '\n\n'
        #body += '- **Object ID:** ' + str(obj_id)  + '\n\n'
        body += '- **Time:** ' + str(time)  + '\n\n'

        # Create some blog entries
        msg_store = MessageStoreProxy(collection=self.blog_collection)
        robblog_path = roslib.packages.get_pkg_dir('soma_utils') 

        for idx,obj in enumerate(objs):
            
            bridge = CvBridge()
            im = bridge.imgmsg_to_cv2(obj.image_mask, desired_encoding="bgr8")
            imgray = cv2.cvtColor(im,cv2.COLOR_BGR2GRAY)
            ret,thresh = cv2.threshold(imgray,1,1,1) #cv2.threshold(imgray,127,255,0)
            contours, hierarchy = cv2.findContours(thresh,cv2.RETR_TREE,cv2.CHAIN_APPROX_SIMPLE)

            bridge = CvBridge()
            cv_image = bridge.imgmsg_to_cv2(obj.image_full, desired_encoding="bgr8")
            
            cv2.drawContours(cv_image,contours,-1,(0,0,255),2)
            full_scene_contour = bridge.cv2_to_imgmsg(cv_image)
            img_id = msg_store.insert(full_scene_contour)

            body += '<font color="red">Detected change (' + str(idx+1) + '/'+ str(len(objs)) + '):</font>\n\n![My helpful screenshot](ObjectID(%s))\n\n' % img_id
            
        e = RobblogEntry(title=str(time) + " Change Detection - " + self.roi_name[roi_id], body= body )
        msg_store.insert(e)

    def gen_blog_entry(self, roi_id, uuid):

        print 'Region: ' + self.get_roi_name(roi_id)
        time = dt.fromtimestamp(int(rospy.get_time()))
        body = '### INTRUSION DETECTION REPORT\n\n'
        body += '- **Region:** ' + self.get_roi_name(roi_id) + '\n\n'
        body += '- **Person UUID:** ' + str(uuid)  + '\n\n'
        body += '- **Time:** ' + str(time)  + '\n\n'
        #body += '- **Summary**: <font color="green">ALLOWED ITEMS (' + str(len(pos_objs)) + ')</font>, <font color="red">NOT-ALLOWED ITEMS (' + str(len(neg_objs)) + ')</font>\n\n'


        # # Create some blog entries
        msg_store = MessageStoreProxy(collection=self.blog_collection)
        robblog_path = roslib.packages.get_pkg_dir('soma_utils') 

        img = rospy.wait_for_message('/upper_body_detector/image', Image, 5)

        bridge = CvBridge()
        cv_image = bridge.imgmsg_to_cv2(img, desired_encoding="bgr8")
        ros_img = bridge.cv2_to_imgmsg(cv_image)
        img_id = msg_store.insert(ros_img)
        body += '<font color="red">Detected person:</font>\n\n![My helpful screenshot](ObjectID(%s))\n\n' % img_id

        e = RobblogEntry(title=str(time) + " Intrusion Detection - " + self.get_roi_name(roi_id), body= body )
        msg_store.insert(e)

 def add_edge(self, or_waypoint, de_waypoint, action) :
     #print 'removing edge: '+edge_name
     rospy.loginfo('Adding Edge from '+or_waypoint+' to '+de_waypoint+' using '+action)
     node_name = or_waypoint
     #nodeindx = self._get_node_index(edged[0])
     msg_store = MessageStoreProxy(collection='topological_maps')
     query = {"name" : node_name, "pointset": self.name}
     query_meta = {}
     query_meta["pointset"] = self.name
     query_meta["map"] = self.map
     available = msg_store.query(TopologicalNode._type, query, query_meta)
     if len(available) == 1 :
         found =False
         for i in available[0][0].edges :
             #print i.node
             if i.node == de_waypoint :
                 found=True
                 break
         if not found :
             edge = Edge()
             edge.node = de_waypoint
             edge.action = action
             edge.top_vel = 0.55
             available[0][0].edges.append(edge)
             msg_store.update(available[0][0], query_meta, query, upsert=True)
         else :
             rospy.logerr("Edge already exist: Try deleting it first")
     else :
         rospy.logerr("Impossible to store in DB "+str(len(available))+" waypoints found after query")
         rospy.logerr("Available data: "+str(available))

    def add_tag_cb(self, msg):
        #rospy.loginfo('Adding Tag '+msg.tag+' to '+str(msg.node))
        succeded = True
        for j in msg.node:
            
            msg_store = MessageStoreProxy(collection='topological_maps')
            query = {"name" : j, "pointset": self.nodes.name}
            query_meta = {}
            query_meta["pointset"] = self.nodes.name
            query_meta["map"] = self.nodes.map
    
            #print query, query_meta
            available = msg_store.query(strands_navigation_msgs.msg.TopologicalNode._type, query, query_meta)
            #print len(available)
            for i in available:
                msgid= i[1]['_id']
                if 'tag' in i[1]:
                    if not msg.tag in i[1]['tag']:
                        i[1]['tag'].append(msg.tag)
                else:
                    a=[]
                    a.append(msg.tag)
                    i[1]['tag']=a
                meta_out = str(i[1])
                
                msg_store.update_id(msgid, i[0], i[1], upsert = False)
                #print trstr
            if len(available) == 0:
                 succeded = False

        return succeded, meta_out

    def __init__(self, config_file=None):

        self._map = dict()
        self._obj = dict()
        self._roi = dict()

        self._roi_cnt = dict()
        self._obj_cnt = dict()

        self._num_of_objs = 0
        self._num_of_rois = 0

        self._soma_utils = dict()
        
        self._obj_msg_store=MessageStoreProxy(collection="soma")
        self._roi_msg_store=MessageStoreProxy(collection="soma_roi")

        if config_file:
            self._config_file = config_file
        else:
            # default file
            rp = RosPack()
            path = rp.get_path('soma_utils') + '/config/'
            filename = 'maps.json'
            self._config_file=path+filename
        self._init_maps()

 def rm_tag_cb(self, msg):
     #rospy.loginfo('Adding Tag '+msg.tag+' to '+str(msg.node))
     succeded = True
     for j in msg.node:
         
         msg_store = MessageStoreProxy(collection='topological_maps')
         query = {"name" : j, "pointset": self.nodes.name}
         query_meta = {}
         query_meta["pointset"] = self.nodes.name
         query_meta["map"] = self.nodes.map
 
         #print query, query_meta
         available = msg_store.query(strands_navigation_msgs.msg.TopologicalNode._type, query, query_meta)
         #print len(available)
         succeded = False
         for i in available:
             msgid= i[1]['_id']
             if 'tag' in i[1]:
                 if msg.tag in i[1]['tag']:
                     print 'removing tag'
                     i[1]['tag'].remove(msg.tag)
                     print 'new list of tags'
                     print i[1]['tag']
                     msg_store.update_id(msgid, i[0], i[1], upsert = False)
                     succeded = True
             meta_out = str(i[1])
             
     return succeded, meta_out

class logTweets(object):


    def __init__(self, name):

        rospy.loginfo(" ...starting")

        self.msg_sub = rospy.Subscriber('/strands_tweets/tweet', strands_tweets.msg.Tweeted, self.tweet_callback, queue_size=1)

        rospy.loginfo(" ...done")


        rospy.spin()

    def tweet_callback(self, msg) :
        self.msg_store = MessageStoreProxy(collection='twitter_log')
        
        to_save = strands_tweets.msg.Tweeted()
        try:
            to_save.depth = rospy.wait_for_message('/head_xtion/depth/image_rect_meters', sensor_msgs.msg.Image, timeout=1.0)
        except rospy.ROSException :
           rospy.logwarn("Failed to get camera depth Image")


        try:
            to_save.photo = rospy.wait_for_message('/head_xtion/rgb/image_color', sensor_msgs.msg.Image, timeout=1.0)
        except rospy.ROSException :
            rospy.logwarn("Failed to get camera rgb Image")

        to_save.text = msg.text
        
        meta = {}
        meta["Description"] = "copy of tweeted images"
        self.msg_store.insert(msg, meta)

    def remove_node(self, node_name) :
        rospy.loginfo('Removing Node: '+node_name)
        msg_store = MessageStoreProxy(collection='topological_maps')
        query = {"name" : node_name, "pointset": self.name}
        query_meta = {}
        query_meta["pointset"] = self.name
        query_meta["map"] = self.map

        available = msg_store.query(TopologicalNode._type, query, query_meta)

        node_found = False
        if len(available) == 1 :
            node_found = True
            rm_id = str(available[0][1]['_id'])
            print rm_id
        else :
            rospy.logerr("Node not found "+str(len(available))+" waypoints found after query")
            #rospy.logerr("Available data: "+str(available))


        if node_found :
            query_meta = {}
            query_meta["pointset"] = self.name
            edges_to_rm = []
            message_list = msg_store.query(TopologicalNode._type, {}, query_meta)
            for i in message_list:
                for j in i[0].edges :
                    if j.node == node_name :
                        edge_rm = i[0].name+'_'+node_name
                        edges_to_rm.append(edge_rm)

            for k in edges_to_rm :
                print 'remove: '+k
                self.remove_edge(k)
            msg_store.delete(rm_id)

	def __init__(self):

		self.skeleton_data = {}  #keeps the published complete skeleton in a dictionary. key = uuid
		self.rate = rospy.Rate(15.0)

		## listeners:
		rospy.Subscriber("skeleton_data/complete", skeleton_complete, self.skeleton_callback)

		## Logging params:
		self._with_mongodb = rospy.get_param("~with_mongodb", "true")
		self._logging = rospy.get_param("~logging", "false")
		self._message_store = rospy.get_param("~message_store_prefix", "people_skeleton")

		if self._logging:
			msg_store = self._message_store + "_world_state"
			rospy.loginfo("Connecting to mongodb...%s" % msg_store)
			self._store_client_world = MessageStoreProxy(collection=msg_store)

			msg_store = self._message_store + "_qstag"
			self._store_client_qstag = MessageStoreProxy(collection=msg_store)


		## QSR INFO:
		self._which_qsr = rospy.get_param("~which_qsr", "rcc3")
		#self.which_qsr = ["qtcbs", "argd"]

		self.set_params()
		self.cln = QSRlib_ROS_Client()

def add_localise_by_topic(tmap, node, json_str):
    #print req
    #data = json.loads(req.content)
    #print data

    msg_store = MessageStoreProxy(collection='topological_maps')
    query = {"name" : node, "pointset": tmap}
    query_meta = {}
    #query_meta["pointset"] = tmap
    #query_meta["map"] = self.nodes.map

    #print query, query_meta
    available = msg_store.query(strands_navigation_msgs.msg.TopologicalNode._type, query, query_meta)
    #print len(available)
    if len(available) != 1:
         #succeded = False
         print 'there are no nodes or more than 1 with that name'
    else:
        #succeded = True
        for i in available:
            if not i[0].localise_by_topic:
                msgid= i[1]['_id']
                i[0].localise_by_topic=json_str
                #print i[0]
                print "Updating %s--%s" %(i[0].pointset, i[0].name)
                msg_store.update_id(msgid, i[0], i[1], upsert = False)

    def add_topological_node(self, node_name, node_pose):
        dist = 8.0
        #Get New Node Name
        if node_name:
            name = node_name
        else:
            name = self.get_new_name()

        rospy.loginfo('Creating Node: '+name)        

        if name in self.names:
            rospy.logerr("Node already exists, try another name")
            return False
        
        #Create Message store
        msg_store = MessageStoreProxy(collection='topological_maps')

        meta = {}
        meta["map"] = self.nodes.map
        meta["pointset"] = self.nodes.name
        meta["node"] = name

        node = strands_navigation_msgs.msg.TopologicalNode()
        node.name = name
        node.map = self.nodes.map
        node.pointset = self.name
        node.pose = node_pose
        node.yaw_goal_tolerance = 0.1
        node.xy_goal_tolerance = 0.3
        node.localise_by_topic = ''
        vertices=[(0.69, 0.287), (0.287, 0.69), (-0.287, 0.69), (-0.69, 0.287), (-0.69, -0.287), (-0.287, -0.69), (0.287, -0.69), (0.69, -0.287)]
        for j in vertices :
            v = strands_navigation_msgs.msg.Vertex()
            v.x = float(j[0])
            v.y = float(j[1])
            node.verts.append(v)

        close_nodes = []
        for i in self.nodes.nodes :
            ndist = node_dist(node, i)
            if ndist < dist :
                if i.name != 'ChargingPoint' :
                    close_nodes.append(i.name)

        for i in close_nodes:
            e = strands_navigation_msgs.msg.Edge()
            e.node = i
            e.action = 'move_base'
            eid = '%s_%s' %(node.name, i)
            e.edge_id = eid
            e.top_vel =0.55
            e.map_2d = node.map
            node.edges.append(e)

        for i in close_nodes:
            self.add_edge(i, node.name, 'move_base', '')

        msg_store.insert(node,meta)
        return True

    def get_soma_objects(self):
        """srv call to mongo and get the list of new objects and locations"""

        msg_store = MessageStoreProxy(database="soma2data", collection="soma2")
        objs = msg_store.query(SOMA2Object._type, message_query={"map_name":self.soma_map,"config":self.soma_conf})
        print "queried soma2 objects >> ", objs
        self.soma_objects = ce.get_soma_objects()
        print "hard coded objects >> ", [self.soma_objects[r].keys() for r in self.soma_objects.keys()]

class PeriodicReplicatorClient(Thread):
    def __init__(self):
        Thread.__init__(self)
        self.dead = Event()
        self.interval = rospy.get_param("mongodb_replication_interval", 60 * 60 * 24) # default: 1 day
        self.delete_after_move = rospy.get_param("mongodb_replication_delete_after_move", False)
        self.replicate_interval = rospy.Duration(self.interval)
        self.database = rospy.get_param("robot/database")
        self.collections = sys.argv[2:]
        try:
            self.collections.append(rospy.get_param("robot/name"))
        except KeyError as e:
            rospy.logerr("specify param \"robot/name\" (e.g. pr1012, olive)")
            exit(1)
        self.periodic = rospy.get_param("~periodic", True)
        self.date_msg_store = MessageStoreProxy(database=self.database,
                                                collection="replication")

    def run(self):
        while not self.dead.wait(self.interval):
            move_before = self.time_after_last_replicate_date()
            self.move_entries(move_before)
            self.insert_replicate_date()

    def time_after_last_replicate_date(self):
        delta = 60 * 60 * 24 # 1 day
        try:
            last_replicated = self.date_msg_store.query(StringList._type, single=True, sort_query=[("$natural",-1)])
            date = last_replicated[1]["inserted_at"]
            rospy.loginfo("last replicated at %s", date)
            delta = (dt.datetime.now() - date).seconds + 60
        except Exception as e:
            rospy.logwarn("failed to search last replicated date from database: %s", e)
        finally:
            return rospy.Duration(delta)

    def insert_replicate_date(self):
        try:
            self.date_msg_store.insert(StringList(self.collections))
        except Exception as e:
            rospy.logwarn("failed to insert last replicate date to database: %s", e)

    def move_entries(self, move_before):
        client = actionlib.SimpleActionClient('move_mongodb_entries', MoveEntriesAction)
        client.wait_for_server()
        goal = MoveEntriesGoal(database=self.database,
                               collections=StringList(self.collections),
                               move_before=move_before,
                               delete_after_move=self.delete_after_move)
        client.send_goal(goal, feedback_cb=self.feedback_cb)
        client.wait_for_result()

    def feedback_cb(self, feedback):
        rospy.loginfo(feedback)

    def cancel(self):
        self.dead.set()

    def loadMap(self, point_set, filename):

        point_set=str(sys.argv[1])
        #map_name=str(sys.argv[3])

        msg_store = MessageStoreProxy(collection='topological_maps')

        query_meta = {}
        query_meta["pointset"] = point_set

        available = len(msg_store.query(TopologicalNode._type, {}, query_meta)) > 0

        print available

        if available <= 0 :
            rospy.logerr("Desired pointset '"+point_set+"' not in datacentre")
            rospy.logerr("Available pointsets: "+str(available))
            raise Exception("Can't find waypoints.")

        else :
            query_meta = {}
            query_meta["pointset"] = point_set

            message_list = msg_store.query(TopologicalNode._type, {}, query_meta)

            #node=TopologicalNode()

            top_map=[]
            for i in message_list:
                nodeinf = {}
                nodeinf["node"] = yaml.load(str(i[0]))
                if nodeinf["node"]["localise_by_topic"]:
                    nodeinf["node"]["localise_by_topic"] = json.dumps(nodeinf["node"]["localise_by_topic"])
                nodeinf["meta"] = i[1] #str(bson.json_util.dumps(i[1], indent=1))
                nodeinf["meta"].pop("last_updated_by", None)
                nodeinf["meta"].pop('inserted_at', None)
                nodeinf["meta"].pop('last_updated_at', None)
                nodeinf["meta"].pop('stored_type', None)            
                nodeinf["meta"].pop('stored_class', None) 
                nodeinf["meta"].pop('inserted_by', None)
                nodeinf["meta"].pop('_id', None)
                top_map.append(nodeinf)
                #val = bson.json_util.dumps(nodeinf["meta"], indent=1)       
            
            
            
            top_map.sort(key=lambda x: x['node']['name'])
            yml = yaml.safe_dump(top_map, default_flow_style=False)
            #print yml
            #print s_output
            
            fh = open(filename, "w")
            #s_output = str(bson.json_util.dumps(nodeinf, indent=1))
            s_output = str(yml)
            #print s_output
            fh.write(s_output)
            fh.close            

 def get_message_store_messages(self, typ=None):
     msgs = []
     proxy = MessageStoreProxy()
     for msg in self._msg_store_objects:
         if typ != msg.typ and typ is not None:
             continue
         proxy.database =  msg.database
         proxy.collection =  msg.collection
         msgs.append(proxy.query_id(msg.obj_id, msg.typ)[0])
     return msgs

 def loadConfig(self, data_set):
     msg_store = MessageStoreProxy(collection="hri_behaviours")
     query_meta = {}
     query_meta["collection"] = data_set
     if len(msg_store.query(std_msgs.msg.String._type, {}, query_meta)) == 0:
         rospy.logerr("Desired dialogue options '"+data_set+"' not in datacentre.")
         raise Exception("Can't find data in datacentre.")
     else:
         message = msg_store.query(std_msgs.msg.String._type, {}, query_meta)
         return json.loads(message[0][0].data)

    def delete_map(self) :

        rospy.loginfo('Deleting map: '+self.name)
        msg_store = MessageStoreProxy(collection='topological_maps')

        query_meta = {}
        query_meta["pointset"] = self.name

        message_list = msg_store.query(TopologicalNode._type, {}, query_meta)
        for i in message_list:
            rm_id = str(i[1]['_id'])
            msg_store.delete(rm_id)

def rename_node(name, new_name, top_map_name):
    """
    Renames a topological map node. All edges will be updated to reflect the change.

    @param name: the current name of the node to be changec
    @param new_name: the new name to give the node
    @param top_map_name: the name of the topological map to work with (pointset)

    @return (number of nodes changed, number of edges changed)
    """

    maps = queries.get_maps()
    if top_map_name not in maps:
        raise Exception("Unknown topological map.")

    msg_store = MessageStoreProxy(collection='topological_maps')
    
    nodes = msg_store.query(TopologicalNode._type, {}, {'pointset':top_map_name})
    node_names = [node.name for node,meta in nodes]
    node_renames = 0
    edge_changes = 0
    node_changes = 0

    if name not in node_names:
        raise Exception("No such node.")
    if new_name in node_names:
        raise Exception("New node name already in use.")

    old_metas = []
    for node, meta in nodes:
        old_metas.append(copy.deepcopy(meta))
        if meta["node"] == name:
            meta["node"] = new_name
        if node.name == name:
            node.name = new_name
            node_renames += 1
            if node_renames > 1:
                raise Exception("More than one node has the same name!")
        for edge in node.edges:
            if edge.node == name:
                edge.node = new_name
            if edge.edge_id.find(name) > -1:
                edge.edge_id = edge.edge_id.replace(name, new_name)
                edge_changes += 1
            
    # Save the changed nodes
    for ((node, meta), old_meta) in zip(nodes, old_metas):
        changed = msg_store.update(node, meta, {'name':old_meta['node'],
                                                'pointset':meta['pointset']})
        if changed.success:
            node_changes += 1

    return (node_changes, edge_changes)