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 general behavior of the rtps_out entity. Following 3 tests are done in sequence:
-- TEST 1
-- Add 2 random sized packets to random input port t1. After t1 is beginning being processed add 2 random sized packets to a random input port t2 that is after the t1 input port,
-- and add 2 random sized packets to a random input port t3 that is before the t1 input port. The packets should come in order : t1p1, t2p1, t3p1, t1p2, t2p2, t3p2.
-- TEST 2
-- Add 4 packets with sizes 4-Bytes, 3-Bytes, 2-Bytes, and 1-Bytes respectively to the input port t1. Add a maximum size packet at input port t2, and a oversized (over maximum size)
-- at input port t3. The expected order should be: t1p1, t2p1. 
-- Note that during the beginning of this test the UUT should be processing input port t3.
-- TEST 3
-- Add one random sized packet to each available input port. The expected order is: t1+1p1, t1+2p1,...,t1p1.
-- Note that during the beginning of this test the UUT should be processing input port t1.

entity L0_rtps_out_test1 is
end entity;

architecture testbench of L0_rtps_out_test1 is
    
    -- *CONSTANT DECLARATION*
    constant MAX_SIZE : natural := 20;
    
    -- *TYPE DECLARATION*
    type TEST_STAGE_TYPE is (IDLE, BUSY);
    type TEST_PACKET_ARRAY_TYPE is array (0 to NUM_ENDPOINTS) of TEST_PACKET_TYPE;
    type TEST_STAGE_ARRAY_TYPE is array (0 to NUM_ENDPOINTS) of TEST_STAGE_TYPE;
    type CNT_STIM_ARRAY_TYPE is array (0 to NUM_ENDPOINTS) of natural;
    
    -- *SIGNAL DECLARATION*
    signal clk, wr_sig, full : std_logic := '0';
    signal reset : std_logic := '1';
    signal data_out : std_logic_vector(WORD_WIDTH-1 downto 0) := (others => '0');
    signal data_in : RTPS_OUT_DATA_TYPE := (others => (others => '0'));
    signal last_word_in, rd_sig, empty : std_logic_vector(0 to NUM_ENDPOINTS) := (others => '0');
    
    signal stim_stage : TEST_STAGE_ARRAY_TYPE := (others => IDLE);
    shared variable stimulus : TEST_PACKET_ARRAY_TYPE := (others => EMPTY_TEST_PACKET);
    
    signal packet_sent : std_logic_vector(0 to NUM_ENDPOINTS) := (others => '0');
    
    signal cnt_stim : CNT_STIM_ARRAY_TYPE := (others => 0);
    signal start : std_logic := '0';
    shared variable SB : osvvm.ScoreBoardPkg_slv.ScoreBoardPType;
    signal stim_done, check_done : std_logic := '0';
    
    -- *FUNCTION DECLARATION*
    procedure push_reference(index : in natural; input : in TEST_PACKET_TYPE) is
        variable tmp : natural := 0;
        variable header_start : natural := 0;
        variable packet_start : natural := input.length;
        variable len : natural := 0;
    begin
        for i in 0 to input.length-1 loop
            if (tmp = index) then
                SB.Push(input.data(i));
                -- Mark End of Header/Start of Packet
                
            end if;
            if (i = header_start and tmp = index) then
                packet_start := header_start+3;
            end if;
            -- Calculate and Push Length
            if (i = packet_start-1) then
                -- Count until End of Current Packet
                loop
                    len := len + 1;
                    if (input.last(len-1+packet_start) = '1') then
                        exit;
                    end if;
                end loop;
                SB.Push(std_logic_vector(to_unsigned(len, WORD_WIDTH)));
            end if;
            if (input.last(i) = '1') then
                tmp := tmp + 1;
                header_start    := i+1;
            end if;
        end loop;
    end procedure;
    
