-- altera vhdl_input_version vhdl_2008
-- XXX: QSYS Fix (https://www.intel.com/content/www/us/en/support/programmable/articles/000079458.html)

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

use work.math_pkg.all;
use work.rtps_package.all;
use work.rtps_config_package.all;
use work.ros_package.all;
use work.Fibonacci_package.all;
use work.GoalStatus_package;
use work.CancelGoal_package;
use work.GoalStatusArray_package;

entity ros_action_server is
    generic (
        TIMEOUT_DURATION    : ROS_DURATION_TYPE;
        MAX_GOALS           : natural;
        MAX_RESULT_REQUESTS : natural
    );
    port (
        -- SYSTEM
        clk                                 : in std_logic;
        reset                               : in std_logic;
        time                                : in ROS_TIME_TYPE;
        -- *GOAL SERVICE*
        start_g                             : out std_logic;
        ack_g                               : in std_logic;
        opcode_g                            : out ROS_SERVICE_OPCODE_TYPE;
        service_info_g                      : in SERVICE_INFO_TYPE;
        request_id_g                        : out REQUEST_ID_TYPE;
        data_available_g                    : in std_logic;
        taken_g                             : in std_logic;
        done_g                              : in std_logic;
        return_code_g                       : in std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0);
        -- REQUEST
        goal_id_g                           : in std_logic_vector(UUID_WIDTH-1 downto 0);
        -- RESPONSE
        accepted_g                          : out std_logic;
        stamp_g                             : out std_logic_vector(ROS_TIME_WIDTH-1 downto 0);
        -- *RESULT SERVICE*
        start_r                             : out std_logic;
        ack_r                               : in std_logic;
        opcode_r                            : out ROS_SERVICE_OPCODE_TYPE;
        service_info_r                      : in SERVICE_INFO_TYPE;
        request_id_r                        : out REQUEST_ID_TYPE;
        data_available_r                    : in std_logic;
        taken_r                             : in std_logic;
        done_r                              : in std_logic;
        return_code_r                       : in std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0);
        result_index                        : out std_logic_vector(WORD_WIDTH-1 downto 0);
        result_sel                          : out std_logic;
        result_sel_ack                      : in std_logic;
        -- REQUEST
        goal_id_r                           : in std_logic_vector(UUID_WIDTH-1 downto 0);
        -- RESPONSE
        status_r                            : out std_logic_vector(CDR_INT8_WIDTH-1 downto 0);
        -- *CANCEL SERVICE*
        start_c                             : out std_logic;
        ack_c                               : in std_logic;
        opcode_c                            : out ROS_SERVICE_OPCODE_TYPE;
        service_info_c                      : in SERVICE_INFO_TYPE;
        request_id_c                        : out REQUEST_ID_TYPE;
        data_available_c                    : in std_logic;
        taken_c                             : in std_logic;
        done_c                              : in std_logic;
        return_code_c                       : in std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0);
        -- REQUEST
        goal_info_goal_id_c                 : in std_logic_vector(UUID_WIDTH-1 downto 0);
        goal_info_stamp_c                   : in std_logic_vector(ROS_TIME_WIDTH-1 downto 0);
        -- RESPONSE
        cancel_return_code_c                : out std_logic_vector(CDR_INT8_WIDTH-1 downto 0);
        goals_canceling_len_c               : out std_logic_vector(CancelGoal_package.RR_GOALS_CANCELING_ADDR_WIDTH-1 downto 0);
        goals_canceling_addr_c              : out std_logic_vector(CancelGoal_package.RR_GOALS_CANCELING_ADDR_WIDTH-1 downto 0);
        goals_canceling_ready_c             : in std_logic;
        goals_canceling_ren_c               : out std_logic;
        goals_canceling_wen_c               : out std_logic;
        goals_canceling_valid_c             : in std_logic;
        goals_canceling_ack_c               : out std_logic;
        goals_canceling_goal_id_r_c         : in std_logic_vector(UUID_WIDTH-1 downto 0);
        goals_canceling_goal_id_w_c         : out std_logic_vector(UUID_WIDTH-1 downto 0);
        goals_canceling_stamp_r_c           : in std_logic_vector(ROS_TIME_WIDTH-1 downto 0);
        goals_canceling_stamp_w_c           : out std_logic_vector(ROS_TIME_WIDTH-1 downto 0);
        -- *FEEDBACK TOPIC*
        start_fb                            : out std_logic;
        opcode_fb                           : out ROS_TOPIC_OPCODE_TYPE;
        ack_fb                              : in std_logic;
        done_fb                             : in std_logic;
        return_code_fb                      : in std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0);
        goal_id_fb                          : out std_logic_vector(UUID_WIDTH-1 downto 0);
        -- *STATUS TOPIC*
        start_s                             : out std_logic;
        opcode_s                            : out ROS_TOPIC_OPCODE_TYPE;
        ack_s                               : in std_logic;
        done_s                              : in std_logic;
        return_code_s                       : in std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0);
        status_list_len_s                   : out std_logic_vector(GoalStatusArray_package.STATUS_LIST_ADDR_WIDTH-1 downto 0);
        status_list_addr_s                  : out std_logic_vector(GoalStatusArray_package.STATUS_LIST_ADDR_WIDTH-1 downto 0);
        status_list_ready_s                 : in std_logic;
        status_list_ren_s                   : out std_logic;
        status_list_wen_s                   : out std_logic;
        status_list_valid_s                 : in std_logic;
        status_list_ack_s                   : out std_logic;
        status_list_goal_info_goal_id_r_s   : in std_logic_vector(UUID_WIDTH-1 downto 0);
        status_list_goal_info_goal_id_w_s   : out std_logic_vector(UUID_WIDTH-1 downto 0);
        status_list_goal_info_stamp_r_s     : in std_logic_vector(ROS_TIME_WIDTH-1 downto 0);
        status_list_goal_info_stamp_w_s     : out std_logic_vector(ROS_TIME_WIDTH-1 downto 0);
        status_list_status_r_s              : in std_logic_vector(CDR_INT8_WIDTH-1 downto 0);
        status_list_status_w_s              : out std_logic_vector(CDR_INT8_WIDTH-1 downto 0);
        -- *USER*
        start                               : in std_logic;
        opcode                              : in ROS_ACTION_OPCODE_TYPE;
        ack                                 : out std_logic;
        done                                : out std_logic;
        return_code                         : out std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0);
        goal_handle_in                      : in std_logic_vector(GOAL_HANDLE_WIDTH-1 downto 0);
        goal_handle_out                     : out std_logic_vector(GOAL_HANDLE_WIDTH-1 downto 0);
        goal_state_in                       : in std_logic_vector(CDR_INT8_WIDTH-1 downto 0);
        goal_state_out                      : out std_logic_vector(CDR_INT8_WIDTH-1 downto 0);
        goal_id                             : out std_logic_vector(UUID_WIDTH-1 downto 0);
        goal_result_index                   : out std_logic_vector(WORD_WIDTH-1 downto 0);
        goal_stamp                          : out ROS_TIME_TYPE;
        -- GOAL
        new_goal_request                    : out std_logic;
        new_goal_handle                     : out std_logic_vector(GOAL_HANDLE_WIDTH-1 downto 0);
        new_goal_result_index               : out std_logic_vector(WORD_WIDTH-1 downto 0);
        new_goal_accepted                   : in std_logic;
        new_goal_response                   : in std_logic;
        -- CANCEL
        cancel_request                      : out std_logic;
        cancel_request_handle               : out std_logic_vector(GOAL_HANDLE_WIDTH-1 downto 0);
        cancel_accepted                     : in std_logic;
        cancel_response                     : in std_logic
    );
end entity;

