'''

Python 3.x library to control an UR robot through its TCP/IP interfaces

Copyright (C) 2017  Martin Huus Bjerge, Rope Robotics ApS, Denmark

Permission is hereby granted, free of charge, to any person obtaining a copy of this software

and associated documentation files (the "Software"), to deal in the Software without restriction,

including without limitation the rights to use, copy, modify, merge, publish, distribute,

sublicense, and/or sell copies of the Software, and to permit persons to whom the Software

is furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all copies

or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,

INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR

PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL "Rope Robotics ApS" BE LIABLE FOR ANY CLAIM,

DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,

OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.

Except as contained in this notice, the name of "Rope Robotics ApS" shall not be used

in advertising or otherwise to promote the sale, use or other dealings in this Software

without prior written authorization from "Rope Robotics ApS".

'''

__author__ = "Martin Huus Bjerge"

__copyright__ = "Copyright 2017, Rope Robotics ApS, Denmark"

__license__ = "MIT License"

from pkg_resources import resource_filename

import URBasic

import threading

import socket

import struct

import select

import numpy as np

import xml.etree.ElementTree as ET

import time

import os.path

DEFAULT_TIMEOUT = 1.0

class Command:

    RTDE_REQUEST_PROTOCOL_VERSION = 86        # ascii V

    RTDE_GET_URCONTROL_VERSION = 118          # ascii v

    RTDE_TEXT_MESSAGE = 77                    # ascii M

    RTDE_DATA_PACKAGE = 85                    # ascii U

    RTDE_CONTROL_PACKAGE_SETUP_OUTPUTS = 79   # ascii O

    RTDE_CONTROL_PACKAGE_SETUP_INPUTS = 73    # ascii I

    RTDE_CONTROL_PACKAGE_START = 83           # ascii S

    RTDE_CONTROL_PACKAGE_PAUSE = 80           # ascii P

class ConnectionState:

    ERROR = 0

    DISCONNECTED = 1

    CONNECTED = 2

    PAUSED = 3

    STARTED = 4

