library ieee;
use ieee.std_logic_1164.all;
use ieee.numeric_std.all;

use work.math_pkg.all;
use work.rtps_package.all;
use work.user_config.all;

-- TODO: Convert ALL integers to natural, and remove all conversions (Arithmetics between natural and unsigned are directly supported)

package rtps_config_package is
    
    constant VENDORID   : std_logic_vector(VENDORID_WIDTH-1 downto 0) := VENDORID_UNKNOWN;
    constant GUIDPREFIX : GUIDPREFIX_TYPE; -- Deferred to Package Body
    type ENTITYID_TYPE is array (0 to NUM_ENDPOINTS-1) of std_logic_vector(ENTITYID_WIDTH-1 downto 0);
    constant ENTITYID   : ENTITYID_TYPE; -- Deferred to Package Body
    constant DOMAIN_ID  : std_logic_vector(DOMAIN_ID_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(USER_DOMAIN_ID, DOMAIN_ID_WIDTH));
    
    -- Smallest Writer Endpoint Lease Duration
    constant MIN_ENDPOINT_LEASE_DURATION : DURATION_TYPE; -- Deferred to package Body
    
    -- (see DDSI-RTPS 2.3 Section 9.6.1)
    -- PB + DG * domain_id + d0
    constant META_IPv4_MULTICAST_PORT: std_logic_vector(UDP_PORT_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(PORT_CONFIG_PB + PORT_CONFIG_D0 + PORT_CONFIG_DG*USER_DOMAIN_ID, UDP_PORT_WIDTH));
    -- (see DDSI-RTPS 2.3 Section 9.6.1)
    -- PB + DG * domainId + d1 + PG * participant_id
    -- participant_id=0
    constant META_IPv4_UNICAST_PORT  : std_logic_vector(UDP_PORT_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(PORT_CONFIG_PB + PORT_CONFIG_D1 + PORT_CONFIG_DG*USER_DOMAIN_ID, UDP_PORT_WIDTH));
    -- (see DDSI-RTPS 2.3 Section 9.6.1)
    -- PB + DG * domainId + d2
    constant USER_IPv4_MULTICAST_PORT: std_logic_vector(UDP_PORT_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(PORT_CONFIG_PB + PORT_CONFIG_D2 + PORT_CONFIG_DG*USER_DOMAIN_ID, UDP_PORT_WIDTH));
    -- (see DDSI-RTPS 2.3 Section 9.6.1)
    -- PB + DG * domainId + d3 + PG * participant_id
    -- participant_id=0
    constant USER_IPv4_UNICAST_PORT  : std_logic_vector(UDP_PORT_WIDTH-1 downto 0) := std_logic_vector(to_unsigned(PORT_CONFIG_PB + PORT_CONFIG_D3 + PORT_CONFIG_DG*USER_DOMAIN_ID, UDP_PORT_WIDTH));
    
    -- ENDPOINT FRAME OPCODES
    constant ENDPOINT_MATCH_OPCODE_WIDTH: natural := 32;
    constant OPCODE_ENDPOINT_MATCH      : std_logic_vector(ENDPOINT_MATCH_OPCODE_WIDTH-1 downto 0) := x"55000000";
    constant OPCODE_ENDPOINT_UNMATCH    : std_logic_vector(ENDPOINT_MATCH_OPCODE_WIDTH-1 downto 0) := x"55000001";
    constant OPCODE_PARTICIPANT_UNMATCH : std_logic_vector(ENDPOINT_MATCH_OPCODE_WIDTH-1 downto 0) := x"55000002";
    constant OPCODE_LIVELINESS_UPDATE   : std_logic_vector(ENDPOINT_MATCH_OPCODE_WIDTH-1 downto 0) := x"55000003";
    
    type HISTORY_CACHE_OPCODE_TYPE is (NOP, ADD_CACHE_CHANGE, GET_CACHE_CHANGE, ACK_CACHE_CHANGE, NACK_CACHE_CHANGE, REMOVE_CACHE_CHANGE, REMOVE_WRITER, GET_MIN_SN, GET_MAX_SN);
    type KEY_HOLDER_OPCODE_TYPE is (NOP, PUSH_DATA, PUSH_SERIALIZED_KEY, READ_KEY_HASH, READ_SERIALIZED_KEY);
    type KEY_GENERATOR_OPCODE_TYPE is (NOP, WRITE_PAYLOAD, READ_KEY, READ_SIZE);
    type HISTORY_CACHE_RESPONSE_TYPE is (OK, REJECTED, INVALID, ERROR);
    type DDS_WRITER_OPCODE_TYPE is (NOP, REGISTER_INSTANCE, WRITE, DISPOSE, UNREGISTER_INSTANCE, LOOKUP_INSTANCE, WAIT_FOR_ACKNOWLEDGEMENTS, GET_OFFERED_DEADLINE_MISSED_STATUS, ASSERT_LIVELINESS, GET_LIVELINESS_LOST_STATUS);
    type DDS_READER_OPCODE_TYPE is (NOP, READ, TAKE, READ_NEXT_SAMPLE, TAKE_NEXT_SAMPLE, READ_INSTANCE, TAKE_INSTANCE, READ_NEXT_INSTANCE, TAKE_NEXT_INSTANCE, GET_SAMPLE_REJECTED_STATUS, GET_REQUESTED_DEADLINE_MISSED_STATUS);
    
    -- Sample Status Info Flags
    constant SSI_DISPOSED_FLAG      : natural := STATUS_INFO_DISPOSED_FLAG;
    constant SSI_UNREGISTERED_FLAG  : natural := STATUS_INFO_UNREGISTERED_FLAG;
    constant SSI_FILTERED_FLAG      : natural := STATUS_INFO_FILTERED_FLAG;
    constant SSI_KEY_HASH_FLAG      : natural := 28; -- Reader Only
    constant SSI_ALIGNED_FLAG       : natural := 29;
    constant SSI_PAYLOAD_FLAG       : natural := 30;
    constant SSI_READ_FLAG          : natural := 31; -- Reader Only
    constant SSI_ACK_FLAG           : natural := 31; -- Writer Only
    
    -- Instance Status Info Flags
    constant ISI_NOT_ALIVE_DISPOSED_FLAG    : natural := 0; -- Reader Only
    constant ISI_DISPOSED_FLAG              : natural := 0; -- Writer Only
    constant ISI_NOT_ALIVE_NO_WRITERS_FLAG  : natural := 1; -- Reader Only
    constant ISI_UNREGISTERED_FLAG          : natural := 1; -- Writer Only
    constant ISI_LIVELINESS_FLAG            : natural := 2;
    constant ISI_VIEW_FLAG                  : natural := 3; -- Reader Only
    constant ISI_MARK_FLAG                  : natural := 4; -- Reader Only
    
    -- Remote Endpoint Flags
    constant READER_FLAGS_WIDTH                     : natural := 16;
    constant READER_EXPECTS_INLINE_QOS_FLAG         : natural := 0;
    constant READER_EXPECTS_HISTORICAL_DATA_FLAG    : natural := 1;
    constant READER_IS_BEST_EFFORT_FLAG             : natural := 2;
    constant DEFAULT_EXPECTS_HISTORICAL_DATA_FLAG   : std_logic := '0';
    constant DEFAULT_IS_BEST_EFFORT_FLAG            : std_logic := '0';
    
    -- Marks the Reader Endpoint in the Endpoint Array
    constant ENDPOINT_READERS           : std_logic_vector(0 to NUM_ENDPOINTS-1) := (0 to NUM_READERS-1 => '1', others => '0');
    
    -- Marks the writers with AUTOMATIC Liveliness Qos
    constant AUTOMATIC_LIVELINESS_WRITERS               : std_logic_vector(0 to NUM_ENDPOINTS-1); --Deferred to package body
    -- Marks the writers with MANUAL BY PARTICIPANT Liveliness Qos
    constant MANUAL_BY_PARTICIPANT_LIVELINESS_WRITERS   : std_logic_vector(0 to NUM_ENDPOINTS-1); --Deferred to package body
    -- Marks the readers with AUTOMATIC Liveliness Qos
    constant AUTOMATIC_LIVELINESS_READERS               : std_logic_vector(0 to NUM_ENDPOINTS-1); --Deferred to package body
    -- Marks the readers with MANUAL BY PARTICIPANT Liveliness Qos
    constant MANUAL_BY_PARTICIPANT_LIVELINESS_READERS   : std_logic_vector(0 to NUM_ENDPOINTS-1); --Deferred to package body
    
    type WORD_ARRAY_TYPE is array (natural range <>) of std_logic_vector(WORD_WIDTH-1 downto 0);
    type OUTPUT_DATA_TYPE is record
        -- Limit DATA to MAX UDPv4 Payload Size - RTPS Header (65487 Bytes)
        data    : WORD_ARRAY_TYPE(0 to 16371);
        length  : natural;
    end record;
    constant READER_ENDPOINT_DATA   : OUTPUT_DATA_TYPE; --Deferred to package body
    constant WRITER_ENDPOINT_DATA   : OUTPUT_DATA_TYPE; --Deferred to package body
    constant PARTICIPANT_DATA       : OUTPUT_DATA_TYPE; --Deferred to package body
    
    type STRING_WORD_ARRAY_TYPE is array (0 to (256/(WORD_WIDTH/8))-1) of std_logic_vector(WORD_WIDTH-1 downto 0);
    type ENDPOINT_STRING_TYPE is array (0 to NUM_ENDPOINTS-1) of STRING_WORD_ARRAY_TYPE;
    constant ENDPOINT_TOPIC     : ENDPOINT_STRING_TYPE; --Deferred to package body
    constant ENDPOINT_TYPE      : ENDPOINT_STRING_TYPE; --Deferred to package body
    constant DOMAIN_TAG         : STRING_WORD_ARRAY_TYPE; -- Deferred to package body
    constant DEFAULT_DOMAIN_TAG : STRING_WORD_ARRAY_TYPE; -- Deferred to package body
    -- TODO: Use everywhere
    constant EMPTY_STRING   : STRING_WORD_ARRAY_TYPE := (others => (others => '0'));
    
    type DEFAULT_QOS_MATCH_TYPE is record
        DURABILITY_QOS          : std_logic_vector(0 to NUM_ENDPOINTS-1);
        PRESENTATION_QOS        : std_logic_vector(0 to NUM_ENDPOINTS-1);
        DEADLINE_QOS            : std_logic_vector(0 to NUM_ENDPOINTS-1);
        LATENCY_BUDGET_QOS      : std_logic_vector(0 to NUM_ENDPOINTS-1);
        OWNERSHIP_QOS           : std_logic_vector(0 to NUM_ENDPOINTS-1);
        LIVELINESS_QOS          : std_logic_vector(0 to NUM_ENDPOINTS-1);
        RELIABILITY_QOS         : std_logic_vector(0 to NUM_ENDPOINTS-1);
        DESTINATION_ORDER_QOS   : std_logic_vector(0 to NUM_ENDPOINTS-1);
    end record;
    constant READER_DEFAULT_QOS_MATCH : DEFAULT_QOS_MATCH_TYPE; -- Deferred to Package Body
    constant WRITER_DEFAULT_QOS_MATCH : DEFAULT_QOS_MATCH_TYPE; -- Deferred to Package Body
    
    type RTPS_OUT_DATA_TYPE is array (0 to NUM_ENDPOINTS) of std_logic_vector(WORD_WIDTH-1 downto 0);
    
    -- Swap "data" to Big Endian representation.
    function endian_swap(swap : std_logic; data : std_logic_vector) return std_logic_vector;
    function endian_swap(swap : std_logic; data : unsigned) return unsigned;
    -- Return the Byte length of the string. (First NUL Byte is included)
    function string_len (str : STRING_WORD_ARRAY_TYPE) return natural;
    
    function boolean_to_std_logic(input : boolean) return std_logic;
    function convert_string (str : USER_STRING_TYPE) return STRING_WORD_ARRAY_TYPE;
    
    function round_slv(slv : std_logic_vector; width : natural) return std_logic_vector;
    
    function check_qos_compatibility(src_is_reader : std_logic; direction : std_logic; remote : unsigned(WORD_WIDTH-1 downto 0); local : unsigned(WORD_WIDTH-1 downto 0)) return boolean;
    function check_qos_compatibility(src_is_reader : std_logic; direction : std_logic; remote : DOUBLE_WORD_ARRAY; local : DOUBLE_WORD_ARRAY) return boolean;
    
    function check_mask(flags : std_logic_vector; mask : std_logic_vector) return boolean;
    
    function gen_inline_qos (id : natural) return OUTPUT_DATA_TYPE;
    
