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;
use work.GoalStatus_package;

-- This testbench tests the General Behaviour of ROS Action.
-- More specifically following tests are done:
--  * Test Unsuported Opcode Operations
--  * Test Take Failure on Client TAKE_* Operations
--  * Test multiple concurrent Goals
--  * Test Encoding/Decoding of Action Messages

entity L1_Fibonacci_ros_action_test1 is
end entity;

architecture testbench of L1_Fibonacci_ros_action_test1 is
    
    constant MAX_GOALS : natural := 2;
    
    type GOAL_ID_ARRAY_TYPE is array (0 to MAX_GOALS-1) of std_logic_vector(UUID_WIDTH-1 downto 0);
    type GOAL_HANDLE_ARRAY_TYPE is array (0 to MAX_GOALS-1) of std_logic_vector(GOAL_HANDLE_WIDTH-1 downto 0);
    type RESULT_INDEX_ARRAY_TYPE is array (0 to MAX_GOALS-1) of natural;
    type SEQUENCE_ID_ARRAY_TYPE is array (0 to MAX_GOALS-1) of std_logic_vector(ROS_SEQUENCE_ID_WIDTH-1 downto 0);
    
    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                  => '1'
    );
    
    signal clk, reset : std_logic := '0';
    signal start_g, start_r : std_logic := '0';
    signal da_g, da_r : std_logic_vector(STATUS_KIND_WIDTH-1 downto 0) := (others => '0');
    signal ret_sg, ret_sr, ret_cg, ret_cr : std_logic_vector(RETURN_CODE_WIDTH-1 downto 0) := RETCODE_OK;
    signal valid_gr, valid_gw, valid_rr, valid_rw, valid_cr, valid_cw, valid_f : std_logic := '0';
    signal ready_gr, ready_gw, ready_rr, ready_rw, ready_cr, ready_cw, ready_f : std_logic := '0';
    signal last_word_gr, last_word_gw, last_word_rr, last_word_rw, last_word_cr, last_word_cw, last_word_f : std_logic := '0';
    signal data_gr, data_gw, data_rr, data_rw, data_cr, data_cw, data_f : std_logic_vector(WORD_WIDTH-1 downto 0) := (others => '0');
    
    signal start_c, start_s, ack_c, ack_s, done_c, done_s, data_available_c_c, data_available_g_c, data_available_r_c, data_available_f_c : std_logic := '0';
    signal opcode_c, opcode_s : ROS_ACTION_OPCODE_TYPE := NOP;
    signal return_code_c, return_code_s : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := (others => '0');
    -- ###GENERATED START###
    signal goal_order_c, new_goal_order_s : std_logic_vector(CDR_LONG_WIDTH-1 downto 0) := (others => '0');
    signal result_seq_len_c, result_seq_len_s, result_seq_addr_c, result_seq_addr_s : std_logic_vector(R_RR_SEQ_ADDR_WIDTH-1 downto 0) := (others => '0');
    signal result_seq_ready_c, result_seq_ready_s, result_seq_ren_c, result_seq_ren_s, result_seq_wen_s, result_seq_valid_c, result_seq_valid_s, result_seq_ack_c, result_seq_ack_s : std_logic := '0';
    signal result_seq_c, result_seq_r_s, result_seq_w_s : std_logic_vector(CDR_LONG_WIDTH-1 downto 0) := (others => '0');
    signal feedback_seq_len_c, feedback_seq_len_s, feedback_seq_addr_c, feedback_seq_addr_s : std_logic_vector(F_SEQ_ADDR_WIDTH-1 downto 0) := (others => '0');
    signal feedback_seq_ready_c, feedback_seq_ready_s, feedback_seq_ren_c, feedback_seq_ren_s, feedback_seq_wen_s, feedback_seq_valid_c, feedback_seq_valid_s, feedback_seq_ack_c, feedback_seq_ack_s : std_logic := '0';
    signal feedback_seq_c, feedback_seq_r_s, feedback_seq_w_s : std_logic_vector(CDR_LONG_WIDTH-1 downto 0) := (others => '0');
    -- ###GENERATED END###        
    signal goal_id_c, goal_id_s, result_goal_id_c, feedback_goal_id_c : std_logic_vector(UUID_WIDTH-1 downto 0) := (others => '0');
    signal goal_accepted_c, new_goal_accepted_s : std_logic := '0';
    signal goal_stamp_c, goal_stamp_s : ROS_TIME_TYPE := ROS_TIME_ZERO;
    signal result_status_c : std_logic_vector(CDR_INT8_WIDTH-1 downto 0) := (others => '0');
    signal service_info_c : SERVICE_INFO_TYPE := EMPTY_SERVICE_INFO;
    signal sequence_id_c : std_logic_vector(ROS_SEQUENCE_ID_WIDTH-1 downto 0) := (others => '0');
    signal goal_handle_in_s, goal_handle_out_s, new_goal_handle_s : std_logic_vector(GOAL_HANDLE_WIDTH-1 downto 0) := (others => '0');
    signal goal_state_in_s, goal_state_out_s : std_logic_vector(CDR_INT8_WIDTH-1 downto 0) := (others => '0');
    signal goal_result_index_s, new_goal_result_index_s, index_result_s : std_logic_vector(WORD_WIDTH-1 downto 0) := (others => '0');
    signal new_goal_request_s, new_goal_response_s, index_result_wen_s, index_result_ready_s : std_logic := '0';
