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 rtps output of the builtin endpoint (Local Liveliness Assertion, Local Heartbeat Generation, Local Participant Announcement, Remote HEARTBEAT Response, Remote ACKNACK Response).
-- This test is a Level 1 Test (Meaning the input/output is not connected directly to the uut) in order to have output in the same format as the input of the system and allow us to compare using existing data generators.
-- The testflow is as follows:
--  * 0s
--      - Match a remote participant
--      - Send HEARTBEAT 0 (Publisher, Empty, Final Flag) [Test Final Flag pasing]
--  * 0.105s
--      - No HEARTBEAT 0 Response
--      - Send HEARTBEAT 1 (Publisher, Empty) [Test empty HEARTBEAT Response]
--      - Send HEARTBEAT 2 (Subscriber, Empty, Final Flag) [Should not change anything, since ACKNACKs are sent in unison, and an ACKNACK is already scheduled]
--      - Send ACKNACK 1 (Message, Expecting SN 1) [Test normal ACKNACK Response]
--  * 0.210s
--      - HEARTBEAT 1 Response [All ACKNACKs sent]
--      - Liveliness of local Writer 4 (with MANUAL_BY_PARTICIPANT Liveliness QOS) is asserted for 1 clock cycle [Test manual liveliness assertion]
--  * 0.310s
--      - ACKNACK 1 Response
--      - Send HEARTBEAT 3 (Publisher, First SN 1, Last SN 5)   [Test HEARTBEAT Suppression Delay]
--  * 0.415s
--      - No HEARTBEAT 3 Response
--      - Send HEARTBEAT 4 (Publisher, First SN 1, Last SN 5)  [Test normal HARTBEAT Response]
--      - Send ACKNACK 2 (Publisher, Expecting 1) [Test ACKNACK Suppression Delay]
--  * 0.450s
--      - Send HEARTBEAT 5 (Subscriber, First Sn 2, Last SN 5) [Test HEARTBEAT Response Delay, Test HEARTBEAT Resposne Update]
--  * 0.520s
--      - HEARTBEAT 4/5 Response
--      - Send ACKNACK 3 (Publisher, Expecting SN 1) [Test normal ACKNACK Response]
--  * 0.620s
--      - No ACKNACK 2 Response
--      - Send ACKNACK 4 (Subscriber, Expecting SN 1) [Test ACKNACK Resposne Delay, Test ACKNACK Response Update]
--  * 0.725s
--      - ACKNACK 3/4 Response
--  * 1.000s
--      - Local HEARTBEAT generation
--      - Liveliness Assertion (Manual & Auto)
--      - Send ACKNACK 5 (Subscriber, Expecting SN 8) [Test ACKNACK SN Response]
--  * 1.205s
--      - ACKNACK 5 Response
--  * 1.500s
--      - Send ACKNACK 6 (Publihser, Expecting SN 18) [Test ACKNACK SN Response]
--  * 1.705s
--      - No ACKNACK 6 Response
--  * 2.000s
--      - Local HEARTBEAT generation
--      - Liveliness Assertion (Auto only)
--      - Send ACKNACK 7 (Message, Expecting SN 5) [Test GAP sending on ACKNACK Response]
--      - Send ACKNACK 8 (Publihser, Expecing SN 2) [Test full ACKNACK Response]
--      - Send ACKNACK 9 (Subscriber, Expecting SN 8) [Test full ACKNACK Response]
--  * 2.205s
--      - ACKNACK 7/8/9 Response
--      - Liveliness of local Writer 0 (with AUTOMATIC Liveliness QoS) is asserted for 1 clock cycle [Test manual liveliness assertion after GAP generation]
--  * 2.500s
--      - Send ACKNACK 10 (Message, Expecting SN 6) [Test ACKNACK SN Response]
--  * 2.705s
--      - No ACKNACK 10 Response
--  * 3.000s
--      - Local HEARTBEAT generation
--      - Liveliness Assertion (Manual & Auto)
--  * 31.000s
--      - Local participant Announcement
--      - Local HEARTBEAT generation
--      - Liveliness Assertion (Auto only)


entity L1_rtps_builtin_endpoint_test1 is
end entity;