architecture arch of ros_action_server is
    
    --*****CONSTANT DECLARATION*****
    -- *GOAL MEMORY*
    -- 4-Byte Word Size of a Goal Entry in Memory
    function gen_goal_frame_size(TIMEOUT_DURATION : ROS_DURATION_TYPE) return natural is
    begin
        if (TIMEOUT_DURATION /= ROS_DURATION_INFINITE) then
            return 12;
        else
            return 10;
        end if;
    end function;
    constant GOAL_FRAME_SIZE            : natural := gen_goal_frame_size(TIMEOUT_DURATION);
    -- Goal Memory Size in 4-Byte Words
    constant GOAL_MEMORY_SIZE           : natural := MAX_GOALS * GOAL_FRAME_SIZE;
    -- Goal Memory Address Width
    constant GOAL_MEMORY_ADDR_WIDTH     : natural := log2c(GOAL_MEMORY_SIZE);
    -- Highest Goal Memory Address
    constant GOAL_MEMORY_MAX_ADDRESS    : unsigned(GOAL_MEMORY_ADDR_WIDTH-1 downto 0) := to_unsigned(GOAL_MEMORY_SIZE-1, GOAL_MEMORY_ADDR_WIDTH);
    -- Highest Goal Frame Address
    constant MAX_GOAL_ADDRESS           : unsigned(GOAL_MEMORY_ADDR_WIDTH-1 downto 0) := GOAL_MEMORY_MAX_ADDRESS - GOAL_FRAME_SIZE + 1;
    -- Address pointing to the beginning of the first Goal Data Frame
    constant FIRST_GOAL_ADDRESS         : unsigned(GOAL_MEMORY_ADDR_WIDTH-1 downto 0) := (others => '0');
    -- *RESULT REQUEST MEMORY*
    -- 4-Byte Word Size of a Result Request Entry in Memory
    constant RRQ_FRAME_SIZE         : natural := 9;
    -- Result Request Memory Size in 4-Byte Words
    constant RRQ_MEMORY_SIZE        : natural := MAX_RESULT_REQUESTS * RRQ_FRAME_SIZE;
    -- Result Request Memory Address Width
    constant RRQ_MEMORY_ADDR_WIDTH  : natural := log2c(RRQ_MEMORY_SIZE);
    -- Highest Result Request Memory Address
    constant RRQ_MEMORY_MAX_ADDRESS : unsigned(RRQ_MEMORY_ADDR_WIDTH-1 downto 0) := to_unsigned(RRQ_MEMORY_SIZE-1, RRQ_MEMORY_ADDR_WIDTH);
    -- Highest Result Request Frame Address
    constant MAX_RRQ_ADDRESS        : unsigned(RRQ_MEMORY_ADDR_WIDTH-1 downto 0) := RRQ_MEMORY_MAX_ADDRESS - RRQ_FRAME_SIZE + 1;
    -- Address pointing to the beginning of the first Result Request Data Frame
    constant FIRST_RRQ_ADDRESS      : unsigned(RRQ_MEMORY_ADDR_WIDTH-1 downto 0) := (others => '0');
    
    -- *GOAL MEMORY FRAME FIELD OFFSETS*
    -- 4-Byte Word Offsets to Beginning of Respective Fields in the Goal Memory Frame
    constant GMF_STATE_OFFSET           : natural := 0;
    constant GMF_GOAL_ID_OFFSET         : natural := 1;
    constant GMF_STAMP_OFFSET           : natural := 5;
    constant GMF_DEADLINE_OFFSET        : natural := 7;
    function gen_gmf_result_index_offset(TIMEOUT_DURATION : ROS_DURATION_TYPE) return natural is
        variable ret : natural := 0;
    begin
        if (TIMEOUT_DURATION = ROS_DURATION_INFINITE) then
            ret := GMF_DEADLINE_OFFSET + 2;
        else
            ret := GMF_DEADLINE_OFFSET;
        end if;
        return ret;
    end function;
    constant GMF_RESULT_INDEX_OFFSET    : natural := gen_gmf_result_index_offset(TIMEOUT_DURATION);
    constant GMF_NEXT_ADDR_OFFSET       : natural := GMF_RESULT_INDEX_OFFSET + 1;
    constant GMF_PREV_ADDR_OFFSET       : natural := GMF_NEXT_ADDR_OFFSET + 1;
    -- *RESULT REQUEST MEMORY FRAME FIELD OFFSETS*
    -- 4-Byte Word Offsets to Beginning of Respective Fields in the Result Request Memory Frame
    constant RMF_GOAL_HANDLE_OFFSET     : natural := 0;
    constant RMF_REQUEST_ID_OFFSET      : natural := 1;
    constant RMF_NEXT_ADDR_OFFSET       : natural := 7;
    constant RMF_PREV_ADDR_OFFSET       : natural := 8;
    
    -- *GOAL MEMORY FRAME FIELD FLAGS*
    -- Flags mapping to the respective Goal Memory Frame Fields
    constant GMF_FLAG_WIDTH         : natural := 7;
    constant GMF_STATE_FLAG         : std_logic_vector(0 to GMF_FLAG_WIDTH-1) := (0 => '1', others => '0');
    constant GMF_GOAL_ID_FLAG       : std_logic_vector(0 to GMF_FLAG_WIDTH-1) := (1 => '1', others => '0');
    constant GMF_STAMP_FLAG         : std_logic_vector(0 to GMF_FLAG_WIDTH-1) := (2 => '1', others => '0');
    constant GMF_DEADLINE_FLAG      : std_logic_vector(0 to GMF_FLAG_WIDTH-1) := (3 => '1', others => '0');
    constant GMF_RESULT_INDEX_FLAG  : std_logic_vector(0 to GMF_FLAG_WIDTH-1) := (4 => '1', others => '0');
    constant GMF_NEXT_ADDR_FLAG     : std_logic_vector(0 to GMF_FLAG_WIDTH-1) := (5 => '1', others => '0');
    constant GMF_PREV_ADDR_FLAG     : std_logic_vector(0 to GMF_FLAG_WIDTH-1) := (6 => '1', others => '0');
    
    --*****TYPE DECLARATION*****
    -- *Cancel Process Search Types*
    -- SEARCH_GOAL_ID   Search goal list for a specific goal id
    -- SEARCH_STAMP     Search goal list for first goal that was accepted at or before a provided timestamp
    -- SEARCH_NONE      No search criteria. All goals are valid candites (Applies from current list position)
    type SEARCH_TYPE is (SEARCH_GOAL_ID, SEARCH_STAMP, SEARCH_NONE);
    -- *Instance Memory Opcodes*
    -- OPCODE       DESCRIPTION
    -- INSERT       Insert goal into memory
    -- GET          Get data of goal pointed by "GOAL_DATA_TYPE.addr" according to "GOAL_DATA_TYPE.field_flags".
    --              Already fetched data of the goal is not modified.
    -- GET_NEXT     Get goal data of next goal (from the goal pointed by "GOAL_DATA_TYPE.addr") according to "GOAL_DATA_TYPE.field_flags".
    --              Set "GOAL_DATA_TYPE.addr" to address of goal or GOAL_MEMORY_MAX_ADDRESS if no other goal in Memory.
    -- GET_PREV     Get goal data of previous goal (from the goal pointed by "GOAL_DATA_TYPE.addr") according to "GOAL_DATA_TYPE.field_flags".
    --              Set "GOAL_DATA_TYPE.addr" to address of goal or GOAL_MEMORY_MAX_ADDRESS if no other goal in Memory.
    -- UPDATE       Update goal data pointed by "GOAL_DATA_TYPE.addr" according to "GOAL_DATA_TYPE.field_flags"
    -- REMOVE       Remove goal pointed by "GOAL_DATA_TYPE.addr"
    type MEM_OPCODE_TYPE is (NOP, INSERT, GET, GET_NEXT, GET_PREV, UPDATE, REMOVE);
    type STAGE_TYPE is (IDLE,WAIT_FOR_DATA,SEARCH_ID,CHECK_STATE,STORE_RRQ,SEND_RESPONSE,WAIT_FOR_RET,TRANSITION_STATE,GET_DATA,CHECK_REQUESTS,GET_REQUEST_ID,REMOVE_REQUEST,WAIT_FOR_MEM,RETURN_USER,CHECK_TIMEOUT,REMOVE_OLDEST,PUBLISH_FEEDBACK,RESET_MEMORY);
    type STATUS_STAGE_TYPE is (IDLE,GET_FIRST,GET_NEXT,WAIT_FOR_DATA,PUSH_STATUS,PUBLISH,WAIT_FOR_PUBLISHER);
    type GOAL_STAGE_TYPE is (IDLE,WAIT_FOR_DATA,WAIT_FOR_USER,ADD_GOAL,SEND_RESPONSE,WAIT_FOR_RET);
    type CANCEL_STAGE_TYPE is (IDLE,WAIT_FOR_DATA,GET,GET_NEXT,CHECK,CHECK_GOAL_ID,CHECK_STAMP,CHECK_STATE,WAIT_FOR_USER,CANCEL_GOAL,ADD_CANCEL,SEND_RESPONSE,WAIT_FOR_RET);
    type MEM_STAGE_TYPE is (IDLE,INSERT,GET,GET_NEXT,GET_PREV,UPDATE,REMOVE,RESET_MEMORY);
    type GOAL_DATA_TYPE is record
        addr        : unsigned(GOAL_MEMORY_ADDR_WIDTH-1 downto 0);
        goal_id     : GUID_TYPE;
        stamp       : ROS_TIME_TYPE;
        deadline    : ROS_DURATION_TYPE;
        res_ind     : std_logic_vector(WORD_WIDTH-1 downto 0);
        state       : std_logic_vector(CDR_INT8_WIDTH-1 downto 0);
        field_flags : std_logic_vector(0 to GMF_FLAG_WIDTH-1);
    end record;
    constant ZERO_GOAL_DATA : GOAL_DATA_TYPE := (
        addr        => (others => '0'),
        goal_id     => GUID_UNKNOWN,
        stamp       => ROS_TIME_ZERO,
        deadline    => ROS_DURATION_INFINITE,
        res_ind     => (others => '0'),
        state       => GoalStatus_package.STATUS_UNKNOWN,
        field_flags => (others => '0')
    );
    
    --*****SIGNAL DECLARATION*****
    -- *GOAL MEMORY CONNECTION SIGNALS*
    signal mem_addr : unsigned(GOAL_MEMORY_ADDR_WIDTH-1 downto 0);
    signal mem_read : std_logic;
    signal mem_read_data, mem_write_data : std_logic_vector(WORD_WIDTH-1 downto 0);
    signal mem_ready_in, mem_valid_in : std_logic;
    signal mem_ready_out, mem_valid_out : std_logic;
    signal mem_abort_read : std_logic;
    -- *RESULT REQUEST MEMORY CONNECTION SIGNALS*
    signal rrq_addr : unsigned(RRQ_MEMORY_ADDR_WIDTH-1 downto 0);
    signal rrq_read : std_logic;
    signal rrq_read_data, rrq_write_data : std_logic_vector(WORD_WIDTH-1 downto 0);
    signal rrq_ready_in, rrq_valid_in : std_logic;
    signal rrq_ready_out, rrq_valid_out : std_logic;
    signal rrq_abort_read : std_logic;
    
    -- *MAIN PROCESS*
    -- FSM state
    signal stage, stage_next : STAGE_TYPE;
    -- Request memory operation
    signal mem_start_r : std_logic;
    -- Opcode of goal memory operation (valid only if mem_start_r is high)
    signal mem_opcode_r : MEM_OPCODE_TYPE;
    -- Signal used to pass data to Goal Memory Process
    signal mem_r : GOAL_DATA_TYPE;
    -- Goal memory data latch
    signal mem_data_r, mem_data_r_next : GOAL_DATA_TYPE;
    -- Head of occupied result requests list
    signal rrq_occupied_head, rrq_occupied_head_next : unsigned(RRQ_MEMORY_ADDR_WIDTH-1 downto 0);
    -- Head of empty result requests list
    signal rrq_empty_head, rrq_empty_head_next : unsigned(RRQ_MEMORY_ADDR_WIDTH-1 downto 0);
    -- General purpose counter
    signal cnt, cnt_next : natural range 0 to 11;
    -- Pointer to current relevant Result Request Memory Frame
    signal rrq_addr_base, rrq_addr_base_next : unsigned(RRQ_MEMORY_ADDR_WIDTH-1 downto 0);
    -- Result Request Memory Frame pointer latch
    signal rrq_addr_latch, rrq_addr_latch_next : unsigned(RRQ_MEMORY_ADDR_WIDTH-1 downto 0);
    -- Count of stored goals that are in a terminal state
    signal terminal_cnt, terminal_cnt_next : natural range 0 to MAX_GOALS;
    -- User return code latch
    signal return_code_latch, return_code_latch_next : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0);
    -- Toggle latch used to trigger stored result request search when a goal is updated to a terminal state
    signal trigger_result, trigger_result_next : std_logic;
    -- Signal used to signal goal status change (Used by status process to generate new goal status list)
    signal trigger_status_r : std_logic;
    -- Result service Request ID latch
    signal request_id_latch, request_id_latch_next : REQUEST_ID_TYPE;
    -- Time of next goal expiration dealine
    signal check_time, check_time_next : ROS_TIME_TYPE;
    -- Temporal time latch (used in time addition calculation)
    signal time_latch, time_latch_next : ROS_TIME_TYPE;
    -- Signals that the main FSM has entered the atomic operation range (Used mutex lock between main and cancel FSMs)
    signal atomic_op_r : std_logic;
    -- Goal handle latch
    signal goal_handle_latch, goal_handle_latch_next : std_logic_vector(GOAL_HANDLE_WIDTH-1 downto 0);
    -- Goal state latch
    signal goal_state_latch, goal_state_latch_next  :std_logic_vector(CDR_INT8_WIDTH-1 downto 0);
    
    -- *GOAL PROCESS*
    -- Goal Process FSM state
    signal goal_stage, goal_stage_next : GOAL_STAGE_TYPE;
    -- Request memory operation
    signal mem_start_g : std_logic;
    -- Opcode of memory operation (valid only if mem_start_g is high)
    signal mem_opcode_g : MEM_OPCODE_TYPE;
    -- Goal timestamp latch
    signal g_stamp, g_stamp_next : ROS_TIME_TYPE;
    -- Goal accept latch
    signal g_accept, g_accept_next : std_logic;
    -- Signal used to signal goal status change (Used by status process to generate new goal status list)
    signal trigger_status_g : std_logic;
    
    -- *CANCEL PROCESS*
    -- Cancel process FSM state
    signal cancel_stage, cancel_stage_next : CANCEL_STAGE_TYPE;
    -- Request memory operation
    signal mem_start_c : std_logic;
    -- Opcode of memory operation (valid only if mem_start_c is high)
    signal mem_opcode_c : MEM_OPCODE_TYPE;
    -- Signal used to pass data to Goal Memory Process
    signal mem_c : GOAL_DATA_TYPE;
    -- General purpose counter
    signal c_cnt, c_cnt_next : natural range 0 to 3;
    -- Goal memory data latch
    signal mem_data_c, mem_data_c_next : GOAL_DATA_TYPE;
    -- Signals the kind of search the cancel FSM is doing (see SEARCH_TYPE definition)
    signal search_type_c, search_type_c_next : SEARCH_TYPE;
    -- Index of goals_canceling sequence
    signal goals_canceling_cnt, goals_canceling_cnt_next : natural range 0 to CancelGoal_package.RR_GOALS_CANCELING_MAX_DEPTH-1;
    -- Cancel response return code latch
    signal cancel_ret_code, cancel_ret_code_next : std_logic_vector(CDR_INT8_WIDTH-1 downto 0);
    -- Signal used to signal goal status change (Used by status process to generate new goal status list)
    signal trigger_status_c : std_logic;
    -- Signals that the cancel FSM has entered the atomic operation range (Used mutex lock between main and cancel FSMs)
    signal atomic_op_c: std_logic;
    
    -- *STATUS PROCESS*
    -- Status process FSM
    signal status_stage, status_stage_next : STATUS_STAGE_TYPE;
    -- Toggle latch that latches status update requests from other processes
    signal trigger_status, trigger_status_next : std_logic;
    -- Index of statusl list sequence
    signal status_list_cnt, status_list_cnt_next : natural range 0 to MAX_GOALS-1;
    -- Request memory operation
    signal mem_start_s : std_logic;
    -- Opcode of memory operation (valid only if mem_start_s is high)
    signal mem_opcode_s : MEM_OPCODE_TYPE;
    -- Signal used to pass data to Goal Memory Process
    signal mem_s : GOAL_DATA_TYPE;
    -- Goal memory data latch
    signal mem_data_s, mem_data_s_next : GOAL_DATA_TYPE;
    -- Test signal used for testbench synchronisation
    signal status_idle_sig : std_logic;
    
    -- *GOAL MEMORY PROCESS*
    -- Memory process FSM state
    signal mem_stage, mem_stage_next : MEM_STAGE_TYPE;
    -- Acknoledge memoryrequest from respective process
    signal mem_ack_r, mem_ack_s, mem_ack_c, mem_ack_g : std_logic;
    -- Head of occupied goal list
    signal mem_occupied_head, mem_occupied_head_next : unsigned(GOAL_MEMORY_ADDR_WIDTH-1 downto 0);
    -- Tail of occupied goal list
    signal mem_occupied_tail, mem_occupied_tail_next : unsigned(GOAL_MEMORY_ADDR_WIDTH-1 downto 0);
    -- Head of empty goal list
    signal mem_empty_head, mem_empty_head_next : unsigned(GOAL_MEMORY_ADDR_WIDTH-1 downto 0);
    -- Tail of empty goal list
    signal mem_empty_tail, mem_empty_tail_next : unsigned(GOAL_MEMORY_ADDR_WIDTH-1 downto 0);
    -- General purpose counter
    signal mem_cnt, mem_cnt_next : natural range 0 to 19;
    -- Pointer to currently relevant goal memory frame
    signal mem_addr_base, mem_addr_base_next : unsigned(GOAL_MEMORY_ADDR_WIDTH-1 downto 0);
    -- Goal memory fram pointer latch
    signal mem_addr_latch, mem_addr_latch_next : unsigned(GOAL_MEMORY_ADDR_WIDTH-1 downto 0);
    -- Result index of current empty head goal
    signal empty_head_res_ind, empty_head_res_ind_next : unsigned(WORD_WIDTH-1 downto 0);
    -- Latch for data from processes
    signal mem_latch_data, mem_latch_data_next : GOAL_DATA_TYPE;
    -- Latch for goal memory data
    signal mem_data, mem_data_next : GOAL_DATA_TYPE;
    -- Signals end of memory operation (And that the memory process is in the IDLE state)
    signal mem_done : std_logic;
    
    -- *FUNCTION DECLARATION*
    -- Returns true if the state is terminal
    function is_terminal (state : std_logic_vector) return boolean is
    begin
        assert (state'length = CDR_INT8_WIDTH) severity FAILURE;
        
        case (state) is
            when GoalStatus_package.STATUS_SUCCEEDED =>
                return TRUE;
            when GoalStatus_package.STATUS_CANCELED =>
                return TRUE;
            when GoalStatus_package.STATUS_ABORTED =>
                return TRUE;
            when others =>
                return FALSE;
        end case;
    end function;
    
begin
    
    goal_mem_ctrl_inst : entity work.mem_ctrl(arch)
        generic map (
            ADDR_WIDTH          => GOAL_MEMORY_ADDR_WIDTH,
            DATA_WIDTH          => WORD_WIDTH,
            MEMORY_DEPTH        => GOAL_MEMORY_SIZE,
            MAX_BURST_LENGTH    => GOAL_FRAME_SIZE
        )
        port map (
            clk         => clk,
            reset       => reset or mem_abort_read,
            addr        => std_logic_vector(mem_addr),
            read        => mem_read,
            ready_in    => mem_ready_in,
            valid_in    => mem_valid_in,
            data_in     => mem_write_data,
            ready_out   => mem_ready_out,
            valid_out   => mem_valid_out,
            data_out    => mem_read_data
        );
    
    rrq_mem_ctrl_inst : entity work.mem_ctrl(arch)
        generic map (
            ADDR_WIDTH          => RRQ_MEMORY_ADDR_WIDTH,
            DATA_WIDTH          => WORD_WIDTH,
            MEMORY_DEPTH        => RRQ_MEMORY_SIZE,
            MAX_BURST_LENGTH    => RRQ_FRAME_SIZE
        )
        port map (
            clk         => clk,
            reset       => reset or rrq_abort_read,
            addr        => std_logic_vector(rrq_addr),
            read        => rrq_read,
            ready_in    => rrq_ready_in,
            valid_in    => rrq_valid_in,
            data_in     => rrq_write_data,
            ready_out   => rrq_ready_out,
            valid_out   => rrq_valid_out,
            data_out    => rrq_read_data
        );
    
    result_index            <= mem_data_r.res_ind;
    status_r                <= mem_data_r.state;
    request_id_r            <= request_id_latch;
    goal_id_fb              <= std_logic_vector(to_unsigned(mem_data_r.goal_id));
    goal_id                 <= std_logic_vector(to_unsigned(mem_data_r.goal_id));
    goal_stamp              <= mem_data_r.stamp;
    goal_state_out          <= mem_data_r.state;
    goal_result_index       <= mem_data_r.res_ind;
    request_id_g            <= service_info_g.request_id;
    accepted_g              <= g_accept;
    stamp_g                 <= std_logic_vector(to_unsigned(g_stamp));
    request_id_c            <= service_info_c.request_id;
    cancel_return_code_c    <= cancel_ret_code;
    goals_canceling_len_c   <= std_logic_vector(to_unsigned(goals_canceling_cnt,goals_canceling_len_c'length));
    status_list_len_s       <= std_logic_vector(to_unsigned(status_list_cnt,status_list_len_s'length));
    
    -- *Main State Machine*
    -- STATE                DESCRIPTION
    -- IDLE                 Idle state
    -- WAIT_FOR_DATA        Wait for result service request
    -- SEARCH_ID            Search for goal with result request goal id
    -- CHECK_STATE          Check if found goal is in terminal state
    -- STORE_RRQ            Store result request in memory
    -- SEND_RESPONSE        Send result service response
    -- WAIT_FOR_RET         Wait for result service response
    -- TRANSITION_STATE     Update goal state
    -- GET_DATA             Get goal data required for result response
    -- CHECK_REQUESTS       Search if updated goal is in stored result requests 
    -- GET_REQUEST_ID       Get data from stored result request
    -- REMOVE_REQUEST       Remove stored result request
    -- WAIT_FOR_MEM         Wait for goal data from memory
    -- RETURN_USER          User return
    -- CHECK_TIMEOUT        Check and remove expired goals
    -- REMOVE_OLDEST        Remove oldest goal in terminal state
    -- PUBLISH_FEEDBACK     Publish feedback
    -- RESET_MEMORY         Reset result request memory to empty state
    main_prc : process(all)
    begin
        -- DEFAULT
        stage_next              <= stage;
        cnt_next                <= cnt;
        mem_data_r_next         <= mem_data_r;
        terminal_cnt_next       <= terminal_cnt;
        return_code_latch_next  <= return_code_latch;
        trigger_result_next     <= trigger_result;
        rrq_addr_base_next      <= rrq_addr_base;
        rrq_addr_latch_next     <= rrq_addr_latch;
        rrq_empty_head_next     <= rrq_empty_head;
        rrq_occupied_head_next  <= rrq_occupied_head;
        request_id_latch_next   <= request_id_latch;
        time_latch_next         <= time_latch;
        check_time_next         <= check_time;
        goal_handle_latch_next  <= goal_handle_latch;
        goal_state_latch_next   <= goal_state_latch;
        -- DEFAULT Unregistered
        ack                 <= '0';
        done                <= '0';
        start_fb            <= '0';
        rrq_abort_read      <= '0';
        result_sel          <= '0';
        atomic_op_r         <= '0';
        trigger_status_r    <= '0';
        mem_start_r         <= '0';
        mem_opcode_r        <= NOP;
        start_r             <= '0';
        opcode_r            <= NOP;
        opcode_fb           <= NOP;
        mem_r               <= ZERO_GOAL_DATA;
        return_code         <= ROS_RET_OK;
        goal_handle_out     <= GOAL_HANDLE_UNKNOWN;
        
        
        case (stage) is
            when IDLE =>
                if (start = '1') then
                    case (opcode) is
                        when GET_GOAL =>
                            mem_start_r         <= '1';
                            mem_opcode_r        <= GET;
                            mem_r.field_flags   <= GMF_GOAL_ID_FLAG or GMF_STAMP_FLAG or GMF_RESULT_INDEX_FLAG or GMF_STATE_FLAG;
                            mem_r.addr          <= unsigned(goal_handle_in);
                            
                            if (mem_ack_r = '1') then
                                ack                     <= '1';
                                stage_next              <= WAIT_FOR_MEM;
                                return_code_latch_next  <= ROS_RET_OK;
                            end if;
                        when GET_LAST_GOAL =>
                            -- No Goals Available
                            if (mem_occupied_head = GOAL_MEMORY_MAX_ADDRESS) then
                                mem_data_r_next.addr    <= GOAL_MEMORY_MAX_ADDRESS;
                                ack                     <= '1';
                                return_code_latch_next  <= ROS_RET_OK;
                                stage_next              <= RETURN_USER;
                            else
                                mem_start_r         <= '1';
                                mem_opcode_r        <= GET;
                                mem_r.field_flags   <= GMF_GOAL_ID_FLAG or GMF_STAMP_FLAG or GMF_RESULT_INDEX_FLAG or GMF_STATE_FLAG;
                                mem_r.addr          <= mem_occupied_head;
                                
                                if (mem_ack_r = '1') then
                                    ack                     <= '1';
                                    stage_next              <= WAIT_FOR_MEM;
                                    return_code_latch_next  <= ROS_RET_OK;
                                end if;
                            end if;
                        when GET_PREVIOUS_GOAL =>
                            mem_start_r         <= '1';
                            mem_opcode_r        <= GET_NEXT;
                            mem_r.field_flags   <= GMF_GOAL_ID_FLAG or GMF_STAMP_FLAG or GMF_RESULT_INDEX_FLAG or GMF_STATE_FLAG;
                            mem_r.addr          <= unsigned(goal_handle_in);
                            
                            if (mem_ack_r = '1') then
                                ack                     <= '1';
                                stage_next              <= WAIT_FOR_MEM;
                                return_code_latch_next  <= ROS_RET_OK;
                            end if;
                        when UPDATE_GOAL_STATE =>
                            -- Atomic Operation Guard
                            if (atomic_op_c = '1' and unsigned(goal_handle_in) = mem_data_c.addr) then
                                -- NOTE: In order to prevent a user side Deadlock, we return with INVALID_ARGUMENT if the cancel FSM is also waiting for a user input in the atomic operation zone.
                                if (cancel_stage = WAIT_FOR_USER) then
                                    ack                     <= '1';
                                    stage_next              <= RETURN_USER;
                                    return_code_latch_next  <= ROS_RET_INVALID_ARGUMENT;
                                end if;
                            else
                                mem_start_r         <= '1';
                                mem_opcode_r        <= GET;
                                mem_r.field_flags   <= GMF_STATE_FLAG;
                                mem_r.addr          <= unsigned(goal_handle_in);
                                
                                if (mem_ack_r = '1') then
                                    -- Latch Input Data
                                    goal_handle_latch_next  <= goal_handle_in;
                                    goal_state_latch_next   <= goal_state_in;
                                    
                                    ack         <= '1';
                                    stage_next  <= TRANSITION_STATE;
                                    cnt_next    <= 0;
                                end if;
                            end if;
                        when PUBLISH_FEEDBACK =>
                            mem_start_r         <= '1';
                            mem_opcode_r        <= GET_NEXT;
                            mem_r.field_flags   <= GMF_GOAL_ID_FLAG;
                            mem_r.addr          <= unsigned(goal_handle_in);
                            
                            if (mem_ack_r = '1') then
                                ack         <= '1';
                                stage_next  <= PUBLISH_FEEDBACK;
                                cnt_next    <= 0;
                            end if;
                        when others =>
                            null;
                    end case;
                elsif (data_available_r = '1' and rrq_empty_head /= RRQ_MEMORY_MAX_ADDRESS) then
                    start_r     <= '1';
                    opcode_r    <= TAKE_REQUEST;
                    
                    if (ack_r = '1') then
                        stage_next  <= WAIT_FOR_DATA;
                    end if;
                elsif (TIMEOUT_DURATION /= ROS_TIME_INFINITE and time >= check_time) then
                    assert (mem_occupied_head /= GOAL_MEMORY_MAX_ADDRESS) severity FAILURE;
                    
                    -- Reset
                    check_time_next <= ROS_TIME_INFINITE;
                    
                    mem_data_r_next.addr    <= mem_occupied_head;
                    stage_next              <= CHECK_TIMEOUT;
                    cnt_next                <= 0;
                elsif (TIMEOUT_DURATION = ROS_TIME_INFINITE and mem_empty_head = GOAL_MEMORY_MAX_ADDRESS and terminal_cnt /= 0) then
                    assert (mem_occupied_tail /= GOAL_MEMORY_MAX_ADDRESS) severity FAILURE;
                    
                    mem_data_r_next.addr    <= mem_occupied_tail;
                    stage_next              <= REMOVE_OLDEST;
                    cnt_next                <= 0;
                end if;
            when WAIT_FOR_DATA =>
                if (done_r = '1') then
                    case (return_code_r) is
                        when ROS_RET_OK =>
                            if (taken_r = '1') then
                                -- Latch Request ID
                                request_id_latch_next   <= service_info_r.request_id;
                                
                                -- No Goals Available
                                if (mem_occupied_head = GOAL_MEMORY_MAX_ADDRESS) then
                                    mem_data_r_next.res_ind <= std_logic_vector(to_unsigned(MAX_GOALS, WORD_WIDTH));
                                    mem_data_r_next.state   <= GoalStatus_package.STATUS_UNKNOWN;
                                    stage_next              <= SEND_RESPONSE;
                                else
                                    stage_next  <= SEARCH_ID;
                                    cnt_next    <= 0;
                                end if;
                            else
                                stage_next  <= IDLE;
                            end if;
                        when others =>
                            -- TODO: Propagate Error?
                            stage_next  <= IDLE;
                    end case;
                end if;
            when SEARCH_ID =>
                case (cnt) is
                    -- GET FIRST
                    when 0 =>
                        assert (mem_occupied_head /= GOAL_MEMORY_MAX_ADDRESS) severity FAILURE;
                        
                        mem_start_r         <= '1';
                        mem_opcode_r        <= GET;
                        mem_r.field_flags   <= GMF_GOAL_ID_FLAG;
                        mem_r.addr          <= mem_occupied_head;
                        
                        if (mem_ack_r = '1') then
                            cnt_next    <= cnt + 2;
                        end if;
                    -- GET NEXT
                    when 1 =>
                        mem_start_r         <= '1';
                        mem_opcode_r        <= GET_NEXT;
                        mem_r.field_flags   <= GMF_GOAL_ID_FLAG;
                        mem_r.addr          <= mem_data_r.addr;
                        
                        if (mem_ack_r = '1') then
                            cnt_next    <= cnt + 1;
                        end if;
                    -- CHECK
                    when 2 =>
                        -- Wait for Memory
                        if (mem_done = '1') then
                            assert check_mask(mem_data.field_flags, GMF_GOAL_ID_FLAG) severity FAILURE;
                            
                            -- No more Goals
                            if (mem_data.addr = GOAL_MEMORY_MAX_ADDRESS) then
                                -- Goal not found
                                mem_data_r_next.res_ind <= std_logic_vector(to_unsigned(MAX_GOALS, WORD_WIDTH));
                                mem_data_r_next.state   <= GoalStatus_package.STATUS_UNKNOWN;
                                stage_next              <= SEND_RESPONSE;
                            else
                                mem_data_r_next <= mem_data;
                                cnt_next        <= cnt + 1;
                            end if;
                        end if;
                    -- CHECK GOAL_ID 1/4
                    when 3 =>
                        if (mem_data_r.goal_id(0) = to_GUID(goal_id_r)(0)) then
                            cnt_next    <= cnt + 1;
                        else
                            cnt_next    <= 1; -- GET_NEXT
                        end if;
                    -- CHECK GOAL_ID 2/4
                    when 4 =>
                        if (mem_data_r.goal_id(1) = to_GUID(goal_id_r)(1)) then
                            cnt_next    <= cnt + 1;
                        else
                            cnt_next    <= 1; -- GET_NEXT
                        end if;
                    -- CHECK GOAL_ID 3/4
                    when 5 =>
                        if (mem_data_r.goal_id(2) = to_GUID(goal_id_r)(2)) then
                            cnt_next    <= cnt + 1;
                        else
                            cnt_next    <= 1; -- GET_NEXT
                        end if;
                    -- CHECK GOAL_ID 4/4
                    when 6 =>
                        if (mem_data_r.goal_id(3) = to_GUID(goal_id_r)(3)) then
                            -- Found Goal
                            stage_next  <= CHECK_STATE;
                            cnt_next    <= 0;
                        else
                            cnt_next    <= 1; -- GET_NEXT
                        end if;
                    when others =>
                        null;
                end case;
            when CHECK_STATE =>
                case (cnt) is
                    -- GET STATE
                    when 0 =>
                        mem_start_r         <= '1';
                        mem_opcode_r        <= GET;
                        mem_r.field_flags   <= GMF_STATE_FLAG or GMF_RESULT_INDEX_FLAG;
                        mem_r.addr          <= mem_data_r.addr;
                        
                        if (mem_ack_r = '1') then
                            cnt_next    <= cnt + 1;
                        end if;
                    -- Check State
                    when 1 =>
                        -- Wait for Memory
                        if (mem_done = '1') then
                            assert (mem_data.addr = GOAL_MEMORY_MAX_ADDRESS) severity FAILURE;
                            assert check_mask(mem_data.field_flags, GMF_STATE_FLAG or GMF_RESULT_INDEX_FLAG) severity FAILURE;
                            
                            mem_data_r_next <= mem_data;
                            
                            if (is_terminal(mem_data.state)) then
                                if (mem_data.state /= GoalStatus_package.STATUS_SUCCEEDED) then
                                    -- NOTE: We are setting result index to MAX_GOALS to effectively "zero" the result.
                                    --       This prevents sending the result of previous succeeded goals, if the current
                                    --       goal is not succeeded.
                                    mem_data_r_next.res_ind <= std_logic_vector(to_unsigned(MAX_GOALS, WORD_WIDTH));
                                end if;
                                stage_next  <= SEND_RESPONSE;
                            else
                                stage_next  <= STORE_RRQ;
                                cnt_next    <= 0;
                            end if;
                        end if;
                    when others =>
                        null;
                end case;
            when STORE_RRQ =>
                assert (rrq_empty_head /= RRQ_MEMORY_MAX_ADDRESS) severity FAILURE;
                
                case (cnt) is
                    -- GET Next Addr
                    when 0 =>
                        rrq_valid_in    <= '1';
                        rrq_addr        <= rrq_empty_head + RMF_NEXT_ADDR_OFFSET;
                        rrq_read        <= '1';
                        
                        -- Memory Flow Control Guard
                        if (rrq_ready_in = '1') then
                            cnt_next    <= cnt + 1;
                        end if;
                    -- SET Goal Handle
                    when 1 =>
                        rrq_valid_in    <= '1';
                        rrq_addr        <= rrq_empty_head + RMF_GOAL_HANDLE_OFFSET;
                        rrq_write_data  <= std_logic_vector(resize(mem_data_r.addr,WORD_WIDTH));
                        
                        -- Memory Flow Control Guard
                        if (rrq_ready_in = '1') then
                            cnt_next    <= cnt + 1;
                        end if;
                    -- SET Request ID 1/6
                    when 2 =>
                        rrq_valid_in    <= '1';
                        rrq_addr        <= rrq_empty_head + RMF_REQUEST_ID_OFFSET;
                        rrq_write_data  <= service_info_c.request_id.writer_guid(0);
                        
                        -- Memory Flow Control Guard
                        if (rrq_ready_in = '1') then
                            cnt_next    <= cnt + 1;
                        end if;
                    -- SET Request ID 2/6
                    when 3 =>
                        rrq_valid_in    <= '1';
                        rrq_addr        <= rrq_empty_head + RMF_REQUEST_ID_OFFSET + 1;
                        rrq_write_data  <= service_info_c.request_id.writer_guid(1);
                        
                        -- Memory Flow Control Guard
                        if (rrq_ready_in = '1') then
                            cnt_next    <= cnt + 1;
                        end if;
                    -- SET Request ID 3/6
                    when 4 =>
                        rrq_valid_in    <= '1';
                        rrq_addr        <= rrq_empty_head + RMF_REQUEST_ID_OFFSET + 2;
                        rrq_write_data  <= service_info_c.request_id.writer_guid(2);
                        
                        -- Memory Flow Control Guard
                        if (rrq_ready_in = '1') then
                            cnt_next    <= cnt + 1;
                        end if;
                    -- SET Request ID 4/6
                    when 5 =>
                        rrq_valid_in    <= '1';
                        rrq_addr        <= rrq_empty_head + RMF_REQUEST_ID_OFFSET + 3;
                        rrq_write_data  <= service_info_c.request_id.writer_guid(3);
                        
                        -- Memory Flow Control Guard
                        if (rrq_ready_in = '1') then
                            cnt_next    <= cnt + 1;
                        end if;
                    -- SET Request ID 5/6
                    when 6 =>
                        rrq_valid_in    <= '1';
                        rrq_addr        <= rrq_empty_head + RMF_REQUEST_ID_OFFSET + 4;
                        rrq_write_data  <= std_logic_vector(service_info_c.request_id.sequence_number(0));
                        
                        -- Memory Flow Control Guard
                        if (rrq_ready_in = '1') then
                            cnt_next    <= cnt + 1;
                        end if;
                    -- SET Request ID 6/6
                    when 7 =>
                        rrq_valid_in    <= '1';
                        rrq_addr        <= rrq_empty_head + RMF_REQUEST_ID_OFFSET + 5;
                        rrq_write_data  <= std_logic_vector(service_info_c.request_id.sequence_number(1));
                        
                        -- Memory Flow Control Guard
                        if (rrq_ready_in = '1') then
                            if (rrq_occupied_head = RRQ_MEMORY_MAX_ADDRESS) then
                                cnt_next    <= cnt + 2;
                            else
                                cnt_next    <= cnt + 1;
                            end if;
                        end if;
                    -- SET Prev Addr (Old Occupied Head)
                    when 8 =>
                        assert (rrq_occupied_head /= RRQ_MEMORY_MAX_ADDRESS) severity FAILURE;
                        
                        rrq_valid_in    <= '1';
                        rrq_addr        <= rrq_occupied_head + RMF_PREV_ADDR_OFFSET;
                        rrq_write_data  <= std_logic_vector(resize(rrq_empty_head,WORD_WIDTH));
                        
                        -- Memory Flow Control Guard
                        if (rrq_ready_in = '1') then
                            cnt_next    <= cnt + 1;
                        end if;
                    -- SET Next Addr
                    when 9 =>
                        rrq_valid_in    <= '1';
                        rrq_addr        <= rrq_empty_head + RMF_NEXT_ADDR_OFFSET;
                        rrq_write_data  <= std_logic_vector(resize(rrq_occupied_head,WORD_WIDTH));
                        
                        -- Memory Flow Control Guard
                        if (rrq_ready_in = '1') then
                            -- Set New Occupied Head
                            rrq_occupied_head_next  <= rrq_empty_head;
                            cnt_next    <= cnt + 1;
                        end if;
                    -- SET Prev Addr
                    when 10 =>
                        rrq_valid_in    <= '1';
                        rrq_addr        <= rrq_empty_head + RMF_PREV_ADDR_OFFSET;
                        rrq_write_data  <= std_logic_vector(resize(RRQ_MEMORY_MAX_ADDRESS,WORD_WIDTH));
                        
                        -- Memory Flow Control Guard
                        if (rrq_ready_in = '1') then
                            cnt_next    <= cnt + 1;
                        end if;
                    -- READ Next Addr
                    when 11 =>
                        rrq_ready_out  <= '1';
                        
                        -- Memory Flow Control Guard
                        if (rrq_valid_out = '1') then
                            -- Set new Empty Head
                            rrq_empty_head_next <= resize(unsigned(rrq_read_data),RRQ_MEMORY_ADDR_WIDTH);
                            -- DONE
                            stage_next  <= IDLE;
                        end if;
                    when others =>
                        null;
                end case;
            when SEND_RESPONSE =>
                assert check_mask(mem_data_r.field_flags, GMF_STATE_FLAG or GMF_RESULT_INDEX_FLAG) severity FAILURE;
                result_sel  <= '1';
                
                if (result_sel_ack = '1') then
                    start_r     <= '1';
                    opcode_r    <= SEND_RESPONSE;
                    
                    if (ack_r = '1') then
                        stage_next  <= WAIT_FOR_RET;
                    end if;
                end if;
            when WAIT_FOR_RET =>
                assert check_mask(mem_data_r.field_flags, GMF_STATE_FLAG or GMF_RESULT_INDEX_FLAG) severity FAILURE;
                result_sel  <= '1';
                
                if (done_r = '1') then
                    -- TODO: Propagate Error?
                    stage_next  <= IDLE;
                     
                    if (trigger_result = '1') then
                        stage_next  <= CHECK_REQUESTS;
                        cnt_next    <= 0;
                    end if;
                end if;
            when TRANSITION_STATE =>
                assert (not (atomic_op_c = '1' and mem_data_r.addr = mem_data_c.addr)) severity FAILURE;
                atomic_op_r <= '1';
                
                case (cnt) is
                    -- CHECK Goal State
                    when 0 =>
                        -- Wait for Memory
                        if (mem_done = '1') then
                            assert check_mask(mem_data.field_flags, GMF_STATE_FLAG) severity FAILURE;
                            
                            case (mem_data.state) is
                                when GoalStatus_package.STATUS_ACCEPTED =>
                                    case (goal_state_latch) is
                                        when GoalStatus_package.STATUS_EXECUTING =>
                                            -- Synthesis Guard
                                            if (TIMEOUT_DURATION /= ROS_TIME_INFINITE and is_terminal(goal_state_in)) then
                                                cnt_next    <= cnt + 1;
                                            else
                                                cnt_next    <= cnt + 3;
                                            end if;
                                        when others =>
                                            ack                     <= '1';
                                            stage_next              <= RETURN_USER;
                                            return_code_latch_next  <= ROS_RET_ACTION_GOAL_EVENT_INVALID;
                                    end case;
                                when GoalStatus_package.STATUS_EXECUTING =>
                                    case (goal_state_latch) is
                                        when GoalStatus_package.STATUS_SUCCEEDED =>
                                            -- Synthesis Guard
                                            if (TIMEOUT_DURATION /= ROS_TIME_INFINITE and is_terminal(goal_state_in)) then
                                                cnt_next    <= cnt + 1;
                                            else
                                                cnt_next    <= cnt + 3;
                                            end if;
                                        when GoalStatus_package.STATUS_ABORTED =>
                                            -- Synthesis Guard
                                            if (TIMEOUT_DURATION /= ROS_TIME_INFINITE and is_terminal(goal_state_in)) then
                                                cnt_next    <= cnt + 1;
                                            else
                                                cnt_next    <= cnt + 3;
                                            end if;
                                        when others =>
                                            ack                     <= '1';
                                            stage_next              <= RETURN_USER;
                                            return_code_latch_next  <= ROS_RET_ACTION_GOAL_EVENT_INVALID;
                                    end case;
                                when GoalStatus_package.STATUS_CANCELING =>
                                    case (goal_state_latch) is
                                        when GoalStatus_package.STATUS_SUCCEEDED =>
                                            -- Synthesis Guard
                                            if (TIMEOUT_DURATION /= ROS_TIME_INFINITE and is_terminal(goal_state_in)) then
                                                cnt_next    <= cnt + 1;
                                            else
                                                cnt_next    <= cnt + 3;
                                            end if;
                                        when GoalStatus_package.STATUS_ABORTED =>
                                            -- Synthesis Guard
                                            if (TIMEOUT_DURATION /= ROS_TIME_INFINITE and is_terminal(goal_state_in)) then
                                                cnt_next    <= cnt + 1;
                                            else
                                                cnt_next    <= cnt + 3;
                                            end if;
                                        when GoalStatus_package.STATUS_CANCELED =>
                                            -- Synthesis Guard
                                            if (TIMEOUT_DURATION /= ROS_TIME_INFINITE and is_terminal(goal_state_in)) then
                                                cnt_next    <= cnt + 1;
                                            else
                                                cnt_next    <= cnt + 3;
                                            end if;
                                        when others =>
                                            ack                     <= '1';
                                            stage_next              <= RETURN_USER;
                                            return_code_latch_next  <= ROS_RET_ACTION_GOAL_EVENT_INVALID;
                                    end case;
                                when others =>
                                    ack                     <= '1';
                                    stage_next              <= RETURN_USER;
                                    return_code_latch_next  <= ROS_RET_ACTION_GOAL_EVENT_INVALID;
                            end case;
                        end if;
                    -- Time Addition 1/2
                    when 1 =>
                        -- Synthesis Guard
                        if (TIMEOUT_DURATION /= ROS_TIME_INFINITE) then
                            time_latch_next.sec     <= std_logic_vector(unsigned(time.sec) + unsigned(TIMEOUT_DURATION.sec));
                            time_latch_next.nanosec <= std_logic_vector(unsigned(time.nanosec) + unsigned(TIMEOUT_DURATION.nanosec));
                            cnt_next    <= cnt + 1;
                        end if;
                    -- Time Addition 2/2
                    when 2 =>
                        -- Synthesis Guard
                        if (TIMEOUT_DURATION /= ROS_TIME_INFINITE) then
                            if (unsigned(time_latch.nanosec) >= to_unsigned(10**9,time_latch.nanosec'length)) then
                                time_latch_next.sec     <= std_logic_vector(unsigned(time_latch.sec) + 1);
                                time_latch_next.nanosec <= std_logic_vector(unsigned(time_latch.nanosec) - to_unsigned(10**9,time_latch.nanosec'length));
                                cnt_next    <= cnt + 1;
                            end if;
                        end if;
                    -- UPDATE Goal State
                    when 3 =>
                        mem_start_r         <= '1';
                        mem_opcode_r        <= UPDATE;
                        mem_r.field_flags   <= GMF_STATE_FLAG;
                        mem_r.addr          <= unsigned(goal_handle_latch);
                        mem_r.state         <= goal_state_latch;
                        
                        -- Synthesis Guard
                        if (TIMEOUT_DURATION /= ROS_TIME_INFINITE and is_terminal(goal_state_in)) then
                            mem_r.field_flags   <= GMF_STATE_FLAG or GMF_DEADLINE_FLAG;
                            mem_r.deadline      <= time_latch;
                            
                            -- Update Checktime
                            if (time_latch < check_time) then
                                check_time_next <= time_latch;
                            end if;
                        end if;
                        
                        if (mem_ack_r = '1') then
                            if (is_terminal(goal_state_in)) then
                                trigger_result_next <= '1';
                                -- Update Terminal Goal Counter
                                terminal_cnt_next   <= terminal_cnt + 1;
                            end if;
                            ack                     <= '1';
                            stage_next              <= RETURN_USER;
                            return_code_latch_next  <= ROS_RET_OK;
                            -- Trigger Status Update
                            trigger_status_r        <= '1';
                        end if;
                    when others =>
                        null;
                end case;
            when GET_DATA =>
                case (cnt) is
                    -- GET Data
                    when 0 =>
                        mem_start_r         <= '1';
                        mem_opcode_r        <= GET;
                        mem_r.field_flags   <= GMF_STATE_FLAG or GMF_RESULT_INDEX_FLAG;
                        mem_r.addr          <= mem_data_r.addr;
                        
                        if (mem_ack_r = '1') then
                            cnt_next    <= cnt + 1;
                        end if;
                    -- READ Data
                    when 1 =>
                        -- Wait for Memory
                        if (mem_done = '1') then
                            assert (mem_data.addr = GOAL_MEMORY_MAX_ADDRESS) severity FAILURE;
                            assert check_mask(mem_data.field_flags, GMF_STATE_FLAG or GMF_RESULT_INDEX_FLAG) severity FAILURE;
                            assert is_terminal(mem_data.state) severity FAILURE;
                            
                            mem_data_r_next <= mem_data;
                            
                            if (mem_data.state /= GoalStatus_package.STATUS_SUCCEEDED) then
                                -- NOTE: We are setting result index to MAX_GOALS to effectively "zero" the result.
                                --       This prevents sending the result of previous succeeded goals, if the current
                                --       goal is not succeeded.
                                mem_data_r_next.res_ind <= std_logic_vector(to_unsigned(MAX_GOALS, WORD_WIDTH));
                            end if;
                            
                            stage_next  <= CHECK_REQUESTS;
                            cnt_next    <= 0;
                        end if;
                    when others =>
                        null;
                end case;
            when CHECK_REQUESTS =>
                case (cnt) is
                    -- GET Goal Handle
                    when 0 =>
                        -- NOTE: We check the base here, because a REMOVE_REQUEST operation may reset the rrq_addr_base
                        if (rrq_addr_base = RRQ_MEMORY_MAX_ADDRESS) then
                            -- Reset
                            trigger_result_next <= '0';
                            
                            -- DONE
                            stage_next  <= IDLE;
                        else
                            rrq_valid_in    <= '1';
                            rrq_addr        <= rrq_addr_base + RMF_GOAL_HANDLE_OFFSET;
                            rrq_read        <= '1';
                            
                            -- Memory Flow Control Guard
                            if (rrq_ready_in = '1') then
                                cnt_next    <= cnt + 1;
                            end if;
                        end if;
                    -- GET Next Addr
                    when 1 =>
                        rrq_valid_in    <= '1';
                        rrq_addr        <= rrq_addr_base + RMF_NEXT_ADDR_OFFSET;
                        rrq_read        <= '1';
                        
                        -- Memory Flow Control Guard
                        if (rrq_ready_in = '1') then
                            cnt_next    <= cnt + 1;
                        end if;
                    -- READ Goal Handle
                    when 2 =>
                        rrq_ready_out  <= '1';
                        
                        -- Memory Flow Control Guard
                        if (rrq_valid_out = '1') then
                            
                            -- Found Result Request of Updated Goal
                            if (resize(unsigned(rrq_read_data),GOAL_MEMORY_ADDR_WIDTH) = mem_data_r.addr) then
                                stage_next      <= GET_REQUEST_ID;
                                cnt_next        <= 0;
                                rrq_abort_read  <= '1';
                            else
                                cnt_next    <= cnt + 1;
                            end if;
                        end if;
                    -- READ Next Addr
                    when 3 =>
                        rrq_ready_out  <= '1';
                        
                        -- Memory Flow Control Guard
                        if (rrq_valid_out = '1') then
                            rrq_addr_base_next <= resize(unsigned(rrq_read_data),GOAL_MEMORY_ADDR_WIDTH);
                            
                            -- No More Stored Requests
                            if (resize(unsigned(rrq_read_data),GOAL_MEMORY_ADDR_WIDTH) = RRQ_MEMORY_MAX_ADDRESS) then
                                -- Reset
                                trigger_result_next <= '0';
                                
                                stage_next  <= IDLE;
                            else
                                -- Next Request
                                cnt_next    <= 0;
                            end if;
                        end if;
                    when others =>
                        null;
                end case;
            when GET_REQUEST_ID =>
                case (cnt) is
                    -- GET Request ID 1/6
                    when 0 =>
                        rrq_valid_in    <= '1';
                        rrq_addr        <= rrq_addr_base + RMF_REQUEST_ID_OFFSET;
                        rrq_read        <= '1';
                        
                        -- Memory Flow Control Guard
                        if (rrq_ready_in = '1') then
                            cnt_next    <= cnt + 1;
                        end if;
                    -- GET Request ID 2/6
                    when 1 =>
                        rrq_valid_in    <= '1';
                        rrq_addr        <= rrq_addr_base + RMF_REQUEST_ID_OFFSET + 1;
                        rrq_read        <= '1';
                        
                        -- Memory Flow Control Guard
                        if (rrq_ready_in = '1') then
                            cnt_next    <= cnt + 1;
                        end if;
                    -- GET Request ID 3/6
                    when 2 =>
                        rrq_valid_in    <= '1';
                        rrq_addr        <= rrq_addr_base + RMF_REQUEST_ID_OFFSET + 2;
                        rrq_read        <= '1';
                        
                        -- Memory Flow Control Guard
                        if (rrq_ready_in = '1') then
                            cnt_next    <= cnt + 1;
                        end if;
                    -- GET Request ID 4/6
                    when 3 =>
                        rrq_valid_in    <= '1';
                        rrq_addr        <= rrq_addr_base + RMF_REQUEST_ID_OFFSET + 3;
                        rrq_read        <= '1';
                        
                        -- Memory Flow Control Guard
                        if (rrq_ready_in = '1') then
                            cnt_next    <= cnt + 1;
                        end if;
                    -- GET Request ID 5/6
                    when 4 =>
                        rrq_valid_in    <= '1';
                        rrq_addr        <= rrq_addr_base + RMF_REQUEST_ID_OFFSET + 4;
                        rrq_read        <= '1';
                        
                        -- Memory Flow Control Guard
                        if (rrq_ready_in = '1') then
                            cnt_next    <= cnt + 1;
                        end if;
                    -- GET Request ID 6/6
                    when 5 =>
                        rrq_valid_in    <= '1';
                        rrq_addr        <= rrq_addr_base + RMF_REQUEST_ID_OFFSET + 5;
                        rrq_read        <= '1';
                        
                        -- Memory Flow Control Guard
                        if (rrq_ready_in = '1') then
                            cnt_next    <= cnt + 1;
                        end if;
                    -- READ Request ID 1/6
                    when 6 =>
                        rrq_ready_out  <= '1';
                        
                        -- Memory Flow Control Guard
                        if (rrq_valid_out = '1') then
                            request_id_latch_next.writer_guid(0)    <= rrq_read_data;
                            
                            cnt_next    <= cnt + 1;
                        end if;
                    -- READ Request ID 2/6
                    when 7 =>
                        rrq_ready_out  <= '1';
                        
                        -- Memory Flow Control Guard
                        if (rrq_valid_out = '1') then
                            request_id_latch_next.writer_guid(1)    <= rrq_read_data;
                            
                            cnt_next    <= cnt + 1;
                        end if;
                    -- READ Request ID 3/6
                    when 8 =>
                        rrq_ready_out  <= '1';
                        
                        -- Memory Flow Control Guard
                        if (rrq_valid_out = '1') then
                            request_id_latch_next.writer_guid(2)    <= rrq_read_data;
                            
                            cnt_next    <= cnt + 1;
                        end if;
                    -- READ Request ID 4/6
                    when 9 =>
                        rrq_ready_out  <= '1';
                        
                        -- Memory Flow Control Guard
                        if (rrq_valid_out = '1') then
                            request_id_latch_next.writer_guid(3)    <= rrq_read_data;
                            
                            cnt_next    <= cnt + 1;
                        end if;
                    -- READ Request ID 5/6
                    when 10 =>
                        rrq_ready_out  <= '1';
                        
                        -- Memory Flow Control Guard
                        if (rrq_valid_out = '1') then
                            request_id_latch_next.sequence_number(0)    <= unsigned(rrq_read_data);
                            
                            cnt_next    <= cnt + 1;
                        end if;
                    -- READ Request ID 6/6
                    when 11 =>
                        rrq_ready_out  <= '1';
                        
                        -- Memory Flow Control Guard
                        if (rrq_valid_out = '1') then
                            request_id_latch_next.sequence_number(1)    <= unsigned(rrq_read_data);
                            
                            stage_next  <= REMOVE_REQUEST;
                            cnt_next    <= 0;
                        end if;
                    when others =>
                        null;
                end case;
            when REMOVE_REQUEST =>
                -- NOTE: After thsi stage the rrq_addr_base is set to the Next request
                case (cnt) is
                    -- GET Next Addr
                    when 0 =>
                        rrq_valid_in    <= '1';
                        rrq_addr        <= rrq_addr_base + RMF_NEXT_ADDR_OFFSET;
                        rrq_read        <= '1';
                        
                        -- Memory Flow Control Guard
                        if (rrq_ready_in = '1') then
                            cnt_next    <= cnt + 1;
                        end if;
                    -- GET Prev Addr
                    when 1 =>
                        rrq_valid_in    <= '1';
                        rrq_addr        <= rrq_addr_base + RMF_PREV_ADDR_OFFSET;
                        rrq_read        <= '1';
                        
                        -- Memory Flow Control Guard
                        if (rrq_ready_in = '1') then
                            cnt_next    <= cnt + 1;
                        end if;
                    -- SET Next Addr
                    when 2 =>
                        rrq_valid_in    <= '1';
                        rrq_addr        <= rrq_addr_base + RMF_NEXT_ADDR_OFFSET;
                        rrq_write_data  <= std_logic_vector(resize(rrq_empty_head,WORD_WIDTH));
                        
                        -- Memory Flow Control Guard
                        if (rrq_ready_in = '1') then
                            -- Set New Empty Head
                            rrq_empty_head_next <= rrq_addr_base;
                            
                            cnt_next    <= cnt + 1;
                        end if;
                    -- READ Next Addr
                    when 3 =>
                        rrq_ready_out  <= '1';
                        
                        -- Memory Flow Control Guard
                        if (rrq_valid_out = '1') then
                            rrq_addr_latch_next <= resize(unsigned(rrq_read_data),RRQ_MEMORY_ADDR_WIDTH);
                            cnt_next            <= cnt + 1;
                        end if;
                    -- READ Previous Addr
                    when 4 =>
                        rrq_ready_out  <= '1';
                        
                        -- Memory Flow Control Guard
                        if (rrq_valid_out = '1') then
                            if (rrq_addr_latch = RRQ_MEMORY_MAX_ADDRESS) then
                                if (resize(unsigned(rrq_read_data),RRQ_MEMORY_ADDR_WIDTH) = RRQ_MEMORY_MAX_ADDRESS) then
                                    -- Reset Occupied Head
                                    rrq_occupied_head_next  <= RRQ_MEMORY_MAX_ADDRESS;
                                    
                                    rrq_addr_base_next  <= RRQ_MEMORY_MAX_ADDRESS;
                                    stage_next          <= SEND_RESPONSE;
                                else
                                    rrq_addr_base_next  <= resize(unsigned(rrq_read_data),RRQ_MEMORY_ADDR_WIDTH);
                                    cnt_next            <= cnt + 2;
                                end if;
                            else
                                rrq_addr_base_next  <= rrq_addr_latch;
                                rrq_addr_latch_next <= resize(unsigned(rrq_read_data),RRQ_MEMORY_ADDR_WIDTH);
                                cnt_next            <= cnt + 1;
                            end if;
                        end if;
                    -- SET Previous Addr (Next Slot)
                    when 6 =>
                        rrq_valid_in    <= '1';
                        rrq_addr        <= rrq_addr_base + RMF_PREV_ADDR_OFFSET;
                        rrq_write_data  <= std_logic_vector(rrq_addr_latch);
                        
                        -- Memory Flow Control Guard
                        if (rrq_ready_in = '1') then
                            if (rrq_addr_latch = RRQ_MEMORY_MAX_ADDRESS) then
                                -- Set New Occupied Head
                                rrq_occupied_head_next  <= rrq_addr_base;
                                
                                stage_next  <= SEND_RESPONSE;
                            else
                                rrq_addr_base_next  <= rrq_addr_latch;
                                rrq_addr_latch_next <= rrq_addr_base;
                                cnt_next            <= cnt + 1;
                            end if;
                        end if;
                    -- SET Next Addr (Previous Slot)
                    when 7 =>
                        rrq_valid_in    <= '1';
                        rrq_addr        <= rrq_addr_base + RMF_NEXT_ADDR_OFFSET;
                        rrq_write_data  <= std_logic_vector(rrq_addr_latch);
                        
                        -- Memory Flow Control Guard
                        if (rrq_ready_in = '1') then
                            
                            rrq_addr_base_next  <= rrq_addr_latch;
                            stage_next          <= SEND_RESPONSE;
                        end if;
                    when others =>
                        null;
                end case;
            when WAIT_FOR_MEM =>
                -- Wait for Memory
                if (mem_done = '1') then
                    assert check_mask(mem_data.field_flags, GMF_GOAL_ID_FLAG or GMF_STAMP_FLAG or GMF_RESULT_INDEX_FLAG or GMF_STATE_FLAG) severity FAILURE;
                    
                    mem_data_r_next <= mem_data;
                    
                    stage_next  <= RETURN_USER;
                end if;
            when RETURN_USER =>
                -- End of Memory
                if (mem_data_r.addr = GOAL_MEMORY_MAX_ADDRESS) then
                    goal_handle_out <= GOAL_HANDLE_UNKNOWN;
                else
                    goal_handle_out <= std_logic_vector(resize(mem_data_r.addr, GOAL_HANDLE_WIDTH));
                end if;
                
                done        <= '1';
                return_code <= return_code_latch;
                
                if (trigger_result = '1') then
                    stage_next  <= GET_DATA;
                    cnt_next    <= 0;
                else
                    stage_next  <= IDLE;
                end if;
            when CHECK_TIMEOUT =>
                -- Synthesis Guard
                if (TIMEOUT_DURATION /= ROS_TIME_INFINITE) then
                    case (cnt) is
                        -- GET
                        when 0 =>
                            mem_start_r         <= '1';
                            mem_opcode_r        <= GET;
                            mem_r.field_flags   <= GMF_DEADLINE_FLAG;
                            mem_r.addr          <= mem_data_r.addr;
                            
                            if (mem_ack_r = '1') then
                                cnt_next    <= cnt + 2;
                            end if;
                        -- GET NEXT
                        when 1 =>
                            mem_start_r         <= '1';
                            mem_opcode_r        <= GET_NEXT;
                            mem_r.field_flags   <= GMF_DEADLINE_FLAG;
                            mem_r.addr          <= mem_data_r.addr;
                            
                            if (mem_ack_r = '1') then
                                cnt_next    <= cnt + 1;
                            end if;
                        -- CHECK
                        when 2 =>
                            -- Wait for Memory
                            if (mem_done = '1') then
                                assert check_mask(mem_data.field_flags, GMF_DEADLINE_FLAG) severity FAILURE;
                                
                                mem_data_r_next <= mem_data;
                                
                                -- No more Goals
                                if (mem_data.addr = GOAL_MEMORY_MAX_ADDRESS) then
                                    -- DONE
                                    stage_next  <= IDLE;
                                else
                                    -- Timeout
                                    if (mem_data.deadline <= time) then
                                        cnt_next    <= cnt + 1;
                                    else
                                        -- Update check Time
                                        if (mem_data.deadline < check_time) then
                                            check_time_next <= mem_data.deadline;
                                        end if;
                                        cnt_next    <= 1;
                                    end if;
                                end if;
                            end if;
                        -- REMOVE
                        when 3 =>
                            -- TODO: This needs to take into account the other FSMs (Status, Cancel, Goal)
                            mem_start_r     <= '1';
                            mem_opcode_r    <= REMOVE;
                            mem_r.addr      <= mem_data_r.addr;
                            
                            if (mem_ack_r = '1') then
                                cnt_next    <= cnt + 1;
                                -- Trigger Status Update
                                trigger_status_r    <= '1';
                                -- Update Terminal Goal Counter
                                terminal_cnt_next   <= terminal_cnt - 1;
                            end if;
                        -- Wait for Remove
                        when 4 =>
                            -- Wait for Memory
                            if (mem_done = '1') then
                                mem_data_r_next <= mem_data;
                                
                                -- No more Goals
                                if (mem_data.addr = GOAL_MEMORY_MAX_ADDRESS) then
                                    if (mem_occupied_head = GOAL_MEMORY_MAX_ADDRESS) then
                                        -- DONE
                                        stage_next  <= IDLE;
                                    else
                                        mem_data_r.addr <= mem_occupied_head;
                                        cnt_next        <= 1;
                                    end if;
                                else
                                    cnt_next    <= 1;
                                end if;
                            end if;
                        when others =>
                            null;
                    end case;
                end if;
            when REMOVE_OLDEST =>
                -- Synthesis Guard
                if (TIMEOUT_DURATION = ROS_TIME_INFINITE) then
                    case (cnt) is
                        -- GET
                        when 0 =>
                            mem_start_r         <= '1';
                            mem_opcode_r        <= GET;
                            mem_r.field_flags   <= GMF_STATE_FLAG;
                            mem_r.addr          <= mem_data_r.addr;
                            
                            if (mem_ack_r = '1') then
                                cnt_next    <= cnt + 2;
                            end if;
                        -- GET NEXT
                        when 1 =>
                            mem_start_r         <= '1';
                            mem_opcode_r        <= GET_PREV;
                            mem_r.field_flags   <= GMF_STATE_FLAG;
                            mem_r.addr          <= mem_data_r.addr;
                            
                            if (mem_ack_r = '1') then
                                cnt_next    <= cnt + 1;
                            end if;
                        -- CHECK
                        when 2 =>
                            -- Wait for Memory
                            if (mem_done = '1') then
                                assert check_mask(mem_data.field_flags, GMF_STATE_FLAG) severity FAILURE;
                                
                                mem_data_r_next <= mem_data;
                                
                                -- No more Goals
                                if (mem_data.addr = GOAL_MEMORY_MAX_ADDRESS) then
                                    -- DONE
                                    stage_next  <= IDLE;
                                else
                                    if (is_terminal(mem_data.state)) then
                                        cnt_next    <= cnt + 1;
                                    else
                                        cnt_next    <= 1;
                                    end if;
                                end if;
                            end if;
                        -- REMOVE
                        when 3 =>
                            mem_start_r     <= '1';
                            mem_opcode_r    <= REMOVE;
                            mem_r.addr      <= mem_data_r.addr;
                            
                            if (mem_ack_r = '1') then
                                cnt_next    <= cnt + 1;
                                -- Trigger Status Update
                                trigger_status_r    <= '1';
                                -- Update Terminal Goal Counter
                                terminal_cnt_next   <= terminal_cnt - 1;
                            end if;
                        -- Wait for Remove
                        when 4 =>
                            -- Wait for Memory
                            if (mem_done = '1') then
                                mem_data_r_next <= mem_data;
                                
                                -- No more Goals
                                if (mem_data.addr = GOAL_MEMORY_MAX_ADDRESS) then
                                    -- DONE
                                    stage_next  <= IDLE;
                                else
                                    -- NOTE: After Removal, mem_data.addr is set to the previous goal, so we need to call GET and not GET_PREV
                                    cnt_next    <= 0;
                                end if;
                            end if;
                        when others =>
                            null;
                    end case;
                end if;
            when PUBLISH_FEEDBACK =>
                case (cnt) is
                    -- Wait for Data
                    when 0 =>
                        if (mem_done = '1') then
                            assert check_mask(mem_data.field_flags, GMF_GOAL_ID_FLAG) severity FAILURE;
                            
                            mem_data_r_next <= mem_data_r;
                            cnt_next        <= cnt + 1;
                        end if;
                    -- Start Publish
                    when 1 =>
                        start_fb    <= '1';
                        opcode_fb   <= PUBLISH;
                        
                        if (ack_fb = '1') then
                            cnt_next    <= cnt + 1;
                        end if;
                    -- Wait for Publish
                    when 2 =>
                        -- Passthrough
                        done        <= done_fb;
                        return_code <= return_code_fb;
                        
                        if (done_fb = '1') then
                            -- DONE
                            stage_next  <= IDLE;
                        end if;
                    when others =>
                        null;
                end case;
            when RESET_MEMORY =>
                case (cnt) is
                    -- SET Next Pointer
                    when 0 =>
                        rrq_valid_in    <= '1';
                        rrq_addr        <= rrq_addr_base + RMF_NEXT_ADDR_OFFSET;
                        if (rrq_addr_base = MAX_RRQ_ADDRESS) then
                            rrq_write_data  <= std_logic_vector(resize(RRQ_MEMORY_MAX_ADDRESS,WORD_WIDTH));
                        else
                            rrq_write_data  <= std_logic_vector(resize(rrq_addr_base + RRQ_FRAME_SIZE,WORD_WIDTH));
                        end if;
                        
                        -- Memory Flow Control Guard
                        if (rrq_ready_in = '1') then
                            cnt_next    <= cnt + 1;
                        end if;
                    -- SET Previous Pointer
                    when 1 =>
                        rrq_valid_in    <= '1';
                        rrq_addr        <= rrq_addr_base + RMF_PREV_ADDR_OFFSET;
                        rrq_write_data  <= std_logic_vector(resize(rrq_addr_latch,WORD_WIDTH));
                        
                        -- Memory Flow Control Guard
                        if (rrq_ready_in = '1') then
                            if (rrq_addr_base = MAX_RRQ_ADDRESS) then
                                -- Initialize Empty and Occupied Heads
                                rrq_empty_head      <= FIRST_RRQ_ADDRESS;
                                rrq_occupied_head   <= RRQ_MEMORY_MAX_ADDRESS;
                                
                                -- DONE
                                stage_next          <= IDLE;
                            else
                                rrq_addr_latch_next <= rrq_addr_base;
                                rrq_addr_base_next  <= rrq_addr_base + RRQ_FRAME_SIZE;
                                cnt_next            <= 0;
                            end if;
                        end if;
                    when others =>
                        null;
                end case;
        end case;
    end process;
    
    -- *Goal State Machine*
    -- STATE            DESCRIPTION
    -- IDLE             Idle state
    -- WAIT_FOR_DATA    Wait goal service request
    -- WAIT_FOR_USER    Wait for user input
    -- ADD_GOAL         Add goal to memory
    -- SEND_RESPONSE    Send goal service response
    -- WAIT_FOR_RET     Wait for goal service response
    goal_prc : process(all)
    begin
        -- DEFAULT
        goal_stage_next     <= goal_stage;
        g_stamp_next        <= g_stamp;
        g_accept_next       <= g_accept;
        -- DEFAULT Unregistered
        trigger_status_g        <= '0';
        mem_start_g             <= '0';
        mem_opcode_g            <= NOP;
        start_g                 <= '0';
        opcode_g                <= NOP;
        new_goal_request        <= '0';
        new_goal_handle         <= GOAL_HANDLE_UNKNOWN;
        new_goal_result_index   <= (others => '0');
        
        case (goal_stage) is
            when IDLE =>
                if (data_available_g = '1' and mem_empty_head /= GOAL_MEMORY_MAX_ADDRESS) then
                    start_g     <= '1';
                    opcode_g    <= TAKE_REQUEST;
                    if (ack_g = '1') then
                        goal_stage_next <= WAIT_FOR_DATA;
                    end if;
                end if;
            when WAIT_FOR_DATA =>
                if (done_g = '1') then
                    case (return_code_g) is
                        when ROS_RET_OK =>
                            if (taken_g = '1') then
                                goal_stage_next <= WAIT_FOR_USER;
                            else
                                goal_stage_next <= IDLE;
                            end if;
                        when others =>
                            -- TODO: Propagate Error?
                            goal_stage_next <= IDLE;
                    end case;
                end if;
            when WAIT_FOR_USER =>
                new_goal_request        <= '1';
                new_goal_handle         <= std_logic_vector(resize(mem_empty_head,GOAL_HANDLE_WIDTH));
                new_goal_result_index   <= std_logic_vector(empty_head_res_ind);
                if (new_goal_response = '1') then
                    g_stamp_next    <= time;
                    if (new_goal_accepted = '1') then
                        goal_stage_next <= ADD_GOAL;
                        g_accept_next   <= '1';
                    else
                        goal_stage_next <= SEND_RESPONSE;
                        g_accept_next   <= '0';
                    end if;
                end if;
            when ADD_GOAL =>
                assert (mem_empty_head /= GOAL_MEMORY_MAX_ADDRESS) severity FAILURE;
                
                mem_start_g     <= '1';
                mem_opcode_g    <= INSERT;
                
                if (mem_ack_g = '1') then
                    goal_stage_next <= SEND_RESPONSE;
                    -- Trigger Status Update
                    trigger_status_g    <= '1';
                end if;
            when SEND_RESPONSE =>
                start_g     <= '1';
                opcode_g    <= SEND_RESPONSE;
                if (ack_g = '1') then
                    goal_stage_next <= WAIT_FOR_RET;
                end if;
            when WAIT_FOR_RET =>
                if (done_g = '1') then
                    -- TODO: Propagate Error?
                    goal_stage_next <= IDLE;
                end if;
        end case;
    end process;
    
    -- *Cancel State Machine*
    -- STATE            DESCRIPTION
    -- IDLE             Idle state
    -- WAIT_FOR_DATA    Wait for cancel service request
    -- GET              Get goal data from memory (Pointed by mem_data_c.addr)
    -- GET_NEXT         Get goal data from memory (Next goal from one pointed by mem_data_c.addr)
    -- CHECK            Determine what data has to be checked for each stored goal
    -- CHECK_GOAL_ID    Check if cancel goal id is same as mem_data_c
    -- CHECK_STAMP      Check if cancel goal stamp is earlier than the one in mem_data_c
    -- CHECK_STATE      Check if mem_data_c is in terminal state
    -- WAIT_FOR_USER    Wait for user input
    -- CANCEL_GOAL      Update goal state to CANCELING
    -- ADD_CANCEL       Store canceling goal for later response
    -- SEND_RESPONSE    Send cancel service response
    -- WAIT_FOR_RET     Wait for cancel service response
    cancel_prc : process(all)
    begin
        -- DEFAULT
        cancel_stage_next           <= cancel_stage;
        c_cnt_next                  <= c_cnt;
        mem_data_c_next             <= mem_data_c;
        search_type_c_next          <= search_type_c;
        goals_canceling_cnt_next    <= goals_canceling_cnt;
        cancel_ret_code_next        <= cancel_ret_code;
        -- DEFAULT Unregistered
        atomic_op_c                 <= '0';
        trigger_status_c            <= '0';
        mem_start_c                 <= '0';
        mem_opcode_c                <= NOP;
        start_c                     <= '0';
        opcode_c                    <= NOP;
        mem_c                       <= ZERO_GOAL_DATA;
        cancel_request              <= '0';
        cancel_request_handle       <= GOAL_HANDLE_UNKNOWN;
        goals_canceling_addr_c      <= (others => '0');
        goals_canceling_ren_c       <= '0';
        goals_canceling_wen_c       <= '0';
        goals_canceling_ack_c       <= '0';
        goals_canceling_goal_id_w_c <= (others => '0');
        goals_canceling_stamp_w_c   <= (others => '0');
        
        case (cancel_stage) is
            when IDLE =>
                if (data_available_c = '1') then
                    start_c     <= '1';
                    opcode_c    <= TAKE_REQUEST;
                    if (ack_c = '1') then
                        cancel_stage_next   <= WAIT_FOR_DATA;
                        
                        --Reset
                        goals_canceling_cnt_next    <= 0;
                    end if;
                end if;
            when WAIT_FOR_DATA =>
                if (done_c = '1') then
                    case (return_code_c) is
                        when ROS_RET_OK =>
                            if (taken_c = '1') then
                                -- No Goals Available
                                if (mem_occupied_head = GOAL_MEMORY_MAX_ADDRESS) then
                                    cancel_ret_code_next    <= CancelGoal_package.RR_ERROR_REJECTED;
                                    cancel_stage_next       <= SEND_RESPONSE;
                                else
                                    if (to_UUID(goal_info_goal_id_c) = UUID_UNKNOWN) then
                                        if (to_ROS_TIME(goal_info_stamp_c) = ROS_TIME_ZERO) then
                                            search_type_c_next  <= SEARCH_NONE;
                                        else
                                            search_type_c_next  <= SEARCH_STAMP;
                                        end if;
                                    else
                                        search_type_c_next  <= SEARCH_GOAL_ID;
                                    end if;
                                    cancel_stage_next       <= GET;
                                    mem_data_c_next.addr    <= mem_occupied_head;
                                end if;
                            else
                                cancel_stage_next   <= IDLE;
                            end if;
                        when others =>
                            -- TODO: Propagate Error?
                            cancel_stage_next   <= IDLE;
                    end case;
                end if;
            when GET =>
                -- Atomic Operation Guard
                if (not (atomic_op_r = '1' and mem_data_c.addr = mem_data_r.addr)) then
                    mem_start_c         <= '1';
                    mem_opcode_c        <= GET;
                    mem_c.field_flags   <= GMF_GOAL_ID_FLAG or GMF_STAMP_FLAG or GMF_STATE_FLAG;
                    mem_c.addr          <= mem_data_c.addr;
                    
                    if (mem_ack_c = '1') then
                        cancel_stage_next   <= CHECK;
                    end if;
                end if;
            when GET_NEXT =>
                mem_start_c         <= '1';
                mem_opcode_c        <= GET_NEXT;
                mem_c.field_flags   <= GMF_GOAL_ID_FLAG or GMF_STAMP_FLAG or GMF_STATE_FLAG;
                mem_c.addr          <= mem_data_c.addr;
                
                if (mem_ack_c = '1') then
                    cancel_stage_next   <= CHECK;
                end if;
            when CHECK =>
                atomic_op_c <= '1';
                -- Wait for Memory
                if (mem_done = '1') then
                    -- End of Memory
                    if (mem_data.addr = GOAL_MEMORY_MAX_ADDRESS) then
                        case (search_type_c) is
                            when SEARCH_GOAL_ID =>
                                cancel_ret_code_next    <= CancelGoal_package.RR_ERROR_UNKNOWN_GOAL_ID;
                                cancel_stage_next       <= SEND_RESPONSE;
                            when others =>
                                -- No Goals Canceled
                                if (goals_canceling_cnt = 0) then
                                    cancel_ret_code_next    <= CancelGoal_package.RR_ERROR_REJECTED;
                                    cancel_stage_next       <= SEND_RESPONSE;
                                else
                                    cancel_ret_code_next    <= CancelGoal_package.RR_ERROR_NONE;
                                    cancel_stage_next       <= SEND_RESPONSE;
                                end if;
                        end case;
                    else
                        assert check_mask(mem_data.field_flags, GMF_GOAL_ID_FLAG or GMF_STAMP_FLAG or GMF_STATE_FLAG) severity FAILURE;
                        
                        mem_data_c_next <= mem_data;
                        
                        -- NOTE: If both the Main and Cancel FSM enter the atomic operation zone at the smae time, the cancel FSM gives way and waits.
                        if (atomic_op_r = '1' and mem_data.addr = mem_data_r.addr) then
                            cancel_stage_next   <= GET;
                        else
                            case (search_type_c) is
                                when SEARCH_GOAL_ID =>
                                    cancel_stage_next   <= CHECK_GOAL_ID;
                                    c_cnt_next          <= 0;
                                when SEARCH_STAMP =>
                                    cancel_stage_next   <= CHECK_STAMP;
                                when SEARCH_NONE =>
                                    cancel_stage_next   <= CHECK_STATE;
                            end case;
                        end if;
                    end if;
                end if;
            when CHECK_GOAL_ID =>
                assert (not (atomic_op_r = '1' and mem_data_c.addr = mem_data_r.addr)) severity FAILURE;
                atomic_op_c <= '1';
                
                case (c_cnt) is
                    -- GOAL_ID 1/4
                    when 0 =>
                        if (mem_data_c.goal_id(0) = to_guid(goal_info_goal_id_c)(0)) then
                            c_cnt_next  <= c_cnt + 1;
                        else
                            cancel_stage_next   <= GET_NEXT;
                        end if;
                    -- GOAL_ID 2/4
                    when 1 =>
                        if (mem_data_c.goal_id(1) = to_guid(goal_info_goal_id_c)(1)) then
                            c_cnt_next  <= c_cnt + 1;
                        else
                            cancel_stage_next   <= GET_NEXT;
                        end if;
                    -- GOAL_ID 3/4
                    when 2 =>
                        if (mem_data_c.goal_id(2) = to_guid(goal_info_goal_id_c)(2)) then
                            c_cnt_next  <= c_cnt + 1;
                        else
                            cancel_stage_next   <= GET_NEXT;
                        end if;
                    -- GOAL_ID 4/4
                    when 3 =>
                        if (mem_data_c.goal_id(3) = to_guid(goal_info_goal_id_c)(3)) then
                            cancel_stage_next   <= CHECK_STATE;
                            c_cnt_next          <= 0;
                        else
                            cancel_stage_next   <= GET_NEXT;
                        end if;
                    when others =>
                        null;
                end case;
            when CHECK_STAMP =>
                assert (not (atomic_op_r = '1' and mem_data_c.addr = mem_data_r.addr)) severity FAILURE;
                atomic_op_c <= '1';
                
                if (mem_data_c.stamp < to_ROS_TIME(goal_info_stamp_c)) then
                    cancel_stage_next   <= CHECK_STATE;
                else
                    cancel_stage_next   <= GET_NEXT;
                end if;
            when CHECK_STATE =>
                assert (not (atomic_op_r = '1' and mem_data_c.addr = mem_data_r.addr)) severity FAILURE;
                atomic_op_c <= '1';
                
                if (is_terminal(mem_data_c.state)) then
                    cancel_ret_code_next    <= CancelGoal_package.RR_ERROR_GOAL_TERMINATED;
                    cancel_stage_next       <= SEND_RESPONSE;
                elsif (mem_data_c.state = GoalStatus_package.STATUS_CANCELING) then
                    -- Skip State Change
                    cancel_stage_next       <= ADD_CANCEL;
                else
                    cancel_stage_next       <= WAIT_FOR_USER;
                end if;
            when WAIT_FOR_USER =>
                assert (not (atomic_op_r = '1' and mem_data_c.addr = mem_data_r.addr)) severity FAILURE;
                atomic_op_c <= '1';
                
                cancel_request          <= '1';
                cancel_request_handle   <= std_logic_vector(mem_data_c.addr);
                if (cancel_response = '1') then
                    if (cancel_accepted = '1') then
                        cancel_stage_next       <= CANCEL_GOAL;
                    else
                        case (search_type_c) is
                            when SEARCH_GOAL_ID =>
                                if (to_ROS_TIME(goal_info_stamp_c) = ROS_TIME_ZERO) then
                                    -- No Goals Canceled
                                    cancel_ret_code_next    <= CancelGoal_package.RR_ERROR_REJECTED;
                                    cancel_stage_next       <= SEND_RESPONSE;
                                else
                                    -- Search for Timestamps
                                    search_type_c_next      <= SEARCH_STAMP;
                                    cancel_stage_next       <= GET;
                                    mem_data_c_next.addr    <= mem_occupied_head;
                                end if;
                            when SEARCH_STAMP =>
                                -- NOTE: Since the goals are added chronologically, we don't have to check the timetsamps from now on
                                search_type_c_next  <= SEARCH_NONE;
                                cancel_stage_next   <= GET_NEXT;
                            when SEARCH_NONE =>
                                -- Continue
                                cancel_stage_next   <= GET_NEXT;
                        end case;
                    end if;
                end if;
            when CANCEL_GOAL =>
                assert (not (atomic_op_r = '1' and mem_data_c.addr = mem_data_r.addr)) severity FAILURE;
                atomic_op_c <= '1';
                
                mem_start_c         <= '1';
                mem_opcode_c        <= UPDATE;
                mem_c.addr          <= mem_data_c.addr;
                mem_c.field_flags   <= GMF_STATE_FLAG;
                mem_c.state         <= GoalStatus_package.STATUS_CANCELING;
                
                if (mem_ack_c = '1') then
                    cancel_stage_next   <= ADD_CANCEL;
                    -- Trigger Status Update
                    trigger_status_c    <= '1';
                end if;
            when ADD_CANCEL =>
                goals_canceling_addr_c      <= std_logic_vector(to_unsigned(goals_canceling_cnt,CancelGoal_package.RR_GOALS_CANCELING_GOAL_ID_ADDR_WIDTH));
                goals_canceling_wen_c       <= '1';
                goals_canceling_goal_id_w_c <= std_logic_vector(to_unsigned(mem_data_c.goal_id));
                goals_canceling_stamp_w_c   <= std_logic_vector(to_unsigned(mem_data_c.stamp));
                if (goals_canceling_ready_c = '1') then
                    -- Reached maximum number of cancel goals
                    if (goals_canceling_cnt = CancelGoal_package.RR_GOALS_CANCELING_MAX_DEPTH) then
                        -- DONE
                        cancel_ret_code_next    <= CancelGoal_package.RR_ERROR_NONE;
                        cancel_stage_next       <= SEND_RESPONSE;
                    else
                        goals_canceling_cnt_next    <= goals_canceling_cnt + 1;
                        
                        case (search_type_c) is
                            when SEARCH_GOAL_ID =>
                                if (to_ROS_TIME(goal_info_stamp_c) = ROS_TIME_ZERO) then
                                    -- DONE
                                    cancel_ret_code_next    <= CancelGoal_package.RR_ERROR_NONE;
                                    cancel_stage_next       <= SEND_RESPONSE;
                                else
                                    assert (mem_occupied_head /= GOAL_MEMORY_MAX_ADDRESS) severity FAILURE;
                                    
                                    -- Search for Timetamps
                                    search_type_c_next      <= SEARCH_STAMP;
                                    cancel_stage_next       <= GET;
                                    mem_data_c_next.addr    <= mem_occupied_head;
                                end if;
                            when SEARCH_STAMP =>
                                -- NOTE: Since the goals are added chronologically, we don't have to check the timetsamps from now on
                                search_type_c_next  <= SEARCH_NONE;
                                cancel_stage_next   <= GET_NEXT;
                            when SEARCH_NONE =>
                                -- Continue
                                cancel_stage_next   <= GET_NEXT;
                        end case;
                    end if;
                end if;
            when SEND_RESPONSE =>
                start_c     <= '1';
                opcode_c    <= SEND_RESPONSE;
                
                if (ack_c = '1') then
                    cancel_stage_next   <= WAIT_FOR_RET;
                end if;
            when WAIT_FOR_RET =>
                if (done_c = '1') then
                    -- TODO: Propagate Error?
                    cancel_stage_next   <= IDLE;
                end if;
            when others =>
                null;
        end case;
    end process;
    
    -- *Status State Machine*
    -- STATE                DESCRIPTION
    -- IDLE                 Idle state
    -- GET_FIRST            Get first goal data from memory
    -- GET_NEXT             Get goal data from memory (Next goal from one pointed by mem_data_c.addr)
    -- WAIT_FOR_DATA        Wait for memory data
    -- PUSH_STATUS          Add goal data to status list
    -- PUBLISH              Publish status list    
    -- WAIT_FOR_PUBLISHER   Wait for status list publisher
    status_prc : process (all)
    begin
        -- DEFAULT
        status_stage_next       <= status_stage;
        trigger_status_next     <= trigger_status;
        status_list_cnt_next    <= status_list_cnt;
        mem_data_s_next         <= mem_data_s;
        -- DEFAULT Unregistered
        status_idle_sig                     <= '0';
        start_s                             <= '0';
        opcode_s                            <= NOP;
        mem_start_s                         <= '0';
        mem_opcode_s                        <= NOP;
        mem_s                               <= ZERO_GOAL_DATA;
        status_list_ren_s                   <= '0';
        status_list_wen_s                   <= '0';
        status_list_ack_s                   <= '0';
        status_list_addr_s                  <= (others => '0');
        status_list_goal_info_goal_id_w_s   <= std_logic_vector(to_unsigned(UUID_UNKNOWN));
        status_list_goal_info_stamp_w_s     <= std_logic_vector(to_unsigned(ROS_TIME_INVALID));
        status_list_status_w_s              <= GoalStatus_package.STATUS_UNKNOWN;
        
        -- Trigger Status Setter
        if (trigger_status_c = '1' or trigger_status_g = '1' or trigger_status_r = '1') then
            trigger_status_next <= '1';
        end if;
        
        case (status_stage) is
            when IDLE =>
                status_idle_sig <= '1';
                if (trigger_status = '1') then
                    -- Trigger Status Resetter
                    trigger_status_next <= '0';
                    
                    -- Reset
                    status_list_cnt_next    <= 0;
                    
                    if (mem_occupied_head = GOAL_MEMORY_MAX_ADDRESS) then
                        status_stage_next   <= PUBLISH;
                    else
                        status_stage_next   <= GET_FIRST;
                    end if;
                end if;
            when GET_FIRST =>
                assert (mem_occupied_head /= GOAL_MEMORY_MAX_ADDRESS) severity FAILURE;
                
                mem_start_s         <= '1';
                mem_opcode_s        <= GET;
                mem_s.addr          <= mem_occupied_head;
                mem_s.field_flags   <= GMF_GOAL_ID_FLAG or GMF_STAMP_FLAG or GMF_STATE_FLAG;
                
                if (mem_ack_s = '1') then
                    status_stage_next   <= WAIT_FOR_DATA;
                end if;
            when GET_NEXT =>
                mem_start_s         <= '1';
                mem_opcode_s        <= GET_NEXT;
                mem_s.addr          <= mem_data_s.addr;
                mem_s.field_flags   <= GMF_GOAL_ID_FLAG or GMF_STAMP_FLAG or GMF_STATE_FLAG;
                
                if (mem_ack_s = '1') then
                    status_stage_next   <= WAIT_FOR_DATA;
                end if;
            when WAIT_FOR_DATA =>
                -- Wait for DATA
                if (mem_done = '1') then
                    mem_data_s_next <= mem_data_s;
                    
                    -- No more Goals
                    if (mem_data.addr = GOAL_MEMORY_MAX_ADDRESS) then
                        status_stage_next   <= PUBLISH;
                    else
                        status_stage_next   <= PUSH_STATUS;
                    end if;
                end if;
            when PUSH_STATUS =>
                status_list_addr_s                  <= std_logic_vector(to_unsigned(status_list_cnt,GoalStatusArray_package.STATUS_LIST_GOAL_INFO_GOAL_ID_ADDR_WIDTH));
                status_list_wen_s                   <= '1';
                status_list_goal_info_goal_id_w_s   <= std_logic_vector(to_unsigned(mem_data_s.goal_id));
                status_list_goal_info_stamp_w_s     <= std_logic_vector(to_unsigned(mem_data_s.stamp));
                status_list_status_w_s              <= mem_data_s.state;
                
                if (status_list_ready_s = '1') then
                    status_list_cnt_next    <= status_list_cnt + 1;
                    
                    if (status_list_cnt = GoalStatusArray_package.STATUS_LIST_MAX_DEPTH-1) then
                        status_stage_next   <= PUBLISH;
                    else
                        status_stage_next   <= GET_NEXT;
                    end if;
                end if;
            when PUBLISH =>
                start_s             <= '1';
                opcode_s            <= PUBLISH;
                
                if (ack_s = '1') then
                    status_stage_next   <= WAIT_FOR_PUBLISHER;
                end if;
            when WAIT_FOR_PUBLISHER =>
                if (done_s = '1') then
                    -- TODO: Propagate Error?
                    status_stage_next   <= IDLE;
                end if;
            when others =>
                null;
        end case;
    end process;
    
    -- *Goal Memory State Machine*
    -- STATE        DESCRIPTION
    -- IDLE         Idle State. Done Signal is pulled high and Memory FSM accepts new memory operations
    -- INSERT       See Memory OPCODE Description
    -- GET          See Memory OPCODE Description
    -- GET_NEXT     See Memory OPCODE Description
    -- GET_PREV     See Memory OPCODE Description
    -- UPDATE       See Memory OPCODE Description
    -- REMOVE       See Memory OPCODE Description
    -- RESET_MEMORY Reset goal memory to empty state
    mem_ctrl_prc : process(all)
    begin
        -- DEFAULT
        mem_stage_next          <= mem_stage;
        mem_cnt_next            <= mem_cnt;
        mem_occupied_head_next  <= mem_occupied_head;
        mem_occupied_tail_next  <= mem_occupied_tail;    
        mem_empty_head_next     <= mem_empty_head;
        mem_empty_tail_next     <= mem_empty_tail;
        empty_head_res_ind_next <= empty_head_res_ind;
        mem_latch_data_next     <= mem_latch_data;
        mem_data_next           <= mem_data;
        mem_addr_base_next      <= mem_addr_base;
        mem_addr_latch_next     <= mem_addr_latch;
        -- DEFAULT unregistered
        mem_ack_g       <= '0';
        mem_ack_r       <= '0';
        mem_ack_c       <= '0';
        mem_ack_s       <= '0';
        mem_done        <= '0';
        mem_abort_read  <= '0';
        mem_ready_out   <= '0';
        mem_valid_in    <= '0';
        mem_read        <= '0';
        mem_done     <= '0';
        mem_addr        <= (others => '0');
        mem_write_data  <= (others => '0');
        
        
        case (mem_stage) is
            -- NOTE: The REMOVE opcode sets mem_data.addr to the previous slot (or GOAL_MEMORY_MAX_ADDRESS if no previous goals)
            --       This allows defined behavious of GET_NEXT when removing goals.
            when IDLE =>
                mem_done    <= '1';
                if (mem_start_g = '1') then
                    mem_ack_g   <= '1';
                    
                    case (mem_opcode_g) is
                        when INSERT =>
                            assert (mem_empty_head /= GOAL_MEMORY_MAX_ADDRESS) severity FAILURE;
                            
                            -- Latch DATA
                            mem_latch_data_next.goal_id <= to_guid(goal_id_g);
                            mem_latch_data_next.stamp   <= g_stamp;
                            
                            mem_addr_base_next  <= mem_empty_head;
                            mem_stage_next      <= INSERT;
                            mem_cnt_next        <= 0;    
                        when others =>
                            assert FALSE severity FAILURE;
                    end case;
                elsif (mem_start_r = '1') then
                    mem_ack_r   <= '1';
                    
                    case (mem_opcode_r) is
                        when GET =>
                            -- Latch DATA
                            mem_latch_data_next <= mem_r;
                            
                            if (mem_r.addr = mem_addr_base) then
                                mem_data_next.field_flags   <= mem_data.field_flags or mem_r.field_flags;
                            else
                                mem_data_next   <= ZERO_GOAL_DATA;
                                mem_data_next.field_flags   <= mem_r.field_flags;
                            end if;
                            
                            mem_addr_base_next  <= mem_r.addr;
                            mem_stage_next  <= GET;
                            
                            if check_mask(mem_r.field_flags,GMF_STATE_FLAG) then
                                mem_cnt_next    <= 0;
                            elsif check_mask(mem_r.field_flags,GMF_GOAL_ID_FLAG) then
                                mem_cnt_next    <= 1;
                            elsif check_mask(mem_r.field_flags,GMF_STAMP_FLAG) then
                                mem_cnt_next    <= 5;
                            elsif (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and check_mask(mem_r.field_flags,GMF_DEADLINE_FLAG)) then
                                mem_cnt_next    <= 7;
                            elsif check_mask(mem_r.field_flags,GMF_RESULT_INDEX_FLAG) then
                                mem_cnt_next    <= 9;
                            else
                                -- DONE
                                mem_stage_next  <= IDLE;
                            end if;
                        when GET_NEXT =>
                            -- Latch DATA
                            mem_latch_data_next <= mem_r;
                            
                            mem_data_next   <= ZERO_GOAL_DATA;
                            mem_data_next.field_flags   <= mem_r.field_flags;
                            
                            mem_addr_base_next  <= mem_r.addr;
                            mem_stage_next  <= GET_NEXT;
                            mem_cnt_next    <= 0;
                        when GET_PREV =>
                            -- Latch DATA
                            mem_latch_data_next <= mem_r;
                            
                            mem_data_next   <= ZERO_GOAL_DATA;
                            mem_data_next.field_flags   <= mem_r.field_flags;
                            
                            mem_addr_base_next  <= mem_r.addr;
                            mem_stage_next  <= GET_PREV;
                            mem_cnt_next    <= 0;
                        when UPDATE =>
                            -- Latch DATA
                            mem_latch_data_next <= mem_r;
                            
                            if (mem_r.addr = mem_addr_base) then
                                mem_data_next.field_flags   <= mem_data.field_flags or mem_r.field_flags;
                            else
                                mem_data_next   <= ZERO_GOAL_DATA;
                                mem_data_next.field_flags   <= mem_r.field_flags;
                            end if;
                            
                            mem_addr_base_next  <= mem_r.addr;
                            mem_stage_next      <= UPDATE;
                            
                            if check_mask(mem_r.field_flags,GMF_STATE_FLAG) then
                                mem_cnt_next    <= 0;
                            elsif check_mask(mem_r.field_flags,GMF_GOAL_ID_FLAG) then
                                mem_cnt_next    <= 1;
                            elsif check_mask(mem_r.field_flags,GMF_STAMP_FLAG) then
                                mem_cnt_next    <= 5;
                            elsif (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and check_mask(mem_r.field_flags,GMF_DEADLINE_FLAG)) then
                                mem_cnt_next    <= 7;
                            elsif check_mask(mem_r.field_flags,GMF_RESULT_INDEX_FLAG) then
                                mem_cnt_next    <= 9;
                            else
                                -- DONE
                                mem_stage_next  <= IDLE;
                            end if;
                        when REMOVE =>
                            mem_addr_base_next  <= mem_r.addr;
                            mem_stage_next      <= REMOVE;
                            mem_cnt_next        <= 0;
                        when others =>
                            assert FALSE severity FAILURE;
                    end case;
                elsif (mem_start_c = '1') then
                    mem_ack_c   <= '1';
                    
                    case (mem_opcode_c) is
                        when GET =>
                            -- Latch DATA
                            mem_latch_data_next <= mem_c;
                            
                            if (mem_c.addr = mem_addr_base) then
                                mem_data_next.field_flags   <= mem_data.field_flags or mem_c.field_flags;
                            else
                                mem_data_next   <= ZERO_GOAL_DATA;
                                mem_data_next.field_flags   <= mem_c.field_flags;
                            end if;
                            
                            mem_addr_base_next  <= mem_c.addr;
                            mem_stage_next  <= GET;
                            
                            if check_mask(mem_c.field_flags,GMF_STATE_FLAG) then
                                mem_cnt_next    <= 0;
                            elsif check_mask(mem_c.field_flags,GMF_GOAL_ID_FLAG) then
                                mem_cnt_next    <= 1;
                            elsif check_mask(mem_c.field_flags,GMF_STAMP_FLAG) then
                                mem_cnt_next    <= 5;
                            elsif (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and check_mask(mem_c.field_flags,GMF_DEADLINE_FLAG)) then
                                mem_cnt_next    <= 7;
                            elsif check_mask(mem_c.field_flags,GMF_RESULT_INDEX_FLAG) then
                                mem_cnt_next    <= 9;
                            else
                                -- DONE
                                mem_stage_next  <= IDLE;
                            end if;
                        when GET_NEXT =>
                            -- Latch DATA
                            mem_latch_data_next <= mem_c;
                            
                            mem_data_next   <= ZERO_GOAL_DATA;
                            mem_data_next.field_flags   <= mem_c.field_flags;
                            
                            mem_addr_base_next  <= mem_c.addr;
                            mem_stage_next  <= GET_NEXT;
                            mem_cnt_next    <= 0;
                        when GET_PREV =>
                            -- Latch DATA
                            mem_latch_data_next <= mem_c;
                            
                            mem_data_next   <= ZERO_GOAL_DATA;
                            mem_data_next.field_flags   <= mem_c.field_flags;
                            
                            mem_addr_base_next  <= mem_c.addr;
                            mem_stage_next  <= GET_PREV;
                            mem_cnt_next    <= 0;
                        when UPDATE =>
                            -- Latch DATA
                            mem_latch_data_next <= mem_c;
                            
                            if (mem_c.addr = mem_addr_base) then
                                mem_data_next.field_flags   <= mem_data.field_flags or mem_c.field_flags;
                            else
                                mem_data_next   <= ZERO_GOAL_DATA;
                                mem_data_next.field_flags   <= mem_c.field_flags;
                            end if;
                            
                            mem_addr_base_next  <= mem_c.addr;
                            mem_stage_next      <= UPDATE;
                            
                            if check_mask(mem_c.field_flags,GMF_STATE_FLAG) then
                                mem_cnt_next    <= 0;
                            elsif check_mask(mem_c.field_flags,GMF_GOAL_ID_FLAG) then
                                mem_cnt_next    <= 1;
                            elsif check_mask(mem_c.field_flags,GMF_STAMP_FLAG) then
                                mem_cnt_next    <= 5;
                            elsif (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and check_mask(mem_c.field_flags,GMF_DEADLINE_FLAG)) then
                                mem_cnt_next    <= 7;
                            elsif check_mask(mem_c.field_flags,GMF_RESULT_INDEX_FLAG) then
                                mem_cnt_next    <= 9;
                            else
                                -- DONE
                                mem_stage_next  <= IDLE;
                            end if;
                        when others =>
                            assert FALSE severity FAILURE;
                    end case;
                elsif (mem_start_s = '1') then
                    mem_ack_s   <= '1';
                    
                    case (mem_opcode_s) is
                        when GET =>
                            -- Latch DATA
                            mem_latch_data_next <= mem_s;
                            
                            if (mem_s.addr = mem_addr_base) then
                                mem_data_next.field_flags   <= mem_data.field_flags or mem_s.field_flags;
                            else
                                mem_data_next   <= ZERO_GOAL_DATA;
                                mem_data_next.field_flags   <= mem_s.field_flags;
                            end if;
                            
                            mem_addr_base_next  <= mem_s.addr;
                            mem_stage_next  <= GET;
                            
                            if check_mask(mem_s.field_flags,GMF_STATE_FLAG) then
                                mem_cnt_next    <= 0;
                            elsif check_mask(mem_s.field_flags,GMF_GOAL_ID_FLAG) then
                                mem_cnt_next    <= 1;
                            elsif check_mask(mem_s.field_flags,GMF_STAMP_FLAG) then
                                mem_cnt_next    <= 5;
                            elsif (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and check_mask(mem_s.field_flags,GMF_DEADLINE_FLAG)) then
                                mem_cnt_next    <= 7;
                            elsif check_mask(mem_s.field_flags,GMF_RESULT_INDEX_FLAG) then
                                mem_cnt_next    <= 9;
                            else
                                -- DONE
                                mem_stage_next  <= IDLE;
                            end if;
                        when GET_NEXT =>
                            -- Latch DATA
                            mem_latch_data_next <= mem_s;
                            
                            mem_data_next   <= ZERO_GOAL_DATA;
                            mem_data_next.field_flags   <= mem_s.field_flags;
                            
                            mem_addr_base_next  <= mem_s.addr;
                            mem_stage_next  <= GET_NEXT;
                            mem_cnt_next    <= 0;
                        when GET_PREV =>
                            -- Latch DATA
                            mem_latch_data_next <= mem_s;
                            
                            mem_data_next   <= ZERO_GOAL_DATA;
                            mem_data_next.field_flags   <= mem_s.field_flags;
                            
                            mem_addr_base_next  <= mem_s.addr;
                            mem_stage_next  <= GET_PREV;
                            mem_cnt_next    <= 0;
                        when others =>
                            assert FALSE severity FAILURE;
                    end case;
                end if;
            when INSERT =>
                case (mem_cnt) is
                    -- GET Next Addr
                    when 0 =>
                        mem_valid_in    <= '1';
                        mem_addr        <= mem_addr_base + GMF_NEXT_ADDR_OFFSET;
                        mem_read        <= '1';
                        
                        -- Memory Flow Control Guard
                        if (mem_ready_in = '1') then
                            mem_cnt_next    <= mem_cnt + 1;
                        end if;
                    -- READ Next Addr
                    when 1 =>
                        mem_ready_out  <= '1';
                        
                        -- Memory Flow Control Guard
                        if (mem_valid_out = '1') then
                            -- SET New Empty Head
                            mem_empty_head_next <= resize(unsigned(mem_read_data),GOAL_MEMORY_ADDR_WIDTH);
                            
                            mem_cnt_next        <= mem_cnt + 1;
                        end if;
                    -- GET Result Index (New Empty Head)
                    when 2 =>
                        mem_valid_in    <= '1';
                        mem_addr        <= mem_empty_head + GMF_RESULT_INDEX_OFFSET;
                        mem_read        <= '1';
                        
                        -- Memory Flow Control Guard
                        if (mem_ready_in = '1') then
                            mem_cnt_next    <= mem_cnt + 1;
                        end if;
                    -- READ Result Index (New Empty Head)
                    when 3 =>
                        mem_ready_out  <= '1';
                        
                        -- Memory Flow Control Guard
                        if (mem_valid_out = '1') then
                            empty_head_res_ind_next <= unsigned(mem_read_data);
                            mem_cnt_next            <= mem_cnt + 1;
                        end if;
                    -- SET Goal State
                    when 4 =>
                        mem_valid_in        <= '1';
                        mem_addr            <= mem_addr_base + GMF_STATE_OFFSET;
                        mem_write_data(CDR_INT8_WIDTH-1 downto 0)   <= GoalStatus_package.STATUS_ACCEPTED;
                        mem_data_next.state <= GoalStatus_package.STATUS_ACCEPTED;
                        
                        -- Memory Flow Control Guard
                        if (mem_ready_in = '1') then
                            mem_cnt_next   <= mem_cnt + 1;
                        end if;
                    -- SET Goal ID 1/4
                    when 5 =>
                        mem_valid_in    <= '1';
                        mem_addr        <= mem_addr_base + GMF_GOAL_ID_OFFSET;
                        mem_write_data  <= mem_latch_data.goal_id(0);
                        
                        -- Memory Flow Control Guard
                        if (mem_ready_in = '1') then
                            mem_cnt_next   <= mem_cnt + 1;
                        end if;
                    -- SET Goal ID 2/4
                    when 6 =>
                        mem_valid_in    <= '1';
                        mem_addr        <= mem_addr_base + GMF_GOAL_ID_OFFSET + 1;
                        mem_write_data  <= mem_latch_data.goal_id(1);
                        
                        -- Memory Flow Control Guard
                        if (mem_ready_in = '1') then
                            mem_cnt_next   <= mem_cnt + 1;
                        end if;
                    -- SET Goal ID 3/4
                    when 7 =>
                        mem_valid_in    <= '1';
                        mem_addr        <= mem_addr_base + GMF_GOAL_ID_OFFSET + 2;
                        mem_write_data  <= mem_latch_data.goal_id(2);
                        
                        -- Memory Flow Control Guard
                        if (mem_ready_in = '1') then
                            mem_cnt_next   <= mem_cnt + 1;
                        end if;
                    -- SET Goal ID 4/4
                    when 8 =>
                        mem_valid_in    <= '1';
                        mem_addr        <= mem_addr_base + GMF_GOAL_ID_OFFSET + 3;
                        mem_write_data  <= mem_latch_data.goal_id(3);
                        mem_data_next.goal_id   <= mem_latch_data.goal_id;
                        
                        -- Memory Flow Control Guard
                        if (mem_ready_in = '1') then
                            mem_cnt_next   <= mem_cnt + 1;
                        end if;
                    -- SET Stamp 1/2
                    when 9 =>
                        mem_valid_in    <= '1';
                        mem_addr        <= mem_addr_base + GMF_STAMP_OFFSET;
                        mem_write_data  <= mem_latch_data.stamp.sec;
                        
                        -- Memory Flow Control Guard
                        if (mem_ready_in = '1') then
                            mem_cnt_next   <= mem_cnt + 1;
                        end if;
                    -- SET Stamp 2/2
                    when 10 =>
                        mem_valid_in    <= '1';
                        mem_addr        <= mem_addr_base + GMF_STAMP_OFFSET + 1;
                        mem_write_data  <= mem_latch_data.stamp.nanosec;
                        mem_data_next.stamp <= mem_latch_data.stamp;
                        
                        -- Memory Flow Control Guard
                        if (mem_ready_in = '1') then
                            -- Synthesis Guard
                            if (TIMEOUT_DURATION /= ROS_DURATION_INFINITE) then
                                mem_cnt_next   <= mem_cnt + 1;
                            else
                                mem_cnt_next   <= mem_cnt + 3;
                            end if;
                        end if;
                    -- SET Deadline 1/2
                    when 11 =>
                        -- Synthesis Guard
                        if (TIMEOUT_DURATION /= ROS_DURATION_INFINITE) then
                            mem_valid_in    <= '1';
                            mem_addr        <= mem_addr_base + GMF_DEADLINE_OFFSET;
                            mem_write_data  <= ROS_TIME_INFINITE.sec;
                            
                            -- Memory Flow Control Guard
                            if (mem_ready_in = '1') then
                                mem_cnt_next   <= mem_cnt + 1;
                            end if;
                        end if;
                    -- SET Deadline 2/2
                    when 12 =>
                        -- Synthesis Guard
                        if (TIMEOUT_DURATION /= ROS_DURATION_INFINITE) then
                            mem_valid_in    <= '1';
                            mem_addr        <= mem_addr_base + GMF_DEADLINE_OFFSET + 1;
                            mem_write_data  <= ROS_TIME_INFINITE.nanosec;
                            mem_data_next.deadline  <= ROS_TIME_INFINITE;
                            
                            -- Memory Flow Control Guard
                            if (mem_ready_in = '1') then
                                mem_cnt_next   <= mem_cnt + 1;
                            end if;
                        end if;
                    -- SET Prev Addr
                    when 13 =>
                        mem_valid_in    <= '1';
                        mem_addr        <= mem_addr_base + GMF_PREV_ADDR_OFFSET;
                        mem_write_data  <= std_logic_vector(resize(GOAL_MEMORY_MAX_ADDRESS,WORD_WIDTH));
                        
                        -- Memory Flow Control Guard
                        if (mem_ready_in = '1') then
                            mem_cnt_next   <= mem_cnt + 1;
                        end if;
                    -- SET Next Addr
                    when 14 =>
                        mem_valid_in    <= '1';
                        mem_addr        <= mem_addr_base + GMF_NEXT_ADDR_OFFSET;
                        mem_write_data  <= std_logic_vector(resize(mem_occupied_head,WORD_WIDTH));
                        mem_data_next.addr  <= mem_addr_base;
                        
                        -- Memory Flow Control Guard
                        if (mem_ready_in = '1') then
                            -- Update Occupied Head
                            mem_occupied_head_next  <= mem_addr_base;
                            -- Initial Occupied Tail
                            if (mem_occupied_tail = GOAL_MEMORY_MAX_ADDRESS) then
                                mem_occupied_tail_next  <= mem_addr_base;
                            end if;
                            
                            -- DONE
                            mem_stage_next  <= IDLE;
                        end if;
                    when others =>
                        null;
                end case;
            when GET =>
                case (mem_cnt) is
                    -- GET Goal State
                    when 0 =>
                        mem_valid_in    <= '1';
                        mem_addr        <= mem_addr_base + GMF_STATE_OFFSET;
                        mem_read        <= '1';
                        
                        -- Memory Flow Control Guard
                        if (mem_ready_in = '1') then
                            if check_mask(mem_latch_data.field_flags,GMF_GOAL_ID_FLAG) then
                                mem_cnt_next    <= 1;
                            elsif check_mask(mem_latch_data.field_flags,GMF_STAMP_FLAG) then
                                mem_cnt_next    <= 5;
                            elsif (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and check_mask(mem_latch_data.field_flags,GMF_DEADLINE_FLAG)) then
                                mem_cnt_next    <= 7;
                            elsif check_mask(mem_latch_data.field_flags,GMF_RESULT_INDEX_FLAG) then
                                mem_cnt_next    <= 9;
                            else
                                mem_cnt_next    <= 10;
                            end if;
                        end if;
                    -- GET Goal ID 1/4
                    when 1 =>
                        mem_valid_in    <= '1';
                        mem_addr        <= mem_addr_base + GMF_GOAL_ID_OFFSET;
                        mem_read        <= '1';
                        
                        -- Memory Flow Control Guard
                        if (mem_ready_in = '1') then
                            mem_cnt_next    <= mem_cnt + 1;
                        end if;
                    -- GET Goal ID 2/4
                    when 2 =>
                        mem_valid_in    <= '1';
                        mem_addr        <= mem_addr_base + GMF_GOAL_ID_OFFSET + 1;
                        mem_read        <= '1';
                        
                        -- Memory Flow Control Guard
                        if (mem_ready_in = '1') then
                            mem_cnt_next    <= mem_cnt + 1;
                        end if;
                    -- GET Goal ID 3/4
                    when 3 =>
                        mem_valid_in    <= '1';
                        mem_addr        <= mem_addr_base + GMF_GOAL_ID_OFFSET + 2;
                        mem_read        <= '1';
                        
                        -- Memory Flow Control Guard
                        if (mem_ready_in = '1') then
                            mem_cnt_next    <= mem_cnt + 1;
                        end if;
                    -- GET Goal ID 4/4
                    when 4 =>
                        mem_valid_in    <= '1';
                        mem_addr        <= mem_addr_base + GMF_GOAL_ID_OFFSET + 3;
                        mem_read        <= '1';
                        
                        -- Memory Flow Control Guard
                        if (mem_ready_in = '1') then
                            if check_mask(mem_latch_data.field_flags,GMF_STAMP_FLAG) then
                                mem_cnt_next    <= 5;
                            elsif (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and check_mask(mem_latch_data.field_flags,GMF_DEADLINE_FLAG)) then
                                mem_cnt_next    <= 7;
                            elsif check_mask(mem_latch_data.field_flags,GMF_RESULT_INDEX_FLAG) then
                                mem_cnt_next    <= 9;
                            else
                                if check_mask(mem_latch_data.field_flags,GMF_STATE_FLAG) then
                                    mem_cnt_next    <= 10;
                                else
                                    mem_cnt_next    <= 11;
                                end if;
                            end if;
                        end if;
                    -- GET Stamp 1/2
                    when 5 =>
                        mem_valid_in    <= '1';
                        mem_addr        <= mem_addr_base + GMF_STAMP_OFFSET;
                        mem_read        <= '1';
                        
                        -- Memory Flow Control Guard
                        if (mem_ready_in = '1') then
                            mem_cnt_next    <= mem_cnt + 1;
                        end if;
                    -- GET Stamp 2/2
                    when 6 =>
                        mem_valid_in    <= '1';
                        mem_addr        <= mem_addr_base + GMF_STAMP_OFFSET + 1;
                        mem_read        <= '1';
                        
                        -- Memory Flow Control Guard
                        if (mem_ready_in = '1') then                            
                            if (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and check_mask(mem_latch_data.field_flags,GMF_DEADLINE_FLAG)) then
                                mem_cnt_next    <= 7;
                            elsif check_mask(mem_latch_data.field_flags,GMF_RESULT_INDEX_FLAG) then
                                mem_cnt_next    <= 9;
                            else
                                if check_mask(mem_latch_data.field_flags,GMF_STATE_FLAG) then
                                    mem_cnt_next    <= 10;
                                elsif check_mask(mem_latch_data.field_flags,GMF_GOAL_ID_FLAG) then
                                    mem_cnt_next    <= 11;
                                else
                                    mem_cnt_next    <= 15;
                                end if;
                            end if;
                        end if;
                    -- GET Deadline 1/2
                    when 7 =>
                        -- Synthesis Guard
                        if (TIMEOUT_DURATION /= ROS_DURATION_INFINITE) then
                            mem_valid_in    <= '1';
                            mem_addr        <= mem_addr_base + GMF_DEADLINE_OFFSET;
                            mem_read        <= '1';
                            
                            -- Memory Flow Control Guard
                            if (mem_ready_in = '1') then
                                mem_cnt_next    <= mem_cnt + 1;
                            end if;
                        end if;
                    -- GET Deadline 2/2
                    when 8 =>
                        -- Synthesis Guard
                        if (TIMEOUT_DURATION /= ROS_DURATION_INFINITE) then
                            mem_valid_in    <= '1';
                            mem_addr        <= mem_addr_base + GMF_DEADLINE_OFFSET + 1;
                            mem_read        <= '1';
                            
                            -- Memory Flow Control Guard
                            if (mem_ready_in = '1') then
                                if check_mask(mem_latch_data.field_flags,GMF_RESULT_INDEX_FLAG) then
                                    mem_cnt_next    <= 9;
                                else
                                    if check_mask(mem_latch_data.field_flags,GMF_STATE_FLAG) then
                                        mem_cnt_next    <= 10;
                                    elsif check_mask(mem_latch_data.field_flags,GMF_GOAL_ID_FLAG) then
                                        mem_cnt_next    <= 11;
                                    elsif check_mask(mem_latch_data.field_flags,GMF_STAMP_FLAG) then
                                        mem_cnt_next    <= 15;
                                    else
                                        mem_cnt_next    <= 17;
                                    end if;
                                end if;
                            end if;
                        end if;
                    -- GET Result Index
                    when 9 =>
                        mem_valid_in    <= '1';
                        mem_addr        <= mem_addr_base + GMF_RESULT_INDEX_OFFSET;
                        mem_read        <= '1';
                        
                        -- Memory Flow Control Guard
                        if (mem_ready_in = '1') then                            
                            if check_mask(mem_latch_data.field_flags,GMF_STATE_FLAG) then
                                mem_cnt_next    <= 10;
                            elsif check_mask(mem_latch_data.field_flags,GMF_GOAL_ID_FLAG) then
                                mem_cnt_next    <= 11;
                            elsif check_mask(mem_latch_data.field_flags,GMF_STAMP_FLAG) then
                                mem_cnt_next    <= 15;
                            elsif (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and check_mask(mem_latch_data.field_flags,GMF_DEADLINE_FLAG)) then
                                mem_cnt_next    <= 17;
                            else
                                mem_cnt_next    <= 19;
                            end if;
                        end if;
                    -- READ Goal State
                    when 10 =>
                        mem_ready_out  <= '1';
                        
                        -- Memory Flow Control Guard
                        if (mem_valid_out = '1') then
                            mem_data_next.state <= mem_read_data(CDR_INT8_WIDTH-1 downto 0);
                            
                            if check_mask(mem_latch_data.field_flags,GMF_GOAL_ID_FLAG) then
                                mem_cnt_next    <= 11;
                            elsif check_mask(mem_latch_data.field_flags,GMF_STAMP_FLAG) then
                                mem_cnt_next    <= 15;
                            elsif (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and check_mask(mem_latch_data.field_flags,GMF_DEADLINE_FLAG)) then
                                mem_cnt_next    <= 17;
                            elsif check_mask(mem_latch_data.field_flags,GMF_RESULT_INDEX_FLAG) then
                                mem_cnt_next    <= 19;
                            else
                                mem_stage_next  <= IDLE;
                            end if;
                        end if;
                    -- READ Goal ID 1/4
                    when 11 =>
                        mem_ready_out  <= '1';
                        
                        -- Memory Flow Control Guard
                        if (mem_valid_out = '1') then
                            mem_data_next.goal_id(0)    <= mem_read_data;
                            
                            mem_cnt_next    <= mem_cnt + 1;
                        end if;
                    -- READ Goal ID 2/4
                    when 12 =>
                        mem_ready_out  <= '1';
                        
                        -- Memory Flow Control Guard
                        if (mem_valid_out = '1') then
                            mem_data_next.goal_id(1)    <= mem_read_data;
                            
                            mem_cnt_next    <= mem_cnt + 1;
                        end if;
                    -- READ Goal ID 3/4
                    when 13 =>
                        mem_ready_out  <= '1';
                        
                        -- Memory Flow Control Guard
                        if (mem_valid_out = '1') then
                            mem_data_next.goal_id(2)    <= mem_read_data;
                            
                            mem_cnt_next    <= mem_cnt + 1;
                        end if;
                    -- READ Goal ID 4/4
                    when 14 =>
                        mem_ready_out  <= '1';
                        
                        -- Memory Flow Control Guard
                        if (mem_valid_out = '1') then
                            mem_data_next.goal_id(3)    <= mem_read_data;
                            
                            if check_mask(mem_latch_data.field_flags,GMF_STAMP_FLAG) then
                                mem_cnt_next    <= 15;
                            elsif (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and check_mask(mem_latch_data.field_flags,GMF_DEADLINE_FLAG)) then
                                mem_cnt_next    <= 17;
                            elsif check_mask(mem_latch_data.field_flags,GMF_RESULT_INDEX_FLAG) then
                                mem_cnt_next    <= 19;
                            else
                                mem_stage_next  <= IDLE;
                            end if;
                        end if;
                    -- READ Stamp 1/2
                    when 15 =>
                        mem_ready_out  <= '1';
                        
                        -- Memory Flow Control Guard
                        if (mem_valid_out = '1') then
                            mem_data_next.stamp.sec     <= mem_read_data;
                            
                            mem_cnt_next    <= mem_cnt + 1;
                        end if;
                    -- READ Stamp 2/2
                    when 16 =>
                        mem_ready_out  <= '1';
                        
                        -- Memory Flow Control Guard
                        if (mem_valid_out = '1') then
                            mem_data_next.stamp.nanosec <= mem_read_data;
                            
                            if (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and check_mask(mem_latch_data.field_flags,GMF_DEADLINE_FLAG)) then
                                mem_cnt_next    <= 17;
                            elsif check_mask(mem_latch_data.field_flags,GMF_RESULT_INDEX_FLAG) then
                                mem_cnt_next    <= 19;
                            else
                                mem_stage_next  <= IDLE;
                            end if;
                        end if;
                    -- READ Deadline 1/2
                    when 17 =>
                        -- Synthesis Guard
                        if (TIMEOUT_DURATION /= ROS_DURATION_INFINITE) then
                            mem_ready_out  <= '1';
                            
                            -- Memory Flow Control Guard
                            if (mem_valid_out = '1') then
                                mem_data_next.deadline.sec  <= mem_read_data;
                                
                                mem_cnt_next    <= mem_cnt + 1;
                            end if;
                        end if;
                    -- READ Deadline 2/2
                    when 18 =>
                        -- Synthesis Guard
                        if (TIMEOUT_DURATION /= ROS_DURATION_INFINITE) then
                            mem_ready_out  <= '1';
                            
                            -- Memory Flow Control Guard
                            if (mem_valid_out = '1') then
                                mem_data_next.deadline.nanosec  <= mem_read_data;
                                
                                if check_mask(mem_latch_data.field_flags,GMF_RESULT_INDEX_FLAG) then
                                    mem_cnt_next    <= 19;
                                else
                                    mem_stage_next  <= IDLE;
                                end if;
                            end if;
                        end if;
                    -- READ Result index
                    when 19 =>
                        mem_ready_out  <= '1';
                        
                        -- Memory Flow Control Guard
                        if (mem_valid_out = '1') then
                            mem_data_next.res_ind   <= mem_read_data;
                            
                            mem_stage_next  <= IDLE;
                        end if;
                    when others =>
                        null;
                end case;
            when GET_NEXT =>
                case (mem_cnt) is
                    -- GET Next Addr
                    when 0 =>
                        mem_valid_in    <= '1';
                        mem_addr        <= mem_addr_base + GMF_NEXT_ADDR_OFFSET;
                        mem_read        <= '1';
                        
                        -- Memory Flow Control Guard
                        if (mem_ready_in = '1') then
                            mem_cnt_next    <= mem_cnt + 1;
                        end if;
                    -- READ Next Addr
                    when 1 =>
                        mem_ready_out  <= '1';
                        
                        -- Memory Flow Control Guard
                        if (mem_valid_out = '1') then
                            mem_addr_base_next  <= resize(unsigned(mem_read_data),GOAL_MEMORY_ADDR_WIDTH);
                            mem_stage_next      <= GET;
                            
                            if check_mask(mem_latch_data.field_flags,GMF_STATE_FLAG) then
                                mem_cnt_next    <= 0;
                            elsif check_mask(mem_latch_data.field_flags,GMF_GOAL_ID_FLAG) then
                                mem_cnt_next    <= 1;
                            elsif check_mask(mem_latch_data.field_flags,GMF_STAMP_FLAG) then
                                mem_cnt_next    <= 5;
                            elsif (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and check_mask(mem_latch_data.field_flags,GMF_DEADLINE_FLAG)) then
                                mem_cnt_next    <= 7;
                            elsif check_mask(mem_latch_data.field_flags,GMF_RESULT_INDEX_FLAG) then
                                mem_cnt_next    <= 9;
                            else
                                -- DONE
                                mem_stage_next  <= IDLE;
                            end if;
                        end if;
                    when others =>
                        null;
                end case;
            when GET_PREV =>
                case (mem_cnt) is
                    -- GET Previous Addr
                    when 0 =>
                        mem_valid_in    <= '1';
                        mem_addr        <= mem_addr_base + GMF_PREV_ADDR_OFFSET;
                        mem_read        <= '1';
                        
                        -- Memory Flow Control Guard
                        if (mem_ready_in = '1') then
                            mem_cnt_next    <= mem_cnt + 1;
                        end if;
                    -- READ Previous Addr
                    when 1 =>
                        mem_ready_out  <= '1';
                        
                        -- Memory Flow Control Guard
                        if (mem_valid_out = '1') then
                            mem_addr_base_next  <= resize(unsigned(mem_read_data),GOAL_MEMORY_ADDR_WIDTH);
                            mem_stage_next      <= GET;
                            
                            if check_mask(mem_latch_data.field_flags,GMF_STATE_FLAG) then
                                mem_cnt_next    <= 0;
                            elsif check_mask(mem_latch_data.field_flags,GMF_GOAL_ID_FLAG) then
                                mem_cnt_next    <= 1;
                            elsif check_mask(mem_latch_data.field_flags,GMF_STAMP_FLAG) then
                                mem_cnt_next    <= 5;
                            elsif (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and check_mask(mem_latch_data.field_flags,GMF_DEADLINE_FLAG)) then
                                mem_cnt_next    <= 7;
                            elsif check_mask(mem_latch_data.field_flags,GMF_RESULT_INDEX_FLAG) then
                                mem_cnt_next    <= 9;
                            else
                                -- DONE
                                mem_stage_next  <= IDLE;
                            end if;
                        end if;
                    when others =>
                        null;
                end case;
            when UPDATE =>
                case (mem_cnt) is
                    -- SET Goal State
                    when 0 =>
                        mem_valid_in    <= '1';
                        mem_addr        <= mem_addr_base + GMF_STATE_OFFSET;
                        mem_write_data  <= (others => '0');
                        mem_write_data(CDR_INT8_WIDTH-1 downto 0)   <= mem_latch_data.state;
                        mem_data_next.state                         <= mem_latch_data.state;
                        
                        -- Memory Flow Control Guard
                        if (mem_ready_in = '1') then
                            if check_mask(mem_latch_data.field_flags,GMF_GOAL_ID_FLAG) then
                                mem_cnt_next    <= 1;
                            elsif check_mask(mem_latch_data.field_flags,GMF_STAMP_FLAG) then
                                mem_cnt_next    <= 5;
                            elsif (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and check_mask(mem_latch_data.field_flags,GMF_DEADLINE_FLAG)) then
                                mem_cnt_next    <= 7;
                            elsif check_mask(mem_latch_data.field_flags,GMF_RESULT_INDEX_FLAG) then
                                mem_cnt_next    <= 9;
                            else
                                mem_stage_next  <= IDLE;
                            end if;
                        end if;
                    -- SET Goal ID 1/4
                    when 1 =>
                        mem_valid_in    <= '1';
                        mem_addr        <= mem_addr_base + GMF_GOAL_ID_OFFSET;
                        mem_write_data  <= mem_latch_data.goal_id(0);
                        
                        -- Memory Flow Control Guard
                        if (mem_ready_in = '1') then
                            mem_cnt_next   <= mem_cnt + 1;
                        end if;
                    -- SET Goal ID 2/4
                    when 2 =>
                        mem_valid_in    <= '1';
                        mem_addr        <= mem_addr_base + GMF_GOAL_ID_OFFSET + 1;
                        mem_write_data  <= mem_latch_data.goal_id(1);
                        
                        -- Memory Flow Control Guard
                        if (mem_ready_in = '1') then
                            mem_cnt_next   <= mem_cnt + 1;
                        end if;
                    -- SET Goal ID 3/4
                    when 3 =>
                        mem_valid_in    <= '1';
                        mem_addr        <= mem_addr_base + GMF_GOAL_ID_OFFSET + 2;
                        mem_write_data  <= mem_latch_data.goal_id(3);
                        
                        -- Memory Flow Control Guard
                        if (mem_ready_in = '1') then
                            mem_cnt_next   <= mem_cnt + 1;
                        end if;
                    -- SET Goal ID 4/4
                    when 4 =>
                        mem_valid_in    <= '1';
                        mem_addr        <= mem_addr_base + GMF_GOAL_ID_OFFSET + 3;
                        mem_write_data  <= mem_latch_data.goal_id(3);
                        mem_data_next.goal_id   <= mem_latch_data.goal_id;
                        
                        -- Memory Flow Control Guard
                        if (mem_ready_in = '1') then
                            if check_mask(mem_latch_data.field_flags,GMF_STAMP_FLAG) then
                                mem_cnt_next    <= 5;
                            elsif (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and check_mask(mem_latch_data.field_flags,GMF_DEADLINE_FLAG)) then
                                mem_cnt_next    <= 7;
                            elsif check_mask(mem_latch_data.field_flags,GMF_RESULT_INDEX_FLAG) then
                                mem_cnt_next    <= 9;
                            else
                                mem_stage_next  <= IDLE;
                            end if;
                        end if;
                    -- SET Stamp 1/2
                    when 5 =>
                        mem_valid_in    <= '1';
                        mem_addr        <= mem_addr_base + GMF_STAMP_OFFSET;
                        mem_write_data  <= mem_latch_data.stamp.sec;
                        
                        -- Memory Flow Control Guard
                        if (mem_ready_in = '1') then
                            mem_cnt_next   <= mem_cnt + 1;
                        end if;
                    -- SET Stamp 2/2
                    when 6 =>
                        mem_valid_in    <= '1';
                        mem_addr        <= mem_addr_base + GMF_STAMP_OFFSET + 1;
                        mem_write_data  <= mem_latch_data.stamp.nanosec;
                        mem_data_next.stamp <= mem_latch_data.stamp;
                        
                        -- Memory Flow Control Guard
                        if (mem_ready_in = '1') then
                            if (TIMEOUT_DURATION /= ROS_DURATION_INFINITE and check_mask(mem_latch_data.field_flags,GMF_DEADLINE_FLAG)) then
                                mem_cnt_next    <= 7;
                            elsif check_mask(mem_latch_data.field_flags,GMF_RESULT_INDEX_FLAG) then
                                mem_cnt_next    <= 9;
                            else
                                mem_stage_next  <= IDLE;
                            end if;
                        end if;
                    -- SET Deadline 1/2
                    when 7 =>
                        mem_valid_in    <= '1';
                        mem_addr        <= mem_addr_base + GMF_DEADLINE_OFFSET;
                        mem_write_data  <= mem_latch_data.deadline.sec;
                        
                        -- Memory Flow Control Guard
                        if (mem_ready_in = '1') then
                            mem_cnt_next   <= mem_cnt + 1;
                        end if;
                    -- SET Deadline 2/2
                    when 8 =>
                        mem_valid_in    <= '1';
                        mem_addr        <= mem_addr_base + GMF_DEADLINE_OFFSET + 1;
                        mem_write_data  <= mem_latch_data.deadline.sec;
                        
                        -- Memory Flow Control Guard
                        if (mem_ready_in = '1') then
                            if check_mask(mem_latch_data.field_flags,GMF_RESULT_INDEX_FLAG) then
                                mem_cnt_next    <= 9;
                            else
                                mem_stage_next  <= IDLE;
                            end if;
                        end if;
                    -- SET Result Index
                    when 9 =>
                        mem_valid_in    <= '1';
                        mem_addr        <= mem_addr_base + GMF_RESULT_INDEX_OFFSET;
                        mem_write_data  <= mem_latch_data.res_ind;
                        mem_data_next.res_ind   <= mem_latch_data.res_ind;
                        
                        -- Memory Flow Control Guard
                        if (mem_ready_in = '1') then
                            -- DONE
                            mem_stage_next  <= IDLE;
                        end if;
                    when others =>
                        null;
                end case;
            when REMOVE =>
                case (mem_cnt) is
                    -- GET Next Addr
                    when 0 =>
                        mem_valid_in    <= '1';
                        mem_addr        <= mem_addr_base + GMF_NEXT_ADDR_OFFSET;
                        mem_read        <= '1';
                        
                        -- Memory Flow Control Guard
                        if (mem_ready_in = '1') then
                            mem_cnt_next    <= mem_cnt + 1;
                        end if;
                    -- GET Previous Addr
                    when 1 =>
                        mem_valid_in    <= '1';
                        mem_addr        <= mem_addr_base + GMF_PREV_ADDR_OFFSET;
                        mem_read        <= '1';
                        
                        -- Memory Flow Control Guard
                        if (mem_ready_in = '1') then
                            mem_cnt_next    <= mem_cnt + 1;
                        end if;
                    -- SET Next Addr
                    when 2 =>
                        mem_valid_in    <= '1';
                        mem_addr        <= mem_addr_base + GMF_NEXT_ADDR_OFFSET;
                        mem_write_data  <= std_logic_vector(resize(GOAL_MEMORY_MAX_ADDRESS,WORD_WIDTH));
                        
                        -- Memory Flow Control Guard
                        if (mem_ready_in = '1') then
                            -- Set New Empty Head
                            mem_empty_head_next <= mem_addr_base;
                            mem_cnt_next        <= mem_cnt + 1;
                        end if;
                    -- SET Next Addr (Current Tail)
                    when 3 =>
                        mem_valid_in    <= '1';
                        mem_addr        <= mem_empty_tail + GMF_NEXT_ADDR_OFFSET;
                        mem_write_data  <= std_logic_vector(resize(mem_addr_base,WORD_WIDTH));
                        
                        -- Memory Flow Control Guard
                        if (mem_ready_in = '1') then
                            -- Set New Empty TAIL
                            mem_empty_tail_next <= mem_addr_base;
                            mem_cnt_next        <= mem_cnt + 1;
                        end if;
                    -- READ Next Addr
                    when 4 =>
                        mem_ready_out  <= '1';
                        
                        -- Memory Flow Control Guard
                        if (mem_valid_out = '1') then
                            mem_addr_latch_next <= resize(unsigned(mem_read_data),GOAL_MEMORY_ADDR_WIDTH);
                            mem_cnt_next        <= mem_cnt + 1;
                        end if;
                    -- READ Previous Addr
                    when 5 =>
                        mem_ready_out  <= '1';
                        
                        -- Memory Flow Control Guard
                        if (mem_valid_out = '1') then
                            -- Set mem_data.addr to previous slot
                            mem_data_next.addr  <= resize(unsigned(mem_read_data),GOAL_MEMORY_ADDR_WIDTH);
                            
                            if (mem_addr_latch = GOAL_MEMORY_MAX_ADDRESS) then
                                -- Set New Occupied Tail
                                mem_occupied_tail_next  <= resize(unsigned(mem_read_data),GOAL_MEMORY_ADDR_WIDTH);
                                
                                if (resize(unsigned(mem_read_data),GOAL_MEMORY_ADDR_WIDTH) = GOAL_MEMORY_MAX_ADDRESS) then
                                    -- Reset Occupied Head
                                    mem_occupied_head_next  <= GOAL_MEMORY_MAX_ADDRESS;
                                    mem_stage_next          <= IDLE;
                                else
                                    mem_addr_base_next      <= resize(unsigned(mem_read_data),GOAL_MEMORY_ADDR_WIDTH);
                                    mem_cnt_next            <= mem_cnt + 2;
                                end if;
                            else
                                mem_addr_base_next  <= mem_addr_latch;
                                mem_addr_latch_next <= resize(unsigned(mem_read_data),GOAL_MEMORY_ADDR_WIDTH);
                                mem_cnt_next        <= mem_cnt + 1;
                            end if;
                        end if;
                    -- SET Previous Addr (Next Slot)
                    when 6 =>
                        mem_valid_in    <= '1';
                        mem_addr        <= mem_addr_base + GMF_PREV_ADDR_OFFSET;
                        mem_write_data  <= std_logic_vector(resize(mem_addr_latch,WORD_WIDTH));
                        
                        -- Memory Flow Control Guard
                        if (mem_ready_in = '1') then
                            if (mem_addr_latch = GOAL_MEMORY_MAX_ADDRESS) then
                                -- Set New Occupied Head
                                mem_occupied_head_next  <= mem_addr_base;
                                -- DONE
                                mem_stage_next  <= IDLE;
                            else
                                mem_addr_base_next  <= mem_addr_latch;
                                mem_addr_latch_next <= mem_addr_base;
                                mem_cnt_next        <= mem_cnt + 1;
                            end if;
                        end if;
                    -- SET Next Addr (Previous Slot)
                    when 7 =>
                        mem_valid_in    <= '1';
                        mem_addr        <= mem_addr_base + GMF_NEXT_ADDR_OFFSET;
                        mem_write_data  <= std_logic_vector(resize(mem_addr_latch,WORD_WIDTH));
                        
                        -- Memory Flow Control Guard
                        if (mem_ready_in = '1') then
                            -- DONE
                            mem_stage_next  <= IDLE;
                        end if;
                    when others =>
                        null;
                end case;
            when RESET_MEMORY =>
                case (mem_cnt) is
                    -- SET Result Index
                    when 0 =>
                        mem_valid_in    <= '1';
                        mem_addr        <= mem_addr_base + GMF_RESULT_INDEX_OFFSET;
                        mem_write_data  <= std_logic_vector(empty_head_res_ind);
                        
                        -- Memory Flow Control Guard
                        if (mem_ready_in = '1') then
                            mem_cnt_next    <= mem_cnt + 1;
                        end if;
                    -- SET Next Pointer
                    when 1 =>
                        mem_valid_in    <= '1';
                        mem_addr        <= mem_addr_base + GMF_NEXT_ADDR_OFFSET;
                        if (mem_addr_base = MAX_GOAL_ADDRESS) then
                            mem_write_data  <= std_logic_vector(resize(GOAL_MEMORY_MAX_ADDRESS,WORD_WIDTH));
                        else
                            mem_write_data  <= std_logic_vector(resize(mem_addr_base + GOAL_FRAME_SIZE,WORD_WIDTH));
                        end if;
                        
                        -- Memory Flow Control Guard
                        if (mem_ready_in = '1') then
                            mem_cnt_next    <= mem_cnt + 1;
                        end if;
                    -- SET Previous Pointer
                    when 2 =>
                        mem_valid_in    <= '1';
                        mem_addr        <= mem_addr_base + GMF_PREV_ADDR_OFFSET;
                        mem_write_data  <= std_logic_vector(resize(mem_addr_latch,WORD_WIDTH));
                        
                        -- Memory Flow Control Guard
                        if (mem_ready_in = '1') then
                            if (mem_addr_base = MAX_GOAL_ADDRESS) then
                                -- Initialize Empty and Occupied Heads
                                mem_empty_head_next     <= FIRST_GOAL_ADDRESS;
                                mem_empty_tail_next     <= MAX_GOAL_ADDRESS;
                                mem_occupied_head_next  <= GOAL_MEMORY_MAX_ADDRESS;
                                mem_occupied_tail_next  <= GOAL_MEMORY_MAX_ADDRESS;
                                empty_head_res_ind_next <= (others => '0');
                                
                                -- DONE
                                mem_stage_next  <= IDLE;
                            else
                                empty_head_res_ind_next <= empty_head_res_ind + 1;
                                mem_addr_latch_next     <= mem_addr_base;
                                mem_addr_base_next      <= mem_addr_base + GOAL_FRAME_SIZE;
                                mem_cnt_next            <= 0;
                            end if;
                        end if;
                    when others =>
                        null;
                end case;
        end case;
    end process;
    
    sync_prc : process(clk)
    begin
        if rising_edge(clk) then
            if (reset = '1') then
                stage               <= RESET_MEMORY;
                goal_stage          <= IDLE;
                cancel_stage        <= IDLE;
                status_stage        <= IDLE;
                mem_stage           <= RESET_MEMORY;
                mem_data_r          <= ZERO_GOAL_DATA;
                mem_data_c          <= ZERO_GOAL_DATA;
                mem_data_s          <= ZERO_GOAL_DATA;
                mem_latch_data      <= ZERO_GOAL_DATA;
                mem_data            <= ZERO_GOAL_DATA;
                rrq_addr_base       <= FIRST_RRQ_ADDRESS;
                rrq_addr_latch      <= RRQ_MEMORY_MAX_ADDRESS;
                mem_addr_base       <= FIRST_GOAL_ADDRESS;
                mem_addr_latch      <= GOAL_MEMORY_MAX_ADDRESS;
                rrq_empty_head      <= RRQ_MEMORY_MAX_ADDRESS;
                rrq_occupied_head   <= RRQ_MEMORY_MAX_ADDRESS;
                mem_occupied_head   <= GOAL_MEMORY_MAX_ADDRESS;
                mem_occupied_tail   <= GOAL_MEMORY_MAX_ADDRESS;
                mem_empty_head      <= GOAL_MEMORY_MAX_ADDRESS;
                mem_empty_tail      <= GOAL_MEMORY_MAX_ADDRESS;
                return_code_latch   <= ROS_RET_OK;
                request_id_latch    <= EMPTY_REQUEST_ID;
                check_time          <= ROS_TIME_INFINITE;
                time_latch          <= ROS_TIME_INVALID;
                goal_handle_latch   <= GOAL_HANDLE_UNKNOWN;
                goal_state_latch    <= GoalStatus_package.STATUS_UNKNOWN;
                g_stamp             <= ROS_TIME_INVALID;
                search_type_c       <= SEARCH_NONE;
                cancel_ret_code     <= CancelGoal_package.RR_ERROR_NONE;
                cnt                 <= 0;
                terminal_cnt        <= 0;
                c_cnt               <= 0;
                goals_canceling_cnt <= 0;
                status_list_cnt     <= 0;
                mem_cnt             <= 0;
                trigger_result      <= '0';
                trigger_status      <= '1'; -- Trigger Status Update immediately
                g_accept            <= '0';
                empty_head_res_ind  <= (others => '0');
            else
                stage               <= stage_next;
                goal_stage          <= goal_stage_next;
                cancel_stage        <= cancel_stage_next;
                status_stage        <= status_stage_next;
                mem_stage           <= mem_stage_next;
                mem_data_r          <= mem_data_r_next;
                mem_data_c          <= mem_data_c_next;
                mem_data_s          <= mem_data_s_next;
                mem_latch_data      <= mem_latch_data_next;
                mem_data            <= mem_data_next;
                rrq_addr_base       <= rrq_addr_base_next;
                rrq_addr_latch      <= rrq_addr_latch_next;
                mem_addr_base       <= mem_addr_base_next;
                mem_addr_latch      <= mem_addr_latch_next;
                rrq_empty_head      <= rrq_empty_head_next;
                rrq_occupied_head   <= rrq_occupied_head_next;
                mem_occupied_head   <= mem_occupied_head_next;
                mem_occupied_tail   <= mem_occupied_tail_next;
                mem_empty_head      <= mem_empty_head_next;
                mem_empty_tail      <= mem_empty_tail_next;
                return_code_latch   <= return_code_latch_next;
                request_id_latch    <= request_id_latch_next;
                check_time          <= check_time_next;
                time_latch          <= time_latch_next;
                goal_handle_latch   <= goal_handle_latch_next;
                goal_state_latch    <= goal_state_latch_next;
                g_stamp             <= g_stamp_next;
                search_type_c       <= search_type_c_next;
                cancel_ret_code     <= cancel_ret_code_next;
                cnt                 <= cnt_next;
                terminal_cnt        <= terminal_cnt_next;
                c_cnt               <= c_cnt_next;
                goals_canceling_cnt <= goals_canceling_cnt_next;
                status_list_cnt     <= status_list_cnt_next;
                mem_cnt             <= mem_cnt_next;
                trigger_result      <= trigger_result_next;
                trigger_status      <= trigger_status_next;
                g_accept            <= g_accept_next;
                empty_head_res_ind  <= empty_head_res_ind_next;
            end if;
        end if;
    end process;
    
end architecture;