begin
    
    uut_c : entity work.Fibonacci_ros_action_client(arch)
        port map (
            clk                         => clk,
            reset                       => reset,
            time                        => ROS_TIME_ZERO,
            g_start_r                   => open,
            g_ack_r                     => '1',
            g_opcode_r                  => open,
            g_instance_state_r          => open,
            g_view_state_r              => open,
            g_sample_state_r            => open,
            g_instance_handle_r         => open,
            g_max_samples_r             => open,
            g_get_data_r                => open,
            g_done_r                    => '1',
            g_return_code_r             => ret_cg,
            g_valid_in_r                => valid_gr,
            g_ready_in_r                => ready_gr,
            g_data_in_r                 => data_gr,
            g_last_word_in_r            => last_word_gr,
            g_sample_info_r             => DEFAULT_SAMPLE_INFO,
            g_sample_info_valid_r       => '1',
            g_sample_info_ack_r         => open,
            g_eoc_r                     => '1',
            g_status_r                  => (others => '0'),
            g_start_w                   => open,
            g_ack_w                     => '1',
            g_opcode_w                  => open,
            g_instance_handle_out_w     => HANDLE_NIL,
            g_source_ts_w               => open,
            g_max_wait_w                => open,
            g_done_w                    => '1',
            g_return_code_w             => RETCODE_OK,
            g_instance_handle_in_w      => open,
            g_valid_out_w               => valid_gw,
            g_ready_out_w               => ready_gw,
            g_data_out_w                => data_gw,
            g_last_word_out_w           => last_word_gw,
            g_valid_in_w                => '0',
            g_ready_in_w                => open,
            g_data_in_w                 => (others => '0'),
            g_last_word_in_w            => '0',
            g_status_w                  => (others => '0'),
            r_start_r                   => open,
            r_ack_r                     => '1',
            r_opcode_r                  => open,
            r_instance_state_r          => open,
            r_view_state_r              => open,
            r_sample_state_r            => open,
            r_instance_handle_r         => open,
            r_max_samples_r             => open,
            r_get_data_r                => open,
            r_done_r                    => '1',
            r_return_code_r             => ret_cr,
            r_valid_in_r                => valid_rr,
            r_ready_in_r                => ready_rr,
            r_data_in_r                 => data_rr,
            r_last_word_in_r            => last_word_rr,
            r_sample_info_r             => DEFAULT_SAMPLE_INFO,
            r_sample_info_valid_r       => '1',
            r_sample_info_ack_r         => open,
            r_eoc_r                     => '1',
            r_status_r                  => (others => '0'),
            r_start_w                   => open,
            r_ack_w                     => '1',
            r_opcode_w                  => open,
            r_instance_handle_out_w     => HANDLE_NIL,
            r_source_ts_w               => open,
            r_max_wait_w                => open,
            r_done_w                    => '1',
            r_return_code_w             => RETCODE_OK,
            r_instance_handle_in_w      => open,
            r_valid_out_w               => valid_rw,
            r_ready_out_w               => ready_rw,
            r_data_out_w                => data_rw,
            r_last_word_out_w           => last_word_rw,
            r_valid_in_w                => '0',
            r_ready_in_w                => open,
            r_data_in_w                 => (others => '0'),
            r_last_word_in_w            => '0',
            r_status_w                  => (others => '0'),
            c_start_r                   => open,
            c_ack_r                     => '1',
            c_opcode_r                  => open,
            c_instance_state_r          => open,
            c_view_state_r              => open,
            c_sample_state_r            => open,
            c_instance_handle_r         => open,
            c_max_samples_r             => open,
            c_get_data_r                => open,
            c_done_r                    => '1',
            c_return_code_r             => RETCODE_NO_DATA,
            c_valid_in_r                => valid_cr,
            c_ready_in_r                => ready_cr,
            c_data_in_r                 => data_cr,
            c_last_word_in_r            => last_word_cr,
            c_sample_info_r             => DEFAULT_SAMPLE_INFO,
            c_sample_info_valid_r       => '1',
            c_sample_info_ack_r         => open,
            c_eoc_r                     => '1',
            c_status_r                  => (others => '0'),
            c_start_w                   => open,
            c_ack_w                     => '1',
            c_opcode_w                  => open,
            c_instance_handle_out_w     => HANDLE_NIL,
            c_source_ts_w               => open,
            c_max_wait_w                => open,
            c_done_w                    => '1',
            c_return_code_w             => RETCODE_OK,
            c_instance_handle_in_w      => open,
            c_valid_out_w               => valid_cw,
            c_ready_out_w               => ready_cw,
            c_data_out_w                => data_cw,
            c_last_word_out_w           => last_word_cw,
            c_valid_in_w                => '0',
            c_ready_in_w                => open,
            c_data_in_w                 => (others => '0'),
            c_last_word_in_w            => '0',
            c_status_w                  => (others => '0'),
            f_start_dds                 => open,
            f_ack_dds                   => '1',
            f_opcode_dds                => open,
            f_instance_state_dds        => open,
            f_view_state_dds            => open,
            f_sample_state_dds          => open,
            f_instance_handle_dds       => open,
            f_max_samples_dds           => open,
            f_get_data_dds              => open,
            f_done_dds                  => '1',
            f_return_code_dds           => RETCODE_OK,
            f_valid_in_dds              => valid_f,
            f_ready_in_dds              => ready_f,
            f_data_in_dds               => data_f,
            f_last_word_in_dds          => last_word_f,
            f_sample_info_dds           => DEFAULT_SAMPLE_INFO,
            f_sample_info_valid_dds     => '1',
            f_sample_info_ack_dds       => open,
            f_eoc_dds                   => '1',
            f_status_dds                => (others => '0'),
            start                       => start_c,
            opcode                      => opcode_c,
            ack                         => ack_c,
            sequence_id                 => sequence_id_c,
            service_info                => service_info_c,
            data_available_g            => data_available_g_c,
            data_available_r            => data_available_r_c,
            data_available_c            => data_available_c_c,
            data_available_f            => data_available_f_c,
            done                        => done_c,
            return_code                 => return_code_c,
            goal_id                     => goal_id_c,
            -- ###GENERATED START###
            goal_order  => goal_order_c,
            -- ###GENERATED END###
            goal_accepted               => goal_accepted_c,
            goal_stamp                  => goal_stamp_c,
            result_goal_id              => result_goal_id_c,
            result_status               => result_status_c,
            -- ###GENERATED START###
            result_seq_len      => result_seq_len_c,
            result_seq_addr     => result_seq_addr_c,
            result_seq_ready    => result_seq_ready_c,
            result_seq_ren      => result_seq_ren_c,
            result_seq_valid    => result_seq_valid_c,
            result_seq_ack      => result_seq_ack_c,
            result_seq          => result_seq_c,
            -- ###GENERATED END###
            cancel_goal_id                  => (others => '0'),
            cancel_stamp                    => ROS_TIME_ZERO,
            cancel_return_code              => open,
            cancel_goals_canceling_len      => open,
            cancel_goals_canceling_addr     => (others => '0'),
            cancel_goals_canceling_ready    => open,
            cancel_goals_canceling_ren      => '0',
            cancel_goals_canceling_valid    => open,
            cancel_goals_canceling_ack      => '0',
            cancel_goals_canceling_goal_id  => open,
            cancel_goals_canceling_stamp    => open,
            -- FEEDBACK SIGNALS
            feedback_goal_id            => feedback_goal_id_c,
            -- ###GENERATED START###
            feedback_seq_len    => feedback_seq_len_c,
            feedback_seq_addr   => feedback_seq_addr_c,
            feedback_seq_ready  => feedback_seq_ready_c,
            feedback_seq_ren    => feedback_seq_ren_c,
            feedback_seq_valid  => feedback_seq_valid_c,
            feedback_seq_ack    => feedback_seq_ack_c,
            feedback_seq        => feedback_seq_c
            -- ###GENERATED END###
        );
    
    uut_s : entity work.Fibonacci_ros_action_server(arch)
        generic map (
            TIMEOUT_DURATION    => ROS_DURATION_INFINITE,
            MAX_GOALS           => MAX_GOALS,
            MAX_RESULT_REQUESTS => MAX_GOALS
        )
        port map (
            clk                         => clk,
            reset                       => reset,
            time                        => ROS_TIME_ZERO,
            g_start_r                   => start_g,
            g_ack_r                     => '1',
            g_opcode_r                  => open,
            g_instance_state_r          => open,
            g_view_state_r              => open,
            g_sample_state_r            => open,
            g_instance_handle_r         => open,
            g_max_samples_r             => open,
            g_get_data_r                => open,
            g_done_r                    => '1',
            g_return_code_r             => ret_sg,
            g_valid_in_r                => valid_gw,
            g_ready_in_r                => ready_gw,
            g_data_in_r                 => data_gw,
            g_last_word_in_r            => last_word_gw,
            g_sample_info_r             => DEFAULT_SAMPLE_INFO,
            g_sample_info_valid_r       => '1',
            g_sample_info_ack_r         => open,
            g_eoc_r                     => '1',
            g_status_r                  => da_g,
            g_start_w                   => open,
            g_ack_w                     => '1',
            g_opcode_w                  => open,
            g_instance_handle_out_w     => HANDLE_NIL,
            g_source_ts_w               => open,
            g_max_wait_w                => open,
            g_done_w                    => '1',
            g_return_code_w             => RETCODE_OK,
            g_instance_handle_in_w      => open,
            g_valid_out_w               => valid_gr,
            g_ready_out_w               => ready_gr,
            g_data_out_w                => data_gr,
            g_last_word_out_w           => last_word_gr,
            g_valid_in_w                => '0',
            g_ready_in_w                => open,
            g_data_in_w                 => (others => '0'),
            g_last_word_in_w            => '0',
            g_status_w                  => (others => '0'),
            r_start_r                   => start_r,
            r_ack_r                     => '1',
            r_opcode_r                  => open,
            r_instance_state_r          => open,
            r_view_state_r              => open,
            r_sample_state_r            => open,
            r_instance_handle_r         => open,
            r_max_samples_r             => open,
            r_get_data_r                => open,
            r_done_r                    => '1',
            r_return_code_r             => ret_sr,
            r_valid_in_r                => valid_rw,
            r_ready_in_r                => ready_rw,
            r_data_in_r                 => data_rw,
            r_last_word_in_r            => last_word_rw,
            r_sample_info_r             => DEFAULT_SAMPLE_INFO,
            r_sample_info_valid_r       => '1',
            r_sample_info_ack_r         => open,
            r_eoc_r                     => '1',
            r_status_r                  => da_r,
            r_start_w                   => open,
            r_ack_w                     => '1',
            r_opcode_w                  => open,
            r_instance_handle_out_w     => HANDLE_NIL,
            r_source_ts_w               => open,
            r_max_wait_w                => open,
            r_done_w                    => '1',
            r_return_code_w             => RETCODE_OK,
            r_instance_handle_in_w      => open,
            r_valid_out_w               => valid_rr,
            r_ready_out_w               => ready_rr,
            r_data_out_w                => data_rr,
            r_last_word_out_w           => last_word_rr,
            r_valid_in_w                => '0',
            r_ready_in_w                => open,
            r_data_in_w                 => (others => '0'),
            r_last_word_in_w            => '0',
            r_status_w                  => (others => '0'),
            c_start_r                   => open,
            c_ack_r                     => '1',
            c_opcode_r                  => open,
            c_instance_state_r          => open,
            c_view_state_r              => open,
            c_sample_state_r            => open,
            c_instance_handle_r         => open,
            c_max_samples_r             => open,
            c_get_data_r                => open,
            c_done_r                    => '1',
            c_return_code_r             => RETCODE_OK,
            c_valid_in_r                => valid_cw,
            c_ready_in_r                => ready_cw,
            c_data_in_r                 => data_cw,
            c_last_word_in_r            => last_word_cw,
            c_sample_info_r             => DEFAULT_SAMPLE_INFO,
            c_sample_info_valid_r       => '1',
            c_sample_info_ack_r         => open,
            c_eoc_r                     => '1',
            c_status_r                  => (others => '0'),
            c_start_w                   => open,
            c_ack_w                     => '1',
            c_opcode_w                  => open,
            c_instance_handle_out_w     => HANDLE_NIL,
            c_source_ts_w               => open,
            c_max_wait_w                => open,
            c_done_w                    => '1',
            c_return_code_w             => RETCODE_OK,
            c_instance_handle_in_w      => open,
            c_valid_out_w               => valid_cr,
            c_ready_out_w               => ready_cr,
            c_data_out_w                => data_cr,
            c_last_word_out_w           => last_word_cr,
            c_valid_in_w                => '0',
            c_ready_in_w                => open,
            c_data_in_w                 => (others => '0'),
            c_last_word_in_w            => '0',
            c_status_w                  => (others => '0'),
            f_start_dds                 => open,
            f_ack_dds                   => '1',
            f_opcode_dds                => open,
            f_instance_handle_out_dds   => HANDLE_NIL,
            f_source_ts_dds             => open,
            f_max_wait_dds              => open,
            f_done_dds                  => '1',
            f_return_code_dds           => RETCODE_OK,
            f_instance_handle_in_dds    => open,
            f_valid_out_dds             => valid_f,
            f_ready_out_dds             => ready_f,
            f_data_out_dds              => data_f,
            f_last_word_out_dds         => last_word_f,
            f_valid_in_dds              => '0',
            f_ready_in_dds              => open,
            f_data_in_dds               => (others => '0'),
            f_last_word_in_dds          => '0',
            f_status_dds                => (others => '0'),
            s_start_dds                 => open,
            s_ack_dds                   => '1',
            s_opcode_dds                => open,
            s_instance_handle_out_dds   => HANDLE_NIL,
            s_source_ts_dds             => open,
            s_max_wait_dds              => open,
            s_done_dds                  => '1',
            s_return_code_dds           => RETCODE_OK,
            s_instance_handle_in_dds    => open,
            s_valid_out_dds             => open,
            s_ready_out_dds             => '1',
            s_data_out_dds              => open,
            s_last_word_out_dds         => open,
            s_valid_in_dds              => '0',
            s_ready_in_dds              => open,
            s_data_in_dds               => (others => '0'),
            s_last_word_in_dds          => '0',
            s_status_dds                => (others => '0'),
            start                       => start_s,
            opcode                      => opcode_s,
            ack                         => ack_s,
            done                        => done_s,
            return_code                 => return_code_s,
            goal_handle_in              => goal_handle_in_s,
            goal_handle_out             => goal_handle_out_s,
            goal_state_in               => goal_state_in_s,
            goal_state_out              => goal_state_out_s,
            goal_id                     => goal_id_s,
            goal_result_index           => goal_result_index_s,
            goal_stamp                  => goal_stamp_s,
            new_goal_request            => new_goal_request_s,
            new_goal_handle             => new_goal_handle_s,
            new_goal_result_index       => new_goal_result_index_s,
            -- ###GENERATED START###
            new_goal_order  => new_goal_order_s,
            -- ###GENERATED END###
            new_goal_accepted           => new_goal_accepted_s,
            new_goal_response           => new_goal_response_s,
            cancel_request              => open,
            cancel_request_handle       => open,
            cancel_accepted             => '1',
            cancel_response             => '1',
            index_result                => index_result_s,
            index_result_wen            => index_result_wen_s,
            index_result_ready          => index_result_ready_s,
            -- ###GENERATED START###
            result_seq_len      => result_seq_len_s,
            result_seq_addr     => result_seq_addr_s,
            result_seq_ready    => result_seq_ready_s,
            result_seq_ren      => result_seq_ren_s,
            result_seq_wen      => result_seq_wen_s,
            result_seq_valid    => result_seq_valid_s,
            result_seq_ack      => result_seq_ack_s,
            result_seq_r        => result_seq_r_s,
            result_seq_w        => result_seq_w_s,
            -- ###GENERATED END###
            -- ###GENERATED START###
            feedback_seq_len    => feedback_seq_len_s,
            feedback_seq_addr   => feedback_seq_addr_s,
            feedback_seq_ready  => feedback_seq_ready_s,
            feedback_seq_ren    => feedback_seq_ren_s,
            feedback_seq_wen    => feedback_seq_wen_s,
            feedback_seq_valid  => feedback_seq_valid_s,
            feedback_seq_ack    => feedback_seq_ack_s,
            feedback_seq_r      => feedback_seq_r_s,
            feedback_seq_w      => feedback_seq_w_s
            -- ###GENERATED END###
        );
    
    stimulus_prc : process
        variable RV : RandomPType;
        variable GOAL, RESULT, FEEDBACK : AlertLogIDType;
        variable sid : SEQUENCE_ID_ARRAY_TYPE := (others => (others => '0'));
        variable test_gid : GOAL_ID_ARRAY_TYPE := (others => (others => '0'));
        variable test_handle : GOAL_HANDLE_ARRAY_TYPE := (others => (others => '0'));
        variable test_ind : RESULT_INDEX_ARRAY_TYPE := (others => 0);
        
        impure function gen_goal_id return std_logic_vector is
        begin
            return RV.RandSlv(UUID_WIDTH);
        end function;
        
        procedure wait_on_sig(signal sig : std_logic) is
        begin
            if (sig /= '1') then
                wait on sig until sig = '1';
            end if;
        end procedure;
        
        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';
            case (op) is
                when SEND_GOAL_REQUEST =>
                    ret_sg  <= RETCODE_OK;
                    da_g    <= DATA_AVAILABLE_STATUS;
                    wait_on_sig(start_g);
                    da_g    <= (others => '0');
                when SEND_RESULT_REQUEST =>
                    ret_sr  <= RETCODE_OK;
                    da_r    <= DATA_AVAILABLE_STATUS;
                    wait_on_sig(start_r);
                    da_r    <= (others => '0');
                when others =>
                    null;
            end case;
            wait_on_sig(done_c);
            case (op) is
                when SEND_GOAL_REQUEST =>
                    ret_sg  <= RETCODE_NO_DATA;
                when SEND_RESULT_REQUEST =>
                    ret_sr  <= RETCODE_NO_DATA;
                when others =>
                    null;
            end case;
            wait for 1 ps; -- Make sure all signals stable
            AlertIf(return_code_c /= ret, "Unexpected Client Response", FAILURE);
        end procedure;
        
        procedure server_op(op : in ROS_ACTION_OPCODE_TYPE; ret : in std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0)) is
        begin
            start_s     <= '1';
            opcode_s    <= op;
            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 /= ret, "Unexpected Client Response", FAILURE);
        end procedure;
    begin
        
        SetAlertLogName("L1_Fibonacci_ros_action_test1 - (With FEEDBACK) - 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        := GetAlertLogID("GOAL", ALERTLOG_BASE_ID);
        RESULT      := GetAlertLogID("RESULT", ALERTLOG_BASE_ID);
        FEEDBACK    := GetAlertLogID("FEEDBACK", ALERTLOG_BASE_ID);
        
        Log("Initial Reset", INFO);
        ret_cg  <= RETCODE_NO_DATA;
        ret_cr  <= RETCODE_NO_DATA;
        ret_sg  <= RETCODE_NO_DATA;
        ret_sr  <= RETCODE_NO_DATA;
        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);
        client_op(UPDATE_GOAL_STATE,ROS_RET_UNSUPPORTED);
        wait until rising_edge(clk);
        
        Log("SERVER: Test Unsupported Opcode", INFO);
        server_op(SEND_GOAL_REQUEST,ROS_RET_UNSUPPORTED);
        wait until rising_edge(clk);
        
        Log("CLIENT: Take Goal Response (No Take)", INFO);
        client_op(TAKE_GOAL_RESPONSE,ROS_RET_ACTION_CLIENT_TAKE_FAILED);
        wait until rising_edge(clk);
        
        Log("CLIENT: Take Result Response (No Take)", INFO);
        client_op(TAKE_RESULT_RESPONSE,ROS_RET_ACTION_CLIENT_TAKE_FAILED);
        wait until rising_edge(clk);
        
        Log("CLIENT: Take Cancel Response (No Take)", INFO);
        client_op(TAKE_CANCEL_RESPONSE,ROS_RET_ACTION_CLIENT_TAKE_FAILED);
        wait until rising_edge(clk);
        
        ret_cg  <= RETCODE_OK;
        ret_cr  <= RETCODE_OK;
        
        for i in 0 to MAX_GOALS-1 loop
            Log("CLIENT: Send GOAL " & integer'image(i) & " Request", INFO);
            test_gid(i) := gen_goal_id;
            goal_id_c       <= test_gid(i);
            goal_order_c    <= RV.RandSlv(goal_order_c'length);
            client_op(SEND_GOAL_REQUEST,ROS_RET_OK);
            sid(i)  := sequence_id_c;
            wait until rising_edge(clk);
            
            Log("SERVER: Check GOAL " & integer'image(i) & " Request", INFO);
            wait until new_goal_request_s = '1';
            AffirmIfEqual(GOAL, new_goal_order_s, goal_order_c);
            test_handle(i)  := new_goal_handle_s;
            test_ind(i)     := to_integer(unsigned(new_goal_result_index_s));
            goal_handle_in_s    <= test_handle(i);
            new_goal_accepted_s <= '1';
            new_goal_response_s <= '1';
            wait until rising_edge(clk);
            new_goal_accepted_s <= '0';
            new_goal_response_s <= '0';
            
            Log("CLIENT: Take GOAL " & integer'image(i) & " Request", INFO);
            client_op(TAKE_GOAL_RESPONSE,ROS_RET_OK);
            AlertIf(to_unsigned(SEQUENCENUMBER_TYPE(service_info_c.request_id.sequence_number)) /= unsigned(sid(i)), "Request ID incorrect", FAILURE);
            wait until rising_edge(clk);
            
            goal_handle_in_s    <= test_handle(i);
            goal_state_in_s     <= GoalStatus_package.STATUS_EXECUTING;
            server_op(UPDATE_GOAL_STATE,ROS_RET_OK);
            wait until rising_edge(clk);
        end loop;
        
        Log("SERVER: Send FEEDBACK", INFO);
        for i in 0 to RV.RandInt(1,10) loop
            feedback_seq_len_s  <= int(i+1,feedback_seq_len_s'length);
            feedback_seq_addr_s <= int(i,feedback_seq_addr_s'length);
            feedback_seq_w_s    <= RV.RandSlv(feedback_seq_w_s'length);
            wait_on_sig(feedback_seq_ready_s);
            feedback_seq_wen_s  <= '1';
            wait until rising_edge(clk);
            feedback_seq_wen_s  <= '0';
        end loop;
        goal_handle_in_s    <= test_handle(0);
        start_s             <= '1';
        opcode_s            <= PUBLISH_FEEDBACK;
        wait_on_sig(ack_s);
        wait until rising_edge(clk);
        start_s     <= '0';
        
        Log("CLIENT: Take FEEDBACK", INFO);
        client_op(TAKE_FEEDBACK,ROS_RET_OK);
        AffirmIfEqual(FEEDBACK, feedback_goal_id_c, test_gid(0));
        AffirmIfEqual(FEEDBACK, feedback_seq_len_c, feedback_seq_len_s);
        for i in 0 to to_integer(unsigned(feedback_seq_len_c))-1 loop
            feedback_seq_addr_s <= int(i,feedback_seq_addr_s'length);
            feedback_seq_addr_c <= int(i,feedback_seq_addr_c'length);
            wait_on_sig(feedback_seq_ready_s);
            wait_on_sig(feedback_seq_ready_c);
            feedback_seq_ren_s <= '1';
            feedback_seq_ren_c <= '1';
            wait until rising_edge(clk);
            feedback_seq_ren_s <= '0';
            feedback_seq_ren_c <= '0';
            wait_on_sig(feedback_seq_valid_s);
            wait_on_sig(feedback_seq_valid_c);
            AffirmIfEqual(FEEDBACK, feedback_seq_c, feedback_seq_r_s);
            feedback_seq_ack_s  <= '1';
            feedback_seq_ack_c  <= '1';
            wait until rising_edge(clk);
            feedback_seq_ack_s  <= '0';
            feedback_seq_ack_c  <= '0';
        end loop;
        
        for i in 0 to MAX_GOALS-1 loop
            Log("CLIENT: Send RESULT Request (Goal " & integer'image(i) & ")", INFO);
            result_goal_id_c    <= test_gid(i);
            client_op(SEND_RESULT_REQUEST,ROS_RET_OK);
            sid(i)  := sequence_id_c;
            wait until rising_edge(clk);
        end loop;
        
        for j in 0 to MAX_GOALS-1 loop
            Log("SERVER: Set RESULT (Goal " & integer'image(j) & ")", INFO);
            index_result_s  <= int(j,index_result_s'length);
            for i in 0 to RV.RandInt(1,10) loop
                result_seq_len_s    <= int(i+1,result_seq_len_s'length);
                result_seq_addr_s   <= int(i,result_seq_addr_s'length);
                result_seq_w_s      <= RV.RandSlv(result_seq_w_s'length);
                wait_on_sig(result_seq_ready_s);
                result_seq_wen_s    <= '1';
                wait until rising_edge(clk);
                result_seq_wen_s    <= '0';
            end loop;
            wait_on_sig(index_result_ready_s);
            index_result_wen_s  <= '1';
            wait until rising_edge(clk);
            index_result_wen_s  <= '0';
            goal_handle_in_s    <= test_handle(j);
            goal_state_in_s     <= GoalStatus_package.STATUS_SUCCEEDED;
            server_op(UPDATE_GOAL_STATE,ROS_RET_OK);
            wait until rising_edge(clk);
            
            Log("SERVER: Take RESULT Response (Goal " & integer'image(j) & ")", INFO);
            goal_id_c   <= test_gid(j);
            client_op(TAKE_RESULT_RESPONSE,ROS_RET_OK);
            AlertIf(to_unsigned(SEQUENCENUMBER_TYPE(service_info_c.request_id.sequence_number)) /= unsigned(sid(j)), "Request ID incorrect", FAILURE);
            AffirmIfEqual(RESULT, result_status_c, GoalStatus_package.STATUS_SUCCEEDED);
            AffirmIfEqual(RESULT, result_seq_len_c, result_seq_len_s);
            for i in 0 to to_integer(unsigned(result_seq_len_c))-1 loop
                result_seq_addr_s   <= int(i,result_seq_addr_s'length);
                result_seq_addr_c   <= int(i,result_seq_addr_c'length);
                wait_on_sig(result_seq_ready_s);
                wait_on_sig(result_seq_ready_c);
                result_seq_ren_s    <= '1';
                result_seq_ren_c    <= '1';
                wait until rising_edge(clk);
                result_seq_ren_s    <= '0';
                result_seq_ren_c    <= '0';
                wait_on_sig(result_seq_valid_s);
                wait_on_sig(result_seq_valid_c);
                AffirmIfEqual(RESULT, result_seq_c, result_seq_r_s);
                result_seq_ack_s    <= '1';
                result_seq_ack_c    <= '1';
                wait until rising_edge(clk);
                result_seq_ack_s    <= '0';
                result_seq_ack_c    <= '0';
            end loop;
        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;