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 matching of remote endpoints. This is done by checking the Match/Unmatch Frame sent to the local endpoints.
-- The local participant is configured with following endpoints (called "fixed" endpoints):
-- NOTE: All Endpoints are configured with topic "TOPIC_1" and type "TYPE_1", except is explicitly stated otherwise.
-- WRITER 0
--  * DEFAULT
-- WRITER 1
--  * TYPE_2
-- WRITER 2
--  * TOPIC_2
--  * TYPE_2
-- WRITER 3
--  * RELIABILITY   BEST_EFFORT
-- WRITER 4
--  * DURABILITY        TRANSIENT_LOCAL
--  * PRESENTATION      TOPIC
--  * COHERENT_ACCESS   TRUE
--  * DEADLINE          15s
--  * LATENCY_BUDGET    1s
--  * LIVELINESS        MANUAL_BY_PARTICIPANT
--  * LEASE_DURATION    15s
--  * DESTINATION_ORDER BY_SOURCE_TIMESTAMP
-- WRITER 5
--  * PRESENTATION      TOPIC
--  * COHERENT_ACCESS   TRUE
--  * ORDERED_ACCESS    TRUE
-- WRITER 6
--  * LATENCY_BUDGET    1s
-- WRITER 7
--  * DURABILITY_SERVICE_CLEANUP_DELAY              86400s
--  * DURABILITY_SERVICE_HISTORY                    KEEP_ALL_HISTORY_QOS
--  * DURABILITY_SERVICE_HISTORY_DEPTH              10
--  * DURABILITY_SERVICE_MAX_SAMPLES                10
--  * DURABILITY_SERVICE_MAX_INSTANCES              5
--  * DURABILITY_SERVICE_MAX_SAMPLES_PER_INSTANCE   5
--  * OWNERSHIP_STRENGTH                            2
--  * MAX_BLOCKING_TIME                             2s
--  * TRANSPORT_PRIORITY                            3
--  * LIFESPAN                                      30s
--  * USER_DATA                                     "USER_DATA"
--  * TOPIC_DATA                                    "TOPIC_DATA"
--  * GROUP_DATA                                    "GROUP_DATA"
-- READER 0
--  * DEFAULT
-- READER 1
--  * TYPE_2
-- READER 2
--  * TOPIC_2
--  * TYPE_2
-- READER 3
--  * DURABILITY    TRANSIENT_LOCAL
-- READER 4
--  * PRESENTATION  TOPIC
-- READER 5
--  * COHERENT_ACCESS   TRUE
-- READER 6
--  * COHERENT_ACCESS   TRUE
--  * ORDERED_ACCESS    TRUE
-- READER 7
--  * DEADLINE          10s
--  * TIME_BASED_FILTER 5s
-- READER 8
--  * DEADLINE          20s
--  * TIME_BASED_FILTER 5s
-- READER 9
--  * LATENCY_BUDGET    500ms
-- READER 10
--  * LATENCY_BUDGET    2s
-- READER 11
--  * LIVELINESS        MANUAL_BY_PARTICIPANT
--  * LEASE_DURATION    20s
-- READER 12
--  * LIVELINESS        MANUAL_BY_TOPIC
--  * LEASE_DURATION    20s
-- READER 13
--  * LEASE_DURATION    10s
-- READER 14
--  * LEASE_DURATION    20s
-- READER 15
--  * RELIABILITY   BEST_EFFORT
-- READER 16
--  * DESTINATION_ORDER     BY_SOURCE_TIMESTAMP

-- The testbench announces 3 remote Participants. Participant 0 and 1 contain the same endpoints as our local Participant, but Participant 1 announces everything in Little Endian format.
-- Participant 2 announces extra endpoints to test matching for Readers/Writers with unsupported/ignored QoS


entity L0_rtps_builtin_endpoint_test2 is
end entity;

architecture testbench of L0_rtps_builtin_endpoint_test2 is
    
    -- *TYPE DECLARATION*
    type TEST_STAGE_TYPE is (IDLE, BUSY);
    type MATCH_MATRIX_TYPE is array (0 to NUM_WRITERS-1) of std_logic_vector(0 to NUM_READERS-1);
    
    -- *SIGNAL DECLARATION*
    signal clk, in_empty, rd_sig, last_word_in, last_word_out: std_logic := '0';
    signal reset : std_logic := '1';
    signal endpoint_wr, endpoint_full   : 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 : TEST_STAGE_TYPE := IDLE;
    shared variable stimulus, reference : 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 : work.ScoreBoardPkg_builtin_endpoint.ScoreBoardPType;
    signal MATCH_MATRIX : MATCH_MATRIX_TYPE := (others => (others => '0'));
    signal stim_done, check_done : std_logic := '0';
    
    -- *FUNCTION DECLARATION*
    procedure wait_on_complete is
    begin
        wait until rising_edge(packet_sent);
    end procedure;
    
    impure function gen_wr(index : natural; reader : boolean) return std_logic_vector is
        variable ret : std_logic_vector(0 to NUM_ENDPOINTS-1) := (others => '0');
    begin
        if (reader) then
            for i in 0 to NUM_WRITERS-1 loop
                ret(NUM_READERS+i)  := MATCH_MATRIX(i)(index);
            end loop;
        else
            for i in 0 to NUM_READERS-1 loop
                ret(i)  := MATCH_MATRIX(index)(i);
            end loop;
        end if;
        return ret;
    end function;
    
