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

-- NOTE: Even though this Test instantiates other components, we only (mis-) use them for the memory they instantiate, and thus this Test only tests the uut device (Level 0)

-- This testbench tests the General Behavour of ROS Action Server.
-- More specifically following tests are done:
-- TEST: NEW GOAL [MEMORY EMPTY]
-- TEST: NEW GOAL [ACCEPT]
-- TEST: NEW GOAL [REJECT]
-- TEST: NEW GOAL [FULL GOAL MEMORY, TIMEOUT=INFINITE, NO TERMINAL STATE GOAL EXISTS]
-- TEST: NEW GOAL [FULL GOAL MEMORY, TIMEOUT=INFINITE, TERMINAL STATE GOAL EXISTS]
-- TEST: GET_GOAL
-- TEST: GET_GOAL [INVALID HANDLE]
-- TEST: GET_FIRST_GOAL [MEMORY NOT EMPTY]
-- TEST: GET_FIRST_GOAL [MEMORY EMPTY]
-- TEST: GET_LAST_GOAL [MEMORY NOT EMPTY]
-- TEST: GET_LAST_GOAL [MEMORY EMPTY]
-- TEST: GET_NEXT_GOAL [NEXT GOAL EXISTS]
-- TEST: GET_NEXT_GOAL [NEXT GOAL DOES NOT EXISTS]
-- TEST: GET_NEXT_GOAL [INVALID HANDLE]
-- TEST: GET_PREVIOUS_GOAL [PREVIOUS GOAL EXISTS]
-- TEST: GET_PREVIOUS_GOAL [PREVIOUS GOAL DOES NOT EXISTS]
-- TEST: GET_PREVIOUS_GOAL [INVALID HANDLE]
-- TEST: UPDATE_GOAL_STATE [VALID STATE]
-- TEST: UPDATE_GOAL_STATE [INVALID STATE]
-- TEST: UPDATE_GOAL_STATE [INVALID HANDLE]
-- TEST: PUBLISH_FEEDBACK
-- TEST: NEW RESULT REQUEST [MEMORY EMPTY]
-- TEST: NEW RESULT REQUEST [GOAL DOES NOT EXIST]
-- TEST: NEW RESULT REQUEST [GOAL TERMINAL]
-- TEST: NEW RESULT REQUEST [GOAL NOT TERMINAL]
-- TEST: NEW RESULT REQUEST [FULL RESULT MEMORY]
-- TEST: STORED RESULT REQUEST [GOAL STATE UPDATES]
-- TEST: STORED RESULT REQUEST [GOAL STATE CHANGES TO TERMINAL]
-- TEST: STORED RESULT REQUEST [MULTIPLE REQUESTS, SAME GOAL, GOAL STATE CHANGES TO TERMINAL]
-- TEST: RESULT REQUEST MEMORY [EMPTY->FULL->EMPTY->FULL]
-- TEST: CANCEL REQUEST [MEMORY EMPTY]
-- TEST: CANCEL REQUEST [GOAL ID SET, GOAL EXISTS]
-- TEST: CANCEL REQUEST [GOAL ID SET, GOAL EXISTS, GOAL TERMINAL]
-- TEST: CANCEL REQUEST [GOAL ID SET, GOAL DOES NOT EXISTS]
-- TEST: CANCEL REQUEST [TIME SET, NO RELEVANT GOALS]
-- TEST: CANCEL REQUEST [TIME SET, RELEVANT GOALS EXIST]
-- TEST: CANCEL REQUEST [NO GOAL ID SET, NO TIME SET, GOALS EXIST]
-- TEST: CANCEL REQUEST [NO GOAL ID SET, NO TIME SET, NO GOALS EXIST]
-- TEST: CANCEL REQUEST [GOAL ID SET, TIME SET, GOAL ID IS NOT IN TIME RANGE]
-- TEST: CANCEL REQUEST [GOAL ID SET, TIME SET, GOAL ID IS IN TIME RANGE]
-- TEST: STATUS UPDATE [NEW GOAL]
-- TEST: STATUS UPDATE [GOAL UPDATE]
-- TEST: STATUS UPDATE [GOAL REMOVED]
-- TEST: STATUS UPDATE [GOAL CANCELED]

entity L0_ros_action_server_test2 is
end entity;