architecture testbench of L1_rtps_builtin_endpoint_test1 is
    
    -- *TYPE DECLARATION*
    type TEST_STAGE_TYPE is (IDLE, BUSY);
    
    -- *SIGNAL DECLARATION*
    signal clk, in_empty, rd_sig, last_word_in, full, wr_sig : std_logic := '0';
    signal reset : std_logic := '1';
    signal data_in, data_out : std_logic_vector(WORD_WIDTH-1 downto 0) := (others => '0');
    signal stim_stage : TEST_STAGE_TYPE := IDLE;
    shared variable stimulus : TEST_PACKET_TYPE := EMPTY_TEST_PACKET;
    signal packet_sent : std_logic := '0';
    signal cnt_stim : natural := 0;
    signal start : std_logic := '0';
    shared variable SB_out : osvvm.ScoreBoardPkg_slv.ScoreBoardPType;
    signal stim_done, check_done : std_logic := '0';
    signal test_time : TIME_TYPE := TIME_ZERO;
    signal fifo_in, fifo_out : std_logic_vector(WORD_WIDTH downto 0) := (others => '0');
    signal fifo_wr, fifo_empty, fifo_full : std_logic := '0';
    signal rtps_out_data : RTPS_OUT_DATA_TYPE := (others => (others => '0'));
    signal rtps_out_rd, rtps_out_last_word_in, rtps_out_empty : std_logic_vector(0 to NUM_ENDPOINTS) := (others => '0');
    signal alive : std_logic_vector (0 to NUM_ENDPOINTS-1) := (others => '0');
    
    -- *FUNCTION DECLARATION*
    procedure wait_on_complete is
    begin
        wait until rising_edge(packet_sent);
    end procedure;
    
    function gen_sn(input : natural) return SEQUENCENUMBER_TYPE is
        variable ret : SEQUENCENUMBER_TYPE;
    begin
        ret(0)  := (others => '0');
        ret(1)  := unsigned(int(input, WORD_WIDTH));
        return ret;
    end function;
    
    procedure gen_acknack(participant : in PARTICIPANT_DATA_TYPE; count : in natural; pub_sn : in SEQUENCENUMBER_TYPE; sub_sn : in SEQUENCENUMBER_TYPE; mes_sn : in SEQUENCENUMBER_TYPE) is
        variable sub        : RTPS_SUBMESSAGE_TYPE := DEFAULT_RTPS_SUBMESSAGE;
        variable OUT_HEADER : OUTPUT_HEADER_TYPE := DEFAULT_OUTPUT_HEADER;
        variable rtps_header: RTPS_HEADER_TYPE := DEFAULT_RTPS_HEADER;
        variable output     : TEST_PACKET_TYPE := EMPTY_TEST_PACKET;
    begin
        -- NOTE: Ha to be made sure uut used same Locator
        OUT_HEADER  := (dest => get_loc(participant, TRUE), src => DEST_LOC.meta.locator(3));
        
        -- OUTPUT HEADER
        gen_output_header(OUT_HEADER, output);
        -- RTPS HEADER
        rtps_header             := DEFAULT_RTPS_HEADER;
        rtps_header.guidPrefix  := GUIDPREFIX;
        gen_rtps_header(rtps_header, output);
        -- PUBLISHER ACKNACK
        sub                         := DEFAULT_RTPS_SUBMESSAGE;
        sub.submessageID            := SID_ACKNACK;
        sub.writerId                := ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_ANNOUNCER;
        sub.readerId                := ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_DETECTOR;
        sub.readerSNState.base      := pub_sn;
        sub.readerSNState.numBits   := int(WORD_WIDTH, CDR_LONG_WIDTH);
        sub.readerSNState.bitmap    := (0 to WORD_WIDTH-1 => '1', others => '0');
        sub.count                   := int(count, CDR_LONG_WIDTH);
        sub.flags(SUBMESSAGE_FINAL_FLAG_POS) := '1';
        gen_rtps_submessage(sub, output);
        -- SUBSCRIBER ACKNACK
        sub                         := DEFAULT_RTPS_SUBMESSAGE;
        sub.submessageID            := SID_ACKNACK;
        sub.writerId                := ENTITYID_SEDP_BUILTIN_PUBLICATIONS_ANNOUNCER;
        sub.readerId                := ENTITYID_SEDP_BUILTIN_PUBLICATIONS_DETECTOR;
        sub.readerSNState.base      := sub_sn;
        sub.readerSNState.numBits   := int(WORD_WIDTH, CDR_LONG_WIDTH);
        sub.readerSNState.bitmap    := (0 to WORD_WIDTH-1 => '1', others => '0');
        sub.count                   := int(count, CDR_LONG_WIDTH);
        sub.flags(SUBMESSAGE_FINAL_FLAG_POS) := '1';
        gen_rtps_submessage(sub, output);
        -- MESSAGE ACKNACK
        sub                         := DEFAULT_RTPS_SUBMESSAGE;
        sub.submessageID            := SID_ACKNACK;
        sub.writerId                := ENTITYID_P2P_BUILTIN_PARTICIPANT_MESSAGE_WRITER;
        sub.readerId                := ENTITYID_P2P_BUILTIN_PARTICIPANT_MESSAGE_READER;
        sub.readerSNState.base      := mes_sn;
        sub.readerSNState.numBits   := int(WORD_WIDTH, CDR_LONG_WIDTH);
        sub.readerSNState.bitmap    := (0 to WORD_WIDTH-1 => '1', others => '0');
        sub.count                   := int(count, CDR_LONG_WIDTH);
        sub.flags(SUBMESSAGE_FINAL_FLAG_POS) := '1';
        gen_rtps_submessage(sub, output);
        fix_output_packet(output);
        
        for i in 0 to output.length-1 loop
            SB_out.Push(output.data(i));
        end loop;
    end procedure;
    
    procedure gen_announcement is
        variable sub        : RTPS_SUBMESSAGE_TYPE := DEFAULT_RTPS_SUBMESSAGE;
        variable OUT_HEADER : OUTPUT_HEADER_TYPE := DEFAULT_OUTPUT_HEADER;
        variable rtps_header: RTPS_HEADER_TYPE := DEFAULT_RTPS_HEADER;
        variable output     : TEST_PACKET_TYPE := EMPTY_TEST_PACKET;
    begin
        -- NOTE: Ha to be made sure uut used same Locator
        OUT_HEADER  := (dest => DEST_LOC.meta.locator(0), src => DEST_LOC.meta.locator(3));
        
        -- OUTPUT HEADER
        gen_output_header(OUT_HEADER, output);
        -- RTPS HEADER
        rtps_header             := DEFAULT_RTPS_HEADER;
        rtps_header.guidPrefix  := GUIDPREFIX;
        gen_rtps_header(rtps_header, output);
        -- PARTICIPANT DATA
        sub                 := DEFAULT_RTPS_SUBMESSAGE;
        sub.submessageID    := SID_DATA;
        sub.writerId        := ENTITYID_SPDP_BUILTIN_PARTICIPANT_ANNOUNCER;
        sub.readerId        := ENTITYID_SPDP_BUILTIN_PARTICIPANT_DETECTOR;
        sub.writerSN        := gen_sn(1);
        sub.flags(SUBMESSAGE_DATA_FLAG_POS) := '1';
        gen_participant_data(THIS_PARTICIPANT_DATA, sub.data);
        gen_sentinel(sub.data);
        gen_rtps_submessage(sub, output);
        fix_output_packet(output);
        
        for i in 0 to output.length-1 loop
            SB_out.Push(output.data(i));
        end loop;
    end procedure;
    
    procedure gen_heartbeat(participant : in PARTICIPANT_DATA_TYPE; count : in natural; man_sn : in SEQUENCENUMBER_TYPE; auto_sn : in SEQUENCENUMBER_TYPE) is
        variable sub        : RTPS_SUBMESSAGE_TYPE := DEFAULT_RTPS_SUBMESSAGE;
        variable OUT_HEADER : OUTPUT_HEADER_TYPE := DEFAULT_OUTPUT_HEADER;
        variable rtps_header: RTPS_HEADER_TYPE := DEFAULT_RTPS_HEADER;
        variable output     : TEST_PACKET_TYPE := EMPTY_TEST_PACKET;
    begin
        -- NOTE: Has to be made sure uut used same Locator
        OUT_HEADER  := (dest => get_loc(participant, TRUE), src => DEST_LOC.meta.locator(3));
        
        -- OUTPUT HEADER
        gen_output_header(OUT_HEADER, output);
        -- RTPS HEADER
        rtps_header             := DEFAULT_RTPS_HEADER;
        rtps_header.guidPrefix  := GUIDPREFIX;
        gen_rtps_header(rtps_header, output);
        -- PUBLISHER HEARTBEAT
        sub                     := DEFAULT_RTPS_SUBMESSAGE;
        sub.submessageID        := SID_HEARTBEAT;
        sub.writerId            := ENTITYID_SEDP_BUILTIN_PUBLICATIONS_ANNOUNCER;
        sub.readerId            := ENTITYID_SEDP_BUILTIN_PUBLICATIONS_DETECTOR;
        sub.firstSN             := gen_sn(1);
        sub.lastSN              := gen_sn(NUM_WRITERS);
        sub.count               := std_logic_vector(to_unsigned(count, CDR_LONG_WIDTH));
        sub.flags(SUBMESSAGE_FINAL_FLAG_POS) := '1';
        gen_rtps_submessage(sub, output);
        -- SUBSCRIBER HEARTBEAT
        sub                     := DEFAULT_RTPS_SUBMESSAGE;
        sub.submessageID        := SID_HEARTBEAT;
        sub.writerId            := ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_ANNOUNCER;
        sub.readerId            := ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_DETECTOR;
        sub.count               := std_logic_vector(to_unsigned(count, CDR_LONG_WIDTH));
        sub.firstSN             := gen_sn(1);
        sub.lastSN              := gen_sn(NUM_READERS);
        sub.flags(SUBMESSAGE_FINAL_FLAG_POS) := '1';
        gen_rtps_submessage(sub, output);
        -- MESSAGE HEARTBEAT
        sub                     := DEFAULT_RTPS_SUBMESSAGE;
        sub.submessageID        := SID_HEARTBEAT;
        sub.writerId            := ENTITYID_P2P_BUILTIN_PARTICIPANT_MESSAGE_WRITER;
        sub.readerId            := ENTITYID_P2P_BUILTIN_PARTICIPANT_MESSAGE_READER;
        sub.count               := std_logic_vector(to_unsigned(count, CDR_LONG_WIDTH));
        sub.firstSN             := man_sn;
        sub.lastSN              := auto_sn;
        sub.flags(SUBMESSAGE_FINAL_FLAG_POS) := '1';
        gen_rtps_submessage(sub, output);
        -- MANUAL LIVELINESS
        if (man_sn+1 = auto_sn) then
            sub                 := DEFAULT_RTPS_SUBMESSAGE;
            sub.submessageID    := SID_DATA;
            sub.writerId        := ENTITYID_P2P_BUILTIN_PARTICIPANT_MESSAGE_WRITER;
            sub.readerId        := ENTITYID_P2P_BUILTIN_PARTICIPANT_MESSAGE_READER;
            sub.writerSN        := man_sn;
            sub.flags(SUBMESSAGE_DATA_FLAG_POS) := '1';
            gen_liveliness_assertion(THIS_PARTICIPANT_DATA, TRUE, sub.data);
            gen_rtps_submessage(sub, output);
        end if;
        -- AUTOMATIC LIVELINESS
        sub                 := DEFAULT_RTPS_SUBMESSAGE;
        sub.submessageID    := SID_DATA;
        sub.writerId        := ENTITYID_P2P_BUILTIN_PARTICIPANT_MESSAGE_WRITER;
        sub.readerId        := ENTITYID_P2P_BUILTIN_PARTICIPANT_MESSAGE_READER;
        sub.writerSN        := auto_sn;
        sub.flags(SUBMESSAGE_DATA_FLAG_POS) := '1';
        gen_liveliness_assertion(THIS_PARTICIPANT_DATA, FALSE, sub.data);
        gen_rtps_submessage(sub, output);
        fix_output_packet(output);
        
        for i in 0 to output.length-1 loop
            SB_out.Push(output.data(i));
        end loop;
    end procedure;
    
    procedure gen_data(participant : in PARTICIPANT_DATA_TYPE; pub : in boolean; subs : in boolean; mes : in boolean; man_sn : in SEQUENCENUMBER_TYPE; auto_sn : in SEQUENCENUMBER_TYPE) is
        variable fixed_readers  : ENDPOINT_DATA_ARRAY_TYPE(0 to NUM_READERS-1) := gen_endpoint_array(TRUE);
        variable fixed_writers  : ENDPOINT_DATA_ARRAY_TYPE(0 to NUM_WRITERS-1) := gen_endpoint_array(FALSE);
        variable sub            : RTPS_SUBMESSAGE_TYPE := DEFAULT_RTPS_SUBMESSAGE;
        variable OUT_HEADER     : OUTPUT_HEADER_TYPE := DEFAULT_OUTPUT_HEADER;
        variable rtps_header    : RTPS_HEADER_TYPE := DEFAULT_RTPS_HEADER;
        variable output         : TEST_PACKET_TYPE := EMPTY_TEST_PACKET;
    begin
        -- NOTE: Ha to be made sure uut used same Locator
        OUT_HEADER  := (dest => get_loc(participant, TRUE), src => DEST_LOC.meta.locator(3));
        
        -- *PUBLISHERS*
        if (pub) then
            -- OUTPUT HEADER
            gen_output_header(OUT_HEADER, output);
            -- RTPS HEADER
            rtps_header             := DEFAULT_RTPS_HEADER;
            rtps_header.guidPrefix  := GUIDPREFIX;
            gen_rtps_header(rtps_header, output);
            for i in 0 to NUM_WRITERS-1 loop
                -- PUBLISHER DATA
                sub                 := DEFAULT_RTPS_SUBMESSAGE;
                sub.submessageID    := SID_DATA;
                sub.writerId        := ENTITYID_SEDP_BUILTIN_PUBLICATIONS_ANNOUNCER;
                sub.readerId        := ENTITYID_SEDP_BUILTIN_PUBLICATIONS_DETECTOR;
                sub.writerSN        := gen_sn(i+1);
                sub.flags(SUBMESSAGE_DATA_FLAG_POS) := '1';
                gen_endpoint_data(fixed_writers(i), sub.data);
                gen_sentinel(sub.data);
                gen_rtps_submessage(sub, output);
            end loop;
            fix_output_packet(output);
            for i in 0 to output.length-1 loop
                SB_out.Push(output.data(i));
            end loop;
            output  := EMPTY_TEST_PACKET;
        end if;
        
        -- *SUBSCRIBERS*
        if (subs) then
            -- OUTPUT HEADER
            gen_output_header(OUT_HEADER, output);
            -- RTPS HEADER
            rtps_header             := DEFAULT_RTPS_HEADER;
            rtps_header.guidPrefix  := GUIDPREFIX;
            gen_rtps_header(rtps_header, output);
            for i in 0 to NUM_READERS-1 loop
                -- PUBLISHER DATA
                sub                 := DEFAULT_RTPS_SUBMESSAGE;
                sub.submessageID    := SID_DATA;
                sub.writerId        := ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_ANNOUNCER;
                sub.readerId        := ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_DETECTOR;
                sub.writerSN        := gen_sn(i+1);
                sub.flags(SUBMESSAGE_DATA_FLAG_POS) := '1';
                gen_endpoint_data(fixed_readers(i), sub.data);
                gen_sentinel(sub.data);
                gen_rtps_submessage(sub, output);
            end loop;
            fix_output_packet(output);
            for i in 0 to output.length-1 loop
                SB_out.Push(output.data(i));
            end loop;
            output  := EMPTY_TEST_PACKET;
        end if;
        
        -- *PARTICIPANT MESSAGE*
        if (mes) then
            -- OUTPUT HEADER
            gen_output_header(OUT_HEADER, output);
            -- RTPS HEADER
            rtps_header             := DEFAULT_RTPS_HEADER;
            rtps_header.guidPrefix  := GUIDPREFIX;
            gen_rtps_header(rtps_header, output);
            -- MANUAL LIVELINESS
            sub                 := DEFAULT_RTPS_SUBMESSAGE;
            sub.submessageID    := SID_DATA;
            sub.writerId        := ENTITYID_P2P_BUILTIN_PARTICIPANT_MESSAGE_WRITER;
            sub.readerId        := ENTITYID_P2P_BUILTIN_PARTICIPANT_MESSAGE_READER;
            sub.writerSN        := man_sn;
            sub.flags(SUBMESSAGE_DATA_FLAG_POS) := '1';
            gen_liveliness_assertion(THIS_PARTICIPANT_DATA, TRUE, sub.data);
            gen_rtps_submessage(sub, output);
            if (man_sn+1 /= auto_sn) then
                -- GAP MESSAGE
                sub                 := DEFAULT_RTPS_SUBMESSAGE;
                sub.submessageID    := SID_GAP;
                sub.writerId        := ENTITYID_P2P_BUILTIN_PARTICIPANT_MESSAGE_WRITER;
                sub.readerId        := ENTITYID_P2P_BUILTIN_PARTICIPANT_MESSAGE_READER;
                sub.gapStart        := man_sn+1;
                sub.gapList.base    := auto_sn-1;
                gen_rtps_submessage(sub, output);
            end if;
            -- AUTOMATIC LIVELINESS
            sub                 := DEFAULT_RTPS_SUBMESSAGE;
            sub.submessageID    := SID_DATA;
            sub.writerId        := ENTITYID_P2P_BUILTIN_PARTICIPANT_MESSAGE_WRITER;
            sub.readerId        := ENTITYID_P2P_BUILTIN_PARTICIPANT_MESSAGE_READER;
            sub.writerSN        := auto_sn;
            sub.flags(SUBMESSAGE_DATA_FLAG_POS) := '1';
            gen_liveliness_assertion(THIS_PARTICIPANT_DATA, FALSE, sub.data);
            gen_rtps_submessage(sub, output);
            fix_output_packet(output);
            for i in 0 to output.length-1 loop
                SB_out.Push(output.data(i));
            end loop;
            output  := EMPTY_TEST_PACKET;
        end if;
    end procedure;
    
