#! /usr/bin/env python

__author__ = "Fabio Giuseppe Di Benedetto"

import socket
import select
import Queue
import re
import json
import subprocess
import rospy
from robotics_msgs.msg import Robot_Event
from robotics_msgs.msg import CB_Event
from robotics_msgs.srv import FIROS_Info, FIROS_InfoResponse


class RCMDriver:

    DEFAULT_NODE_NAME = "rcmdriver"
    DEFAULT_OUT_TOPIC = "/rcm/robot_event"
    DEFAULT_IN_TOPIC = "/firos/cb_event"
    DEFAULT_SERVICE = "/rcm/firos_info"
    DEFAULT_QUEUE_SIZE = 10
    #
    PARAM_PORT = "port="
    # these are constants duplicated from RCMDriver and used to deal with that
    # component
    COMMAND_KEY = "rcm_d_cmd"
    NOTIFY_CONNECTION = "r_connection"
    NOTIFY_DISCONNECTION = "r_disconnection"
    R_NAME_KEY = "r_name"
    ENTITY_CREATED = "e_created"
    ENTITY_REMOVED = "e_removed"
    ASK_INFO = "r_info"
    R_INFO_KEY = "r_info_data"
    # for topics extraction
    RT_NAME_KEY = "ros_topic_name"
    RT_MSG_KEY = "ros_topic_message"
    # commonly used flags
    READ_ONLY = select.POLLIN | select.POLLPRI | select.POLLHUP | select.POLLERR
    READ_WRITE = READ_ONLY | select.POLLOUT
    DEFAULT_DRIVER_PORT = 9998

    def __init__(self, port=DEFAULT_DRIVER_PORT):
        rospy.init_node(self.DEFAULT_NODE_NAME)
        argv = rospy.myargv()
        p_port = None
        if len(argv) > 1:
            # there are parameters passed
            for arg in argv: