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 output handling (ACKNACK) of the RTPS Reader
-- 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 (Heartbeat Suppression and Response delay are configured to 5s):
--  * 0s
--      - Match Remote Endpoints 0,1,2
--      - Send HEARTBEAT 0 from Endpoint 0 (Empty, Final Flag) [Test Final Flag Handling]
--  * 1s
--      - Send HEARTBEAT 1 from Endpoint 1 (Empty) [Test Empty HEARTBEAT Response]
--  * 2s
--      - Send HEARTBEAT 2 from Endpoint 2 (First SN 1, Last SN 5, Final Flag) [Test normal HEARTBEAT Handling, Test Durability Behaviour]
--  * 5s
--      - No HEARTBEAT 0 Response
--      - Send HEARTBEAT 3 from Endpoint 0 (First SN 1, Last SN 1, Final Flag) [Test normal HEARTBEAT Handling]
--  * 6s
--      - HEARTBEAT 1 Response
--  * 7s
--      - HEARTBEAT 2 Response
--  * 8s
--      - Send HEARTBEAT 4 from Endpoint 1 (First SN 2, Last SN 2, Final Flag) [Test HEARTBEAT Suppression Delay]
--  * 9s
--      - Send HEARTBEAT 5 from Endpoint 0 (First SN 3, Last SN 4, Final Flag) [Test HEARTBEAT Response Delay]
--  * 10s
--      - HEARTBEAT 3/5 Response
--  * 13s
--      - No HEARTBEAT 4 Response
--      - Send HEARTBEAT 6 from Endpoint 2 (First SN 6, Last SN 10, Final Flag) [Test HEARTBEAT SN Obsoletion]
--  * 17s
--      - HEARTBEAT 6 Response

entity L1_rtps_reader_test1_trk is
end entity;

architecture testbench of L1_rtps_reader_test1_trk is
    
    -- *CONSTANT DECLARATION*
    constant MAX_REMOTE_ENDPOINTS    : natural := 3;
    
    -- *TYPE DECLARATION*
    type SEND_STAGE_TYPE is (IDLE, BUSY);
    
    -- *SIGNAL DECLARATION*
    signal clk, empty_user, empty_meta, rd_user, rd_meta, last_word_in_user, last_word_in_meta : std_logic := '0';
    signal reset : std_logic := '1';
    signal data_in_user, data_in_meta, data_out : std_logic_vector(WORD_WIDTH-1 downto 0) := (others => '0');
    signal wr_sig, full : std_logic := '0';
    signal stim_stage_user, stim_stage_meta : SEND_STAGE_TYPE := IDLE;
    shared variable stimulus_user, stimulus_meta, reference : TEST_PACKET_TYPE := EMPTY_TEST_PACKET;
    signal packet_sent_user, packet_sent_meta : std_logic := '0';
    signal cnt_stim_meta, cnt_stim_user, count : natural := 0;
    signal start_meta, start_user : std_logic := '0';
    shared variable SB_out : osvvm.ScoreBoardPkg_slv.ScoreBoardPType;
    signal stim_done, out_check_done, test_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');
    
    -- *FUNCTION DECLARATION*
    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;
    
begin
    
    -- Unit Under Test
    uut : entity work.rtps_reader(arch)
        generic map (
            ENTITYID                    => DEFAULT_READER_ENTITYID,
            RELIABILITY_QOS             => RELIABLE_RELIABILITY_QOS,
            LIVELINESS_QOS              => AUTOMATIC_LIVELINESS_QOS,
            DURABILITY_QOS              => TRANSIENT_LOCAL_DURABILITY_QOS,
            HEARTBEAT_RESPONSE_DELAY    => gen_duration(5,0),
            HEARTBEAT_SUPPRESSION_DELAY => gen_duration(5,0),
            LEASE_DURATION              => DURATION_INFINITE,
            WITH_KEY                    => TRUE,
            MAX_REMOTE_ENDPOINTS        => MAX_REMOTE_ENDPOINTS
        )
        port map (
            -- SYSTEM
            clk                 => clk,
            reset               => reset,
            time                => test_time,
            empty_user          => empty_user or packet_sent_user,
            rd_user             => rd_user,
            data_in_user        => data_in_user,
            last_word_in_user   => last_word_in_user,
            empty_meta          => empty_meta or packet_sent_meta,
            rd_meta             => rd_meta,
            data_in_meta        => data_in_meta,
            last_word_in_meta   => last_word_in_meta,
            wr_ro               => fifo_wr,
            full_ro             => fifo_full,
            last_word_out_ro    => fifo_in(WORD_WIDTH),
            data_out_ro         => fifo_in(WORD_WIDTH-1 downto 0),
            start_hc            => open,
            opcode_hc           => open,
            ack_hc              => '1',
            done_hc             => '1',
            ret_hc              => OK,
            data_out_hc         => open,
            valid_out_hc        => open,
            ready_out_hc        => '1',
            last_word_out_hc    => open
        );
    
    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 RV                     : RandomPType;
        variable e0, e1, e2, endpoint   : ENDPOINT_DATA_TYPE := DEFAULT_ENDPOINT_DATA;
        variable sub                    : RTPS_SUBMESSAGE_TYPE := DEFAULT_RTPS_SUBMESSAGE;
        variable cc                     : CACHE_CHANGE_TYPE := DEFAULT_CACHE_CHANGE;
        variable OUT_HEADER             : OUTPUT_HEADER_TYPE := DEFAULT_OUTPUT_HEADER;
        variable rtps_header            : RTPS_HEADER_TYPE := DEFAULT_RTPS_HEADER;
        
        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_meta_test is
        begin
            start_meta  <= '1';
            wait until rising_edge(clk);
            start_meta  <= '0';
            wait until rising_edge(clk);
        end procedure;
        
        procedure start_user_test is
        begin
            start_user  <= '1';
            wait until rising_edge(clk);
            start_user  <= '0';
            wait until rising_edge(clk);
        end procedure;
        
        procedure push_reference is
        begin
            for i in 0 to reference.length-1 loop
                SB_out.Push(reference.data(i));
            end loop;
        end procedure;
        
        procedure wait_on_meta_sent is
        begin
            wait until rising_edge(packet_sent_meta);
        end procedure;
        
        procedure wait_on_user_sent is
        begin
            wait until rising_edge(packet_sent_user);
        end procedure;
        
        procedure wait_on_out_check is
        begin
            if (out_check_done /= '1') then
                wait until out_check_done = '1';
            end if;
        end procedure;
        
        procedure wait_on_completion is
        begin
            if (test_done /= '1') then
                wait until test_done = '1';
            end if;
        end procedure;
        
        procedure wait_on_idle is
        begin
            if (idle_sig /= '1' or mem_op_done /= '1') then
                wait until idle_sig = '1' and mem_op_done = '1';
            end if;
        end procedure;
        
    begin
        
        SetAlertLogName("rtps_reader - Level 1 - (Transient Local, Reliable, Keyed) - 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);
        
        -- Endpoint 1
        e0                          := DEFAULT_ENDPOINT_DATA;
        e0.reader                   := FALSE;
        e0.nr                       := 0;
        e0.match                    := MATCH;
        e0.entityid                 := RV.RandSlv(ENTITYID_WIDTH);
        e0.participant.guidPrefix   := gen_rand_guid_prefix;
        e0.unicastLocatorList       := (numLocators => int(1,CDR_LONG_WIDTH), locator => (0 => gen_rand_loc_2, others => EMPTY_LOCATOR));
        -- Endpoint 2
        e1                          := DEFAULT_ENDPOINT_DATA;
        e1.reader                   := FALSE;
        e1.nr                       := 1;
        e1.match                    := MATCH;
        e1.entityid                 := RV.RandSlv(ENTITYID_WIDTH);
        e1.participant.guidPrefix   := gen_rand_guid_prefix;
        e1.unicastLocatorList       := (numLocators => int(1,CDR_LONG_WIDTH), locator => (0 => gen_rand_loc_2, others => EMPTY_LOCATOR));
        -- Endpoint 3
        e2                          := DEFAULT_ENDPOINT_DATA;
        e2.reader                   := FALSE;
        e2.nr                       := 2;
        e2.match                    := MATCH;
        e2.entityid                 := RV.RandSlv(ENTITYID_WIDTH);
        e2.participant.guidPrefix   := gen_rand_guid_prefix;
        e2.unicastLocatorList       := (numLocators => int(1,CDR_LONG_WIDTH), locator => (0 => gen_rand_loc_2, others => EMPTY_LOCATOR));
        
        Log("Initiating Test", INFO);
        Log("Current Time: 0s", INFO);
        count       <= 1;
        stim_done   <= '0';
        start_meta  <= '0';
        start_user  <= '0';
        reset       <= '1';
        wait until rising_edge(clk);
        wait until rising_edge(clk);
        reset   <= '0';
        
        Log("Insert Endpoint 0,1,2", INFO);
        gen_endpoint_match_frame(e0, stimulus_meta);
        gen_endpoint_match_frame(e1, stimulus_meta);
        gen_endpoint_match_frame(e2, stimulus_meta);
        start_meta_test;
        wait_on_meta_sent;
        stimulus_meta   := EMPTY_TEST_PACKET;
        stimulus_user   := EMPTY_TEST_PACKET;
        reference       := EMPTY_TEST_PACKET;
        
        Log("Endpoint 0 Heartbeat [First 1, Last 0, Final Flag] (No Response)", INFO);
        endpoint            := e0;
        sub                 := DEFAULT_RTPS_SUBMESSAGE;
        sub.submessageID    := SID_HEARTBEAT;
        sub.writerId        := endpoint.entityid;
        sub.readerId        := DEFAULT_READER_ENTITYID;
        sub.firstSN         := gen_sn(1);
        sub.lastSN          := gen_sn(0);
        sub.flags(SUBMESSAGE_FINAL_FLAG_POS) := '1';
        gen_rtps_handler_out(sub, get_loc(endpoint), FALSE, TIME_INVALID, endpoint.participant.guidPrefix, stimulus_user);
        start_user_test;
        wait_on_user_sent;
        stimulus_meta   := EMPTY_TEST_PACKET;
        stimulus_user   := EMPTY_TEST_PACKET;
        reference       := EMPTY_TEST_PACKET;
        
        wait_on_idle;
        Log("Current Time: 1s", INFO);
        test_time   <= gen_duration(1,0);
        wait until rising_edge(clk);
        
        Log("Endpoint 1 Heartbeat [First 1, Last 0] (ACKNACK Response)", INFO);
        endpoint            := e1;
        sub                 := DEFAULT_RTPS_SUBMESSAGE;
        sub.submessageID    := SID_HEARTBEAT;
        sub.writerId        := endpoint.entityid;
        sub.readerId        := DEFAULT_READER_ENTITYID;
        sub.firstSN         := gen_sn(1);
        sub.lastSN          := gen_sn(0);
        gen_rtps_handler_out(sub, get_loc(endpoint), FALSE, TIME_INVALID, endpoint.participant.guidPrefix, stimulus_user);
        start_user_test;
        wait_on_user_sent;
        stimulus_meta   := EMPTY_TEST_PACKET;
        stimulus_user   := EMPTY_TEST_PACKET;
        reference       := EMPTY_TEST_PACKET;
        
        wait_on_idle;
        Log("Current Time: 2s", INFO);
        test_time   <= gen_duration(2,0);
        wait until rising_edge(clk);
        
        Log("Endpoint 2 Heartbeat [First 1, Last 5, Final Flag] (ACKNACK Response)", INFO);
        endpoint            := e2;
        sub                 := DEFAULT_RTPS_SUBMESSAGE;
        sub.submessageID    := SID_HEARTBEAT;
        sub.writerId        := endpoint.entityid;
        sub.readerId        := DEFAULT_READER_ENTITYID;
        sub.firstSN         := gen_sn(1);
        sub.lastSN          := gen_sn(5);
        sub.flags(SUBMESSAGE_FINAL_FLAG_POS) := '1';
        gen_rtps_handler_out(sub, get_loc(endpoint), FALSE, TIME_INVALID, endpoint.participant.guidPrefix, stimulus_user);
        start_user_test;
        wait_on_user_sent;
        stimulus_meta   := EMPTY_TEST_PACKET;
        stimulus_user   := EMPTY_TEST_PACKET;
        reference       := EMPTY_TEST_PACKET;
        
        wait_on_idle;
        Log("Current Time: 5s", INFO);
        test_time   <= gen_duration(5,0);
        wait until rising_edge(clk);
        
        Log("Endpoint 0 Heartbeat [First 1, Last 1, Final Flag] (ACKNACK Response)", INFO);
        endpoint            := e0;
        sub                 := DEFAULT_RTPS_SUBMESSAGE;
        sub.submessageID    := SID_HEARTBEAT;
        sub.writerId        := endpoint.entityid;
        sub.readerId        := DEFAULT_READER_ENTITYID;
        sub.firstSN         := gen_sn(1);
        sub.lastSN          := gen_sn(1);
        sub.flags(SUBMESSAGE_FINAL_FLAG_POS) := '1';
        gen_rtps_handler_out(sub, get_loc(endpoint), FALSE, TIME_INVALID, endpoint.participant.guidPrefix, stimulus_user);
        start_user_test;
        wait_on_user_sent;
        stimulus_meta   := EMPTY_TEST_PACKET;
        stimulus_user   := EMPTY_TEST_PACKET;
        reference       := EMPTY_TEST_PACKET;
        
        wait_on_idle;
        Log("Current Time: 6s", INFO);
        test_time   <= gen_duration(6,0);
        wait until rising_edge(clk);
        
        Log("Check Endpoint 1 ACKNACK Response", INFO);
        endpoint    := e1;
        -- OUTPUT HEADER
        OUT_HEADER  := DEFAULT_OUTPUT_HEADER;
        OUT_HEADER  := (dest => get_loc(endpoint), src => DEST_LOC.user.locator(1));
        gen_output_header(OUT_HEADER, reference);
        -- RTPS HEADER
        rtps_header             := DEFAULT_RTPS_HEADER;
        rtps_header.guidPrefix  := GUIDPREFIX;
        gen_rtps_header(rtps_header, reference);
        -- ACKNACK
        sub                     := DEFAULT_RTPS_SUBMESSAGE;
        sub.submessageID        := SID_ACKNACK;
        sub.writerId            := endpoint.entityid;
        sub.readerId            := DEFAULT_READER_ENTITYID;
        sub.count               := std_logic_vector(to_unsigned(count, CDR_LONG_WIDTH));
        sub.readerSNState   := (base => gen_sn(1), numBits => int(32, CDR_LONG_WIDTH), bitmap => (0 to 31 => '1', others => '0'));
        sub.flags(SUBMESSAGE_FINAL_FLAG_POS) := '1';
        gen_rtps_submessage(sub, reference);
        fix_output_packet(reference);
        push_reference;
        wait until rising_edge(clk); -- Prevent Fall through (Allow out_check_done to go low)
        wait_on_out_check;
        stimulus_meta   := EMPTY_TEST_PACKET;
        stimulus_user   := EMPTY_TEST_PACKET;
        reference       := EMPTY_TEST_PACKET;
        count   <= count + 1;
        
        wait_on_idle;
        Log("Current Time: 7s", INFO);
        test_time   <= gen_duration(7,0);
        wait until rising_edge(clk);
        
        Log("Check Endpoint 2 ACKNACK Response", INFO);
        endpoint    := e2;
        -- OUTPUT HEADER
        OUT_HEADER  := DEFAULT_OUTPUT_HEADER;
        OUT_HEADER  := (dest => get_loc(endpoint), src => DEST_LOC.user.locator(1));
        gen_output_header(OUT_HEADER, reference);
        -- RTPS HEADER
        rtps_header             := DEFAULT_RTPS_HEADER;
        rtps_header.guidPrefix  := GUIDPREFIX;
        gen_rtps_header(rtps_header, reference);
        -- ACKNACK
        sub                     := DEFAULT_RTPS_SUBMESSAGE;
        sub.submessageID        := SID_ACKNACK;
        sub.writerId            := endpoint.entityid;
        sub.readerId            := DEFAULT_READER_ENTITYID;
        sub.count               := std_logic_vector(to_unsigned(count, CDR_LONG_WIDTH));
        sub.readerSNState   := (base => gen_sn(1), numBits => int(32, CDR_LONG_WIDTH), bitmap => (0 to 31 => '1', others => '0'));
        sub.flags(SUBMESSAGE_FINAL_FLAG_POS) := '1';
        gen_rtps_submessage(sub, reference);
        fix_output_packet(reference);
        push_reference;
        wait until rising_edge(clk); -- Prevent Fall through (Allow out_check_done to go low)
        wait_on_out_check;
        stimulus_meta   := EMPTY_TEST_PACKET;
        stimulus_user   := EMPTY_TEST_PACKET;
        reference       := EMPTY_TEST_PACKET;
        count   <= count + 1;
        
        wait_on_idle;
        Log("Current Time: 8s", INFO);
        test_time   <= gen_duration(8,0);
        wait until rising_edge(clk);
        
        Log("Endpoint 1 Heartbeat [First 2, Last 2, Final Flag] (No Response, Suppression Delay)", INFO);
        endpoint            := e1;
        sub                 := DEFAULT_RTPS_SUBMESSAGE;
        sub.submessageID    := SID_HEARTBEAT;
        sub.writerId        := endpoint.entityid;
        sub.readerId        := DEFAULT_READER_ENTITYID;
        sub.firstSN         := gen_sn(2);
        sub.lastSN          := gen_sn(2);
        sub.flags(SUBMESSAGE_FINAL_FLAG_POS) := '1';
        gen_rtps_handler_out(sub, get_loc(endpoint), FALSE, TIME_INVALID, endpoint.participant.guidPrefix, stimulus_user);
        start_user_test;
        wait_on_user_sent;
        stimulus_meta   := EMPTY_TEST_PACKET;
        stimulus_user   := EMPTY_TEST_PACKET;
        reference       := EMPTY_TEST_PACKET;
        
        wait_on_idle;
        Log("Current Time: 9s", INFO);
        test_time   <= gen_duration(9,0);
        wait until rising_edge(clk);
        
        Log("Endpoint 0 Heartbeat [First 3, Last 4, Final Flag] (Update expected Response)", INFO);
        endpoint            := e0;
        sub                 := DEFAULT_RTPS_SUBMESSAGE;
        sub.submessageID    := SID_HEARTBEAT;
        sub.writerId        := endpoint.entityid;
        sub.readerId        := DEFAULT_READER_ENTITYID;
        sub.firstSN         := gen_sn(3);
        sub.lastSN          := gen_sn(4);
        sub.flags(SUBMESSAGE_FINAL_FLAG_POS) := '1';
        gen_rtps_handler_out(sub, get_loc(endpoint), FALSE, TIME_INVALID, endpoint.participant.guidPrefix, stimulus_user);
        start_user_test;
        wait_on_user_sent;
        stimulus_meta   := EMPTY_TEST_PACKET;
        stimulus_user   := EMPTY_TEST_PACKET;
        reference       := EMPTY_TEST_PACKET;
        
        wait_on_idle;
        Log("Current Time: 10s", INFO);
        test_time   <= gen_duration(10,0);
        wait until rising_edge(clk);
        
        Log("Check Endpoint 0 ACKNACK Response", INFO);
        endpoint    := e0;
        -- OUTPUT HEADER
        OUT_HEADER  := DEFAULT_OUTPUT_HEADER;
        OUT_HEADER  := (dest => get_loc(endpoint), src => DEST_LOC.user.locator(1));
        gen_output_header(OUT_HEADER, reference);
        -- RTPS HEADER
        rtps_header             := DEFAULT_RTPS_HEADER;
        rtps_header.guidPrefix  := GUIDPREFIX;
        gen_rtps_header(rtps_header, reference);
        -- ACKNACK
        sub                     := DEFAULT_RTPS_SUBMESSAGE;
        sub.submessageID        := SID_ACKNACK;
        sub.writerId            := endpoint.entityid;
        sub.readerId            := DEFAULT_READER_ENTITYID;
        sub.count               := std_logic_vector(to_unsigned(count, CDR_LONG_WIDTH));
        sub.readerSNState   := (base => gen_sn(3), numBits => int(32, CDR_LONG_WIDTH), bitmap => (0 to 31 => '1', others => '0'));
        sub.flags(SUBMESSAGE_FINAL_FLAG_POS) := '1';
        gen_rtps_submessage(sub, reference);
        fix_output_packet(reference);
        push_reference;
        wait until rising_edge(clk); -- Prevent Fall through (Allow out_check_done to go low)
        wait_on_out_check;
        stimulus_meta   := EMPTY_TEST_PACKET;
        stimulus_user   := EMPTY_TEST_PACKET;
        reference       := EMPTY_TEST_PACKET;
        count   <= count + 1;
        
        wait_on_idle;
        Log("Current Time: 12s", INFO);
        test_time   <= gen_duration(12,0);
        wait until rising_edge(clk);
        wait until rising_edge(clk);
        
        
        wait_on_idle;
        Log("Current Time: 13s", INFO);
        test_time   <= gen_duration(13,0);
        wait until rising_edge(clk);
        
        Log("Endpoint 2 Heartbeat [First 6, Last 10, Final Flag] (Response expected)", INFO);
        endpoint            := e2;
        sub                 := DEFAULT_RTPS_SUBMESSAGE;
        sub.submessageID    := SID_HEARTBEAT;
        sub.writerId        := endpoint.entityid;
        sub.readerId        := DEFAULT_READER_ENTITYID;
        sub.firstSN         := gen_sn(6);
        sub.lastSN          := gen_sn(10);
        sub.flags(SUBMESSAGE_FINAL_FLAG_POS) := '1';
        gen_rtps_handler_out(sub, get_loc(endpoint), FALSE, TIME_INVALID, endpoint.participant.guidPrefix, stimulus_user);
        start_user_test;
        wait_on_user_sent;
        stimulus_meta   := EMPTY_TEST_PACKET;
        stimulus_user   := EMPTY_TEST_PACKET;
        reference       := EMPTY_TEST_PACKET;
        
        wait_on_idle;
        Log("Current Time: 17s", INFO);
        test_time   <= gen_duration(18,0);
        wait until rising_edge(clk);
        
        Log("Check Endpoint 2 ACKNACK Response", INFO);
        endpoint    := e2;
        -- OUTPUT HEADER
        OUT_HEADER  := DEFAULT_OUTPUT_HEADER;
        OUT_HEADER  := (dest => get_loc(endpoint), src => DEST_LOC.user.locator(1));
        gen_output_header(OUT_HEADER, reference);
        -- RTPS HEADER
        rtps_header             := DEFAULT_RTPS_HEADER;
        rtps_header.guidPrefix  := GUIDPREFIX;
        gen_rtps_header(rtps_header, reference);
        -- ACKNACK
        sub                     := DEFAULT_RTPS_SUBMESSAGE;
        sub.submessageID        := SID_ACKNACK;
        sub.writerId            := endpoint.entityid;
        sub.readerId            := DEFAULT_READER_ENTITYID;
        sub.count               := std_logic_vector(to_unsigned(count, CDR_LONG_WIDTH));
        sub.readerSNState   := (base => gen_sn(6), numBits => int(32, CDR_LONG_WIDTH), bitmap => (0 to 31 => '1', others => '0'));
        sub.flags(SUBMESSAGE_FINAL_FLAG_POS) := '1';
        gen_rtps_submessage(sub, reference);
        fix_output_packet(reference);
        push_reference;
        wait_on_out_check;
        stimulus_meta   := EMPTY_TEST_PACKET;
        stimulus_user   := EMPTY_TEST_PACKET;
        reference       := EMPTY_TEST_PACKET;
        count   <= count + 1;
        
        stim_done   <= '1';
        wait_on_completion;
        TranscriptOpen(RESULTS_FILE, APPEND_MODE);
        SetTranscriptMirror;
        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;
    
    empty_meta_prc : process
    begin
        empty_meta  <= '0';
        wait until rd_meta = '1';
        wait until rising_edge(clk);
        empty_meta  <= '1';
        wait until rising_edge(clk);
    end process;
    
    empty_user_prc : process
    begin
        empty_user  <= '0';
        wait until rd_user = '1';
        wait until rising_edge(clk);
        empty_user  <= '1';
        wait until rising_edge(clk);
    end process;
    
    rtps_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(empty_meta = '1' and rd_meta = '1', "Input FIFO read signal high while empty signal high (meta)", ERROR);
            alertif(empty_user = '1' and rd_user = '1', "Input FIFO read signal high while empty signal high (user)", ERROR);
            alertif(full = '1' and wr_sig = '1', "Output FIFO write signal high while full signal high", ERROR);
        end if;
    end process;
    
    input_meta_prc : process(all)
    begin
        data_in_meta        <= stimulus_meta.data(cnt_stim_meta);
        last_word_in_meta   <= stimulus_meta.last(cnt_stim_meta);
        
        if rising_edge(clk) then
            if (reset = '1') then
                cnt_stim_meta       <= 0;
                stim_stage_meta     <= IDLE;
                packet_sent_meta    <= '1';
            else
                case (stim_stage_meta) is
                    when IDLE =>
                        if (start_meta = '1' and stimulus_meta.length /= 0) then
                            stim_stage_meta     <= BUSY;
                            packet_sent_meta    <= '0';
                        end if;
                    when BUSY =>
                        if (rd_meta = '1') then
                            if (cnt_stim_meta = stimulus_meta.length-1) then
                                stim_stage_meta     <= IDLE;
                                packet_sent_meta    <= '1';
                                cnt_stim_meta       <= 0;
                            else
                                cnt_stim_meta   <= cnt_stim_meta + 1;
                            end if;
                        end if;
                end case;
            end if;
        end if;
    end process;
    
    input_user_prc : process(all)
    begin
        data_in_user        <= stimulus_user.data(cnt_stim_user);
        last_word_in_user   <= stimulus_user.last(cnt_stim_user);
        
        if rising_edge(clk) then
            if (reset = '1') then
                cnt_stim_user       <= 0;
                stim_stage_user     <= IDLE;
                packet_sent_user    <= '1';
            else
                case (stim_stage_user) is
                    when IDLE =>
                        if (start_user = '1' and stimulus_user.length /= 0) then
                            stim_stage_user     <= BUSY;
                            packet_sent_user    <= '0';
                        end if;
                    when BUSY =>
                        if (rd_user = '1') then
                            if (cnt_stim_user = stimulus_user.length-1) then
                                stim_stage_user     <= IDLE;
                                packet_sent_user    <= '1';
                                cnt_stim_user       <= 0;
                            else
                                cnt_stim_user   <= cnt_stim_user + 1;
                            end if;
                        end if;
                end case;
            end if;
        end if;
    end process;
    
    done_proc : process(clk)
    begin
        if rising_edge(clk) then
            if (stim_done = '1' and SB_out.empty) then
                test_done   <= '1';
            else
                test_done   <= '0';
            end if;
        end if;
    end process;
    
    output_check_prc : process(all)
    begin
        if rising_edge(clk) then
            if (wr_sig = '1') then
                SB_out.Check(data_out);
            end if;
            if (SB_out.empty) then
                out_check_done  <= '1';
            else
                out_check_done  <= '0';
            end if;
        end if;
    end process;
    
    watchdog : process
    begin
        wait for 1 ms;
        Alert("Test timeout", FAILURE);
        std.env.stop;
    end process;
    
end architecture;