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.Fibonacci_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_Fibonacci_ros_action_feedback_test2 is
end entity;

architecture testbench of L1_Fibonacci_ros_action_feedback_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 goal_id_sub, goal_id_pub : std_logic_vector(UUID_WIDTH-1 downto 0) := (others => '0');
    signal seq_len_sub, seq_len_pub, seq_addr_sub, seq_addr_pub : std_logic_vector(F_SEQ_ADDR_WIDTH-1 downto 0) := (others => '0');
    signal seq_ready_sub, seq_ready_pub, seq_ren_sub, seq_ren_pub, seq_wen_pub, seq_valid_sub, seq_valid_pub, seq_ack_sub, seq_ack_pub : std_logic := '0';
    signal seq_sub, seq_r_pub, seq_w_pub : std_logic_vector(CDR_LONG_WIDTH-1 downto 0) := (others => '0');
begin
    
    uut_sub : entity work.Fibonacci_ros_action_feedback_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,
            goal_id                             => goal_id_sub,
            seq_len                             => seq_len_sub,
            seq_addr                            => seq_addr_sub,
            seq_ready                           => seq_ready_sub,
            seq_ren                             => seq_ren_sub,
            seq_valid                           => seq_valid_sub,
            seq_ack                             => seq_ack_sub,
            seq                                 => seq_sub,
            message_info_user                   => message_info_sub,
            taken_user                          => taken_sub
        );
    
    uut_pub : entity work.Fibonacci_ros_action_feedback_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,--
            goal_id                 => goal_id_pub,
            seq_len                 => seq_len_pub,
            seq_addr                => seq_addr_pub,
            seq_ready               => seq_ready_pub,
            seq_ren                 => seq_ren_pub,
            seq_wen                 => seq_wen_pub,
            seq_valid               => seq_valid_pub,
            seq_ack                 => seq_ack_pub,
            seq_r                   => seq_r_pub,
            seq_w                   => seq_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, SEQ : 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_Fibonacci_ros_action_feedback_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);
        SEQ     := GetAlertLogID("SEQ", 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);
        goal_id_pub <= RV.RandSlv(goal_id_pub'length);
        for i in 0 to 9 loop
            seq_len_pub     <= int(10,seq_len_pub'length);
            seq_addr_pub    <= int(i,seq_addr_pub'length);
            seq_w_pub       <= RV.RandSlv(seq_w_pub'length);
            wait_on_sig(seq_ready_pub);
            seq_wen_pub     <= '1';
            wait for TEST_CLOCK_PERIOD;
            seq_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(GOAL_ID, goal_id_sub, goal_id_pub);
        AffirmIfEqual(SEQ, seq_len_sub, seq_len_pub);
        for i in 0 to to_integer(unsigned(seq_len_pub))-1 loop
            seq_addr_pub    <= int(i,seq_addr_pub'length);
            seq_addr_sub    <= int(i,seq_addr_sub'length);
            wait_on_sig(seq_ready_pub);
            wait_on_sig(seq_ready_sub);
            seq_ren_pub <= '1';
            seq_ren_sub <= '1';
            wait for TEST_CLOCK_PERIOD;
            seq_ren_pub <= '0';
            seq_ren_sub <= '0';
            wait_on_sig(seq_valid_pub);
            wait_on_sig(seq_valid_sub);
            AffirmIfEqual(SEQ, seq_sub, seq_r_pub);
            seq_ack_pub <= '1';
            seq_ack_sub <= '1';
            wait for TEST_CLOCK_PERIOD;
            seq_ack_pub <= '0';
            seq_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;