begin
    
    -- Unit Under Test
    uut : entity work.rtps_builtin_endpoint(arch)
        generic map (
            MAX_REMOTE_PARTICIPANTS => 1
        )
        port map (
            -- SYSTEM
            clk                 => clk,
            reset               => reset,
            time                => test_time,
            -- FROM RTPS HANDLER
            empty               => in_empty or packet_sent,
            rd                  => rd_sig,
            data_in             => data_in,
            last_word_in        => last_word_in,
            -- FROM USER ENDPOINT
            alive               => alive,
            -- TO USER ENDPOINT
            full_ue             => (others => '0'),
            wr_ue               => open,
            data_out_ue         => open,
            last_word_out_ue    => open,
            -- RTPS OUTPUT
            full_ro             => fifo_full,
            wr_ro               => fifo_wr,
            data_out_ro         => fifo_in(WORD_WIDTH-1 downto 0),
            last_word_out_ro    => fifo_in(WORD_WIDTH)
        );
    
    fifo_inst : configuration work.FWFT_FIFO_cfg
        generic map (
            FIFO_DEPTH  => 2,
            DATA_WIDTH  => WORD_WIDTH+1
        )
        port map
        (
            reset       => reset,
            clk         => clk,
            data_in     => fifo_in,
            write       => fifo_wr,
            read        => rtps_out_rd(0),
            data_out    => fifo_out,
            empty       => fifo_empty,
            full        => fifo_full,
            free        => open
        );
    
    rtps_out_data           <= (0 => fifo_out(WORD_WIDTH-1 downto 0), others => (others => '0'));
    rtps_out_last_word_in   <= (0 => fifo_out(WORD_WIDTH), others => '0');
    rtps_out_empty          <= (0 => fifo_empty, others => '1');
    
    rtps_out_inst : entity work.rtps_out(arch)
        port map (
            clk         => clk,
            reset       => reset,
            data_in     => rtps_out_data,
            last_word_in=> rtps_out_last_word_in,
            rd          => rtps_out_rd,
            empty       => rtps_out_empty,
            data_out    => data_out,
            wr          => wr_sig,
            full        => full
        );
        
    
    stimulus_prc : process
        variable sub : RTPS_SUBMESSAGE_TYPE := DEFAULT_RTPS_SUBMESSAGE;
        variable RV                 : RandomPType;
        variable p0, participant    : PARTICIPANT_DATA_TYPE := DEFAULT_PARTICIPANT_DATA;
        
        alias idle_sig is <<signal uut.idle_sig : std_logic>>;
        alias mem_op_done is <<signal uut.mem_op_done : std_logic>>;
        
        -- 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;
        
        impure function gen_rand_entityid_2(reader : boolean) return std_logic_vector is
            variable ret : std_logic_vector(ENTITYID_WIDTH-1 downto 0) := (others => '0');
        begin
            gen_rand_entityid(RV, reader, ret);
            return ret;
        end function;
        
        impure function gen_rand_guid_prefix return GUIDPREFIX_TYPE is
            variable ret : GUIDPREFIX_TYPE;
        begin
            ret := (0 => RV.RandSlv(WORD_WIDTH), 1 => RV.RandSlv(WORD_WIDTH), 2 => RV.RandSlv(WORD_WIDTH));
            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;
        
        -- NOTE: This procedure waits until the idle_sig is high for at least
        --       two consecutive clock cycles.
        procedure wait_on_idle is
            variable first : boolean := TRUE;
        begin
            loop
                if (idle_sig /= '1' or mem_op_done /= '1') then
                    wait until idle_sig = '1' and mem_op_done = '1';
                elsif (not first) then
                    exit;
                end if;
                wait until rising_edge(clk);
                wait until rising_edge(clk);
                first := FALSE;
            end loop;
        end procedure;
        
    begin
        
        assert (TEST_STRING = "TEST_CONFIG_1") report "user_config incompatible with testbench." severity FAILURE;
        
        SetAlertLogName("rtps_builtin_endpoint - Level 1 - RTPS Output");
        SetAlertEnable(FAILURE, TRUE);
        SetAlertEnable(ERROR, TRUE);
        SetAlertEnable(WARNING, TRUE);
        SetLogEnable(DEBUG, FALSE);
        SetLogEnable(PASSED, FALSE);
        SetLogEnable(INFO, TRUE);
        RV.InitSeed(RV'instance_name);
        
        -- Participant 0
        p0.guidPrefix                   := gen_rand_guid_prefix;
        p0.nr                           := 0;
        p0.metatrafficUnicastLocatorList:= (numLocators => int(1,CDR_LONG_WIDTH), locator => (0 => gen_rand_loc_2, others => EMPTY_LOCATOR));
        p0.defaultUnicastLocatorList    := (numLocators => int(1,CDR_LONG_WIDTH), locator => (0 => gen_rand_loc_2, others => EMPTY_LOCATOR));
        p0.availableBuiltinEndpoints(DISC_BUILTIN_ENDPOINT_SUBSCRIPTIONS_DETECTOR) := '1';
        p0.availableBuiltinEndpoints(DISC_BUILTIN_ENDPOINT_PUBLICATIONS_ANNOUNCER) := '1';
        
        Log("Initiating Test", INFO);
        Log("Current Time: 0s", INFO);
        test_time   <= TIME_ZERO;
        alive       <= (others => '0');
        stim_done   <= '0';
        start       <= '0';
        reset       <= '1';
        wait until rising_edge(clk);
        wait until rising_edge(clk);
        reset       <= '0';
        
        
        Log("Match Remote Participant", INFO);
        -- RTPS SUBMESSAGE
        sub                 := DEFAULT_RTPS_SUBMESSAGE;
        sub.submessageID    := SID_DATA;
        sub.writerId        := ENTITYID_SPDP_BUILTIN_PARTICIPANT_ANNOUNCER;
        sub.readerId        := ENTITYID_SPDP_BUILTIN_PARTICIPANT_DETECTOR;
        sub.writerSN        := gen_sn(1);
        sub.flags(SUBMESSAGE_DATA_FLAG_POS) := '1';
        -- PARTICIPANT
        participant := p0;
        -- GENERATE STIMULUS
        gen_participant_data(participant, sub.data);
        gen_sentinel(sub.data);
        gen_rtps_handler_out(sub, participant, stimulus);
        start_test;
        wait_on_complete;
        wait_on_idle;
        stimulus    := EMPTY_TEST_PACKET;
        
        -- *ACKNACK*
        
        Log("Send Remote Publisher Heartbeat [Empty, Final Flag] (No Response, Final Flag)", INFO);
        -- RTPS SUBMESSAGE
        sub                 := DEFAULT_RTPS_SUBMESSAGE;
        sub.submessageID    := SID_HEARTBEAT;
        sub.writerId        := ENTITYID_SEDP_BUILTIN_PUBLICATIONS_ANNOUNCER;
        sub.readerId        := ENTITYID_SEDP_BUILTIN_PUBLICATIONS_DETECTOR;
        sub.firstSN         := gen_sn(1);
        sub.lastSN          := gen_sn(0);
        sub.flags(SUBMESSAGE_FINAL_FLAG_POS) := '1';
        -- PARTICIPANT
        participant := p0;
        -- GENERATE STIMULUS
        gen_rtps_handler_out(sub, participant, stimulus);
        start_test;
        wait_on_complete;
        wait_on_idle;
        stimulus    := EMPTY_TEST_PACKET;
        
        -- Progress time past the HEARTBEAT RESPONSE DELAY
        Log("Current Time: 0.105s", INFO);
        test_time   <= gen_duration(0,105*(10**6));
        
        Log("Send Remote Publisher Heartbeat [Empty] (Response)", INFO);
        -- RTPS SUBMESSAGE
        sub                 := DEFAULT_RTPS_SUBMESSAGE;
        sub.submessageID    := SID_HEARTBEAT;
        sub.writerId        := ENTITYID_SEDP_BUILTIN_PUBLICATIONS_ANNOUNCER;
        sub.readerId        := ENTITYID_SEDP_BUILTIN_PUBLICATIONS_DETECTOR;
        sub.firstSN         := gen_sn(1);
        sub.lastSN          := gen_sn(0);
        -- PARTICIPANT
        participant := p0;
        -- GENERATE STIMULUS
        gen_rtps_handler_out(sub, participant, stimulus);
        start_test;
        wait_on_complete;
        wait_on_idle;
        stimulus    := EMPTY_TEST_PACKET;
        
        Log("Send Remote Subscriber Heartbeat [Empty, Final Flag] (No Change)", INFO);
        -- RTPS SUBMESSAGE
        sub                 := DEFAULT_RTPS_SUBMESSAGE;
        sub.submessageID    := SID_HEARTBEAT;
        sub.writerId        := ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_ANNOUNCER;
        sub.readerId        := ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_DETECTOR;
        sub.firstSN         := gen_sn(1);
        sub.lastSN          := gen_sn(0);
        sub.flags(SUBMESSAGE_FINAL_FLAG_POS) := '1';
        -- PARTICIPANT
        participant := p0;
        -- GENERATE STIMULUS
        gen_rtps_handler_out(sub, participant, stimulus);
        start_test;
        wait_on_complete;
        wait_on_idle;
        stimulus    := EMPTY_TEST_PACKET;
        
        Log("Send Remote Message AckNack [Expecting 1] (Response)", INFO);
        -- RTPS SUBMESSAGE
        sub                 := DEFAULT_RTPS_SUBMESSAGE;
        sub.submessageID    := SID_ACKNACK;
        sub.writerId        := ENTITYID_P2P_BUILTIN_PARTICIPANT_MESSAGE_WRITER;
        sub.readerId        := ENTITYID_P2P_BUILTIN_PARTICIPANT_MESSAGE_READER;
        sub.readerSNState.base  := gen_sn(1);
        -- PARTICIPANT
        participant := p0;
        -- GENERATE STIMULUS
        gen_rtps_handler_out(sub, participant, stimulus);
        start_test;
        wait_on_complete;
        wait_on_idle;
        stimulus    := EMPTY_TEST_PACKET;
        
        Log("Current Time: 0.210s", INFO);
        test_time   <= gen_duration(0,210*(10**6));
        
        gen_acknack(p0, 1, gen_sn(1), gen_sn(1), gen_sn(1));
        wait_on_idle;
        
        
        Log("Toggle local Writer 4 Liveliness for 1 clock cycle (MANUAL_BY_PARTICIPANT_QOS)", INFO);
        alive(NUM_READERS+4)    <= '1';
        wait until rising_edge(clk);
        alive    <= (others => '0');
        wait_on_idle;
        
        Log("Current Time: 0.310s", INFO);
        test_time   <= gen_duration(0,310*(10**6));
        gen_data(p0, FALSE, FALSE, TRUE, gen_sn(1), gen_sn(2));
        wait_on_idle;
        
        Log("Send Remote Publisher Heartbeat [First 1, Last 1] (No Response, Suppression Delay)", INFO);
        -- RTPS SUBMESSAGE
        sub                 := DEFAULT_RTPS_SUBMESSAGE;
        sub.submessageID    := SID_HEARTBEAT;
        sub.writerId        := ENTITYID_SEDP_BUILTIN_PUBLICATIONS_ANNOUNCER;
        sub.readerId        := ENTITYID_SEDP_BUILTIN_PUBLICATIONS_DETECTOR;
        sub.firstSN         := gen_sn(1);
        sub.lastSN          := gen_sn(1);
        -- PARTICIPANT
        participant := p0;
        -- GENERATE STIMULUS
        gen_rtps_handler_out(sub, participant, stimulus);
        start_test;
        wait_on_complete;
        wait_on_idle;
        stimulus    := EMPTY_TEST_PACKET;
        
        Log("Current Time: 0.415s", INFO);
        test_time   <= gen_duration(0,415*(10**6));
        wait_on_idle;
        
        Log("Send Remote Publisher Heartbeat [First 1, Last 5] (Response)", INFO);
        -- RTPS SUBMESSAGE
        sub                 := DEFAULT_RTPS_SUBMESSAGE;
        sub.submessageID    := SID_HEARTBEAT;
        sub.writerId        := ENTITYID_SEDP_BUILTIN_PUBLICATIONS_ANNOUNCER;
        sub.readerId        := ENTITYID_SEDP_BUILTIN_PUBLICATIONS_DETECTOR;
        sub.firstSN         := gen_sn(1);
        sub.lastSN          := gen_sn(5);
        -- PARTICIPANT
        participant := p0;
        -- GENERATE STIMULUS
        gen_rtps_handler_out(sub, participant, stimulus);
        start_test;
        wait_on_complete;
        wait_on_idle;
        stimulus    := EMPTY_TEST_PACKET;
        
        Log("Send Remote Publisher AckNack [Expecting 1] (No Response, Suppression Delay)", INFO);
        -- RTPS SUBMESSAGE
        sub                 := DEFAULT_RTPS_SUBMESSAGE;
        sub.submessageID    := SID_ACKNACK;
        sub.writerId        := ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_ANNOUNCER;
        sub.readerId        := ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_DETECTOR;
        sub.readerSNState.base  := gen_sn(1);
        -- PARTICIPANT
        participant := p0;
        -- GENERATE STIMULUS
        gen_rtps_handler_out(sub, participant, stimulus);
        start_test;
        wait_on_complete;
        wait_on_idle;
        stimulus    := EMPTY_TEST_PACKET;
        
        Log("Current Time: 0.450s", INFO);
        test_time   <= gen_duration(0,450*(10**6));
        wait_on_idle;
        
        Log("Send Remote Subscriber Heartbeat [First 2, Last 5] (Update Response)", INFO);
        -- RTPS SUBMESSAGE
        sub                 := DEFAULT_RTPS_SUBMESSAGE;
        sub.submessageID    := SID_HEARTBEAT;
        sub.writerId        := ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_ANNOUNCER;
        sub.readerId        := ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_DETECTOR;
        sub.firstSN         := gen_sn(2);
        sub.lastSN          := gen_sn(5);
        -- PARTICIPANT
        participant := p0;
        -- GENERATE STIMULUS
        gen_rtps_handler_out(sub, participant, stimulus);
        start_test;
        wait_on_complete;
        wait_on_idle;
        stimulus    := EMPTY_TEST_PACKET;
        
        Log("Current Time: 0.520s", INFO);
        test_time   <= gen_duration(0,520*(10**6));
        gen_acknack(p0, 2, gen_sn(1), gen_sn(2), gen_sn(1));
        wait_on_idle;
        
        Log("Send Remote Publisher AckNack [Expecting 1] (Response)", INFO);
        -- RTPS SUBMESSAGE
        sub                 := DEFAULT_RTPS_SUBMESSAGE;
        sub.submessageID    := SID_ACKNACK;
        sub.writerId        := ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_ANNOUNCER;
        sub.readerId        := ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_DETECTOR;
        sub.readerSNState.base  := gen_sn(1);
        -- PARTICIPANT
        participant := p0;
        -- GENERATE STIMULUS
        gen_rtps_handler_out(sub, participant, stimulus);
        start_test;
        wait_on_complete;
        wait_on_idle;
        stimulus    := EMPTY_TEST_PACKET;
        
        Log("Current Time: 0.620s", INFO);
        test_time   <= gen_duration(0,620*(10**6));
        wait_on_idle;
        
        Log("Send Remote Subscriber AckNack [Expecting 1] (Update Response)", INFO);
        -- RTPS SUBMESSAGE
        sub                 := DEFAULT_RTPS_SUBMESSAGE;
        sub.submessageID    := SID_ACKNACK;
        sub.writerId        := ENTITYID_SEDP_BUILTIN_PUBLICATIONS_ANNOUNCER;
        sub.readerId        := ENTITYID_SEDP_BUILTIN_PUBLICATIONS_DETECTOR;
        sub.readerSNState.base  := gen_sn(1);
        -- PARTICIPANT
        participant := p0;
        -- GENERATE STIMULUS
        gen_rtps_handler_out(sub, participant, stimulus);
        start_test;
        wait_on_complete;
        wait_on_idle;
        stimulus    := EMPTY_TEST_PACKET;
        
        Log("Current Time: 0.725s", INFO);
        test_time   <= gen_duration(0,725*(10**6));
        gen_data(p0, TRUE, TRUE, FALSE, SEQUENCENUMBER_UNKNOWN, SEQUENCENUMBER_UNKNOWN);
        wait_on_idle;
        
        Log("Current Time: 1s", INFO);
        test_time   <= gen_duration(1,0);
        gen_heartbeat(p0, 3, gen_sn(3), gen_sn(4));
        wait_on_idle;
        
        Log("Send Remote Subscriber AckNack [Expecting 8] (Response)", INFO);
        -- RTPS SUBMESSAGE
        sub                 := DEFAULT_RTPS_SUBMESSAGE;
        sub.submessageID    := SID_ACKNACK;
        sub.writerId        := ENTITYID_SEDP_BUILTIN_PUBLICATIONS_ANNOUNCER;
        sub.readerId        := ENTITYID_SEDP_BUILTIN_PUBLICATIONS_DETECTOR;
        sub.readerSNState.base  := gen_sn(8);
        -- PARTICIPANT
        participant := p0;
        -- GENERATE STIMULUS
        gen_rtps_handler_out(sub, participant, stimulus);
        start_test;
        wait_on_complete;
        wait_on_idle;
        stimulus    := EMPTY_TEST_PACKET;
        
        Log("Current Time: 1.205s", INFO);
        test_time   <= gen_duration(1,205*(10**6));
        gen_data(p0, TRUE, FALSE, FALSE, SEQUENCENUMBER_UNKNOWN, SEQUENCENUMBER_UNKNOWN);
        wait_on_idle;
        
        Log("Current Time: 1.500s", INFO);
        test_time   <= gen_duration(1,500*(10**6));
        wait_on_idle;
        
        Log("Send Remote Publisher AckNack [Expecting 18] (No Response)", INFO);
        -- RTPS SUBMESSAGE
        sub                 := DEFAULT_RTPS_SUBMESSAGE;
        sub.submessageID    := SID_ACKNACK;
        sub.writerId        := ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_ANNOUNCER;
        sub.readerId        := ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_DETECTOR;
        sub.readerSNState.base  := gen_sn(18);
        -- PARTICIPANT
        participant := p0;
        -- GENERATE STIMULUS
        gen_rtps_handler_out(sub, participant, stimulus);
        start_test;
        wait_on_complete;
        wait_on_idle;
        stimulus    := EMPTY_TEST_PACKET;
        
        Log("Current Time: 1.705s", INFO);
        test_time   <= gen_duration(1,705*(10**6));
        wait_on_idle;
        
        Log("Current Time: 2s", INFO);
        test_time   <= gen_duration(2,0);
        gen_heartbeat(p0, 4, gen_sn(3), gen_sn(5));
        wait_on_idle;
        
        Log("Send Remote Message AckNack [Expecting 5] (Response)", INFO);
        -- RTPS SUBMESSAGE
        sub                 := DEFAULT_RTPS_SUBMESSAGE;
        sub.submessageID    := SID_ACKNACK;
        sub.writerId        := ENTITYID_P2P_BUILTIN_PARTICIPANT_MESSAGE_WRITER;
        sub.readerId        := ENTITYID_P2P_BUILTIN_PARTICIPANT_MESSAGE_READER;
        sub.readerSNState.base  := gen_sn(5);
        -- PARTICIPANT
        participant := p0;
        -- GENERATE STIMULUS
        gen_rtps_handler_out(sub, participant, stimulus);
        start_test;
        wait_on_complete;
        wait_on_idle;
        stimulus    := EMPTY_TEST_PACKET;
        
        Log("Send Remote Publisher AckNack [Expecting 2] (No Response)", INFO);
        -- RTPS SUBMESSAGE
        sub                 := DEFAULT_RTPS_SUBMESSAGE;
        sub.submessageID    := SID_ACKNACK;
        sub.writerId        := ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_ANNOUNCER;
        sub.readerId        := ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_DETECTOR;
        sub.readerSNState.base  := gen_sn(2);
        -- PARTICIPANT
        participant := p0;
        -- GENERATE STIMULUS
        gen_rtps_handler_out(sub, participant, stimulus);
        start_test;
        wait_on_complete;
        wait_on_idle;
        stimulus    := EMPTY_TEST_PACKET;
        
        Log("Send Remote Subscriber AckNack [Expecting 8] (Response)", INFO);
        -- RTPS SUBMESSAGE
        sub                 := DEFAULT_RTPS_SUBMESSAGE;
        sub.submessageID    := SID_ACKNACK;
        sub.writerId        := ENTITYID_SEDP_BUILTIN_PUBLICATIONS_ANNOUNCER;
        sub.readerId        := ENTITYID_SEDP_BUILTIN_PUBLICATIONS_DETECTOR;
        sub.readerSNState.base  := gen_sn(8);
        -- PARTICIPANT
        participant := p0;
        -- GENERATE STIMULUS
        gen_rtps_handler_out(sub, participant, stimulus);
        start_test;
        wait_on_complete;
        wait_on_idle;
        stimulus    := EMPTY_TEST_PACKET;
        
        Log("Current Time: 2.205s", INFO);
        test_time   <= gen_duration(2,205*(10**6));
        gen_data(p0, TRUE, TRUE, TRUE, gen_sn(3), gen_sn(5));
        wait_on_idle;
        
        Log("Toggle local Writer 0 Liveliness for 1 clock cycle (AUTOMATIC_QOS)", INFO);
        alive(NUM_READERS)    <= '1';
        wait until rising_edge(clk);
        alive    <= (others => '0');
        wait_on_idle;
        
        Log("Current Time: 2.500s", INFO);
        test_time   <= gen_duration(2,500*(10**6));
        wait_on_idle;
        
        Log("Send Remote Message AckNack [Expecting 6] (No Response)", INFO);
        -- RTPS SUBMESSAGE
        sub                 := DEFAULT_RTPS_SUBMESSAGE;
        sub.submessageID    := SID_ACKNACK;
        sub.writerId        := ENTITYID_P2P_BUILTIN_PARTICIPANT_MESSAGE_WRITER;
        sub.readerId        := ENTITYID_P2P_BUILTIN_PARTICIPANT_MESSAGE_READER;
        sub.readerSNState.base  := gen_sn(6);
        -- PARTICIPANT
        participant := p0;
        -- GENERATE STIMULUS
        gen_rtps_handler_out(sub, participant, stimulus);
        start_test;
        wait_on_complete;
        wait_on_idle;
        stimulus    := EMPTY_TEST_PACKET;
        
        Log("Current Time: 2.705s", INFO);
        test_time   <= gen_duration(2,705*(10**6));
        wait_on_idle;
        
        Log("Current Time: 3s", INFO);
        test_time   <= gen_duration(3,0);
        gen_heartbeat(p0, 5, gen_sn(6), gen_sn(7));
        wait_on_idle;
        
        Log("Current Time: 31s", INFO);
        test_time   <= gen_duration(31,0);
        gen_announcement;
        gen_heartbeat(p0, 6, gen_sn(6), gen_sn(8));
        wait_on_idle;
        
        TranscriptOpen(RESULTS_FILE, APPEND_MODE);
        SetTranscriptMirror;
        stim_done   <= '1';
        wait until check_done = '1';
        AlertIf(not SB_out.empty, "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;
    
    in_empty_prc : process
    begin
        in_empty        <= '0';
        wait until rd_sig = '1';
        wait until rising_edge(clk);
        in_empty        <= '1';
        wait until rising_edge(clk);
    end process;
    
    endpoint_full_prc : process
    begin
        full   <= '0';
        wait until wr_sig = '1';
        wait until rising_edge(clk);
        full   <= '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(full = '1' and wr_sig = '1', "Output FIFO write signal high while full signal high", ERROR);
        end if;
    end process;
    
    input_prc : process(all)
    begin
        data_in         <= stimulus.data(cnt_stim);
        last_word_in    <= stimulus.last(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;
                            cnt_stim    <= 0;
                            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_check_prc : process(all)
    begin
        check_done  <= '0';
        if rising_edge(clk) then
            if (wr_sig = '1') then
                SB_out.Check(data_out);
            end if;
            if (stim_done = '1' and SB_out.empty) then
                check_done  <= '1';
            end if;
        end if;
    end process;
    
    watchdog : process
    begin
        wait for 1 ms;
        Alert("Test timeout", FAILURE);
        std.env.stop;
    end process;
    
end architecture;