library ieee;
use ieee.std_logic_1164.all;
use ieee.numeric_std.all;

library osvvm; -- Utility Library
context osvvm.OsvvmContext;

library Testbench_ROS_Lib2;
library Testbench_ROS_Lib3;

use work.rtps_package.all;
use work.ros_package.all;
use work.rtps_test_package.all;

-- This testbench test the general the general ROS Service operation by interconnecting a system with a service client, with a system with a service server.
-- Libraries are used to allow to use systems with different configurations. Testbench_ROS_Lib2 contains the ROS service server, and Testbench_ROS_Lib3 
-- contains the ROS service client.
-- The ROS Service is the AddTwoInts service used in the example tutorials, with sends a request with 2 64-bit integers, and gets a response of their 64-bit
-- summation.


entity L2_AddTwoInts_ros_srv_test1 is
end entity;

architecture testbench of L2_AddTwoInts_ros_srv_test1 is
    
    signal clk, reset : std_logic := '0';
    signal time : TIME_TYPE := TIME_ZERO;
    signal empty_s, empty_c, read_s, read_c, full_s, full_c, write_s, write_c : std_logic := '0';
    signal data_in_s, data_in_c, data_out_s, data_out_c : std_logic_vector(WORD_WIDTH-1 downto 0) := (others => '0');
    
    signal start_c, ack_c, taken_c, done_c, data_available_c : std_logic := '0';
    signal opcode_c : ROS_SERVICE_OPCODE_TYPE := NOP;
    signal service_info_c : Testbench_ROS_Lib3.ros_package.SERVICE_INFO_TYPE := Testbench_ROS_Lib3.ros_package.EMPTY_SERVICE_INFO;
    signal sequence_id_c : std_logic_vector(ROS_SEQUENCE_ID_WIDTH-1 downto 0) := (others => '0');
    signal a_c, b_c, sum_c : std_logic_vector(CDR_LONG_LONG_WIDTH-1 downto 0) := (others => '0');
    signal return_code_c : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := ROS_RET_ERROR;
    
    -- HACK:    For some reason (possibly Modelsim/Questasim Bug) I cannot declare the opcode signals from their respective Library sources, 
    --          because I cannot use a qualified expression to define an enumeration literal (and direct type casting cannot be done because 
    --          the types are not closely related). So I have to do define explicit type conversions between the types...
    
    function to_Lib3(input : ROS_SERVICE_OPCODE_TYPE) return Testbench_ROS_Lib3.ros_package.ROS_SERVICE_OPCODE_TYPE is
    begin
        return Testbench_ROS_Lib3.ros_package.ROS_SERVICE_OPCODE_TYPE'VAL(ROS_SERVICE_OPCODE_TYPE'POS(input));
    end function;
    