architecture testbench of L0_ros_action_server_test2 is
    
    constant MAX_GOALS : natural := 4;
    constant MAX_RESULT_REQUESTS : natural := 2;
    
    type STATUS_CHECK_STAGE_TYPE is (IDLE, CHECK_1, CHECK_2, DONE_STATUS);
    
    type TEST_GOAL_TYPE is record
        goal_id : std_logic_vector(UUID_WIDTH-1 downto 0);
        stamp   : std_logic_vector(ROS_TIME_WIDTH-1 downto 0);
        state   : std_logic_vector(CDR_INT8_WIDTH-1 downto 0);
        handle  : std_logic_vector(GOAL_HANDLE_WIDTH-1 downto 0);
        index   : std_logic_vector(WORD_WIDTH-1 downto 0);
    end record;
    type TEST_GOAL_ARRAY_TYPE is array (0 to MAX_GOALS-1) of TEST_GOAL_TYPE;
    type TEST_CANCEL_ARRAY_TYPE is array (0 to MAX_GOALS-1) of natural;
    
    constant ZERO_TEST_GOAL : TEST_GOAL_TYPE := (
        goal_id => (others => '0'),
        stamp   => (others => '0'),
        state   => STATUS_UNKNOWN,
        handle  => GOAL_HANDLE_UNKNOWN,
        index   => (others => '0')
    );
    
    type REQUEST_ID_ARRAY_TYPE is array (0 to MAX_RESULT_REQUESTS-1) of REQUEST_ID_TYPE;
    
    constant ZERO_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, selector_s, selector_c : std_logic := '0';
    signal t : ROS_TIME_TYPE := ROS_TIME_ZERO;
    signal status_stage : STATUS_CHECK_STAGE_TYPE := IDLE;
    
    signal start_g, ack_g, data_available_g, taken_g, done_g, accepted_g : std_logic := '0';
    signal opcode_g : ROS_SERVICE_OPCODE_TYPE := NOP;
    signal service_info_g : SERVICE_INFO_TYPE := EMPTY_SERVICE_INFO;
    signal request_id_g : REQUEST_ID_TYPE := EMPTY_REQUEST_ID;
    signal return_code_g : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := ROS_RET_OK;
    signal goal_id_g : std_logic_vector(UUID_WIDTH-1 downto 0) := (others => '0');
    signal stamp_g : std_logic_vector(ROS_TIME_WIDTH-1 downto 0) := (others => '0');
    
    signal start_r, ack_r, data_available_r, taken_r, done_r, result_sel, result_sel_ack : std_logic := '0';
    signal opcode_r : ROS_SERVICE_OPCODE_TYPE := NOP;
    signal service_info_r : SERVICE_INFO_TYPE := EMPTY_SERVICE_INFO;
    signal request_id_r : REQUEST_ID_TYPE := EMPTY_REQUEST_ID;
    signal return_code_r : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := ROS_RET_OK;
    signal result_index : std_logic_vector(WORD_WIDTH-1 downto 0) := (others => '0');
    signal goal_id_r : std_logic_vector(UUID_WIDTH-1 downto 0) := (others => '0');
    signal status_r : std_logic_vector(CDR_INT8_WIDTH-1 downto 0) := (others => '0');
    
    signal start_c, ack_c, data_available_c, taken_c, done_c : std_logic := '0';
    signal opcode_c : ROS_SERVICE_OPCODE_TYPE := NOP;
    signal service_info_c : SERVICE_INFO_TYPE := EMPTY_SERVICE_INFO;
    signal request_id_c : REQUEST_ID_TYPE := EMPTY_REQUEST_ID;
    signal return_code_c : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := ROS_RET_OK;
    signal goal_info_goal_id_c, goals_canceling_goal_id_r, goals_canceling_goal_id_w : std_logic_vector(UUID_WIDTH-1 downto 0) := (others => '0');
    signal goal_info_stamp_c : std_logic_vector(ROS_TIME_WIDTH-1 downto 0) := (others => '0');
    signal cancel_return_code_c : std_logic_vector(CDR_INT8_WIDTH-1 downto 0) := (others => '0');
    signal goals_canceling_len, goals_canceling_addr, goals_canceling_addr_c, goals_canceling_addr_u : std_logic_vector(CancelGoal_package.RR_GOALS_CANCELING_ADDR_WIDTH-1 downto 0) := (others => '0');
    signal goals_canceling_ready, goals_canceling_ren, goals_canceling_wen, goals_canceling_valid, goals_canceling_ack : std_logic := '0';
    signal goals_canceling_stamp_r, goals_canceling_stamp_w : std_logic_vector(ROS_TIME_WIDTH-1 downto 0) := (others => '0');
    
    signal start_fb, ack_fb, done_fb : std_logic := '0';
    signal opcode_fb : ROS_TOPIC_OPCODE_TYPE := NOP;
    signal return_code_fb : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := (others => '0');
    signal goal_id_fb : std_logic_vector(UUID_WIDTH-1 downto 0) := (others => '0');
    
    signal start_s, ack_s, done_s : std_logic := '0';
    signal opcode_s : ROS_TOPIC_OPCODE_TYPE := NOP;
    signal return_code_s : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := (others => '0');
    signal status_list_len, status_list_addr, status_list_addr_s, status_list_addr_u : std_logic_vector(GoalStatusArray_package.STATUS_LIST_ADDR_WIDTH-1 downto 0) := (others => '0');
    signal status_list_ready, status_list_ren, status_list_wen, status_list_valid, status_list_ack : std_logic := '0';
    signal status_list_goal_info_goal_id_r, status_list_goal_info_goal_id_w : std_logic_vector(UUID_WIDTH-1 downto 0) := (others => '0');
    signal status_list_goal_info_stamp_r, status_list_goal_info_stamp_w : std_logic_vector(ROS_TIME_WIDTH-1 downto 0) := (others => '0');
    signal status_list_status_r, status_list_status_w : std_logic_vector(CDR_INT8_WIDTH-1 downto 0) := (others => '0');
    
    signal start, ack, done : std_logic := '0';
    signal opcode : ROS_ACTION_OPCODE_TYPE := NOP;
    signal return_code : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0) := (others => '0');
    signal goal_handle_in, goal_handle_out : std_logic_vector(GOAL_HANDLE_WIDTH-1 downto 0) := (others => '0');
    signal goal_state_in, goal_state_out : std_logic_vector(CDR_INT8_WIDTH-1 downto 0) := (others => '0');
    signal goal_id : std_logic_vector(UUID_WIDTH-1 downto 0) := (others => '0');
    signal goal_result_index : std_logic_vector(WORD_WIDTH-1 downto 0) := (others => '0');
    signal goal_stamp : ROS_TIME_TYPE := ROS_TIME_ZERO;
    signal new_goal_request, new_goal_accepted, new_goal_response, cancel_request, cancel_accepted, cancel_response : std_logic := '0';
    signal new_goal_handle, cancel_request_handle : std_logic_vector(GOAL_HANDLE_WIDTH-1 downto 0) := (others => '0');
    signal new_goal_result_index : std_logic_vector(WORD_WIDTH-1 downto 0) := (others => '0');
    
    shared variable test_goals : TEST_GOAL_ARRAY_TYPE := (others => ZERO_TEST_GOAL);
    shared variable test_goal_cnt : natural := 0;
    shared variable GOAL, RESULT, FEEDBACK, STATUS, USER, CANCEL : AlertLogIDType;
    
    function from_request_id(input : REQUEST_ID_TYPE) return std_logic_vector is
        variable ret : std_logic_vector(GUID_WIDTH+63 downto 0);
    begin
        ret(GUID_WIDTH+63 downto 64)    := std_logic_vector(to_unsigned(input.writer_guid));
        ret(63 downto 0)                := std_logic_vector(to_unsigned(input.sequence_number));
        return ret;
    end function;
    
