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 Services.
-- 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 Service Messages

entity L1_Fibonacci_ros_action_result_srv_test2 is
end entity;

architecture testbench of L1_Fibonacci_ros_action_result_srv_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_rq, valid_rr, ready_rq, ready_rr, last_word_rq, last_word_rr : std_logic := '0';
    signal data_rq, data_rr : std_logic_vector(WORD_WIDTH-1 downto 0) := (others => '0');
    signal start_c, start_s, ack_c, ack_s, taken_c, taken_s, done_c, done_s : std_logic := '0';
    signal opcode_c, opcode_s : ROS_SERVICE_OPCODE_TYPE := NOP;
    signal return_code_c, return_code_s : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := (others => '0');
    signal service_info_c, service_info_s : SERVICE_INFO_TYPE := EMPTY_SERVICE_INFO;
    signal sequence_id_c : std_logic_vector(ROS_SEQUENCE_ID_WIDTH-1 downto 0) := (others => '0');
    signal request_id_s : REQUEST_ID_TYPE := EMPTY_REQUEST_ID;
    signal sample_info_cr, sample_info_sr : SAMPLE_INFO_TYPE := DEFAULT_SAMPLE_INFO;
    signal start_sr, start_cr : std_logic := '0';
    signal return_code_cr, return_code_cw, return_code_sr, return_code_sw : std_logic_vector(RETURN_CODE_WIDTH-1 downto 0) := (others => '0');
    signal ready_sw, ready_cw, last_word_out_cw, last_word_out_sw, selector : std_logic := '0';
    --
    signal goal_id_c, goal_id_s : std_logic_vector(UUID_WIDTH-1 downto 0) := (others => '0');
    signal status_c, status_s : std_logic_vector(CDR_INT8_WIDTH-1 downto 0) := (others => '0');
    signal seq_len_c, seq_len_s : std_logic_vector(R_RR_SEQ_ADDR_WIDTH-1 downto 0) := (others => '0');
    signal seq_addr_c, seq_addr_s : std_logic_vector(R_RR_SEQ_ADDR_WIDTH-1 downto 0) := (others => '0');
    signal seq_ready_c, seq_ready_s, seq_ren_c, seq_ren_s, seq_wen_s, seq_valid_c, seq_valid_s, seq_ack_c, seq_ack_s : std_logic := '0';
    signal seq_c, seq_r_s, seq_w_s : std_logic_vector(CDR_LONG_WIDTH-1 downto 0) := (others => '0');