begin
    
    empty <= packet_sent;
    
    -- Unit Under Test
    uut : entity work.rtps_out(arch)
        generic map (
            MAX_BUFFER_SIZE => MAX_SIZE-3
        )
        port map (
            clk         => clk,
            reset       => reset,
            data_in     => data_in,
            last_word_in=> last_word_in,
            rd          => rd_sig,
            empty       => empty,
            data_out    => data_out,
            wr          => wr_sig,
            full        => full
        );
    
    stimulus_prc : process
        variable RV         : RandomPType;
        variable t1, t2, t3 : natural := 0;
        variable tmp        : std_logic_vector(0 to NUM_ENDPOINTS) := (others => '0');
        variable tmp_packet : TEST_PACKET_TYPE := EMPTY_TEST_PACKET;
        
        procedure gen_rand_packet(size : in natural; output : inout TEST_PACKET_TYPE) is
            variable tmp : natural := 0;
        begin
            if (size = 0) then
                tmp := RV.RandInt(4, MAX_SIZE-1);
            else
                tmp := size;
            end if;
            Log("Packet Size: " & to_string(tmp), DEBUG);
            
            for i in 0 to tmp-1 loop
                output.data(output.length) := RV.RandSlv(WORD_WIDTH);
                Log("DATA: " & to_hstring(output.data(output.length)), DEBUG);
                output.length   := output.length + 1;
            end loop;
            output.last(output.length-1) := '1';
        end procedure;
        
        procedure start_test is
        begin
            start <= '1';
            wait until rising_edge(clk);
            start       <= '0';
            wait until rising_edge(clk);
        end procedure;
    begin
        assert (NUM_ENDPOINTS >= 2) report "Testbench needs at least 2 Endpoints" severity FAILURE;
        assert (MAX_SIZE > 4) report "MAX_SIZE has to be larger than 4" severity FAILURE;
        
        SetAlertLogName("rtps_out - Level 0 - Generic");
        SetAlertEnable(FAILURE, TRUE);
        SetAlertEnable(ERROR, TRUE);
        SetAlertEnable(WARNING, TRUE);
        SetLogEnable(DEBUG, FALSE);
        SetLogEnable(PASSED, FALSE);
        SetLogEnable(INFO, TRUE);
        RV.InitSeed(RV'instance_name);
        
        -- 
        
        Log("Initiating Test", INFO);
        stim_done   <= '0';
        start       <= '0';
        reset       <= '1';
        wait until rising_edge(clk);
        wait until rising_edge(clk);
        reset   <= '0';
        
        Log("Begin Test 1", INFO);
        t1 := RV.RandInt(1, NUM_ENDPOINTS-1);
        Log("T1: " & to_string(t1), DEBUG);
        t2 := RV.RandInt(t1+1, NUM_ENDPOINTS);
        Log("T2: " & to_string(t2), DEBUG);
        t3 := RV.RandInt(0, t1-1);
        Log("T3: " & to_string(t3), DEBUG);
        -- Generate 2 Packets
        gen_rand_packet(0, stimulus(t1));
        gen_rand_packet(0, stimulus(t1));
        -- Push T1 Packet 0
        push_reference(0, stimulus(t1));
        start_test;
        -- Wait for UUT do reach t1
        wait until rd_sig(t1) = '1';
        -- Generate 2 Packets for T2 and T3
        gen_rand_packet(0, stimulus(t2));
        gen_rand_packet(0, stimulus(t2));
        gen_rand_packet(0, stimulus(t3));
        gen_rand_packet(0, stimulus(t3));
        -- Push T2 Packet 0
        push_reference(0, stimulus(t2));
        -- Push T3 Packet 0
        push_reference(0, stimulus(t3));
        -- Push T1 Packet 1
        push_reference(1, stimulus(t1));
        -- Push T2 Packet 1
        push_reference(1, stimulus(t2));
        -- Push T3 Packet 1
        push_reference(1, stimulus(t3));
        start_test;
        -- Wait until begining of t3 sending
        tmp     := (others => '1');
        tmp(t3) := '0';
        wait on rd_sig until packet_sent = tmp and rd_sig = not tmp;
        
        -- Reset Input
        tmp_packet      := stimulus(t3);
        stimulus        := (others => EMPTY_TEST_PACKET);
        stimulus(t3)    := tmp_packet;
        Log("Begin Test 2", INFO);
        -- Min Valid Packet
        gen_rand_packet(4, stimulus(t1));
        push_reference(0, stimulus(t1));
        -- MAX Valid Packet
        gen_rand_packet(MAX_SIZE, stimulus(t2));
        push_reference(0, stimulus(t2));
        -- Invalid Packet (Over size)
        gen_rand_packet(MAX_SIZE+1, stimulus(t3));
        -- Invalid Packet [Packet too small]
        gen_rand_packet(3, stimulus(t1));
        gen_rand_packet(2, stimulus(t1));
        gen_rand_packet(1, stimulus(t1));
        start_test;
        -- Wait until all but t1 sent
        tmp     := (others => '1');
        tmp(t1) := '0';
        wait on rd_sig until packet_sent = tmp and rd_sig = not tmp;
        
        -- reset Input
        tmp_packet      := stimulus(t1);
        stimulus        := (others => EMPTY_TEST_PACKET)    ;
        stimulus(t1)    := tmp_packet;
        Log("Begin Test 3", INFO);
        for i in 1 to NUM_ENDPOINTS+1 loop
            t3 := (t1+i) mod (NUM_ENDPOINTS+1);
            Log("Generate package for input port : " & to_string(t3), DEBUG);
            gen_rand_packet(0, stimulus(t3));
            if (t3 = t1) then
                -- t1 has 4 Packets in Queue
                push_reference(4, stimulus(t3));
            else
                push_reference(0, stimulus(t3));
            end if;
        end loop;
        start_test;
        -- Wait until all sent
        wait on packet_sent until packet_sent(t1) = '1';
        
        
        TranscriptOpen(RESULTS_FILE, APPEND_MODE);
        SetTranscriptMirror;
        stim_done   <= '1';
        wait until check_done = '1';
        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;
    
    endpoint_full_prc : process
    begin
        full   <= '0';
        wait until wr_sig = '1';
        wait until rising_edge(clk);
        full   <= '1';
        wait until rising_edge(clk);
    end process;
    
    alert_prc : process(all)
    begin
        if rising_edge(clk) then
            alertif((empty and rd_sig) /= (rd_sig'range => '0'), "Input FIFO read signal high while empty signal high", ERROR);
            alertif((full and wr_sig) = '1', "Output FIFO write signal high while full signal high", ERROR);
        end if;
    end process;
    
    input_prc_gen : for i in 0 to NUM_ENDPOINTS generate
    begin
        input_prc : process(all)
        begin
            data_in(i)      <= stimulus(i).data(cnt_stim(i));
            last_word_in(i) <= stimulus(i).last(cnt_stim(i));
            
            if rising_edge(clk) then
                if (reset = '1') then
                    cnt_stim(i)    <= 0;
                    stim_stage(i)  <= IDLE;
                    packet_sent(i) <= '1';
                else
                    case (stim_stage(i)) is
                        when IDLE =>
                            if (start = '1' and stimulus(i).length /= 0) then
                                stim_stage(i)  <= BUSY;
                                packet_sent(i) <= '0';
                            end if;
                        when BUSY =>
                            if (rd_sig(i) = '1') then
                                if (cnt_stim(i) = stimulus(i).length-1) then
                                    stim_stage(i)  <= IDLE;
                                    packet_sent(i) <= '1';
                                    cnt_stim(i)    <= 0;
                                else
                                    cnt_stim(i)    <= cnt_stim(i) + 1;
                                end if;
                            end if;
                    end case;
                end if;
            end if;
        end process;
    end generate;
    
    
    output_check_prc : process(all)
    begin
        check_done  <= '0';
        if rising_edge(clk) then
            if (wr_sig = '1') then
                SB.Check(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 5 ms;
        Alert("Test timeout", FAILURE);
        std.env.stop;
    end process;
    
end architecture;