begin
    
    uut : entity work.ros_action_server(arch)
        generic map (
            TIMEOUT_DURATION    => ROS_DURATION_INFINITE,
            MAX_GOALS           => MAX_GOALS,
            MAX_RESULT_REQUESTS => 2
        )
        port map (
            clk                                 => clk,
            reset                               => reset,
            time                                => t,
            start_g                             => start_g,
            ack_g                               => ack_g,
            opcode_g                            => opcode_g,
            service_info_g                      => service_info_g,
            request_id_g                        => request_id_g,
            data_available_g                    => data_available_g,
            taken_g                             => taken_g,
            done_g                              => done_g,
            return_code_g                       => return_code_g,
            goal_id_g                           => goal_id_g,
            accepted_g                          => accepted_g,
            stamp_g                             => stamp_g,
            start_r                             => start_r,
            ack_r                               => ack_r,
            opcode_r                            => opcode_r,
            service_info_r                      => service_info_r,
            request_id_r                        => request_id_r,
            data_available_r                    => data_available_r,
            taken_r                             => taken_r,
            done_r                              => done_r,
            return_code_r                       => return_code_r,
            result_index                        => result_index,
            result_sel                          => result_sel,
            result_sel_ack                      => result_sel_ack,
            goal_id_r                           => goal_id_r,
            status_r                            => status_r,
            start_c                             => start_c,
            ack_c                               => ack_c,
            opcode_c                            => opcode_c,
            service_info_c                      => service_info_c,
            request_id_c                        => request_id_c,
            data_available_c                    => data_available_c,
            taken_c                             => taken_c,
            done_c                              => done_c,
            return_code_c                       => return_code_c,
            goal_info_goal_id_c                 => goal_info_goal_id_c,
            goal_info_stamp_c                   => goal_info_stamp_c,
            cancel_return_code_c                => cancel_return_code_c,
            goals_canceling_len_c               => goals_canceling_len,
            goals_canceling_addr_c              => goals_canceling_addr_c,
            goals_canceling_ready_c             => goals_canceling_ready,
            goals_canceling_ren_c               => open,
            goals_canceling_wen_c               => goals_canceling_wen,
            goals_canceling_valid_c             => goals_canceling_valid,
            goals_canceling_ack_c               => open,
            goals_canceling_goal_id_r_c         => (others => '0'),
            goals_canceling_goal_id_w_c         => goals_canceling_goal_id_w,
            goals_canceling_stamp_r_c           => (others => '0'),
            goals_canceling_stamp_w_c           => goals_canceling_stamp_w,
            start_fb                            => start_fb,
            opcode_fb                           => opcode_fb,
            ack_fb                              => ack_fb,
            done_fb                             => done_fb,
            return_code_fb                      => return_code_fb,
            goal_id_fb                          => goal_id_fb,
            start_s                             => start_s,
            opcode_s                            => opcode_s,
            ack_s                               => ack_s,
            done_s                              => done_s,
            return_code_s                       => return_code_s,
            status_list_len_s                   => status_list_len,
            status_list_addr_s                  => status_list_addr_s,
            status_list_ready_s                 => status_list_ready,
            status_list_ren_s                   => open,
            status_list_wen_s                   => status_list_wen,
            status_list_valid_s                 => status_list_valid,
            status_list_ack_s                   => open,
            status_list_goal_info_goal_id_r_s   => (others => '0'),
            status_list_goal_info_goal_id_w_s   => status_list_goal_info_goal_id_w,
            status_list_goal_info_stamp_r_s     => (others => '0'),
            status_list_goal_info_stamp_w_s     => status_list_goal_info_stamp_w,
            status_list_status_r_s              => status_list_status_r,
            status_list_status_w_s              => status_list_status_w,
            start                               => start,
            opcode                              => opcode,
            ack                                 => ack,
            done                                => done,
            return_code                         => return_code,
            goal_handle_in                      => goal_handle_in,
            goal_handle_out                     => goal_handle_out,
            goal_state_in                       => goal_state_in,
            goal_state_out                      => goal_state_out,
            goal_id                             => goal_id,
            goal_result_index                   => goal_result_index,
            goal_stamp                          => goal_stamp,
            new_goal_request                    => new_goal_request,
            new_goal_handle                     => new_goal_handle,
            new_goal_result_index               => new_goal_result_index,
            new_goal_accepted                   => new_goal_accepted,
            new_goal_response                   => new_goal_response,
            cancel_request                      => cancel_request,
            cancel_request_handle               => cancel_request_handle,
            cancel_accepted                     => cancel_accepted,
            cancel_response                     => cancel_response
        );
    
    cancel_array : entity work.CancelGoal_ros_srv_server(arch)
        port map (
            clk                                 => clk,
            reset                               => reset,
            start_r                             => open,
            ack_r                               => '0',
            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                              => '0',
            return_code_r                       => (others => '0'),
            valid_in_r                          => '0',
            ready_in_r                          => open,
            data_in_r                           => (others => '0'),
            last_word_in_r                      => '0',
            sample_info_r                       => ZERO_SAMPLE_INFO,
            sample_info_valid_r                 => '0',
            sample_info_ack_r                   => open,
            eoc_r                               => '0',
            status_r                            => (others => '0'),
            start_w                             => open,
            ack_w                               => '0',
            opcode_w                            => open,
            instance_handle_out_w               => HANDLE_NIL,
            source_ts_w                         => open,
            max_wait_w                          => open,
            done_w                              => '0',
            return_code_w                       => (others => '0'),
            instance_handle_in_w                => open,
            valid_out_w                         => open,
            ready_out_w                         => '0',
            data_out_w                          => open,
            last_word_out_w                     => open,
            valid_in_w                          => '0',
            ready_in_w                          => open,
            data_in_w                           => (others => '0'),
            last_word_in_w                      => '0',
            status_w                            => (others => '0'),
            start_user                          => '0',
            ack_user                            => open,
            opcode_user                         => NOP,
            service_info_user                   => open,
            request_id_user                     => EMPTY_REQUEST_ID,
            data_available_user                 => open,
            taken_user                          => open,
            goal_info_goal_id                   => open,
            goal_info_stamp                     => open,
            return_code                         => (others => '0'),
            goals_canceling_len                 => goals_canceling_len,
            goals_canceling_addr                => goals_canceling_addr,
            goals_canceling_ready               => goals_canceling_ready,
            goals_canceling_ren                 => goals_canceling_ren,
            goals_canceling_wen                 => goals_canceling_wen,
            goals_canceling_valid               => goals_canceling_valid,
            goals_canceling_ack                 => goals_canceling_ack,
            goals_canceling_goal_id_r           => goals_canceling_goal_id_r,
            goals_canceling_goal_id_w           => goals_canceling_goal_id_w,
            goals_canceling_stamp_r             => goals_canceling_stamp_r,
            goals_canceling_stamp_w             => goals_canceling_stamp_w,
            done_user                           => open,
            return_code_user                    => open
        );
        
    status_array : entity work.GoalStatusArray_ros_pub(arch)
        port map (
            clk                             => clk,
            reset                           => reset,
            start_dds                       => open,
            ack_dds                         => '0',
            opcode_dds                      => open,
            instance_handle_out_dds         => HANDLE_NIL,
            source_ts_dds                   => open,
            max_wait_dds                    => open,
            done_dds                        => '0',
            return_code_dds                 => (others => '0'),
            instance_handle_in_dds          => open,
            valid_out_dds                   => open,
            ready_out_dds                   => '0',
            data_out_dds                    => open,
            last_word_out_dds               => open,
            valid_in_dds                    => '0',
            ready_in_dds                    => open,
            data_in_dds                     => (others => '0'),
            last_word_in_dds                => '0',
            status_dds                      => (others => '0'),
            start_user                      => '0',
            opcode_user                     => NOP,
            ack_user                        => open,
            status_list_len                 => status_list_len,
            status_list_addr                => status_list_addr,
            status_list_ready               => status_list_ready,
            status_list_ren                 => status_list_ren,
            status_list_wen                 => status_list_wen,
            status_list_valid               => status_list_valid,
            status_list_ack                 => status_list_ack,
            status_list_goal_info_goal_id_r => status_list_goal_info_goal_id_r,
            status_list_goal_info_goal_id_w => status_list_goal_info_goal_id_w,
            status_list_goal_info_stamp_r   => status_list_goal_info_stamp_r,
            status_list_goal_info_stamp_w   => status_list_goal_info_stamp_w,
            status_list_status_r            => status_list_status_r,
            status_list_status_w            => status_list_status_w,
            done_user                       => open,
            return_code_user                => open
        );
    
    status_list_addr        <= status_list_addr_s when (selector_s = '0') else status_list_addr_u;
    goals_canceling_addr    <= goals_canceling_addr_c when (selector_c = '0') else goals_canceling_addr_u;
    
    stimulus_prc : process
        variable RV : RandomPType;
        variable test_rrq : REQUEST_ID_ARRAY_TYPE := (others => EMPTY_REQUEST_ID);
        variable test_cancel : TEST_CANCEL_ARRAY_TYPE := (others => 0);
        
        alias idle_sig is <<signal uut.idle_sig : std_logic>>;
        alias mem_done is <<signal uut.mem_done : std_logic>>;
        
        procedure wait_on_sig(signal sig : std_logic) is
        begin
            if (sig /= '1') then
                wait on sig until sig = '1';
            end if;
        end procedure;
        
        impure function gen_request_id return REQUEST_ID_TYPE is
            variable ret : REQUEST_ID_TYPE := EMPTY_REQUEST_ID;
        begin
            ret.writer_guid         := to_guid(RV.RandSlv(GUID_WIDTH));
            ret.sequence_number(0)  := unsigned(RV.RandSlv(ret.sequence_number(0)'length));
            ret.sequence_number(1)  := unsigned(RV.RandSlv(ret.sequence_number(1)'length));
            return ret;
        end function;
        
        impure function gen_goal_id return std_logic_vector is
        begin
            return RV.RandSlv(UUID_WIDTH);
        end function;
        
        impure function cancel_len return natural is
            variable ret : natural := 0;
        begin
            for i in 0 to MAX_GOALS-1 loop
                if (test_cancel(i) = MAX_GOALS) then
                    exit;
                end if;
                ret := ret + 1;
            end loop;
            return ret;
        end function;
        
        function ros_gen_time(t : time) return std_logic_vector is
            variable tmp : ROS_TIME_TYPE;
        begin
            tmp := gen_duration(t);
            return std_logic_vector(to_unsigned(tmp));
        end function;
        
        procedure get_goal_request(index : in natural; goal_id : in std_logic_vector(UUID_WIDTH-1 downto 0)) is
        begin
            data_available_g    <= '1';
            wait_on_sig(start_g);
            wait for 1 ps; -- Make sure all signals stable
            AlertIf(opcode_g /= TAKE_REQUEST, "Goal service opcode incorrect", FAILURE);
            ack_g   <= '1';
            wait until rising_edge(clk);
            ack_g   <= '0';
            service_info_g.request_id <= gen_request_id;
            goal_id_g           <= goal_id;
            data_available_g    <= '0';
            done_g              <= '1';
            taken_g             <= '1';
            return_code_g       <= RETCODE_OK;
            wait until rising_edge(clk);
            done_g          <= '0';
            wait until rising_edge(clk);
            if (index < test_goals'length) then
                test_goals(index).goal_id   := goal_id_g;
            end if;
        end procedure;
        
        procedure user_input_goal(index : in natural; accept : in boolean) is
        begin
            wait_on_sig(new_goal_request);
            AlertIf((to_integer(unsigned(new_goal_handle)) / 10) /= to_integer(unsigned(new_goal_result_index)), "Result index out of sync", FAILURE);
            if (accept) then
                test_goals(index).handle    := new_goal_handle;
                test_goals(index).index     := new_goal_result_index;
            end if;
            new_goal_accepted   <= '1' when accept else '0';
            new_goal_response   <= '1';
            wait until rising_edge(clk);
            if (accept) then
                test_goals(index).stamp     := std_logic_vector(to_unsigned(t));
                test_goals(index).state     := STATUS_ACCEPTED;
                test_goal_cnt               := test_goal_cnt + 1;
            end if;
            new_goal_accepted   <= '0';
            new_goal_response   <= '0';
            wait until rising_edge(clk);
        end procedure;
        
        procedure get_goal_response(index : in natural) is
        begin
            wait_on_sig(start_g);
            AlertIf(opcode_g /= SEND_RESPONSE, "Goal service opcode incorrect", FAILURE);
            ack_g   <= '1';
            wait until rising_edge(clk);
            ack_g   <= '0';
            wait for 1 ps; -- Make sure all signals stable
            AffirmIfEqual(GOAL, from_request_id(request_id_g), from_request_id(service_info_g.request_id));
            if (index < test_goals'length) then
                AffirmIfEqual(GOAL, accepted_g, '1');
            else
                AffirmIfEqual(GOAL, accepted_g, '0');
            end if;
            --AffirmIfEqual(GOAL, stamp_g, test_goals(index).stamp);
            AffirmIfEqual(GOAL, stamp_g, std_logic_vector(to_unsigned(t)));
            done_g              <= '1';
            return_code_g       <= RETCODE_OK;
            wait until rising_edge(clk);
            done_g          <= '0';
            wait until rising_edge(clk);
        end procedure;
        
        procedure get_result_request(index : in natural; goal_id : in std_logic_vector(UUID_WIDTH-1 downto 0)) is
        begin
            data_available_r    <= '1';
            wait_on_sig(start_r);
            wait for 1 ps; -- Make sure all signals stable
            AlertIf(opcode_r /= TAKE_REQUEST, "Result service opcode incorrect", FAILURE);
            ack_r   <= '1';
            wait until rising_edge(clk);
            ack_r   <= '0';
            service_info_r.request_id <= gen_request_id;
            goal_id_r           <= goal_id;
            data_available_r    <= '0';
            done_r              <= '1';
            taken_r             <= '1';
            return_code_r       <= RETCODE_OK;
            wait until rising_edge(clk);
            done_r          <= '0';
            wait until rising_edge(clk);
            if (index < test_rrq'length) then
                test_rrq(index) := service_info_r.request_id;
            end if;
        end procedure;
        
        procedure get_result_response(index_goal : in natural; index_request : in natural) is
        begin
            wait_on_sig(result_sel);
            wait for 1 ps; -- Make sure all signals stable
            if (index_goal < test_goals'length and test_goals(index_goal).state = STATUS_SUCCEEDED) then
                AffirmIfEqual(RESULT, result_index, test_goals(index_goal).index);
            else
                AffirmIfEqual(RESULT, result_index, int(MAX_GOALS,result_index'length));
            end if;
            result_sel_ack  <= '1';
            wait_on_sig(start_r);
            wait for 1 ps; -- Make sure all signals stable
            AlertIf(opcode_r /= SEND_RESPONSE, "Result service opcode incorrect", FAILURE);
            ack_r   <= '1';
            wait until rising_edge(clk);
            ack_r   <= '0';
            wait for 1 ps; -- Make sure all signals stable
            if (index_request < test_rrq'length) then
                AffirmIfEqual(RESULT, from_request_id(request_id_r), from_request_id(test_rrq(index_request)));
            else
                AffirmIfEqual(RESULT, from_request_id(request_id_r), from_request_id(service_info_r.request_id));
            end if;
            if (index_goal < test_goals'length) then
                AffirmIfEqual(RESULT, status_r, test_goals(index_goal).state);
            else
                AffirmIfEqual(RESULT, status_r, STATUS_UNKNOWN);
            end if;
            done_r              <= '1';
            return_code_r       <= RETCODE_OK;
            wait until rising_edge(clk);
            done_r          <= '0';
            result_sel_ack  <= '0';
            wait for 1 ps; -- Make sure all signals stable
            AlertIf(result_sel = '1', "result_sel still high", FAILURE);
            wait until rising_edge(clk);
        end procedure;
        
        procedure get_cancel_request(goal_id : in std_logic_vector(UUID_WIDTH-1 downto 0); stamp : in std_logic_vector(ROS_TIME_WIDTH-1 downto 0)) is
        begin
            data_available_c    <= '1';
            wait_on_sig(start_c);
            wait for 1 ps; -- Make sure all signals stable
            AlertIf(opcode_c /= TAKE_REQUEST, "Result service opcode incorrect", FAILURE);
            ack_c   <= '1';
            wait until rising_edge(clk);
            ack_c   <= '0';
            service_info_c.request_id <= gen_request_id;
            goal_info_goal_id_c <= goal_id;
            goal_info_stamp_c   <= stamp;
            data_available_c    <= '0';
            done_c              <= '1';
            taken_c             <= '1';
            return_code_c       <= RETCODE_OK;
            wait until rising_edge(clk);
            done_c          <= '0';
            wait until rising_edge(clk);
        end procedure;
        
        procedure user_input_cancel(index : in natural; accept : in boolean) is
        begin
            wait_on_sig(cancel_request);
            AffirmIfEqual(CANCEL, cancel_request_handle, test_goals(index).handle);
            cancel_accepted     <= '1' when accept else '0';
            cancel_response     <= '1';
            wait until rising_edge(clk);
            if (accept) then
                test_goals(index).state := STATUS_CANCELING;
            end if;
            cancel_accepted     <= '0';
            cancel_response     <= '0';
            wait until rising_edge(clk);
        end procedure;
        
        procedure get_cancel_response(ret_code : in std_logic_vector(CDR_INT8_WIDTH-1 downto 0)) is
        begin
            wait_on_sig(start_c);
            AlertIf(opcode_c /= SEND_RESPONSE, "Goal service opcode incorrect", FAILURE);
            ack_c   <= '1';
            wait until rising_edge(clk);
            ack_c   <= '0';
            selector_c  <= '1';
            wait for 1 ps; -- Make sure all signals stable
            AffirmIfEqual(CANCEL, from_request_id(request_id_c), from_request_id(service_info_c.request_id));
            AffirmIfEqual(CANCEL, cancel_return_code_c, ret_code);
            AffirmIfEqual(CANCEL, goals_canceling_len, int(cancel_len,goals_canceling_len'length));
            for i in 0 to cancel_len-1 loop
                goals_canceling_addr_u  <= int(i,goals_canceling_addr_u'length);
                wait_on_sig(goals_canceling_ready);
                goals_canceling_ren     <= '1';
                wait until rising_edge(clk);
                goals_canceling_ren     <= '0';
                wait_on_sig(goals_canceling_valid);
                AffirmIfEqual(CANCEL, goals_canceling_goal_id_r, test_goals(test_cancel(i)).goal_id);
                AffirmIfEqual(CANCEL, goals_canceling_stamp_r, test_goals(test_cancel(i)).stamp);
                goals_canceling_ack     <= '1';
                wait until rising_edge(clk);
                goals_canceling_ack     <= '0';
            end loop;
            done_c          <= '1';
            return_code_c   <= RETCODE_OK;
            wait until rising_edge(clk);
            selector_c      <= '0';
            done_c          <= '0';
            wait until rising_edge(clk);
        end procedure;
        
        procedure user_op(op : in ROS_ACTION_OPCODE_TYPE) is
        begin
            start   <= '1';
            opcode  <= op;
            wait_on_sig(ack);
            wait until rising_edge(clk);
            start   <= '0';
            wait_on_sig(done);
        end procedure;
        
        procedure user_fb_op(goal_id : in std_logic_vector(UUID_WIDTH-1 downto 0)) is
        begin
            start   <= '1';
            opcode  <= PUBLISH_FEEDBACK;
            wait_on_sig(ack);
            wait until rising_edge(clk);
            start   <= '0';
            wait_on_sig(start_fb);
            wait for 1 ps; -- Make sure all signals stable
            AlertIf(opcode_fb /= PUBLISH, "Goal service opcode incorrect", FAILURE);
            ack_fb  <= '1';
            wait until rising_edge(clk);
            ack_fb   <= '0';
            AffirmIfEqual(FEEDBACK, goal_id_fb, goal_id);
            done_fb         <= '1';
            return_code_fb  <= RETCODE_OK;
            wait until rising_edge(clk);
            AffirmIfEqual(FEEDBACK, done, done_fb);
            AffirmIfEqual(FEEDBACK, return_code, return_code_fb);
            done_fb         <= '0';
            wait until rising_edge(clk);
        end procedure;
        
        -- NOTE: This procedure waits until the idle_sig and mem_done is high for at least
        --       two consecutive clock cycles.
        procedure wait_on_idle is
            variable first : boolean := TRUE;
        begin
            loop
                if (not (idle_sig = '1' and mem_done = '1')) then
                    wait until idle_sig = '1' and mem_done = '1';
                elsif (not first) then
                    exit;
                end if;
                wait until rising_edge(clk);
                wait until rising_edge(clk);
                first := FALSE;
            end loop;
        end procedure;
    begin
        
        SetAlertLogName("L0_ros_action_server_test2 - (TIMEOUT INFINITE) - 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);
        CANCEL      := GetAlertLogID("CANCEL", ALERTLOG_BASE_ID);
        FEEDBACK    := GetAlertLogID("FEEDBACK", ALERTLOG_BASE_ID);
        STATUS      := GetAlertLogID("STATUS", ALERTLOG_BASE_ID);
        USER        := GetAlertLogID("USER", ALERTLOG_BASE_ID);
        
        Log("Initial Reset", INFO);
        selector_c  <= '0';
        t    <= gen_duration(0 sec);
        reset   <= '1';
        wait until rising_edge(clk);
        wait until rising_edge(clk);
        reset   <= '0';
        
        -- TEST: GET_FIRST_GOAL [MEMORY EMPTY]
        
        Log("GET_FIRST_GOAL [No Goal]", INFO);
        t    <= gen_duration(1 sec);
        user_op(GET_FIRST_GOAL);
        wait for 1 ps; -- Make sure all signals stable
        AffirmIfEqual(STATUS, goal_handle_out, GOAL_HANDLE_UNKNOWN);
        AlertIf(return_code /= ROS_RET_OK, "User operation return code incorrect", FAILURE);
        wait_on_idle;
        
        -- TEST: GET_LAST_GOAL [MEMORY EMPTY]
        
        Log("GET_LAST_GOAL [No Goal]", INFO);
        t    <= gen_duration(2 sec);
        user_op(GET_LAST_GOAL);
        wait for 1 ps; -- Make sure all signals stable
        AffirmIfEqual(STATUS, goal_handle_out, GOAL_HANDLE_UNKNOWN);
        AlertIf(return_code /= ROS_RET_OK, "User operation return code incorrect", FAILURE);
        wait_on_idle;
        
        -- TEST: GET_GOAL [INVALID HANDLE]
        
        Log("GET_GOAL [Invalid Handle]", INFO);
        t    <= gen_duration(3 sec);
        goal_handle_in  <= (others => '0');
        user_op(GET_GOAL);
        wait for 1 ps; -- Make sure all signals stable
        AlertIf(return_code /= ROS_RET_ACTION_GOAL_HANDLE_INVALID, "User operation return code incorrect", FAILURE);
        wait_on_idle;
        
        -- TEST: GET_NEXT_GOAL [INVALID HANDLE]
        
        Log("GET_NEXT_GOAL [Invalid Handle]", INFO);
        t    <= gen_duration(4 sec);
        goal_handle_in  <= (others => '0');
        user_op(GET_NEXT_GOAL);
        wait for 1 ps; -- Make sure all signals stable
        AlertIf(return_code /= ROS_RET_ACTION_GOAL_HANDLE_INVALID, "User operation return code incorrect", FAILURE);
        wait_on_idle;
        
        -- TEST: GET_PREVIOUS_GOAL [INVALID HANDLE]
        
        Log("GET_PREVIOUS_GOAL [Invalid Handle]", INFO);
        t    <= gen_duration(5 sec);
        goal_handle_in  <= (others => '0');
        user_op(GET_PREVIOUS_GOAL);
        wait for 1 ps; -- Make sure all signals stable
        AlertIf(return_code /= ROS_RET_ACTION_GOAL_HANDLE_INVALID, "User operation return code incorrect", FAILURE);
        wait_on_idle;
        
        -- TEST: UPDATE_GOAL_STATE [INVALID HANDLE]
        
        Log("UPDATE_GOAL_STATE [Invalid Handle]", INFO);
        t    <= gen_duration(6 sec);
        goal_handle_in  <= (others => '0');
        goal_state_in   <= STATUS_EXECUTING;
        user_op(UPDATE_GOAL_STATE);
        wait for 1 ps; -- Make sure all signals stable
        AlertIf(return_code /= ROS_RET_ACTION_GOAL_HANDLE_INVALID, "User operation return code incorrect", FAILURE);
        wait_on_idle;
        
        -- TEST: NEW RESULT REQUEST [MEMORY EMPTY]
        
        Log("New Result Request [Goal 1]", INFO);
        t    <= gen_duration(7 sec);
        get_result_request(2, gen_goal_id);
        get_result_response(5,2);
        wait_on_idle;
        
        -- TEST: CANCEL REQUEST [MEMORY EMPTY]
        -- TEST: CANCEL REQUEST [NO GOAL ID SET, NO TIME SET, NO GOALS EXIST]
        
        Log("Cancel Request [ALL]", INFO);
        t    <= gen_duration(8 sec);
        test_cancel := (others => MAX_GOALS);
        get_cancel_request((others => '0'), (others => '0'));
        get_cancel_response(CancelGoal_package.RR_ERROR_REJECTED);
        wait_on_idle;
        
        -- TEST: NEW GOAL [ACCEPT]
        -- TEST: STATUS UPDATE [NEW GOAL]
        -- TEST: NEW GOAL [MEMORY EMPTY]
        
        Log("New Goal [Goal 1, Accept]", INFO);
        t    <= gen_duration(9 sec);
        get_goal_request(0, goal_id_r);
        user_input_goal(0, TRUE);
        get_goal_response(0);
        wait_on_idle;
        
        -- TEST: NEW GOAL [REJECT]
        
        Log("New Goal [Goal 2, Reject]", INFO);
        t    <= gen_duration(10 sec);
        get_goal_request(5, gen_goal_id);
        user_input_goal(5,FALSE);
        get_goal_response(5);
        wait_on_idle;
        
        Log("New Goal [Goal 3, Accept]", INFO);
        t    <= gen_duration(11 sec);
        get_goal_request(1, gen_goal_id);
        user_input_goal(1,TRUE);
        get_goal_response(1);
        wait_on_idle;
        
        Log("New Goal [Goal 4, Accept]", INFO);
        t    <= gen_duration(12 sec);
        get_goal_request(2, gen_goal_id);
        user_input_goal(2,TRUE);
        get_goal_response(2);
        wait_on_idle;
        
        Log("New Goal [Goal 5, Accept]", INFO);
        t    <= gen_duration(13 sec);
        get_goal_request(3, gen_goal_id);
        user_input_goal(3,TRUE);
        get_goal_response(3);
        wait_on_idle;
        
        -- TEST: NEW GOAL [FULL GOAL MEMORY, TIMEOUT=INFINITE, NO TERMINAL STATE GOAL EXISTS]
        
        data_available_g    <= '1';
        t    <= gen_duration(14 sec);
        wait until rising_edge(clk);
        wait until rising_edge(clk);
        AlertIf(start_r = '1', "Goal service operation on full memory", FAILURE);
        
        -- TEST: UPDATE_GOAL_STATE [VALID STATE]
        -- TEST: STATUS UPDATE [GOAL UPDATE]
        
        Log("UPDATE_GOAL_STATE [Goal 3, Executing]", INFO);
        t    <= gen_duration(15 sec);
        goal_handle_in  <= test_goals(1).handle;
        goal_state_in   <= STATUS_EXECUTING;
        user_op(UPDATE_GOAL_STATE);
        test_goals(1).state := STATUS_EXECUTING;
        wait for 1 ps; -- Make sure all signals stable
        AlertIf(return_code /= ROS_RET_OK, "User operation return code incorrect", FAILURE);
        wait_on_idle;
        
        Log("UPDATE_GOAL_STATE [Goal 5, Executing]", INFO);
        t    <= gen_duration(16 sec);
        goal_handle_in  <= test_goals(3).handle;
        goal_state_in   <= STATUS_EXECUTING;
        user_op(UPDATE_GOAL_STATE);
        test_goals(3).state := STATUS_EXECUTING;
        wait for 1 ps; -- Make sure all signals stable
        AlertIf(return_code /= ROS_RET_OK, "User operation return code incorrect", FAILURE);
        wait_on_idle;
        
        -- TEST: GET_FIRST_GOAL [MEMORY NOT EMPTY]
        
        Log("GET_FIRST_GOAL [Goal 5]", INFO);
        t    <= gen_duration(17 sec);
        user_op(GET_FIRST_GOAL);
        wait for 1 ps; -- Make sure all signals stable
        AffirmIfEqual(STATUS, goal_handle_out, test_goals(0).handle);
        AffirmIfEqual(STATUS, goal_id, test_goals(0).goal_id);
        AffirmIfEqual(STATUS, std_logic_vector(to_unsigned(goal_stamp)), test_goals(0).stamp);
        AffirmIfEqual(STATUS, goal_state_out, test_goals(0).state);
        AlertIf(return_code /= ROS_RET_OK, "User operation return code incorrect", FAILURE);
        wait_on_idle;
        
        -- TEST: GET_LAST_GOAL [MEMORY NOT EMPTY]
        
        Log("GET_LAST_GOAL [Goal 5]", INFO);
        t    <= gen_duration(18 sec);
        user_op(GET_LAST_GOAL);
        wait for 1 ps; -- Make sure all signals stable
        AffirmIfEqual(STATUS, goal_handle_out, test_goals(3).handle);
        AffirmIfEqual(STATUS, goal_id, test_goals(3).goal_id);
        AffirmIfEqual(STATUS, std_logic_vector(to_unsigned(goal_stamp)), test_goals(3).stamp);
        AffirmIfEqual(STATUS, goal_state_out, test_goals(3).state);
        AlertIf(return_code /= ROS_RET_OK, "User operation return code incorrect", FAILURE);
        wait_on_idle;
        
        -- TEST: GET_PREVIOUS_GOAL [PREVIOUS GOAL DOES NOT EXISTS]
        
        Log("GET_PREVIOUS_GOAL [No Goal]", INFO);
        t    <= gen_duration(19 sec);
        goal_handle_in  <= test_goals(0).handle;
        user_op(GET_PREVIOUS_GOAL);
        wait for 1 ps; -- Make sure all signals stable
        AffirmIfEqual(STATUS, goal_handle_out, GOAL_HANDLE_UNKNOWN);
        AlertIf(return_code /= ROS_RET_OK, "User operation return code incorrect", FAILURE);
        wait_on_idle;
        
        -- TEST: GET_PREVIOUS_GOAL [PREVIOUS GOAL EXISTS]
        
        Log("GET_PREVIOUS_GOAL [Goal 4]", INFO);
        t    <= gen_duration(20 sec);
        goal_handle_in  <= test_goals(3).handle;
        user_op(GET_PREVIOUS_GOAL);
        wait for 1 ps; -- Make sure all signals stable
        AffirmIfEqual(STATUS, goal_handle_out, test_goals(2).handle);
        AffirmIfEqual(STATUS, goal_id, test_goals(2).goal_id);
        AffirmIfEqual(STATUS, std_logic_vector(to_unsigned(goal_stamp)), test_goals(2).stamp);
        AffirmIfEqual(STATUS, goal_state_out, test_goals(2).state);
        AlertIf(return_code /= ROS_RET_OK, "User operation return code incorrect", FAILURE);
        wait_on_idle;
        
        -- TEST: GET_NEXT_GOAL [NEXT GOAL DOES NOT EXISTS]
        
        Log("GET_NEXT_GOAL [No Goal]", INFO);
        t    <= gen_duration(21 sec);
        goal_handle_in  <= test_goals(3).handle;
        user_op(GET_NEXT_GOAL);
        wait for 1 ps; -- Make sure all signals stable
        AffirmIfEqual(STATUS, goal_handle_out, GOAL_HANDLE_UNKNOWN);
        AlertIf(return_code /= ROS_RET_OK, "User operation return code incorrect", FAILURE);
        wait_on_idle;
        
        -- TEST: GET_NEXT_GOAL [NEXT GOAL EXISTS]
        
        Log("GET_NEXT_GOAL [Goal 0]", INFO);
        t    <= gen_duration(22 sec);
        goal_handle_in  <= test_goals(0).handle;
        user_op(GET_NEXT_GOAL);
        wait for 1 ps; -- Make sure all signals stable
        AffirmIfEqual(STATUS, goal_handle_out, test_goals(1).handle);
        AffirmIfEqual(STATUS, goal_id, test_goals(1).goal_id);
        AffirmIfEqual(STATUS, std_logic_vector(to_unsigned(goal_stamp)), test_goals(1).stamp);
        AffirmIfEqual(STATUS, goal_state_out, test_goals(1).state);
        AlertIf(return_code /= ROS_RET_OK, "User operation return code incorrect", FAILURE);
        wait_on_idle;
        
        -- TEST: NEW RESULT REQUEST [GOAL DOES NOT EXIST]
        
        Log("New Result Request [Unknown Goal]", INFO);
        t    <= gen_duration(23 sec);
        get_result_request(2, gen_goal_id);
        get_result_response(5,2);
        wait_on_idle;
        
        -- TEST: NEW RESULT REQUEST [GOAL NOT TERMINAL]
        
        Log("New Result Request [Goal 1, Store]", INFO);
        t    <= gen_duration(24 sec);
        get_result_request(0, test_goals(0).goal_id);
        wait_on_idle;
        
        -- TEST: STORED RESULT REQUEST [GOAL STATE UPDATES]
        
        Log("UPDATE_GOAL_STATE [Goal 1, Executing]", INFO);
        t    <= gen_duration(25 sec);
        goal_handle_in  <= test_goals(0).handle;
        goal_state_in   <= STATUS_EXECUTING;
        user_op(UPDATE_GOAL_STATE);
        test_goals(0).state := STATUS_EXECUTING;
        wait for 1 ps; -- Make sure all signals stable
        AlertIf(return_code /= ROS_RET_OK, "User operation return code incorrect", FAILURE);
        wait_on_idle;
        
        data_available_g    <= '0';
        
        Log("UPDATE_GOAL_STATE [Goal 5, Succeeded]", INFO);
        t    <= gen_duration(26 sec);
        goal_handle_in  <= test_goals(3).handle;
        goal_state_in   <= STATUS_SUCCEEDED;
        user_op(UPDATE_GOAL_STATE);
        test_goals(3).state := STATUS_SUCCEEDED;
        wait for 1 ps; -- Make sure all signals stable
        AlertIf(return_code /= ROS_RET_OK, "User operation return code incorrect", FAILURE);
        wait_on_idle;
        
        -- TEST: NEW RESULT REQUEST [GOAL TERMINAL]
        
        Log("New Result Request [Goal 5]", INFO);
        t    <= gen_duration(27 sec);
        get_result_request(2, test_goals(3).goal_id);
        get_result_response(3,2);
        wait_on_idle;
        
        Log("New Result Request [Goal 3, Store]", INFO);
        t    <= gen_duration(28 sec);
        get_result_request(1, test_goals(1).goal_id);
        wait_on_idle;
        
        -- TEST: NEW RESULT REQUEST [FULL RESULT MEMORY]
        
        data_available_r    <= '1';
        t    <= gen_duration(29 sec);
        wait until rising_edge(clk);
        wait until rising_edge(clk);
        AlertIf(start_r = '1', "Result service operation on full memory", FAILURE);
        
        -- TEST: STORED RESULT REQUEST [GOAL STATE CHANGES TO TERMINAL]
        
        Log("UPDATE_GOAL_STATE [Goal 1, Aborted]", INFO);
        t    <= gen_duration(30 sec);
        goal_handle_in  <= test_goals(0).handle;
        goal_state_in   <= STATUS_ABORTED;
        user_op(UPDATE_GOAL_STATE);
        test_goals(0).state := STATUS_ABORTED;
        wait for 1 ps; -- Make sure all signals stable
        AlertIf(return_code /= ROS_RET_OK, "User operation return code incorrect", FAILURE);
        get_result_response(0,0);
        wait_on_idle;
        AlertIf(start_r /= '1', "No Result service operation on available memory", FAILURE);
        
        Log("New Result Request [Goal 4, Store]", INFO);
        t    <= gen_duration(31 sec);
        get_result_request(0, test_goals(2).goal_id);
        wait_on_idle;
        
        -- TEST: UPDATE_GOAL_STATE [INVALID STATE]
        
        Log("UPDATE_GOAL_STATE [Goal 1, Invalid State Transition]", INFO);
        t    <= gen_duration(32 sec);
        goal_handle_in  <= test_goals(0).handle;
        goal_state_in   <= STATUS_SUCCEEDED;
        user_op(UPDATE_GOAL_STATE);
        wait for 1 ps; -- Make sure all signals stable
        AlertIf(return_code /= ROS_RET_ACTION_GOAL_EVENT_INVALID, "User operation return code incorrect", FAILURE);
        wait_on_idle;
        
        -- TEST: CANCEL REQUEST [GOAL ID SET, GOAL DOES NOT EXISTS]
        
        Log("Cancel Request [Unknown Goal] (ERROR_REJECTED)", INFO);
        t    <= gen_duration(33 sec);
        test_cancel := (others => MAX_GOALS);
        get_cancel_request(gen_goal_id, (others => '0'));
        get_cancel_response(CancelGoal_package.RR_ERROR_UNKNOWN_GOAL_ID);
        wait_on_idle;
        
        -- TEST: CANCEL REQUEST [GOAL ID SET, GOAL EXISTS, GOAL TERMINAL]
        
        Log("Cancel Request [Goal 1] (ERROR_GOAL_TERMINATED)", INFO);
        t    <= gen_duration(34 sec);
        test_cancel := (others => MAX_GOALS);
        get_cancel_request(test_goals(0).goal_id, (others => '0'));
        get_cancel_response(CancelGoal_package.RR_ERROR_GOAL_TERMINATED);
        wait_on_idle;
        
        -- TEST: CANCEL REQUEST [GOAL ID SET, GOAL EXISTS]
        -- TEST: STATUS UPDATE [GOAL CANCELED]
        
        Log("Cancel Request [Goal 3] (Goal 3 Accept)", INFO);
        t    <= gen_duration(35 sec);
        test_cancel := (0 => 1, others => MAX_GOALS);
        get_cancel_request(test_goals(1).goal_id, (others => '0'));
        user_input_cancel(1,TRUE);
        get_cancel_response(CancelGoal_package.RR_ERROR_NONE);
        wait_on_idle;
        
        -- TEST: CANCEL REQUEST [NO GOAL ID SET, NO TIME SET, GOALS EXIST]
        
        Log("Cancel Request [ALL] (Goal 4 Accept)", INFO);
        t    <= gen_duration(36 sec);
        test_cancel := (0 => 2, 1 => 1, others => MAX_GOALS);
        get_cancel_request((others => '0'), (others => '0'));
        user_input_cancel(2,TRUE);
        get_cancel_response(CancelGoal_package.RR_ERROR_NONE);
        wait_on_idle;
        
        Log("UPDATE_GOAL_STATE [Goal 3, Canceled]", INFO);
        t    <= gen_duration(37 sec);
        goal_handle_in  <= test_goals(1).handle;
        goal_state_in   <= STATUS_CANCELED;
        user_op(UPDATE_GOAL_STATE);
        test_goals(1).state := STATUS_CANCELED;
        wait for 1 ps; -- Make sure all signals stable
        AlertIf(return_code /= ROS_RET_OK, "User operation return code incorrect", FAILURE);
        get_result_response(1,1);
        wait_on_idle;
        
        Log("UPDATE_GOAL_STATE [Goal 4, Abort]", INFO);
        t    <= gen_duration(38 sec);
        goal_handle_in  <= test_goals(2).handle;
        goal_state_in   <= STATUS_ABORTED;
        user_op(UPDATE_GOAL_STATE);
        test_goals(2).state := STATUS_ABORTED;
        wait for 1 ps; -- Make sure all signals stable
        AlertIf(return_code /= ROS_RET_OK, "User operation return code incorrect", FAILURE);
        get_result_response(2,0);
        wait_on_idle;
        
        -- TEST: NEW GOAL [FULL GOAL MEMORY, TIMEOUT=INFINITE, TERMINAL STATE GOAL EXISTS]
        -- TEST: STATUS UPDATE [GOAL REMOVED]
        
        Log("New Goal [Goal 6, Accept]", INFO);
        t    <= gen_duration(46 sec);
        test_goals(0)   := test_goals(1);
        test_goals(1)   := test_goals(2);
        test_goals(2)   := test_goals(3);
        test_goal_cnt   := test_goal_cnt - 1;
        get_goal_request(3, gen_goal_id);
        user_input_goal(3,TRUE);
        get_goal_response(3);
        wait_on_idle;
        
        -- TEST: GET_GOAL
        
        Log("GET_GOAL [Goal 6]", INFO);
        t    <= gen_duration(47 sec);
        goal_handle_in  <= test_goals(3).handle;
        user_op(GET_GOAL);
        wait for 1 ps; -- Make sure all signals stable
        AffirmIfEqual(STATUS, goal_id, test_goals(3).goal_id);
        AffirmIfEqual(STATUS, std_logic_vector(to_unsigned(goal_stamp)), test_goals(3).stamp);
        AffirmIfEqual(STATUS, goal_state_out, test_goals(3).state);
        AlertIf(return_code /= ROS_RET_OK, "User operation return code incorrect", FAILURE);
        wait_on_idle;
        
        -- TEST: CANCEL REQUEST [TIME SET, NO RELEVANT GOALS]
        
        Log("Cancel Request [Time Set] (NO GOALS)", INFO);
        t    <= gen_duration(48 sec);
        test_cancel := (others => MAX_GOALS);
        get_cancel_request((others => '0'), ros_gen_time(45 sec));
        get_cancel_response(CancelGoal_package.RR_ERROR_REJECTED);
        wait_on_idle;
        
        -- TEST: CANCEL REQUEST [TIME SET, RELEVANT GOALS EXIST]
        
        Log("Cancel Request [Time Set] (Goal 6 Accept)", INFO);
        t    <= gen_duration(61 sec);
        test_cancel := (0 => 3, others => MAX_GOALS);
        get_cancel_request((others => '0'), ros_gen_time(46 sec));
        user_input_cancel(3,TRUE);
        get_cancel_response(CancelGoal_package.RR_ERROR_NONE);
        wait_on_idle;
        
        Log("UPDATE_GOAL_STATE [Goal 6, Canceled]", INFO);
        t    <= gen_duration(62 sec);
        goal_handle_in  <= test_goals(3).handle;
        goal_state_in   <= STATUS_CANCELED;
        user_op(UPDATE_GOAL_STATE);
        test_goals(3).state := STATUS_CANCELED;
        wait for 1 ps; -- Make sure all signals stable
        AlertIf(return_code /= ROS_RET_OK, "User operation return code incorrect", FAILURE);
        wait_on_idle;
        
        -- TEST: RESULT REQUEST MEMORY [EMPTY->FULL->EMPTY->FULL]
        
        Log("New Goal [Goal 7, Accept]", INFO);
        t    <= gen_duration(83 sec);
        test_goals(0)   := test_goals(1);
        test_goals(1)   := test_goals(2);
        test_goals(2)   := test_goals(3);
        test_goal_cnt   := test_goal_cnt - 1;
        get_goal_request(3, gen_goal_id);
        user_input_goal(3, TRUE);
        get_goal_response(3);
        wait_on_idle;
        
        Log("New Goal [Goal 8, Accept]", INFO);
        t    <= gen_duration(84 sec);
        test_goals(0)   := test_goals(1);
        test_goals(1)   := test_goals(2);
        test_goals(2)   := test_goals(3);
        test_goal_cnt   := test_goal_cnt - 1;
        get_goal_request(3, gen_goal_id);
        user_input_goal(3, TRUE);
        get_goal_response(3);
        wait_on_idle;
        
        Log("New Goal [Goal 9, Accept]", INFO);
        t    <= gen_duration(85 sec);
        test_goals(0)   := test_goals(1);
        test_goals(1)   := test_goals(2);
        test_goals(2)   := test_goals(3);
        test_goal_cnt   := test_goal_cnt - 1;
        get_goal_request(3, gen_goal_id);
        user_input_goal(3, TRUE);
        get_goal_response(3);
        wait_on_idle;
        
        Log("New Goal [Goal 10, Accept]", INFO);
        t    <= gen_duration(86 sec);
        test_goals(0)   := test_goals(1);
        test_goals(1)   := test_goals(2);
        test_goals(2)   := test_goals(3);
        test_goal_cnt   := test_goal_cnt - 1;
        get_goal_request(3, gen_goal_id);
        user_input_goal(3, TRUE);
        get_goal_response(3);
        wait_on_idle;
        
        -- TEST: GOAL MEMORY [EMPTY->FULL->EMPTY->FULL]
        
        Log("New Result Request [Goal 8, Store]", INFO);
        t    <= gen_duration(87 sec);
        get_result_request(0, test_goals(1).goal_id);
        wait_on_idle;
        
        Log("New Result Request [Goal 8, Store]", INFO);
        t    <= gen_duration(88 sec);
        get_result_request(1, test_goals(1).goal_id);
        wait_on_idle;
        
        -- TEST: CANCEL REQUEST [GOAL ID SET, TIME SET, GOAL ID IS NOT IN TIME RANGE]
        
        Log("Cancel Request [Goal Set, Time Set] (Goal 10 Reject, Goal 8 Reject, Goal 7 Accept)", INFO);
        t    <= gen_duration(89 sec);
        test_cancel := (0 => 0, others => MAX_GOALS);
        get_cancel_request(test_goals(3).goal_id, ros_gen_time(84 sec));
        user_input_cancel(3,FALSE);
        user_input_cancel(1,FALSE);
        user_input_cancel(0,TRUE);
        get_cancel_response(CancelGoal_package.RR_ERROR_NONE);
        wait_on_idle;
        
        -- TEST: CANCEL REQUEST [GOAL ID SET, TIME SET, GOAL ID IS IN TIME RANGE]
        
        Log("Cancel Request [Goal Set, Time Set] (Goal 9 Reject, Goal 8 Accept)", INFO);
        t    <= gen_duration(90 sec);
        test_cancel := (0 => 1, 1 => 0, others => MAX_GOALS);
        get_cancel_request(test_goals(2).goal_id, ros_gen_time(86 sec));
        user_input_cancel(2,FALSE);
        user_input_cancel(3,FALSE);
        user_input_cancel(2,FALSE);
        user_input_cancel(1,TRUE);
        get_cancel_response(CancelGoal_package.RR_ERROR_NONE);
        wait_on_idle;
        
        Log("UPDATE_GOAL_STATE [Goal 7, Abort]", INFO);
        t    <= gen_duration(91 sec);
        goal_handle_in  <= test_goals(0).handle;
        goal_state_in   <= STATUS_ABORTED;
        user_op(UPDATE_GOAL_STATE);
        test_goals(0).state := STATUS_ABORTED;
        wait for 1 ps; -- Make sure all signals stable
        AlertIf(return_code /= ROS_RET_OK, "User operation return code incorrect", FAILURE);
        wait_on_idle;
        
        -- TEST: STORED RESULT REQUEST [MULTIPLE REQUESTS, SAME GOAL, GOAL STATE CHANGES TO TERMINAL]
        
        Log("UPDATE_GOAL_STATE [Goal 8, Succeeded]", INFO);
        t    <= gen_duration(92 sec);
        goal_handle_in  <= test_goals(1).handle;
        goal_state_in   <= STATUS_SUCCEEDED;
        user_op(UPDATE_GOAL_STATE);
        test_goals(1).state := STATUS_SUCCEEDED;
        wait for 1 ps; -- Make sure all signals stable
        AlertIf(return_code /= ROS_RET_OK, "User operation return code incorrect", FAILURE);
        get_result_response(1,1);
        get_result_response(1,0);
        wait_on_idle;
        
        -- TEST: PUBLISH_FEEDBACK
        
        Log("FEEDBACK [Goal 9]", INFO);
        t    <= gen_duration(93 sec);
        goal_handle_in  <= test_goals(2).handle;
        user_fb_op(test_goals(2).goal_id);
        
        TranscriptOpen(RESULTS_FILE, APPEND_MODE);
        SetTranscriptMirror;
        ReportAlerts;
        TranscriptClose;
        std.env.stop;
        wait;
    end process;
    
    status_check_prc : process (all)
        variable cnt : natural := 0;
    begin
        if rising_edge(clk) then
            if (reset = '1') then
                status_stage        <= IDLE;
                cnt                 := 0;
            else
                case(status_stage) is
                    when IDLE =>
                        if (start_s = '1') then
                            AlertIf(opcode_s /= PUBLISH, "Status publisher opcode incorrect", FAILURE);
                            AffirmIfEqual(STATUS, status_list_len, int(test_goal_cnt,status_list_len'length));
                            cnt := 0;
                            if (test_goal_cnt = 0) then
                                status_stage    <= DONE_STATUS;
                            else
                                status_stage    <= CHECK_1;
                            end if;
                        end if;
                    when CHECK_1 =>
                        if (status_list_ready = '1') then
                            status_stage    <= CHECK_2;
                        end if;
                    when CHECK_2 =>
                        if (status_list_valid = '1') then
                            AffirmIfEqual(STATUS, status_list_goal_info_goal_id_r, test_goals(test_goal_cnt-1-cnt).goal_id);
                            AffirmIfEqual(STATUS, status_list_goal_info_stamp_r, test_goals(test_goal_cnt-1-cnt).stamp);
                            AffirmIfEqual(STATUS, status_list_status_r, test_goals(test_goal_cnt-1-cnt).state);
                            if (cnt = test_goal_cnt-1) then
                                status_stage    <= DONE_STATUS;
                            else
                                cnt             := cnt + 1;
                                status_stage    <= CHECK_1;
                            end if;
                        end if;
                    when DONE_STATUS =>
                        status_stage    <= IDLE;
                end case;
            end if;
        end if;
        
        -- DEFAULT
        status_list_addr_u  <= (others => '0');
        status_list_ren     <= '0';
        status_list_ack     <= '0';
        ack_s               <= '0';
        done_s              <= '0';
        selector_s          <= '0';
        return_code_s       <= RETCODE_ERROR;
        
        case(status_stage) is
            when IDLE =>
                if (start_s = '1') then
                    ack_s           <= '1';
                end if;
            when CHECK_1 =>
                selector_s          <= '1';
                status_list_addr_u  <= int(cnt,status_list_addr'length);
                status_list_ren     <= '1';
            when CHECK_2 =>
                selector_s          <= '1';
                if (status_list_valid = '1') then
                    status_list_ack <= '1';
                end if;
            when DONE_STATUS =>
                done_s          <= '1';
                return_code_s   <= RETCODE_OK;
            when others =>
                null;
        end case;
    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;