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

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

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

-- This testbench tests the Input Handling of the rtps_handler. It generates stimulus and compares the output to an expected value.
-- This testbench covers following:
-- RTPS HEADER
--  * The Message has less than the required number of octets to contain a full Header
--  * Its protocol value does not match the value of PROTOCOL_RTPS
--  * The major protocol version is larger than the major protocol version supported by the implementation
-- DATA SUBEMSSAGE
--  * Empty DATA Submessage (No payload/inline-QoS)
--  * META/USER Traffic
--  * Little/Big Endian
--  * Extra Header Bytes (not aligned to 4-Byte boundary)
--  * Invalid Flag setting (KEY and DATA Flag set)
--  * PacketSize invalid (SubmessageLength > PacketSize)
--  * SubmessageLength invalid 
--  * writerSN invalid
-- ACKNACK SUBMESSAGE
--  * META/USER Traffic
--  * Little/Big Endian
--  * PacketSize invalid (SubmessageLength > PacketSize)
--  * SubmessageLength invalid
--  * readerSNState invalid (Base invalid)
--  * readerSNState invalid (NumBits invalid)
-- GAP SUBMESSAGE
--  * META/USER Traffic
--  * Little/Big Endian
--  * PacketSize invalid (SubmessageLength > PacketSize)
--  * SubmessageLength invalid
--  * gapStart invalid
--  * gapList invalid (Base invalid)
--  * gapList invalid (NumBits invalid)
-- HEARTBEAT SUBMESSAGE
--  * META/USER Traffic
--  * Little/Big Endian
--  * PacketSize invalid (SubmessageLength > PacketSize)
--  * SubmessageLength invalid
--  * firstSN invalid
--  * lastSN invalid
--  * lastSN.value < firstSN.value - 1
-- INFO_SOURCE SUBMESSAGE
--  * Little/Big Endian
--  * Protocol Version missmatch
--  * SubmessageLength invalid
-- INFO_DESTINATION SUBMESSAGE
--  * Little/Big Endian
--  * Destination GUID Prefix missmatch
--  * SubmessageLength invalid
-- INFO_REPLY SUBMESSAGE
--  * Little/Big Endian
--  * Empty Locator List
--  * Valid/Invalid Unicast/Multicast (Both invalid Ports and Addresses are tested)
--  * Invalid only
--  * SubmessageLength invalid
-- INFO_REPLY_IP4 SUBMESSAGE
--  * Little/Big Endian
--  * Valid/Invalid Unicast/Multicast (Both invalid Ports and Addresses are tested)
--  * Invalid only
--  * SubmessageLength invalid
-- INFO_TIMESTAMP SUBMESSAGE
--  * Little/Big Endian
--  * Invalidate Flag Set
--  * SubmessageLength invalid
-- PAD SUBMESSAGE
--  * 28 Padding Bytes
-- UKNOWN SUBMESSAGE
--  * DATA_FRAG Submessage
--  * HEARTBEAT_FRAG Submessage
--  * NACK_FRAG Submessage
--  * UKNOWN SID

entity L0_rtps_handler_test1 is
end entity;

architecture testbench of L0_rtps_handler_test1 is
    
    -- *TYPE DECLARATION*
    type TEST_STAGE_TYPE is (IDLE, BUSY);
    
    -- *SIGNAL DECLARATION*
    signal clk, reset, in_empty, rd_sig, builtin_full, builtin_wr, last_word_out : std_logic := '0';
    signal user_full, user_wr : std_logic_vector(0 to NUM_ENDPOINTS-1) := (others => '0');
    signal data_in, data_out : std_logic_vector(WORD_WIDTH-1 downto 0) := (others => '0');
    signal stim_stage, ref_stage : TEST_STAGE_TYPE := IDLE;
    shared variable stimulus, reference : TEST_PACKET_TYPE := EMPTY_TEST_PACKET;
    signal packet_sent, packet_checked : std_logic := '0';
    signal cnt_stim, cnt_ref : natural := 0;
    signal start : std_logic := '0';
    
    -- *FUNCTION DECLARATION*
    procedure wait_on_complete is
    begin
        wait until rising_edge(packet_sent);
        if (packet_checked /= '1') then
            wait until (packet_checked = '1');
        end if;
    end procedure;
    