begin
    
    server_inst : entity Testbench_ROS_Lib2.L2_Testbench_ROS_Lib2(arch)
        port map (
            -- SYSTEM
            clk             => clk,
            reset           => reset,
            time            => Testbench_ROS_Lib2.rtps_package.TIME_TYPE(time),
            -- INPUT
            empty           => empty_s,
            read            => read_s,
            data_in         => data_in_s,
            -- OUTPUT
            full            => full_s,
            write           => write_s,
            data_out        => data_out_s
        );
    
    fifo_s_c_inst : configuration Testbench_ROS_Lib2.FWFT_FIFO_cfg
        generic map (
            FIFO_DEPTH  => 65536/(WORD_WIDTH/BYTE_WIDTH),
            DATA_WIDTH  => WORD_WIDTH
        )
        port map
        (
            reset       => reset,
            clk         => clk,
            data_in     => data_out_s,
            write       => write_s,
            read        => read_c,
            data_out    => data_in_c,
            empty       => empty_c,
            full        => full_s,
            free        => open
        );
    
    fifo_c_s_inst : configuration Testbench_ROS_Lib2.FWFT_FIFO_cfg
        generic map (
            FIFO_DEPTH  => 65536/(WORD_WIDTH/BYTE_WIDTH),
            DATA_WIDTH  => WORD_WIDTH
        )
        port map
        (
            reset       => reset,
            clk         => clk,
            data_in     => data_out_c,
            write       => write_c,
            read        => read_s,
            data_out    => data_in_s,
            empty       => empty_s,
            full        => full_c,
            free        => open
        );
    
    client_inst : entity Testbench_ROS_Lib3.L2_Testbench_ROS_Lib3(arch)
        port map (
            -- SYSTEM
            clk                 => clk,
            reset               => reset,
            time                => Testbench_ROS_Lib3.rtps_package.TIME_TYPE(time),
            -- INPUT
            empty               => empty_c,
            read                => read_c,
            data_in             => data_in_c,
            -- OUTPUT
            full                => full_c,
            write               => write_c,
            data_out            => data_out_c,
            -- SERVICE CLIENT
            start_c             => start_c,
            ack_c               => ack_c,
            opcode_c            => to_Lib3(opcode_c),
            service_info_c      => service_info_c,
            sequence_id_c       => sequence_id_c,
            taken_c             => taken_c,
            data_available_c    => data_available_c,
            a_c                 => a_c,
            b_c                 => b_c,
            sum_c               => sum_c,
            done_c              => done_c,
            return_code_c       => return_code_c
        );
    
    stimulus_prc : process
        variable RV : RandomPType;
        variable ref : std_logic_vector(CDR_LONG_LONG_WIDTH-1 downto 0);
        variable sid : std_logic_vector(ROS_SEQUENCE_ID_WIDTH-1 downto 0);
        
        -- Waits until signal toggles high for at least 1 ps
        procedure wait_on_sig(signal sig : std_logic) is
        begin
            loop
                if (sig /= '1') then
                    wait on sig until sig = '1';
                end if;
                -- Prevent triggering on glitches
                wait for 1 ps;
                if (sig = '1') then
                    exit;
                end if;
            end loop;
        end procedure;
    begin
        
        SetAlertLogName("L2_AddTwoInts_ros_srv_test1 - General");
        SetAlertEnable(FAILURE, TRUE);
        SetAlertEnable(ERROR, TRUE);
        SetAlertEnable(WARNING, TRUE);
        SetLogEnable(DEBUG, FALSE);
        SetLogEnable(PASSED, FALSE);
        SetLogEnable(INFO, TRUE);
        RV.InitSeed(RV'instance_name);
        
        Log("Initial Reset", INFO);
        reset   <= '1';
        wait until rising_edge(clk);
        wait until rising_edge(clk);
        reset   <= '0';
        
        Log("Sent Request", INFO);
        start_c     <= '1';
        opcode_c    <= SEND_REQUEST;
        a_c         <= std_logic_vector(resize(unsigned(RV.RandSlv(CDR_LONG_WIDTH)),a_c'length));
        b_c         <= std_logic_vector(resize(unsigned(RV.RandSlv(CDR_LONG_WIDTH)),a_c'length));
        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);
        sid := sequence_id_c;
        
        Log("Wait for Response", INFO);
        wait_on_sig(data_available_c);
        
        Log("Take & compare Response", INFO);
        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);
        AffirmIfEqual(unsigned(sum_c), unsigned(a_c) + unsigned(b_c));
        AffirmIfEqual(to_unsigned(SEQUENCENUMBER_TYPE(service_info_c.request_id.sequence_number)), unsigned(sid));
        
        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;
    
    time_prc : process(clk)
    begin
        if rising_edge(clk) then
            if (reset = '1') then
                time    <= TIME_ZERO;
            else
                time    <= time + gen_duration(TEST_CLOCK_PERIOD);
            end if;
        end if;
    end process;
    
    watchdog : process
    begin
        wait for 1 ms;
        Alert("Test timeout", FAILURE);
        std.env.stop;
    end process;
    
end architecture;