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.ros_package.all;
use work.rtps_test_package.all;
use work.GoalStatusArray_package.all;

-- This testbench tests the General Behavour of ROS publishers and subscribers
-- More specifically following tests are done:
--  * Test Unssuported Opcode Operations
--  * Test RETCODE_NO_DATA response from DDS Reader
--  * Test RETCODE_ERROR response from DDS Reader
--  * Test RETCODE_ERROR response from DDS Writer
--  * Test Sample with No Valid response from DDS Reader
--  * Test Little Endian Encoding/Decoding of Messages

entity L1_GoalStatusArray_ros_test2 is
end entity;

architecture testbench of L1_GoalStatusArray_ros_test2 is
    
    constant DEFAULT_SAMPLE_INFO : SAMPLE_INFO_TYPE := (
        sample_state                => ANY_SAMPLE_STATE,
        view_state                  => ANY_VIEW_STATE,
        instance_state              => ANY_INSTANCE_STATE,
        source_timestamp            => TIME_ZERO,
        instance_handle             => HANDLE_NIL,
        publication_handle          => HANDLE_NIL,
        disposed_generation_count   => (others => '0'),
        no_writers_generation_count => (others => '0'),
        sample_rank                 => (others => '0'),
        generation_rank             => (others => '0'),
        absolute_generation_rank    => (others => '0'),
        valid_data                  => '0'
    );
    
    signal clk, reset : std_logic := '0';
    signal valid, ready, last_word, last_word_out_w : std_logic := '0';
    signal data : std_logic_vector(WORD_WIDTH-1 downto 0) := (others => '0');
    signal start_sub, start_pub, ack_sub, ack_pub, taken_sub, taken_pub, done_sub, done_pub : std_logic := '0';
    signal opcode_sub, opcode_pub : ROS_TOPIC_OPCODE_TYPE := NOP;
    signal return_code_sub, return_code_pub : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := (others => '0');
    signal sample_info_r : SAMPLE_INFO_TYPE := DEFAULT_SAMPLE_INFO;
    signal start_r : std_logic := '0';
    signal return_code_r, return_code_w : std_logic_vector(RETURN_CODE_WIDTH-1 downto 0) := (others => '0');
    signal ready_w, selector : std_logic := '0';
    signal message_info_sub : MESSAGE_INFO_TYPE := EMPTY_MESSAGE_INFO;
    --
    signal status_list_len_pub, status_list_len_sub, status_list_addr_pub, status_list_addr_sub : std_logic_vector(STATUS_LIST_ADDR_WIDTH-1 downto 0) := (others => '0');
    signal status_list_ready_pub, status_list_ready_sub, status_list_ren_pub, status_list_ren_sub, status_list_valid_pub, status_list_valid_sub, status_list_ack_pub, status_list_ack_sub, status_list_wen_pub : std_logic := '0';
    signal status_list_goal_info_goal_id_r_pub, status_list_goal_info_goal_id_w_pub, status_list_goal_info_goal_id_sub : std_logic_vector(UUID_WIDTH-1 downto 0) := (others => '0');
    signal status_list_goal_info_stamp_r_pub, status_list_goal_info_stamp_w_pub, status_list_goal_info_stamp_sub : std_logic_vector(ROS_TIME_WIDTH-1 downto 0) := (others => '0');
    signal status_list_status_r_pub, status_list_status_w_pub, status_list_status_sub : std_logic_vector(CDR_INT8_WIDTH-1 downto 0) := (others => '0');