class RTDE(threading.Thread): #, metaclass=Singleton

    '''

    Interface to UR robot Real Time Data Exchange interface.

    See this site for more detail:

    http://www.universal-robots.com/how-tos-and-faqs/how-to/ur-how-tos/real-time-data-exchange-rtde-guide-22229/

    The constructor takes a UR robot hostname as input and a path to a RTDE configuration file.

    Input parameters:

    host (string):  Hostname or IP of UR Robot (RT CLient server)

    conf_filename (string):  Path to xml file describing what channels to activate

    logger (URBasis_DataLogging obj): A instance if a logger object if common logging is needed.

    Example:

    rob = URBasic.rtde.RTDE('192.168.56.101', 'rtde_configuration.xml')

    rob.close_rtde()

    '''

    def __init__(self, robotModel, conf_filename=None):

        '''

        Constructor see class description for more info.

        '''

        if(False):

            assert isinstance(robotModel, URBasic.robotModel.RobotModel)  ### This line is to get code completion for RobotModel

        self.__robotModel = robotModel

        logger = URBasic.dataLogging.DataLogging()

        name = logger.AddEventLogging(__name__,log2Consol=False)

        self._logger = logger.__dict__[name]

        self.__reconnectTimeout = 600 #Seconds (while in run)

        self.__dataSend = RTDEDataObject()

        # always use the rtdeCOnfiguration in the packages folder

        conf_filename = resource_filename(__name__, 'rtdeConfigurationDefault.xml')

        self.__conf_filename = conf_filename

        self.__stop_event = True

        threading.Thread.__init__(self)

        self.__dataEvent = threading.Condition()

        self.__conn_state = ConnectionState.DISCONNECTED

        self.__sock = None

        self.__rtde_output_names = None

        self.__rtde_output_config = None

        self.__rtde_input_names = None

        self.__rtde_input_initValues = None

        self.__rtde_input_config = None

        self.__controllerVersion = None

        self.__protocol_version = None

        self.__packageCounter = 0

        self.start()

        self._logger.info('RTDE constructor done')

    def __connect(self):

        '''

        Initialize RTDE connection to host and set up data interfaces based on configuration XML.

        Return value:

        success (boolean)

        '''

        if self.__sock:

            return True

        try:

            self.__sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)

            self.__sock.setsockopt(socket.IPPROTO_TCP, socket.TCP_NODELAY, 1)

            self.__sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)

            self.__sock.settimeout(DEFAULT_TIMEOUT)

            self.__sock.connect((self.__robotModel.ipAddress, 30004))

            self.__conn_state = ConnectionState.CONNECTED

        except (socket.timeout, socket.error):

            if self.__sock:

                self.sock.close()

            self.__sock = None

            return False

        return True

    def __disconnect(self):

        '''

        Close the RTDE connection.

        '''

        if self.__sock:

            self.__sock.close()

            self.__sock = None

        self.__conn_state = ConnectionState.DISCONNECTED

        return True

    def __isConnected(self):

        '''

        Returns True if the connection is open.

        Return value:

        open (boolean)

        '''

        return self.__conn_state > ConnectionState.DISCONNECTED

    def isRunning(self):

        '''

        Return True if RTDE interface is running

        '''

        return self.__conn_state >= ConnectionState.STARTED

    def __getControllerVersion(self):

        '''

        Returns the software version of the robot controller running the RTDE server.

        Return values:

        major (int)

        minor (int)

        bugfix (int)

        '''

        cmd = Command.RTDE_GET_URCONTROL_VERSION

        self.__send(cmd)

    def __negotiateProtocolVersion(self, protocol):

        '''

        Negotiate the protocol version with the server.

        Returns True if the controller supports the specified protocol version.

        We recommend that you use this to ensure full compatibility between your

        application and future versions of the robot controller.

        Input parameters:

        protocol (int): protocol version number

        Return value:

        success (boolean)

        '''

        cmd = Command.RTDE_REQUEST_PROTOCOL_VERSION

        payload = struct.pack('>H',protocol)

        self.__send(cmd, payload)

    def __setupInput(self, input_variables=None, types=[], initValues=None):

        '''

        Configure an input package that the external(this) application will send to the robot controller.

        An input package is a collection of input input_variables that the external application will provide

        to the robot controller in a single update. Variables is a list of variable names and should be

        a subset of the names supported as input by the RTDE interface.The list of types is optional,

        but if any types are provided it should have the same length as the input_variables list.

        The provided types will be matched with the types that the RTDE interface expects and the

        function returns None if they are not equal. Multiple input packages can be configured.

        The returned InputObject has a reference to the recipe id which is used to identify the

        specific input format when sending an update.

        If input_variables is empty, xml configuration file is used.

        Input parameters:

        input_variables (list<string> or Str): [Optional] Variable names from the list of possible RTDE inputs

        types (list<string> or str): [Optional] Types matching the input_variables

        Return value:

        success (boolean)

        '''

        if input_variables is None:

            tree = ET.parse(self.__conf_filename)

            root = tree.getroot()

            #setup data that can be send

            recive = root.find('send')

            input_variables = []

            initValues = []

            for child in recive:

                input_variables.append(child.attrib['name'])

                initValues.append(float(child.attrib['initValue']))

        cmd = Command.RTDE_CONTROL_PACKAGE_SETUP_INPUTS

        if type(input_variables) is list:

            payload = ','.join(input_variables)

        elif type(input_variables) is str:

            payload = input_variables

        else:

            self._logger.error('Variables must be list of stings or a single string, input_variables is: ' + str(type(input_variables)))

            return None

        self.__rtde_input_names = input_variables

        self.__rtde_input_initValues = initValues

        payload = payload.encode('utf-8')

        self.__send(cmd, payload)

        return True

    def __setupOutput(self, output_variables=None, types=[]):

        '''

        Configure an output package that the robot controller will send to the

        external(this) application at the control frequency. Variables is a list of

        variable names and should be a subset of the names supported as output by the

        RTDE interface. The list of types is optional, but if any types are provided

        it should have the same length as the output_variables list. The provided types will

        be matched with the types that the RTDE interface expects and the function

        returns False if they are not equal. Only one output package format can be

        specified and hence no recipe id is used for output.

        If output_variables is empty, xml configuration file is used.

        Input parameters:

        output_variables (list<string> or str): [Optional] Variable names from the list of possible RTDE outputs

        types (list<string> or str): [Optional] Types matching the output_variables

        Return value:

        success (boolean)

        '''

        if output_variables is None:

            if not os.path.isfile(self.__conf_filename):

                self._logger.error("Configuration file don't exist : " + self.__conf_filename)

                return False

            tree = ET.parse(self.__conf_filename)

            root = tree.getroot()

            #Setup data to be recived

            recive = root.find('receive')

            output_variables = ['timestamp']

            for child in recive:

                output_variables.append(child.attrib['name'])

        cmd = Command.RTDE_CONTROL_PACKAGE_SETUP_OUTPUTS

        if type(output_variables) is list:

            payload = ','.join(output_variables)

        elif type(output_variables) is str:

            payload = output_variables

        else:

            self._logger.error('Variables must be list of stings or a single string, output_variables is: ' + str(type(output_variables)))

            return None

        self.__rtde_output_names = output_variables

        payload = payload.encode('utf-8')

        self.__send(cmd, payload)

        return True

    def __sendStart(self):

        '''

        Sends a start command to the RTDE server.

        Setup of all inputs and outputs must be done before starting the RTDE interface

        Return value:

        success (boolean)

        '''

        cmd = Command.RTDE_CONTROL_PACKAGE_START

        self.__send(cmd)

        return True

    def __sendPause(self):

        '''

        Sends a pause command to the RTDE server

        When paused it is possible to change the input and output configurations

        Return value:

        success (boolean)

        '''

        cmd = Command.RTDE_CONTROL_PACKAGE_PAUSE

        self.__send(cmd)

        return True

    def sendData(self):

        '''

        Send the contents of a RTDEDataObject as input to the RTDE server.

        Returns True if successful.

        Return value:

        success (boolean)

        '''

        if self.__conn_state != ConnectionState.STARTED:

            self._logger.error('Cannot send when RTDE is inactive')

            return

        #if not (self.__rtde_input_config.names.has_key(self.__dataSend.recipe_id)):

        #    self._logger.error('Input configuration id not found: ' + str(self.__dataSend.recipe_id))

        #    return

        if self.__robotModel.StopRunningFlag():

            self._logger.info('"sendData" send ignored due to "stopRunningFlag" True')

            return

        #config = self.__rtde_input_config[self.__dataSend.recipe_id]

        config = self.__rtde_input_config

        return self.__send(Command.RTDE_DATA_PACKAGE, config.pack(self.__dataSend))

    def setData(self, variable_name, value):

        '''

        Set data to be send to the robot

        Object is locked while updating to avoid sending half updated values,

        hence send all values as two lists of equal lengths

        Input parameters:

        variable_name (List/str):  Variable name from the list of possible RTDE inputs

        value (list/int/double)

        Return value:

        Status (Bool): True=Data sucesfull updated, False=Data not updated

        '''

        #check if input is list of equal length

        if type(variable_name) is list:

            if type(variable_name) != type(value):

                raise ValueError("RTDE " + str(variable_name) + " is not type of " + str(value))

                #return False

            if len(variable_name) != len(value):

                raise ValueError("List of RTDE Output values does not have same length as list of variable names")

                #return False

            for ii in range(len(value)):

                if self.hasattr(self.__rtde_input_config.names, variable_name[ii]):

                    self.__dataSend.__dict__[variable_name[ii]] = value[ii]

                else:

                    raise ValueError(str(variable_name[ii]) + " not found in RTDE OUTPUT config")

                    #return False

        else:

            if variable_name in self.__rtde_input_config.names:

                self.__dataSend.__dict__[variable_name] = value

            else:

                raise ValueError(str(variable_name) + " not found in RTDE OUTPUT config")

    def __send(self, command, payload=bytes()):

        '''

        Send command and data (payload) to Robot Controller

        and receive the respond from the Robot Controller.

        Input parameters:

        cmd (int)

        payload (bytes)

        Return value:

        success (boolean)

        '''

        fmt = '>HB'

        size = struct.calcsize(fmt) + len(payload)

        buf = struct.pack(fmt, size, command) + payload

        if self.__sock is None:

            self._logger.debug('Unable to send: not connected to Robot')

            return False

        (_, writable, _) = select.select([], [self.__sock], [], DEFAULT_TIMEOUT)

        if len(writable):

            self.__sock.sendall(buf)

            return True

        else:

            self._logger.info("RTDE disconnected")

            self.__disconnect()

            return False

    def __receive(self):

        byte_buffer = bytes()

        (readable, _, _) = select.select([self.__sock], [], [], DEFAULT_TIMEOUT)

        if (len(readable)):

            more = self.__sock.recv(4096)  # TODO: MAKE THIS 4096 instead of 16384

            if len(more) == 0:

                self._logger.info("RTDE disconnected")

                self.__disconnect()

                return None

            byte_buffer +=  more

        while len(byte_buffer) >= 3:

            (packet_size, packet_command) = struct.unpack_from('>HB', byte_buffer)

            buffer_length = len(byte_buffer)

            if ((buffer_length) >= packet_size):

                packet, byte_buffer = byte_buffer[3:packet_size], byte_buffer[packet_size:]

                data = self.__decodePayload(packet_command, packet)

                if(packet_command == Command.RTDE_GET_URCONTROL_VERSION):

                    self.__verifyControllerVersion(data)

                elif(packet_command == Command.RTDE_REQUEST_PROTOCOL_VERSION):

                    self.__verifyProtocolVersion(data)

                elif(packet_command == Command.RTDE_CONTROL_PACKAGE_SETUP_INPUTS):

                    self.__rtde_input_config = data

                    self.__rtde_input_config.names = self.__rtde_input_names

                    #self.__rtde_input_config[self.__rtde_input_config.id] = self.__rtde_input_config

                    self.__dataSend = RTDEDataObject.create_empty(self.__rtde_input_names, self.__rtde_input_config.id)

                    if self.__rtde_input_initValues is not None:

                        for ii in range(len(self.__rtde_input_config.names)):

                            if 'UINT8' == self.__rtde_input_config.types[ii]:

                                self.setData(self.__rtde_input_config.names[ii], int(self.__rtde_input_initValues[ii]))

                            elif 'UINT32' == self.__rtde_input_config.types[ii]:

                                self.setData(self.__rtde_input_config.names[ii], int(self.__rtde_input_initValues[ii]))

                            elif 'INT32' == self.__rtde_input_config.types[ii]:

                                self.setData(self.__rtde_input_config.names[ii], int(self.__rtde_input_initValues[ii]))

                            elif 'DOUBLE' == self.__rtde_input_config.types[ii]:

                                self.setData(self.__rtde_input_config.names[ii], (self.__rtde_input_initValues[ii]))

                            else:

                                self._logger.error('Unknown data type')

                elif(packet_command == Command.RTDE_CONTROL_PACKAGE_SETUP_OUTPUTS):

                    self.__rtde_output_config = data

                    self.__rtde_output_config.names = self.__rtde_output_names

                elif(packet_command == Command.RTDE_CONTROL_PACKAGE_START):

                    self._logger.info('RTDE started')

                    self.__conn_state = ConnectionState.STARTED

                elif(packet_command == Command.RTDE_CONTROL_PACKAGE_PAUSE):

                    self._logger.info('RTDE paused')

                    self.__conn_state = ConnectionState.PAUSED

                elif(packet_command == Command.RTDE_DATA_PACKAGE):

                    self.__updateModel(data)

                elif(packet_command == 0):

                    byte_buffer = bytes()

            else:  # TODO: make this so that it still checks the packet content

                print("skipping package - unexpected packet_size - length: " , str(len(byte_buffer)), str(packet_size), str(packet_command))

                byte_buffer = bytes()

        if len(byte_buffer) != 0:

            self._logger.warning('skipping package - not a package but buffer was not empty')

            byte_buffer = bytes()

    def __updateModel(self, rtde_data_package):

        self.__packageCounter = self.__packageCounter + 1

        #print("got a rtde package nr " + str(self.__packageCounter))

        #print("RTDe Package keys:", rtde_data_package.keys())

        if(self.__packageCounter % 1000 == 0):

            self._logger.info("Total packages: " + str(self.__packageCounter))

        if(self.__robotModel.dataDir['timestamp'] != None):

            delta = rtde_data_package['timestamp'] - self.__robotModel.dataDir['timestamp']

            if(delta > 0.00800001):

                self._logger.error("Lost some RTDE at " + str(rtde_data_package['timestamp']) + " - " + str(delta*1000) + " milliseconds since last package")

        for tagname in rtde_data_package.keys():

            self.__robotModel.dataDir[tagname] = rtde_data_package[tagname]

    def __verifyControllerVersion(self, data):

        self.__controllerVersion = data

        (major, minor, bugfix, build) = self.__controllerVersion

        if major and minor and bugfix:

            self._logger.info('Controller version: ' + str(major) + '.' + str(minor) + '.' + str(bugfix) + '-' + str(build))

            if major <= 3 and minor <= 2 and bugfix < 19171:

                self._logger.error("Please upgrade your controller to minimum version 3.2.19171")

                raise ValueError("Please upgrade your controller to minimum version 3.2.19171")

    def __verifyProtocolVersion(self, data):

        self.__protocol_version = data

        if(self.__protocol_version != 1):

            raise ValueError("We only support protocol version 1 at the moment")

    def __decodePayload(self, cmd, payload):

        #print(cmd)

        '''

        Decode the package received from the Robot

        payload (bytes)

        Return value(s):

        Output from Robot controller (type is depended on the cmd value)

        '''

        if cmd == Command.RTDE_REQUEST_PROTOCOL_VERSION:

            if len(payload) != 1:

                self._logger.error('RTDE_REQUEST_PROTOCOL_VERSION: Wrong payload size')

                return None

            return struct.unpack_from('>B', payload)[0]

        elif cmd == Command.RTDE_GET_URCONTROL_VERSION:

            if 12 == len(payload):

                return np.append(np.array(struct.unpack_from('>III', payload)), 0)

            elif 16 == len(payload):

                return np.array(struct.unpack_from('>IIII', payload))

            else:

                self._logger.error('RTDE_GET_URCONTROL_VERSION: Wrong payload size')

                return None

        elif cmd == Command.RTDE_TEXT_MESSAGE:

            if len(payload) < 1:

                self._logger.error('RTDE_TEXT_MESSAGE: No payload')

                return None

            EXCEPTION_MESSAGE = 0

            ERROR_MESSAGE = 1

            WARNING_MESSAGE = 2

            INFO_MESSAGE = 3

            fmt = ">" + str(len(payload)) + "B"

            out = struct.unpack_from(fmt, payload)

            level = out[0]

            message = ''.join(map(chr,out[1:]))

            if(level == EXCEPTION_MESSAGE or

               level == ERROR_MESSAGE):

                self._logger.error('Server message: ' + message)

            elif level == WARNING_MESSAGE:

                self._logger.warning('Server message: ' + message)

            elif level == INFO_MESSAGE:

                self._logger.info('Server message: ' + message)

        elif cmd == Command.RTDE_CONTROL_PACKAGE_SETUP_OUTPUTS:

            if len(payload) < 1:

                self._logger.error('RTDE_CONTROL_PACKAGE_SETUP_OUTPUTS: No payload')

                return None

            has_recipe_id = False

            output_config = RTDE_IO_Config.unpack_recipe(payload, has_recipe_id)

            return output_config

        elif cmd == Command.RTDE_CONTROL_PACKAGE_SETUP_INPUTS:

            if len(payload) < 1:

                self._logger.error('RTDE_CONTROL_PACKAGE_SETUP_INPUTS: No payload')

                return None

            has_recipe_id = True

            input_config = RTDE_IO_Config.unpack_recipe(payload, has_recipe_id)

            return input_config

        elif cmd == Command.RTDE_CONTROL_PACKAGE_START:

            if len(payload) != 1:

                self._logger.error('RTDE_CONTROL_PACKAGE_START: Wrong payload size')

                return None

            return bool(struct.unpack_from('>B', payload)[0])

        elif cmd == Command.RTDE_CONTROL_PACKAGE_PAUSE:

            if len(payload) != 1:

                self._logger.error('RTDE_CONTROL_PACKAGE_PAUSE: Wrong payload size')

                return None

            return bool(struct.unpack_from('>B', payload)[0])

        elif cmd == Command.RTDE_DATA_PACKAGE:

            if self.__rtde_output_config is None:

                self._logger.error('RTDE_DATA_PACKAGE: Missing output configuration')

                return None

            output = self.__rtde_output_config.unpack(payload)

            return output

        else:

            self._logger.error('Unknown RTDE command type: ' + chr(cmd) + str(cmd))

    def __listEquals(self, l1, l2):

        if len(l1) != len(l2):

            return False

        for i in range(len((l1))):

            if l1[i] != l2[i]:

                return False

        return True

    def __wait(self):

        '''Wait while the data receiving thread is receiving a new data set.'''

        cnt = 0

        while self.__conn_state < ConnectionState.STARTED:

            time.sleep(1)

            cnt +=1

            if cnt>5:

                self._logger.warning('wait_rtde timed out while RTDE interface not running')

                return False

        with self.__dataEvent:

            self.__dataEvent.wait()

        return True

    '''Threading Data receive'''

    def close(self):

        if self.__stop_event is False:

            self.__stop_event = True

            self.__wait()

            self.join()

            self.__disconnect()

    def run(self):

        self.__stop_event = False

        t0 = time.time()

        while (time.time()-t0<self.__reconnectTimeout) and self.__conn_state != ConnectionState.STARTED:

            self.__connect()

            self.__disconnect()

            self.__connect()

            self.__getControllerVersion()

            self.__receive()

            self.__negotiateProtocolVersion(1)

            self.__receive()

            self.__setupOutput()

            self.__receive()

            self.__setupInput()

            self.__receive()

            self.__sendStart()

            self.__receive()

            #time.sleep(0.5)

        if self.__conn_state != ConnectionState.STARTED:

            self._logger.error("RTDE interface not able to connect and timed out!")

            return

        while (not self.__stop_event) and (time.time()-t0<self.__reconnectTimeout):

            try:

                #self.__receive(Command.RTDE_DATA_PACKAGE)

                #startTime = time.time()

                self.__receive()

                t0 = time.time()

                #delta = t0-startTime

                #print("Time to recieve: " + str(delta))

            except Exception:

                if self.__conn_state >= ConnectionState.STARTED:

                    self.__conn_state = ConnectionState.ERROR

                    self._logger.error("RTDE interface stopped running")

                self.__sendPause()

                if not self.__sendStart():

                    self.__disconnect()

                    time.sleep(1)

                    self.__connect()

                    self.__setupOutput()

                    self.__setupInput()

                    self.__sendStart()

                if self.__conn_state == ConnectionState.STARTED:

                    self._logger.info("RTDE interface restarted")

                else:

                    self._logger.warning("RTDE reconnection failed!")

        self.__sendPause()

        with self.__dataEvent:

            self.__dataEvent.notifyAll()

        self._logger.info("RTDE interface is stopped")

