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 Routing of the rtps_handler. It generates stimulus for different destinations and compares the selected output to the expected.
-- This testbench covers following:
--  * METATRAFFIC/USERTRAFFIC/UNKNOWN Destination Addresses (All Address/Port Combinations)
--  * UNKNOWN/BUILT-IN/USER/ENTITY_UNKNOWN Entity Destinations
--  * READER/WRITER Source


entity L0_rtps_handler_test2 is
end entity;

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

    -- Unit Under Test
    uut : entity work.rtps_handler(arch)
        port map (
            clk             => clk,
            reset           => reset,
            empty           => in_empty or packet_sent,
            rd              => rd_sig,
            data_in         => data_in,
            data_out        => data_out,
            builtin_full    => builtin_full,
            builtin_wr      => builtin_wr,
            user_full       => user_full,
            user_wr         => user_wr,
            last_word_out   => last_word_out
        );
    
    stimulus_prc : process
        variable rtps_header        : RTPS_HEADER_TYPE := DEFAULT_RTPS_HEADER;
        variable rtps_data          : RTPS_SUBMESSAGE_TYPE := DEFAULT_RTPS_SUBMESSAGE;
        variable check_cnt          : natural := 0;
        variable RV                 : RandomPType;
        variable UDP                : OUTPUT_HEADER_TYPE;
        variable tmp_loc_list       : LOCATOR_LIST_TYPE;
        variable ref1, ref2, ref3   : std_logic_vector(0 to NUM_ENDPOINTS);
        variable tmp_id             : std_logic_vector(ENTITYID_WIDTH-1 downto 0);
        variable is_meta            : boolean;
        
        -- Wrapper to use procedure as function
        impure function gen_rand_loc_2 return LOCATOR_TYPE is
            variable ret : LOCATOR_TYPE := EMPTY_LOCATOR;
        begin
            gen_rand_loc(RV, ret);
            return ret;
        end function;
        
        procedure start_test is
        begin
            start <= '1';
            wait until rising_edge(clk);
            start <= '0';
            wait until rising_edge(clk);
        end procedure;
    begin
        
        SetAlertLogName("rtps_handler - Level 0 - Output Routing");
        SetAlertEnable(FAILURE, TRUE);
        SetAlertEnable(ERROR, TRUE);
        SetAlertEnable(WARNING, TRUE);
        SetLogEnable(DEBUG, FALSE);
        SetLogEnable(PASSED, FALSE);
        SetLogEnable(INFO, TRUE);
        RV.InitSeed(RV'instance_name);
        
        
        -- UDP Header
        UDP         := (src => gen_rand_loc_2, dest => EMPTY_LOCATOR);
        
        -- Default Valid Data Submessage
        rtps_data                                   := DEFAULT_RTPS_SUBMESSAGE;
        rtps_data.flags(SUBMESSAGE_DATA_FLAG_POS)   := '1';
        rtps_data.data                              := (length => 2, data => (0 => RV.RandSlv(WORD_WIDTH), 1 => RV.RandSlv(WORD_WIDTH), others => (others => '0')), last => (others => '0'));
        
        
        
        Log("Initiating Test", INFO);
        in_empty        <= '0';
        builtin_full    <= '0';
        user_full       <= (others => '0');
        start           <= '0';
        reset           <= '1';
        wait until rising_edge(clk);
        wait until rising_edge(clk);
        reset   <= '0';
        
        for i in 0 to 2 loop    -- USER/META/UKNOWN Traffic
            case (i) is
                -- META TRAFFIC ADDRESSES
                when 0 =>
                    tmp_loc_list := DEST_LOC.meta;
                    ref1         := (NUM_ENDPOINTS => '1', others => '0');
                    is_meta      := TRUE;
                -- USER TRAFFIC ADDRESSES
                when 1 =>
                    tmp_loc_list := DEST_LOC.user;
                    ref1         := (NUM_ENDPOINTS => '0', others => '1');
                    is_meta      := FALSE;
                -- UKNOWN ADDRESSES
                when others =>
                    ref1         := (others => '0');
                    tmp_loc_list := (   numLocators => int(2,CDR_LONG_WIDTH), 
                                        locator => (
                                            0 => (
                                                kind    => LOCATOR_KIND_UDPv4,
                                                portn   => (LOCATOR_PORT_WIDTH-1 downto UDP_PORT_WIDTH => '0') & META_IPv4_MULTICAST_PORT,
                                                addr    => RV.RandSlv(LOCATOR_ADDR_WIDTH)
                                            ),
                                            1 => (
                                                kind    => LOCATOR_KIND_UDPv4,
                                                portn   => RV.RandSlv(LOCATOR_PORT_WIDTH),
                                                addr    => (LOCATOR_ADDR_WIDTH-1 downto IPv4_ADDRESS_WIDTH => '0') & DEFAULT_IPv4_META_ADDRESS
                                            ),
                                            others => EMPTY_LOCATOR
                                        )
                                    );
                    is_meta      := FALSE;
            end case;
            for j in 0 to to_integer(unsigned(tmp_loc_list.numLocators))-1 loop -- Destination Address
                UDP.dest := tmp_loc_list.locator(j);
                
                for k in 0 to NUM_ENDPOINTS+2 loop -- Destination Entity ID
                    case (k) is
                        -- UKNOWN
                        when NUM_ENDPOINTS =>
                            ref2               := (others => '0');
                            tmp_id                      := RV.RandSlv(ENTITYID_WIDTH);
                            tmp_id(ENTITY_KIND_H_RANGE) := USER_DEFINED_ENTITY;
                            tmp_id(ENTITY_KIND_L_RANGE) := UNKNOWN_KIND;
                        -- ENTITYID_UKNOWN
                        when NUM_ENDPOINTS+1 =>
                            tmp_id  := ENTITYID_UNKNOWN;
                            ref2    := (others => '1');
                        -- BUILT_IN
                        when NUM_ENDPOINTS+2 =>
                            tmp_id  := ENTITYID_PARTICIPANT;
                            ref2    := (NUM_ENDPOINTS => '1', others => '0');
                        -- USER
                        when others =>
                            tmp_id  := ENTITYID(k);
                            ref2    := (k => '1', others => '0');
                    end case;
                    
                    for l in 0 to 1 loop -- Reader/Writer Source
                        case (l) is
                            -- Writer Source
                            when 0 =>
                                rtps_data.submessageID  := SID_DATA;
                                rtps_data.readerId      := tmp_id;
                                rtps_data.writerId      := DEFAULT_WRITER_ENTITYID;
                                ref3                    := (NUM_ENDPOINTS => '1', NUM_READERS-1 downto 0 => '1', others => '0');
                            -- Reader Source
                            when others =>
                                rtps_data.submessageID  := SID_ACKNACK;
                                rtps_data.readerId      := DEFAULT_READER_ENTITYID;
                                rtps_data.writerId      := tmp_id;
                                ref3                    := (NUM_ENDPOINTS => '1', NUM_ENDPOINTS-1 downto NUM_READERS => '1', others => '0');
                        end case;
                        
                        -- *STIMULUS*
                        Log("Sending Packet. Config i=" & integer'image(i) & ", j=" & integer'image(j) & ", k=" & integer'image(k) & ", l=" & integer'image(l), INFO);
                        --report "ref1: " & to_string(ref1);
                        --report "ref2: " & to_string(ref2);
                        --report "ref3: " & to_string(ref3);
                        -- UDP Header
                        gen_output_header(UDP, stimulus);
                        -- RTPS Header
                        gen_rtps_header(rtps_header, stimulus);
                        -- Valid Data
                        gen_rtps_submessage(rtps_data, stimulus);
                        if ((ref1 and ref2 and ref3) /= (ref1'range => '0')) then
                            gen_rtps_handler_out(rtps_data, UDP.src, is_meta, TIME_INVALID, rtps_header.guidPrefix, dummy);
                        end if;
                        -- Finalize Packet
                        fix_output_packet(stimulus);
                        reference   <= ref1 and ref2 and ref3;
                        start_test;
                        wait_on_complete;
                        check_cnt := check_cnt + dummy.length;
                        stimulus.length := 0;
                        dummy.length := 0;
                    end loop;
                end loop;
            end loop;
        end loop;
        
        TranscriptOpen(RESULTS_FILE, APPEND_MODE);
        SetTranscriptMirror;
        AlertIf(GetAffirmCount < check_cnt, "Incomplete test run");
        ReportAlerts;
        TranscriptClose;
        std.env.stop;
        wait;
    end process;
    
    clock_prc : process
    begin
        clk <= '0';
        wait for 25 ns;
        clk <= '1';
        wait for 25 ns;
    end process;
    
    input_prc : process(all)
    begin
        data_in     <= stimulus.data(cnt_stim);
        
        if rising_edge(clk) then
            if (reset = '1') then
                cnt_stim    <= 0;
                stim_stage  <= IDLE;
                packet_sent <= '1';
            else
                case (stim_stage) is
                    when IDLE =>
                        if (start = '1' and stimulus.length /= 0) then
                            stim_stage  <= BUSY;
                            packet_sent <= '0';
                        end if;
                    when BUSY =>
                        if (rd_sig = '1') then
                            if (cnt_stim = stimulus.length-1) then
                                stim_stage  <= IDLE;
                                packet_sent <= '1';
                                cnt_stim    <= 0;
                            else
                                cnt_stim    <= cnt_stim + 1;
                            end if;
                        end if;
                end case;
            end if;
        end if;
    end process;
    
    output_prc : process(all)
    begin
        case (ref_stage) is
            when IDLE =>
                packet_checked  <= '1';
            when BUSY =>
                packet_checked  <= '0';
        end case;
        
        if rising_edge(clk) then
            if (reset = '1') then
                cnt_ref         <= 0;
                ref_stage       <= IDLE;
            else
                case (ref_stage) is
                    when IDLE =>
                        if (start = '1') then
                            ref_stage   <= BUSY;
                            cnt_ref     <= 0;
                        end if;
                    when BUSY =>
                        if (cnt_ref = dummy.length) then
                            ref_stage   <= IDLE;
                        elsif (builtin_wr = '1' or user_wr /= (0 to NUM_ENDPOINTS-1 => '0')) then
                            AffirmIfEqual((user_wr & builtin_wr), reference);
                            cnt_ref <= cnt_ref + 1;
                        end if;
                end case;
            end if;
        end if;
    end process;
    
    watchdog : process
    begin
        wait for 1 ms;
        Alert("Test timeout", FAILURE);
        std.env.stop;
    end process;
    
end architecture;