begin
    
    uut_sub : entity work.GoalStatusArray_ros_sub(arch)
        port map (
            clk                                 => clk,
            reset                               => reset,
            start_dds                           => start_r,--
            ack_dds                             => '1',--
            opcode_dds                          => open,
            instance_state_dds                  => open,
            view_state_dds                      => open,
            sample_state_dds                    => open,
            instance_handle_dds                 => open,
            max_samples_dds                     => open,
            get_data_dds                        => open,--
            done_dds                            => '1',--
            return_code_dds                     => return_code_r,--
            valid_in_dds                        => valid,--
            ready_in_dds                        => ready,--
            data_in_dds                         => data,--
            last_word_in_dds                    => last_word,--
            sample_info_dds                     => sample_info_r,
            sample_info_valid_dds               => '1',--
            sample_info_ack_dds                 => open,--
            eoc_dds                             => '1',--
            status_dds                          => (others => '0'),
            start_user                          => start_sub,
            opcode_user                         => opcode_sub,
            ack_user                            => ack_sub,
            done_user                           => done_sub,
            return_code_user                    => return_code_sub,
            data_available_user                 => open,
            status_list_len                     => status_list_len_sub,
            status_list_addr                    => status_list_addr_sub,
            status_list_ready                   => status_list_ready_sub,
            status_list_ren                     => status_list_ren_sub,
            status_list_valid                   => status_list_valid_sub,
            status_list_ack                     => status_list_ack_sub,
            status_list_goal_info_goal_id       => status_list_goal_info_goal_id_sub,
            status_list_goal_info_stamp         => status_list_goal_info_stamp_sub,
            status_list_status                  => status_list_status_sub,
            message_info_user                   => message_info_sub,
            taken_user                          => taken_sub
        );
    
    uut_pub : entity work.GoalStatusArray_ros_pub(arch)
        generic map (
            LITTLE_ENDIAN   => '1'
        )
        port map (
            clk                             => clk,
            reset                           => reset,
            start_dds                       => open,--
            ack_dds                         => '1',--
            opcode_dds                      => open,
            instance_handle_out_dds         => HANDLE_NIL,
            source_ts_dds                   => open,
            max_wait_dds                    => open,
            done_dds                        => '1',--
            return_code_dds                 => return_code_w,--
            instance_handle_in_dds          => open,
            valid_out_dds                   => valid,--
            ready_out_dds                   => ready_w,--ready,--
            data_out_dds                    => data,--
            last_word_out_dds               => last_word_out_w,--last_word,--
            valid_in_dds                    => '0',
            ready_in_dds                    => open,
            data_in_dds                     => (others => '0'),
            last_word_in_dds                => '0',
            status_dds                      => (others => '0'),
            start_user                      => start_pub,--
            ack_user                        => ack_pub,--
            opcode_user                     => opcode_pub,--
            status_list_len                 => status_list_len_pub,
            status_list_addr                => status_list_addr_pub,
            status_list_ready               => status_list_ready_pub,
            status_list_ren                 => status_list_ren_pub,
            status_list_wen                 => status_list_wen_pub,
            status_list_valid               => status_list_valid_pub,
            status_list_ack                 => status_list_ack_pub,
            status_list_goal_info_goal_id_r => status_list_goal_info_goal_id_r_pub,
            status_list_goal_info_goal_id_w => status_list_goal_info_goal_id_w_pub,
            status_list_goal_info_stamp_r   => status_list_goal_info_stamp_r_pub,
            status_list_goal_info_stamp_w   => status_list_goal_info_stamp_w_pub,
            status_list_status_r            => status_list_status_r_pub,
            status_list_status_w            => status_list_status_w_pub,
            done_user                       => done_pub,--
            return_code_user                => return_code_pub --
        );
    
    process (all)
    begin
        if (selector = '1') then
            ready_w     <= '1';
            last_word   <= '0';
        else
            ready_w     <= ready;
            last_word   <= last_word_out_w;
        end if;
    end process;
    
    stimulus_prc : process
        variable RV : RandomPType;
        variable GOAL_ID, STAMP, STATUS : AlertLogIDType;
        
        procedure wait_on_sig(signal sig : std_logic) is
        begin
            if (sig /= '1') then
                wait on sig until sig = '1';
            end if;
        end procedure;
    begin
        
        SetAlertLogName("L1_GoalStatusArray_ros_test2 - (Little Endian) - General");
        SetAlertEnable(FAILURE, TRUE);
        SetAlertEnable(ERROR, TRUE);
        SetAlertEnable(WARNING, TRUE);
        SetLogEnable(DEBUG, FALSE);
        SetLogEnable(PASSED, FALSE);
        SetLogEnable(INFO, TRUE);
        RV.InitSeed(RV'instance_name);
        GOAL_ID := GetAlertLogID("GOAL_ID", ALERTLOG_BASE_ID);
        STAMP   := GetAlertLogID("STAMP", ALERTLOG_BASE_ID);
        STATUS  := GetAlertLogID("STATUS", ALERTLOG_BASE_ID);
        
        Log("Initial Reset", INFO);
        selector                    <= '1';
        return_code_r               <= RETCODE_OK;
        return_code_w               <= RETCODE_OK;
        sample_info_r.valid_data    <= '0';
        start_sub                   <= '0';
        start_pub                   <= '0';
        reset   <= '1';
        wait until rising_edge(clk);
        wait until rising_edge(clk);
        reset   <= '0';
        
        Log("SUBSCRIBER: Test Unsupported Opcode", INFO);
        start_sub     <= '1';
        opcode_sub    <= PUBLISH;
        wait_on_sig(ack_sub);
        wait until rising_edge(clk);
        start_sub     <= '0';
        wait_on_sig(done_sub);
        wait for 1 ps; -- Make sure all signals stable
        AlertIf(return_code_sub /= ROS_RET_UNSUPPORTED, "Unexpected Subscriber Response", FAILURE);
        wait until rising_edge(clk);
        
        Log("PUBLISHER: Test Unsupported Opcode", INFO);
        start_pub   <= '1';
        opcode_pub  <= TAKE;
        wait_on_sig(ack_pub);
        wait until rising_edge(clk);
        start_pub   <= '0';
        wait_on_sig(done_pub);
        wait for 1 ps; -- Make sure all signals stable
        AlertIf(return_code_pub /= ROS_RET_UNSUPPORTED, "Unexpected Publisher Response", FAILURE);
        wait until rising_edge(clk);
        
        Log("SUBSCRIBER: Test No Data", INFO);
        return_code_r   <= RETCODE_NO_DATA;
        start_sub       <= '1';
        opcode_sub      <= TAKE;
        wait_on_sig(ack_sub);
        wait until rising_edge(clk);
        start_sub       <= '0';
        wait_on_sig(done_sub);
        wait for 1 ps; -- Make sure all signals stable
        AlertIf(return_code_sub /= ROS_RET_OK, "Unexpected Subscriber Response", FAILURE);
        AlertIf(taken_sub /= '0', "Subscriber taken is unexpectedly set", FAILURE);
        wait until rising_edge(clk);
        
        Log("SUBSCRIBER: Test Reader Error", INFO);
        return_code_r   <= RETCODE_ERROR;
        start_sub       <= '1';
        opcode_sub      <= TAKE;
        wait_on_sig(ack_sub);
        wait until rising_edge(clk);
        start_sub       <= '0';
        wait_on_sig(done_sub);
        wait for 1 ps; -- Make sure all signals stable
        AlertIf(return_code_sub /= ROS_RET_ERROR, "Unexpected Subscriber Response", FAILURE);
        wait until rising_edge(clk);
        
        Log("PUBLISHER: Test Writer Error", INFO);
        return_code_w   <= RETCODE_ERROR;
        start_pub       <= '1';
        opcode_pub      <= PUBLISH;
        wait_on_sig(ack_pub);
        wait until rising_edge(clk);
        start_pub       <= '0';
        wait_on_sig(done_pub);
        wait for 1 ps; -- Make sure all signals stable
        AlertIf(return_code_pub /= ROS_RET_ERROR, "Unexpected Publisher Response", FAILURE);
        wait until rising_edge(clk);
        
        return_code_r   <= RETCODE_OK;
        return_code_w   <= RETCODE_OK;
        selector        <= '0';
        
        Log("Setting Setting Data in Publisher Side", INFO);
        for i in 0 to 10 loop
            status_list_len_pub                 <= int(11,status_list_len_pub'length);
            status_list_addr_pub                <= int(i,status_list_addr_pub'length);
            status_list_goal_info_goal_id_w_pub <= RV.RandSlv(status_list_goal_info_goal_id_w_pub'length);
            status_list_goal_info_stamp_w_pub   <= RV.RandSlv(status_list_goal_info_stamp_w_pub'length);
            status_list_status_w_pub            <= RV.RandSlv(status_list_status_w_pub'length);
            wait_on_sig(status_list_ready_pub);
            status_list_wen_pub <= '1';
            wait for TEST_CLOCK_PERIOD;
            status_list_wen_pub <= '0';
        end loop;
        
        Log("Publish message", INFO);
        start_pub     <= '1';
        opcode_pub    <= PUBLISH;
        wait_on_sig(ack_pub);
        wait until rising_edge(clk);
        start_pub     <= '0';
        
        Log("Take message", INFO);
        start_sub     <= '1';
        opcode_sub    <= TAKE;
        wait_on_sig(ack_sub);
        wait until rising_edge(clk);
        start_sub     <= '0';
        
        -- TEST NO VALID DATA
        wait_on_sig(start_r);
        wait until rising_edge(clk);
        wait until rising_edge(clk);
        wait_on_sig(start_r);
        sample_info_r.valid_data    <= '1';
        
        Log("Wait for Data", INFO);
        wait_on_sig(done_sub);
        wait until rising_edge(clk);
        
        AlertIf(return_code_sub /= ROS_RET_OK, "Subscriber did Return ERROR", FAILURE);
        AlertIf(taken_sub /= '1', "Subscriber did not take Message", FAILURE);
        
        Log("Compare Messages", INFO);
        AffirmIfEqual(status_list_len_sub, status_list_len_pub);
        for i in 0 to to_integer(unsigned(status_list_len_pub))-1 loop
            status_list_addr_pub    <= int(i,status_list_addr_pub'length);
            status_list_addr_sub    <= int(i,status_list_addr_sub'length);
            wait_on_sig(status_list_ready_pub);
            wait_on_sig(status_list_ready_sub);
            status_list_ren_pub <= '1';
            status_list_ren_sub <= '1';
            wait for TEST_CLOCK_PERIOD;
            status_list_ren_pub <= '0';
            status_list_ren_sub <= '0';
            wait_on_sig(status_list_valid_pub);
            wait_on_sig(status_list_valid_sub);
            AffirmIfEqual(GOAL_ID, status_list_goal_info_goal_id_sub, status_list_goal_info_goal_id_r_pub);
            AffirmIfEqual(STAMP, status_list_goal_info_stamp_sub, status_list_goal_info_stamp_r_pub);
            AffirmIfEqual(STATUS, status_list_status_sub, status_list_status_r_pub);
            status_list_ack_pub <= '1';
            status_list_ack_sub <= '1';
            wait for TEST_CLOCK_PERIOD;
            status_list_ack_pub <= '0';
            status_list_ack_sub <= '0';
        end loop;
        
        TranscriptOpen(RESULTS_FILE, APPEND_MODE);
        SetTranscriptMirror;
        ReportAlerts;
        TranscriptClose;
        std.env.stop;
        wait;
    end process;
    
    clock_prc : process
    begin
        clk <= '0';
        wait for TEST_CLOCK_PERIOD/2;
        clk <= '1';
        wait for TEST_CLOCK_PERIOD/2;
    end process;
    
    watchdog : process
    begin
        wait for 1 ms;
        Alert("Test timeout", FAILURE);
        std.env.stop;
    end process;
    
end architecture;