class RTDE_IO_Config(object):

    __slots__ = ['id', 'names', 'types', 'fmt']

    @staticmethod

    def unpack_recipe(buf, has_recipe_id):

        rmd = RTDE_IO_Config();

        if has_recipe_id:

            rmd.id = struct.unpack_from('>B', buf)[0]

            fmt = ">" + str(len(buf)) + "B"

            buf = struct.unpack_from(fmt, buf)

            buf = ''.join(map(chr,buf[1:]))

            rmd.types = buf.split(',')

            rmd.fmt = '>B'

        else:

            fmt = ">" + str(len(buf)) + "B"

            buf = struct.unpack_from(fmt, buf)

            buf = ''.join(map(chr,buf[:]))

            rmd.types = buf.split(',')

            rmd.fmt = '>'

        for i in rmd.types:

            if i=='INT32':

                rmd.fmt += 'i'

            elif i=='UINT32':

                rmd.fmt += 'I'

            elif i=='VECTOR6D':

                rmd.fmt += 'd'*6

            elif i=='VECTOR3D':

                rmd.fmt += 'd'*3

            elif i=='VECTOR6INT32':

                rmd.fmt += 'i'*6

            elif i=='VECTOR6UINT32':

                rmd.fmt += 'I'*6

            elif i=='DOUBLE':

                rmd.fmt += 'd'

            elif i=='UINT64':

                rmd.fmt += 'Q'

            elif i=='UINT8':

                rmd.fmt += 'B'

            elif i=='IN_USE':

                raise ValueError('An input parameter is already in use.')

            else:

                raise ValueError('Unknown data type: ' + i)

        return rmd

    def pack(self, state):

        l = state.pack(self.names, self.types)

        return struct.pack(self.fmt, *l)

    def unpack(self, data):

        li =  struct.unpack_from(self.fmt, data)

        return RTDEDataObject.unpack(li, self.names, self.types)

