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 Liveliness Handling (Active Remote Readers) of the RTPS Writer.
-- The testbench checks the memory contents at specific times.

entity L0_rtps_writer_test2_vrkdp is
end entity;

architecture testbench of L0_rtps_writer_test2_vrkdp is
    
    -- *CONSTANT DECLARATION*
    constant MAX_REMOTE_ENDPOINTS    : natural := 3;
    
    -- *TYPE DECLARATION*
    type TEST_STAGE_TYPE is (IDLE, BUSY);
    type TEST_RAM_TYPE is array (0 to (MAX_REMOTE_ENDPOINTS*READER_ENDPOINT_FRAME_SIZE_A)-1) of std_logic_vector(WORD_WIDTH-1 downto 0);
    
    -- *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_meta, data_in_user : std_logic_vector(WORD_WIDTH-1 downto 0) := (others => '0');
    signal start_hc : std_logic := '0';
    signal stim_stage_user, stim_stage_meta : TEST_STAGE_TYPE := IDLE;
    shared variable stimulus_user, stimulus_meta : TEST_PACKET_TYPE := EMPTY_TEST_PACKET;
    signal packet_sent_user, packet_sent_meta : std_logic := '0';
    signal cnt_stim_user, cnt_stim_meta : natural := 0;
    signal start_user, start_meta : std_logic := '0';
    signal test_time : TIME_TYPE := TIME_ZERO;
    shared variable SB_mem : work.ScoreBoardPkg_MemoryTest.ScoreBoardPType;
    signal stim_done, mem_check_done, test_done : std_logic := '0';
    signal check_trigger : std_logic := '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_writer(arch)
        generic map (
            RELIABILTY_QOS              => RELIABLE_RELIABILITY_QOS,
            LIVELINESS_QOS              => AUTOMATIC_LIVELINESS_QOS,
            DURABILITY_QOS              => VOLATILE_DURABILITY_QOS,
            DESTINATION_ORDER_QOS       => BY_RECEPTION_TIMESTAMP_DESTINATION_ORDER_QOS,
            ACKNACK_RESPONSE_DELAY      => DURATION_ZERO,
            ACKNACK_SUPPRESSION_DELAY   => DURATION_ZERO,
            LEASE_DURATION              => gen_duration(1,0),
            HEARTBEAT_PERIOD            => DURATION_INFINITE,
            ENTITYID                    => DEFAULT_WRITER_ENTITYID,
            WITH_KEY                    => TRUE,
            PUSH_MODE                   => TRUE,
            INLINE_QOS                  => gen_inline_qos(NUM_READERS+7),
            MAX_REMOTE_ENDPOINTS        => MAX_REMOTE_ENDPOINTS
        )
        port map (
            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,
            alive_sig               => open,
            wr_rtps                 => open,
            full_rtps               => '0',
            last_word_out_rtps      => open,
            data_out_rtps           => open,
            liveliness_assertion    => '0',
            data_available          => '0',
            start_hc                => start_hc,
            opcode_hc               => open,
            ack_hc                  => '0',
            seq_nr_hc               => open,
            done_hc                 => '0',
            ret_hc                  => ERROR,
            get_data_hc             => open,
            data_in_hc              => (others => '0'),
            valid_in_hc             => '0',
            ready_in_hc             => open,
            last_word_in_hc         => '0',
            cc_instance_handle      => HANDLE_NIL,
            cc_kind                 => ALIVE,
            cc_source_timestamp     => TIME_INVALID,
            cc_seq_nr               => SEQUENCENUMBER_UNKNOWN
        );
    
    stimulus_prc : process
        variable RV                         : RandomPType;
        variable e0, e1, e2, e3, endpoint   : ENDPOINT_DATA_TYPE := DEFAULT_ENDPOINT_DATA;
        variable sub                        : RTPS_SUBMESSAGE_TYPE := DEFAULT_RTPS_SUBMESSAGE;
        
        alias idle_sig is <<signal uut.idle_sig : 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_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 start_mem_check is
        begin
            check_trigger   <= '1';
            wait until rising_edge(clk);
            check_trigger   <= '0';
            wait until rising_edge(clk);
        end procedure;
        
        procedure wait_on_user_sent is
        begin
            wait until rising_edge(packet_sent_user);
        end procedure;
        
        procedure wait_on_meta_sent is
        begin
            wait until rising_edge(packet_sent_meta);
        end procedure;
        
        procedure wait_on_mem_check is
        begin
            if (mem_check_done /= '1') then
                wait until mem_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') then
                wait until idle_sig = '1';
            end if;
        end procedure;
        
    begin
        
        SetAlertLogName("rtps_writer (Volatile, Reliable, Keyed, By Reception Timestamp, Push Mode) - Level 0 - Reader Liveliness Handling");
        SetAlertEnable(FAILURE, TRUE);
        SetAlertEnable(ERROR, TRUE);
        SetAlertEnable(WARNING, TRUE);
        SetLogEnable(DEBUG, FALSE);
        SetLogEnable(PASSED, FALSE);
        SetLogEnable(INFO, TRUE);
        RV.InitSeed(RV'instance_name);
        
        -- Endpoint 0
        e0                          := DEFAULT_ENDPOINT_DATA;
        e0.participant.guidPrefix   := gen_rand_guid_prefix;
        e0.entityid                 := RV.RandSlv(ENTITYID_WIDTH);
        e0.unicastLocatorList       := (numLocators => int(1,CDR_LONG_WIDTH), locator => (0 => gen_rand_loc_2, others => EMPTY_LOCATOR));
        -- Endpoint 1
        e1                          := DEFAULT_ENDPOINT_DATA;
        e1.participant.guidPrefix   := gen_rand_guid_prefix;
        e1.entityid                 := RV.RandSlv(ENTITYID_WIDTH);
        e1.unicastLocatorList       := (numLocators => int(1,CDR_LONG_WIDTH), locator => (0 => gen_rand_loc_2, others => EMPTY_LOCATOR));
        e1.reliability              := BEST_EFFORT_RELIABILITY_QOS;
        -- Endpoint 2
        e2                          := DEFAULT_ENDPOINT_DATA;
        e2.participant.guidPrefix   := gen_rand_guid_prefix;
        e2.entityid                 := RV.RandSlv(ENTITYID_WIDTH);
        e2.unicastLocatorList       := (numLocators => int(1,CDR_LONG_WIDTH), locator => (0 => gen_rand_loc_2, others => EMPTY_LOCATOR));
        e2.expectsInlineQoS(0)      := '1';
        -- Endpoint 3
        e3                          := DEFAULT_ENDPOINT_DATA;
        e3.participant.guidPrefix   := gen_rand_guid_prefix;
        e3.entityid                 := RV.RandSlv(ENTITYID_WIDTH);
        e3.unicastLocatorList       := (numLocators => int(1,CDR_LONG_WIDTH), locator => (0 => gen_rand_loc_2, others => EMPTY_LOCATOR));
        e3.expectsInlineQoS(0)      := '1';
        e3.durability               := TRANSIENT_LOCAL_DURABILITY_QOS;
        e3.reliability              := BEST_EFFORT_RELIABILITY_QOS;
        
        Log("Initiating Test", INFO);
        Log("Current Time: 0s", INFO);
        test_time       <= TIME_ZERO;
        stim_done       <= '0';
        start_meta      <= '0';
        start_user      <= '0';
        check_trigger   <= '0';
        reset           <= '1';
        wait until rising_edge(clk);
        wait until rising_edge(clk);
        reset   <= '0';
        
        
        Log("Insert Endpoint 0", INFO);
        endpoint        := e0;
        endpoint.nr     := 0;
        endpoint.match  := MATCH;
        gen_endpoint_match_frame(endpoint, stimulus_meta);
        SB_mem.Push(gen_reader_endpoint_mem_frame_a(endpoint));
        start_meta_test;
        wait_on_meta_sent;
        stimulus_meta   := EMPTY_TEST_PACKET;
        stimulus_user   := EMPTY_TEST_PACKET;
        start_mem_check;
        wait_on_mem_check;
        -- MEMORY STATE [e0,0,0]
        
        Log("Insert Endpoint 1", INFO);
        endpoint            := e1;
        endpoint.nr         := 1;
        endpoint.match      := MATCH;
        gen_endpoint_match_frame(endpoint, stimulus_meta);
        SB_mem.Push(gen_reader_endpoint_mem_frame_a(endpoint));
        start_meta_test;
        wait_on_meta_sent;
        stimulus_meta   := EMPTY_TEST_PACKET;
        stimulus_user   := EMPTY_TEST_PACKET;
        start_mem_check;
        wait_on_mem_check;
        -- MEMORY STATE [e0,e1,0]
        
        Log("Insert Endpoint 2", INFO);
        endpoint            := e2;
        endpoint.nr         := 2;
        endpoint.match      := MATCH;
        gen_endpoint_match_frame(endpoint, stimulus_meta);
        SB_mem.Push(gen_reader_endpoint_mem_frame_a(endpoint));
        start_meta_test;
        wait_on_meta_sent;
        stimulus_meta   := EMPTY_TEST_PACKET;
        stimulus_user   := EMPTY_TEST_PACKET;
        start_mem_check;
        wait_on_mem_check;
        -- MEMORY STATE [e0,e1,e2]
        
        Log("Current Time: 0.5s", INFO);
        test_time   <= gen_duration(0,500);
        wait until rising_edge(clk);
        wait until rising_edge(clk); -- Allow idle_sig to go low
        wait_on_idle;
        
        Log("Endpoint 2 sent ACKNACK [Request SN 1]", INFO);
        endpoint            := e2;
        sub                 := DEFAULT_RTPS_SUBMESSAGE;
        sub.submessageID    := SID_ACKNACK;
        sub.writerId        := DEFAULT_WRITER_ENTITYID;
        sub.readerId        := endpoint.entityid;
        sub.readerSNState   := (base => gen_sn(1), numBits => int(1, CDR_LONG_WIDTH), bitmap => (0 => '1', others => '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;
        wait_on_idle;
        
        Log("Current Time: 1s", INFO);
        test_time   <= gen_duration(1,0);
        wait until rising_edge(clk);
        wait until rising_edge(clk); -- Allow idle_sig to go low
        wait_on_idle;
        
        Log("Check Removal of Endpoint 0", INFO);
        -- Re-check Mem-State
        endpoint        := e0;
        endpoint.nr     := 0;
        endpoint.match  := UNMATCH;
        SB_mem.Push(gen_reader_endpoint_mem_frame_a(endpoint));
        start_mem_check;
        wait_on_mem_check;
        endpoint        := e1;
        endpoint.nr     := 1;
        endpoint.match  := MATCH;
        SB_mem.Push(gen_reader_endpoint_mem_frame_a(endpoint));
        start_mem_check;
        wait_on_mem_check;
        endpoint        := e2;
        endpoint.nr     := 2;
        endpoint.match  := MATCH;
        SB_mem.Push(gen_reader_endpoint_mem_frame_a(endpoint));
        start_mem_check;
        wait_on_mem_check;
        -- MEMORY STATE [0,e1,e2]
        
        Log("Insert Endpoint 3", INFO);
        endpoint            := e3;
        endpoint.nr         := 0;
        endpoint.match      := MATCH;
        gen_endpoint_match_frame(endpoint, stimulus_meta);
        SB_mem.Push(gen_reader_endpoint_mem_frame_a(endpoint));
        start_meta_test;
        wait_on_meta_sent;
        stimulus_meta   := EMPTY_TEST_PACKET;
        stimulus_user   := EMPTY_TEST_PACKET;
        start_mem_check;
        wait_on_mem_check;
        -- MEMORY STATE [e3,e1,e2]
        
        Log("Current Time: 1.5s", INFO);
        test_time   <= gen_duration(1,500);
        wait until rising_edge(clk);
        wait until rising_edge(clk); -- Allow idle_sig to go low
        wait_on_idle;
        
        Log("Check Removal of Endpoint 2", INFO);
        -- Re-check Mem-State
        endpoint        := e3;
        endpoint.nr     := 0;
        endpoint.match  := MATCH;
        SB_mem.Push(gen_reader_endpoint_mem_frame_a(endpoint));
        start_mem_check;
        wait_on_mem_check;
        endpoint        := e1;
        endpoint.nr     := 1;
        endpoint.match  := MATCH;
        SB_mem.Push(gen_reader_endpoint_mem_frame_a(endpoint));
        start_mem_check;
        wait_on_mem_check;
        endpoint        := e2;
        endpoint.nr     := 2;
        endpoint.match  := UNMATCH;
        SB_mem.Push(gen_reader_endpoint_mem_frame_a(endpoint));
        start_mem_check;
        wait_on_mem_check;
        -- MEMORY STATE [e3,e1,0]
        
        Log("Current Time: 2s", INFO);
        test_time   <= gen_duration(2,0);
        wait until rising_edge(clk);
        wait until rising_edge(clk); -- Allow idle_sig to go low
        wait_on_idle;
        
        Log("Check Memory State", INFO);
        -- Re-check Mem-State
        endpoint        := e3;
        endpoint.nr     := 0;
        endpoint.match  := MATCH;
        SB_mem.Push(gen_reader_endpoint_mem_frame_a(endpoint));
        start_mem_check;
        wait_on_mem_check;
        endpoint        := e1;
        endpoint.nr     := 1;
        endpoint.match  := MATCH;
        SB_mem.Push(gen_reader_endpoint_mem_frame_a(endpoint));
        start_mem_check;
        wait_on_mem_check;
        -- MEMORY STATE [e3,e1,0]
        
        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;
    
    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(start_hc = '1', "Unexpected History Cache Operation initiated", FAILURE);
        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_mem.empty) then
                test_done   <= '1';
            else
                test_done   <= '0';
            end if;
        end if;
    end process;
    
    mem_check_prc : process
        alias mem is <<signal uut.mem_ctrl_inst.ram_inst.mem : TEST_RAM_TYPE>>;
        alias mem_op_done is <<signal uut.mem_op_done : std_logic>>;
        alias idle_sig is <<signal uut.idle_sig : std_logic>>;
        variable reference : TEST_READER_ENDPOINT_MEMORY_FRAME_TYPE_A;
    begin
        mem_check_done  <= '0';
        -- SAFEGUARD: (Prevent Fall-through Behavior)
        if (reset /= '0') then
            wait until reset = '0';
        end if;
        -- Wait for Trigger
        wait until rising_edge(check_trigger);
        -- Delay 1 clk (Allow trigger to go low)
        wait until rising_edge(clk);
        -- Wait for UUT IDLE state
        if (idle_sig /= '1') then
            wait until idle_sig = '1';
        end if;
        -- Wait for ongoing memory operation
        if (mem_op_done /= '1') then
            wait until mem_op_done = '1';
        end if;
        while (not SB_mem.empty) loop
            SB_mem.Pop(reference);
            for i in 0 to reference'length-1 loop
                AffirmIf(?? (mem(reference(i).addr) ?= reference(i).data), "Address: " & integer'image(reference(i).addr) & " Received: " & to_hstring(mem(reference(i).addr)) & " Expected: " & to_hstring(reference(i).data));
            end loop;
        end loop;
        -- Toggle High for one clock cycle
        mem_check_done  <= '1';
        wait until rising_edge(clk);
    end process;
    
    watchdog : process
    begin
        wait for 1 ms;
        Alert("Test timeout", FAILURE);
        std.env.stop;
    end process;
    
end architecture;