end package;

package body rtps_config_package is
    
    procedure assertions is
    begin
        assert (NUM_ENDPOINTS = (NUM_READERS+NUM_WRITERS)) severity failure;
        
        assert (ENDPOINT_WITH_KEY'length = NUM_ENDPOINTS) report "Endpoint arrays have to be NUM_ENDPOINTS long" severity failure;
        assert (ENDPOINT_TOPIC_STRING'length = NUM_ENDPOINTS) report "Endpoint arrays have to be NUM_ENDPOINTS long" severity failure;
        assert (ENDPOINT_TYPE_STRING'length = NUM_ENDPOINTS) report "Endpoint arrays have to be NUM_ENDPOINTS long" severity failure;
        assert (ENDPOINT_HEARTBEAT_PERIOD'length = NUM_ENDPOINTS) report "Endpoint arrays have to be NUM_ENDPOINTS long" severity failure;
        assert (ENDPOINT_HEARTBEAT_RESPONSE_DELAY'length = NUM_ENDPOINTS) report "Endpoint arrays have to be NUM_ENDPOINTS long" severity failure;
        assert (ENDPOINT_HEARTBEAT_SUPPRESSION_DELAY'length = NUM_ENDPOINTS) report "Endpoint arrays have to be NUM_ENDPOINTS long" severity failure;
        assert (ENDPOINT_ACKNACK_RESPONSE_DELAY'length = NUM_ENDPOINTS) report "Endpoint arrays have to be NUM_ENDPOINTS long" severity failure;
        assert (ENDPOINT_ACKNACK_SUPPRESSION_DELAY'length = NUM_ENDPOINTS) report "Endpoint arrays have to be NUM_ENDPOINTS long" severity failure;
        assert (ENDPOINT_DURABILITY_QOS'length = NUM_ENDPOINTS) report "Endpoint arrays have to be NUM_ENDPOINTS long" severity failure;
        assert (ENDPOINT_DURABILITY_SERVICE_CLEANUP_DELAY'length = NUM_ENDPOINTS) report "Endpoint arrays have to be NUM_ENDPOINTS long" severity failure;
        assert (ENDPOINT_DURABILITY_SERVICE_HISTORY'length = NUM_ENDPOINTS) report "Endpoint arrays have to be NUM_ENDPOINTS long" severity failure;
        assert (ENDPOINT_DURABILITY_SERVICE_HISTORY_DEPTH'length = NUM_ENDPOINTS) report "Endpoint arrays have to be NUM_ENDPOINTS long" severity failure;
        assert (ENDPOINT_DURABILITY_SERVICE_MAX_SAMPLES'length = NUM_ENDPOINTS) report "Endpoint arrays have to be NUM_ENDPOINTS long" severity failure;
        assert (ENDPOINT_DURABILITY_SERVICE_MAX_INSTANCES'length = NUM_ENDPOINTS) report "Endpoint arrays have to be NUM_ENDPOINTS long" severity failure;
        assert (ENDPOINT_DURABILITY_SERVICE_MAX_SAMPLES_PER_INSTANCE'length = NUM_ENDPOINTS) report "Endpoint arrays have to be NUM_ENDPOINTS long" severity failure;
        assert (ENDPOINT_PRESENTATION_QOS'length = NUM_ENDPOINTS) report "Endpoint arrays have to be NUM_ENDPOINTS long" severity failure;
        assert (ENDPOINT_COHERENT_ACCESS'length = NUM_ENDPOINTS) report "Endpoint arrays have to be NUM_ENDPOINTS long" severity failure;
        assert (ENDPOINT_ORDERED_ACCESS'length = NUM_ENDPOINTS) report "Endpoint arrays have to be NUM_ENDPOINTS long" severity failure;
        assert (ENDPOINT_DEADLINE_QOS'length = NUM_ENDPOINTS) report "Endpoint arrays have to be NUM_ENDPOINTS long" severity failure;
        assert (ENDPOINT_LATENCY_BUDGET_QOS'length = NUM_ENDPOINTS) report "Endpoint arrays have to be NUM_ENDPOINTS long" severity failure;
        assert (ENDPOINT_OWNERSHIP_QOS'length = NUM_ENDPOINTS) report "Endpoint arrays have to be NUM_ENDPOINTS long" severity failure;
        assert (ENDPOINT_OWNERSHIP_STRENGTH_QOS'length = NUM_ENDPOINTS) report "Endpoint arrays have to be NUM_ENDPOINTS long" severity failure;
        assert (ENDPOINT_LIVELINESS_QOS'length = NUM_ENDPOINTS) report "Endpoint arrays have to be NUM_ENDPOINTS long" severity failure;
        assert (ENDPOINT_LEASE_DURATION'length = NUM_ENDPOINTS) report "Endpoint arrays have to be NUM_ENDPOINTS long" severity failure;
        assert (ENDPOINT_TIME_BASED_FILTER_QOS'length = NUM_ENDPOINTS) report "Endpoint arrays have to be NUM_ENDPOINTS long" severity failure;
        assert (ENDPOINT_RELIABILITY_QOS'length = NUM_ENDPOINTS) report "Endpoint arrays have to be NUM_ENDPOINTS long" severity failure;
        assert (ENDPOINT_MAX_BLOCKING_TIME'length = NUM_ENDPOINTS) report "Endpoint arrays have to be NUM_ENDPOINTS long" severity failure;
        assert (ENDPOINT_TRANSPORT_PRIORITY_QOS'length = NUM_ENDPOINTS) report "Endpoint arrays have to be NUM_ENDPOINTS long" severity failure;
        assert (ENDPOINT_LIFESPAN_QOS'length = NUM_ENDPOINTS) report "Endpoint arrays have to be NUM_ENDPOINTS long" severity failure;
        assert (ENDPOINT_DESTINATION_ORDER_QOS'length = NUM_ENDPOINTS) report "Endpoint arrays have to be NUM_ENDPOINTS long" severity failure;
        assert (ENDPOINT_HISTORY_QOS'length = NUM_ENDPOINTS) report "Endpoint arrays have to be NUM_ENDPOINTS long" severity failure;
        assert (ENDPOINT_HISTORY_DEPTH'length = NUM_ENDPOINTS) report "Endpoint arrays have to be NUM_ENDPOINTS long" severity failure;
        assert (ENDPOINT_MAX_SAMPLES'length = NUM_ENDPOINTS) report "Endpoint arrays have to be NUM_ENDPOINTS long" severity failure;
        assert (ENDPOINT_MAX_INSTANCES'length = NUM_ENDPOINTS) report "Endpoint arrays have to be NUM_ENDPOINTS long" severity failure;
        assert (ENDPOINT_MAX_SAMPLES_PER_INSTANCE'length = NUM_ENDPOINTS) report "Endpoint arrays have to be NUM_ENDPOINTS long" severity failure;
        assert (ENDPOINT_AUTODISPOSE_UNREGISTERED_INSTANCES'length = NUM_ENDPOINTS) report "Endpoint arrays have to be NUM_ENDPOINTS long" severity failure;
        assert (ENDPOINT_AUTOPURGE_NOWRITER_SAMPLES_DELAY'length = NUM_ENDPOINTS) report "Endpoint arrays have to be NUM_ENDPOINTS long" severity failure;
        assert (ENDPOINT_AUTOPURGE_DISPOSED_SAMPLES_DELAY'length = NUM_ENDPOINTS) report "Endpoint arrays have to be NUM_ENDPOINTS long" severity failure;
        
        assert (PARTICIPANT_ANNOUNCEMENT_PERIOD+DURATION_DELTA <= PARTICIPANT_LEASE_DURATION) report "Participant Announcement Period has to be less than the Participant Lease Duration" severity failure;
        
        for i in 0 to NUM_ENDPOINTS-1 loop
            assert (unsigned(ENDPOINT_DURABILITY_QOS(i)) < unsigned(TRANSIENT_DURABILITY_QOS)) report "TRANSIENT and PERSISTENT Durability QoS not supported" severity failure;
            assert (ENDPOINT_PRESENTATION_QOS(i) /= GROUP_PRESENTATION_QOS) report "GROUP Presentation QoS not supported" severity failure;
            assert (ENDPOINT_DEADLINE_QOS(i) >= ENDPOINT_TIME_BASED_FILTER_QOS(i)) report "DEADLINE Qos cannot be less than TIME_BASED_FILTER QoS" severity failure;
            assert (ENDPOINT_OWNERSHIP_QOS(i) = SHARED_OWNERSHIP_QOS) report "Only SHARED Ownership QoS supported" severity failure;
            assert (unsigned(ENDPOINT_MAX_SAMPLES(i)) >= unsigned(ENDPOINT_MAX_SAMPLES_PER_INSTANCE(i))) report "MAX_SAMPLES cannot be less than MAX_SAMPLES_PER_INSTANCE in Resource Limits QoS" severity failure;
        end loop;
    end procedure;
    
    function boolean_to_std_logic(input : boolean) return std_logic is
        variable ret : std_logic := '0';
    begin
        ret := '0';
        if (input = TRUE) then
            ret := '1';
        end if;
        return ret;
    end function;
    
    function gen_entyid return ENTITYID_TYPE is
        variable ret : ENTITYID_TYPE;
    begin
        ret := (others => (others => '0'));
        for i in 0 to ret'length-1 loop
            -- (see DDSI-RTPS 2.3 Section 9.3.1.2)
            -- Entity Kind Mapping 
            ret(i)(ENTITY_KIND_H_RANGE) := USER_DEFINED_ENTITY;
            if (i <= NUM_READERS-1) then
                if (ENDPOINT_WITH_KEY(i)) then
                    ret(i)(ENTITY_KIND_L_RANGE)  := READER_WITH_KEY;
                else
                    ret(i)(ENTITY_KIND_L_RANGE)  := READER_NO_KEY;
                end if;
            else
                if (ENDPOINT_WITH_KEY(i)) then
                    ret(i)(ENTITY_KIND_L_RANGE)  := WRITER_WITH_KEY;
                else
                    ret(i)(ENTITY_KIND_L_RANGE)  := WRITER_NO_KEY;
                end if;
            end if;
            -- ID Mapping
            ret(i)(ENTITYID_WIDTH-1 downto 8)  := std_logic_vector(to_unsigned(i,ENTITYID_WIDTH-8));
        end loop;
        return ret;
    end function;
    
    constant ENTITYID : ENTITYID_TYPE := gen_entyid;
    
    function gen_guidprefix return GUIDPREFIX_TYPE is
        variable tmp : std_logic_vector(GUIDPREFIX_WIDTH-1 downto 0) := (others => '0');
        variable ret : GUIDPREFIX_TYPE := (others => (others => '0'));
    begin
        tmp := (others => '0');
        ret := (others => (others => '0'));
        -- First two bytes have to be Vendor ID (see DDSI-RTPS 2.3 Section 9.3.1.5)
        tmp(GUIDPREFIX_WIDTH-1 downto GUIDPREFIX_WIDTH-VENDORID_WIDTH)  := VENDORID;
        -- Next we insert the MAC address for uniqueness
        tmp(GUIDPREFIX_WIDTH-VENDORID_WIDTH-1 downto GUIDPREFIX_WIDTH-VENDORID_WIDTH-48)    := MAC_ADDRESS;
        -- Next we insert the Domain ID
        tmp(DOMAIN_ID_WIDTH-1 downto 0) := DOMAIN_ID;
        
        -- Convert to return type
        for i in 0 to ret'length-1 loop
			ret(i)  := tmp(GUIDPREFIX_WIDTH-(i*ret(i)'length)-1 downto GUIDPREFIX_WIDTH-(i*ret(i)'length)-ret(i)'length);
        end loop;
        return ret;
    end function;
    
    constant GUIDPREFIX : GUIDPREFIX_TYPE := gen_guidprefix;
    
    function find_min_lease_duration return DURATION_TYPE is
        variable ret : DURATION_TYPE := (others => (others => '0'));
    begin
        ret := DURATION_INFINITE;
        -- Sanity Check
        if (NUM_WRITERS = 0) then
            return ret;
        end if;
        
        -- Iterate through writers
        for i in NUM_READERS to NUM_ENDPOINTS-1 loop
            -- Do not consider "MANUAL_BY_TOPIC" Liveliness
            if (ENDPOINT_LIVELINESS_QOS(i) /= MANUAL_BY_TOPIC_LIVELINESS_QOS) then
                -- Find Minimum Lease Duration
                if (ENDPOINT_LEASE_DURATION(i) < ret) then
                    ret := ENDPOINT_LEASE_DURATION(i);
                end if;
            end if;
        end loop;
        return ret;
    end function;
    
    constant MIN_ENDPOINT_LEASE_DURATION : DURATION_TYPE := find_min_lease_duration;
    
    function convert_string (str : USER_STRING_TYPE) return STRING_WORD_ARRAY_TYPE is
        variable ret : STRING_WORD_ARRAY_TYPE := (others => (others => '0'));
    begin
        ret := (others => (others => '0'));
        for i in 0 to ret'length-1 loop
            ret(i) := std_logic_vector(to_unsigned(character'POS(str((i*4)+1)), 8)) & std_logic_vector(to_unsigned(character'POS(str((i*4)+2)), 8)) & std_logic_vector(to_unsigned(character'POS(str((i*4)+3)), 8)) & std_logic_vector(to_unsigned(character'POS(str((i*4)+4)), 8));
        end loop;
        return ret;
    end function;
    
    function convert_endpoint_string (string_array : USER_STRING_ARRAY_TYPE) return ENDPOINT_STRING_TYPE is
        variable ret : ENDPOINT_STRING_TYPE := (others => (others => (others => '0')));
    begin
        ret := (others => (others => (others => '0')));
        for i in 0 to ret'length-1 loop
            ret(i) := convert_string(string_array(i));
        end loop;
        return ret;
    end function;
    
    constant ENDPOINT_TOPIC     : ENDPOINT_STRING_TYPE := convert_endpoint_string(ENDPOINT_TOPIC_STRING);
    constant ENDPOINT_TYPE      : ENDPOINT_STRING_TYPE := convert_endpoint_string(ENDPOINT_TYPE_STRING);
    constant DOMAIN_TAG         : STRING_WORD_ARRAY_TYPE := convert_string(USER_DOMAIN_TAG);
    constant DEFAULT_DOMAIN_TAG : STRING_WORD_ARRAY_TYPE := convert_string(DEFAULT_USER_DOMAIN_TAG);
    
    function string_len (str : STRING_WORD_ARRAY_TYPE) return natural is
        variable ret    : natural := 0;
        variable done   : boolean := FALSE;
    begin
        ret     := 0;
        done    := FALSE;
        for i in 0 to str'length-1 loop
            for j in 0 to (WORD_WIDTH/BYTE_WIDTH)-1 loop
                -- Count Bytes
                ret := ret + 1;
                -- Exit on first NULL byte (NULL Byte included in count)
                if (str(i)(WORD_WIDTH-(j*BYTE_WIDTH)-1 downto WORD_WIDTH-(j*BYTE_WIDTH)-BYTE_WIDTH) = (BYTE_WIDTH-1 downto 0 => '0')) then
                    done := TRUE;
                    exit;
                end if;
            end loop;
            if (done) then
                exit;
            end if;
        end loop;
        return ret;
    end function;
    
    function gen_reader_endpoint_data return OUTPUT_DATA_TYPE is
        variable ret    : OUTPUT_DATA_TYPE := (data => (others => (others => '0')), length => 0);
        variable ind    : natural := 0;
        variable len    : natural := 0;
        variable tmp    : natural := 0;
    begin
        ret.data    := (others => (others => '0'));
        ret.length  := 0;
        -- Sanity Check
        if (NUM_READERS = 0) then
            return ret;
        end if;
        len         := 0;
        ind         := 0;
        
        -- RTPS Submessages
        -- One DATA Submessage for each Reader
        for i in 0 to NUM_READERS-1 loop
            -- RTPS Submessage Header
            ret.data(ind)       := SID_DATA & "00000100" & x"0000";
            -- DATA Header (extraFlags, octetsToInlineQoS)
            len := len + 1;
            ret.data(ind+len)   := x"0000" & std_logic_vector(to_unsigned(16, 16));
            -- DATA Header (Reader Entity ID)
            len := len + 1;
            ret.data(ind+len)   := ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_DETECTOR;
            -- DATA Header (Writer Entity ID)
            len := len + 1;
            ret.data(ind+len)   := ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_ANNOUNCER;
            -- DATA Header (Sequence Number)
            len := len + 1;
            ret.data(ind+len)   := (others => '0');
            -- DATA Header (Sequence Number)
            len := len + 1;
            ret.data(ind+len)   := std_logic_vector(to_unsigned(i+1, ret.data(0)'length));
            -- Serialized Payload Header
            len := len + 1;
            ret.data(ind+len)   := PL_CDR_BE & x"0000";
            -- Serialized Payload BEGIN
            -- GUID
            len := len + 1;
            ret.data(ind+len):= PID_ENDPOINT_GUID & std_logic_vector(to_unsigned(16, 16));
            len := len + 1;
            ret.data(ind+len)   := GUIDPREFIX(0);
            len := len + 1;
            ret.data(ind+len)   := GUIDPREFIX(1);
            len := len + 1;
            ret.data(ind+len)   := GUIDPREFIX(2);
            len := len + 1;
            ret.data(ind+len)   := ENTITYID(i);
            -- EXPECTS INLINE QOS
            len := len + 1;
            ret.data(ind+len)   := PID_EXPECTS_INLINE_QOS & std_logic_vector(to_unsigned(4, 16));
            len := len + 1;
            ret.data(ind+len)   := (24 => '1', others => '0');
            -- TOPIC NAME
            tmp := string_len(ENDPOINT_TOPIC(i));
            len := len + 1;
            ret.data(ind+len)   := PID_TOPIC_NAME & std_logic_vector(to_unsigned((round_div(tmp,4)+1)*4, 16));
            len := len + 1;
            ret.data(ind+len)   := std_logic_vector(to_unsigned(tmp, 32));
            for j in 0 to round_div(tmp,4)-1 loop
                len := len + 1;
                ret.data(ind+len)   := ENDPOINT_TOPIC(i)(j);
            end loop;
            -- TYPE NAME
            tmp := string_len(ENDPOINT_TYPE(i));
            len := len + 1;
            ret.data(ind+len)   := PID_TYPE_NAME & std_logic_vector(to_unsigned((round_div(tmp,4)+1)*4, 16));
            len := len + 1;
            ret.data(ind+len)   := std_logic_vector(to_unsigned(tmp, 32));
            for j in 0 to round_div(tmp,4)-1 loop
                len := len + 1;
                ret.data(ind+len)   := ENDPOINT_TYPE(i)(j);
            end loop;
            -- DURABILITY
            if (ENDPOINT_DURABILITY_QOS(i) /= DEFAULT_DURABILITY_QOS) then
                len := len + 1;
                ret.data(ind+len)   := PID_DURABILITY & std_logic_vector(to_unsigned(4, 16));
                len := len + 1;
                ret.data(ind+len)   := ENDPOINT_DURABILITY_QOS(i);
            end if;
            -- DURABILITY SERVICE
            if (ENDPOINT_DURABILITY_SERVICE_CLEANUP_DELAY(i) /= DEFAULT_DURABILITY_SERVICE_CLEANUP_DELAY or ENDPOINT_DURABILITY_SERVICE_HISTORY(i) /= DEFAULT_DURABILITY_SERVICE_HISTORY or 
                ENDPOINT_DURABILITY_SERVICE_HISTORY_DEPTH(i) /= DEFAULT_DURABILITY_SERVICE_HISTORY_DEPTH or ENDPOINT_DURABILITY_SERVICE_MAX_SAMPLES(i) /= DEFAULT_DURABILITY_SERVICE_MAX_SAMPLES or 
                ENDPOINT_DURABILITY_SERVICE_MAX_INSTANCES(i) /= DEFAULT_DURABILITY_SERVICE_MAX_INSTANCES or ENDPOINT_DURABILITY_SERVICE_MAX_SAMPLES_PER_INSTANCE(i) /= DEFAULT_DURABILITY_SERVICE_MAX_SAMPLES_PER_INSTANCE
            ) then
                len := len + 1;
                ret.data(ind+len)   := PID_DURABILITY_SERVICE & std_logic_vector(to_unsigned(28, 16));
                len := len + 1;
                ret.data(ind+len)   := std_logic_vector(ENDPOINT_DURABILITY_SERVICE_CLEANUP_DELAY(i)(0));
                len := len + 1;
                ret.data(ind+len)   := std_logic_vector(ENDPOINT_DURABILITY_SERVICE_CLEANUP_DELAY(i)(1));
                len := len + 1;
                ret.data(ind+len)   := ENDPOINT_DURABILITY_SERVICE_HISTORY(i);
                len := len + 1;
                ret.data(ind+len)   := ENDPOINT_DURABILITY_SERVICE_HISTORY_DEPTH(i);
                len := len + 1;
                ret.data(ind+len)   := ENDPOINT_DURABILITY_SERVICE_MAX_SAMPLES(i);
                len := len + 1;
                ret.data(ind+len)   := ENDPOINT_DURABILITY_SERVICE_MAX_INSTANCES(i);
                len := len + 1;
                ret.data(ind+len)   := ENDPOINT_DURABILITY_SERVICE_MAX_SAMPLES_PER_INSTANCE(i);
            end if;
            -- PRESENTATION
            if (ENDPOINT_PRESENTATION_QOS(i) /= DEFAULT_PRESENTATION_QOS or ENDPOINT_COHERENT_ACCESS(i) /= DEFAULT_COHERENT_ACCESS or ENDPOINT_ORDERED_ACCESS(i) /= DEFAULT_ORDERED_ACCESS) then
                len := len + 1;
                ret.data(ind+len)   := PID_PRESENTATION & std_logic_vector(to_unsigned(8, 16));
                len := len + 1;
                ret.data(ind+len)   := ENDPOINT_PRESENTATION_QOS(i);
                len := len + 1;
                ret.data(ind+len)   := (24 => boolean_to_std_logic(ENDPOINT_COHERENT_ACCESS(i)), 16 => boolean_to_std_logic(ENDPOINT_ORDERED_ACCESS(i)), others => '0');
            end if;
            -- DEADLINE
            if (ENDPOINT_DEADLINE_QOS(i) /= DEFAULT_DEADLINE_QOS) then
                len := len + 1;
                ret.data(ind+len)   := PID_DEADLINE & std_logic_vector(to_unsigned(8, 16));
                len := len + 1;
                ret.data(ind+len)   := std_logic_vector(ENDPOINT_DEADLINE_QOS(i)(0));
                len := len + 1;
                ret.data(ind+len)   := std_logic_vector(ENDPOINT_DEADLINE_QOS(i)(1));
            end if;
            -- LATENCY_BUDGET
            if (ENDPOINT_LATENCY_BUDGET_QOS(i) /= DEFAULT_LATENCY_BUDGET_QOS) then
                len := len + 1;
                ret.data(ind+len)   := PID_LATENCY_BUDGET & std_logic_vector(to_unsigned(8, 16));
                len := len + 1;
                ret.data(ind+len)   := std_logic_vector(ENDPOINT_LATENCY_BUDGET_QOS(i)(0));
                len := len + 1;
                ret.data(ind+len)   := std_logic_vector(ENDPOINT_LATENCY_BUDGET_QOS(i)(1));
            end if;
            -- OWNERSHIP
            if (ENDPOINT_OWNERSHIP_QOS(i) /= DEFAULT_OWNERSHIP_QOS) then
                len := len + 1;
                ret.data(ind+len)   := PID_OWNERSHIP & std_logic_vector(to_unsigned(4, 16));
                len := len + 1;
                ret.data(ind+len)   := ENDPOINT_OWNERSHIP_QOS(i);
            end if;
            -- LIVELINESS
            if (ENDPOINT_LIVELINESS_QOS(i) /= DEFAULT_LIVELINESS_QOS or ENDPOINT_LEASE_DURATION(i) /= DEFAULT_LEASE_DURATION) then
                len := len + 1;
                ret.data(ind+len)   := PID_LIVELINESS & std_logic_vector(to_unsigned(12, 16));
                len := len + 1;
                ret.data(ind+len)   := ENDPOINT_LIVELINESS_QOS(i);
                len := len + 1;
                ret.data(ind+len)   := std_logic_vector(ENDPOINT_LEASE_DURATION(i)(0));
                len := len + 1;
                ret.data(ind+len)   := std_logic_vector(ENDPOINT_LEASE_DURATION(i)(1));
            end if;
            -- TIME BASED FILTER
            if (ENDPOINT_TIME_BASED_FILTER_QOS(i) /= DEFAULT_LATENCY_BUDGET_QOS) then
                len := len + 1;
                ret.data(ind+len)   := PID_TIME_BASED_FILTER & std_logic_vector(to_unsigned(8, 16));
                len := len + 1;
                ret.data(ind+len)   := std_logic_vector(ENDPOINT_TIME_BASED_FILTER_QOS(i)(0));
                len := len + 1;
                ret.data(ind+len)   := std_logic_vector(ENDPOINT_TIME_BASED_FILTER_QOS(i)(1));
            end if;
            -- RELIABILITY
            if (ENDPOINT_RELIABILITY_QOS(i) /= DEFAULT_RELIABILTY_QOS or ENDPOINT_MAX_BLOCKING_TIME(i) /= DEFAULT_MAX_BLOCKING_TIME) then
                len := len + 1;
                ret.data(ind+len)   := PID_RELIABILITY & std_logic_vector(to_unsigned(12, 16));
                len := len + 1;
                ret.data(ind+len)   := ENDPOINT_RELIABILITY_QOS(i);
                len := len + 1;
                ret.data(ind+len)   := std_logic_vector(ENDPOINT_MAX_BLOCKING_TIME(i)(0));
                len := len + 1;
                ret.data(ind+len)   := std_logic_vector(ENDPOINT_MAX_BLOCKING_TIME(i)(1));
            end if;
            -- DESTINATION ORDER
            if (ENDPOINT_DESTINATION_ORDER_QOS(i) /= DEFAULT_DESTINATION_ORDER_QOS) then
                len := len + 1;
                ret.data(ind+len)   := PID_DESTINATION_ORDER & std_logic_vector(to_unsigned(4, 16));
                len := len + 1;
                ret.data(ind+len)   := ENDPOINT_DESTINATION_ORDER_QOS(i);
            end if;
            -- SENTINEL
            len := len + 1;
            ret.data(ind+len)   := PID_SENTINEL & std_logic_vector(to_unsigned(0, 16));
            -- Fix Submessage Length
            ret.data(ind)(15 downto 0)  := std_logic_vector(to_unsigned(len*4, 16));
            -- Reset Pointers
            ind := ind + len + 1;
            len := 0;
        end loop;
        -- Store Total Length
        ret.length := ind;
        return ret;
    end function;
    
    constant READER_ENDPOINT_DATA   : OUTPUT_DATA_TYPE := gen_reader_endpoint_data;
    
    function gen_writer_endpoint_data return OUTPUT_DATA_TYPE is
        variable ret    : OUTPUT_DATA_TYPE := (data => (others => (others => '0')), length => 0);
        variable ind    : natural := 0;
        variable len    : natural := 0;
        variable tmp    : natural := 0;
    begin
        ret.data    := (others => (others => '0'));
        ret.length  := 0;
        -- Sanity Check
        if (NUM_WRITERS = 0) then
            return ret;
        end if;
        len         := 0;
        ind         := 0;
        
        -- RTPS Submessages
        -- One DATA Submessage for each Writer Endpoint
        for i in NUM_READERS to NUM_ENDPOINTS-1 loop
            -- RTPS Submessage Header
            ret.data(ind)       := SID_DATA & "00000100" & x"0000";
            -- DATA Header (extraFlags, octetsToInlineQoS)
            len := len + 1;
            ret.data(ind+len)   := x"0000" & std_logic_vector(to_unsigned(16, 16));
            -- DATA Header (Reader Entity ID)
            len := len + 1;
            ret.data(ind+len)   := ENTITYID_SEDP_BUILTIN_PUBLICATIONS_DETECTOR;
            -- DATA Header (Writer Entity ID)
            len := len + 1;
            ret.data(ind+len)   := ENTITYID_SEDP_BUILTIN_PUBLICATIONS_ANNOUNCER;
            -- DATA Header (Sequence Number)
            len := len + 1;
            ret.data(ind+len)   := (others => '0');
            -- DATA Header (Sequence Number)
            len := len + 1;
            ret.data(ind+len)   := std_logic_vector(to_unsigned(i-NUM_READERS+1, ret.data(0)'length));
            -- Serialized Payload Header
            len := len + 1;
            ret.data(ind+len)   := PL_CDR_BE & x"0000";
            -- Serialized Payload BEGIN
            -- GUID
            len := len + 1;
            ret.data(ind+len):= PID_ENDPOINT_GUID & std_logic_vector(to_unsigned(16, 16));
            len := len + 1;
            ret.data(ind+len)   := GUIDPREFIX(0);
            len := len + 1;
            ret.data(ind+len)   := GUIDPREFIX(1);
            len := len + 1;
            ret.data(ind+len)   := GUIDPREFIX(2);
            len := len + 1;
            ret.data(ind+len)   := ENTITYID(i);
            -- TOPIC NAME
            tmp := string_len(ENDPOINT_TOPIC(i));
            len := len + 1;
            ret.data(ind+len)   := PID_TOPIC_NAME & std_logic_vector(to_unsigned((round_div(tmp,4)+1)*4, 16));
            len := len + 1;
            ret.data(ind+len)   := std_logic_vector(to_unsigned(tmp, 32));
            for j in 0 to round_div(tmp,4)-1 loop
                len := len + 1;
                ret.data(ind+len)   := ENDPOINT_TOPIC(i)(j);
            end loop;
            -- TYPE NAME
            tmp := string_len(ENDPOINT_TYPE(i));
            len := len + 1;
            ret.data(ind+len)   := PID_TYPE_NAME & std_logic_vector(to_unsigned((round_div(tmp,4)+1)*4, 16));
            len := len + 1;
            ret.data(ind+len)   := std_logic_vector(to_unsigned(tmp, 32));
            for j in 0 to round_div(tmp,4)-1 loop
                len := len + 1;
                ret.data(ind+len)   := ENDPOINT_TYPE(i)(j);
            end loop;
            -- DURABILITY
            if (ENDPOINT_DURABILITY_QOS(i) /= DEFAULT_DURABILITY_QOS) then
                len := len + 1;
                ret.data(ind+len)   := PID_DURABILITY & std_logic_vector(to_unsigned(4, 16));
                len := len + 1;
                ret.data(ind+len)   := ENDPOINT_DURABILITY_QOS(i);
            end if;
            -- DURABILITY SERVICE
            if (ENDPOINT_DURABILITY_SERVICE_CLEANUP_DELAY(i) /= DEFAULT_DURABILITY_SERVICE_CLEANUP_DELAY or ENDPOINT_DURABILITY_SERVICE_HISTORY(i) /= DEFAULT_DURABILITY_SERVICE_HISTORY or 
                ENDPOINT_DURABILITY_SERVICE_HISTORY_DEPTH(i) /= DEFAULT_DURABILITY_SERVICE_HISTORY_DEPTH or ENDPOINT_DURABILITY_SERVICE_MAX_SAMPLES(i) /= DEFAULT_DURABILITY_SERVICE_MAX_SAMPLES or 
                ENDPOINT_DURABILITY_SERVICE_MAX_INSTANCES(i) /= DEFAULT_DURABILITY_SERVICE_MAX_INSTANCES or ENDPOINT_DURABILITY_SERVICE_MAX_SAMPLES_PER_INSTANCE(i) /= DEFAULT_DURABILITY_SERVICE_MAX_SAMPLES_PER_INSTANCE
            ) then
                len := len + 1;
                ret.data(ind+len)   := PID_DURABILITY_SERVICE & std_logic_vector(to_unsigned(28, 16));
                len := len + 1;
                ret.data(ind+len)   := std_logic_vector(ENDPOINT_DURABILITY_SERVICE_CLEANUP_DELAY(i)(0));
                len := len + 1;
                ret.data(ind+len)   := std_logic_vector(ENDPOINT_DURABILITY_SERVICE_CLEANUP_DELAY(i)(1));
                len := len + 1;
                ret.data(ind+len)   := ENDPOINT_DURABILITY_SERVICE_HISTORY(i);
                len := len + 1;
                ret.data(ind+len)   := ENDPOINT_DURABILITY_SERVICE_HISTORY_DEPTH(i);
                len := len + 1;
                ret.data(ind+len)   := ENDPOINT_DURABILITY_SERVICE_MAX_SAMPLES(i);
                len := len + 1;
                ret.data(ind+len)   := ENDPOINT_DURABILITY_SERVICE_MAX_INSTANCES(i);
                len := len + 1;
                ret.data(ind+len)   := ENDPOINT_DURABILITY_SERVICE_MAX_SAMPLES_PER_INSTANCE(i);
            end if;
            -- PRESENTATION
            if (ENDPOINT_PRESENTATION_QOS(i) /= DEFAULT_PRESENTATION_QOS or ENDPOINT_COHERENT_ACCESS(i) /= DEFAULT_COHERENT_ACCESS or ENDPOINT_ORDERED_ACCESS(i) /= DEFAULT_ORDERED_ACCESS) then
                len := len + 1;
                ret.data(ind+len)   := PID_PRESENTATION & std_logic_vector(to_unsigned(8, 16));
                len := len + 1;
                ret.data(ind+len)   := ENDPOINT_PRESENTATION_QOS(i);
                len := len + 1;
                ret.data(ind+len)   := (24 => boolean_to_std_logic(ENDPOINT_COHERENT_ACCESS(i)), 16 => boolean_to_std_logic(ENDPOINT_ORDERED_ACCESS(i)), others => '0');
            end if;
            -- DEADLINE
            if (ENDPOINT_DEADLINE_QOS(i) /= DEFAULT_DEADLINE_QOS) then
                len := len + 1;
                ret.data(ind+len)   := PID_DEADLINE & std_logic_vector(to_unsigned(8, 16));
                len := len + 1;
                ret.data(ind+len)   := std_logic_vector(ENDPOINT_DEADLINE_QOS(i)(0));
                len := len + 1;
                ret.data(ind+len)   := std_logic_vector(ENDPOINT_DEADLINE_QOS(i)(1));
            end if;
            -- LATENCY_BUDGET
            if (ENDPOINT_LATENCY_BUDGET_QOS(i) /= DEFAULT_LATENCY_BUDGET_QOS) then
                len := len + 1;
                ret.data(ind+len)   := PID_LATENCY_BUDGET & std_logic_vector(to_unsigned(8, 16));
                len := len + 1;
                ret.data(ind+len)   := std_logic_vector(ENDPOINT_LATENCY_BUDGET_QOS(i)(0));
                len := len + 1;
                ret.data(ind+len)   := std_logic_vector(ENDPOINT_LATENCY_BUDGET_QOS(i)(1));
            end if;
            -- OWNERSHIP
            if (ENDPOINT_OWNERSHIP_QOS(i) /= DEFAULT_OWNERSHIP_QOS) then
                len := len + 1;
                ret.data(ind+len)   := PID_OWNERSHIP & std_logic_vector(to_unsigned(4, 16));
                len := len + 1;
                ret.data(ind+len)   := ENDPOINT_OWNERSHIP_QOS(i);
            end if;
            -- OWNERSHIP STRENGTH
            if (ENDPOINT_OWNERSHIP_STRENGTH_QOS(i) /= DEFAULT_OWNERSHIP_STRENGTH_QOS) then
                len := len + 1;
                ret.data(ind+len)   := PID_OWNERSHIP_STRENGTH & std_logic_vector(to_unsigned(4, 16));
                len := len + 1;
                ret.data(ind+len)   := ENDPOINT_OWNERSHIP_STRENGTH_QOS(i);
            end if;
            -- LIVELINESS
            if (ENDPOINT_LIVELINESS_QOS(i) /= DEFAULT_LIVELINESS_QOS or ENDPOINT_LEASE_DURATION(i) /= DEFAULT_LEASE_DURATION) then
                len := len + 1;
                ret.data(ind+len)   := PID_LIVELINESS & std_logic_vector(to_unsigned(12, 16));
                len := len + 1;
                ret.data(ind+len)   := ENDPOINT_LIVELINESS_QOS(i);
                len := len + 1;
                ret.data(ind+len)   := std_logic_vector(ENDPOINT_LEASE_DURATION(i)(0));
                len := len + 1;
                ret.data(ind+len)   := std_logic_vector(ENDPOINT_LEASE_DURATION(i)(1));
            end if;
            -- RELIABILITY
            if (ENDPOINT_RELIABILITY_QOS(i) /= DEFAULT_RELIABILTY_QOS or ENDPOINT_MAX_BLOCKING_TIME(i) /= DEFAULT_MAX_BLOCKING_TIME) then
                len := len + 1;
                ret.data(ind+len)   := PID_RELIABILITY & std_logic_vector(to_unsigned(12, 16));
                len := len + 1;
                ret.data(ind+len)   := ENDPOINT_RELIABILITY_QOS(i);
                len := len + 1;
                ret.data(ind+len)   := std_logic_vector(ENDPOINT_MAX_BLOCKING_TIME(i)(0));
                len := len + 1;
                ret.data(ind+len)   := std_logic_vector(ENDPOINT_MAX_BLOCKING_TIME(i)(1));
            end if;
            -- TRANSPORT PRIORITY
            if (ENDPOINT_TRANSPORT_PRIORITY_QOS(i) /= DEFAULT_TRANSPORT_PRIORITY_QOS) then
                len := len + 1;
                ret.data(ind+len)   := PID_TRANSPORT_PRIORITY & std_logic_vector(to_unsigned(4, 16));
                len := len + 1;
                ret.data(ind+len)   := ENDPOINT_TRANSPORT_PRIORITY_QOS(i);
            end if;
            -- LIFESPAN
            if (ENDPOINT_LIFESPAN_QOS(i) /= DEFAULT_LIFESPAN_QOS) then
                len := len + 1;
                ret.data(ind+len)   := PID_LIFESPAN & std_logic_vector(to_unsigned(8, 16));
                len := len + 1;
                ret.data(ind+len)   := std_logic_vector(ENDPOINT_LIFESPAN_QOS(i)(0));
                len := len + 1;
                ret.data(ind+len)   := std_logic_vector(ENDPOINT_LIFESPAN_QOS(i)(1));
            end if;
            -- DESTINATION ORDER
            if (ENDPOINT_DESTINATION_ORDER_QOS(i) /= DEFAULT_DESTINATION_ORDER_QOS) then
                len := len + 1;
                ret.data(ind+len)   := PID_DESTINATION_ORDER & std_logic_vector(to_unsigned(4, 16));
                len := len + 1;
                ret.data(ind+len)   := ENDPOINT_DESTINATION_ORDER_QOS(i);
            end if;
            -- TODO: MAX_SIZE_SERIALIZED
            -- SENTINEL
            len := len + 1;
            ret.data(ind+len)   := PID_SENTINEL & std_logic_vector(to_unsigned(0, 16));
            -- Fix Submessage Length
            ret.data(ind)(15 downto 0)  := std_logic_vector(to_unsigned(len*4, 16));
            -- Reset Pointers
            ind := ind + len + 1;
            len := 0;
        end loop;
        -- Store Total Length
        ret.length := ind;
        return ret;
    end function;
    
    constant WRITER_ENDPOINT_DATA  : OUTPUT_DATA_TYPE := gen_writer_endpoint_data;
    
    function gen_inline_qos (id : natural) return OUTPUT_DATA_TYPE is
        variable ret    : OUTPUT_DATA_TYPE := (data => (others => (others => '0')), length => 0);
        variable ind    : natural := 0;
        variable tmp    : natural := 0;
    begin
        assert (id >= NUM_READERS) report "Inline Qos can only be generated for Writer Endpoints" severity FAILURE;
        
        -- TOPIC NAME
        tmp := string_len(ENDPOINT_TOPIC(id));
        ret.data(ret.length)    := PID_TOPIC_NAME & std_logic_vector(to_unsigned((round_div(tmp,4)+1)*4, 16));
        ret.length              := ret.length + 1;
        ret.data(ret.length)    := std_logic_vector(to_unsigned(tmp, 32));
        for j in 0 to round_div(tmp,4)-1 loop
            ret.length              := ret.length + 1;
            ret.data(ret.length)    := ENDPOINT_TOPIC(id)(j);
        end loop;
        -- DURABILITY
        if (ENDPOINT_DURABILITY_QOS(id) /= DEFAULT_DURABILITY_QOS) then
            ret.length              := ret.length + 1;
            ret.data(ret.length)    := PID_DURABILITY & std_logic_vector(to_unsigned(4, 16));
            ret.length              := ret.length + 1;
            ret.data(ret.length)    := ENDPOINT_DURABILITY_QOS(id);
        end if;
        -- PRESENTATION
        if (ENDPOINT_PRESENTATION_QOS(id) /= DEFAULT_PRESENTATION_QOS or ENDPOINT_COHERENT_ACCESS(id) /= DEFAULT_COHERENT_ACCESS or ENDPOINT_ORDERED_ACCESS(id) /= DEFAULT_ORDERED_ACCESS) then
            ret.length              := ret.length + 1;
            ret.data(ret.length)    := PID_PRESENTATION & std_logic_vector(to_unsigned(8, 16));
            ret.length              := ret.length + 1;
            ret.data(ret.length)    := ENDPOINT_PRESENTATION_QOS(id);
            ret.length              := ret.length + 1;
            ret.data(ret.length)    := (24 => boolean_to_std_logic(ENDPOINT_COHERENT_ACCESS(id)), 16 => boolean_to_std_logic(ENDPOINT_ORDERED_ACCESS(id)), others => '0');
        end if;
        -- DEADLINE
        if (ENDPOINT_DEADLINE_QOS(id) /= DEFAULT_DEADLINE_QOS) then
            ret.length              := ret.length + 1;
            ret.data(ret.length)    := PID_DEADLINE & std_logic_vector(to_unsigned(8, 16));
            ret.length              := ret.length + 1;
            ret.data(ret.length)    := std_logic_vector(ENDPOINT_DEADLINE_QOS(id)(0));
            ret.length              := ret.length + 1;
            ret.data(ret.length)    := std_logic_vector(ENDPOINT_DEADLINE_QOS(id)(1));
        end if;
        -- LATENCY_BUDGET
        if (ENDPOINT_LATENCY_BUDGET_QOS(id) /= DEFAULT_LATENCY_BUDGET_QOS) then
            ret.length              := ret.length + 1;
            ret.data(ret.length)    := PID_LATENCY_BUDGET & std_logic_vector(to_unsigned(8, 16));
            ret.length              := ret.length + 1;
            ret.data(ret.length)    := std_logic_vector(ENDPOINT_LATENCY_BUDGET_QOS(id)(0));
            ret.length              := ret.length + 1;
            ret.data(ret.length)    := std_logic_vector(ENDPOINT_LATENCY_BUDGET_QOS(id)(1));
        end if;
        -- OWNERSHIP
        if (ENDPOINT_OWNERSHIP_QOS(id) /= DEFAULT_OWNERSHIP_QOS) then
            ret.length              := ret.length + 1;
            ret.data(ret.length)    := PID_OWNERSHIP & std_logic_vector(to_unsigned(4, 16));
            ret.length              := ret.length + 1;
            ret.data(ret.length)    := ENDPOINT_OWNERSHIP_QOS(id);
        end if;
        -- OWNERSHIP STRENGTH
        if (ENDPOINT_OWNERSHIP_STRENGTH_QOS(id) /= DEFAULT_OWNERSHIP_STRENGTH_QOS) then
            ret.length              := ret.length + 1;
            ret.data(ret.length)    := PID_OWNERSHIP_STRENGTH & std_logic_vector(to_unsigned(4, 16));
            ret.length              := ret.length + 1;
            ret.data(ret.length)    := ENDPOINT_OWNERSHIP_STRENGTH_QOS(id);
        end if;
        -- LIVELINESS
        if (ENDPOINT_LIVELINESS_QOS(id) /= DEFAULT_LIVELINESS_QOS or ENDPOINT_LEASE_DURATION(id) /= DEFAULT_LEASE_DURATION) then
            ret.length              := ret.length + 1;
            ret.data(ret.length)    := PID_LIVELINESS & std_logic_vector(to_unsigned(12, 16));
            ret.length              := ret.length + 1;
            ret.data(ret.length)    := ENDPOINT_LIVELINESS_QOS(id);
            ret.length              := ret.length + 1;
            ret.data(ret.length)    := std_logic_vector(ENDPOINT_LEASE_DURATION(id)(0));
            ret.length              := ret.length + 1;
            ret.data(ret.length)    := std_logic_vector(ENDPOINT_LEASE_DURATION(id)(1));
        end if;
        -- RELIABILITY
        if (ENDPOINT_RELIABILITY_QOS(id) /= DEFAULT_RELIABILTY_QOS or ENDPOINT_MAX_BLOCKING_TIME(id) /= DEFAULT_MAX_BLOCKING_TIME) then
            ret.length              := ret.length + 1;
            ret.data(ret.length)    := PID_RELIABILITY & std_logic_vector(to_unsigned(12, 16));
            ret.length              := ret.length + 1;
            ret.data(ret.length)    := ENDPOINT_RELIABILITY_QOS(id);
            ret.length              := ret.length + 1;
            ret.data(ret.length)    := std_logic_vector(ENDPOINT_MAX_BLOCKING_TIME(id)(0));
            ret.length              := ret.length + 1;
            ret.data(ret.length)    := std_logic_vector(ENDPOINT_MAX_BLOCKING_TIME(id)(1));
        end if;
        -- TRANSPORT PRIORITY
        if (ENDPOINT_TRANSPORT_PRIORITY_QOS(id) /= DEFAULT_TRANSPORT_PRIORITY_QOS) then
            ret.length              := ret.length + 1;
            ret.data(ret.length)    := PID_TRANSPORT_PRIORITY & std_logic_vector(to_unsigned(4, 16));
            ret.length              := ret.length + 1;
            ret.data(ret.length)    := ENDPOINT_TRANSPORT_PRIORITY_QOS(id);
        end if;
        -- LIFESPAN
        if (ENDPOINT_LIFESPAN_QOS(id) /= DEFAULT_LIFESPAN_QOS) then
            ret.length              := ret.length + 1;
            ret.data(ret.length)    := PID_LIFESPAN & std_logic_vector(to_unsigned(8, 16));
            ret.length              := ret.length + 1;
            ret.data(ret.length)    := std_logic_vector(ENDPOINT_LIFESPAN_QOS(id)(0));
            ret.length              := ret.length + 1;
            ret.data(ret.length)    := std_logic_vector(ENDPOINT_LIFESPAN_QOS(id)(1));
        end if;
        -- DESTINATION ORDER
        if (ENDPOINT_DESTINATION_ORDER_QOS(id) /= DEFAULT_DESTINATION_ORDER_QOS) then
            ret.length              := ret.length + 1;
            ret.data(ret.length)    := PID_DESTINATION_ORDER & std_logic_vector(to_unsigned(4, 16));
            ret.length              := ret.length + 1;
            ret.data(ret.length)    := ENDPOINT_DESTINATION_ORDER_QOS(id);
        end if;
        -- SENTINEL
        ret.length              := ret.length + 1;
        ret.data(ret.length)    := PID_SENTINEL & std_logic_vector(to_unsigned(0, 16));
        -- Final Length
        ret.length              := ret.length + 1;
        return ret;
    end function;
    
    function gen_participant_data return OUTPUT_DATA_TYPE is
        variable ret    : OUTPUT_DATA_TYPE := (data => (others => (others => '0')), length => 0);
        variable len    : natural := 0;
        variable tmp    : natural := 0;
    begin
        ret.data    := (others => (others => '0'));
        ret.length  := 0;
        len         := 0;
        
        -- RTPS DATA SUBMESSAGE
        -- RTPS Submessage Header
        ret.data(len)       := SID_DATA & "00000100" & x"0000";
        -- DATA Header (extraFlags, octetsToInlineQoS)
        len := len + 1;
        ret.data(len)   := x"0000" & std_logic_vector(to_unsigned(16, 16));
        -- DATA Header (Reader Entity ID)
        len := len + 1;
        ret.data(len)   := ENTITYID_SPDP_BUILTIN_PARTICIPANT_DETECTOR;
        -- DATA Header (Writer Entity ID)
        len := len + 1;
        ret.data(len)   := ENTITYID_SPDP_BUILTIN_PARTICIPANT_ANNOUNCER;
        -- DATA Header (Sequence Number)
        len := len + 1;
        ret.data(len)   := (others => '0');
        -- DATA Header (Sequence Number)
        len := len + 1;
        ret.data(len)   := std_logic_vector(to_unsigned(1, ret.data(len)'length));
        -- Serialized Payload Header
        len := len + 1;
        ret.data(len)   := PL_CDR_BE & x"0000";
        -- Serialized Payload BEGIN
        -- GUID
        len := len + 1;
        ret.data(len):= PID_PARTICIPANT_GUID & std_logic_vector(to_unsigned(16, 16));
        len := len + 1;
        ret.data(len)   := GUIDPREFIX(0);
        len := len + 1;
        ret.data(len)   := GUIDPREFIX(1);
        len := len + 1;
        ret.data(len)   := GUIDPREFIX(2);
        len := len + 1;
        ret.data(len)   := ENTITYID_PARTICIPANT;
        -- DOMAIN ID
        len := len + 1;
        ret.data(len):= PID_DOMAIN_ID & std_logic_vector(to_unsigned(4, 16));
        len := len + 1;
        ret.data(len):= DOMAIN_ID;
        -- DOMAIN TAG
        if (DOMAIN_TAG /= DEFAULT_DOMAIN_TAG) then
            tmp := string_len(DOMAIN_TAG);
            len := len + 1;
            ret.data(len)   := PID_DOMAIN_TAG & std_logic_vector(to_unsigned((round_div(tmp,4)+1)*4, 16));
            len := len + 1;
            ret.data(len)   := std_logic_vector(to_unsigned(tmp, 32));
            for j in 0 to round_div(tmp,4)-1 loop
                len := len + 1;
                ret.data(len)   := DOMAIN_TAG(j);
            end loop;
        end if;
        -- PROTOCOL VERSION
        len := len + 1;
        ret.data(len)   := PID_PROTOCOL_VERSION & std_logic_vector(to_unsigned(4, 16));
        len := len + 1;
        ret.data(len)               := (others => '0');
		ret.data(len)(31 downto 16) := PROTOCOLVERSION_2_4;
        -- VENDORID
        len := len + 1;
        ret.data(len)   := PID_VENDORID & std_logic_vector(to_unsigned(4, 16));
        len := len + 1;
        ret.data(len)               := (others => '0');
		ret.data(len)(31 downto 16) := VENDORID;
        -- TODO: Expects inline QoS of Participant
        -- METATRAFFIC MULTICAST LOCATOR
        len := len + 1;
        ret.data(len)   := PID_METATRAFFIC_MULTICAST_LOCATOR & std_logic_vector(to_unsigned(24, 16));
        len := len + 1;
        ret.data(len)   := LOCATOR_KIND_UDPv4;
        len := len + 1;
        ret.data(len)               := (others => '0');
		ret.data(len)(15 downto 0)  := META_IPv4_MULTICAST_PORT;
        len := len + 1;
        ret.data(len)   := (others => '0');
        len := len + 1;
        ret.data(len)   := (others => '0');
        len := len + 1;
        ret.data(len)   := (others => '0');
        len := len + 1;
        ret.data(len)   := DEFAULT_IPv4_ADDRESS;
        -- METATRAFFIC UNICAST LOCATOR
        len := len + 1;
        ret.data(len)   := PID_METATRAFFIC_UNICAST_LOCATOR & std_logic_vector(to_unsigned(24, 16));
        len := len + 1;
        ret.data(len)   := LOCATOR_KIND_UDPv4;
        len := len + 1;
        ret.data(len)               := (others => '0');
		ret.data(len)(15 downto 0)  := META_IPv4_UNICAST_PORT;
        len := len + 1;
        ret.data(len)   := (others => '0');
        len := len + 1;
        ret.data(len)   := (others => '0');
        len := len + 1;
        ret.data(len)   := (others => '0');
        len := len + 1;
        ret.data(len)   := DEFAULT_IPv4_ADDRESS;
        -- DEFAULT MULTICAST LOCATOR
        len := len + 1;
        ret.data(len)   := PID_DEFAULT_MULTICAST_LOCATOR & std_logic_vector(to_unsigned(24, 16));
        len := len + 1;
        ret.data(len)   := LOCATOR_KIND_UDPv4;
        len := len + 1;
        ret.data(len)               := (others => '0');
		ret.data(len)(15 downto 0)  := USER_IPv4_MULTICAST_PORT;
        len := len + 1;
        ret.data(len)   := (others => '0');
        len := len + 1;
        ret.data(len)   := (others => '0');
        len := len + 1;
        ret.data(len)   := (others => '0');
        len := len + 1;
        ret.data(len)   := DEFAULT_IPv4_ADDRESS;
        -- DEFAULT UNICAST LOCATOR
        len := len + 1;
        ret.data(len)   := PID_DEFAULT_UNICAST_LOCATOR & std_logic_vector(to_unsigned(24, 16));
        len := len + 1;
        ret.data(len)   := LOCATOR_KIND_UDPv4;
        len := len + 1;
        ret.data(len)               := (others => '0');
		ret.data(len)(15 downto 0)  := USER_IPv4_UNICAST_PORT;
        len := len + 1;
        ret.data(len)   := (others => '0');
        len := len + 1;
        ret.data(len)   := (others => '0');
        len := len + 1;
        ret.data(len)   := (others => '0');
        len := len + 1;
        ret.data(len)   := DEFAULT_IPv4_ADDRESS;
        -- LEASE DURATION
        if (PARTICIPANT_LEASE_DURATION /= DEFAULT_PARTICIPANT_LEASE_DURATION) then
            len := len + 1;
            ret.data(len)   := PID_PARTICIPANT_LEASE_DURATION & std_logic_vector(to_unsigned(8, 16));
            len := len + 1;
            ret.data(len)   := std_logic_vector(PARTICIPANT_LEASE_DURATION(0));
            len := len + 1;
            ret.data(len)   := std_logic_vector(PARTICIPANT_LEASE_DURATION(1));
        end if;
        -- AVAILABLE ENDPOINTS
        len := len + 1;
        ret.data(len)   := PID_BUILTIN_ENDPOINT_SET & std_logic_vector(to_unsigned(4, 16));
        len := len + 1;
        ret.data(len)   := (DISC_BUILTIN_ENDPOINT_PARTICIPANT_ANNOUNCER => '1', DISC_BUILTIN_ENDPOINT_PARTICIPANT_DETECTOR => '1', BUILTIN_ENDPOINT_PARTICIPANT_MESSAGE_DATA_WRITER => '1', BUILTIN_ENDPOINT_PARTICIPANT_MESSAGE_DATA_READER => '1', others => '0');
        if (NUM_READERS > 0) then
            ret.data(len)(DISC_BUILTIN_ENDPOINT_PUBLICATIONS_DETECTOR)  := '1';
            ret.data(len)(DISC_BUILTIN_ENDPOINT_SUBSCRIPTIONS_ANNOUNCER):= '1';
        end if;
        if (NUM_WRITERS > 0) then
            ret.data(len)(DISC_BUILTIN_ENDPOINT_PUBLICATIONS_ANNOUNCER) := '1';
            ret.data(len)(DISC_BUILTIN_ENDPOINT_SUBSCRIPTIONS_DETECTOR) := '1';
        end if;
        -- MANUAL LIVELINESS COUNT
        len := len + 1;
        ret.data(len)   := PID_PARTICIPANT_MANUAL_LIVELINESS_COUNT & std_logic_vector(to_unsigned(4, 16));
        len := len + 1;
        ret.data(len)   := (others => '0');
        -- SENTINEL
        len := len + 1;
        ret.data(len)   := PID_SENTINEL & std_logic_vector(to_unsigned(0, 16));
        -- Fix Submessage Length
        ret.data(0)(15 downto 0)  := std_logic_vector(to_unsigned(len*4, 16));
        -- Store Length
        ret.length := len + 1;
        return ret;
    end function;
    
    constant PARTICIPANT_DATA   : OUTPUT_DATA_TYPE := gen_participant_data;
    
    
    -- Direction 0 : Offered <= Requested
    -- Direction 1 : Offered >= Requested
    -- src_is_reader 1 : Remote is Subscriber-Requested
    -- src_is_reader 0 : Remote is Publisher-Offered
    function check_qos_compatibility(src_is_reader : std_logic; direction : std_logic; remote : unsigned(WORD_WIDTH-1 downto 0); local : unsigned(WORD_WIDTH-1 downto 0)) return boolean is
        variable ret : boolean;
    begin
        -- Default Match
        ret := TRUE;
        
        if ((src_is_reader xor direction) = '1') then
            if (remote < local) then
                ret := FALSE;
            end if;
        else        
            if (remote > local) then
                ret := FALSE;
            end if;
        end if;
        
        return ret;
    end function;
    
    -- Direction 0 : Offered <= Requested
    -- Direction 1 : Offered >= Requested
    -- src_is_reader 1 : Remote is Subscriber-Requested
    -- src_is_reader 0 : Remote is Publisher-Offered
    function check_qos_compatibility(src_is_reader : std_logic; direction : std_logic; remote : DOUBLE_WORD_ARRAY; local : DOUBLE_WORD_ARRAY) return boolean is
        variable ret : boolean;
    begin
        -- Default Match
        ret := TRUE;
        
        if ((src_is_reader xor direction) = '1') then
            if (remote < local) then
                ret := FALSE;
            end if;
        else        
            if (remote > local) then
                ret := FALSE;
            end if;
        end if;
        
        return ret;
    end function;
    
    function gen_default_qos_match(is_reader : boolean) return DEFAULT_QOS_MATCH_TYPE is
        variable ret : DEFAULT_QOS_MATCH_TYPE;
    begin
        -- Default Match
        ret := (others => (others => '1'));
        
        for i in 0 to NUM_ENDPOINTS-1 loop
            -- DURABILITY
            if (not check_qos_compatibility(boolean_to_std_logic(is_reader), '1', unsigned(DEFAULT_DURABILITY_QOS), unsigned(ENDPOINT_DURABILITY_QOS(i)))) then
                ret.DURABILITY_QOS(i) := '0';
            end if;
            -- PRESENTATION
            if (not check_qos_compatibility(boolean_to_std_logic(is_reader), '1', unsigned(DEFAULT_PRESENTATION_QOS), unsigned(ENDPOINT_PRESENTATION_QOS(i)))) then
                ret.PRESENTATION_QOS(i) := '0';
            end if;
            if (is_reader) then
                if (DEFAULT_COHERENT_ACCESS and not ENDPOINT_COHERENT_ACCESS(i)) then
                    ret.PRESENTATION_QOS(i) := '0';
                end if;
                if (DEFAULT_ORDERED_ACCESS and not ENDPOINT_ORDERED_ACCESS(i)) then
                    ret.PRESENTATION_QOS(i) := '0';
                end if;
            else
                if (not DEFAULT_COHERENT_ACCESS and ENDPOINT_COHERENT_ACCESS(i)) then
                    ret.PRESENTATION_QOS(i) := '0';
                end if;
                if (not DEFAULT_ORDERED_ACCESS and ENDPOINT_ORDERED_ACCESS(i)) then
                    ret.PRESENTATION_QOS(i) := '0';
                end if;
            end if;
            -- DEADLINE
            if (not check_qos_compatibility(boolean_to_std_logic(is_reader), '0', DEFAULT_DEADLINE_QOS(0), ENDPOINT_DEADLINE_QOS(i)(0))) then
                ret.DEADLINE_QOS(i) := '0';
            end if;
            if (not check_qos_compatibility(boolean_to_std_logic(is_reader), '0', DEFAULT_DEADLINE_QOS(1), ENDPOINT_DEADLINE_QOS(i)(1))) then
                ret.DEADLINE_QOS(i) := '0';
            end if;
            -- LATENCY_BUDGET
            if (not check_qos_compatibility(boolean_to_std_logic(is_reader), '0', DEFAULT_LATENCY_BUDGET_QOS(0), ENDPOINT_LATENCY_BUDGET_QOS(i)(0))) then
                ret.LATENCY_BUDGET_QOS(i) := '0';
            end if;
            if (not check_qos_compatibility(boolean_to_std_logic(is_reader), '0', DEFAULT_LATENCY_BUDGET_QOS(1), ENDPOINT_LATENCY_BUDGET_QOS(i)(1))) then
                ret.LATENCY_BUDGET_QOS(i) := '0';
            end if;
            -- OWNERSHIP
            if (DEFAULT_OWNERSHIP_QOS /= ENDPOINT_OWNERSHIP_QOS(i)) then
                ret.OWNERSHIP_QOS(i) := '0';
            end if;
            -- LIVELINESS
            if (not check_qos_compatibility(boolean_to_std_logic(is_reader), '1', unsigned(DEFAULT_LIVELINESS_QOS), unsigned(ENDPOINT_LIVELINESS_QOS(i)))) then
                ret.LIVELINESS_QOS(i) := '0';
            end if;
            if (not check_qos_compatibility(boolean_to_std_logic(is_reader), '0', DEFAULT_LEASE_DURATION(0), ENDPOINT_LEASE_DURATION(i)(0))) then
                ret.LIVELINESS_QOS(i) := '0';
            end if;
            if (not check_qos_compatibility(boolean_to_std_logic(is_reader), '0', DEFAULT_LEASE_DURATION(1), ENDPOINT_LEASE_DURATION(i)(1))) then
                ret.LIVELINESS_QOS(i) := '0';
            end if;
            -- RELIABILITY
            if (not check_qos_compatibility(boolean_to_std_logic(is_reader), '1', unsigned(DEFAULT_RELIABILTY_QOS), unsigned(ENDPOINT_RELIABILITY_QOS(i)))) then
                ret.RELIABILITY_QOS(i) := '0';
            end if;
            -- DESTINATION_ORDER
            if (not check_qos_compatibility(boolean_to_std_logic(is_reader), '1', unsigned(DEFAULT_DESTINATION_ORDER_QOS), unsigned(ENDPOINT_DESTINATION_ORDER_QOS(i)))) then
                ret.DESTINATION_ORDER_QOS(i) := '0';
            end if;
        end loop;
        return ret;
    end function;
    
    constant READER_DEFAULT_QOS_MATCH : DEFAULT_QOS_MATCH_TYPE := gen_default_qos_match(TRUE);
    constant WRITER_DEFAULT_QOS_MATCH : DEFAULT_QOS_MATCH_TYPE := gen_default_qos_match(FALSE);
    
    function gen_liveliness_slv(qos : std_logic_vector(CDR_ENUMERATION_WIDTH-1 downto 0)) return std_logic_vector is
        variable ret : std_logic_vector(0 to NUM_ENDPOINTS-1) := (others => '0');
    begin
        for i in 0 to NUM_ENDPOINTS-1 loop
            if (ENDPOINT_LIVELINESS_QOS(i) = qos) then
                ret(i)  := '1';
            end if;
        end loop;
        return ret;
    end function;
    
    constant AUTOMATIC_LIVELINESS_WRITERS               : std_logic_vector(0 to NUM_ENDPOINTS-1) := gen_liveliness_slv(AUTOMATIC_LIVELINESS_QOS) and (not ENDPOINT_READERS);
    constant MANUAL_BY_PARTICIPANT_LIVELINESS_WRITERS   : std_logic_vector(0 to NUM_ENDPOINTS-1) := gen_liveliness_slv(MANUAL_BY_PARTICIPANT_LIVELINESS_QOS) and (not ENDPOINT_READERS);
    constant AUTOMATIC_LIVELINESS_READERS               : std_logic_vector(0 to NUM_ENDPOINTS-1) := gen_liveliness_slv(AUTOMATIC_LIVELINESS_QOS) and ENDPOINT_READERS;
    constant MANUAL_BY_PARTICIPANT_LIVELINESS_READERS   : std_logic_vector(0 to NUM_ENDPOINTS-1) := gen_liveliness_slv(MANUAL_BY_PARTICIPANT_LIVELINESS_QOS) and ENDPOINT_READERS;
    
    -- Returns the 'data' argument either as is, or with reversed Byte order, depending on the 
    -- 'swap' argument.
    function endian_swap(swap : std_logic; data : std_logic_vector) return std_logic_vector is
        variable ret : std_logic_vector(data'range);
    begin
        -- Assert that Data Signal is Byte aligned
        assert (data'length mod 8 = 0) severity failure;
        -- Little Endian
        if (swap = '1') then
            -- Reverse byte Order
            for i in 0 to (data'length/8)-1 loop
                ret(i*8+8-1 downto i*8) := data(((data'length/8)-1-i)*8+8-1 downto ((data'length/8)-1-i)*8);
            end loop;
        -- Big Endian
        else
            ret := data;
        end if;
        return ret;
    end function;
    
    -- Returns the 'data' argument either as is, or with reversed Byte order, depending on the 
    -- 'swap' argument.
    function endian_swap(swap : std_logic; data : unsigned) return unsigned is
    begin
        return unsigned(endian_swap(swap, std_logic_vector(data)));
    end function;
    
    function round_slv(slv : std_logic_vector; width : natural) return std_logic_vector is
        variable ret : std_logic_vector(width-1 downto 0) := (others => '0');
    begin
        ret := slv(slv'length-1 downto slv'length-width);
        if (slv(slv'length-width-1 downto 0) /= (slv'length-width-1 downto 0 => '0')) then
            ret := std_logic_vector(unsigned(ret) + 1);
        end if;
        return ret;
    end function;
    
    function check_mask(flags : std_logic_vector; mask : std_logic_vector) return boolean is
    begin
        assert (flags'length = mask'length) report "Flag and mask Signal have unequal length" severity FAILURE;
        if ((flags and mask) = mask) then
            return TRUE;
        else
            return FALSE;
        end if;
    end function;
    
end package body;