class RTDEDataObject(object):

    '''

    Data container for data send to or received from the Robot Controller.

    The Object will have attributes for each of that data tags received or send.

    e.g.  obj.actual_digital_output_bits

    '''

    recipe_id = None

    def pack(self, names, types):

        if len(names) != len(types):

            raise ValueError('List sizes are not identical.')

        l = []

        if(self.recipe_id is not None):

            l.append(self.recipe_id)

        for i in range(len(names)):

            if self.__dict__[names[i]] is None:

                raise ValueError('Uninitialized parameter: ' + names[i])

            if types[i].startswith('VECTOR'):

                l.extend(self.__dict__[names[i]])

            else:

                l.append(self.__dict__[names[i]])

        return l

    @staticmethod

    def unpack(data, names, types):

        if len(names) != len(types):

            raise ValueError('List sizes are not identical.')

        obj = dict()

        offset = 0

        for i in range(len(names)):

            obj[names[i]] = RTDEDataObject.unpack_field(data, offset, types[i])

            offset += RTDEDataObject.get_item_size(types[i])

        return obj

    @staticmethod

    def create_empty(names, recipe_id):

        obj = RTDEDataObject()

        for i in range(len(names)):

            obj.__dict__[names[i]] = None

        obj.recipe_id = recipe_id

        return obj

    @staticmethod

    def get_item_size(data_type):

        if data_type.startswith('VECTOR6'):

            return 6

        elif data_type.startswith('VECTOR3'):

            return 3

        return 1

    @staticmethod

    def unpack_field(data, offset, data_type):

        size = RTDEDataObject.get_item_size(data_type)

        if(data_type == 'VECTOR6D' or

           data_type == 'VECTOR3D'):

            return np.array([float(data[offset+i]) for i in range(size)])

        elif(data_type == 'VECTOR6UINT32'):

            return np.array([int(data[offset+i]) for i in range(size)])

        elif(data_type == 'DOUBLE'):

            return float(data[offset])

        elif(data_type == 'UINT32' or

             data_type == 'UINT64'):

            return int(data[offset])

        elif(data_type == 'VECTOR6INT32'):

            return np.array([int(data[offset+i]) for i in range(size)])

        elif(data_type == 'INT32' or

             data_type == 'UINT8'):

            return int(data[offset])

        raise ValueError('unpack_field: unknown data type: ' + data_type)