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

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

library Testbench_ROS_Lib4;
library Testbench_ROS_Lib5;

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

-- This testbench test the general the general ROS Action operation by interconnecting a system with a action client, with a system with a action server.
-- Libraries are used to allow to use systems with different configurations. Testbench_ROS_Lib4 contains the ROS service server, and Testbench_ROS_Lib5 
-- contains the ROS service client.
-- The ROS Service is the Fibonacci service used in the example tutorials, which sends a a goal request of order 10, and gets the fibonacci sequence as
-- result.


entity L2_Fibonacci_ros_action_test1 is
end entity;

architecture testbench of L2_Fibonacci_ros_action_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, done_c, data_available_c_c, data_available_g_c, data_available_r_c, data_available_f_c : std_logic := '0';
    signal opcode_c : ROS_ACTION_OPCODE_TYPE := NOP;
    signal sequence_id_c : std_logic_vector(ROS_SEQUENCE_ID_WIDTH-1 downto 0) := (others => '0');
    signal service_info_c : Testbench_ROS_Lib5.ros_package.SERVICE_INFO_TYPE := Testbench_ROS_Lib5.ros_package.EMPTY_SERVICE_INFO;
    signal return_code_c : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := (others => '0');
    signal goal_id_c, result_goal_id_c, cancel_goal_id_c, cancel_goals_canceling_goal_id_c, feedback_goal_id_c : std_logic_vector(UUID_WIDTH-1 downto 0) := (others => '0');
    signal goal_order_c, result_seq_c, feedback_seq_c : std_logic_vector(CDR_LONG_WIDTH-1 downto 0) := (others => '0');
    signal goal_accepted_c, result_seq_ready_c, result_seq_ren_c, result_seq_valid_c, result_seq_ack_c : std_logic := '0';
    signal goal_stamp_c, cancel_stamp_c : Testbench_ROS_Lib5.ros_package.ROS_TIME_TYPE := Testbench_ROS_Lib5.ros_package.ROS_TIME_ZERO;
    signal result_status_c, cancel_return_code_c : std_logic_vector(CDR_INT8_WIDTH-1 downto 0) := (others => '0');
    signal result_seq_len_c, result_seq_addr_c : std_logic_vector(Testbench_ROS_Lib5.Fibonacci_package.R_RR_SEQ_ADDR_WIDTH-1 downto 0) := (others => '0');
    signal cancel_goals_canceling_len_c, cancel_goals_canceling_addr_c : std_logic_vector(Testbench_ROS_Lib5.CancelGoal_package.RR_GOALS_CANCELING_ADDR_WIDTH-1 downto 0) := (others => '0');
    signal cancel_goals_canceling_ready_c, cancel_goals_canceling_ren_c, cancel_goals_canceling_valid_c, cancel_goals_canceling_ack_c : std_logic := '0';
    signal cancel_goals_canceling_stamp_c : std_logic_vector(ROS_TIME_WIDTH-1 downto 0) := (others => '0');
    signal feedback_seq_len_c, feedback_seq_addr_c : std_logic_vector(Testbench_ROS_Lib5.Fibonacci_package.F_SEQ_ADDR_WIDTH-1 downto 0) := (others => '0');
    signal feedback_seq_ready_c, feedback_seq_ren_c, feedback_seq_valid_c, feedback_seq_ack_c : std_logic := '0';
    
    -- 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_Lib5(input : ROS_ACTION_OPCODE_TYPE) return Testbench_ROS_Lib5.ros_package.ROS_ACTION_OPCODE_TYPE is
    begin
        return Testbench_ROS_Lib5.ros_package.ROS_ACTION_OPCODE_TYPE'VAL(ROS_ACTION_OPCODE_TYPE'POS(input));
    end function;
    