begin
    
    uut_c : entity work.Fibonacci_ros_action_result_srv_client(arch)
        port map (
            clk                                 => clk,
            reset                               => reset,
            start_r                             => start_cr,--
            ack_r                               => '1',--
            opcode_r                            => open,
            instance_state_r                    => open,
            view_state_r                        => open,
            sample_state_r                      => open,
            instance_handle_r                   => open,
            max_samples_r                       => open,
            get_data_r                          => open,--
            done_r                              => '1',--
            return_code_r                       => return_code_cr,--
            valid_in_r                          => valid_rr,--
            ready_in_r                          => ready_rr,--
            data_in_r                           => data_rr,--
            last_word_in_r                      => last_word_rr,--
            sample_info_r                       => sample_info_cr,
            sample_info_valid_r                 => '1',--
            sample_info_ack_r                   => open,--
            eoc_r                               => '1',--
            status_r                            => (others => '0'),
            start_w                             => open,--
            ack_w                               => '1',--
            opcode_w                            => open,
            instance_handle_out_w               => HANDLE_NIL,
            source_ts_w                         => open,
            max_wait_w                          => open,
            done_w                              => '1',--
            return_code_w                       => return_code_cw,
            instance_handle_in_w                => open,
            valid_out_w                         => valid_rq,--
            ready_out_w                         => ready_cw,--ready_rq,--
            data_out_w                          => data_rq,--
            last_word_out_w                     => last_word_out_cw,--last_word_rq,--
            valid_in_w                          => '0',
            ready_in_w                          => open,
            data_in_w                           => (others => '0'),
            last_word_in_w                      => '0',
            status_w                            => (others => '0'),
            start_user                          => start_c,--
            ack_user                            => ack_c,--
            opcode_user                         => opcode_c,--
            service_info_user                   => service_info_c,--
            sequence_id_user                    => sequence_id_c,--
            taken_user                          => taken_c,--
            data_available_user                 => open,--
            goal_id                             => goal_id_c,
            status                              => status_c,
            seq_len                             => seq_len_c,
            seq_addr                            => seq_addr_c,
            seq_ready                           => seq_ready_c,
            seq_ren                             => seq_ren_c,
            seq_valid                           => seq_valid_c,
            seq_ack                             => seq_ack_c,
            seq                                 => seq_c,
            done_user                           => done_c,--
            return_code_user                    => return_code_c --
        );
    
    uut_s : entity work.Fibonacci_ros_action_result_srv_server(arch)
        port map (
            clk                                 => clk,
            reset                               => reset,
            start_r                             => start_sr,--
            ack_r                               => '1',--
            opcode_r                            => open,
            instance_state_r                    => open,
            view_state_r                        => open,
            sample_state_r                      => open,
            instance_handle_r                   => open,
            max_samples_r                       => open,
            get_data_r                          => open,--
            done_r                              => '1',--
            return_code_r                       => return_code_sr,--
            valid_in_r                          => valid_rq,--
            ready_in_r                          => ready_rq,--
            data_in_r                           => data_rq,--
            last_word_in_r                      => last_word_rq,--
            sample_info_r                       => sample_info_sr,
            sample_info_valid_r                 => '1',--
            sample_info_ack_r                   => open,--
            eoc_r                               => '1',--
            status_r                            => (others => '0'),
            start_w                             => open,--
            ack_w                               => '1',--
            opcode_w                            => open,
            instance_handle_out_w               => HANDLE_NIL,
            source_ts_w                         => open,
            max_wait_w                          => open,
            done_w                              => '1',--
            return_code_w                       => return_code_sw,--
            instance_handle_in_w                => open,
            valid_out_w                         => valid_rr,--
            ready_out_w                         => ready_sw,--ready_rr,--
            data_out_w                          => data_rr,--
            last_word_out_w                     => last_word_out_sw,--last_word_rr,--
            valid_in_w                          => '0',
            ready_in_w                          => open,
            data_in_w                           => (others => '0'),
            last_word_in_w                      => '0',
            status_w                            => (others => '0'),
            start_user                          => start_s,--
            ack_user                            => ack_s,--
            opcode_user                         => opcode_s,--
            service_info_user                   => service_info_s,--
            request_id_user                     => request_id_s,--
            taken_user                          => taken_s,--
            data_available_user                 => open,--
            goal_id                             => goal_id_s,
            status                              => status_s,
            seq_len                             => seq_len_s,
            seq_addr                            => seq_addr_s,
            seq_ready                           => seq_ready_s,
            seq_ren                             => seq_ren_s,
            seq_wen                             => seq_wen_s,
            seq_valid                           => seq_valid_s,
            seq_ack                             => seq_ack_s,
            seq_r                               => seq_r_s,
            seq_w                               => seq_w_s,
            done_user                           => done_s,--
            return_code_user                    => return_code_s --
        );
    
    process (all)
    begin
        if (selector = '1') then
            ready_cw        <= '1';
            ready_sw        <= '1';
            last_word_rq    <= '0';
            last_word_rr    <= '0';
        else
            ready_cw        <= ready_rq;
            ready_sw        <= ready_rr;
            last_word_rq    <= last_word_out_cw;
            last_word_rr    <= last_word_out_sw;
        end if;
    end process;
    
    stimulus_prc : process
        variable RV : RandomPType;
        variable GOAL_ID, STATUS, SEQ : AlertLogIDType;
        variable sid : std_logic_vector(ROS_SEQUENCE_ID_WIDTH-1 downto 0);
        
        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_result_srv_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);
        STATUS  := GetAlertLogID("STATUS", ALERTLOG_BASE_ID);
        SEQ     := GetAlertLogID("SQ", ALERTLOG_BASE_ID);
        
        Log("Initial Reset", INFO);
        selector            <= '1';
        return_code_cr      <= RETCODE_OK;
        return_code_cw      <= RETCODE_OK;
        return_code_sr      <= RETCODE_OK;
        return_code_sw      <= RETCODE_OK;
        sample_info_cr.valid_data   <= '0';
        sample_info_sr.valid_data   <= '0';
        start_c <= '0';
        start_s <= '0';
        reset   <= '1';
        wait until rising_edge(clk);
        wait until rising_edge(clk);
        reset   <= '0';
        
        Log("CLIENT: Test Unsupported Opcode", INFO);
        start_c     <= '1';
        opcode_c    <= TAKE_REQUEST;
        wait_on_sig(ack_c);
        wait until rising_edge(clk);
        start_c     <= '0';
        wait_on_sig(done_c);
        wait for 1 ps; -- Make sure all signals stable
        AlertIf(return_code_c /= ROS_RET_UNSUPPORTED, "Unexpected Client Response", FAILURE);
        wait until rising_edge(clk);
        
        Log("SERVER: Test Unsupported Opcode", INFO);
        start_s     <= '1';
        opcode_s    <= TAKE_RESPONSE;
        wait_on_sig(ack_s);
        wait until rising_edge(clk);
        start_s     <= '0';
        wait_on_sig(done_s);
        wait for 1 ps; -- Make sure all signals stable
        AlertIf(return_code_s /= ROS_RET_UNSUPPORTED, "Unexpected Server Response", FAILURE);
        wait until rising_edge(clk);
        
        Log("CLIENT: Test No Data", INFO);
        return_code_cr  <= RETCODE_NO_DATA;
        start_c     <= '1';
        opcode_c    <= TAKE_RESPONSE;
        wait_on_sig(ack_c);
        wait until rising_edge(clk);
        start_c     <= '0';
        wait_on_sig(done_c);
        wait for 1 ps; -- Make sure all signals stable
        AlertIf(return_code_c /= ROS_RET_OK, "Unexpected Client Response", FAILURE);
        AlertIf(taken_c /= '0', "Client taken is unexpectedly set", FAILURE);
        wait until rising_edge(clk);
        
        Log("SERVER: Test No Data", INFO);
        return_code_sr  <= RETCODE_NO_DATA;
        start_s     <= '1';
        opcode_s    <= TAKE_REQUEST;
        wait_on_sig(ack_s);
        wait until rising_edge(clk);
        start_s     <= '0';
        wait_on_sig(done_s);
        wait for 1 ps; -- Make sure all signals stable
        AlertIf(return_code_s /= ROS_RET_OK, "Unexpected Server Response", FAILURE);
        AlertIf(taken_s /= '0', "Server taken is unexpectedly set", FAILURE);
        wait until rising_edge(clk);
        
        Log("CLIENT: Test Reader Error", INFO);
        return_code_cr  <= RETCODE_ERROR;
        start_c     <= '1';
        opcode_c    <= TAKE_RESPONSE;
        wait_on_sig(ack_c);
        wait until rising_edge(clk);
        start_c     <= '0';
        wait_on_sig(done_c);
        wait for 1 ps; -- Make sure all signals stable
        AlertIf(return_code_c /= ROS_RET_ERROR, "Unexpected Client Response", FAILURE);
        wait until rising_edge(clk);
        
        Log("SERVER: Test Reader Error", INFO);
        return_code_sr  <= RETCODE_ERROR;
        start_s     <= '1';
        opcode_s    <= TAKE_REQUEST;
        wait_on_sig(ack_s);
        wait until rising_edge(clk);
        start_s     <= '0';
        wait_on_sig(done_s);
        wait for 1 ps; -- Make sure all signals stable
        AlertIf(return_code_s /= ROS_RET_ERROR, "Unexpected Server Response", FAILURE);
        wait until rising_edge(clk);
        
        Log("CLIENT: Test Writer Error", INFO);
        return_code_cw  <= RETCODE_ERROR;
        start_c     <= '1';
        opcode_c    <= SEND_REQUEST;
        wait_on_sig(ack_c);
        wait until rising_edge(clk);
        start_c     <= '0';
        wait_on_sig(done_c);
        wait for 1 ps; -- Make sure all signals stable
        AlertIf(return_code_c /= ROS_RET_ERROR, "Unexpected Client Response", FAILURE);
        wait until rising_edge(clk);
        
        Log("SERVER: Test Writer Error", INFO);
        return_code_sw  <= RETCODE_ERROR;
        start_s     <= '1';
        opcode_s    <= SEND_RESPONSE;
        wait_on_sig(ack_s);
        wait until rising_edge(clk);
        start_s     <= '0';
        wait_on_sig(done_s);
        wait for 1 ps; -- Make sure all signals stable
        AlertIf(return_code_s /= ROS_RET_ERROR, "Unexpected Server Response", FAILURE);
        wait until rising_edge(clk);
        
        return_code_cr  <= RETCODE_OK;
        return_code_cw  <= RETCODE_OK;
        return_code_sr  <= RETCODE_OK;
        return_code_sw  <= RETCODE_OK;
        selector        <= '0';
        
        Log("Setting Request", INFO);
        -- Static
        goal_id_c   <= RV.RandSlv(goal_id_c'length);
        wait for 0 ns;
        
        Log("CLIENT: Send Request", INFO);
        start_c     <= '1';
        opcode_c    <= SEND_REQUEST;
        wait_on_sig(ack_c);
        wait until rising_edge(clk);
        start_c     <= '0';
        
        Log("SERVER: Take Request", INFO);
        start_s     <= '1';
        opcode_s    <= TAKE_REQUEST;
        wait_on_sig(ack_s);
        wait until rising_edge(clk);
        start_s     <= '0';
        
        -- TEST NO VALID DATA
        wait_on_sig(start_sr);
        wait until rising_edge(clk);
        wait until rising_edge(clk);
        wait_on_sig(start_sr);
        sample_info_sr.valid_data   <= '1';
        
        Log("Wait for Request on Server", INFO);
        wait_on_sig(done_c);
        sid := sequence_id_c;
        wait_on_sig(done_s);
        wait for 1 ps; -- Make sure all signals stable
        AlertIf(return_code_s /= ROS_RET_OK, "Server did Return ERROR", FAILURE);
        AlertIf(taken_s /= '1', "Server did not take Request", FAILURE);
        AlertIf(to_unsigned(SEQUENCENUMBER_TYPE(service_info_s.request_id.sequence_number)) /= unsigned(sid), "Request ID incorrect", FAILURE);
        
        Log("Compare Request", INFO);
        AffirmIfEqual(GOAL_ID, goal_id_s, goal_id_c);
        
        Log("Setting Response", INFO);
        status_s    <= RV.RandSlv(status_s'length);
        for i in 0 to 9 loop
            seq_len_s   <= int(10,seq_len_s'length);
            seq_addr_s  <= int(i,seq_addr_s'length);
            seq_w_s     <= RV.RandSlv(seq_w_s'length);
            wait_on_sig(seq_ready_s);
            seq_wen_s   <= '1';
            wait for TEST_CLOCK_PERIOD;
            seq_wen_s   <= '0';
        end loop;
        
        Log("SERVER: Send Response", INFO);
        start_s     <= '1';
        opcode_s    <= SEND_RESPONSE;
        request_id_s.sequence_number    <= to_double_word(unsigned(sid));
        wait_on_sig(ack_s);
        wait until rising_edge(clk);
        start_s     <= '0';
        
        Log("CLIENT: Take Response", INFO);
        start_c     <= '1';
        opcode_c    <= TAKE_RESPONSE;
        wait_on_sig(ack_c);
        wait until rising_edge(clk);
        start_c     <= '0';
        
        -- TEST NO VALID DATA
        wait_on_sig(start_cr);
        wait until rising_edge(clk);
        wait until rising_edge(clk);
        wait_on_sig(start_cr);
        sample_info_cr.valid_data   <= '1';
        
        Log("Wait for Response on Client", INFO);
        wait_on_sig(done_s);
        wait_on_sig(done_c);
        wait for 1 ps; -- Make sure all signals stable
        AlertIf(return_code_c /= ROS_RET_OK, "Client did Return ERROR", FAILURE);
        AlertIf(taken_c /= '1', "Client did not take Response", FAILURE);
        AlertIf(to_unsigned(SEQUENCENUMBER_TYPE(service_info_c.request_id.sequence_number)) /= unsigned(sid), "Request ID incorrect", FAILURE);
        
        Log("Compare Response", INFO);
        AffirmIfEqual(STATUS, status_c, status_s);
        AffirmIfEqual(SEQ, seq_len_c, seq_len_s);
        for i in 0 to to_integer(unsigned(seq_len_s))-1 loop
            seq_addr_s  <= int(i,seq_addr_s'length);
            seq_addr_c  <= int(i,seq_addr_c'length);
            wait_on_sig(seq_ready_s);
            wait_on_sig(seq_ready_c);
            seq_ren_s   <= '1';
            seq_ren_c   <= '1';
            wait for TEST_CLOCK_PERIOD;
            seq_ren_s   <= '0';
            seq_ren_c   <= '0';
            wait_on_sig(seq_valid_s);
            wait_on_sig(seq_valid_c);
            AffirmIfEqual(SEQ, seq_c, seq_r_s);
            seq_ack_s   <= '1';
            seq_ack_c   <= '1';
            wait for TEST_CLOCK_PERIOD;
            seq_ack_s   <= '0';
            seq_ack_c   <= '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;