begin

    -- Unit Under Test
    uut : entity work.rtps_handler(arch)
        port map (
            clk             => clk,
            reset           => reset,
            empty           => in_empty or packet_sent,
            rd              => rd_sig,
            data_in         => data_in,
            data_out        => data_out,
            builtin_full    => builtin_full,
            builtin_wr      => builtin_wr,
            user_full       => user_full,
            user_wr         => user_wr,
            last_word_out   => last_word_out
        );
    
    stimulus_prc : process
        variable rtps_header    : RTPS_HEADER_TYPE := DEFAULT_RTPS_HEADER;
        variable rtps_sub       : RTPS_SUBMESSAGE_TYPE := DEFAULT_RTPS_SUBMESSAGE;
        variable rtps_data      : RTPS_SUBMESSAGE_TYPE := DEFAULT_RTPS_SUBMESSAGE;
        variable tmp_loc        : LOCATOR_TYPE := EMPTY_LOCATOR;
        variable ts             : TIME_TYPE := TIME_INVALID;
        variable check_cnt      : natural := 0;
        variable RV             : RandomPType;
        variable RAND_DATA      : TEST_PACKET_TYPE := EMPTY_TEST_PACKET;
        variable UDP_META       : OUTPUT_HEADER_TYPE;
        variable UDP_USER       : OUTPUT_HEADER_TYPE;
        
        -- Wrapper to use procedure as function
        impure function gen_rand_loc_2 return LOCATOR_TYPE is
            variable ret : LOCATOR_TYPE := EMPTY_LOCATOR;
        begin
            gen_rand_loc(RV, ret);
            return ret;
        end function;
        
        procedure start_test is
        begin
            start <= '1';
            wait until rising_edge(clk);
            start <= '0';
            wait until rising_edge(clk);
        end procedure;
    begin
        
        SetAlertLogName("rtps_handler - Level 0 - Input Handling");
        SetAlertEnable(FAILURE, TRUE);
        SetAlertEnable(ERROR, TRUE);
        SetAlertEnable(WARNING, TRUE);
        SetLogEnable(DEBUG, FALSE);
        SetLogEnable(PASSED, FALSE);
        SetLogEnable(INFO, TRUE);
        RV.InitSeed(RV'instance_name);
        
        
        -- Random Values
        RAND_DATA   := (length => 2, data => (0 => RV.RandSlv(WORD_WIDTH), 1 => RV.RandSlv(WORD_WIDTH), others => (others => '0')), last => (others => '0'));
        UDP_META    := (src => gen_rand_loc_2, dest => DEST_LOC.meta.locator(0));
        UDP_USER    := (src => gen_rand_loc_2, dest => DEST_LOC.user.locator(0));
        
        -- Default Valid Data Submessage (Used to test Packet drop)
        -- NOTE: Each time a stimulus is generated, the Sequence Number is (manually) incremented
        rtps_data                                   := DEFAULT_RTPS_SUBMESSAGE;
        rtps_data.submessageID                      := SID_DATA;
        rtps_data.writerId                          := DEFAULT_WRITER_ENTITYID;
        rtps_data.flags(SUBMESSAGE_DATA_FLAG_POS)   := '1';
        rtps_data.data                              := RAND_DATA;
        
        
        
        Log("Initiating Test", INFO);
        start   <= '0';
        reset   <= '1';
        wait until rising_edge(clk);
        wait until rising_edge(clk);
        reset   <= '0';
        
        -- *RTPS HEADER*
        Log("Sending invalid RTPS Header [Packet Size to small]", INFO);
        -- UDP Header
        gen_output_header(UDP_META, stimulus);
        -- RTPS Header [RTPS Header Length > PacketSize]
        rtps_header := DEFAULT_RTPS_HEADER;
        gen_rtps_header(rtps_header, stimulus);
        stimulus.length := stimulus.length-1;
        -- Finalize Packet
        fix_output_packet(stimulus);
        start_test;
        wait_on_complete;
        check_cnt := check_cnt + reference.length;
        stimulus    := EMPTY_TEST_PACKET;
        reference   := EMPTY_TEST_PACKET;
        
        Log("Sending invalid RTPS Header [Protocol Missmatch]", INFO);
        -- UDP Header
        gen_output_header(UDP_META, stimulus);
        -- RTPS Header [Incompatible Protocol]
        rtps_header             := DEFAULT_RTPS_HEADER;
        rtps_header.protocol    := RV.RandSlv(PROTOCOL_WIDTH);
        gen_rtps_header(rtps_header, stimulus);
        -- Valid Data (Dropped)
        gen_rtps_submessage(rtps_data, stimulus);
        rtps_data.writerSN := rtps_data.writerSN + 1;
        -- Finalize Packet
        fix_output_packet(stimulus);
        start_test;
        wait_on_complete;
        check_cnt := check_cnt + reference.length;
        stimulus    := EMPTY_TEST_PACKET;
        reference   := EMPTY_TEST_PACKET;
        
        Log("Sending invalid RTPS Header [Protocol Major Version Missmatch]", INFO);
        -- UDP Header
        gen_output_header(UDP_META, stimulus);
        -- RTPS Header [Incompatible Version]
        rtps_header             := DEFAULT_RTPS_HEADER;
        rtps_header.version     := PROTOCOLVERSION_1_0;
        gen_rtps_header(rtps_header, stimulus);
        -- Valid Data (Dropped)
        gen_rtps_submessage(rtps_data, stimulus);
        rtps_data.writerSN := rtps_data.writerSN + 1;
        -- Finalize Packet
        fix_output_packet(stimulus);
        start_test;
        wait_on_complete;
        check_cnt := check_cnt + reference.length;
        stimulus    := EMPTY_TEST_PACKET;
        reference   := EMPTY_TEST_PACKET;
        
        -- Valid RTPS Header from here on
        rtps_header := DEFAULT_RTPS_HEADER;
        
        -- *DATA SUBMESSAGE*
        Log("Sending valid DATA [Empty Submessage]", INFO);
        -- UDP Header
        gen_output_header(UDP_META, stimulus);
        -- RTPS Header
        gen_rtps_header(rtps_header, stimulus);
        -- Valid DATA [Empty DATA] (Dropped)
        rtps_sub                := DEFAULT_RTPS_SUBMESSAGE;
        rtps_sub.submessageID   := SID_DATA;
        rtps_sub.writerId       := DEFAULT_WRITER_ENTITYID;
        gen_rtps_submessage(rtps_sub, stimulus);
        -- Finalize Packet
        fix_output_packet(stimulus);
        start_test;
        wait_on_complete;
        check_cnt := check_cnt + reference.length;
        stimulus    := EMPTY_TEST_PACKET;
        reference   := EMPTY_TEST_PACKET;
        
        Log("Sending valid DATA [Meta Traffic, Both Endianness]", INFO);
        -- UDP Header
        gen_output_header(UDP_META, stimulus);
        -- RTPS Header
        gen_rtps_header(rtps_header, stimulus);
        -- Valid DATA [META Traffic, Big Endian] (Expected)
        rtps_sub                                    := DEFAULT_RTPS_SUBMESSAGE;
        rtps_sub.submessageID                       := SID_DATA;
        rtps_sub.writerId                           := DEFAULT_WRITER_ENTITYID;
        rtps_sub.flags(SUBMESSAGE_DATA_FLAG_POS)    := '1';
        rtps_sub.data                               := RAND_DATA;
        gen_rtps_submessage(rtps_sub, stimulus);
        gen_rtps_handler_out(rtps_sub, UDP_META.src, TRUE, TIME_INVALID, rtps_header.guidPrefix, reference);
        -- Valid DATA [META Traffic, Little Endian] (Expected)
        rtps_sub.flags(SUBMESSAGE_ENDIAN_FLAG_POS)  := '1';
        gen_rtps_submessage(rtps_sub, stimulus);
        gen_rtps_handler_out(rtps_sub, UDP_META.src, TRUE, TIME_INVALID, rtps_header.guidPrefix, reference);
        -- Finalize Packet
        fix_output_packet(stimulus);
        start_test;
        wait_on_complete;
        check_cnt := check_cnt + reference.length;
        stimulus    := EMPTY_TEST_PACKET;
        reference   := EMPTY_TEST_PACKET;
        
        Log("Sending valid DATA [User Traffic]", INFO);
        -- UDP Header
        gen_output_header(UDP_USER, stimulus);
        -- RTPS Header
        gen_rtps_header(rtps_header, stimulus);
        -- Valid DATA [USER Traffic, Big Endian] (Expected)
        rtps_sub                                    := DEFAULT_RTPS_SUBMESSAGE;
        rtps_sub.submessageID                       := SID_DATA;
        rtps_sub.writerId                           := DEFAULT_WRITER_ENTITYID;
        rtps_sub.flags(SUBMESSAGE_DATA_FLAG_POS)    := '1';
        rtps_sub.data                               := RAND_DATA;
        gen_rtps_submessage(rtps_sub, stimulus);
        gen_rtps_handler_out(rtps_sub, UDP_USER.src, FALSE, TIME_INVALID, rtps_header.guidPrefix, reference);
        -- Finalize Packet
        fix_output_packet(stimulus);
        start_test;
        wait_on_complete;
        check_cnt := check_cnt + reference.length;
        stimulus    := EMPTY_TEST_PACKET;
        reference   := EMPTY_TEST_PACKET;
        
        Log("Sending valid DATA [Extra Header Bytes]", INFO);
        -- UDP Header
        gen_output_header(UDP_META, stimulus);
        -- RTPS Header
        gen_rtps_header(rtps_header, stimulus);
        -- Valid DATA [+7 Unknown Header Bytes] (Expected)
        rtps_sub                                    := DEFAULT_RTPS_SUBMESSAGE;
        rtps_sub.submessageID                       := SID_DATA;
        rtps_sub.writerId                           := DEFAULT_WRITER_ENTITYID;
        rtps_sub.flags(SUBMESSAGE_DATA_FLAG_POS)    := '1';
        rtps_sub.data                               := RAND_DATA;
        rtps_sub.octetsToInlineQos                  := int(23,16);
        gen_rtps_submessage(rtps_sub, stimulus);
        gen_rtps_handler_out(rtps_sub, UDP_META.src, TRUE, TIME_INVALID, rtps_header.guidPrefix, reference);
        -- Finalize Packet
        fix_output_packet(stimulus);
        start_test;
        wait_on_complete;
        check_cnt := check_cnt + reference.length;
        stimulus    := EMPTY_TEST_PACKET;
        reference   := EMPTY_TEST_PACKET;
        
        Log("Sending invalid DATA [DATA and KEY Flag set]", INFO);
        -- UDP Header
        gen_output_header(UDP_META, stimulus);
        -- RTPS Header
        gen_rtps_header(rtps_header, stimulus);
        -- Valid DATA [Invalid Flags] (Dropped)
        rtps_sub                                    := DEFAULT_RTPS_SUBMESSAGE;
        rtps_sub.submessageID                       := SID_DATA;
        rtps_sub.writerId                           := DEFAULT_WRITER_ENTITYID;
        rtps_sub.flags(SUBMESSAGE_DATA_FLAG_POS)    := '1';
        rtps_sub.flags(SUBMESSAGE_KEY_FLAG_POS)     := '1';
        rtps_sub.data                               := RAND_DATA;
        gen_rtps_submessage(rtps_sub, stimulus);
        -- Valid Data (Expected)
        gen_rtps_submessage(rtps_data, stimulus);
        gen_rtps_handler_out(rtps_data, UDP_META.src, TRUE, TIME_INVALID, rtps_header.guidPrefix, reference);
        rtps_data.writerSN := rtps_data.writerSN + 1;
        -- Finalize Packet
        fix_output_packet(stimulus);
        start_test;
        wait_on_complete;
        check_cnt := check_cnt + reference.length;
        stimulus    := EMPTY_TEST_PACKET;
        reference   := EMPTY_TEST_PACKET;
        
        Log("Sending invalid DATA [Packet Size too small]", INFO);
        -- UDP Header
        gen_output_header(UDP_META, stimulus);
        -- RTPS Header
        gen_rtps_header(rtps_header, stimulus);
        -- Valid DATA [SubLength > PackeSize] (Dropped)
        rtps_sub                                    := DEFAULT_RTPS_SUBMESSAGE;
        rtps_sub.submessageID                       := SID_DATA;
        rtps_sub.writerId                           := DEFAULT_WRITER_ENTITYID;
        rtps_sub.flags(SUBMESSAGE_DATA_FLAG_POS)    := '1';
        rtps_sub.data                               := RAND_DATA;
        gen_rtps_submessage(rtps_sub, stimulus);
        stimulus.length := stimulus.length-1;
        -- Finalize Packet
        fix_output_packet(stimulus);
        start_test;
        wait_on_complete;
        check_cnt := check_cnt + reference.length;
        stimulus    := EMPTY_TEST_PACKET;
        reference   := EMPTY_TEST_PACKET;
        
        Log("Sending invalid DATA [Submessage Length too small]", INFO);
        -- UDP Header
        gen_output_header(UDP_META, stimulus);
        -- RTPS Header
        gen_rtps_header(rtps_header, stimulus);
        -- Valid DATA [SubLength invalid] (Dropped)
        rtps_sub                                    := DEFAULT_RTPS_SUBMESSAGE;
        rtps_sub.submessageID                       := SID_DATA;
        rtps_sub.writerId                           := DEFAULT_WRITER_ENTITYID;
        rtps_sub.flags(SUBMESSAGE_DATA_FLAG_POS)    := '1';
        rtps_sub.data                               := RAND_DATA;
        rtps_sub.submessageLength                   := int(16,16);
        gen_rtps_submessage(rtps_sub, stimulus);
        -- Valid Data (Dropped)
        gen_rtps_submessage(rtps_data, stimulus);
        rtps_data.writerSN := rtps_data.writerSN + 1;
        -- Finalize Packet
        fix_output_packet(stimulus);
        start_test;
        wait_on_complete;
        check_cnt := check_cnt + reference.length;
        stimulus    := EMPTY_TEST_PACKET;
        reference   := EMPTY_TEST_PACKET;
        
        Log("Sending invalid DATA [Invalid writerSN]", INFO);
        -- UDP Header
        gen_output_header(UDP_META, stimulus);
        -- RTPS Header
        gen_rtps_header(rtps_header, stimulus);
        -- Valid DATA [Invalid writerSN] (Dropped)
        rtps_sub                                    := DEFAULT_RTPS_SUBMESSAGE;
        rtps_sub.submessageID                       := SID_DATA;
        rtps_sub.writerId                           := DEFAULT_WRITER_ENTITYID;
        rtps_sub.flags(SUBMESSAGE_DATA_FLAG_POS)    := '1';
        rtps_sub.data                               := RAND_DATA;
        rtps_sub.writerSN                           := (others => (others => '0'));
        gen_rtps_submessage(rtps_sub, stimulus);
        -- Valid Data (Dropped)
        gen_rtps_submessage(rtps_data, stimulus);
        rtps_data.writerSN := rtps_data.writerSN + 1;
        -- Finalize Packet
        fix_output_packet(stimulus);
        start_test;
        wait_on_complete;
        check_cnt := check_cnt + reference.length;
        stimulus    := EMPTY_TEST_PACKET;
        reference   := EMPTY_TEST_PACKET;
        
        
        -- *ACKNACK SUBMESSAGE*
        Log("Sending valid ACKNACK [Meta Traffic, Both Endianness]", INFO);
        -- UDP Header
        gen_output_header(UDP_META, stimulus);
        -- RTPS Header
        gen_rtps_header(rtps_header, stimulus);
        -- Valid ACKNACK [META Traffic, Big Endian] (Expected)
        rtps_sub                                    := DEFAULT_RTPS_SUBMESSAGE;
        rtps_sub.submessageID                       := SID_ACKNACK;
        rtps_sub.readerId                           := DEFAULT_READER_ENTITYID;
        gen_rtps_submessage(rtps_sub, stimulus);
        gen_rtps_handler_out(rtps_sub, UDP_META.src, TRUE, TIME_INVALID, rtps_header.guidPrefix, reference);
        -- Valid ACKNACK [META Traffic, Little Endian] (Expected)
        rtps_sub.flags(SUBMESSAGE_ENDIAN_FLAG_POS)  := '1';
        gen_rtps_submessage(rtps_sub, stimulus);
        gen_rtps_handler_out(rtps_sub, UDP_META.src, TRUE, TIME_INVALID, rtps_header.guidPrefix, reference);
        -- Finalize Packet
        fix_output_packet(stimulus);
        start_test;
        wait_on_complete;
        check_cnt := check_cnt + reference.length;
        stimulus    := EMPTY_TEST_PACKET;
        reference   := EMPTY_TEST_PACKET;
        
        Log("Sending valid ACKNACK [User Traffic]", INFO);
        -- UDP Header
        gen_output_header(UDP_USER, stimulus);
        -- RTPS Header
        gen_rtps_header(rtps_header, stimulus);
        -- Valid ACKNACK [USER Traffic, Big Endian] (Expected)
        rtps_sub                                    := DEFAULT_RTPS_SUBMESSAGE;
        rtps_sub.submessageID                       := SID_ACKNACK;
        rtps_sub.readerId                           := DEFAULT_READER_ENTITYID;
        gen_rtps_submessage(rtps_sub, stimulus);
        gen_rtps_handler_out(rtps_sub, UDP_USER.src, FALSE, TIME_INVALID, rtps_header.guidPrefix, reference);
        -- Finalize Packet
        fix_output_packet(stimulus);
        start_test;
        wait_on_complete;
        check_cnt := check_cnt + reference.length;
        stimulus    := EMPTY_TEST_PACKET;
        reference   := EMPTY_TEST_PACKET;
        
        Log("Sending invalid ACKNACK [Packet Size too small]", INFO);
        -- UDP Header
        gen_output_header(UDP_META, stimulus);
        -- RTPS Header
        gen_rtps_header(rtps_header, stimulus);
        -- Invalid ACKNACK [SubLength > PacketSize] (Dropped)
        rtps_sub                := DEFAULT_RTPS_SUBMESSAGE;
        rtps_sub.submessageID   := SID_ACKNACK;
        rtps_sub.readerId       := DEFAULT_READER_ENTITYID;
        gen_rtps_submessage(rtps_sub, stimulus);
        stimulus.length := stimulus.length-1;
        -- Finalize Packet
        fix_output_packet(stimulus);
        start_test;
        wait_on_complete;
        check_cnt := check_cnt + reference.length;
        stimulus    := EMPTY_TEST_PACKET;
        reference   := EMPTY_TEST_PACKET;
        
        Log("Sending invalid ACKNACK [Submessage Length too small]", INFO);
        -- UDP Header
        gen_output_header(UDP_META, stimulus);
        -- RTPS Header
        gen_rtps_header(rtps_header, stimulus);
        -- Invalid ACKNACK [SubLength invalid] (Dropped)
        rtps_sub                    := DEFAULT_RTPS_SUBMESSAGE;
        rtps_sub.submessageID       := SID_ACKNACK;
        rtps_sub.readerId           := DEFAULT_READER_ENTITYID;
        rtps_sub.submessageLength   := int(20,16);
        gen_rtps_submessage(rtps_sub, stimulus);
        -- Valid Data (Dropped)
        gen_rtps_submessage(rtps_data, stimulus);
        rtps_data.writerSN := rtps_data.writerSN + 1;
        -- Finalize Packet
        fix_output_packet(stimulus);
        start_test;
        wait_on_complete;
        check_cnt := check_cnt + reference.length;
        stimulus    := EMPTY_TEST_PACKET;
        reference   := EMPTY_TEST_PACKET;
        
        Log("Sending invalid ACKNACK [ReaderSNState invalid (Base invalid)]", INFO);
        -- UDP Header
        gen_output_header(UDP_META, stimulus);
        -- RTPS Header
        gen_rtps_header(rtps_header, stimulus);
        -- Invalid ACKNACK [readerSNState invalid (Base Invalid)] (Dropped)
        rtps_sub                    := DEFAULT_RTPS_SUBMESSAGE;
        rtps_sub.submessageID       := SID_ACKNACK;
        rtps_sub.readerId           := DEFAULT_READER_ENTITYID;
        rtps_sub.readerSNState.base := (others => (others => '0'));
        gen_rtps_submessage(rtps_sub, stimulus);
        -- Valid DATA (Dropped)
        gen_rtps_submessage(rtps_data, stimulus);
        rtps_data.writerSN := rtps_data.writerSN + 1;
        -- Finalize Packet
        fix_output_packet(stimulus);
        start_test;
        wait_on_complete;
        check_cnt := check_cnt + reference.length;
        stimulus    := EMPTY_TEST_PACKET;
        reference   := EMPTY_TEST_PACKET;
        
        Log("Sending invalid ACKNACK [ReaderSNState invalid (NumBits invalid)]", INFO);
        -- UDP Header
        gen_output_header(UDP_META, stimulus);
        -- RTPS Header
        gen_rtps_header(rtps_header, stimulus);
        -- Invalid ACKNACK [readerSNState invalid (NumBits Invalid)] (Dropped)
        rtps_sub                        := DEFAULT_RTPS_SUBMESSAGE;
        rtps_sub.submessageID           := SID_ACKNACK;
        rtps_sub.readerId               := DEFAULT_READER_ENTITYID;
        rtps_sub.readerSNState.numBits  := int(257, 32);
        gen_rtps_submessage(rtps_sub, stimulus);
        -- Valid DATA (Dropped)
        gen_rtps_submessage(rtps_data, stimulus);
        rtps_data.writerSN := rtps_data.writerSN + 1;
        -- Finalize Packet
        fix_output_packet(stimulus);
        start_test;
        wait_on_complete;
        check_cnt := check_cnt + reference.length;
        stimulus    := EMPTY_TEST_PACKET;
        reference   := EMPTY_TEST_PACKET;
        
        -- *GAP SUBMESSAGE*
        Log("Sending valid GAP [Meta Traffic, Both Endianness]", INFO);
        -- UDP Header
        gen_output_header(UDP_META, stimulus);
        -- RTPS Header
        gen_rtps_header(rtps_header, stimulus);
        -- Valid GAP [META Traffic, Big Endian]
        rtps_sub                        := DEFAULT_RTPS_SUBMESSAGE;
        rtps_sub.submessageID           := SID_GAP;
        rtps_sub.writerId               := DEFAULT_WRITER_ENTITYID;
        gen_rtps_submessage(rtps_sub, stimulus);
        gen_rtps_handler_out(rtps_sub, UDP_META.src, TRUE, TIME_INVALID, rtps_header.guidPrefix, reference);
        -- Valid GAP [META Traffic, Little Endian]
        rtps_sub.flags(SUBMESSAGE_ENDIAN_FLAG_POS)  := '1';
        gen_rtps_submessage(rtps_sub, stimulus);
        gen_rtps_handler_out(rtps_sub, UDP_META.src, TRUE, TIME_INVALID, rtps_header.guidPrefix, reference);
        -- Finalize Packet
        fix_output_packet(stimulus);
        start_test;
        wait_on_complete;
        check_cnt := check_cnt + reference.length;
        stimulus    := EMPTY_TEST_PACKET;
        reference   := EMPTY_TEST_PACKET;
        
        Log("Sending valid GAP [User Traffic]", INFO);
        -- UDP Header
        gen_output_header(UDP_USER, stimulus);
        -- RTPS Header
        gen_rtps_header(rtps_header, stimulus);
        -- Valid GAP [User Traffic, Big Endian]
        rtps_sub                        := DEFAULT_RTPS_SUBMESSAGE;
        rtps_sub.submessageID           := SID_GAP;
        rtps_sub.writerId               := DEFAULT_WRITER_ENTITYID;
        gen_rtps_submessage(rtps_sub, stimulus);
        gen_rtps_handler_out(rtps_sub, UDP_USER.src, FALSE, TIME_INVALID, rtps_header.guidPrefix, reference);
        -- Finalize Packet
        fix_output_packet(stimulus);
        start_test;
        wait_on_complete;
        check_cnt := check_cnt + reference.length;
        stimulus    := EMPTY_TEST_PACKET;
        reference   := EMPTY_TEST_PACKET;
        
        Log("Sending invalid GAP [Packet Size too small]", INFO);
        -- UDP Header
        gen_output_header(UDP_META, stimulus);
        -- RTPS Header
        gen_rtps_header(rtps_header, stimulus);
        -- Invalid GAP [SubLength > PacketSize] (Dropped)
        rtps_sub                        := DEFAULT_RTPS_SUBMESSAGE;
        rtps_sub.submessageID           := SID_GAP;
        rtps_sub.writerId               := DEFAULT_WRITER_ENTITYID;
        gen_rtps_submessage(rtps_sub, stimulus);
        stimulus.length := stimulus.length-1;
        -- Finalize Packet
        fix_output_packet(stimulus);
        start_test;
        wait_on_complete;
        check_cnt := check_cnt + reference.length;
        stimulus    := EMPTY_TEST_PACKET;
        reference   := EMPTY_TEST_PACKET;
        
        Log("Sending invalid GAP [Submessage Length too small]", INFO);
        -- UDP Header
        gen_output_header(UDP_META, stimulus);
        -- RTPS Header
        gen_rtps_header(rtps_header, stimulus);
        -- Invalid GAP [SubLength invalid] (Dropped)
        rtps_sub                        := DEFAULT_RTPS_SUBMESSAGE;
        rtps_sub.submessageID           := SID_GAP;
        rtps_sub.writerId               := DEFAULT_WRITER_ENTITYID;
        rtps_sub.submessageLength       := int(24,16);
        gen_rtps_submessage(rtps_sub, stimulus);
        -- Valid Data (Dropped)
        gen_rtps_submessage(rtps_data, stimulus);
        rtps_data.writerSN := rtps_data.writerSN + 1;
        -- Finalize Packet
        fix_output_packet(stimulus);
        start_test;
        wait_on_complete;
        check_cnt := check_cnt + reference.length;
        stimulus    := EMPTY_TEST_PACKET;
        reference   := EMPTY_TEST_PACKET;
        
        Log("Sending invalid GAP [GapStart invalid]", INFO);
        -- UDP Header
        gen_output_header(UDP_META, stimulus);
        -- RTPS Header
        gen_rtps_header(rtps_header, stimulus);
        -- Invalid GAP [GapStart invalid] (Dropped)
        rtps_sub                        := DEFAULT_RTPS_SUBMESSAGE;
        rtps_sub.submessageID           := SID_GAP;
        rtps_sub.writerId               := DEFAULT_WRITER_ENTITYID;
        rtps_sub.gapStart               := (others => (others => '0'));
        gen_rtps_submessage(rtps_sub, stimulus);
        -- Valid Data (Dropped)
        gen_rtps_submessage(rtps_data, stimulus);
        rtps_data.writerSN := rtps_data.writerSN + 1;
        -- Finalize Packet
        fix_output_packet(stimulus);
        start_test;
        wait_on_complete;
        check_cnt := check_cnt + reference.length;
        stimulus    := EMPTY_TEST_PACKET;
        reference   := EMPTY_TEST_PACKET;
        
        Log("Sending invalid GAP [GapList invalid (Base invalid)]", INFO);
        -- UDP Header
        gen_output_header(UDP_META, stimulus);
        -- RTPS Header
        gen_rtps_header(rtps_header, stimulus);
        -- Invalid GAP [GapList invalid (Base invalid)] (Dropped)
        rtps_sub                        := DEFAULT_RTPS_SUBMESSAGE;
        rtps_sub.submessageID           := SID_GAP;
        rtps_sub.writerId               := DEFAULT_WRITER_ENTITYID;
        rtps_sub.gapList.base           := (others => (others => '0'));
        gen_rtps_submessage(rtps_sub, stimulus);
        -- Valid Data (Dropped)
        gen_rtps_submessage(rtps_data, stimulus);
        rtps_data.writerSN := rtps_data.writerSN + 1;
        -- Finalize Packet
        fix_output_packet(stimulus);
        start_test;
        wait_on_complete;
        check_cnt := check_cnt + reference.length;
        stimulus    := EMPTY_TEST_PACKET;
        reference   := EMPTY_TEST_PACKET;
        
        Log("Sending invalid GAP [GapList invalid (NumBits invalid)]", INFO);
        -- UDP Header
        gen_output_header(UDP_META, stimulus);
        -- RTPS Header
        gen_rtps_header(rtps_header, stimulus);
        -- Invalid GAP [GapList invalid (NumBits invalid)] (Dropped)
        rtps_sub                        := DEFAULT_RTPS_SUBMESSAGE;
        rtps_sub.submessageID           := SID_GAP;
        rtps_sub.writerId               := DEFAULT_WRITER_ENTITYID;
        rtps_sub.gapList.numBits        := int(257,32);
        gen_rtps_submessage(rtps_sub, stimulus);
        -- Valid Data (Dropped)
        gen_rtps_submessage(rtps_data, stimulus);
        rtps_data.writerSN := rtps_data.writerSN + 1;
        -- Finalize Packet
        fix_output_packet(stimulus);
        start_test;
        wait_on_complete;
        check_cnt := check_cnt + reference.length;
        stimulus    := EMPTY_TEST_PACKET;
        reference   := EMPTY_TEST_PACKET;
        
        
        -- *HEARTBEAT SUBMESSAGE*
        Log("Sending valid HEARTBEAT [Meta Traffic, Both Endianness]", INFO);
        -- UDP Header
        gen_output_header(UDP_META, stimulus);
        -- RTPS Header
        gen_rtps_header(rtps_header, stimulus);
        -- Valid HEARTBEAT [META Traffic, Big Endian] (Expected)
        rtps_sub                        := DEFAULT_RTPS_SUBMESSAGE;
        rtps_sub.submessageID           := SID_HEARTBEAT;
        rtps_sub.writerId               := DEFAULT_WRITER_ENTITYID;
        gen_rtps_submessage(rtps_sub, stimulus);
        gen_rtps_handler_out(rtps_sub, UDP_META.src, TRUE, TIME_INVALID, rtps_header.guidPrefix, reference);
        -- Valid HEARTBEAT [META Traffic, Little Endian] (Expected)
        rtps_sub.flags(SUBMESSAGE_ENDIAN_FLAG_POS)  := '1';
        gen_rtps_submessage(rtps_sub, stimulus);
        gen_rtps_handler_out(rtps_sub, UDP_META.src, TRUE, TIME_INVALID, rtps_header.guidPrefix, reference);
        -- Finalize Packet
        fix_output_packet(stimulus);
        start_test;
        wait_on_complete;
        check_cnt := check_cnt + reference.length;
        stimulus    := EMPTY_TEST_PACKET;
        reference   := EMPTY_TEST_PACKET;
        
        Log("Sending valid HEARTBEAT [User Traffic]", INFO);
        -- UDP Header
        gen_output_header(UDP_USER, stimulus);
        -- RTPS Header
        gen_rtps_header(rtps_header, stimulus);
        -- Valid HEARTBEAT [USER Traffic, Big Endian] (Expected)
        rtps_sub                        := DEFAULT_RTPS_SUBMESSAGE;
        rtps_sub.submessageID           := SID_HEARTBEAT;
        rtps_sub.writerId               := DEFAULT_WRITER_ENTITYID;
        gen_rtps_submessage(rtps_sub, stimulus);
        gen_rtps_handler_out(rtps_sub, UDP_USER.src, FALSE, TIME_INVALID, rtps_header.guidPrefix, reference);
        -- Finalize Packet
        fix_output_packet(stimulus);
        start_test;
        wait_on_complete;
        check_cnt := check_cnt + reference.length;
        stimulus    := EMPTY_TEST_PACKET;
        reference   := EMPTY_TEST_PACKET;
        
        Log("Sending invalid HEARTBEAT [Packet Size too small]", INFO);
        -- UDP Header
        gen_output_header(UDP_META, stimulus);
        -- RTPS Header
        gen_rtps_header(rtps_header, stimulus);
        -- Invalid HEARTBEAT [SubLength > PackeSize] (Dropped)
        rtps_sub                        := DEFAULT_RTPS_SUBMESSAGE;
        rtps_sub.submessageID           := SID_HEARTBEAT;
        rtps_sub.writerId               := DEFAULT_WRITER_ENTITYID;
        gen_rtps_submessage(rtps_sub, stimulus);
        stimulus.length := stimulus.length-1;
        -- Finalize Packet
        fix_output_packet(stimulus);
        start_test;
        wait_on_complete;
        check_cnt := check_cnt + reference.length;
        stimulus    := EMPTY_TEST_PACKET;
        reference   := EMPTY_TEST_PACKET;
        
        Log("Sending invalid HEARTBEAT [Submessage Length too small]", INFO);
        -- UDP Header
        gen_output_header(UDP_META, stimulus);
        -- RTPS Header
        gen_rtps_header(rtps_header, stimulus);
        -- Invalid HEARTBEAT [SubLength invalid] (Dropped)
        rtps_sub                        := DEFAULT_RTPS_SUBMESSAGE;
        rtps_sub.submessageID           := SID_HEARTBEAT;
        rtps_sub.writerId               := DEFAULT_WRITER_ENTITYID;
        rtps_sub.submessageLength       := int(24,16);
        gen_rtps_submessage(rtps_sub, stimulus);
        -- Valid Data (Dropped)
        gen_rtps_submessage(rtps_data, stimulus);
        rtps_data.writerSN := rtps_data.writerSN + 1;
        -- Finalize Packet
        fix_output_packet(stimulus);
        start_test;
        wait_on_complete;
        check_cnt := check_cnt + reference.length;
        stimulus    := EMPTY_TEST_PACKET;
        reference   := EMPTY_TEST_PACKET;
        
        Log("Sending invalid HEARTBEAT [FirstSN invalid]", INFO);
        -- UDP Header
        gen_output_header(UDP_META, stimulus);
        -- RTPS Header
        gen_rtps_header(rtps_header, stimulus);
        -- Invalid HEARTBEAT [FirstSN invalid] (Dropped)
        rtps_sub                        := DEFAULT_RTPS_SUBMESSAGE;
        rtps_sub.submessageID           := SID_HEARTBEAT;
        rtps_sub.writerId               := DEFAULT_WRITER_ENTITYID;
        rtps_sub.firstSN                := (others => (others => '0'));
        gen_rtps_submessage(rtps_sub, stimulus);
        -- Valid Data (Dropped)
        gen_rtps_submessage(rtps_data, stimulus);
        rtps_data.writerSN := rtps_data.writerSN + 1;
        -- Finalize Packet
        fix_output_packet(stimulus);
        start_test;
        wait_on_complete;
        check_cnt := check_cnt + reference.length;
        stimulus    := EMPTY_TEST_PACKET;
        reference   := EMPTY_TEST_PACKET;
        
        Log("Sending invalid HEARTBEAT [LastSN invalid]", INFO);
        -- UDP Header
        gen_output_header(UDP_META, stimulus);
        -- RTPS Header
        gen_rtps_header(rtps_header, stimulus);
        -- Invalid HEARTBEAT [LastSN invalid] (Dropped)
        rtps_sub                        := DEFAULT_RTPS_SUBMESSAGE;
        rtps_sub.submessageID           := SID_HEARTBEAT;
        rtps_sub.writerId               := DEFAULT_WRITER_ENTITYID;
        rtps_sub.lastSN                 := (others => (others => '1')); -- Negative
        gen_rtps_submessage(rtps_sub, stimulus);
        -- Valid Data (Dropped)
        gen_rtps_submessage(rtps_data, stimulus);
        rtps_data.writerSN := rtps_data.writerSN + 1;
        -- Finalize Packet
        fix_output_packet(stimulus);
        start_test;
        wait_on_complete;
        check_cnt := check_cnt + reference.length;
        stimulus    := EMPTY_TEST_PACKET;
        reference   := EMPTY_TEST_PACKET;
        
        Log("Sending invalid HEARTBEAT [LastSN < FirstSN - 1]", INFO);
        -- UDP Header
        gen_output_header(UDP_META, stimulus);
        -- RTPS Header
        gen_rtps_header(rtps_header, stimulus);
        -- Invalid HEARTBEAT [LastSN invalid] (Dropped)
        rtps_sub                        := DEFAULT_RTPS_SUBMESSAGE;
        rtps_sub.submessageID           := SID_HEARTBEAT;
        rtps_sub.writerId               := DEFAULT_WRITER_ENTITYID;
        rtps_sub.firstSN                := ((others => '0'), unsigned(int(5,WORD_WIDTH))); -- 5
        rtps_sub.lastSN                 := ((others => '0'), unsigned(int(1,WORD_WIDTH))); -- 1
        gen_rtps_submessage(rtps_sub, stimulus);
        -- Valid Data (Dropped)
        gen_rtps_submessage(rtps_data, stimulus);
        rtps_data.writerSN := rtps_data.writerSN + 1;
        -- Finalize Packet
        fix_output_packet(stimulus);
        start_test;
        wait_on_complete;
        check_cnt := check_cnt + reference.length;
        stimulus    := EMPTY_TEST_PACKET;
        reference   := EMPTY_TEST_PACKET;
        
        -- *INFO_SOURCE SUBMESSAGE*
        Log("Testing INFO_SOURCE interpretation", INFO);
        -- UDP Header
        gen_output_header(UDP_META, stimulus);
        -- RTPS Header
        gen_rtps_header(rtps_header, stimulus);
        -- Valid Data (Expected)
        gen_rtps_submessage(rtps_data, stimulus);
        gen_rtps_handler_out(rtps_data, UDP_META.src, TRUE, TIME_INVALID, rtps_header.guidPrefix, reference);
        rtps_data.writerSN := rtps_data.writerSN + 1;
        -- Valid INFO_SOURCE [Big Endian] (Change Source GUID Prefix)
        rtps_sub                        := DEFAULT_RTPS_SUBMESSAGE;
        rtps_sub.submessageID           := SID_INFO_SRC;
        rtps_sub.guidPrefix(0)          := RV.RandSlv(WORD_WIDTH);
        gen_rtps_submessage(rtps_sub, stimulus);
        -- Valid Data (Expected)
        gen_rtps_submessage(rtps_data, stimulus);
        gen_rtps_handler_out(rtps_data, UDP_META.src, TRUE, TIME_INVALID, rtps_sub.guidPrefix, reference);
        rtps_data.writerSN := rtps_data.writerSN + 1;
        -- Valid INFO_SOURCE [Little Endian] (Change Source GUID Prefix)
        rtps_sub.flags(SUBMESSAGE_ENDIAN_FLAG_POS)  := '1';
        rtps_sub.guidPrefix(0)                      := RV.RandSlv(WORD_WIDTH);
        gen_rtps_submessage(rtps_sub, stimulus);
        -- Valid Data (Expected)
        gen_rtps_submessage(rtps_data, stimulus);
        gen_rtps_handler_out(rtps_data, UDP_META.src, TRUE, TIME_INVALID, rtps_sub.guidPrefix, reference);
        rtps_data.writerSN := rtps_data.writerSN + 1;
        -- Valid INFO_SOURCE [Protocol Major Version Missmatch] (Dropped)
        rtps_sub                        := DEFAULT_RTPS_SUBMESSAGE;
        rtps_sub.submessageID           := SID_INFO_SRC;
        rtps_sub.version                := PROTOCOLVERSION_1_0;
        gen_rtps_submessage(rtps_sub, stimulus);
        -- Valid Data (Dropped)
        gen_rtps_submessage(rtps_data, stimulus);
        rtps_data.writerSN := rtps_data.writerSN + 1;
        -- Finalize Packet
        fix_output_packet(stimulus);
        start_test;
        wait_on_complete;
        check_cnt := check_cnt + reference.length;
        stimulus    := EMPTY_TEST_PACKET;
        reference   := EMPTY_TEST_PACKET;
        
        Log("Sending invalid INFO_SOURCE [Submessage Length too small]", INFO);
        -- UDP Header
        gen_output_header(UDP_META, stimulus);
        -- RTPS Header
        gen_rtps_header(rtps_header, stimulus);
        -- Invalid INFO_SOURCE [SubLength invalid] (Dropped)
        rtps_sub                        := DEFAULT_RTPS_SUBMESSAGE;
        rtps_sub.submessageID           := SID_INFO_SRC;
        rtps_sub.submessageLength       := int(16,16);
        gen_rtps_submessage(rtps_sub, stimulus);
        -- Valid Data (Dropped)
        gen_rtps_submessage(rtps_data, stimulus);
        rtps_data.writerSN := rtps_data.writerSN + 1;
        -- Finalize Packet
        fix_output_packet(stimulus);
        start_test;
        wait_on_complete;
        check_cnt := check_cnt + reference.length;
        stimulus    := EMPTY_TEST_PACKET;
        reference   := EMPTY_TEST_PACKET;
        
        
        -- *INFO_DESTINATION SUBMESSAGE*
        Log("Testing INFO_DESTINATION interpretation", INFO);
        -- UDP Header
        gen_output_header(UDP_META, stimulus);
        -- RTPS Header
        gen_rtps_header(rtps_header, stimulus);
        -- Valid Data (Expected)
        gen_rtps_submessage(rtps_data, stimulus);
        gen_rtps_handler_out(rtps_data, UDP_META.src, TRUE, TIME_INVALID, rtps_header.guidPrefix, reference);
        rtps_data.writerSN := rtps_data.writerSN + 1;
        -- Valid INFO_DESTINATION [Expected GUID Prefix, Big Endian] (No Change)
        rtps_sub                        := DEFAULT_RTPS_SUBMESSAGE;
        rtps_sub.submessageID           := SID_INFO_DST;
        rtps_sub.guidPrefix             := GUIDPREFIX;
        gen_rtps_submessage(rtps_sub, stimulus);
        -- Valid Data (Expected)
        gen_rtps_submessage(rtps_data, stimulus);
        gen_rtps_handler_out(rtps_data, UDP_META.src, TRUE, TIME_INVALID, rtps_header.guidPrefix, reference);
        rtps_data.writerSN := rtps_data.writerSN + 1;
        -- Valid INFO_DESTINATION [Expected GUID Prefix, Little Endian] (No Change)
        rtps_sub.flags(SUBMESSAGE_ENDIAN_FLAG_POS)  := '1';
        gen_rtps_submessage(rtps_sub, stimulus);
        -- Valid Data (Expected)
        gen_rtps_submessage(rtps_data, stimulus);
        gen_rtps_handler_out(rtps_data, UDP_META.src, TRUE, TIME_INVALID, rtps_header.guidPrefix, reference);
        rtps_data.writerSN := rtps_data.writerSN + 1;
        -- Valid INFO_DESTINATION [Unexpected GUID Prefix] (Dropped)
        rtps_sub                        := DEFAULT_RTPS_SUBMESSAGE;
        rtps_sub.submessageID           := SID_INFO_DST;
        rtps_sub.guidPrefix             := DEFAULT_GUIDPREFIX;
        gen_rtps_submessage(rtps_sub, stimulus);
        -- Valid Data (Dropped)
        gen_rtps_submessage(rtps_data, stimulus);
        rtps_data.writerSN := rtps_data.writerSN + 1;
        -- Finalize Packet
        fix_output_packet(stimulus);
        start_test;
        wait_on_complete;
        check_cnt := check_cnt + reference.length;
        stimulus    := EMPTY_TEST_PACKET;
        reference   := EMPTY_TEST_PACKET;
        
        Log("Sending invalid INFO_DESTINATION [Submessage Length too small]", INFO);
        -- UDP Header
        gen_output_header(UDP_META, stimulus);
        -- RTPS Header
        gen_rtps_header(rtps_header, stimulus);
        -- Invalid INFO_DESTINATION [SubLength invalid] (Dropped)
        rtps_sub                        := DEFAULT_RTPS_SUBMESSAGE;
        rtps_sub.submessageID           := SID_INFO_DST;
        rtps_sub.submessageLength       := int(8,16);
        gen_rtps_submessage(rtps_sub, stimulus);
        -- Valid Data (Dropped)
        gen_rtps_submessage(rtps_data, stimulus);
        rtps_data.writerSN := rtps_data.writerSN + 1;
        -- Finalize Packet
        fix_output_packet(stimulus);
        start_test;
        wait_on_complete;
        check_cnt := check_cnt + reference.length;
        stimulus    := EMPTY_TEST_PACKET;
        reference   := EMPTY_TEST_PACKET;
        
        
        -- *INFO_REPLY SUBMESSAGE*
        Log("Testing INFO_REPLY interpretation", INFO);
        -- UDP Header
        gen_output_header(UDP_META, stimulus);
        -- RTPS Header
        gen_rtps_header(rtps_header, stimulus);
        -- Valid Data (Expected)
        gen_rtps_submessage(rtps_data, stimulus);
        gen_rtps_handler_out(rtps_data, UDP_META.src, TRUE, TIME_INVALID, rtps_header.guidPrefix, reference);
        rtps_data.writerSN := rtps_data.writerSN + 1;
        -- Valid INFO_REPLY [Empty] (No Change)
        rtps_sub                        := DEFAULT_RTPS_SUBMESSAGE;
        rtps_sub.submessageID           := SID_INFO_REPLY;
        gen_rtps_submessage(rtps_sub, stimulus);
        -- Valid Data (Expected)
        gen_rtps_submessage(rtps_data, stimulus);
        gen_rtps_handler_out(rtps_data, UDP_META.src, TRUE, TIME_INVALID, rtps_header.guidPrefix, reference);
        rtps_data.writerSN := rtps_data.writerSN + 1;
        -- Valid INFO_REPLY [2 valid Unicast, Big Endian] (Second Unicast applied)
        rtps_sub.unicastLocatorList.numLocators         := int(2,32);
        rtps_sub.unicastLocatorList.locator(0)          := gen_rand_loc_2;
        rtps_sub.unicastLocatorList.locator(1)          := gen_rand_loc_2;
        gen_rtps_submessage(rtps_sub, stimulus);
        tmp_loc := rtps_sub.unicastLocatorList.locator(1);
        -- Valid Data (Expected)
        gen_rtps_submessage(rtps_data, stimulus);
        gen_rtps_handler_out(rtps_data, tmp_loc, TRUE, TIME_INVALID, rtps_header.guidPrefix, reference);
        rtps_data.writerSN := rtps_data.writerSN + 1;
        -- Valid INFO_REPLY [1 valid Unicast, 1 invalid Unicast, 2 valid Multicast, Little Endian] (Second Multicast applied)
        rtps_sub.flags(SUBMESSAGE_ENDIAN_FLAG_POS)      := '1';
        rtps_sub.unicastLocatorList.locator(1).portn    := (others => '0'); -- Invalid Port
        rtps_sub.flags(SUBMESSAGE_MULTICAST_FLAG_POS)   := '1';
        rtps_sub.multicastLocatorList.numLocators       := int(2,32);
        rtps_sub.multicastLocatorList.locator(0)        := gen_rand_loc_2;
        rtps_sub.multicastLocatorList.locator(1)        := gen_rand_loc_2;
        gen_rtps_submessage(rtps_sub, stimulus);
        tmp_loc := rtps_sub.multicastLocatorList.locator(1);
        -- Valid Data (Expected)
        gen_rtps_submessage(rtps_data, stimulus);
        gen_rtps_handler_out(rtps_data, tmp_loc, TRUE, TIME_INVALID, rtps_header.guidPrefix, reference);
        rtps_data.writerSN := rtps_data.writerSN + 1;
        -- Valid INFO_REPLY [2 invalid Unicast, 2 invalid Multicast] (No change)
        rtps_sub.flags(SUBMESSAGE_ENDIAN_FLAG_POS)      := '0';
        rtps_sub.unicastLocatorList.locator(0).addr     := (others => '0'); -- Invalid Address
        rtps_sub.multicastLocatorList.locator(0).addr   := (others => '0'); -- Invalid Address
        rtps_sub.multicastLocatorList.locator(1).portn  := (others => '0'); -- Invalid Port
        gen_rtps_submessage(rtps_sub, stimulus);
        -- Valid Data (Expected)
        gen_rtps_submessage(rtps_data, stimulus);
        gen_rtps_handler_out(rtps_data, tmp_loc, TRUE, TIME_INVALID, rtps_header.guidPrefix, reference);
        rtps_data.writerSN := rtps_data.writerSN + 1;
        -- Finalize Packet
        fix_output_packet(stimulus);
        start_test;
        wait_on_complete;
        check_cnt := check_cnt + reference.length;
        stimulus    := EMPTY_TEST_PACKET;
        reference   := EMPTY_TEST_PACKET;
        
        Log("Sending invalid INFO_REPLY [Submessage Length too small]", INFO);
        -- UDP Header
        gen_output_header(UDP_META, stimulus);
        -- RTPS Header
        gen_rtps_header(rtps_header, stimulus);
        -- Invalid INFO_REPLY [SubLength invalid] (Dropped)
        rtps_sub                                := DEFAULT_RTPS_SUBMESSAGE;
        rtps_sub.submessageID                   := SID_INFO_REPLY;
        rtps_sub.unicastLocatorList.numLocators := int(2,32);
        rtps_sub.unicastLocatorList.locator(0)  := gen_rand_loc_2;
        rtps_sub.unicastLocatorList.locator(1)  := gen_rand_loc_2;
        rtps_sub.submessageLength               := int(48,16);
        gen_rtps_submessage(rtps_sub, stimulus);
        -- Valid Data (Dropped)
        gen_rtps_submessage(rtps_data, stimulus);
        rtps_data.writerSN := rtps_data.writerSN + 1;
        -- Finalize Packet
        fix_output_packet(stimulus);
        start_test;
        wait_on_complete;
        check_cnt := check_cnt + reference.length;
        stimulus    := EMPTY_TEST_PACKET;
        reference   := EMPTY_TEST_PACKET;
        
        
        -- *INFO_REPLY_IP4 SUBMESSAGE*
        Log("Testing INFO_REPLY_IP4 interpretation", INFO);
        -- UDP Header
        gen_output_header(UDP_META, stimulus);
        -- RTPS Header
        gen_rtps_header(rtps_header, stimulus);
        -- Valid Data (Expected)
        gen_rtps_submessage(rtps_data, stimulus);
        gen_rtps_handler_out(rtps_data, UDP_META.src, TRUE, TIME_INVALID, rtps_header.guidPrefix, reference);
        rtps_data.writerSN := rtps_data.writerSN + 1;
        -- Valid INFO_REPLY_IP4 [Valid Unicast, Big Endian] (Unicast applied)
        rtps_sub                        := DEFAULT_RTPS_SUBMESSAGE;
        rtps_sub.submessageID           := SID_INFO_REPLY_IP4;
        rtps_sub.unicastLocator         := gen_rand_loc_2;
        gen_rtps_submessage(rtps_sub, stimulus);
        tmp_loc := rtps_sub.unicastLocator;
        -- Valid Data (Expected)
        gen_rtps_submessage(rtps_data, stimulus);
        gen_rtps_handler_out(rtps_data, tmp_loc, TRUE, TIME_INVALID, rtps_header.guidPrefix, reference);
        rtps_data.writerSN := rtps_data.writerSN + 1;
        -- Valid INFO_REPLY_IP4 [Invalid Unicast, Valid Multicast, Little Endian] (Multicast applied)
        rtps_sub.flags(SUBMESSAGE_ENDIAN_FLAG_POS)      := '1';
        rtps_sub.flags(SUBMESSAGE_MULTICAST_FLAG_POS)   := '1';
        rtps_sub.unicastLocator.addr                    := (others => '0'); -- Invalid Address
        rtps_sub.multicastLocator                       := gen_rand_loc_2;
        gen_rtps_submessage(rtps_sub, stimulus);
        tmp_loc := rtps_sub.multicastLocator;
        -- Valid Data (Expected)
        gen_rtps_submessage(rtps_data, stimulus);
        gen_rtps_handler_out(rtps_data, tmp_loc, TRUE, TIME_INVALID, rtps_header.guidPrefix, reference);
        rtps_data.writerSN := rtps_data.writerSN + 1;
        -- Valid INFO_REPLY_IP4 [Invalid Unicast, Invalid Multicast] (No change)
        rtps_sub.flags(SUBMESSAGE_ENDIAN_FLAG_POS)  := '0';
        rtps_sub.multicastLocator.portn             := (others => '0'); -- Invalid Port
        gen_rtps_submessage(rtps_sub, stimulus);
        -- Valid Data (Expected)
        gen_rtps_submessage(rtps_data, stimulus);
        gen_rtps_handler_out(rtps_data, tmp_loc, TRUE, TIME_INVALID, rtps_header.guidPrefix, reference);
        rtps_data.writerSN := rtps_data.writerSN + 1;
        -- Finalize Packet
        fix_output_packet(stimulus);
        start_test;
        wait_on_complete;
        check_cnt := check_cnt + reference.length;
        stimulus    := EMPTY_TEST_PACKET;
        reference   := EMPTY_TEST_PACKET;
        
        Log("Sending invalid INFO_REPLY_IP4 [Submessage Length too small]", INFO);
        -- UDP Header
        gen_output_header(UDP_META, stimulus);
        -- RTPS Header
        gen_rtps_header(rtps_header, stimulus);
        -- Invalid INFO_REPLY_IP4 [SubLength invalid] (Dropped)
        rtps_sub                        := DEFAULT_RTPS_SUBMESSAGE;
        rtps_sub.submessageID           := SID_INFO_REPLY_IP4;
        rtps_sub.submessageLength       := int(4,16);
        gen_rtps_submessage(rtps_sub, stimulus);
        -- Valid Data (Dropped)
        gen_rtps_submessage(rtps_data, stimulus);
        rtps_data.writerSN := rtps_data.writerSN + 1;
        -- Finalize Packet
        fix_output_packet(stimulus);
        start_test;
        wait_on_complete;
        check_cnt := check_cnt + reference.length;
        stimulus    := EMPTY_TEST_PACKET;
        reference   := EMPTY_TEST_PACKET;
        
        -- *INFO TS*
        Log("Testing INFO_TIMESTAMP interpretation", INFO);
        -- UDP Header
        gen_output_header(UDP_META, stimulus);
        -- RTPS Header
        gen_rtps_header(rtps_header, stimulus);
        -- Valid Data (Expected)
        gen_rtps_submessage(rtps_data, stimulus);
        gen_rtps_handler_out(rtps_data, UDP_META.src, TRUE, TIME_INVALID, rtps_header.guidPrefix, reference);
        rtps_data.writerSN := rtps_data.writerSN + 1;
        -- Valid INFO_TIMESTAMP [Big Endian] (Change Timestamp)
        rtps_sub                        := DEFAULT_RTPS_SUBMESSAGE;
        rtps_sub.submessageID           := SID_INFO_TS;
        rtps_sub.timestamp              := (unsigned(RV.RandSlv(WORD_WIDTH)), unsigned(RV.RandSlv(WORD_WIDTH)));
        gen_rtps_submessage(rtps_sub, stimulus);
        ts  := rtps_sub.timestamp;
        -- Valid Data (Expected)
        gen_rtps_submessage(rtps_data, stimulus);
        gen_rtps_handler_out(rtps_data, UDP_META.src, TRUE, ts, rtps_header.guidPrefix, reference);
        rtps_data.writerSN := rtps_data.writerSN + 1;
        -- Valid INFO_TIMESTAMP [Little Endian] (Change Timestamp)
        rtps_sub.flags(SUBMESSAGE_ENDIAN_FLAG_POS)  := '1';
        rtps_sub.timestamp                          := (unsigned(RV.RandSlv(WORD_WIDTH)), unsigned(RV.RandSlv(WORD_WIDTH)));
        gen_rtps_submessage(rtps_sub, stimulus);
        ts  := rtps_sub.timestamp;
        -- Valid Data (Expected)
        gen_rtps_submessage(rtps_data, stimulus);
        gen_rtps_handler_out(rtps_data, UDP_META.src, TRUE, ts, rtps_header.guidPrefix, reference);
        rtps_data.writerSN := rtps_data.writerSN + 1;
        -- Valid INFO_TIMESTAMP [Invalidate Time] (Invalidate Timestamp)
        rtps_sub.flags(SUBMESSAGE_ENDIAN_FLAG_POS)      := '0';
        rtps_sub.flags(SUBMESSAGE_INVALIDATE_FLAG_POS)  := '1';
        gen_rtps_submessage(rtps_sub, stimulus);
        ts  := TIME_INVALID;
        -- Valid Data (Expected)
        gen_rtps_submessage(rtps_data, stimulus);
        gen_rtps_handler_out(rtps_data, UDP_META.src, TRUE, ts, rtps_header.guidPrefix, reference);
        rtps_data.writerSN := rtps_data.writerSN + 1;
        -- Finalize Packet
        fix_output_packet(stimulus);
        start_test;
        wait_on_complete;
        check_cnt := check_cnt + reference.length;
        stimulus    := EMPTY_TEST_PACKET;
        reference   := EMPTY_TEST_PACKET;
        
        Log("Sending invalid INFO_TMESTAMP [Submessage Length too small]", INFO);
        -- UDP Header
        gen_output_header(UDP_META, stimulus);
        -- RTPS Header
        gen_rtps_header(rtps_header, stimulus);
        -- Invalid INFO_TIMESTAMP [SubLength invalid] (Dropped)
        rtps_sub                        := DEFAULT_RTPS_SUBMESSAGE;
        rtps_sub.submessageID           := SID_INFO_TS;
        rtps_sub.submessageLength       := int(4,16);
        gen_rtps_submessage(rtps_sub, stimulus);
        -- Valid Data (Dropped)
        gen_rtps_submessage(rtps_data, stimulus);
        rtps_data.writerSN := rtps_data.writerSN + 1;
        -- Finalize Packet
        fix_output_packet(stimulus);
        start_test;
        wait_on_complete;
        check_cnt := check_cnt + reference.length;
        stimulus    := EMPTY_TEST_PACKET;
        reference   := EMPTY_TEST_PACKET;
        
        
        -- *PAD*
        Log("Testing PAD interpretation", INFO);
        -- UDP Header
        gen_output_header(UDP_META, stimulus);
        -- RTPS Header
        gen_rtps_header(rtps_header, stimulus);
        -- Valid Data (Expected)
        gen_rtps_submessage(rtps_data, stimulus);
        gen_rtps_handler_out(rtps_data, UDP_META.src, TRUE, TIME_INVALID, rtps_header.guidPrefix, reference);
        rtps_data.writerSN := rtps_data.writerSN + 1;
        -- Valid PAD (No Change)
        rtps_sub                        := DEFAULT_RTPS_SUBMESSAGE;
        rtps_sub.submessageID           := SID_PAD;
        rtps_sub.submessageLength       := int(28,16);
        gen_rtps_submessage(rtps_sub, stimulus);
        -- Valid Data (Expected)
        gen_rtps_submessage(rtps_data, stimulus);
        gen_rtps_handler_out(rtps_data, UDP_META.src, TRUE, TIME_INVALID, rtps_header.guidPrefix, reference);
        rtps_data.writerSN := rtps_data.writerSN + 1;
        -- Finalize Packet
        fix_output_packet(stimulus);
        start_test;
        wait_on_complete;
        check_cnt := check_cnt + reference.length;
        stimulus    := EMPTY_TEST_PACKET;
        reference   := EMPTY_TEST_PACKET;
        
        
        -- *UNKNOWN*
        Log("Testing unknown Submessage handling", INFO);
        -- UDP Header
        gen_output_header(UDP_META, stimulus);
        -- RTPS Header
        gen_rtps_header(rtps_header, stimulus);
        -- Valid Data (Expected)
        gen_rtps_submessage(rtps_data, stimulus);
        gen_rtps_handler_out(rtps_data, UDP_META.src, TRUE, TIME_INVALID, rtps_header.guidPrefix, reference);
        rtps_data.writerSN := rtps_data.writerSN + 1;
        -- Valid DATA_FRAG (Dropped)
        rtps_sub                        := DEFAULT_RTPS_SUBMESSAGE;
        rtps_sub.submessageID           := SID_DATA_FRAG;
        rtps_sub.writerId               := DEFAULT_WRITER_ENTITYID;
        gen_rtps_submessage(rtps_sub, stimulus);
        -- Valid Data (Expected)
        gen_rtps_submessage(rtps_data, stimulus);
        gen_rtps_handler_out(rtps_data, UDP_META.src, TRUE, TIME_INVALID, rtps_header.guidPrefix, reference);
        rtps_data.writerSN := rtps_data.writerSN + 1;
        -- Valid HEARTBEAT_FRAG (Dropped)
        rtps_sub                        := DEFAULT_RTPS_SUBMESSAGE;
        rtps_sub.submessageID           := SID_HEARTBEAT_FRAG;
        rtps_sub.writerId               := DEFAULT_WRITER_ENTITYID;
        gen_rtps_submessage(rtps_sub, stimulus);
        -- Valid Data (Expected)
        gen_rtps_submessage(rtps_data, stimulus);
        gen_rtps_handler_out(rtps_data, UDP_META.src, TRUE, TIME_INVALID, rtps_header.guidPrefix, reference);
        rtps_data.writerSN := rtps_data.writerSN + 1;
        -- Valid NACK_FRAG (Dropped)
        rtps_sub                        := DEFAULT_RTPS_SUBMESSAGE;
        rtps_sub.submessageID           := SID_NACK_FRAG;
        rtps_sub.readerId               := DEFAULT_READER_ENTITYID;
        gen_rtps_submessage(rtps_sub, stimulus);
        -- Valid Data (Expected)
        gen_rtps_submessage(rtps_data, stimulus);
        gen_rtps_handler_out(rtps_data, UDP_META.src, TRUE, TIME_INVALID, rtps_header.guidPrefix, reference);
        rtps_data.writerSN := rtps_data.writerSN + 1;
        -- UKNOWN (Dropped)
        rtps_sub                        := DEFAULT_RTPS_SUBMESSAGE;
        rtps_sub.submessageID           := (others => '1');
        rtps_sub.submessageLength       := int(28,16);
        gen_rtps_submessage(rtps_sub, stimulus);
        -- Valid Data (Expected)
        gen_rtps_submessage(rtps_data, stimulus);
        gen_rtps_handler_out(rtps_data, UDP_META.src, TRUE, TIME_INVALID, rtps_header.guidPrefix, reference);
        rtps_data.writerSN := rtps_data.writerSN + 1;
        -- Finalize Packet
        fix_output_packet(stimulus);
        start_test;
        wait_on_complete;
        check_cnt := check_cnt + reference.length;
        stimulus    := EMPTY_TEST_PACKET;
        reference   := EMPTY_TEST_PACKET;
        
        TranscriptOpen(RESULTS_FILE, APPEND_MODE);
        SetTranscriptMirror;
        AlertIf(GetAffirmCount < check_cnt, "Incomplete test run");
        ReportAlerts;
        TranscriptClose;
        std.env.stop;
        wait;
    end process;
    
    clock_prc : process
    begin
        clk <= '0';
        wait for 25 ns;
        clk <= '1';
        wait for 25 ns;
    end process;
    
    fifo_ctl_prc : process
    begin
        in_empty        <= '0';
        builtin_full    <= '0';
        user_full       <= (others => '0');
        wait until rising_edge(clk);
        in_empty        <= '1';
        builtin_full    <= '0';
        user_full       <= (others => '0');
        wait until rising_edge(clk);
        in_empty        <= '0';
        builtin_full    <= '1';
        user_full       <= (others => '0');
        wait until rising_edge(clk);
        in_empty        <= '0';
        builtin_full    <= '0';
        user_full       <= (others => '1');
        wait until rising_edge(clk);
    end process;
    
    alert_prc : process(all)
    begin
        if rising_edge(clk) then
            alertif(in_empty = '1' and rd_sig = '1', "Input FIFO read signal high while empty signal high", ERROR);
            alertif(builtin_full = '1' and builtin_wr = '1', "Builtin FIFO write signal high while full signal high", ERROR);
            alertif(user_full /= (0 to NUM_ENDPOINTS-1 => '0') and (user_wr /= (0 to NUM_ENDPOINTS-1 => '0')), "User FIFO write signal high while full signal high", ERROR);
        end if;
    end process;
    
    input_prc : process(all)
    begin
        data_in     <= stimulus.data(cnt_stim);
        
        if rising_edge(clk) then
            if (reset = '1') then
                cnt_stim    <= 0;
                stim_stage  <= IDLE;
                packet_sent <= '1';
            else
                case (stim_stage) is
                    when IDLE =>
                        if (start = '1' and stimulus.length /= 0) then
                            stim_stage  <= BUSY;
                            packet_sent <= '0';
                        end if;
                    when BUSY =>
                        if (rd_sig = '1') then
                            if (cnt_stim = stimulus.length-1) then
                                stim_stage  <= IDLE;
                                packet_sent <= '1';
                                cnt_stim    <= 0;
                            else
                                cnt_stim    <= cnt_stim + 1;
                            end if;
                        end if;
                end case;
            end if;
        end if;
    end process;
    
    output_prc : process(all)
    begin
        case (ref_stage) is
            when IDLE =>
                packet_checked  <= '1';
            when BUSY =>
                packet_checked  <= '0';
        end case;
        
        if rising_edge(clk) then
            if (reset = '1') then
                cnt_ref         <= 0;
                ref_stage       <= IDLE;
            else
                case (ref_stage) is
                    when IDLE =>
                        if (start = '1') then
                            ref_stage   <= BUSY;
                            cnt_ref     <= 0;
                        end if;
                    when BUSY =>
                        if (cnt_ref = reference.length) then
                            ref_stage   <= IDLE;
                        elsif (builtin_wr = '1' or user_wr /= (0 to NUM_ENDPOINTS-1 => '0')) then
                            AffirmIfEqual(last_word_out & data_out, reference.last(cnt_ref) & reference.data(cnt_ref));
                            cnt_ref <= cnt_ref + 1;
                        end if;
                end case;
            end if;
        end if;
    end process;
    
    watchdog : process
    begin
        wait for 1 ms;
        Alert("Test timeout", FAILURE);
        std.env.stop;
    end process;
    
end architecture;