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

library osvvm; -- Utility Library
context osvvm.OsvvmContext;

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

package rtps_test_package is
    
    constant RESULTS_FILE : string := "./Test_Results.txt";
    
    constant DEFAULT_GUIDPREFIX : GUIDPREFIX_TYPE; -- Deferred to Package Body
    constant DEFAULT_ENTITYID   : std_logic_vector(ENTITYID_WIDTH-1 downto 0); -- Deferred to Package Body
    
    type TEST_RAM_TYPE is array (0 to (MAX_REMOTE_PARTICIPANTS*PARTICIPANT_FRAME_SIZE)-1) of std_logic_vector(WORD_WIDTH-1 downto 0);
    
    type MATCH_TYPE is (MATCH, UNMATCH);
    
    type TEST_PACKET_TYPE is record
        -- Limit Packet Size to 2^16 (IPv4 size limit)
        data    : WORD_ARRAY_TYPE(0 to (2**16)/4);
        last    : std_logic_vector(0 to (2**16)/4);
        length  : natural;
    end record;
    
    constant EMPTY_TEST_PACKET : TEST_PACKET_TYPE; -- Deferred to Package Body
    
    type TEST_MEMORY_ELEMENT_TYPE is record
        addr    : natural;
        data    : std_logic_vector(WORD_WIDTH-1 downto 0);
    end record;
    
    type TEST_MEMORY_TYPE is array (natural range <>) of TEST_MEMORY_ELEMENT_TYPE;
    
    subtype TEST_PARTICIPANT_MEMORY_FRAME_TYPE is TEST_MEMORY_TYPE(0 to PARTICIPANT_FRAME_SIZE-1);
    
    constant LOCATOR_PORT_WIDTH : natural := CDR_LONG_WIDTH;
    constant LOCATOR_ADDR_WIDTH : natural := 4*CDR_LONG_WIDTH;
    
    type LOCATOR_TYPE is record
        kind        : std_logic_vector(LOCATOR_KIND_WIDTH-1 downto 0);
        portn       : std_logic_vector(CDR_LONG_WIDTH-1 downto 0);
        addr        : std_logic_vector(LOCATOR_ADDR_WIDTH-1 downto 0);
    end record;
    
    constant EMPTY_LOCATOR : LOCATOR_TYPE; -- Deferred to Package Body
    
    -- Limit Number of Locators to maximum number that fits in a maximum length RTPS Submessage
    type LOCATOR_TYPE_ARRAY is array (0 to ((2**16)/4)/6) of LOCATOR_TYPE;
    
    type LOCATOR_LIST_TYPE is record
        numLocators : std_logic_vector(CDR_LONG_WIDTH-1 downto 0);
        locator     : LOCATOR_TYPE_ARRAY;
    end record;
    
    constant EMPTY_LOCATOR_LIST : LOCATOR_LIST_TYPE; -- Deferred to Package Body
    
    type SEQUENCENUMBER_SET_TYPE is record
        base    : SEQUENCENUMBER_TYPE;
        numBits : std_logic_vector(CDR_LONG_WIDTH-1 downto 0);
        bitmap  : std_logic_vector(0 to 255);
    end record;
    
    constant DEFAULT_SEQUENCENUMBER_SET : SEQUENCENUMBER_SET_TYPE; -- Deferred to package Body
    
    type FRAGMENTNUMBER_SET_TYPE is record
        base    : std_logic_vector(CDR_LONG_WIDTH-1 downto 0);
        numBits : std_logic_vector(CDR_LONG_WIDTH-1 downto 0);
        bitmap  : std_logic_vector(0 to 255);
    end record;
    
    constant DEFAULT_FRAGMENTNUMBER_SET : FRAGMENTNUMBER_SET_TYPE; -- Deferred to Package Body
    
    -- Valid Destination Locators
    type DEST_LOCATOR_LIST_TYPE is record
        meta    : LOCATOR_LIST_TYPE;
        user    : LOCATOR_LIST_TYPE;
    end record;
    
    constant DEST_LOC : DEST_LOCATOR_LIST_TYPE; -- Deferred to Package Body
    
    -- *OUTPUT HEADER*
    type OUTPUT_HEADER_TYPE is record
        src     : LOCATOR_TYPE;
        dest    : LOCATOR_TYPE;
    end record;
    
    constant DEFAULT_OUTPUT_HEADER : OUTPUT_HEADER_TYPE; -- Deferred to Package Body
    
    -- *RTPS HEADER*
    type RTPS_HEADER_TYPE is record 
        protocol    : std_logic_vector(PROTOCOL_WIDTH-1 downto 0);
        version     : std_logic_vector(PROTOCOLVERSION_WIDTH-1 downto 0);
        vendorId    : std_logic_vector(VENDORID_WIDTH-1 downto 0);
        guidPrefix  : GUIDPREFIX_TYPE;
    end record;
    
    constant DEFAULT_RTPS_HEADER : RTPS_HEADER_TYPE; -- Deferred to Package Body
    
    -- *GENERIC RTPS SUBMESSAGE HEADER*
    type RTPS_SUBMESSAGE_TYPE is record
        submessageID            : std_logic_vector(SUBMESSAGE_ID_WIDTH-1 downto 0);
        flags                   : std_logic_vector(SUBMESSAGE_FLAGS_WIDTH-1 downto 0);
        submessageLength        : std_logic_vector(SUBMESSAGE_LENGTH_WIDTH-1 downto 0);
        readerId                : std_logic_vector(ENTITYID_WIDTH-1 downto 0);
        writerId                : std_logic_vector(ENTITYID_WIDTH-1 downto 0);
        extraFlags              : std_logic_vector(SUBMESSAGE_DATA_EXTRA_FLAGS_WIDTH-1 downto 0);
        octetsToInlineQos       : std_logic_vector(SUBMESSAGE_LENGTH_WIDTH-1 downto 0);
        readerSNState           : SEQUENCENUMBER_SET_TYPE;
        count                   : std_logic_vector(COUNT_WIDTH-1 downto 0);
        writerSN                : SEQUENCENUMBER_TYPE;
        fragmentNumberState     : FRAGMENTNUMBER_SET_TYPE;
        data                    : TEST_PACKET_TYPE;
        fragmentStartingNumber  : std_logic_vector(CDR_LONG_WIDTH-1 downto 0);
        fragmentsInSubmessage   : std_logic_vector(CDR_SHORT_WIDTH-1 downto 0);
        fragmentSize            : std_logic_vector(CDR_SHORT_WIDTH-1 downto 0);
        sampleSize              : std_logic_vector(CDR_LONG_WIDTH-1 downto 0);
        gapStart                : SEQUENCENUMBER_TYPE;
        gapList                 : SEQUENCENUMBER_SET_TYPE;
        firstSN                 : SEQUENCENUMBER_TYPE;
        lastSN                  : SEQUENCENUMBER_TYPE;
        lastFragmentNum         : std_logic_vector(CDR_LONG_WIDTH-1 downto 0);
        guidPrefix              : GUIDPREFIX_TYPE;
        unicastLocatorList      : LOCATOR_LIST_TYPE;
        multicastLocatorList    : LOCATOR_LIST_TYPE;
        unicastLocator          : LOCATOR_TYPE;
        multicastLocator        : LOCATOR_TYPE;
        unused                  : std_logic_vector(31 downto 0);
        version                 : std_logic_vector(PROTOCOLVERSION_WIDTH-1 downto 0);
        vendorId                : std_logic_vector(VENDORID_WIDTH-1 downto 0);
        timestamp               : TIME_TYPE;
    end record;
    
    constant DEFAULT_RTPS_SUBMESSAGE : RTPS_SUBMESSAGE_TYPE; -- Deferred to Package Body
    
    type PARTICIPANT_DATA_TYPE is record
        littleEndian                    : std_logic;
        domainId                        : std_logic_vector(DOMAIN_ID_WIDTH-1 downto 0);
        domainTag                       : STRING_WORD_ARRAY_TYPE;
        protocolVersion                 : std_logic_vector(PROTOCOLVERSION_WIDTH-1 downto 0);
        vendorId                        : std_logic_vector(VENDORID_WIDTH-1 downto 0);
        guidPrefix                      : GUIDPREFIX_TYPE;
        entityId                        : std_logic_vector(ENTITYID_WIDTH-1 downto 0);
        expectsInlineQoS                : std_logic_vector(CDR_BOOLEAN_WIDTH-1 downto 0);
        metatrafficUnicastLocatorList   : LOCATOR_LIST_TYPE;
        metatrafficMulticastLocatorList : LOCATOR_LIST_TYPE;
        defaultUnicastLocatorList       : LOCATOR_LIST_TYPE;
        defaultMulticastLocatorList     : LOCATOR_LIST_TYPE;
        leaseDuration                   : DURATION_TYPE;
        manualLivelinessCount           : std_logic_vector(CDR_LONG_WIDTH-1 downto 0);
        builtinEndpointQoS              : std_logic_vector(CDR_LONG_WIDTH-1 downto 0);
        availableBuiltinEndpoints       : std_logic_vector(CDR_LONG_WIDTH-1 downto 0);
        match                           : MATCH_TYPE;
        nr                              : natural;
    end record;
    
    constant DEFAULT_PARTICIPANT_DATA : PARTICIPANT_DATA_TYPE; -- Deferred to Pckage Body
    constant THIS_PARTICIPANT_DATA : PARTICIPANT_DATA_TYPE; -- Deferred to Pckage Body
    
    type ENDPOINT_DATA_TYPE is record
        littleEndian                                    : std_logic;
        participant                                     : PARTICIPANT_DATA_TYPE;
        entityId                                        : std_logic_vector(ENTITYID_WIDTH-1 downto 0);
        topic_name                                      : STRING_WORD_ARRAY_TYPE;
        type_name                                       : STRING_WORD_ARRAY_TYPE;
        durability                                      : std_logic_vector(CDR_ENUMERATION_WIDTH-1 downto 0);
        durability_service_cleanup_delay                : DURATION_TYPE;
        durability_service_history                      : std_logic_vector(CDR_ENUMERATION_WIDTH-1 downto 0);
        durability_service_history_depth                : std_logic_vector(CDR_LONG_WIDTH-1 downto 0);
        durability_service_max_samples                  : std_logic_vector(CDR_LONG_WIDTH-1 downto 0);
        durability_service_max_instances                : std_logic_vector(CDR_LONG_WIDTH-1 downto 0);
        durability_service_max_samples_per_instances    : std_logic_vector(CDR_LONG_WIDTH-1 downto 0);
        presentation                                    : std_logic_vector(CDR_ENUMERATION_WIDTH-1 downto 0);
        coherent_access                                 : std_logic_vector(CDR_BOOLEAN_WIDTH-1 downto 0);
        ordered_access                                  : std_logic_vector(CDR_BOOLEAN_WIDTH-1 downto 0);
        deadline                                        : DURATION_TYPE;
        latency_budget                                  : DURATION_TYPE;
        ownership                                       : std_logic_vector(CDR_ENUMERATION_WIDTH-1 downto 0);
        ownership_strength                              : std_logic_vector(CDR_LONG_WIDTH-1 downto 0);
        liveliness                                      : std_logic_vector(CDR_ENUMERATION_WIDTH-1 downto 0);
        leaseDuration                                   : DURATION_TYPE;
        time_based_filter                               : DURATION_TYPE;
        reliability                                     : std_logic_vector(CDR_ENUMERATION_WIDTH-1 downto 0);
        max_blocking_time                               : DURATION_TYPE;
        transportnpriority                              : std_logic_vector(CDR_LONG_WIDTH-1 downto 0);
        lifespan                                        : DURATION_TYPE;
        destination_order                               : std_logic_vector(CDR_ENUMERATION_WIDTH-1 downto 0);
        -- Represent Data as Strings (Ease of Use)
        user_data                                       : STRING_WORD_ARRAY_TYPE;
        topic_data                                      : STRING_WORD_ARRAY_TYPE;
        group_data                                      : STRING_WORD_ARRAY_TYPE;
        expectsInlineQoS                                : std_logic_vector(CDR_BOOLEAN_WIDTH-1 downto 0);
        unicastLocatorList                              : LOCATOR_LIST_TYPE;
        multicastLocatorList                            : LOCATOR_LIST_TYPE;
        max_size_serialized                             : std_logic_vector(CDR_LONG_WIDTH-1 downto 0);
        match                                           : MATCH_TYPE;
    end record;
    
    constant DEFAULT_ENDPOINT_DATA : ENDPOINT_DATA_TYPE; -- Deferred to Package Body
    type ENDPOINT_DATA_ARRAY_TYPE is array (natural range <>) of ENDPOINT_DATA_TYPE;
    
    function test_memory_match (A,B : TEST_MEMORY_TYPE) return boolean;
    function to_string (input : TEST_MEMORY_TYPE) return string;
    
    procedure gen_output_header(ref : in OUTPUT_HEADER_TYPE; output : inout TEST_PACKET_TYPE);
    procedure fix_output_packet(output : inout TEST_PACKET_TYPE);
    
    procedure gen_rtps_header( ref : in RTPS_HEADER_TYPE; output : inout TEST_PACKET_TYPE);
    
    procedure gen_rtps_submessage( ref : in RTPS_SUBMESSAGE_TYPE; output : inout TEST_PACKET_TYPE);
    procedure gen_rtps_handler_out(ref : in RTPS_SUBMESSAGE_TYPE; loc : in LOCATOR_TYPE; is_meta : in boolean; ts : in TIME_TYPE; src_guid : in GUIDPREFIX_TYPE; output : inout TEST_PACKET_TYPE);
    procedure gen_rtps_handler_out(ref : in RTPS_SUBMESSAGE_TYPE; participant : PARTICIPANT_DATA_TYPE; output : inout TEST_PACKET_TYPE);
    procedure gen_rtps_handler_out(ref : in RTPS_SUBMESSAGE_TYPE; endpoint : ENDPOINT_DATA_TYPE; output : inout TEST_PACKET_TYPE);
    
    procedure gen_participant_data( ref : in PARTICIPANT_DATA_TYPE; output : inout TEST_PACKET_TYPE);
    procedure gen_participant_data( ref : in PARTICIPANT_DATA_TYPE; output : inout TEST_PACKET_TYPE; pid : in std_logic_vector(PARAMETER_ID_WIDTH-1 downto 0); offset : in integer);
    function gen_participant_mem_frame (ref : PARTICIPANT_DATA_TYPE) return TEST_PARTICIPANT_MEMORY_FRAME_TYPE;
    
    procedure gen_endpoint_data( ref : in ENDPOINT_DATA_TYPE; output : inout TEST_PACKET_TYPE);
    procedure gen_endpoint_data( ref : in ENDPOINT_DATA_TYPE; output : inout TEST_PACKET_TYPE; pid : in std_logic_vector(PARAMETER_ID_WIDTH-1 downto 0); offset : in integer);
    procedure gen_endpoint_match_frame( ref : in ENDPOINT_DATA_TYPE; output : inout TEST_PACKET_TYPE);
    procedure gen_participant_match_frame( ref : in PARTICIPANT_DATA_TYPE; output : inout TEST_PACKET_TYPE);
    procedure gen_liveliness_update_frame( ref : in PARTICIPANT_DATA_TYPE; output : inout TEST_PACKET_TYPE);
    function gen_endpoint_array(readers : boolean) return ENDPOINT_DATA_ARRAY_TYPE;
    
    procedure gen_liveliness_assertion(participant : in PARTICIPANT_DATA_TYPE; manual : in boolean; extra_data : in TEST_PACKET_TYPE; output : inout TEST_PACKET_TYPE);
    procedure gen_liveliness_assertion(participant : in PARTICIPANT_DATA_TYPE; manual : in boolean; output : inout TEST_PACKET_TYPE);
    
    procedure gen_sentinel(output : inout TEST_PACKET_TYPE);
    procedure gen_parameter(pid : in std_logic_vector(PARAMETER_ID_WIDTH-1 downto 0); data : in TEST_PACKET_TYPE; output : inout TEST_PACKET_TYPE);
    
    procedure gen_rand_loc(RV : inout RandomPType; ret : out LOCATOR_TYPE);
    procedure gen_rand_entityid(RV : inout RandomPType; reader : boolean; ret : out std_logic_vector(ENTITYID_WIDTH-1 downto 0));
    function int(n : integer; width : natural) return std_logic_vector;
    function to_string1 (input : std_logic_vector) return string;
    
    -- NOTE: This assume a specific sending order (Multicast before Unicast)!
    function get_loc (ref : PARTICIPANT_DATA_TYPE; meta : boolean) return LOCATOR_TYPE;
    function get_loc (ref : ENDPOINT_DATA_TYPE) return LOCATOR_TYPE;
    