begin
    
    server_inst : entity Testbench_ROS_Lib4.L2_Testbench_ROS_Lib4(arch)
        port map (
            -- SYSTEM
            clk             => clk,
            reset           => reset,
            time            => Testbench_ROS_Lib4.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_Lib4.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_Lib4.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_Lib5.L2_Testbench_ROS_Lib5(arch)
        port map (
            -- SYSTEM
            clk                                 => clk,
            reset                               => reset,
            time                                => Testbench_ROS_Lib5.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,
            opcode_c                            => to_Lib5(opcode_c),
            ack_c                               => ack_c,
            sequence_id_c                       => sequence_id_c,
            service_info_c                      => service_info_c,
            data_available_g_c                  => data_available_g_c,
            data_available_r_c                  => data_available_r_c,
            data_available_c_c                  => data_available_c_c,
            data_available_f_c                  => data_available_f_c,
            done_c                              => done_c,
            return_code_c                       => return_code_c,
            goal_id_c                           => goal_id_c,
            goal_order_c                        => goal_order_c,
            goal_accepted_c                     => goal_accepted_c,
            goal_stamp_c                        => goal_stamp_c,
            result_goal_id_c                    => result_goal_id_c,
            result_status_c                     => result_status_c,
            result_seq_len_c                    => result_seq_len_c,
            result_seq_addr_c                   => result_seq_addr_c,
            result_seq_ready_c                  => result_seq_ready_c,
            result_seq_ren_c                    => result_seq_ren_c,
            result_seq_valid_c                  => result_seq_valid_c,
            result_seq_ack_c                    => result_seq_ack_c,
            result_seq_c                        => result_seq_c,
            cancel_goal_id_c                    => cancel_goal_id_c,
            cancel_stamp_c                      => cancel_stamp_c,
            cancel_return_code_c                => cancel_return_code_c,
            cancel_goals_canceling_len_c        => cancel_goals_canceling_len_c,
            cancel_goals_canceling_addr_c       => cancel_goals_canceling_addr_c,
            cancel_goals_canceling_ready_c      => cancel_goals_canceling_ready_c,
            cancel_goals_canceling_ren_c        => cancel_goals_canceling_ren_c,
            cancel_goals_canceling_valid_c      => cancel_goals_canceling_valid_c,
            cancel_goals_canceling_ack_c        => cancel_goals_canceling_ack_c,
            cancel_goals_canceling_goal_id_c    => cancel_goals_canceling_goal_id_c,
            cancel_goals_canceling_stamp_c      => cancel_goals_canceling_stamp_c,
            feedback_goal_id_c                  => feedback_goal_id_c,
            feedback_seq_len_c                  => feedback_seq_len_c,
            feedback_seq_addr_c                 => feedback_seq_addr_c,
            feedback_seq_ready_c                => feedback_seq_ready_c,
            feedback_seq_ren_c                  => feedback_seq_ren_c,
            feedback_seq_valid_c                => feedback_seq_valid_c,
            feedback_seq_ack_c                  => feedback_seq_ack_c,
            feedback_seq_c                      => feedback_seq_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);
        variable a, b, c : natural;
        
        -- 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;
        
        impure function gen_goal_id return std_logic_vector is
        begin
            return RV.RandSlv(UUID_WIDTH);
        end function;
        
        procedure client_op(op : in ROS_ACTION_OPCODE_TYPE; ret : in std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0)) is
        begin
            start_c     <= '1';
            opcode_c    <= op;
            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 /= ret, "Unexpected Client Response", FAILURE);
        end procedure;
    begin
        
        SetAlertLogName("L2_Fibonacci_ros_action_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 Goal Request (Goal 1, Order 10)", INFO);
        goal_id_c       <= gen_goal_id;
        goal_order_c    <= int(10,goal_order_c'length);
        client_op(SEND_GOAL_REQUEST,ROS_RET_OK);
        sid := sequence_id_c;
        wait until rising_edge(clk);
        
        wait_on_sig(data_available_g_c);
        
        Log("Take Goal Response", INFO);
        client_op(TAKE_GOAL_RESPONSE,ROS_RET_OK);
        AffirmIfEqual(to_unsigned(SEQUENCENUMBER_TYPE(service_info_c.request_id.sequence_number)), unsigned(sid));
        AffirmIfEqual(goal_accepted_c, '1');
        wait until rising_edge(clk);
        
        Log("Sent Result Request (Goal 1)", INFO);
        result_goal_id_c    <= goal_id_c;
        client_op(SEND_RESULT_REQUEST,ROS_RET_OK);
        sid := sequence_id_c;
        wait until rising_edge(clk);
        
        wait_on_sig(data_available_r_c);
        
        Log("Take Result Response", INFO);
        client_op(TAKE_RESULT_RESPONSE,ROS_RET_OK);
        AffirmIfEqual(to_unsigned(SEQUENCENUMBER_TYPE(service_info_c.request_id.sequence_number)), unsigned(sid));
        AffirmIfEqual(result_status_c, Testbench_ROS_Lib5.GoalStatus_package.STATUS_SUCCEEDED);
        AffirmIfEqual(result_seq_len_c, int(11,result_seq_len_c'length));
        a   := 0;
        b   := 1;
        c   := a + b;
        for i in 0 to to_integer(unsigned(result_seq_len_c))-1 loop
            result_seq_addr_c   <= int(i,result_seq_addr_c'length);
            wait_on_sig(result_seq_ready_c);
            result_seq_ren_c    <= '1';
            wait until rising_edge(clk);
            result_seq_ren_c    <= '0';
            wait_on_sig(result_seq_valid_c);
            AffirmIfEqual(result_seq_c, int(a,result_seq_c'length));
            a   := b;
            b   := c;
            c   := a + b;
            result_seq_ack_c    <= '1';
            wait until rising_edge(clk);
            result_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;
    
    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;