begin
    
    -- Unit Under Test
    uut : entity work.rtps_builtin_endpoint(arch)
        generic map (
            MAX_REMOTE_PARTICIPANTS => 3
        )
        port map (
            clk             => clk,
            reset           => reset,
            empty           => in_empty or packet_sent,
            rd              => rd_sig,
            data_in         => data_in,
            data_out        => data_out,
            last_word_in    => last_word_in,
            time            => TIME_ZERO,
            endpoint_full   => endpoint_full,
            endpoint_wr     => endpoint_wr,
            rtps_wr         => open,
            rtps_full       => '0',
            last_word_out   => last_word_out,
            alive           => (others => '0')
        );
    
    stimulus_prc : process
        variable sub, sub_p, sub_s      : RTPS_SUBMESSAGE_TYPE := DEFAULT_RTPS_SUBMESSAGE;
        variable check_cnt              : natural := 0;
        variable RV                     : RandomPType;
        variable p0, p1, p2, participant: PARTICIPANT_DATA_TYPE := DEFAULT_PARTICIPANT_DATA;
        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 endpoint, e1, e2       : ENDPOINT_DATA_TYPE := DEFAULT_ENDPOINT_DATA;
        variable p0_snp, p1_snp, p2_snp : SEQUENCENUMBER_TYPE := FIRST_SEQUENCENUMBER;
        variable p0_sns, p1_sns, p2_sns : SEQUENCENUMBER_TYPE := FIRST_SEQUENCENUMBER;
        variable p_sn                   : SEQUENCENUMBER_TYPE := FIRST_SEQUENCENUMBER;
        variable wr_sig                 : std_logic_vector(0 to NUM_ENDPOINTS-1) := (others => '0');
        variable user_data, topic_data, group_data : STRING_WORD_ARRAY_TYPE := EMPTY_STRING;
        variable partition              : TEST_PACKET_TYPE := EMPTY_TEST_PACKET;
        
        -- 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;
        
        procedure push_endpoint_reference is
        begin
            -- MATCH
            if (wr_sig /= (wr_sig'range => '0')) then
                endpoint.match  := MATCH;
                gen_endpoint_match_frame(endpoint, reference);
                for i in 0 to reference.length-1 loop
                    SB.Push(wr_sig & reference.last(i) & reference.data(i));
                end loop;
                reference := EMPTY_TEST_PACKET;
            end if;
            -- UNMATCH
            if ((not wr_sig) /= (wr_sig'range => '0')) then
                endpoint.match  := UNMATCH;
                gen_endpoint_match_frame(endpoint, reference);
                for i in 0 to reference.length-1 loop
                    SB.Push((not wr_sig) & reference.last(i) & reference.data(i));
                end loop;
                reference := EMPTY_TEST_PACKET;
            end if;
        end procedure;
        
        procedure push_participant_reference is
            variable wr_sig : std_logic_vector(NUM_ENDPOINTS-1 downto 0) := (others => '1');
        begin
            gen_participant_match_frame(participant, reference);
            for i in 0 to reference.length-1 loop
                SB.Push(wr_sig & reference.last(i) & reference.data(i));
            end loop;
            reference   := EMPTY_TEST_PACKET;
        end procedure;
        
        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;
    begin
        
        assert (TEST_STRING = "TEST_CONFIG_1") report "user_config incompatible with testbench." severity FAILURE;
        
        SetAlertLogName("rtps_builtin_endpoint - Level 0 - Endpoint Matching");
        SetAlertEnable(FAILURE, TRUE);
        SetAlertEnable(ERROR, TRUE);
        SetAlertEnable(WARNING, TRUE);
        SetLogEnable(DEBUG, FALSE);
        SetLogEnable(PASSED, FALSE);
        SetLogEnable(INFO, TRUE);
        RV.InitSeed(RV'instance_name);
        
        -- Populate MATCH Matrix
        MATCH_MATRIX    <= (
            0   => (0 => '1', 9 => '1', 10 => '1', 15 => '1', others => '0'),
            1   => (1 => '1', others => '0'),
            2   => (2 => '1', others => '0'),
            3   => (15 => '1', others => '0'),
            4   => (0 => '1', 3 => '1', 4 => '1', 5 => '1', 8 => '1', 9 => '1', 10 => '1', 11 => '1', 14 => '1', 15 => '1', 16 => '1', others => '0'),
            5   => (0 => '1', 4 => '1', 5 => '1', 6 => '1', 9 => '1', 10 => '1', 15 => '1', others => '0'),
            6   => (10 => '1', others => '0'),
            7   => (0 => '1', 9 => '1', 10 => '1', 15 => '1', others => '0')
        );
        
        -- Participant 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.flags(SUBMESSAGE_DATA_FLAG_POS)    := '1';
        
        -- Publisher Endpoint RTPS Submessage
        sub_p                                    := DEFAULT_RTPS_SUBMESSAGE;
        sub_p.submessageID                       := SID_DATA;
        sub_p.writerId                           := ENTITYID_SEDP_BUILTIN_PUBLICATIONS_ANNOUNCER;
        sub_p.readerId                           := ENTITYID_SEDP_BUILTIN_PUBLICATIONS_DETECTOR;
        sub_p.flags(SUBMESSAGE_DATA_FLAG_POS)    := '1';
        
        -- Subscriber Endpoint RTPS Submessage
        sub_s                                    := DEFAULT_RTPS_SUBMESSAGE;
        sub_s.submessageID                       := SID_DATA;
        sub_s.writerId                           := ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_ANNOUNCER;
        sub_s.readerId                           := ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_DETECTOR;
        sub_s.flags(SUBMESSAGE_DATA_FLAG_POS)    := '1';
        
        -- Participant 0 (Contains the fixed Endpoints)
        p0.guidPrefix                   := gen_rand_guid_prefix;
        p0.nr                           := 0;
        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_SUBSCRIPTIONS_ANNOUNCER):= '1';
        p0.availableBuiltinEndpoints(DISC_BUILTIN_ENDPOINT_PUBLICATIONS_DETECTOR)  := '1';
        p0.availableBuiltinEndpoints(DISC_BUILTIN_ENDPOINT_PUBLICATIONS_ANNOUNCER) := '1';
        
        -- Participant 1 (Contains the fixed Endpoints in Little Endian)
        p1.guidPrefix                   := gen_rand_guid_prefix;
        p1.nr                           := 1;
        p1.littleEndian                 := '1';
        p1.defaultUnicastLocatorList    := (numLocators => int(1,CDR_LONG_WIDTH), locator => (0 => gen_rand_loc_2, others => EMPTY_LOCATOR));
        p1.availableBuiltinEndpoints(DISC_BUILTIN_ENDPOINT_SUBSCRIPTIONS_DETECTOR) := '1';
        p1.availableBuiltinEndpoints(DISC_BUILTIN_ENDPOINT_SUBSCRIPTIONS_ANNOUNCER):= '1';
        p1.availableBuiltinEndpoints(DISC_BUILTIN_ENDPOINT_PUBLICATIONS_DETECTOR)  := '1';
        p1.availableBuiltinEndpoints(DISC_BUILTIN_ENDPOINT_PUBLICATIONS_ANNOUNCER) := '1';
        
        -- Participant 2 (Used for dyncamic Endpoints)
        p2.guidPrefix                   := gen_rand_guid_prefix;
        p2.nr                           := 2;
        p2.defaultUnicastLocatorList    := (numLocators => int(1,CDR_LONG_WIDTH), locator => (0 => gen_rand_loc_2, others => EMPTY_LOCATOR));
        p2.availableBuiltinEndpoints(DISC_BUILTIN_ENDPOINT_SUBSCRIPTIONS_DETECTOR) := '1';
        p2.availableBuiltinEndpoints(DISC_BUILTIN_ENDPOINT_SUBSCRIPTIONS_ANNOUNCER):= '1';
        p2.availableBuiltinEndpoints(DISC_BUILTIN_ENDPOINT_PUBLICATIONS_DETECTOR)  := '1';
        p2.availableBuiltinEndpoints(DISC_BUILTIN_ENDPOINT_PUBLICATIONS_ANNOUNCER) := '1';
        
        for i in 0 to fixed_writers'length-1 loop
            fixed_writers(i).entityId   := gen_rand_entityid_2(FALSE);
        end loop;
        for i in 0 to fixed_readers'length-1 loop
            fixed_readers(i).entityId   := gen_rand_entityid_2(TRUE);
        end loop;
        
        e1.participant  := p2;
        e1.entityId     := gen_rand_entityid_2(FALSE);
        e1.topic_name   := ENDPOINT_TOPIC(0);
        e1.type_name    := ENDPOINT_TYPE(0);
        e1.reader       := FALSE;
        e2.participant  := p2;
        e2.entityId     := gen_rand_entityid_2(TRUE);
        e2.topic_name   := ENDPOINT_TOPIC(0);
        e2.type_name    := ENDPOINT_TYPE(0);
        e2.reader       := TRUE;
        
        user_data   := convert_string("USER_DATA" & (10 to 256 => NUL));
        group_data  := convert_string("GROUP_DATA" & (11 to 256 => NUL));
        topic_data  := convert_string("TOPIC_DATA" & (11 to 256 => NUL));
        
        -- Partition
        partition.data(0)   := int(10, CDR_LONG_WIDTH);
        partition.data(1)   := x"50415254";
        partition.data(2)   := x"4954494f";
        partition.data(3)   := x"4e000000";
        partition.length    := 4;
        
        Log("Initiating Test", INFO);
        stim_done   <= '0';
        start       <= '0';
        reset       <= '1';
        wait until rising_edge(clk);
        wait until rising_edge(clk);
        reset   <= '0';
        
        -- *Insert Participant 0*
        -- Contains the same Endpoints as our Participant, as defined above
        Log("Match Participant 0 [Compatible]", INFO);
        sub.writerSN    := p_sn;
        participant     := p0;
        gen_participant_data(participant, sub.data);
        gen_sentinel(sub.data);
        gen_rtps_handler_out(sub, participant, stimulus);
        start_test;
        wait_on_complete;
        stimulus    := EMPTY_TEST_PACKET;
        sub.data    := EMPTY_TEST_PACKET;
        p_sn       := p_sn + 1;
        
        -- *Insert Participant 1*
        -- Contains the same Endpoints as Participant 0, but in little Endian
        Log("Match Participant 1 [Compatible, Little Endian]", INFO);
        sub.writerSN    := p_sn;
        participant     := p1;
        gen_participant_data(participant, sub.data);
        gen_sentinel(sub.data);
        gen_rtps_handler_out(sub, participant, stimulus);
        start_test;
        wait_on_complete;
        stimulus    := EMPTY_TEST_PACKET;
        sub.data    := EMPTY_TEST_PACKET;
        p_sn       := p_sn + 1;
        
        -- *Insert Participant 2*
        -- Used for extra Endpoint testing (e.g. unsupported QoS)
        Log("Match Participant 2 [Compatible]", INFO);
        sub.writerSN    := p_sn;
        participant     := p2;
        gen_participant_data(participant, sub.data);
        gen_sentinel(sub.data);
        gen_rtps_handler_out(sub, participant, stimulus);
        start_test;
        wait_on_complete;
        stimulus    := EMPTY_TEST_PACKET;
        sub.data    := EMPTY_TEST_PACKET;
        p_sn       := p_sn + 1;
        
        -- *Fixed Writers (Participant 0)*
        for i in 0 to NUM_WRITERS-1 loop
            Log("Fixed Writer " & to_string(i) & ", Participant 0", INFO);
            sub_p.writerSN  := p0_snp;
            endpoint        := fixed_writers(i);
            endpoint.participant := p0;
            gen_endpoint_data(endpoint, sub_p.data);
            gen_sentinel(sub_p.data);
            gen_rtps_handler_out(sub_p, endpoint, stimulus);
            wr_sig  := gen_wr(i, FALSE);
            push_endpoint_reference;
            start_test;
            wait_on_complete;
            stimulus    := EMPTY_TEST_PACKET;
            sub_p.data  := EMPTY_TEST_PACKET;
            p0_snp      := p0_snp + 1;
        end loop;
        
        -- *Fixed Writers (Participant 1)[Little Endian]*
        for i in 0 to NUM_WRITERS-1 loop
            Log("Fixed Writer " & to_string(i) & ", Participant 1 [Little Endian]", INFO);
            sub_p.writerSN  := p1_snp;
            endpoint        := fixed_writers(i);
            endpoint.participant := p1;
            endpoint.littleEndian:= '1';
            gen_endpoint_data(endpoint, sub_p.data);
            gen_sentinel(sub_p.data);
            gen_rtps_handler_out(sub_p, endpoint, stimulus);
            wr_sig  := gen_wr(i, FALSE);
            push_endpoint_reference;
            start_test;
            wait_on_complete;
            stimulus    := EMPTY_TEST_PACKET;
            sub_p.data  := EMPTY_TEST_PACKET;
            p1_snp      := p1_snp + 1;
        end loop;
        
        -- *Fixed Readers (Participant 0)*
        for i in 0 to NUM_READERS-1 loop
            Log("Fixed Reader " & to_string(i) & ", Participant 0", INFO);
            sub_s.writerSN  := p0_sns;
            endpoint        := fixed_readers(i);
            endpoint.participant := p0;
            gen_endpoint_data(endpoint, sub_s.data);
            gen_sentinel(sub_s.data);
            gen_rtps_handler_out(sub_s, endpoint, stimulus);
            wr_sig  := gen_wr(i, TRUE);
            push_endpoint_reference;
            start_test;
            wait_on_complete;
            stimulus    := EMPTY_TEST_PACKET;
            sub_s.data  := EMPTY_TEST_PACKET;
            p0_sns      := p0_sns + 1;
        end loop;
        
        -- *Fixed Readers (Participant 1)[Little Endian]*
        for i in 0 to NUM_READERS-1 loop
            Log("Fixed Reader " & to_string(i) & ", Participant 1", INFO);
            sub_s.writerSN  := p1_sns;
            endpoint        := fixed_readers(i);
            endpoint.participant := p1;
            endpoint.littleEndian:= '1';
            gen_endpoint_data(endpoint, sub_s.data);
            gen_sentinel(sub_s.data);
            gen_rtps_handler_out(sub_s, endpoint, stimulus);
            wr_sig  := gen_wr(i, TRUE);
            push_endpoint_reference;
            start_test;
            wait_on_complete;
            stimulus    := EMPTY_TEST_PACKET;
            sub_s.data  := EMPTY_TEST_PACKET;
            p1_sns      := p1_sns + 1;
        end loop;
        
        -- *Remove Participant 1*
        Log("Unmatch Participant 1", INFO);
        sub.writerSN    := p_sn;
        participant     := p1;
        participant.domainId    := int(2, DOMAIN_ID_WIDTH);
        participant.match       := UNMATCH;
        gen_participant_data(participant, sub.data);
        gen_sentinel(sub.data);
        gen_rtps_handler_out(sub, participant, stimulus);
        push_participant_reference;
        start_test;
        wait_on_complete;
        stimulus    := EMPTY_TEST_PACKET;
        reference   := EMPTY_TEST_PACKET;
        sub.data    := EMPTY_TEST_PACKET;
        p_sn       := p_sn + 1;
        
        -- *Participant 2 Endpoints*
        
        -- Writers
        
        Log("Match Participant 2 Writer [Durability Persistent]", INFO);
        sub_p.writerSN  := p2_snp;
        endpoint                := DEFAULT_ENDPOINT_DATA;
        endpoint.reader         := FALSE;
        endpoint.entityId       := gen_rand_entityid_2(FALSE);
        endpoint.participant    := p2;
        endpoint.topic_name     := ENDPOINT_TOPIC(0);
        endpoint.type_name      := ENDPOINT_TYPE(0);
        endpoint.durability     := PERSISTENT_DURABILITY_QOS;
        gen_endpoint_data(endpoint, sub_p.data);
        gen_sentinel(sub_p.data);
        gen_rtps_handler_out(sub_p, endpoint, stimulus);
        wr_sig  := (0 => '1', 3 => '1', 9 => '1', 10 => '1', 15 => '1', others => '0');
        push_endpoint_reference;
        start_test;
        wait_on_complete;
        stimulus    := EMPTY_TEST_PACKET;
        reference   := EMPTY_TEST_PACKET;
        sub_p.data  := EMPTY_TEST_PACKET;
        p2_snp      := p2_snp + 1;
        
        Log("Match Participant 2 Writer [Presentation Group]", INFO);
        sub_p.writerSN  := p2_snp;
        endpoint                := DEFAULT_ENDPOINT_DATA;
        endpoint.reader         := FALSE;
        endpoint.entityId       := gen_rand_entityid_2(FALSE);
        endpoint.participant    := p2;
        endpoint.topic_name     := ENDPOINT_TOPIC(0);
        endpoint.type_name      := ENDPOINT_TYPE(0);
        endpoint.presentation   := GROUP_PRESENTATION_QOS;
        gen_endpoint_data(endpoint, sub_p.data);
        gen_sentinel(sub_p.data);
        gen_rtps_handler_out(sub_p, endpoint, stimulus);
        wr_sig  := (0 => '1', 4 => '1', 9 => '1', 10 => '1', 15 => '1', others => '0');
        push_endpoint_reference;
        start_test;
        wait_on_complete;
        stimulus    := EMPTY_TEST_PACKET;
        reference   := EMPTY_TEST_PACKET;
        sub_p.data  := EMPTY_TEST_PACKET;
        p2_snp      := p2_snp + 1;
        
        Log("Ignore Participant 2 Writer [Ownership Exclusive]", INFO);
        sub_p.writerSN  := p2_snp;
        endpoint                := DEFAULT_ENDPOINT_DATA;
        endpoint.reader         := FALSE;
        endpoint.entityId       := gen_rand_entityid_2(FALSE);
        endpoint.participant    := p2;
        endpoint.topic_name     := ENDPOINT_TOPIC(0);
        endpoint.type_name      := ENDPOINT_TYPE(0);
        endpoint.ownership      := EXCLUSIVE_OWNERSHIP_QOS;
        gen_endpoint_data(endpoint, sub_p.data);
        gen_sentinel(sub_p.data);
        gen_rtps_handler_out(sub_p, endpoint, stimulus);
        wr_sig  := (others => '0');
        push_endpoint_reference;
        start_test;
        wait_on_complete;
        stimulus    := EMPTY_TEST_PACKET;
        reference   := EMPTY_TEST_PACKET;
        sub_p.data  := EMPTY_TEST_PACKET;
        p2_snp      := p2_snp + 1;
        
        Log("Match Participant 2 Writer [USER_DATA, TOPIC_DATA, GROUP_DATA, MAX_SERIALIZED_PAYLOAD]", INFO);
        sub_p.writerSN  := p2_snp;
        endpoint                    := DEFAULT_ENDPOINT_DATA;
        endpoint.reader             := FALSE;
        endpoint.entityId           := gen_rand_entityid_2(FALSE);
        endpoint.participant        := p2;
        endpoint.topic_name         := ENDPOINT_TOPIC(0);
        endpoint.type_name          := ENDPOINT_TYPE(0);
        endpoint.user_data          := user_data;
        endpoint.topic_data         := topic_data;
        endpoint.group_data         := group_data;
        endpoint.max_size_serialized:= std_logic_vector(to_unsigned(65000, CDR_LONG_WIDTH));
        gen_endpoint_data(endpoint, sub_p.data);
        gen_sentinel(sub_p.data);
        gen_rtps_handler_out(sub_p, endpoint, stimulus);
        wr_sig  := (0 => '1', 9 => '1', 10 => '1', 15 => '1', others => '0');
        push_endpoint_reference;
        start_test;
        wait_on_complete;
        stimulus    := EMPTY_TEST_PACKET;
        reference   := EMPTY_TEST_PACKET;
        sub_p.data  := EMPTY_TEST_PACKET;
        p2_snp      := p2_snp + 1;
        
        Log("Ignore Participant 2 Writer [Incompatible MAX_SERIALIZED_PAYLOAD]", INFO);
        sub_p.writerSN  := p2_snp;
        endpoint                    := DEFAULT_ENDPOINT_DATA;
        endpoint.reader             := FALSE;
        endpoint.entityId           := gen_rand_entityid_2(FALSE);
        endpoint.participant        := p2;
        endpoint.topic_name         := ENDPOINT_TOPIC(0);
        endpoint.type_name          := ENDPOINT_TYPE(0);
        endpoint.max_size_serialized:= std_logic_vector(to_unsigned(66000, CDR_LONG_WIDTH));
        gen_endpoint_data(endpoint, sub_p.data);
        gen_sentinel(sub_p.data);
        gen_rtps_handler_out(sub_p, endpoint, stimulus);
        wr_sig  := (others => '0');
        push_endpoint_reference;
        start_test;
        wait_on_complete;
        stimulus    := EMPTY_TEST_PACKET;
        reference   := EMPTY_TEST_PACKET;
        sub_p.data  := EMPTY_TEST_PACKET;
        p2_snp      := p2_snp + 1;
        
        Log("Ignore Participant 2 Writer [Non-Default Partition]", INFO);
        sub_p.writerSN  := p2_snp;
        endpoint                    := DEFAULT_ENDPOINT_DATA;
        endpoint.reader             := FALSE;
        endpoint.entityId           := gen_rand_entityid_2(FALSE);
        endpoint.participant        := p2;
        endpoint.topic_name         := ENDPOINT_TOPIC(0);
        endpoint.type_name          := ENDPOINT_TYPE(0);
        gen_endpoint_data(endpoint, sub_p.data);
        gen_parameter(PID_PARTITION, partition, sub_p.data);
        gen_sentinel(sub_p.data);
        gen_rtps_handler_out(sub_p, endpoint, stimulus);
        wr_sig  := (others => '0');
        push_endpoint_reference;
        start_test;
        wait_on_complete;
        stimulus    := EMPTY_TEST_PACKET;
        reference   := EMPTY_TEST_PACKET;
        sub_p.data  := EMPTY_TEST_PACKET;
        p2_snp      := p2_snp + 1;
        
        -- Readers
        
        Log("Ignore Participant 2 Reader [Durability Persistent]", INFO);
        sub_s.writerSN  := p2_sns;
        endpoint                := DEFAULT_ENDPOINT_DATA;
        endpoint.reader         := TRUE;
        endpoint.entityId       := gen_rand_entityid_2(TRUE);
        endpoint.participant    := p2;
        endpoint.topic_name     := ENDPOINT_TOPIC(0);
        endpoint.type_name      := ENDPOINT_TYPE(0);
        endpoint.durability     := PERSISTENT_DURABILITY_QOS;
        gen_endpoint_data(endpoint, sub_s.data);
        gen_sentinel(sub_s.data);
        gen_rtps_handler_out(sub_s, endpoint, stimulus);
        wr_sig  := (others => '0');
        push_endpoint_reference;
        start_test;
        wait_on_complete;
        stimulus    := EMPTY_TEST_PACKET;
        reference   := EMPTY_TEST_PACKET;
        sub_s.data  := EMPTY_TEST_PACKET;
        p2_sns      := p2_sns + 1;
        
        Log("Ignore Participant 2 Reader [Presentation Group]", INFO);
        sub_s.writerSN  := p2_sns;
        endpoint                := DEFAULT_ENDPOINT_DATA;
        endpoint.reader         := TRUE;
        endpoint.entityId       := gen_rand_entityid_2(TRUE);
        endpoint.participant    := p2;
        endpoint.topic_name     := ENDPOINT_TOPIC(0);
        endpoint.type_name      := ENDPOINT_TYPE(0);
        endpoint.presentation   := GROUP_PRESENTATION_QOS;
        gen_endpoint_data(endpoint, sub_s.data);
        gen_sentinel(sub_s.data);
        gen_rtps_handler_out(sub_s, endpoint, stimulus);
        wr_sig  := (others => '0');
        push_endpoint_reference;
        start_test;
        wait_on_complete;
        stimulus    := EMPTY_TEST_PACKET;
        reference   := EMPTY_TEST_PACKET;
        sub_s.data  := EMPTY_TEST_PACKET;
        p2_sns      := p2_sns + 1;
        
        
        Log("Ignore Participant 2 Reader [Ownership Exclusive]", INFO);
        sub_s.writerSN  := p2_sns;
        endpoint                := DEFAULT_ENDPOINT_DATA;
        endpoint.reader         := TRUE;
        endpoint.entityId       := gen_rand_entityid_2(TRUE);
        endpoint.participant    := p2;
        endpoint.topic_name     := ENDPOINT_TOPIC(0);
        endpoint.type_name      := ENDPOINT_TYPE(0);
        endpoint.ownership      := EXCLUSIVE_OWNERSHIP_QOS;
        gen_endpoint_data(endpoint, sub_s.data);
        gen_sentinel(sub_s.data);
        gen_rtps_handler_out(sub_s, endpoint, stimulus);
        wr_sig  := (others => '0');
        push_endpoint_reference;
        start_test;
        wait_on_complete;
        stimulus    := EMPTY_TEST_PACKET;
        reference   := EMPTY_TEST_PACKET;
        sub_s.data  := EMPTY_TEST_PACKET;
        p2_sns      := p2_sns + 1;
        
        Log("Match Participant 2 Reader [USER_DATA, TOPIC_DATA, GROUP_DATA, EXPECTS_INLINE_QOS]", INFO);
        sub_s.writerSN  := p2_sns;
        endpoint                    := DEFAULT_ENDPOINT_DATA;
        endpoint.reader             := TRUE;
        endpoint.entityId           := gen_rand_entityid_2(TRUE);
        endpoint.participant        := p2;
        endpoint.topic_name         := ENDPOINT_TOPIC(0);
        endpoint.type_name          := ENDPOINT_TYPE(0);
        endpoint.user_data          := user_data;
        endpoint.topic_data         := topic_data;
        endpoint.group_data         := group_data;
        endpoint.expectsInlineQoS(0):= '1';
        gen_endpoint_data(endpoint, sub_s.data);
        gen_sentinel(sub_s.data);
        gen_rtps_handler_out(sub_s, endpoint, stimulus);
        wr_sig  := (NUM_READERS => '1', NUM_READERS+4 => '1', NUM_READERS+5 => '1', NUM_READERS+7 => '1', others => '0');
        push_endpoint_reference;
        start_test;
        wait_on_complete;
        stimulus    := EMPTY_TEST_PACKET;
        reference   := EMPTY_TEST_PACKET;
        sub_s.data  := EMPTY_TEST_PACKET;
        p2_sns      := p2_sns + 1;
        
        Log("Ignore Participant 2 Reader [Non-Default Partition]", INFO);
        sub_s.writerSN  := p2_sns;
        endpoint                := DEFAULT_ENDPOINT_DATA;
        endpoint.reader         := TRUE;
        endpoint.entityId       := gen_rand_entityid_2(TRUE);
        endpoint.participant    := p2;
        endpoint.topic_name     := ENDPOINT_TOPIC(0);
        endpoint.type_name      := ENDPOINT_TYPE(0);
        gen_endpoint_data(endpoint, sub_s.data);
        gen_parameter(PID_PARTITION, partition, sub_s.data);
        gen_sentinel(sub_s.data);
        gen_rtps_handler_out(sub_s, endpoint, stimulus);
        wr_sig  := (others => '0');
        push_endpoint_reference;
        start_test;
        wait_on_complete;
        stimulus    := EMPTY_TEST_PACKET;
        reference   := EMPTY_TEST_PACKET;
        sub_s.data  := EMPTY_TEST_PACKET;
        p2_sns      := p2_sns + 1;
        
        -- Reader/Writer Match/Unmatch
        
        Log("Match Participant 2 Writer [Default]", INFO);
        sub_p.writerSN  := p2_snp;
        endpoint    := e1;
        gen_endpoint_data(endpoint, sub_p.data);
        gen_sentinel(sub_p.data);
        gen_rtps_handler_out(sub_p, endpoint, stimulus);
        wr_sig  := (0 => '1', 9 => '1', 10 => '1', 15 => '1', others => '0');
        push_endpoint_reference;
        start_test;
        wait_on_complete;
        stimulus    := EMPTY_TEST_PACKET;
        reference   := EMPTY_TEST_PACKET;
        sub_p.data  := EMPTY_TEST_PACKET;
        p2_snp      := p2_snp + 1;
        
        Log("Match Participant 2 Reader [Default]", INFO);
        sub_s.writerSN  := p2_sns;
        endpoint    := e2;
        gen_endpoint_data(endpoint, sub_s.data);
        gen_sentinel(sub_s.data);
        gen_rtps_handler_out(sub_s, endpoint, stimulus);
        wr_sig  := (NUM_READERS => '1', NUM_READERS+4 => '1', NUM_READERS+5 => '1', NUM_READERS+7 => '1', others => '0');
        push_endpoint_reference;
        start_test;
        wait_on_complete;
        stimulus    := EMPTY_TEST_PACKET;
        reference   := EMPTY_TEST_PACKET;
        sub_s.data  := EMPTY_TEST_PACKET;
        p2_sns      := p2_sns + 1;
        
        Log("Update Participant 2 Writer [Unicast Locators (1 valid, 2 invalid)]", INFO);
        sub_p.writerSN  := p2_snp;
        endpoint    := e1;
        endpoint.unicastLocatorList := (numLocators => int(4,CDR_LONG_WIDTH), locator => (0 => gen_rand_loc_2, 1 => gen_rand_loc_2, 2 => gen_rand_loc_2, 3 => gen_rand_loc_2, others => EMPTY_LOCATOR));
        endpoint.unicastLocatorList.locator(1).kind   := LOCATOR_KIND_UDPv6;
        endpoint.unicastLocatorList.locator(2).addr   := (others => '0');
        endpoint.unicastLocatorList.locator(3).portn  := (others => '0');
        gen_endpoint_data(endpoint, sub_p.data);
        gen_sentinel(sub_p.data);
        gen_rtps_handler_out(sub_p, endpoint, stimulus);
        wr_sig  := (0 => '1', 9 => '1', 10 => '1', 15 => '1', others => '0');
        push_endpoint_reference;
        start_test;
        wait_on_complete;
        stimulus    := EMPTY_TEST_PACKET;
        reference   := EMPTY_TEST_PACKET;
        sub_p.data  := EMPTY_TEST_PACKET;
        p2_snp      := p2_snp + 1;
        
        Log("Update Participant 2 Reader [Multicast Locator (1 valid, 2 invalid)]", INFO);
        sub_s.writerSN  := p2_sns;
        endpoint    := e2;
        endpoint.multicastLocatorList := (numLocators => int(4,CDR_LONG_WIDTH), locator => (0 => gen_rand_loc_2, 1 => gen_rand_loc_2, 2 => gen_rand_loc_2, 3 => gen_rand_loc_2, others => EMPTY_LOCATOR));
        endpoint.multicastLocatorList.locator(1).kind     := LOCATOR_KIND_UDPv6;
        endpoint.multicastLocatorList.locator(2).addr     := (others => '0');
        endpoint.multicastLocatorList.locator(3).portn    := (others => '0');
        gen_endpoint_data(endpoint, sub_s.data);
        gen_sentinel(sub_s.data);
        gen_rtps_handler_out(sub_s, endpoint, stimulus);
        wr_sig  := (NUM_READERS => '1', NUM_READERS+4 => '1', NUM_READERS+5 => '1', NUM_READERS+7 => '1', others => '0');
        push_endpoint_reference;
        start_test;
        wait_on_complete;
        stimulus    := EMPTY_TEST_PACKET;
        reference   := EMPTY_TEST_PACKET;
        sub_s.data  := EMPTY_TEST_PACKET;
        p2_sns      := p2_sns + 1;
        
        Log("Change Match Participant 2 Writer [Reliability Best Effort]", INFO);
        sub_p.writerSN  := p2_snp;
        endpoint    := e1;
        endpoint.reliability  := BEST_EFFORT_RELIABILITY_QOS;
        gen_endpoint_data(endpoint, sub_p.data);
        gen_sentinel(sub_p.data);
        gen_rtps_handler_out(sub_p, endpoint, stimulus);
        wr_sig  := (15 => '1', others => '0');
        push_endpoint_reference;
        start_test;
        wait_on_complete;
        stimulus    := EMPTY_TEST_PACKET;
        reference   := EMPTY_TEST_PACKET;
        sub_p.data  := EMPTY_TEST_PACKET;
        p2_snp      := p2_snp + 1;
        
        Log("Change Match Participant 2 Reader [Topic & Type Change]", INFO);
        sub_s.writerSN  := p2_sns;
        endpoint    := e2;
        endpoint.topic_name   := ENDPOINT_TOPIC(2);
        endpoint.type_name    := ENDPOINT_TYPE(2);
        gen_endpoint_data(endpoint, sub_s.data);
        gen_sentinel(sub_s.data);
        gen_rtps_handler_out(sub_s, endpoint, stimulus);
        wr_sig      := (NUM_READERS+2 => '1', others => '0');
        push_endpoint_reference;
        start_test;
        wait_on_complete;
        stimulus    := EMPTY_TEST_PACKET;
        reference   := EMPTY_TEST_PACKET;
        sub_s.data  := EMPTY_TEST_PACKET;
        p2_sns      := p2_sns + 1;
        
        stim_done   <= '1';
        wait until check_done = '1';
        TranscriptOpen(RESULTS_FILE, APPEND_MODE);
        SetTranscriptMirror;
        AlertIf(not SB.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
        endpoint_full   <= (others => '0');
        wait until (or endpoint_wr) = '1';
        wait until rising_edge(clk);
        endpoint_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(endpoint_full /= (0 to NUM_ENDPOINTS-1 => '0') and (endpoint_wr /= (0 to NUM_ENDPOINTS-1 => '0')), "Endpoint 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;
                            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
        check_done  <= '0';
        if rising_edge(clk) then
            if (endpoint_wr /= (0 to NUM_ENDPOINTS-1 => '0')) then
                SB.Check(endpoint_wr & last_word_out & data_out);
            end if;
            if (stim_done = '1' and SB.empty) then
                check_done  <= '1';
            end if;
        end if;
    end process;
    
    watchdog : process
    begin
        wait for 2 ms;
        Alert("Test timeout", FAILURE);
        std.env.stop;
    end process;
    
end architecture;