end package;

package body rtps_test_package is
    
    -- *UTILITY FUNCTIONS*
    procedure gen_rand_loc(RV : inout RandomPType; ret : out LOCATOR_TYPE) is
    begin
        ret                                     := EMPTY_LOCATOR;
        ret.kind                                := LOCATOR_KIND_UDPv4;
        ret.portn(UDP_PORT_WIDTH-1 downto 0)    := RV.RandSlv(UDP_PORT_WIDTH);
        ret.addr(IPv4_ADDRESS_WIDTH-1 downto 0) := RV.RandSlv(IPv4_ADDRESS_WIDTH);
    end procedure;
    
    procedure gen_rand_entityid(RV : inout RandomPType; reader : boolean; ret : out std_logic_vector(ENTITYID_WIDTH-1 downto 0)) is
    begin
        ret                     := RV.RandSlv(ENTITYID_WIDTH);
        ret(ENTITY_KIND_H_RANGE):= USER_DEFINED_ENTITY;
        if (reader) then
            ret(ENTITY_KIND_L_RANGE):= READER_NO_KEY;
        else
            ret(ENTITY_KIND_L_RANGE):= WRITER_NO_KEY;
        end if;
    end procedure;
    
    function int(n : integer; width : natural) return std_logic_vector is
    begin
        return std_logic_vector(to_signed(n, width));
    end function;
    
    -- *DEFERRED DEFINITIONS*
    
    constant DEFAULT_GUIDPREFIX : GUIDPREFIX_TYPE := (0 => x"da27cc3c", 1 => x"687ddcde", 2 => x"88bce3d1");
    
    constant DEFAULT_ENTITYID   : std_logic_vector(ENTITYID_WIDTH-1 downto 0) := x"b9cbad8d";
    
    constant EMPTY_TEST_PACKET : TEST_PACKET_TYPE := (length => 0, data => (others => (others => '0')), last => (others => '0'));
    
    constant EMPTY_LOCATOR : LOCATOR_TYPE := (
        kind        => LOCATOR_KIND_INVALID,
        portn       => (others => '0'),
        addr        => (others => '0')
    );
    
    constant EMPTY_LOCATOR_LIST : LOCATOR_LIST_TYPE := (
        numLocators => (others => '0'),
        locator     => (others => EMPTY_LOCATOR)
    );
    
    constant DEFAULT_SEQUENCENUMBER_SET : SEQUENCENUMBER_SET_TYPE := (
        base    => FIRST_SEQUENCENUMBER,
        numBits => (others => '0'),
        bitmap  => (others => '0')
    );
    
    constant DEFAULT_FRAGMENTNUMBER_SET : FRAGMENTNUMBER_SET_TYPE := (
        base    => int(1,CDR_LONG_WIDTH),
        numBits => (others => '0'),
        bitmap  => (others => '0')
    );
    
    constant DEST_LOC : DEST_LOCATOR_LIST_TYPE := (
        meta => (
            numLocators => int(4,CDR_LONG_WIDTH),
            locator     => (
                0 => (
                    kind    => LOCATOR_KIND_UDPv4,
                    portn   => (LOCATOR_PORT_WIDTH-1 downto UDP_PORT_WIDTH => '0') & META_IPv4_MULTICAST_PORT,
                    addr    => (LOCATOR_ADDR_WIDTH-1 downto IPv4_ADDRESS_WIDTH => '0') & DEFAULT_IPv4_META_ADDRESS
                ), 
                1 => (
                    kind    => LOCATOR_KIND_UDPv4,
                    portn   => (LOCATOR_PORT_WIDTH-1 downto UDP_PORT_WIDTH => '0') & META_IPv4_UNICAST_PORT,
                    addr    => (LOCATOR_ADDR_WIDTH-1 downto IPv4_ADDRESS_WIDTH => '0') & DEFAULT_IPv4_META_ADDRESS
                ),
                2 => (
                    kind    => LOCATOR_KIND_UDPv4,
                    portn   => (LOCATOR_PORT_WIDTH-1 downto UDP_PORT_WIDTH => '0') & META_IPv4_MULTICAST_PORT,
                    addr    => (LOCATOR_ADDR_WIDTH-1 downto IPv4_ADDRESS_WIDTH => '0') & DEFAULT_IPv4_ADDRESS
                ), 
                3 => (
                    kind    => LOCATOR_KIND_UDPv4,
                    portn   => (LOCATOR_PORT_WIDTH-1 downto UDP_PORT_WIDTH => '0') & META_IPv4_UNICAST_PORT,
                    addr    => (LOCATOR_ADDR_WIDTH-1 downto IPv4_ADDRESS_WIDTH => '0') & DEFAULT_IPv4_ADDRESS
                ),
                others => EMPTY_LOCATOR
            )
        ),
        user => (
            numLocators => int(2,CDR_LONG_WIDTH),
            locator     => (
                0 => (
                    kind    => LOCATOR_KIND_UDPv4,
                    portn   => (LOCATOR_PORT_WIDTH-1 downto UDP_PORT_WIDTH => '0') & USER_IPv4_MULTICAST_PORT,
                    addr    => (LOCATOR_ADDR_WIDTH-1 downto IPv4_ADDRESS_WIDTH => '0') & DEFAULT_IPv4_ADDRESS
                ), 
                1 => (
                    kind    => LOCATOR_KIND_UDPv4,
                    portn   => (LOCATOR_PORT_WIDTH-1 downto UDP_PORT_WIDTH => '0') & USER_IPv4_UNICAST_PORT,
                    addr    => (LOCATOR_ADDR_WIDTH-1 downto IPv4_ADDRESS_WIDTH => '0') & DEFAULT_IPv4_ADDRESS
                ),
                others => EMPTY_LOCATOR
            )
        )
    );
    
    constant DEFAULT_OUTPUT_HEADER : OUTPUT_HEADER_TYPE := (
        src     => EMPTY_LOCATOR,
        dest    => EMPTY_LOCATOR
    );
    
    constant DEFAULT_RTPS_HEADER : RTPS_HEADER_TYPE := (
        protocol    => PROTOCOL_RTPS,
        version     => PROTOCOLVERSION_2_4,
        vendorId    => VENDORID_UNKNOWN,
        guidPrefix  => DEFAULT_GUIDPREFIX
    );
    
    constant DEFAULT_RTPS_SUBMESSAGE : RTPS_SUBMESSAGE_TYPE := (
        submessageID            => (others => '0'),
        flags                   => (others => '0'),
        submessageLength        => (others => '1'),
        readerId                => ENTITYID_UNKNOWN,
        writerId                => ENTITYID_UNKNOWN,
        readerSNState           => DEFAULT_SEQUENCENUMBER_SET,
        count                   => int(1,COUNT_WIDTH),
        extraFlags              => (others => '0'),
        octetsToInlineQos       => int(16, SUBMESSAGE_LENGTH_WIDTH),
        writerSN                => FIRST_SEQUENCENUMBER,
        fragmentNumberState     => DEFAULT_FRAGMENTNUMBER_SET,
        data                    => EMPTY_TEST_PACKET,
        fragmentStartingNumber  => int(1,CDR_LONG_WIDTH),
        fragmentsInSubmessage   => int(1,CDR_SHORT_WIDTH),
        fragmentSize            => (others => '0'),
        sampleSize              => (others => '0'),
        gapStart                => FIRST_SEQUENCENUMBER,
        gapList                 => DEFAULT_SEQUENCENUMBER_SET,
        firstSN                 => FIRST_SEQUENCENUMBER,
        lastSN                  => (others => (others => '0')),
        lastFragmentNum         => int(1,CDR_LONG_WIDTH),
        guidPrefix              => DEFAULT_GUIDPREFIX,
        unicastLocatorList      => EMPTY_LOCATOR_LIST,
        multicastLocatorList    => EMPTY_LOCATOR_LIST,
        unicastLocator          => EMPTY_LOCATOR,
        multicastLocator        => EMPTY_LOCATOR,
        unused                  => (others => '0'),
        version                 => PROTOCOLVERSION_2_4,
        vendorId                => VENDORID_UNKNOWN,
        timestamp               => TIME_ZERO
    );
    
    constant DEFAULT_PARTICIPANT_DATA : PARTICIPANT_DATA_TYPE := (
        littleEndian                    => '0',
        domainId                        => DOMAIN_ID,
        domainTag                       => DOMAIN_TAG,
        protocolVersion                 => PROTOCOLVERSION_2_4,
        vendorId                        => VENDORID_UNKNOWN,
        guidPrefix                      => GUIDPREFIX_UNKNOWN,
        entityId                        => ENTITYID_PARTICIPANT,
        expectsInlineQoS                => (others => '0'),
        metatrafficUnicastLocatorList   => EMPTY_LOCATOR_LIST,
        metatrafficMulticastLocatorList => EMPTY_LOCATOR_LIST,
        defaultUnicastLocatorList       => EMPTY_LOCATOR_LIST, -- NOTE: Has to be initialized
        defaultMulticastLocatorList     => EMPTY_LOCATOR_LIST,
        leaseDuration                   => PARTICIPANT_LEASE_DURATION,
        manualLivelinessCount           => (others => '0'),
        builtinEndpointQoS              => (others => '0'),
        availableBuiltinEndpoints       => (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'),
        match                           => MATCH,
        nr                              => 0
    );
    
    constant THIS_PARTICIPANT_DATA : PARTICIPANT_DATA_TYPE := (
        littleEndian                    => '0',
        domainId                        => DOMAIN_ID,
        domainTag                       => DOMAIN_TAG,
        protocolVersion                 => PROTOCOLVERSION_2_4,
        vendorId                        => VENDORID_UNKNOWN,
        guidPrefix                      => GUIDPREFIX,
        entityId                        => ENTITYID_PARTICIPANT,
        expectsInlineQoS                => (others => '0'),
        metatrafficUnicastLocatorList   => (numLocators => int(1,CDR_LONG_WIDTH), locator => (0 => DEST_LOC.meta.locator(3), others => EMPTY_LOCATOR)),
        metatrafficMulticastLocatorList => (numLocators => int(1,CDR_LONG_WIDTH), locator => (0 => DEST_LOC.meta.locator(2), others => EMPTY_LOCATOR)),
        defaultUnicastLocatorList       => (numLocators => int(1,CDR_LONG_WIDTH), locator => (0 => DEST_LOC.user.locator(1), others => EMPTY_LOCATOR)),
        defaultMulticastLocatorList     => (numLocators => int(1,CDR_LONG_WIDTH), locator => (0 => DEST_LOC.user.locator(0), others => EMPTY_LOCATOR)),
        leaseDuration                   => PARTICIPANT_LEASE_DURATION,
        manualLivelinessCount           => (others => '0'),
        builtinEndpointQoS              => (others => '0'),
        availableBuiltinEndpoints       => (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', DISC_BUILTIN_ENDPOINT_PUBLICATIONS_ANNOUNCER => '1', DISC_BUILTIN_ENDPOINT_PUBLICATIONS_DETECTOR => '1', DISC_BUILTIN_ENDPOINT_SUBSCRIPTIONS_ANNOUNCER => '1', DISC_BUILTIN_ENDPOINT_SUBSCRIPTIONS_DETECTOR => '1', others => '0'),
        match                           => MATCH,
        nr                              => 0
    );
    
    constant DEFAULT_ENDPOINT_DATA : ENDPOINT_DATA_TYPE := (
        littleEndian                                    => '0',
        participant                                     => DEFAULT_PARTICIPANT_DATA,
        entityId                                        => DEFAULT_ENTITYID,
        topic_name                                      => EMPTY_STRING,
        type_name                                       => EMPTY_STRING,
        durability                                      => DEFAULT_DURABILITY_QOS,
        durability_service_cleanup_delay                => DEFAULT_DURABILITY_SERVICE_CLEANUP_DELAY,
        durability_service_history                      => DEFAULT_DURABILITY_SERVICE_HISTORY,
        durability_service_history_depth                => DEFAULT_DURABILITY_SERVICE_HISTORY_DEPTH,
        durability_service_max_samples                  => DEFAULT_DURABILITY_SERVICE_MAX_SAMPLES,
        durability_service_max_instances                => DEFAULT_DURABILITY_SERVICE_MAX_INSTANCES,
        durability_service_max_samples_per_instances    => DEFAULT_DURABILITY_SERVICE_MAX_SAMPLES_PER_INSTANCE,
        presentation                                    => DEFAULT_PRESENTATION_QOS,
        coherent_access                                 => (0 => boolean_to_std_logic(DEFAULT_COHERENT_ACCESS), others => '0'),
        ordered_access                                  => (0 => boolean_to_std_logic(DEFAULT_ORDERED_ACCESS), others => '0'),
        deadline                                        => DEFAULT_DEADLINE_QOS,
        latency_budget                                  => DEFAULT_LATENCY_BUDGET_QOS,
        ownership                                       => DEFAULT_OWNERSHIP_QOS,
        ownership_strength                              => DEFAULT_OWNERSHIP_STRENGTH_QOS,
        liveliness                                      => DEFAULT_LIVELINESS_QOS,
        leaseDuration                                   => DEFAULT_LEASE_DURATION,
        time_based_filter                               => DEFAULT_TIME_BASED_FILTER_QOS,
        reliability                                     => DEFAULT_RELIABILTY_QOS,
        max_blocking_time                               => DEFAULT_MAX_BLOCKING_TIME,
        transportnpriority                              => DEFAULT_TRANSPORT_PRIORITY_QOS,
        lifespan                                        => DEFAULT_LIFESPAN_QOS,
        destination_order                               => DEFAULT_DESTINATION_ORDER_QOS,
        user_data                                       => EMPTY_STRING,
        topic_data                                      => EMPTY_STRING,
        group_data                                      => EMPTY_STRING,
        expectsInlineQoS                                => (others => '0'),
        unicastLocatorList                              => EMPTY_LOCATOR_LIST,
        multicastLocatorList                            => EMPTY_LOCATOR_LIST,
        max_size_serialized                             => (others => '0'),
        match                                           => MATCH
    );
    
    -- *PACKAGE INTERNAL HELPER FUNCTIONS*
    
    procedure store_byte(in_off : in natural range 0 to 3; input : in std_logic_vector(WORD_WIDTH-1 downto 0); out_off : in natural range 0 to 3; output : inout TEST_PACKET_TYPE) is
    begin
        output.data(output.length)((BYTE_WIDTH*out_off)+BYTE_WIDTH-1 downto (BYTE_WIDTH*out_off)) := input((BYTE_WIDTH*in_off)+BYTE_WIDTH-1 downto (BYTE_WIDTH*in_off));
    end procedure;
    
    procedure gen_output_header(ref : in OUTPUT_HEADER_TYPE; output : inout TEST_PACKET_TYPE) is
    begin
        -- IPv4 Source Address
        output.data(output.length)  := ref.src.addr(IPv4_ADDRESS_WIDTH-1 downto 0);
        output.length               := output.length + 1;
        -- IPv4 Destination Address
        output.data(output.length)  := ref.dest.addr(IPv4_ADDRESS_WIDTH-1 downto 0);
        output.length               := output.length + 1;
        -- Source Port & Destination Port
        output.data(output.length)  := ref.src.portn(UDP_PORT_WIDTH-1 downto 0) & ref.dest.portn(UDP_PORT_WIDTH-1 downto 0);
        output.length               := output.length + 1;
        -- Packet Length
        output.data(output.length)  := (others => '0');
        output.length               := output.length + 1;
    end procedure;
    
    procedure fix_output_packet(output : inout TEST_PACKET_TYPE) is
    begin
        assert (output.length < 2**UDP_PORT_WIDTH) report "Exceeded maximum UDP Packet Size" severity error;
        -- Fix Packet Length
        output.data(3)  := int(output.length-4, WORD_WIDTH);
    end procedure;
    
    procedure gen_rtps_header( ref : in RTPS_HEADER_TYPE; output : inout TEST_PACKET_TYPE) is
    begin
        -- Protocol
        output.data(output.length) := ref.protocol;
        output.length       := output.length + 1;
        -- Protocol Version & Vendor ID
        output.data(output.length) := ref.version & ref.vendorId;
        output.length       := output.length + 1;
        -- GUID Prefix
        output.data(output.length) := ref.guidPrefix(0);
        output.length       := output.length + 1;
        output.data(output.length) := ref.guidPrefix(1);
        output.length       := output.length + 1;
        output.data(output.length) := ref.guidPrefix(2);
        output.length       := output.length + 1;
    end procedure;
    
    procedure write_sequence_ns(input : in SEQUENCENUMBER_SET_TYPE; littleEndian : in std_logic; output : inout TEST_PACKET_TYPE) is
    begin
        -- Sequence Number State (Base)
        output.data(output.length) := endian_swap(littleEndian, std_logic_vector(input.base(0)));
        output.length       := output.length + 1;
        output.data(output.length) := endian_swap(littleEndian, std_logic_vector(input.base(1)));
        output.length       := output.length + 1;
        -- Sequence Number State (NumBits)
        assert (unsigned(input.numBits) <= 256) report "SequenceNS.numbits is higher than 256." severity warning;
        output.data(output.length) := endian_swap(littleEndian, input.numBits);
        output.length       := output.length + 1;
        -- Sequence Number State (Bitmap)
        for i in 0 to round_div(to_integer(unsigned(input.numBits)),WORD_WIDTH)-1 loop
            if (i < MAX_BITMAP_WIDTH/WORD_WIDTH) then
                output.data(output.length) := endian_swap(littleEndian, input.bitmap(i*32 to i*32+31));
                output.length       := output.length + 1;
            end if;
        end loop;
    end procedure;
    
    procedure write_fragment_ns(input : in FRAGMENTNUMBER_SET_TYPE; littleEndian : in std_logic; output : inout TEST_PACKET_TYPE) is
    begin
        -- Sequence Number State (Base)
        output.data(output.length) := endian_swap(littleEndian, input.base);
        output.length       := output.length + 1;
        -- Sequence Number State (NumBits)
        assert (unsigned(input.numBits) <= 256) report "SequenceNS.numbits is higher than 256." severity warning;
        output.data(output.length) := endian_swap(littleEndian, input.numBits);
        output.length       := output.length + 1;
        -- Sequence Number State (Bitmap)
        for i in 0 to round_div(to_integer(unsigned(input.numBits)),WORD_WIDTH)-1 loop
            if (i < MAX_BITMAP_WIDTH/WORD_WIDTH) then
                output.data(output.length) := endian_swap(littleEndian, input.bitmap(i*32 to i*32+31));
                output.length       := output.length + 1;
            end if;
        end loop;
    end procedure;
    
    procedure write_locator_list(input : in LOCATOR_LIST_TYPE; littleEndian : in std_logic; output : inout TEST_PACKET_TYPE) is
    begin
        -- NumLocators
        output.data(output.length) := endian_swap(littleEndian, input.numLocators);
        output.length       := output.length + 1;
        for i in 0 to to_integer(unsigned(input.numLocators))-1 loop
            -- Kind
            output.data(output.length) := endian_swap(littleEndian, input.locator(i).kind);
            output.length       := output.length + 1;
            -- Port
            output.data(output.length) := endian_swap(littleEndian, input.locator(i).portn);
            output.length       := output.length + 1;
            -- Address
            output.data(output.length) := input.locator(i).addr(127 downto 96);
            output.length       := output.length + 1;
            output.data(output.length) := input.locator(i).addr(95 downto 64);
            output.length       := output.length + 1;
            output.data(output.length) := input.locator(i).addr(63 downto 32);
            output.length       := output.length + 1;
            output.data(output.length) := input.locator(i).addr(31 downto 0);
            output.length       := output.length + 1;
        end loop;
    end procedure;
    
    function is_valid_loc (ref : LOCATOR_TYPE) return boolean is
    begin
        if (ref.kind /= LOCATOR_KIND_UDPv4) then
            return FALSE;
        elsif (ref.addr(IPv4_ADDRESS_WIDTH-1 downto 0) = (IPv4_ADDRESS_WIDTH-1 downto 0 => '0')) then
            return FALSE;
        elsif (ref.portn(UDP_PORT_WIDTH-1 downto 0) = (UDP_PORT_WIDTH-1 downto 0 => '0')) then
            return FALSE;
        else
            return TRUE;
        end if;
    end function;
    
    function get_loc (ref : LOCATOR_LIST_TYPE) return LOCATOR_TYPE is
        variable ret : LOCATOR_TYPE := EMPTY_LOCATOR;
    begin
        for i in 0 to to_integer(unsigned(ref.numLocators))-1 loop
            if (is_valid_loc(ref.locator(i))) then
                ret := ref.locator(i);
            end if;
        end loop;
        
        return ret;
    end function;
    
    function get_loc (ref : PARTICIPANT_DATA_TYPE; meta : boolean) return LOCATOR_TYPE is
    begin
        if (meta) then
            if (is_valid_loc(get_loc(ref.metatrafficUnicastLocatorList))) then
                return get_loc(ref.metatrafficUnicastLocatorList);
            elsif (is_valid_loc(get_loc(ref.metatrafficMulticastLocatorList))) then
                return get_loc(ref.metatrafficMulticastLocatorList);
            else
                return DEST_LOC.meta.locator(0);
            end if;
        else
            if (is_valid_loc(get_loc(ref.defaultUnicastLocatorList))) then
                return get_loc(ref.defaultUnicastLocatorList);
            elsif (is_valid_loc(get_loc(ref.defaultMulticastLocatorList))) then
                return get_loc(ref.defaultMulticastLocatorList);
            else
                return EMPTY_LOCATOR;
            end if;
        end if;
    end function;
    
    function get_loc (ref : ENDPOINT_DATA_TYPE) return LOCATOR_TYPE is
    begin
        if (is_valid_loc(get_loc(ref.unicastLocatorList))) then
            return get_loc(ref.unicastLocatorList);
        elsif (is_valid_loc(get_loc(ref.multicastLocatorList))) then
            return get_loc(ref.multicastLocatorList);
        elsif (is_valid_loc(get_loc(ref.participant.defaultUnicastLocatorList))) then
            return get_loc(ref.participant.defaultUnicastLocatorList);
        elsif (is_valid_loc(get_loc(ref.participant.defaultMulticastLocatorList))) then
            return get_loc(ref.participant.defaultMulticastLocatorList);
        else
            assert(FALSE) report "Endpoint has no valid Locators" severity FAILURE;
            return EMPTY_LOCATOR;
        end if;
    end function;
    
    procedure gen_rtps_submessage( ref : in RTPS_SUBMESSAGE_TYPE; output : inout TEST_PACKET_TYPE) is
        variable start  : natural := 0;
        variable tmp    : natural := 0;
    begin
        -- *GENERAL*
        start               := output.length;
        -- Submessage ID & Submessage Flags & Submessage Length
        output.data(output.length) := ref.submessageID & ref.flags & endian_swap(ref.flags(SUBMESSAGE_ENDIAN_FLAG_POS), ref.submessageLength);
        output.length       := output.length + 1;
        
        -- *DATA/DATA_FRAG*
        if (ref.submessageID = SID_DATA or ref.submessageID = SID_DATA_FRAG) then
            -- Extra Flags & octetsToInlineQos
            output.data(output.length) := ref.extraFlags & endian_swap(ref.flags(SUBMESSAGE_ENDIAN_FLAG_POS), ref.octetsToInlineQos);
            output.length       := output.length + 1;
        end if;
        
        -- *ACKNACK/NACK_FRAG/HEARTBEAT/HEARTBEAT_FRAG/GAP/DATA/DATA_FRAG*
        if (ref.submessageID = SID_ACKNACK or ref.submessageID = SID_NACK_FRAG or ref.submessageID = SID_HEARTBEAT or ref.submessageID = SID_HEARTBEAT_FRAG or ref.submessageID = SID_GAP or ref.submessageID = SID_DATA or ref.submessageID = SID_DATA_FRAG) then
            -- Reader ID
            output.data(output.length) := ref.readerId;
            output.length       := output.length + 1;
            -- Writer ID
            output.data(output.length) := ref.writerId;
            output.length       := output.length + 1;
        end if;
        
        -- *NACK_FRAG/DATA/DATA_FRAG/HEARTBEAT_FRAG*
        if (ref.submessageID = SID_NACK_FRAG or ref.submessageID = SID_DATA or ref.submessageID = SID_DATA_FRAG or ref.submessageID = SID_HEARTBEAT_FRAG) then
            output.data(output.length) := endian_swap(ref.flags(SUBMESSAGE_ENDIAN_FLAG_POS), std_logic_vector(ref.writerSN(0)));
            output.length       := output.length + 1;
            output.data(output.length) := endian_swap(ref.flags(SUBMESSAGE_ENDIAN_FLAG_POS), std_logic_vector(ref.writerSN(1)));
            output.length       := output.length + 1;
        end if;
        
        -- *ACKNACK*
        if (ref.submessageID = SID_ACKNACK) then
            -- Reader State
            write_sequence_ns(ref.readerSNState, ref.flags(SUBMESSAGE_ENDIAN_FLAG_POS), output);
        -- *NACK_FRAG*
        elsif (ref.submessageID = SID_NACK_FRAG) then
            -- Fragment Number State
            write_fragment_ns(ref.fragmentNumberState, ref.flags(SUBMESSAGE_ENDIAN_FLAG_POS), output);
        -- *HEARTBEAT*
        elsif (ref.submessageID = SID_HEARTBEAT) then
            -- First Sequence Number
            output.data(output.length) := endian_swap(ref.flags(SUBMESSAGE_ENDIAN_FLAG_POS), std_logic_vector(ref.firstSN(0)));
            output.length       := output.length + 1;
            output.data(output.length) := endian_swap(ref.flags(SUBMESSAGE_ENDIAN_FLAG_POS), std_logic_vector(ref.firstSN(1)));
            output.length       := output.length + 1;
            -- Last Sequence Number
            output.data(output.length) := endian_swap(ref.flags(SUBMESSAGE_ENDIAN_FLAG_POS), std_logic_vector(ref.lastSN(0)));
            output.length       := output.length + 1;
            output.data(output.length) := endian_swap(ref.flags(SUBMESSAGE_ENDIAN_FLAG_POS), std_logic_vector(ref.lastSN(1)));
            output.length       := output.length + 1;
        -- *HEARTBEAT_FRAG*
        elsif (ref.submessageID = SID_HEARTBEAT_FRAG) then
            -- Last Fargment Number
            output.data(output.length) := endian_swap(ref.flags(SUBMESSAGE_ENDIAN_FLAG_POS), ref.lastFragmentNum);
            output.length       := output.length + 1;
        end if;
        
        -- *ACKNACK/NACK_FRAG/HEARTBEAT/HEARTBEAT_FRAG*
        if (ref.submessageID = SID_ACKNACK or ref.submessageID = SID_HEARTBEAT or ref.submessageID = SID_NACK_FRAG or ref.submessageID = SID_HEARTBEAT_FRAG) then
            output.data(output.length) := endian_swap(ref.flags(SUBMESSAGE_ENDIAN_FLAG_POS), ref.count);
            output.length       := output.length + 1;
        end if;
        
        -- *GAP*
        if (ref.submessageID = SID_GAP) then
            -- Gap Start
            output.data(output.length) := endian_swap(ref.flags(SUBMESSAGE_ENDIAN_FLAG_POS), std_logic_vector(ref.gapStart(0)));
            output.length       := output.length + 1;
            output.data(output.length) := endian_swap(ref.flags(SUBMESSAGE_ENDIAN_FLAG_POS), std_logic_vector(ref.gapStart(1)));
            output.length       := output.length + 1;
            -- Gap List
            write_sequence_ns(ref.gapList, ref.flags(SUBMESSAGE_ENDIAN_FLAG_POS), output);
        end if;
        
        -- *DATA_FRAG*
        if (ref.submessageID = SID_DATA_FRAG) then
            -- Fragment Number
            output.data(output.length) := endian_swap(ref.flags(SUBMESSAGE_ENDIAN_FLAG_POS), ref.fragmentStartingNumber);
            output.length       := output.length + 1;
            -- FragmentsinSubmessage & FragmentSize
            output.data(output.length) := endian_swap(ref.flags(SUBMESSAGE_ENDIAN_FLAG_POS), ref.fragmentsInSubmessage) & endian_swap(ref.flags(SUBMESSAGE_ENDIAN_FLAG_POS), ref.fragmentSize);
            output.length       := output.length + 1;
            -- Sample Size
            output.data(output.length) := endian_swap(ref.flags(SUBMESSAGE_ENDIAN_FLAG_POS), ref.sampleSize);
            output.length       := output.length + 1;
        end if;
        
        -- *DATA/DATA_FRAG*
        if (ref.submessageID = SID_DATA or ref.submessageID = SID_DATA_FRAG) then
            -- EXTRA HEADER
            tmp := to_integer(unsigned(ref.octetsToInlineQos)) - 16;
            if (tmp > 0) then
                for i in 0 to tmp-1 loop
                    store_byte((i+1) mod 4, x"DEADBEEF", (i+1) mod 4, output);
                    if (i mod 4 = 3) then
                        output.length       := output.length + 1;
                        output.data(output.length) := (others => '0');
                    end if;
                end loop;
            end if;
            -- DATA
            if (ref.data.length > 0) then
                for i in 0 to (ref.data.length*4)-1 loop
                    store_byte((i+tmp) mod 4, ref.data.data(i/4), i mod 4, output);
                    if ((i+tmp) mod 4 = 3) then
                        output.length       := output.length + 1;
                        output.data(output.length) := (others => '0');
                    end if;
                end loop;
            end if;
            -- Fix Alignement
            if ((tmp + (ref.data.length*4)) mod 4 /= 0) then
                output.length       := output.length + 1;
            end if;
        end if;
        
        -- *INFO_SRC*
        if (ref.submessageID = SID_INFO_SRC) then
            -- UNUSED
            output.data(output.length) := endian_swap(ref.flags(SUBMESSAGE_ENDIAN_FLAG_POS), ref.unused);
            output.length       := output.length + 1;
            -- Protocol Version & Vendor ID
            output.data(output.length) := ref.version & ref.vendorId;
            output.length       := output.length + 1;
        -- *INFO_REPLY*
        elsif (ref.submessageID = SID_INFO_REPLY) then
            write_locator_list(ref.unicastLocatorList, ref.flags(SUBMESSAGE_ENDIAN_FLAG_POS), output);
            if (ref.flags(SUBMESSAGE_MULTICAST_FLAG_POS) = '1') then
                write_locator_list(ref.multicastLocatorList, ref.flags(SUBMESSAGE_ENDIAN_FLAG_POS), output);
            end if;
        -- *INFO_REPLY_IP4*
        elsif (ref.submessageID = SID_INFO_REPLY_IP4) then
            -- Unicast Locator (Address)
            output.data(output.length) := endian_swap(ref.flags(SUBMESSAGE_ENDIAN_FLAG_POS), ref.unicastLocator.addr(CDR_LONG_WIDTH-1 downto 0));
            output.length       := output.length + 1;
            -- Unicast Locator (Port)
            output.data(output.length) := endian_swap(ref.flags(SUBMESSAGE_ENDIAN_FLAG_POS), ref.unicastLocator.portn);
            output.length       := output.length + 1;
            if (ref.flags(SUBMESSAGE_MULTICAST_FLAG_POS) = '1') then
                -- Multicast Locator (Address)
                output.data(output.length) := endian_swap(ref.flags(SUBMESSAGE_ENDIAN_FLAG_POS), ref.multicastLocator.addr(CDR_LONG_WIDTH-1 downto 0));
                output.length       := output.length + 1;
                -- Multicast Locator (Port)
                output.data(output.length) := endian_swap(ref.flags(SUBMESSAGE_ENDIAN_FLAG_POS), ref.multicastLocator.portn);
                output.length       := output.length + 1;
            end if;
        -- *INFO_TS*
        elsif (ref.submessageID = SID_INFO_TS) then
            -- Timestamp
            if (ref.flags(SUBMESSAGE_INVALIDATE_FLAG_POS) = '0') then
                output.data(output.length) := endian_swap(ref.flags(SUBMESSAGE_ENDIAN_FLAG_POS), std_logic_vector(ref.timestamp(0)));
                output.length       := output.length + 1;
                output.data(output.length) := endian_swap(ref.flags(SUBMESSAGE_ENDIAN_FLAG_POS), std_logic_vector(ref.timestamp(1)));
                output.length       := output.length + 1;
            end if;
        end if;
        
        -- *INFO_DEST/INFO_SRC*
        if (ref.submessageID = SID_INFO_DST or ref.submessageID = SID_INFO_SRC) then
            -- GUID Prefix
            output.data(output.length) := ref.guidPrefix(0);
            output.length       := output.length + 1;
            output.data(output.length) := ref.guidPrefix(1);
            output.length       := output.length + 1;
            output.data(output.length) := ref.guidPrefix(2);
            output.length       := output.length + 1;
        end if;
        
        -- *ACKNACK/NACK_FRAG/HEARTBEAT/HEARTBEAT_FRAG/GAP/DATA/DATA_FRAG*
        if (ref.submessageID = SID_ACKNACK or ref.submessageID = SID_NACK_FRAG or ref.submessageID = SID_HEARTBEAT or ref.submessageID = SID_HEARTBEAT_FRAG or ref.submessageID = SID_GAP or ref.submessageID = SID_DATA or ref.submessageID = SID_DATA_FRAG or ref.submessageID = SID_INFO_TS or ref.submessageID = SID_INFO_SRC or ref.submessageID = SID_INFO_DST or ref.submessageID = SID_INFO_REPLY or ref.submessageID =SID_INFO_REPLY_IP4) then
            -- Fix Submessage Length
            if (ref.submessageLength = (ref.submessageLength'range => '1')) then
                output.data(start)(SUBMESSAGE_LENGTH_WIDTH-1 downto 0) := endian_swap(ref.flags(SUBMESSAGE_ENDIAN_FLAG_POS), int((output.length-start-1)*4,SUBMESSAGE_LENGTH_WIDTH));
            -- Fix Packet Length
            else
                output.length := start + to_integer(unsigned(ref.submessageLength));
            end if;
        -- *PAD/UKNOWN*
        else
            -- Padding
            tmp := to_integer(unsigned(ref.submessageLength));
            assert (tmp mod 4 = 0) report "Padding Length has to be multiple of four" severity FAILURE;
            for i in 0 to (tmp/4)-1 loop
                output.data(output.length) := (others => '0');
                output.length       := output.length + 1;
            end loop;
        end if;
    end procedure;
    
    procedure gen_rtps_handler_out(ref : in RTPS_SUBMESSAGE_TYPE; loc : in LOCATOR_TYPE; is_meta : in boolean; ts : in TIME_TYPE; src_guid : in GUIDPREFIX_TYPE; output : inout TEST_PACKET_TYPE) is
    begin
        -- Opcode & Flags & Source Port
        output.data(output.length) := ref.submessageID & ref.flags & loc.portn(UDP_PORT_WIDTH-1 downto 0);
        output.length       := output.length + 1;
        -- Source Address
        output.data(output.length) := loc.addr(IPv4_ADDRESS_WIDTH-1 downto 0);
        output.length       := output.length + 1;
        -- Source GUID Prefix
        output.data(output.length) := src_guid(0);
        output.length       := output.length + 1;
        output.data(output.length) := src_guid(1);
        output.length       := output.length + 1;
        output.data(output.length) := src_guid(2);
        output.length       := output.length + 1;
        -- Source Entity ID
        if (ref.submessageID = SID_ACKNACK or ref.submessageID = SID_NACK_FRAG) then
            output.data(output.length) := ref.readerId;
            output.length       := output.length + 1;
        else
            output.data(output.length) := ref.writerId;
            output.length       := output.length + 1;
        end if;
        -- Destination Enity ID
        if (is_meta) then
            if (ref.submessageID = SID_ACKNACK or ref.submessageID = SID_NACK_FRAG) then
                output.data(output.length) := ref.writerId;
                output.length       := output.length + 1;
            else
                output.data(output.length) := ref.readerId;
                output.length       := output.length + 1;
            end if;
        end if;
        
        -- *DATA*
        if (ref.submessageID = SID_DATA) then
            -- Sequence Number
            output.data(output.length) := std_logic_vector(ref.writerSN(0));
            output.length       := output.length + 1;
            output.data(output.length) := std_logic_vector(ref.writerSN(1));
            output.length       := output.length + 1;
            -- Timestamp
            if (not is_meta) then
                output.data(output.length) := std_logic_vector(ts(0));
                output.length       := output.length + 1;
                output.data(output.length) := std_logic_vector(ts(1));
                output.length       := output.length + 1;
            end if;
            -- DATA PAYLOAD
            if (ref.data.length > 0) then
                for i in 0 to ref.data.length-1 loop
                    output.data(output.length) := ref.data.data(i);
                    output.length       := output.length + 1;
                end loop;
            end if;
        end if;
        
        -- *ACKNACK*
        if (ref.submessageID = SID_ACKNACK) then
            -- Reader State
            write_sequence_ns(ref.readerSNState, '0', output);
        -- *HEARTBEAT*
        elsif (ref.submessageID = SID_HEARTBEAT) then
            -- First Sequence Number
            output.data(output.length) := std_logic_vector(ref.firstSN(0));
            output.length       := output.length + 1;
            output.data(output.length) := std_logic_vector(ref.firstSN(1));
            output.length       := output.length + 1;
            -- Last Sequence Number
            output.data(output.length) := std_logic_vector(ref.lastSN(0));
            output.length       := output.length + 1;
            output.data(output.length) := std_logic_vector(ref.lastSN(1));
            output.length       := output.length + 1;
        end if;
        
        -- *ACKNACK/HEARTBEAT*
        if (ref.submessageID = SID_ACKNACK or ref.submessageID = SID_HEARTBEAT) then
            output.data(output.length) := ref.count;
            output.length       := output.length + 1;
        end if;
        
        -- *GAP*
        if (ref.submessageID = SID_GAP) then
            -- Gap Start
            output.data(output.length) := std_logic_vector(ref.gapStart(0));
            output.length       := output.length + 1;
            output.data(output.length) := std_logic_vector(ref.gapStart(1));
            output.length       := output.length + 1;
            -- Gap List
            write_sequence_ns(ref.gapList, '0', output);
            -- UNUSED
            output.data(output.length)  := (others => '0');
            output.length               := output.length + 1;
        end if;
        
        -- Mark Last Word
        output.last(output.length-1)    := '1';
    end procedure;
    
    procedure gen_rtps_handler_out(ref : in RTPS_SUBMESSAGE_TYPE; participant : PARTICIPANT_DATA_TYPE; output : inout TEST_PACKET_TYPE) is
    begin
        gen_rtps_handler_out(ref, get_loc(participant, TRUE), TRUE, TIME_INVALID, participant.guidPrefix, output);
    end procedure;
    
    procedure gen_rtps_handler_out(ref : in RTPS_SUBMESSAGE_TYPE; endpoint : ENDPOINT_DATA_TYPE; output : inout TEST_PACKET_TYPE) is
    begin
        gen_rtps_handler_out(ref, get_loc(endpoint.participant, TRUE), TRUE, TIME_INVALID, endpoint.participant.guidPrefix, output);
    end procedure;
    
    procedure gen_liveliness_assertion(participant : in PARTICIPANT_DATA_TYPE; manual : in boolean; extra_data : in TEST_PACKET_TYPE; output : inout TEST_PACKET_TYPE) is
    begin
        -- Representation Identifier & Representation Options
        output.data(output.length)  := CDR_BE & x"0000";
        output.length               := output.length + 1;
        -- GUID Prefix
        output.data(output.length)  := participant.guidPrefix(0);
        output.length               := output.length + 1;
        output.data(output.length)  := participant.guidPrefix(1);
        output.length               := output.length + 1;
        output.data(output.length)  := participant.guidPrefix(2);
        output.length               := output.length + 1;
        -- Participant Message Kind
        if (manual) then
            output.data(output.length)  := PARTICIPANT_MESSAGE_DATA_KIND_MANUAL_LIVELINESS_UPDATE;
        else
            output.data(output.length)  := PARTICIPANT_MESSAGE_DATA_KIND_AUTOMATIC_LIVELINESS_UPDATE;
        end if;
        output.length   := output.length + 1;
        -- DATA Length
        output.data(output.length)  := int(extra_data.length, WORD_WIDTH);
        output.length               := output.length + 1;
        for i in 0 to extra_data.length-1 loop
            output.data(output.length)  := extra_data.data(i);
            output.length               := output.length + 1;
        end loop;
    end procedure;
    
    procedure gen_liveliness_assertion(participant : in PARTICIPANT_DATA_TYPE; manual : in boolean; output : inout TEST_PACKET_TYPE) is
    begin
        gen_liveliness_assertion(participant, manual, EMPTY_TEST_PACKET, output);
    end procedure;
    
    procedure gen_participant_data( ref : in PARTICIPANT_DATA_TYPE; output : inout TEST_PACKET_TYPE; pid : in std_logic_vector(PARAMETER_ID_WIDTH-1 downto 0); offset : in integer) is
        variable tmp : natural := 0;
    begin
        -- Representation Identifier & Representation Options
        if (ref.littleEndian = '0') then
            output.data(output.length) := PL_CDR_BE & x"0000";
        else
            output.data(output.length) := PL_CDR_LE & x"0000";
        end if;
        output.length       := output.length + 1;
        -- GUID
        if (pid = PID_PARTICIPANT_GUID) then
            assert (16+(offset*4) >= 0) report "Parameter Length < 0" severity FAILURE;
            output.data(output.length) := PID_PARTICIPANT_GUID & endian_swap(ref.littleEndian, int(16+(offset*4), PARAMETER_LENGTH_WIDTH));
        else
            output.data(output.length) := PID_PARTICIPANT_GUID & endian_swap(ref.littleEndian, int(16, PARAMETER_LENGTH_WIDTH));
        end if;
        output.length       := output.length + 1;
        output.data(output.length) := ref.guidPrefix(0);
        output.length       := output.length + 1;
        output.data(output.length) := ref.guidPrefix(1);
        output.length       := output.length + 1;
        output.data(output.length) := ref.guidPrefix(2);
        output.length       := output.length + 1;
        output.data(output.length) := ref.entityId;
        output.length       := output.length + 1;
        if (pid = PID_PARTICIPANT_GUID) then
            output.length       := output.length + offset;
        end if;
        -- DOMAIN ID
        if (pid = PID_DOMAIN_ID) then
            assert (4+(offset*4) >= 0) report "Parameter Length < 0" severity FAILURE;
            output.data(output.length) := PID_DOMAIN_ID & endian_swap(ref.littleEndian, int(4+(offset*4), PARAMETER_LENGTH_WIDTH));
        else
            output.data(output.length) := PID_DOMAIN_ID & endian_swap(ref.littleEndian, int(4, PARAMETER_LENGTH_WIDTH));
        end if;
        output.length       := output.length + 1;
        output.data(output.length) := endian_swap(ref.littleEndian, ref.domainId);
        output.length       := output.length + 1;
        if (pid = PID_DOMAIN_ID) then
            output.length       := output.length + offset;
        end if;
        -- DOMAIN TAG
        if (ref.domainTag /= DEFAULT_DOMAIN_TAG or pid = PID_DOMAIN_TAG) then
            tmp                 := string_len(ref.domainTag);
            if (pid = PID_DOMAIN_TAG) then
                assert (((round_div(tmp,4)+1)*4)+(offset*4) >= 0) report "Parameter Length < 0" severity FAILURE;
                output.data(output.length) := PID_DOMAIN_TAG & endian_swap(ref.littleEndian, int(((round_div(tmp,4)+1)*4)+(offset*4), PARAMETER_LENGTH_WIDTH));
            else
                output.data(output.length) := PID_DOMAIN_TAG & endian_swap(ref.littleEndian, int((round_div(tmp,4)+1)*4, PARAMETER_LENGTH_WIDTH));
            end if;
            output.length       := output.length + 1;
            output.data(output.length) := endian_swap(ref.littleEndian, int(tmp, CDR_LONG_WIDTH));
            output.length       := output.length + 1;
            for i in 0 to round_div(tmp,4)-1 loop
                output.data(output.length) := ref.domainTag(i);
                output.length       := output.length + 1;
            end loop;
            if (pid = PID_DOMAIN_TAG) then
                output.length       := output.length + offset;
            end if;
        end if;
        -- PROTOCOL VERSION
        if (pid = PID_PROTOCOL_VERSION) then
            assert (4+(offset*4) >= 0) report "Parameter Length < 0" severity FAILURE;
            output.data(output.length) := PID_PROTOCOL_VERSION & endian_swap(ref.littleEndian, int(4+(offset*4), PARAMETER_LENGTH_WIDTH));
        else
            output.data(output.length) := PID_PROTOCOL_VERSION & endian_swap(ref.littleEndian, int(4, PARAMETER_LENGTH_WIDTH));
        end if;
        output.length       := output.length + 1;
        output.data(output.length)                 := (others => '0');
        output.data(output.length)(31 downto 16)   := ref.protocolVersion;
        output.length       := output.length + 1;
        if (pid = PID_PROTOCOL_VERSION) then
            output.length       := output.length + offset;
        end if;
        -- VENDORID
        if (pid = PID_VENDORID) then
            assert (4+(offset*4) >= 0) report "Parameter Length < 0" severity FAILURE;
            output.data(output.length) := PID_VENDORID & endian_swap(ref.littleEndian, int(4+(offset*4),PARAMETER_LENGTH_WIDTH));
        else
            output.data(output.length) := PID_VENDORID & endian_swap(ref.littleEndian, int(4,PARAMETER_LENGTH_WIDTH));
        end if;
        output.length       := output.length + 1;
        output.data(output.length)                 := (others => '0');
        output.data(output.length)(31 downto 16)   := ref.vendorId;
        output.length                       := output.length + 1;
        if (pid = PID_VENDORID) then
            output.length       := output.length + offset;
        end if;
        -- EXPECTS IN-LINE QOS
        if (ref.expectsInlineQoS(0) /= DEFAULT_EXPECTS_INLINE_QOS or pid = PID_EXPECTS_INLINE_QOS) then
            if (pid = PID_EXPECTS_INLINE_QOS) then
                assert (4+(offset*4) >= 0) report "Parameter Length < 0" severity FAILURE;
                output.data(output.length) := PID_EXPECTS_INLINE_QOS & endian_swap(ref.littleEndian, int(4+(offset*4),PARAMETER_LENGTH_WIDTH));
            else
                output.data(output.length) := PID_EXPECTS_INLINE_QOS & endian_swap(ref.littleEndian, int(4,PARAMETER_LENGTH_WIDTH));
            end if;
            output.length       := output.length + 1;
            output.data(output.length)                 := (others => '0');
            output.data(output.length)(31 downto 24)   := ref.expectsInlineQoS; -- 1 Byte, No endian swap
            output.length                       := output.length + 1;
            if (pid = PID_EXPECTS_INLINE_QOS) then
                output.length       := output.length + offset;
            end if;
        end if;
        -- METATRAFFIC MULTICAST LOCATOR
        if (ref.metatrafficMulticastLocatorList.numLocators /= (ref.metatrafficMulticastLocatorList.numLocators'range => '0')) then
            tmp                 := to_integer(unsigned(ref.metatrafficMulticastLocatorList.numLocators));
            for i in 0 to tmp-1 loop
                if (pid = PID_METATRAFFIC_MULTICAST_LOCATOR) then
                    assert (24+(offset*4) >= 0) report "Parameter Length < 0" severity FAILURE;
                    output.data(output.length) := PID_METATRAFFIC_MULTICAST_LOCATOR & endian_swap(ref.littleEndian, int(24+(offset*4),PARAMETER_LENGTH_WIDTH));
                else
                    output.data(output.length) := PID_METATRAFFIC_MULTICAST_LOCATOR & endian_swap(ref.littleEndian, int(24,PARAMETER_LENGTH_WIDTH));
                end if;
                output.length       := output.length + 1;            
                output.data(output.length) := endian_swap(ref.littleEndian, ref.metatrafficMulticastLocatorList.locator(i).kind);
                output.length       := output.length + 1;
                output.data(output.length) := endian_swap(ref.littleEndian, ref.metatrafficMulticastLocatorList.locator(i).portn);
                output.length       := output.length + 1;
                output.data(output.length) := ref.metatrafficMulticastLocatorList.locator(i).addr(127 downto 96);
                output.length       := output.length + 1;
                output.data(output.length) := ref.metatrafficMulticastLocatorList.locator(i).addr(95 downto 64);
                output.length       := output.length + 1;
                output.data(output.length) := ref.metatrafficMulticastLocatorList.locator(i).addr(63 downto 32);
                output.length       := output.length + 1;
                output.data(output.length) := ref.metatrafficMulticastLocatorList.locator(i).addr(31 downto 0);
                output.length       := output.length + 1;
                if (pid = PID_METATRAFFIC_MULTICAST_LOCATOR) then
                    output.length       := output.length + offset;
                end if;
            end loop;
        end if;
        -- METATRAFFIC UNICAST LOCATOR
        if (ref.metatrafficUnicastLocatorList.numLocators /= (ref.metatrafficUnicastLocatorList.numLocators'range => '0')) then
            tmp                 := to_integer(unsigned(ref.metatrafficUnicastLocatorList.numLocators));
            for i in 0 to tmp-1 loop
                if (pid = PID_METATRAFFIC_UNICAST_LOCATOR) then
                    assert (24+(offset*4) >= 0) report "Parameter Length < 0" severity FAILURE;
                    output.data(output.length) := PID_METATRAFFIC_UNICAST_LOCATOR & endian_swap(ref.littleEndian, int(24+(offset*4),PARAMETER_LENGTH_WIDTH));
                else
                    output.data(output.length) := PID_METATRAFFIC_UNICAST_LOCATOR & endian_swap(ref.littleEndian, int(24,PARAMETER_LENGTH_WIDTH));
                end if;
                output.length       := output.length + 1;            
                output.data(output.length) := endian_swap(ref.littleEndian, ref.metatrafficUnicastLocatorList.locator(i).kind);
                output.length       := output.length + 1;
                output.data(output.length) := endian_swap(ref.littleEndian, ref.metatrafficUnicastLocatorList.locator(i).portn);
                output.length       := output.length + 1;
                output.data(output.length) := ref.metatrafficUnicastLocatorList.locator(i).addr(127 downto 96);
                output.length       := output.length + 1;
                output.data(output.length) := ref.metatrafficUnicastLocatorList.locator(i).addr(95 downto 64);
                output.length       := output.length + 1;
                output.data(output.length) := ref.metatrafficUnicastLocatorList.locator(i).addr(63 downto 32);
                output.length       := output.length + 1;
                output.data(output.length) := ref.metatrafficUnicastLocatorList.locator(i).addr(31 downto 0);
                output.length       := output.length + 1;
                if (pid = PID_METATRAFFIC_UNICAST_LOCATOR) then
                    output.length       := output.length + offset;
                end if;
            end loop;
        end if;
        -- DEFAULT MULTICAST LOCATOR
        if (ref.defaultMulticastLocatorList.numLocators /= (ref.defaultMulticastLocatorList.numLocators'range => '0')) then
            tmp                 := to_integer(unsigned(ref.defaultMulticastLocatorList.numLocators));
            for i in 0 to tmp-1 loop
                if (pid = PID_DEFAULT_MULTICAST_LOCATOR) then
                    assert (24+(offset*4) >= 0) report "Parameter Length < 0" severity FAILURE;
                    output.data(output.length) := PID_DEFAULT_MULTICAST_LOCATOR & endian_swap(ref.littleEndian, int(24+(offset*4),PARAMETER_LENGTH_WIDTH));
                else
                    output.data(output.length) := PID_DEFAULT_MULTICAST_LOCATOR & endian_swap(ref.littleEndian, int(24,PARAMETER_LENGTH_WIDTH));
                end if;
                output.length       := output.length + 1;
                output.data(output.length) := endian_swap(ref.littleEndian, ref.defaultMulticastLocatorList.locator(i).kind);
                output.length       := output.length + 1;
                output.data(output.length) := endian_swap(ref.littleEndian, ref.defaultMulticastLocatorList.locator(i).portn);
                output.length       := output.length + 1;
                output.data(output.length) := ref.defaultMulticastLocatorList.locator(i).addr(127 downto 96);
                output.length       := output.length + 1;
                output.data(output.length) := ref.defaultMulticastLocatorList.locator(i).addr(95 downto 64);
                output.length       := output.length + 1;
                output.data(output.length) := ref.defaultMulticastLocatorList.locator(i).addr(63 downto 32);
                output.length       := output.length + 1;
                output.data(output.length) := ref.defaultMulticastLocatorList.locator(i).addr(31 downto 0);
                output.length       := output.length + 1;
                if (pid = PID_DEFAULT_MULTICAST_LOCATOR) then
                    output.length       := output.length + offset;
                end if;
            end loop;
        end if;
        -- DEFAULT UNICAST LOCATOR
        assert (unsigned(ref.defaultUnicastLocatorList.numLocators) > 0) report "PARTICIPANT_DATA: Participant needs to have at least one Default Unicast Locator." severity error;
        if (ref.defaultUnicastLocatorList.numLocators /= (ref.defaultUnicastLocatorList.numLocators'range => '0')) then
            tmp                 := to_integer(unsigned(ref.defaultUnicastLocatorList.numLocators));
            for i in 0 to tmp-1 loop
                if (pid = PID_DEFAULT_UNICAST_LOCATOR) then
                    assert (24+(offset*4) >= 0) report "Parameter Length < 0" severity FAILURE;
                    output.data(output.length) := PID_DEFAULT_UNICAST_LOCATOR & endian_swap(ref.littleEndian, int(24+(offset*4),PARAMETER_LENGTH_WIDTH));
                else
                    output.data(output.length) := PID_DEFAULT_UNICAST_LOCATOR & endian_swap(ref.littleEndian, int(24,PARAMETER_LENGTH_WIDTH));
                end if;
                output.length       := output.length + 1;            
                output.data(output.length) := endian_swap(ref.littleEndian, ref.defaultUnicastLocatorList.locator(i).kind);
                output.length       := output.length + 1;
                output.data(output.length) := endian_swap(ref.littleEndian, ref.defaultUnicastLocatorList.locator(i).portn);
                output.length       := output.length + 1;
                output.data(output.length) := ref.defaultUnicastLocatorList.locator(i).addr(127 downto 96);
                output.length       := output.length + 1;
                output.data(output.length) := ref.defaultUnicastLocatorList.locator(i).addr(95 downto 64);
                output.length       := output.length + 1;
                output.data(output.length) := ref.defaultUnicastLocatorList.locator(i).addr(63 downto 32);
                output.length       := output.length + 1;
                output.data(output.length) := ref.defaultUnicastLocatorList.locator(i).addr(31 downto 0);
                output.length       := output.length + 1;
                if (pid = PID_DEFAULT_UNICAST_LOCATOR) then
                    output.length       := output.length + offset;
                end if;
            end loop;
        end if;
        -- LEASE DURATION
        if (ref.leaseDuration /= DEFAULT_PARTICIPANT_LEASE_DURATION or pid = PID_PARTICIPANT_LEASE_DURATION) then
            if (pid = PID_PARTICIPANT_LEASE_DURATION) then
                assert (8+(offset*4) >= 0) report "Parameter Length < 0" severity FAILURE;
                output.data(output.length) := PID_PARTICIPANT_LEASE_DURATION & endian_swap(ref.littleEndian, int(8+(offset*4),PARAMETER_LENGTH_WIDTH));
            else
                output.data(output.length) := PID_PARTICIPANT_LEASE_DURATION & endian_swap(ref.littleEndian, int(8,PARAMETER_LENGTH_WIDTH));
            end if;
            output.length       := output.length + 1;
            output.data(output.length) := endian_swap(ref.littleEndian, std_logic_vector(ref.leaseDuration(0)));
            output.length       := output.length + 1;
            output.data(output.length) := endian_swap(ref.littleEndian, std_logic_vector(ref.leaseDuration(1)));
            output.length       := output.length + 1;
            if (pid = PID_PARTICIPANT_LEASE_DURATION) then
                output.length       := output.length + offset;
            end if;
        end if;
        -- AVAILABLE ENDPOINTS
        if (pid = PID_BUILTIN_ENDPOINT_SET) then
            assert (4+(offset*4) >= 0) report "Parameter Length < 0" severity FAILURE;
            output.data(output.length) := PID_BUILTIN_ENDPOINT_SET & endian_swap(ref.littleEndian, int(4+(offset*4),PARAMETER_LENGTH_WIDTH));
        else
            output.data(output.length) := PID_BUILTIN_ENDPOINT_SET & endian_swap(ref.littleEndian, int(4,PARAMETER_LENGTH_WIDTH));
        end if;
        output.length       := output.length + 1;
        output.data(output.length) := endian_swap(ref.littleEndian, ref.availableBuiltinEndpoints);
        output.length       := output.length + 1;
        if (pid = PID_BUILTIN_ENDPOINT_SET) then
            output.length       := output.length + offset;
        end if;
        -- BUILTIN ENDPOINT QOS
        if (ref.builtinEndpointQoS /= DEFAULT_BUILTIN_ENDPOINT_QOS or pid = PID_BUILTIN_ENDPOINT_QOS) then
            if (pid = PID_BUILTIN_ENDPOINT_QOS) then
                assert (4+(offset*4) >= 0) report "Parameter Length < 0" severity FAILURE;
                output.data(output.length) := PID_BUILTIN_ENDPOINT_QOS & endian_swap(ref.littleEndian, int(4+(offset*4),PARAMETER_LENGTH_WIDTH));
            else
                output.data(output.length) := PID_BUILTIN_ENDPOINT_QOS & endian_swap(ref.littleEndian, int(4,PARAMETER_LENGTH_WIDTH));
            end if;
            output.length       := output.length + 1;
            output.data(output.length) := endian_swap(ref.littleEndian, ref.builtinEndpointQoS);
            output.length       := output.length + 1;
            if (pid = PID_BUILTIN_ENDPOINT_QOS) then
                output.length       := output.length + offset;
            end if;
        end if;
        -- MANUAL LIVELINESS COUNT
        if (pid = PID_PARTICIPANT_MANUAL_LIVELINESS_COUNT) then
            assert (4+(offset*4) >= 0) report "Parameter Length < 0" severity FAILURE;
            output.data(output.length) := PID_PARTICIPANT_MANUAL_LIVELINESS_COUNT & endian_swap(ref.littleEndian, int(4+(offset*4),PARAMETER_LENGTH_WIDTH));
        else
            output.data(output.length) := PID_PARTICIPANT_MANUAL_LIVELINESS_COUNT & endian_swap(ref.littleEndian, int(4,PARAMETER_LENGTH_WIDTH));
        end if;
        output.length       := output.length + 1;
        output.data(output.length) := endian_swap(ref.littleEndian, ref.manualLivelinessCount);
        output.length       := output.length + 1;
        if (pid = PID_PARTICIPANT_MANUAL_LIVELINESS_COUNT) then
            output.length       := output.length + offset;
        end if;
    end procedure;
    
    procedure gen_participant_data( ref : in PARTICIPANT_DATA_TYPE; output : inout TEST_PACKET_TYPE) is
    begin
        gen_participant_data(ref, output, PID_PAD, 0);
    end procedure;
    
    function gen_participant_mem_frame (ref : PARTICIPANT_DATA_TYPE) return TEST_PARTICIPANT_MEMORY_FRAME_TYPE is
        variable ret        : TEST_PARTICIPANT_MEMORY_FRAME_TYPE;
        variable start      : natural;
        variable meta_loc   : LOCATOR_TYPE;
        variable user_loc   : LOCATOR_TYPE;
    begin
        ret := (others => (addr => 0, data => (others => '0')));
        
        -- Calculate Start Address
        start := PARTICIPANT_FRAME_SIZE * ref.nr;
        
        -- Fetch relevant Locators
        meta_loc    := get_loc (ref, TRUE);
        user_loc    := get_loc (ref, FALSE);
        
        for i in 0 to PARTICIPANT_FRAME_SIZE-1 loop
            ret(i).addr := start + i;
            case (i) is
                -- GUID Prefix 1/3
                when 0 =>
                    if (ref.match = MATCH) then
                        ret(i).data := ref.guidPrefix(0);
                    else
                        ret(i).data := (others => '0');
                    end if;
                -- GUID Prefix 2/3
                when 1 =>
                    if (ref.match = MATCH) then
                        ret(i).data := ref.guidPrefix(1);
                    else
                        ret(i).data := (others => '0');
                    end if;
                -- GUID Prefix 3/3
                when 2 =>
                    if (ref.match = MATCH) then
                        ret(i).data := ref.guidPrefix(2);
                    else
                        ret(i).data := (others => '0');
                    end if;
                -- METATRAFFIC IPv4 Address
                when 3 =>
                    if (ref.match = MATCH) then
                        ret(i).data := meta_loc.addr(IPv4_ADDRESS_WIDTH-1 downto 0);
                    else
                        ret(i).data := (others => '-');
                    end if;
                -- DEFAULT IPv4 Address
                when 4 =>
                    if (ref.match = MATCH) then
                        ret(i).data := user_loc.addr(IPv4_ADDRESS_WIDTH-1 downto 0);
                    else
                        ret(i).data := (others => '-');
                    end if;
                -- METATRAFFIC & DEFAULT UDP Port
                when 5 =>
                    if (ref.match = MATCH) then
                        ret(i).data := meta_loc.portn(UDP_PORT_WIDTH-1 downto 0) & user_loc.portn(UDP_PORT_WIDTH-1 downto 0);
                    else
                        ret(i).data := (others => '-');
                    end if;
                -- Lease Duration 1/2
                when 8 =>
                    if (ref.match = MATCH) then
                        ret(i).data := std_logic_vector(ref.leaseDuration(0));
                    else
                        ret(i).data := (others => '-');
                    end if;
                -- Lease Duration 2/2
                when 9 =>
                    if (ref.match = MATCH) then
                        ret(i).data := std_logic_vector(ref.leaseDuration(1));
                    else
                        ret(i).data := (others => '-');
                    end if;
                -- EXTRA FLAGS
                when 12 =>
                    if (ref.match = MATCH) then
                        ret(i).data := (0 => ref.expectsInlineQoS(0), others => '-');
                    else
                        ret(i).data := (others => '-');
                    end if;
                -- Other Fields Ignored
                when others =>
                    ret(i).data := (others => '-');
            end case;
        end loop;
        return ret;
    end function;
    
    procedure gen_participant_match_frame( ref : in PARTICIPANT_DATA_TYPE; output : inout TEST_PACKET_TYPE) is
    begin
        -- OPCODE
        case (ref.match) is
            when MATCH =>
                report "No Endpoint Frame generated on Participant match" severity WARNING;
                return;
            when UNMATCH =>
                output.data(output.length)  := OPCODE_PARTICIPANT_UNMATCH;
                output.length               := output.length + 1;
            when others => 
                null;
        end case;
        -- GUID Prefix
        output.data(output.length)  := ref.guidPrefix(0);
        output.length               := output.length + 1;
        output.data(output.length)  := ref.guidPrefix(1);
        output.length               := output.length + 1;
        output.data(output.length)  := ref.guidPrefix(2);
        output.length               := output.length + 1;
        
        -- Mark Last Word
        output.last(output.length-1)    := '1';
    end procedure;
    
    procedure gen_liveliness_update_frame( ref : in PARTICIPANT_DATA_TYPE; output : inout TEST_PACKET_TYPE) is
    begin
        -- OPCODE
        output.data(output.length)  := OPCODE_LIVELINESS_UPDATE;
        output.length               := output.length + 1;
        -- GUID Prefix
        output.data(output.length)  := ref.guidPrefix(0);
        output.length               := output.length + 1;
        output.data(output.length)  := ref.guidPrefix(1);
        output.length               := output.length + 1;
        output.data(output.length)  := ref.guidPrefix(2);
        output.length               := output.length + 1;
        
        -- Mark Last Word
        output.last(output.length-1)    := '1';
    end procedure;
    
    -- The Arguments "pid" and "offset" can be used to modify the parameter generation. More specifically, the length of the parameter denoted by "pid" is modified by "offset" 4-Byte words.
    -- Also setting "pid" forces the respective parameter to be writen out even if it is equal to the default value.
    procedure gen_endpoint_data( ref : in ENDPOINT_DATA_TYPE; output : inout TEST_PACKET_TYPE; pid : in std_logic_vector(PARAMETER_ID_WIDTH-1 downto 0); offset : in integer) is
        variable tmp : natural := 0;
    begin
        -- Representation Identifier & Representation Options
        if (ref.littleEndian = '0') then
            output.data(output.length) := PL_CDR_BE & x"0000";
        else
            output.data(output.length) := PL_CDR_LE & x"0000";
        end if;
        output.length       := output.length + 1;
        -- GUID
        if (pid = PID_ENDPOINT_GUID) then
            assert (16+(offset*4) >= 0) report "Parameter Length < 0" severity FAILURE;
            output.data(output.length) := PID_ENDPOINT_GUID & endian_swap(ref.littleEndian, int(16+(offset*4),PARAMETER_LENGTH_WIDTH));
        else
            output.data(output.length) := PID_ENDPOINT_GUID & endian_swap(ref.littleEndian, int(16,PARAMETER_LENGTH_WIDTH));
        end if;
        output.length       := output.length + 1;
        output.data(output.length) := ref.participant.guidPrefix(0);
        output.length       := output.length + 1;
        output.data(output.length) := ref.participant.guidPrefix(1);
        output.length       := output.length + 1;
        output.data(output.length) := ref.participant.guidPrefix(2);
        output.length       := output.length + 1;
        output.data(output.length) := ref.entityId;
        output.length       := output.length + 1;
        if (pid = PID_ENDPOINT_GUID) then
            output.length       := output.length + offset;
        end if;
        -- EXPECTS IN-LINE QOS
        if (ref.expectsInlineQoS(0) /= DEFAULT_EXPECTS_INLINE_QOS or pid = PID_EXPECTS_INLINE_QOS) then
            if (pid = PID_EXPECTS_INLINE_QOS) then
                assert (4+(offset*4) >= 0) report "Parameter Length < 0" severity FAILURE;
                output.data(output.length) := PID_EXPECTS_INLINE_QOS & endian_swap(ref.littleEndian, int(4+(offset*4),PARAMETER_LENGTH_WIDTH));
            else
                output.data(output.length) := PID_EXPECTS_INLINE_QOS & endian_swap(ref.littleEndian, int(4,PARAMETER_LENGTH_WIDTH));
            end if;
            output.length       := output.length + 1;
            output.data(output.length)                 := (others => '0');
            output.data(output.length)(31 downto 24)   := ref.expectsInlineQoS; -- 1 Byte, No endian swap
            output.length                       := output.length + 1;
            if (pid = PID_EXPECTS_INLINE_QOS) then
                output.length       := output.length + offset;
            end if;
        end if;
        -- TOPIC NAME
        tmp := string_len(ref.topic_name);
        if (pid = PID_TOPIC_NAME) then
            assert (((round_div(tmp,4)+1)*4)+(offset*4) >= 0) report "Parameter Length < 0" severity FAILURE;
            output.data(output.length) := PID_TOPIC_NAME & endian_swap(ref.littleEndian, int(((round_div(tmp,4)+1)*4)+(offset*4),PARAMETER_LENGTH_WIDTH));
        else
            output.data(output.length) := PID_TOPIC_NAME & endian_swap(ref.littleEndian, int((round_div(tmp,4)+1)*4,PARAMETER_LENGTH_WIDTH));
        end if;
        output.length       := output.length + 1;
        output.data(output.length) := endian_swap(ref.littleEndian, int(tmp, CDR_LONG_WIDTH));
        output.length       := output.length + 1;
        for i in 0 to round_div(tmp,4)-1 loop
            output.data(output.length) := ref.topic_name(i);
            output.length       := output.length + 1;
        end loop;
        if (pid = PID_TOPIC_NAME) then
            output.length       := output.length + offset;
        end if;
        -- TYPE NAME
        tmp := string_len(ref.type_name);
        if (pid = PID_TYPE_NAME) then
            assert (((round_div(tmp,4)+1)*4)+(offset*4) >= 0) report "Parameter Length < 0" severity FAILURE;
            output.data(output.length) := PID_TYPE_NAME & endian_swap(ref.littleEndian, int(((round_div(tmp,4)+1)*4)+(offset*4),PARAMETER_LENGTH_WIDTH));
        else
            output.data(output.length) := PID_TYPE_NAME & endian_swap(ref.littleEndian, int((round_div(tmp,4)+1)*4,PARAMETER_LENGTH_WIDTH));
        end if;
        output.length       := output.length + 1;
        output.data(output.length) := endian_swap(ref.littleEndian, int(tmp,CDR_LONG_WIDTH));
        output.length       := output.length + 1;
        for i in 0 to round_div(tmp,4)-1 loop
            output.data(output.length) := ref.type_name(i);
            output.length       := output.length + 1;
        end loop;
        if (pid = PID_TYPE_NAME) then
            output.length       := output.length + offset;
        end if;
        -- DURABILITY
        if (ref.durability /= DEFAULT_DURABILITY_QOS or pid = PID_DURABILITY) then
            if (pid = PID_DURABILITY) then
                assert (4+(offset*4) >= 0) report "Parameter Length < 0" severity FAILURE;
                output.data(output.length) := PID_DURABILITY & endian_swap(ref.littleEndian, int(4+(offset*4),PARAMETER_LENGTH_WIDTH));
            else
                output.data(output.length) := PID_DURABILITY & endian_swap(ref.littleEndian, int(4,PARAMETER_LENGTH_WIDTH));
            end if;
            output.length       := output.length + 1;
            output.data(output.length) := endian_swap(ref.littleEndian, ref.durability);
            output.length       := output.length + 1;
            if (pid = PID_DURABILITY) then
                output.length       := output.length + offset;
            end if;
        end if;
        -- DURABILITY SERVICE
        if (ref.durability_service_cleanup_delay /= DEFAULT_DURABILITY_SERVICE_CLEANUP_DELAY or ref.durability_service_history /= DEFAULT_DURABILITY_SERVICE_HISTORY or 
        ref.durability_service_history_depth /= DEFAULT_DURABILITY_SERVICE_HISTORY_DEPTH or ref.durability_service_max_samples /= DEFAULT_DURABILITY_SERVICE_MAX_SAMPLES or 
        ref.durability_service_max_instances /= DEFAULT_DURABILITY_SERVICE_MAX_INSTANCES or ref.durability_service_max_samples_per_instances /= DEFAULT_DURABILITY_SERVICE_MAX_SAMPLES_PER_INSTANCE or pid = PID_DURABILITY_SERVICE) then
            if (pid = PID_DURABILITY_SERVICE) then
                assert (28+(offset*4) >= 0) report "Parameter Length < 0" severity FAILURE;
                output.data(output.length) := PID_DURABILITY_SERVICE & endian_swap(ref.littleEndian, int(28+(offset*4),PARAMETER_LENGTH_WIDTH));
            else
                output.data(output.length) := PID_DURABILITY_SERVICE & endian_swap(ref.littleEndian, int(28,PARAMETER_LENGTH_WIDTH));
            end if;
            output.length       := output.length + 1;
            output.data(output.length) := endian_swap(ref.littleEndian, std_logic_vector(ref.durability_service_cleanup_delay(0)));
            output.length       := output.length + 1;
            output.data(output.length) := endian_swap(ref.littleEndian, std_logic_vector(ref.durability_service_cleanup_delay(1)));
            output.length       := output.length + 1;
            output.data(output.length) := endian_swap(ref.littleEndian, ref.durability_service_history);
            output.length       := output.length + 1;
            output.data(output.length) := endian_swap(ref.littleEndian, ref.durability_service_history_depth);
            output.length       := output.length + 1;
            output.data(output.length) := endian_swap(ref.littleEndian, ref.durability_service_max_samples);
            output.length       := output.length + 1;
            output.data(output.length) := endian_swap(ref.littleEndian, ref.durability_service_max_instances);
            output.length       := output.length + 1;
            output.data(output.length) := endian_swap(ref.littleEndian, ref.durability_service_max_samples_per_instances);
            output.length       := output.length + 1;
            if (pid = PID_DURABILITY_SERVICE) then
                output.length       := output.length + offset;
            end if;
        end if;
        -- PRESENTATION
        if (ref.presentation /= DEFAULT_PRESENTATION_QOS or ref.coherent_access(0) /= boolean_to_std_logic(DEFAULT_COHERENT_ACCESS) or ref.ordered_access(0) /= boolean_to_std_logic(DEFAULT_ORDERED_ACCESS) or pid = PID_PRESENTATION) then
            if (pid = PID_PRESENTATION) then
                assert (8+(offset*4) >= 0) report "Parameter Length < 0" severity FAILURE;
                output.data(output.length) := PID_PRESENTATION & endian_swap(ref.littleEndian, int(8+(offset*4),PARAMETER_LENGTH_WIDTH));
            else
                output.data(output.length) := PID_PRESENTATION & endian_swap(ref.littleEndian, int(8,PARAMETER_LENGTH_WIDTH));
            end if;
            output.length       := output.length + 1;
            output.data(output.length) := endian_swap(ref.littleEndian, ref.presentation);
            output.length       := output.length + 1;
            output.data(output.length) := endian_swap(ref.littleEndian, ref.coherent_access) & endian_swap(ref.littleEndian, ref.ordered_access) & (0 to 15 => '0');
            output.length       := output.length + 1;
            if (pid = PID_PRESENTATION) then
                output.length       := output.length + offset;
            end if;
        end if;
        -- DEADLINE
        if (ref.deadline /= DEFAULT_DEADLINE_QOS or pid = PID_DEADLINE) then
            if (pid = PID_DEADLINE) then
                assert (8+(offset*4) >= 0) report "Parameter Length < 0" severity FAILURE;
                output.data(output.length) := PID_DEADLINE & endian_swap(ref.littleEndian, int(8+(offset*4),PARAMETER_LENGTH_WIDTH));
            else
                output.data(output.length) := PID_DEADLINE & endian_swap(ref.littleEndian, int(8,PARAMETER_LENGTH_WIDTH));
            end if;
            output.length       := output.length + 1;
            output.data(output.length) := endian_swap(ref.littleEndian, std_logic_vector(ref.deadline(0)));
            output.length       := output.length + 1;
            output.data(output.length) := endian_swap(ref.littleEndian, std_logic_vector(ref.deadline(1)));
            output.length       := output.length + 1;
            if (pid = PID_DEADLINE) then
                output.length       := output.length + offset;
            end if;
        end if;
        -- LATENCY BUDGET
        if (ref.latency_budget /= DEFAULT_LATENCY_BUDGET_QOS or pid = PID_LATENCY_BUDGET) then
            if (pid = PID_LATENCY_BUDGET) then
                assert (8+(offset*4) >= 0) report "Parameter Length < 0" severity FAILURE;
                output.data(output.length) := PID_LATENCY_BUDGET & endian_swap(ref.littleEndian, int(8+(offset*4),PARAMETER_LENGTH_WIDTH));
            else
                output.data(output.length) := PID_LATENCY_BUDGET & endian_swap(ref.littleEndian, int(8,PARAMETER_LENGTH_WIDTH));
            end if;
            output.length       := output.length + 1;
            output.data(output.length) := endian_swap(ref.littleEndian, std_logic_vector(ref.latency_budget(0)));
            output.length       := output.length + 1;
            output.data(output.length) := endian_swap(ref.littleEndian, std_logic_vector(ref.latency_budget(1)));
            output.length       := output.length + 1;
            if (pid = PID_LATENCY_BUDGET) then
                output.length       := output.length + offset;
            end if;
        end if;
        -- OWNERSHIP
        if (ref.ownership /= DEFAULT_OWNERSHIP_QOS or pid = PID_OWNERSHIP) then
            if (pid = PID_OWNERSHIP) then
                assert (4+(offset*4) >= 0) report "Parameter Length < 0" severity FAILURE;
                output.data(output.length) := PID_OWNERSHIP & endian_swap(ref.littleEndian, int(4+(offset*4),PARAMETER_LENGTH_WIDTH));
            else
                output.data(output.length) := PID_OWNERSHIP & endian_swap(ref.littleEndian, int(4,PARAMETER_LENGTH_WIDTH));
            end if;
            output.length       := output.length + 1;
            output.data(output.length) := endian_swap(ref.littleEndian, ref.ownership);
            output.length       := output.length + 1;
            if (pid = PID_OWNERSHIP) then
                output.length       := output.length + offset;
            end if;
        end if;
        -- OWNERSHIP STRENGTH
        if (ref.ownership_strength /= DEFAULT_OWNERSHIP_STRENGTH_QOS or pid = PID_OWNERSHIP_STRENGTH) then
            if (pid = PID_OWNERSHIP_STRENGTH) then
                assert (4+(offset*4) >= 0) report "Parameter Length < 0" severity FAILURE;
                output.data(output.length) := PID_OWNERSHIP_STRENGTH & endian_swap(ref.littleEndian, int(4+(offset*4),PARAMETER_LENGTH_WIDTH));
            else
                output.data(output.length) := PID_OWNERSHIP_STRENGTH & endian_swap(ref.littleEndian, int(4,PARAMETER_LENGTH_WIDTH));
            end if;
            output.length       := output.length + 1;
            output.data(output.length) := endian_swap(ref.littleEndian, ref.ownership_strength);
            output.length       := output.length + 1;
            if (pid = PID_OWNERSHIP_STRENGTH) then
                output.length       := output.length + offset;
            end if;
        end if;
        -- LIVELINESS
        if (ref.liveliness /= DEFAULT_LIVELINESS_QOS or ref.leaseDuration /= DEFAULT_LEASE_DURATION or pid = PID_LIVELINESS) then
            if (pid = PID_LIVELINESS) then
                assert (12+(offset*4) >= 0) report "Parameter Length < 0" severity FAILURE;
                output.data(output.length) := PID_LIVELINESS & endian_swap(ref.littleEndian, int(12+(offset*4),PARAMETER_LENGTH_WIDTH));
            else
                output.data(output.length) := PID_LIVELINESS & endian_swap(ref.littleEndian, int(12,PARAMETER_LENGTH_WIDTH));
            end if;
            output.length       := output.length + 1;
            output.data(output.length) := endian_swap(ref.littleEndian, ref.liveliness);
            output.length       := output.length + 1;
            output.data(output.length) := endian_swap(ref.littleEndian, std_logic_vector(ref.leaseDuration(0)));
            output.length       := output.length + 1;
            output.data(output.length) := endian_swap(ref.littleEndian, std_logic_vector(ref.leaseDuration(1)));
            output.length       := output.length + 1;
            if (pid = PID_LIVELINESS) then
                output.length       := output.length + offset;
            end if;
        end if;
        -- TIME BASED FILTER
        if (ref.time_based_filter /= DEFAULT_TIME_BASED_FILTER_QOS or pid = PID_TIME_BASED_FILTER) then
            if (pid = PID_TIME_BASED_FILTER) then
                assert (8+(offset*4) >= 0) report "Parameter Length < 0" severity FAILURE;
                output.data(output.length) := PID_TIME_BASED_FILTER & endian_swap(ref.littleEndian, int(8+(offset*4),PARAMETER_LENGTH_WIDTH));
            else
                output.data(output.length) := PID_TIME_BASED_FILTER & endian_swap(ref.littleEndian, int(8,PARAMETER_LENGTH_WIDTH));
            end if;
            output.length       := output.length + 1;
            output.data(output.length) := endian_swap(ref.littleEndian, std_logic_vector(ref.time_based_filter(0)));
            output.length       := output.length + 1;
            output.data(output.length) := endian_swap(ref.littleEndian, std_logic_vector(ref.time_based_filter(1)));
            output.length       := output.length + 1;
            if (pid = PID_TIME_BASED_FILTER) then
                output.length       := output.length + offset;
            end if;
        end if;
        -- RELIABILITY
        if (ref.reliability /= DEFAULT_RELIABILTY_QOS or ref.max_blocking_time /= DEFAULT_MAX_BLOCKING_TIME or pid = PID_RELIABILITY) then
            if (pid = PID_RELIABILITY) then
                assert (12+(offset*4) >= 0) report "Parameter Length < 0" severity FAILURE;
                output.data(output.length) := PID_RELIABILITY & endian_swap(ref.littleEndian, int(12+(offset*4),PARAMETER_LENGTH_WIDTH));
            else
                output.data(output.length) := PID_RELIABILITY & endian_swap(ref.littleEndian, int(12,PARAMETER_LENGTH_WIDTH));
            end if;
            output.length       := output.length + 1;
            output.data(output.length) := endian_swap(ref.littleEndian, ref.reliability);
            output.length       := output.length + 1;
            output.data(output.length) := endian_swap(ref.littleEndian, std_logic_vector(ref.max_blocking_time(0)));
            output.length       := output.length + 1;
            output.data(output.length) := endian_swap(ref.littleEndian, std_logic_vector(ref.max_blocking_time(1)));
            output.length       := output.length + 1;
            if (pid = PID_RELIABILITY) then
                output.length       := output.length + offset;
            end if;
        end if;
        -- TRANSPORT PRIORITY
        if (ref.transportnpriority /= DEFAULT_TRANSPORT_PRIORITY_QOS or pid = PID_TRANSPORT_PRIORITY) then
            if (pid = PID_TRANSPORT_PRIORITY) then
                assert (4+(offset*4) >= 0) report "Parameter Length < 0" severity FAILURE;
                output.data(output.length) := PID_TRANSPORT_PRIORITY & endian_swap(ref.littleEndian, int(4+(offset*4),PARAMETER_LENGTH_WIDTH));
            else
                output.data(output.length) := PID_TRANSPORT_PRIORITY & endian_swap(ref.littleEndian, int(4,PARAMETER_LENGTH_WIDTH));
            end if;
            output.length       := output.length + 1;
            output.data(output.length) := endian_swap(ref.littleEndian, ref.transportnpriority);
            output.length       := output.length + 1;
            if (pid = PID_TRANSPORT_PRIORITY) then
                output.length       := output.length + offset;
            end if;
        end if;
        -- LIFESPAN
        if (ref.lifespan /= DEFAULT_LIFESPAN_QOS or pid = PID_LIFESPAN) then
            if (pid = PID_LIFESPAN) then
                assert (8+(offset*4) >= 0) report "Parameter Length < 0" severity FAILURE;
                output.data(output.length) := PID_LIFESPAN & endian_swap(ref.littleEndian, int(8+(offset*4),PARAMETER_LENGTH_WIDTH));
            else
                output.data(output.length) := PID_LIFESPAN & endian_swap(ref.littleEndian, int(8,PARAMETER_LENGTH_WIDTH));
            end if;
            output.length       := output.length + 1;
            output.data(output.length) := endian_swap(ref.littleEndian, std_logic_vector(ref.lifespan(0)));
            output.length       := output.length + 1;
            output.data(output.length) := endian_swap(ref.littleEndian, std_logic_vector(ref.lifespan(1)));
            output.length       := output.length + 1;
            if (pid = PID_LIFESPAN) then
                output.length       := output.length + offset;
            end if;
        end if;
        -- DESTINATION ORDER
        if (ref.destination_order /= DEFAULT_DESTINATION_ORDER_QOS or pid = PID_DESTINATION_ORDER) then
            if (pid = PID_DESTINATION_ORDER) then
                assert (4+(offset*4) >= 0) report "Parameter Length < 0" severity FAILURE;
                output.data(output.length) := PID_DESTINATION_ORDER & endian_swap(ref.littleEndian, int(4+(offset*4),PARAMETER_LENGTH_WIDTH));
            else
                output.data(output.length) := PID_DESTINATION_ORDER & endian_swap(ref.littleEndian, int(4,PARAMETER_LENGTH_WIDTH));
            end if;
            output.length       := output.length + 1;
            output.data(output.length) := endian_swap(ref.littleEndian, ref.destination_order);
            output.length       := output.length + 1;
            if (pid = PID_DESTINATION_ORDER) then
                output.length       := output.length + offset;
            end if;
        end if;
        -- MULTICAST LOCATORS
        if (unsigned(ref.multicastLocatorList.numLocators) > 0) then
            tmp := to_integer(unsigned(ref.multicastLocatorList.numLocators));
            for i in 0 to tmp-1 loop
                if (pid = PID_MULTICAST_LOCATOR) then
                    assert (24+(offset*4) >= 0) report "Parameter Length < 0" severity FAILURE;
                    output.data(output.length) := PID_MULTICAST_LOCATOR & endian_swap(ref.littleEndian, int(24+(offset*4),PARAMETER_LENGTH_WIDTH));
                else
                    output.data(output.length) := PID_MULTICAST_LOCATOR & endian_swap(ref.littleEndian, int(24,PARAMETER_LENGTH_WIDTH));
                end if;
                output.length       := output.length + 1;
                output.data(output.length) := endian_swap(ref.littleEndian, ref.multicastLocatorList.locator(i).kind);
                output.length       := output.length + 1;
                output.data(output.length) := endian_swap(ref.littleEndian, ref.multicastLocatorList.locator(i).portn);
                output.length       := output.length + 1;
                output.data(output.length) := ref.multicastLocatorList.locator(i).addr(127 downto 96);
                output.length       := output.length + 1;
                output.data(output.length) := ref.multicastLocatorList.locator(i).addr(95 downto 64);
                output.length       := output.length + 1;
                output.data(output.length) := ref.multicastLocatorList.locator(i).addr(63 downto 32);
                output.length       := output.length + 1;
                output.data(output.length) := ref.multicastLocatorList.locator(i).addr(31 downto 0);
                output.length       := output.length + 1;
                if (pid = PID_MULTICAST_LOCATOR) then
                    output.length       := output.length + offset;
                end if;
            end loop;
        end if;
        -- UNICAST LOCATORS
        if (unsigned(ref.unicastLocatorList.numLocators) > 0) then
            tmp := to_integer(unsigned(ref.unicastLocatorList.numLocators));
            for i in 0 to tmp-1 loop
                if (pid = PID_UNICAST_LOCATOR) then
                    assert (24+(offset*4) >= 0) report "Parameter Length < 0" severity FAILURE;
                    output.data(output.length) := PID_UNICAST_LOCATOR & endian_swap(ref.littleEndian, int(24+(offset*4),PARAMETER_LENGTH_WIDTH));
                else
                    output.data(output.length) := PID_UNICAST_LOCATOR & endian_swap(ref.littleEndian, int(24,PARAMETER_LENGTH_WIDTH));
                end if;
                output.length       := output.length + 1;
                output.data(output.length) := endian_swap(ref.littleEndian, ref.unicastLocatorList.locator(i).kind);
                output.length       := output.length + 1;
                output.data(output.length) := endian_swap(ref.littleEndian, ref.unicastLocatorList.locator(i).portn);
                output.length       := output.length + 1;
                output.data(output.length) := ref.unicastLocatorList.locator(i).addr(127 downto 96);
                output.length       := output.length + 1;
                output.data(output.length) := ref.unicastLocatorList.locator(i).addr(95 downto 64);
                output.length       := output.length + 1;
                output.data(output.length) := ref.unicastLocatorList.locator(i).addr(63 downto 32);
                output.length       := output.length + 1;
                output.data(output.length) := ref.unicastLocatorList.locator(i).addr(31 downto 0);
                output.length       := output.length + 1;
                if (pid = PID_UNICAST_LOCATOR) then
                    output.length       := output.length + offset;
                end if;
            end loop;
        end if;
        -- USER DATA
        tmp := string_len(ref.user_data);
        if (tmp > 1) then
            output.data(output.length) := PID_USER_DATA & endian_swap(ref.littleEndian, int((round_div(tmp,4)+1)*4,PARAMETER_LENGTH_WIDTH));
            output.length       := output.length + 1;
            output.data(output.length) := endian_swap(ref.littleEndian, int(tmp, 32));
            output.length       := output.length + 1;
            for i in 0 to round_div(tmp,4)-1 loop
                output.data(output.length) := ref.user_data(i);
                output.length       := output.length + 1;
            end loop;
        end if;
        -- TOPIC DATA
        tmp := string_len(ref.topic_data);
        if (tmp > 1) then
            output.data(output.length) := PID_TOPIC_DATA & endian_swap(ref.littleEndian, int((round_div(tmp,4)+1)*4,PARAMETER_LENGTH_WIDTH));
            output.length       := output.length + 1;
            output.data(output.length) := endian_swap(ref.littleEndian, int(tmp,32));
            output.length       := output.length + 1;
            for i in 0 to round_div(tmp,4)-1 loop
                output.data(output.length) := ref.topic_data(i);
                output.length       := output.length + 1;
            end loop;
        end if;
        -- GROUP DATA
        tmp := string_len(ref.group_data);
        if (tmp > 1) then
            output.data(output.length) := PID_GROUP_DATA & endian_swap(ref.littleEndian, int((round_div(tmp,4)+1)*4,PARAMETER_LENGTH_WIDTH));
            output.length       := output.length + 1;
            output.data(output.length) := endian_swap(ref.littleEndian, int(tmp,32));
            output.length       := output.length + 1;
            for i in 0 to round_div(tmp,4)-1 loop
                output.data(output.length) := ref.group_data(i);
                output.length       := output.length + 1;
            end loop;
        end if;
        -- MAX SIZE SERIALIZED
        -- NOTE: PID_DATA_MAX_SIZE_SERIALIZED has no default value, but we use the value zero as default for not sending the parameter
        if (unsigned(ref.max_size_serialized) /= 0) then
            if (pid = PID_DATA_MAX_SIZE_SERIALIZED) then
                assert (4+(offset*4) >= 0) report "Parameter Length < 0" severity FAILURE;
                output.data(output.length) := PID_DATA_MAX_SIZE_SERIALIZED & endian_swap(ref.littleEndian, int(4+(offset*4),PARAMETER_LENGTH_WIDTH));
            else
                output.data(output.length) := PID_DATA_MAX_SIZE_SERIALIZED & endian_swap(ref.littleEndian, int(4,PARAMETER_LENGTH_WIDTH));
            end if;
            output.length       := output.length + 1;
            output.data(output.length) := endian_swap(ref.littleEndian, ref.max_size_serialized);
            output.length       := output.length + 1;
            if (pid = PID_DATA_MAX_SIZE_SERIALIZED) then
                output.length       := output.length + offset;
            end if;
        end if;
    end procedure;
    
    procedure gen_endpoint_data( ref : in ENDPOINT_DATA_TYPE; output : inout TEST_PACKET_TYPE) is
    begin
        gen_endpoint_data(ref, output, PID_PAD, 0);
    end procedure;
    
    procedure gen_endpoint_match_frame( ref : in ENDPOINT_DATA_TYPE; output : inout TEST_PACKET_TYPE) is
        variable loc   : LOCATOR_TYPE;
    begin
        -- Fetch relevant Locator
        loc := get_loc(ref);
        
        -- OPCODE
        case (ref.match) is
            when MATCH =>
                output.data(output.length)  := OPCODE_ENDPOINT_MATCH;
                output.length               := output.length + 1;
            when UNMATCH =>
                output.data(output.length)  := OPCODE_ENDPOINT_UNMATCH;
                output.length               := output.length + 1;
            when others => 
                null;
        end case;
        -- GUID Prefix
        output.data(output.length)  := ref.participant.guidPrefix(0);
        output.length               := output.length + 1;
        output.data(output.length)  := ref.participant.guidPrefix(1);
        output.length               := output.length + 1;
        output.data(output.length)  := ref.participant.guidPrefix(2);
        output.length               := output.length + 1;
        -- ENTITY ID
        output.data(output.length)  := ref.entityId;
        output.length               := output.length + 1;
        if (ref.match = MATCH) then
            -- IPv4 Address
            output.data(output.length)  := loc.addr(IPv4_ADDRESS_WIDTH-1 downto 0);
            output.length               := output.length + 1;
            -- Port & expectsInlineQoS
            output.data(output.length)  := (others => '0');
            output.data(output.length)(WORD_WIDTH-1 downto UDP_PORT_WIDTH)  := loc.portn(UDP_PORT_WIDTH-1 downto 0);
            output.data(output.length)(0)  := ref.expectsInlineQoS(0);
            output.length               := output.length + 1;
        end if;
        
        -- Mark Last Word
        output.last(output.length-1)    := '1';
    end procedure;
    
    procedure gen_sentinel(output : inout TEST_PACKET_TYPE) is
    begin
        output.data(output.length)  := PID_SENTINEL & (PARAMETER_LENGTH_WIDTH-1 downto 0 => '0');
        output.length               := output.length + 1;
    end procedure;
    
    procedure gen_parameter(pid : in std_logic_vector(PARAMETER_ID_WIDTH-1 downto 0); data : in TEST_PACKET_TYPE; output : inout TEST_PACKET_TYPE) is
    begin
        -- PARAMETER HEADER
        output.data(output.length) := pid & int(data.length*4,PARAMETER_LENGTH_WIDTH);
        output.length       := output.length + 1;
        -- DATA
        for i in 0 to data.length-1 loop
            output.data(output.length)  := data.data(i);
            output.length               := output.length + 1;
        end loop;
    end procedure;
    
    function test_memory_match (A,B : TEST_MEMORY_TYPE) return boolean is
    begin
        if (A'length /= B'length) then
            return FALSE;
        end if;
        
        for i in 0 to A'length loop
            if (A(i).addr /= B(i).addr or (A(i).data ?/= B(i).data) = '1') then
                return FALSE;
            end if;
        end loop;
        return TRUE;
    end function;
    
    function gen_endpoint_array(readers : boolean) return ENDPOINT_DATA_ARRAY_TYPE is
        variable ret_readers : ENDPOINT_DATA_ARRAY_TYPE(0 to NUM_READERS-1) := (others => DEFAULT_ENDPOINT_DATA);
        variable ret_writers : ENDPOINT_DATA_ARRAY_TYPE(0 to NUM_WRITERS-1) := (others => DEFAULT_ENDPOINT_DATA);
    begin
        if (readers) then
            for i in 0 to NUM_READERS-1 loop
                ret_readers(i).topic_name                                       := ENDPOINT_TOPIC(i);
                ret_readers(i).type_name                                        := ENDPOINT_TYPE(i);
                ret_readers(i).durability                                       := ENDPOINT_DURABILITY_QOS(i);
                ret_readers(i).durability_service_cleanup_delay                 := ENDPOINT_DURABILITY_SERVICE_CLEANUP_DELAY(i);
                ret_readers(i).durability_service_history                       := ENDPOINT_DURABILITY_SERVICE_HISTORY(i);
                ret_readers(i).durability_service_history_depth                 := ENDPOINT_DURABILITY_SERVICE_HISTORY_DEPTH(i);
                ret_readers(i).durability_service_max_samples                   := ENDPOINT_DURABILITY_SERVICE_MAX_SAMPLES(i);
                ret_readers(i).durability_service_max_instances                 := ENDPOINT_DURABILITY_SERVICE_MAX_INSTANCES(i);
                ret_readers(i).durability_service_max_samples_per_instances     := ENDPOINT_DURABILITY_SERVICE_MAX_SAMPLES_PER_INSTANCE(i);
                ret_readers(i).presentation                                     := ENDPOINT_PRESENTATION_QOS(i);
                ret_readers(i).coherent_access(0)                               := boolean_to_std_logic(ENDPOINT_COHERENT_ACCESS(i));
                ret_readers(i).ordered_access(0)                                := boolean_to_std_logic(ENDPOINT_ORDERED_ACCESS(i));
                ret_readers(i).deadline                                         := ENDPOINT_DEADLINE_QOS(i);
                ret_readers(i).latency_budget                                   := ENDPOINT_LATENCY_BUDGET_QOS(i);
                ret_readers(i).ownership                                        := ENDPOINT_OWNERSHIP_QOS(i);
                ret_readers(i).ownership_strength                               := ENDPOINT_OWNERSHIP_STRENGTH_QOS(i);
                ret_readers(i).liveliness                                       := ENDPOINT_LIVELINESS_QOS(i);
                ret_readers(i).leaseDuration                                    := ENDPOINT_LEASE_DURATION(i);
                ret_readers(i).time_based_filter                                := ENDPOINT_TIME_BASED_FILTER_QOS(i);
                ret_readers(i).reliability                                      := ENDPOINT_RELIABILITY_QOS(i);
                ret_readers(i).max_blocking_time                                := ENDPOINT_MAX_BLOCKING_TIME(i);
                ret_readers(i).transportnpriority                               := ENDPOINT_TRANSPORT_PRIORITY_QOS(i);
                ret_readers(i).lifespan                                         := ENDPOINT_LIFESPAN_QOS(i);
                ret_readers(i).destination_order                                := ENDPOINT_DESTINATION_ORDER_QOS(i);
                ret_readers(i).expectsInlineQoS(0)                              := '1';
                ret_readers(i).participant                                      := THIS_PARTICIPANT_DATA;
                ret_readers(i).entityId                                         := ENTITYID(i);
            end loop;
            return ret_readers;
        else
            for i in NUM_READERS to NUM_ENDPOINTS-1 loop
                ret_writers(i-NUM_READERS).topic_name                                   := ENDPOINT_TOPIC(i);
                ret_writers(i-NUM_READERS).type_name                                    := ENDPOINT_TYPE(i);
                ret_writers(i-NUM_READERS).durability                                   := ENDPOINT_DURABILITY_QOS(i);
                ret_writers(i-NUM_READERS).durability_service_cleanup_delay             := ENDPOINT_DURABILITY_SERVICE_CLEANUP_DELAY(i);
                ret_writers(i-NUM_READERS).durability_service_history                   := ENDPOINT_DURABILITY_SERVICE_HISTORY(i);
                ret_writers(i-NUM_READERS).durability_service_history_depth             := ENDPOINT_DURABILITY_SERVICE_HISTORY_DEPTH(i);
                ret_writers(i-NUM_READERS).durability_service_max_samples               := ENDPOINT_DURABILITY_SERVICE_MAX_SAMPLES(i);
                ret_writers(i-NUM_READERS).durability_service_max_instances             := ENDPOINT_DURABILITY_SERVICE_MAX_INSTANCES(i);
                ret_writers(i-NUM_READERS).durability_service_max_samples_per_instances := ENDPOINT_DURABILITY_SERVICE_MAX_SAMPLES_PER_INSTANCE(i);
                ret_writers(i-NUM_READERS).presentation                                 := ENDPOINT_PRESENTATION_QOS(i);
                ret_writers(i-NUM_READERS).coherent_access(0)                           := boolean_to_std_logic(ENDPOINT_COHERENT_ACCESS(i));
                ret_writers(i-NUM_READERS).ordered_access(0)                            := boolean_to_std_logic(ENDPOINT_ORDERED_ACCESS(i));
                ret_writers(i-NUM_READERS).deadline                                     := ENDPOINT_DEADLINE_QOS(i);
                ret_writers(i-NUM_READERS).latency_budget                               := ENDPOINT_LATENCY_BUDGET_QOS(i);
                ret_writers(i-NUM_READERS).ownership                                    := ENDPOINT_OWNERSHIP_QOS(i);
                ret_writers(i-NUM_READERS).ownership_strength                           := ENDPOINT_OWNERSHIP_STRENGTH_QOS(i);
                ret_writers(i-NUM_READERS).liveliness                                   := ENDPOINT_LIVELINESS_QOS(i);
                ret_writers(i-NUM_READERS).leaseDuration                                := ENDPOINT_LEASE_DURATION(i);
                ret_writers(i-NUM_READERS).time_based_filter                            := ENDPOINT_TIME_BASED_FILTER_QOS(i);
                ret_writers(i-NUM_READERS).reliability                                  := ENDPOINT_RELIABILITY_QOS(i);
                ret_writers(i-NUM_READERS).max_blocking_time                            := ENDPOINT_MAX_BLOCKING_TIME(i);
                ret_writers(i-NUM_READERS).transportnpriority                           := ENDPOINT_TRANSPORT_PRIORITY_QOS(i);
                ret_writers(i-NUM_READERS).lifespan                                     := ENDPOINT_LIFESPAN_QOS(i);
                ret_writers(i-NUM_READERS).destination_order                            := ENDPOINT_DESTINATION_ORDER_QOS(i);
                ret_writers(i-NUM_READERS).participant                                  := THIS_PARTICIPANT_DATA;
                ret_writers(i-NUM_READERS).entityId                                     := ENTITYID(i);
            end loop;
            return ret_writers;
        end if;
    end function;
    
    function to_string (input : TEST_MEMORY_TYPE) return string is
    begin
        return "length: " & integer'image(input'length) & ", start_addr: " & integer'image(input(0).addr) & "end_addr: " & integer'image(input(input'length-1).addr);
    end function;
    
    function to_string1 (input : std_logic_vector) return string is
    begin
        return "wr: " & to_string(input(0 to NUM_ENDPOINTS-1)) & ", last_word: " & to_string(input(NUM_ENDPOINTS)) & ", data: " & to_hstring(input(NUM_ENDPOINTS+1 to input'length-1));
    end function;
    
end package body;