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

entity Fibonacci_ros_action_server is
    generic (
        TIMEOUT_DURATION    : ROS_DURATION_TYPE;
        MAX_GOALS           : natural;
        MAX_RESULT_REQUESTS : natural;
        ENABLE_FEEDBACK     : std_logic := '1';
        LITTLE_ENDIAN       : std_logic := '0'
    );
    port (
        -- SYSTEM
        clk                         : in std_logic;
        reset                       : in std_logic;
        time                        : in ROS_TIME_TYPE;
        -- *GOAL SERVICE*
        -- DDS READER
        g_start_r                   : out std_logic;
        g_ack_r                     : in std_logic;
        g_opcode_r                  : out DDS_READER_OPCODE_TYPE;
        g_instance_state_r          : out std_logic_vector(INSTANCE_STATE_KIND_WIDTH-1 downto 0);
        g_view_state_r              : out std_logic_vector(VIEW_STATE_KIND_WIDTH-1 downto 0);
        g_sample_state_r            : out std_logic_vector(SAMPLE_STATE_KIND_WIDTH-1 downto 0);
        g_instance_handle_r         : out INSTANCE_HANDLE_TYPE;
        g_max_samples_r             : out std_logic_vector(MAX_SAMPLES_WIDTH-1 downto 0);
        g_get_data_r                : out std_logic;
        g_done_r                    : in std_logic;
        g_return_code_r             : in std_logic_vector(RETURN_CODE_WIDTH-1 downto 0);
        g_valid_in_r                : in std_logic;
        g_ready_in_r                : out std_logic;
        g_data_in_r                 : in std_logic_vector(WORD_WIDTH-1 downto 0);
        g_last_word_in_r            : in std_logic;
        g_sample_info_r             : in SAMPLE_INFO_TYPE;
        g_sample_info_valid_r       : in std_logic;
        g_sample_info_ack_r         : out std_logic;
        g_eoc_r                     : in std_logic;
        g_status_r                  : in std_logic_vector(STATUS_KIND_WIDTH-1 downto 0);
        -- DDS WRITER
        g_start_w                   : out std_logic;
        g_ack_w                     : in std_logic;
        g_opcode_w                  : out DDS_WRITER_OPCODE_TYPE;
        g_instance_handle_in_w      : out INSTANCE_HANDLE_TYPE;
        g_source_ts_w               : out TIME_TYPE;
        g_max_wait_w                : out DURATION_TYPE;
        g_done_w                    : in std_logic;
        g_return_code_w             : in std_logic_vector(RETURN_CODE_WIDTH-1 downto 0);
        g_instance_handle_out_w     : in INSTANCE_HANDLE_TYPE;
        g_valid_out_w               : out std_logic;
        g_ready_out_w               : in std_logic;
        g_data_out_w                : out std_logic_vector(WORD_WIDTH-1 downto 0);
        g_last_word_out_w           : out std_logic;
        g_valid_in_w                : in std_logic;
        g_ready_in_w                : out std_logic;
        g_data_in_w                 : in std_logic_vector(WORD_WIDTH-1 downto 0);
        g_last_word_in_w            : in std_logic;
        g_status_w                  : in std_logic_vector(STATUS_KIND_WIDTH-1 downto 0);
        -- *RESULT SERVICE*
        -- DDS READER
        r_start_r                   : out std_logic;
        r_ack_r                     : in std_logic;
        r_opcode_r                  : out DDS_READER_OPCODE_TYPE;
        r_instance_state_r          : out std_logic_vector(INSTANCE_STATE_KIND_WIDTH-1 downto 0);
        r_view_state_r              : out std_logic_vector(VIEW_STATE_KIND_WIDTH-1 downto 0);
        r_sample_state_r            : out std_logic_vector(SAMPLE_STATE_KIND_WIDTH-1 downto 0);
        r_instance_handle_r         : out INSTANCE_HANDLE_TYPE;
        r_max_samples_r             : out std_logic_vector(MAX_SAMPLES_WIDTH-1 downto 0);
        r_get_data_r                : out std_logic;
        r_done_r                    : in std_logic;
        r_return_code_r             : in std_logic_vector(RETURN_CODE_WIDTH-1 downto 0);
        r_valid_in_r                : in std_logic;
        r_ready_in_r                : out std_logic;
        r_data_in_r                 : in std_logic_vector(WORD_WIDTH-1 downto 0);
        r_last_word_in_r            : in std_logic;
        r_sample_info_r             : in SAMPLE_INFO_TYPE;
        r_sample_info_valid_r       : in std_logic;
        r_sample_info_ack_r         : out std_logic;
        r_eoc_r                     : in std_logic;
        r_status_r                  : in std_logic_vector(STATUS_KIND_WIDTH-1 downto 0);
        -- DDS WRITER
        r_start_w                   : out std_logic;
        r_ack_w                     : in std_logic;
        r_opcode_w                  : out DDS_WRITER_OPCODE_TYPE;
        r_instance_handle_in_w      : out INSTANCE_HANDLE_TYPE;
        r_source_ts_w               : out TIME_TYPE;
        r_max_wait_w                : out DURATION_TYPE;
        r_done_w                    : in std_logic;
        r_return_code_w             : in std_logic_vector(RETURN_CODE_WIDTH-1 downto 0);
        r_instance_handle_out_w     : in INSTANCE_HANDLE_TYPE;
        r_valid_out_w               : out std_logic;
        r_ready_out_w               : in std_logic;
        r_data_out_w                : out std_logic_vector(WORD_WIDTH-1 downto 0);
        r_last_word_out_w           : out std_logic;
        r_valid_in_w                : in std_logic;
        r_ready_in_w                : out std_logic;
        r_data_in_w                 : in std_logic_vector(WORD_WIDTH-1 downto 0);
        r_last_word_in_w            : in std_logic;
        r_status_w                  : in std_logic_vector(STATUS_KIND_WIDTH-1 downto 0);
        -- *CANCEL SERVICE*
        -- DDS READER
        c_start_r                   : out std_logic;
        c_ack_r                     : in std_logic;
        c_opcode_r                  : out DDS_READER_OPCODE_TYPE;
        c_instance_state_r          : out std_logic_vector(INSTANCE_STATE_KIND_WIDTH-1 downto 0);
        c_view_state_r              : out std_logic_vector(VIEW_STATE_KIND_WIDTH-1 downto 0);
        c_sample_state_r            : out std_logic_vector(SAMPLE_STATE_KIND_WIDTH-1 downto 0);
        c_instance_handle_r         : out INSTANCE_HANDLE_TYPE;
        c_max_samples_r             : out std_logic_vector(MAX_SAMPLES_WIDTH-1 downto 0);
        c_get_data_r                : out std_logic;
        c_done_r                    : in std_logic;
        c_return_code_r             : in std_logic_vector(RETURN_CODE_WIDTH-1 downto 0);
        c_valid_in_r                : in std_logic;
        c_ready_in_r                : out std_logic;
        c_data_in_r                 : in std_logic_vector(WORD_WIDTH-1 downto 0);
        c_last_word_in_r            : in std_logic;
        c_sample_info_r             : in SAMPLE_INFO_TYPE;
        c_sample_info_valid_r       : in std_logic;
        c_sample_info_ack_r         : out std_logic;
        c_eoc_r                     : in std_logic;
        c_status_r                  : in std_logic_vector(STATUS_KIND_WIDTH-1 downto 0);
        -- DDS WRITER
        c_start_w                   : out std_logic;
        c_ack_w                     : in std_logic;
        c_opcode_w                  : out DDS_WRITER_OPCODE_TYPE;
        c_instance_handle_in_w      : out INSTANCE_HANDLE_TYPE;
        c_source_ts_w               : out TIME_TYPE;
        c_max_wait_w                : out DURATION_TYPE;
        c_done_w                    : in std_logic;
        c_return_code_w             : in std_logic_vector(RETURN_CODE_WIDTH-1 downto 0);
        c_instance_handle_out_w     : in INSTANCE_HANDLE_TYPE;
        c_valid_out_w               : out std_logic;
        c_ready_out_w               : in std_logic;
        c_data_out_w                : out std_logic_vector(WORD_WIDTH-1 downto 0);
        c_last_word_out_w           : out std_logic;
        c_valid_in_w                : in std_logic;
        c_ready_in_w                : out std_logic;
        c_data_in_w                 : in std_logic_vector(WORD_WIDTH-1 downto 0);
        c_last_word_in_w            : in std_logic;
        c_status_w                  : in std_logic_vector(STATUS_KIND_WIDTH-1 downto 0);
        -- *FEEDBACK PUBLISHER*
        f_start_dds                 : out std_logic;
        f_ack_dds                   : in std_logic;
        f_opcode_dds                : out DDS_WRITER_OPCODE_TYPE;
        f_instance_handle_in_dds    : out INSTANCE_HANDLE_TYPE;
        f_source_ts_dds             : out TIME_TYPE;
        f_max_wait_dds              : out DURATION_TYPE;
        f_done_dds                  : in std_logic;
        f_return_code_dds           : in std_logic_vector(RETURN_CODE_WIDTH-1 downto 0);
        f_instance_handle_out_dds   : in INSTANCE_HANDLE_TYPE;
        f_valid_out_dds             : out std_logic;
        f_ready_out_dds             : in std_logic;
        f_data_out_dds              : out std_logic_vector(WORD_WIDTH-1 downto 0);
        f_last_word_out_dds         : out std_logic;
        f_valid_in_dds              : in std_logic;
        f_ready_in_dds              : out std_logic;
        f_data_in_dds               : in std_logic_vector(WORD_WIDTH-1 downto 0);
        f_last_word_in_dds          : in std_logic;
        f_status_dds                : in std_logic_vector(STATUS_KIND_WIDTH-1 downto 0);
        -- *STATUS PUBLISHER*
        s_start_dds                 : out std_logic;
        s_ack_dds                   : in std_logic;
        s_opcode_dds                : out DDS_WRITER_OPCODE_TYPE;
        s_instance_handle_in_dds    : out INSTANCE_HANDLE_TYPE;
        s_source_ts_dds             : out TIME_TYPE;
        s_max_wait_dds              : out DURATION_TYPE;
        s_done_dds                  : in std_logic;
        s_return_code_dds           : in std_logic_vector(RETURN_CODE_WIDTH-1 downto 0);
        s_instance_handle_out_dds   : in INSTANCE_HANDLE_TYPE;
        s_valid_out_dds             : out std_logic;
        s_ready_out_dds             : in std_logic;
        s_data_out_dds              : out std_logic_vector(WORD_WIDTH-1 downto 0);
        s_last_word_out_dds         : out std_logic;
        s_valid_in_dds              : in std_logic;
        s_ready_in_dds              : out std_logic;
        s_data_in_dds               : in std_logic_vector(WORD_WIDTH-1 downto 0);
        s_last_word_in_dds          : in std_logic;
        s_status_dds                : in std_logic_vector(STATUS_KIND_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);
        -- ###GENERATED START###
        new_goal_order              : out std_logic_vector(CDR_LONG_WIDTH-1 downto 0);
        -- ###GENERATED END###
        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;
        -- RESULT SIGNALS
        index_result                : in std_logic_vector(WORD_WIDTH-1 downto 0);
        index_result_wen            : in std_logic;
        index_result_ready          : out std_logic;
        -- ###GENERATED START###
        result_seq_len              : in std_logic_vector(R_RR_SEQ_ADDR_WIDTH-1 downto 0);
        result_seq_addr             : in std_logic_vector(R_RR_SEQ_ADDR_WIDTH-1 downto 0);
        result_seq_ready            : out std_logic;
        result_seq_ren              : in std_logic;
        result_seq_wen              : in std_logic;
        result_seq_valid            : out std_logic;
        result_seq_ack              : in std_logic;
        result_seq_r                : out std_logic_vector(CDR_LONG_WIDTH-1 downto 0);
        result_seq_w                : in std_logic_vector(CDR_LONG_WIDTH-1 downto 0);
        -- ###GENERATED END###
        -- FEEDBACK SIGNALS
        -- ###GENERATED START###
        feedback_seq_len            : in std_logic_vector(F_SEQ_ADDR_WIDTH-1 downto 0);
        feedback_seq_addr           : in std_logic_vector(F_SEQ_ADDR_WIDTH-1 downto 0);
        feedback_seq_ready          : out std_logic;
        feedback_seq_ren            : in std_logic;
        feedback_seq_wen            : in std_logic;
        feedback_seq_valid          : out std_logic;
        feedback_seq_ack            : in std_logic;
        feedback_seq_r              : out std_logic_vector(CDR_LONG_WIDTH-1 downto 0);
        feedback_seq_w              : in std_logic_vector(CDR_LONG_WIDTH-1 downto 0)
        -- ###GENERATED END###
    );
end entity;

architecture arch of Fibonacci_ros_action_server is
    
    --*****CONSTANT DECLARATION*****
    constant RESULT_INDEX_WIDTH : natural := log2c(MAX_GOALS);
    
    --*****TYPE DECLARATION*****
    type STAGE_TYPE is (IDLE,GET_RESULT,READ_RESULT,PASSTHROUGH,PUSH_SEQUENCE);
    -- ###GENERATED START###
    type R_SEQ_ADDR_TYPE is array (0 to MAX_GOALS-1) of std_logic_vector(R_RR_SEQ_ADDR_WIDTH-1 downto 0);
    type R_SEQ_DATA_TYPE is array (0 to MAX_GOALS-1) of std_logic_vector(CDR_LONG_WIDTH-1 downto 0);
    -- ###GENERATED END###
    
    --*****SIGNAL DECLARATION*****
    signal abort_mem : std_logic;
    signal stage, stage_next : STAGE_TYPE;
    signal cnt, cnt_next : natural range 0 to 5;
    signal g_start_user, r_start_user, c_start_user, g_ack_user, r_ack_user, c_ack_user, g_data_available_user, r_data_available_user, c_data_available_user, g_taken_user, r_taken_user, c_taken_user, g_done_user, r_done_user, c_done_user : std_logic;
    signal g_opcode_user, r_opcode_user, c_opcode_user : ROS_SERVICE_OPCODE_TYPE;
    signal g_service_info_user, r_service_info_user, c_service_info_user : SERVICE_INFO_TYPE;
    signal g_request_id_user, r_request_id_user, c_request_id_user : REQUEST_ID_TYPE;
    signal g_return_code_user, r_return_code_user, c_return_code_user : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0);
    signal g_goal_id, r_goal_id, f_goal_id : std_logic_vector(UUID_WIDTH-1 downto 0);
    signal g_accepted : std_logic;
    signal g_stamp : std_logic_vector(ROS_TIME_WIDTH-1 downto 0);
    signal r_status : std_logic_vector(CDR_INT8_WIDTH-1 downto 0);
    signal r_index : std_logic_vector(WORD_WIDTH-1 downto 0);
    signal r_sel, r_sel_ack :std_logic;
    signal c_goal_info_goal_id  : std_logic_vector(UUID_WIDTH-1 downto 0);
    signal c_goal_info_stamp : std_logic_vector(ROS_TIME_WIDTH-1 downto 0);
    signal c_return_code : std_logic_vector(CDR_INT8_WIDTH-1 downto 0);
    signal c_goals_canceling_len : std_logic_vector(CancelGoal_package.RR_GOALS_CANCELING_ADDR_WIDTH-1 downto 0);
    signal c_goals_canceling_addr : std_logic_vector(CancelGoal_package.RR_GOALS_CANCELING_ADDR_WIDTH-1 downto 0);
    signal c_goals_canceling_ready, c_goals_canceling_ren, c_goals_canceling_wen, c_goals_canceling_valid, c_goals_canceling_ack : std_logic;
    signal c_goals_canceling_goal_id_r, c_goals_canceling_goal_id_w : std_logic_vector(UUID_WIDTH-1 downto 0);
    signal c_goals_canceling_stamp_r, c_goals_canceling_stamp_w : std_logic_vector(ROS_TIME_WIDTH-1 downto 0);
    signal s_start_user, f_start_user, s_ack_user, f_ack_user, s_done_user, f_done_user : std_logic;
    signal s_opcode_user, f_opcode_user : ROS_TOPIC_OPCODE_TYPE;
    signal s_return_code_user, f_return_code_user : std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0);
    signal s_status_list_len, s_status_list_addr : std_logic_vector(GoalStatusArray_package.STATUS_LIST_ADDR_WIDTH-1 downto 0);
    signal s_status_list_ready, s_status_list_ren, s_status_list_wen, s_status_list_valid, s_status_list_ack : std_logic;
    signal s_status_list_goal_info_goal_id_r, s_status_list_goal_info_goal_id_w : std_logic_vector(UUID_WIDTH-1 downto 0);
    signal s_status_list_goal_info_stamp_r, s_status_list_goal_info_stamp_w : std_logic_vector(ROS_TIME_WIDTH-1 downto 0);
    signal s_status_list_status_r, s_status_list_status_w : std_logic_vector(CDR_INT8_WIDTH-1 downto 0);
    signal result_index_latch, result_index_latch_next : std_logic_vector(RESULT_INDEX_WIDTH-1 downto 0);
    -- ###GENERATED START###
    signal seq_len_sig, seq_addr_sig : std_logic_vector(R_RR_SEQ_ADDR_WIDTH-1 downto 0);
    signal seq_ready_sig, seq_wen_sig : std_logic;
    signal seq_w_sig : std_logic_vector(CDR_LONG_WIDTH-1 downto 0);
    
    signal seq_cnt, seq_cnt_next : natural range 0 to R_RR_SEQ_MAX_DEPTH-1;
    -- r_seq_mem Signals
    signal r_seq_mem_addr : R_SEQ_ADDR_TYPE;
    signal r_seq_mem_read, r_seq_mem_ready_in, r_seq_mem_ready_out, r_seq_mem_valid_in, r_seq_mem_valid_out : std_logic_vector(0 to MAX_GOALS-1);
    signal r_seq_mem_data_in, r_seq_mem_data_out : R_SEQ_DATA_TYPE;
    -- r_seq_len_mem Signals
    signal r_seq_len_mem_addr : std_logic_vector(RESULT_INDEX_WIDTH-1 downto 0);
    signal r_seq_len_mem_read, r_seq_len_mem_ready_in, r_seq_len_mem_ready_out, r_seq_len_mem_valid_in, r_seq_len_mem_valid_out : std_logic;
    signal r_seq_len_mem_data_in, r_seq_len_mem_data_out : std_logic_vector(R_RR_SEQ_ADDR_WIDTH-1 downto 0);
    -- ###GENERATED END###
    
begin
    
    goal_srv_server_inst : entity work.Fibonacci_ros_action_goal_srv_server(arch)
        generic map (
            LITTLE_ENDIAN   => LITTLE_ENDIAN
        )
        port map (
            clk                     => clk,
            reset                   => reset,
            start_r                 => g_start_r,
            ack_r                   => g_ack_r,
            opcode_r                => g_opcode_r,
            instance_state_r        => g_instance_state_r,
            view_state_r            => g_view_state_r,
            sample_state_r          => g_sample_state_r,
            instance_handle_r       => g_instance_handle_r,
            max_samples_r           => g_max_samples_r,
            get_data_r              => g_get_data_r,
            done_r                  => g_done_r,
            return_code_r           => g_return_code_r,
            valid_in_r              => g_valid_in_r,
            ready_in_r              => g_ready_in_r,
            data_in_r               => g_data_in_r,
            last_word_in_r          => g_last_word_in_r,
            sample_info_r           => g_sample_info_r,
            sample_info_valid_r     => g_sample_info_valid_r,
            sample_info_ack_r       => g_sample_info_ack_r,
            eoc_r                   => g_eoc_r,
            status_r                => g_status_r,
            start_w                 => g_start_w,
            ack_w                   => g_ack_w,
            opcode_w                => g_opcode_w,
            instance_handle_out_w   => g_instance_handle_out_w,
            source_ts_w             => g_source_ts_w,
            max_wait_w              => g_max_wait_w,
            done_w                  => g_done_w,
            return_code_w           => g_return_code_w,
            instance_handle_in_w    => g_instance_handle_in_w,
            valid_out_w             => g_valid_out_w,
            ready_out_w             => g_ready_out_w,
            data_out_w              => g_data_out_w,
            last_word_out_w         => g_last_word_out_w,
            valid_in_w              => g_valid_in_w,
            ready_in_w              => g_ready_in_w,
            data_in_w               => g_data_in_w,
            last_word_in_w          => g_last_word_in_w,
            status_w                => g_status_w,
            start_user              => g_start_user,
            ack_user                => g_ack_user,
            opcode_user             => g_opcode_user,
            service_info_user       => g_service_info_user,
            request_id_user         => g_request_id_user,
            data_available_user     => g_data_available_user,
            taken_user              => g_taken_user,
            goal_id                 => g_goal_id,
            -- ###GENERATED START###
            order                   => new_goal_order,
            -- ###GENERATED END###
            accepted                => g_accepted,
            stamp                   => g_stamp,
            done_user               => g_done_user,
            return_code_user        => g_return_code_user
        );
    
    result_srv_server_inst : entity work.Fibonacci_ros_action_result_srv_server(arch)
        generic map (
            LITTLE_ENDIAN   => LITTLE_ENDIAN
        )
        port map (
            clk                     => clk,
            reset                   => reset,
            start_r                 => r_start_r,
            ack_r                   => r_ack_r,
            opcode_r                => r_opcode_r,
            instance_state_r        => r_instance_state_r,
            view_state_r            => r_view_state_r,
            sample_state_r          => r_sample_state_r,
            instance_handle_r       => r_instance_handle_r,
            max_samples_r           => r_max_samples_r,
            get_data_r              => r_get_data_r,
            done_r                  => r_done_r,
            return_code_r           => r_return_code_r,
            valid_in_r              => r_valid_in_r,
            ready_in_r              => r_ready_in_r,
            data_in_r               => r_data_in_r,
            last_word_in_r          => r_last_word_in_r,
            sample_info_r           => r_sample_info_r,
            sample_info_valid_r     => r_sample_info_valid_r,
            sample_info_ack_r       => r_sample_info_ack_r,
            eoc_r                   => r_eoc_r,
            status_r                => r_status_r,
            start_w                 => r_start_w,
            ack_w                   => r_ack_w,
            opcode_w                => r_opcode_w,
            instance_handle_out_w   => r_instance_handle_out_w,
            source_ts_w             => r_source_ts_w,
            max_wait_w              => r_max_wait_w,
            done_w                  => r_done_w,
            return_code_w           => r_return_code_w,
            instance_handle_in_w    => r_instance_handle_in_w,
            valid_out_w             => r_valid_out_w,
            ready_out_w             => r_ready_out_w,
            data_out_w              => r_data_out_w,
            last_word_out_w         => r_last_word_out_w,
            valid_in_w              => r_valid_in_w,
            ready_in_w              => r_ready_in_w,
            data_in_w               => r_data_in_w,
            last_word_in_w          => r_last_word_in_w,
            status_w                => r_status_w,
            start_user              => r_start_user,
            ack_user                => r_ack_user,
            opcode_user             => r_opcode_user,
            service_info_user       => r_service_info_user,
            request_id_user         => r_request_id_user,
            data_available_user     => r_data_available_user,
            taken_user              => r_taken_user,
            goal_id                 => r_goal_id,
            status                  => r_status,
            -- ###GENERATED START###
            seq_len                 => seq_len_sig,
            seq_addr                => seq_addr_sig,
            seq_ready               => seq_ready_sig,
            seq_ren                 => '0',
            seq_wen                 => seq_wen_sig,
            seq_valid               => open,
            seq_ack                 => '1',
            seq_r                   => open,
            seq_w                   => seq_w_sig,
            -- ###GENERATED END###
            done_user               => r_done_user,
            return_code_user        => r_return_code_user
        );
        
    cancel_srv_server_inst : entity work.CancelGoal_ros_srv_server(arch)
        generic map (
            LITTLE_ENDIAN   => LITTLE_ENDIAN
        )
        port map (
            clk                         => clk,
            reset                       => reset,
            start_r                     => c_start_r,
            ack_r                       => c_ack_r,
            opcode_r                    => c_opcode_r,
            instance_state_r            => c_instance_state_r,
            view_state_r                => c_view_state_r,
            sample_state_r              => c_sample_state_r,
            instance_handle_r           => c_instance_handle_r,
            max_samples_r               => c_max_samples_r,
            get_data_r                  => c_get_data_r,
            done_r                      => c_done_r,
            return_code_r               => c_return_code_r,
            valid_in_r                  => c_valid_in_r,
            ready_in_r                  => c_ready_in_r,
            data_in_r                   => c_data_in_r,
            last_word_in_r              => c_last_word_in_r,
            sample_info_r               => c_sample_info_r,
            sample_info_valid_r         => c_sample_info_valid_r,
            sample_info_ack_r           => c_sample_info_ack_r,
            eoc_r                       => c_eoc_r,
            status_r                    => c_status_r,
            start_w                     => c_start_w,
            ack_w                       => c_ack_w,
            opcode_w                    => c_opcode_w,
            instance_handle_out_w       => c_instance_handle_out_w,
            source_ts_w                 => c_source_ts_w,
            max_wait_w                  => c_max_wait_w,
            done_w                      => c_done_w,
            return_code_w               => c_return_code_w,
            instance_handle_in_w        => c_instance_handle_in_w,
            valid_out_w                 => c_valid_out_w,
            ready_out_w                 => c_ready_out_w,
            data_out_w                  => c_data_out_w,
            last_word_out_w             => c_last_word_out_w,
            valid_in_w                  => c_valid_in_w,
            ready_in_w                  => c_ready_in_w,
            data_in_w                   => c_data_in_w,
            last_word_in_w              => c_last_word_in_w,
            status_w                    => c_status_w,
            start_user                  => c_start_user,
            ack_user                    => c_ack_user,
            opcode_user                 => c_opcode_user,
            service_info_user           => c_service_info_user,
            request_id_user             => c_request_id_user,
            data_available_user         => c_data_available_user,
            taken_user                  => c_taken_user,
            goal_info_goal_id           => c_goal_info_goal_id,
            goal_info_stamp             => c_goal_info_stamp,
            return_code                 => c_return_code,
            goals_canceling_len         => c_goals_canceling_len,
            goals_canceling_addr        => c_goals_canceling_addr,
            goals_canceling_ready       => c_goals_canceling_ready,
            goals_canceling_ren         => c_goals_canceling_ren,
            goals_canceling_wen         => c_goals_canceling_wen,
            goals_canceling_valid       => c_goals_canceling_valid,
            goals_canceling_ack         => c_goals_canceling_ack,
            goals_canceling_goal_id_r   => c_goals_canceling_goal_id_r,
            goals_canceling_goal_id_w   => c_goals_canceling_goal_id_w,
            goals_canceling_stamp_r     => c_goals_canceling_stamp_r,
            goals_canceling_stamp_w     => c_goals_canceling_stamp_w,
            done_user                   => c_done_user,
            return_code_user            => c_return_code_user
        );
    
    feedback_gen : if ENABLE_FEEDBACK = '1' generate
        feedback_pub_inst : entity work.Fibonacci_ros_action_feedback_pub(arch)
            generic map (
                LITTLE_ENDIAN   => LITTLE_ENDIAN
            )
            port map (
                clk                     => clk,
                reset                   => reset,
                start_dds               => f_start_dds,
                ack_dds                 => f_ack_dds,
                opcode_dds              => f_opcode_dds,
                instance_handle_out_dds => f_instance_handle_out_dds,
                source_ts_dds           => f_source_ts_dds,
                max_wait_dds            => f_max_wait_dds,
                done_dds                => f_done_dds,
                return_code_dds         => f_return_code_dds,
                instance_handle_in_dds  => f_instance_handle_in_dds,
                valid_out_dds           => f_valid_out_dds,
                ready_out_dds           => f_ready_out_dds,
                data_out_dds            => f_data_out_dds,
                last_word_out_dds       => f_last_word_out_dds,
                valid_in_dds            => f_valid_in_dds,
                ready_in_dds            => f_ready_in_dds,
                data_in_dds             => f_data_in_dds,
                last_word_in_dds        => f_last_word_in_dds,
                status_dds              => f_status_dds,
                start_user              => f_start_user,
                opcode_user             => f_opcode_user,
                ack_user                => f_ack_user,
                -- ###GENERATED START###
                goal_id                 => f_goal_id,
                seq_len                 => feedback_seq_len,
                seq_addr                => feedback_seq_addr,
                seq_ready               => feedback_seq_ready,
                seq_ren                 => feedback_seq_ren,
                seq_wen                 => feedback_seq_wen,
                seq_valid               => feedback_seq_valid,
                seq_ack                 => feedback_seq_ack,
                seq_r                   => feedback_seq_r,
                seq_w                   => feedback_seq_w,
                -- ###GENERATED END###
                done_user               => f_done_user,
                return_code_user        => f_return_code_user
            );
    else generate
        f_start_dds                 <= '0';
        f_opcode_dds                <= NOP;
        f_instance_handle_in_dds    <= HANDLE_NIL;
        f_source_ts_dds             <= TIME_INVALID;
        f_max_wait_dds              <= DURATION_ZERO;
        f_valid_out_dds             <= '0';
        f_data_out_dds              <= (others => '0');
        f_last_word_out_dds         <= '0';
        f_ready_in_dds              <= '0';
        -- ###GENERATED START###
        feedback_seq_ready          <= '0';
        feedback_seq_valid          <= '0';
        feedback_seq_r              <= (others => '0');
        -- ###GENERATED END###
    end generate;
    
    status_pub_inst : entity work.GoalStatusArray_ros_pub(arch)
        generic map (
            LITTLE_ENDIAN   => LITTLE_ENDIAN
        )
        port map (
            clk                             => clk,
            reset                           => reset,
            start_dds                       => s_start_dds,
            ack_dds                         => s_ack_dds,
            opcode_dds                      => s_opcode_dds,
            instance_handle_out_dds         => s_instance_handle_out_dds,
            source_ts_dds                   => s_source_ts_dds,
            max_wait_dds                    => s_max_wait_dds,
            done_dds                        => s_done_dds,
            return_code_dds                 => s_return_code_dds,
            instance_handle_in_dds          => s_instance_handle_in_dds,
            valid_out_dds                   => s_valid_out_dds,
            ready_out_dds                   => s_ready_out_dds,
            data_out_dds                    => s_data_out_dds,
            last_word_out_dds               => s_last_word_out_dds,
            valid_in_dds                    => s_valid_in_dds,
            ready_in_dds                    => s_ready_in_dds,
            data_in_dds                     => s_data_in_dds,
            last_word_in_dds                => s_last_word_in_dds,
            status_dds                      => s_status_dds,
            start_user                      => s_start_user,
            opcode_user                     => s_opcode_user,
            ack_user                        => s_ack_user,
            status_list_len                 => s_status_list_len,
            status_list_addr                => s_status_list_addr,
            status_list_ready               => s_status_list_ready,
            status_list_ren                 => s_status_list_ren,
            status_list_wen                 => s_status_list_wen,
            status_list_valid               => s_status_list_valid,
            status_list_ack                 => s_status_list_ack,
            status_list_goal_info_goal_id_r => s_status_list_goal_info_goal_id_r,
            status_list_goal_info_goal_id_w => s_status_list_goal_info_goal_id_w,
            status_list_goal_info_stamp_r   => s_status_list_goal_info_stamp_r,
            status_list_goal_info_stamp_w   => s_status_list_goal_info_stamp_w,
            status_list_status_r            => s_status_list_status_r,
            status_list_status_w            => s_status_list_status_w,
            done_user                       => s_done_user,
            return_code_user                => s_return_code_user
        );
    
    action_server_inst : entity work.ros_action_server(arch)
        generic map (
            TIMEOUT_DURATION    => TIMEOUT_DURATION,
            MAX_GOALS           => MAX_GOALS,
            MAX_RESULT_REQUESTS => MAX_RESULT_REQUESTS,
            ENABLE_FEEDBACK     => ENABLE_FEEDBACK    
        )
        port map (
            clk                                 => clk,
            reset                               => reset,
            time                                => time,
            -- *GOAL SERVICE*
            start_g                             => g_start_user,
            ack_g                               => g_ack_user,
            opcode_g                            => g_opcode_user,
            service_info_g                      => g_service_info_user,
            request_id_g                        => g_request_id_user,
            data_available_g                    => g_data_available_user,
            taken_g                             => g_taken_user,
            goal_id_g                           => g_goal_id,
            accepted_g                          => g_accepted,
            stamp_g                             => g_stamp,
            done_g                              => g_done_user,
            return_code_g                       => g_return_code_user,
            -- *RESULT SERVICE*
            start_r                             => r_start_user,
            ack_r                               => r_ack_user,
            opcode_r                            => r_opcode_user,
            service_info_r                      => r_service_info_user,
            request_id_r                        => r_request_id_user,
            data_available_r                    => r_data_available_user,
            taken_r                             => r_taken_user,
            result_index                        => r_index,
            result_sel                          => r_sel,
            result_sel_ack                      => r_sel_ack,
            goal_id_r                           => r_goal_id,
            status_r                            => r_status,
            done_r                              => r_done_user,
            return_code_r                       => r_return_code_user,
            -- *CANCEL SERVICE*
            start_c                             => c_start_user,
            ack_c                               => c_ack_user,
            opcode_c                            => c_opcode_user,
            service_info_c                      => c_service_info_user,
            request_id_c                        => c_request_id_user,
            data_available_c                    => c_data_available_user,
            taken_c                             => c_taken_user,
            goal_info_goal_id_c                 => c_goal_info_goal_id,
            goal_info_stamp_c                   => c_goal_info_stamp,
            cancel_return_code_c                => c_return_code,
            goals_canceling_len_c               => c_goals_canceling_len,
            goals_canceling_addr_c              => c_goals_canceling_addr,
            goals_canceling_ready_c             => c_goals_canceling_ready,
            goals_canceling_ren_c               => c_goals_canceling_ren,
            goals_canceling_wen_c               => c_goals_canceling_wen,
            goals_canceling_valid_c             => c_goals_canceling_valid,
            goals_canceling_ack_c               => c_goals_canceling_ack,
            goals_canceling_goal_id_r_c         => c_goals_canceling_goal_id_r,
            goals_canceling_goal_id_w_c         => c_goals_canceling_goal_id_w,
            goals_canceling_stamp_r_c           => c_goals_canceling_stamp_r,
            goals_canceling_stamp_w_c           => c_goals_canceling_stamp_w,
            done_c                              => c_done_user,
            return_code_c                       => c_return_code_user,
            -- *FEEDBACK TOPIC*
            start_fb                            => f_start_user,
            opcode_fb                           => f_opcode_user,
            ack_fb                              => f_ack_user,
            goal_id_fb                          => f_goal_id,
            done_fb                             => f_done_user,
            return_code_fb                      => f_return_code_user,
            -- *STATUS TOPIC*
            start_s                             => s_start_user,
            opcode_s                            => s_opcode_user,
            ack_s                               => s_ack_user,
            status_list_len_s                   => s_status_list_len,
            status_list_addr_s                  => s_status_list_addr,
            status_list_ready_s                 => s_status_list_ready,
            status_list_ren_s                   => s_status_list_ren,
            status_list_wen_s                   => s_status_list_wen,
            status_list_valid_s                 => s_status_list_valid,
            status_list_ack_s                   => s_status_list_ack,
            status_list_goal_info_goal_id_r_s   => s_status_list_goal_info_goal_id_r,
            status_list_goal_info_goal_id_w_s   => s_status_list_goal_info_goal_id_w,
            status_list_goal_info_stamp_r_s     => s_status_list_goal_info_stamp_r,
            status_list_goal_info_stamp_w_s     => s_status_list_goal_info_stamp_w,
            status_list_status_r_s              => s_status_list_status_r,
            status_list_status_w_s              => s_status_list_status_w,
            done_s                              => s_done_user,
            return_code_s                       => s_return_code_user,
            -- USER
            start                               => start,
            opcode                              => opcode,
            ack                                 => ack,
            goal_handle_in                      => goal_handle_in,
            goal_handle_out                     => goal_handle_out,
            goal_state_in                       => goal_state_in,
            goal_state_out                      => goal_state_out,
            goal_id                             => goal_id,
            goal_result_index                   => goal_result_index,
            goal_stamp                          => goal_stamp,
            new_goal_request                    => new_goal_request,
            new_goal_handle                     => new_goal_handle,
            new_goal_result_index               => new_goal_result_index,
            new_goal_accepted                   => new_goal_accepted,
            new_goal_response                   => new_goal_response,
            cancel_request                      => cancel_request,
            cancel_request_handle               => cancel_request_handle,
            cancel_accepted                     => cancel_accepted,
            cancel_response                     => cancel_response,
            done                                => done,
            return_code                         => return_code
        );
    
    -- ###GENERATED START###
    r_seq_len_mem : entity work.mem_ctrl(arch)
            generic map (
                ADDR_WIDTH          => RESULT_INDEX_WIDTH,
                DATA_WIDTH          => R_RR_SEQ_ADDR_WIDTH,
                MEMORY_DEPTH        => MAX_GOALS,
                MAX_BURST_LENGTH    => 1
            )
            port map (
                clk         => clk,
                reset       => reset or abort_mem,
                addr        => r_seq_len_mem_addr,
                read        => r_seq_len_mem_read,
                ready_in    => r_seq_len_mem_ready_in,
                valid_in    => r_seq_len_mem_valid_in,
                data_in     => r_seq_len_mem_data_in,
                ready_out   => r_seq_len_mem_ready_out,
                valid_out   => r_seq_len_mem_valid_out,
                data_out    => r_seq_len_mem_data_out
            );
    
    r_seq_gen : for i in 0 to MAX_GOALS-1 generate
        r_seq_mem : entity work.mem_ctrl(arch)
            generic map (
                ADDR_WIDTH          => R_RR_SEQ_ADDR_WIDTH,
                DATA_WIDTH          => CDR_LONG_WIDTH,
                MEMORY_DEPTH        => R_RR_SEQ_MAX_DEPTH,
                MAX_BURST_LENGTH    => 1
            )
            port map (
                clk         => clk,
                reset       => reset or abort_mem,
                addr        => r_seq_mem_addr(i),
                read        => r_seq_mem_read(i),
                ready_in    => r_seq_mem_ready_in(i),
                valid_in    => r_seq_mem_valid_in(i),
                data_in     => r_seq_mem_data_in(i),
                ready_out   => r_seq_mem_ready_out(i),
                valid_out   => r_seq_mem_valid_out(i),
                data_out    => r_seq_mem_data_out(i)
            );
    end generate;
    -- ###GENERATED END###
    
    result_mux : process(all)
    begin
        -- DEFAULT
        stage_next              <= stage;
        cnt_next                <= cnt;
        result_index_latch_next <= result_index_latch;
        -- DEFAULT Unregistered
        index_result_ready  <= '0';
        r_sel_ack           <= '0';
        abort_mem           <= '0';
        -- ###GENERATED START###
        seq_cnt_next            <= seq_cnt;
        r_seq_len_mem_addr      <= (others => '0');
        r_seq_len_mem_read      <= '0';
        r_seq_len_mem_valid_in  <= '0';
        r_seq_len_mem_data_in   <= (others => '0');
        r_seq_len_mem_ready_out <= '0';
        r_seq_mem_addr          <= (others => (others => '0'));
        r_seq_mem_read          <= (others => '0');
        r_seq_mem_valid_in      <= (others => '0');
        r_seq_mem_data_in       <= (others => (others => '0'));
        r_seq_mem_ready_out     <= (others => '0');
        result_seq_r            <= (others => '0');
        result_seq_valid        <= '0';
        result_seq_ready        <= '0';
        seq_len_sig             <= (others => '0');
        seq_addr_sig            <= (others => '0');
        seq_wen_sig             <= '0';
        seq_w_sig               <= (others => '0');
        -- ###GENERATED END###
        
        case (stage) is
            when IDLE =>
                if (r_sel = '1') then
                    stage_next  <= GET_RESULT;
                    -- Reset
                    abort_mem   <= '1';
                else
                    index_result_ready      <= r_seq_len_mem_ready_in; -- AND all memories
                    
                    -- ###GENERATED START###
                    r_seq_len_mem_data_in   <= result_seq_len;
                    r_seq_len_mem_addr      <= std_logic_vector(resize(unsigned(index_result), RESULT_INDEX_WIDTH));
                    r_seq_len_mem_valid_in  <= index_result_wen;
                    
                    result_seq_r        <= r_seq_mem_data_out(to_integer(unsigned(result_index_latch)));
                    result_seq_valid    <= r_seq_mem_valid_out(to_integer(unsigned(result_index_latch)));
                    r_seq_mem_data_in   <= (others => result_seq_w);
                    result_seq_ready    <= r_seq_mem_ready_in(to_integer(unsigned(index_result)));
                    r_seq_mem_addr(to_integer(unsigned(index_result)))      <= result_seq_addr;
                    r_seq_mem_read(to_integer(unsigned(index_result)))      <= result_seq_ren;
                    r_seq_mem_valid_in(to_integer(unsigned(index_result)))  <= result_seq_ren or result_seq_wen;
                    r_seq_mem_ready_out <= (others => result_seq_ack);
                    -- Latch Address for output
                    if (result_seq_ren = '1') then
                        result_index_latch_next <= std_logic_vector(resize(unsigned(index_result), RESULT_INDEX_WIDTH));
                    end if;
                    -- ###GENERATED END###
                end if;
            when GET_RESULT =>
                assert (r_sel = '1') severity FAILURE;
                
                -- ###GENERATED START###
                r_seq_len_mem_addr  <= std_logic_vector(resize(unsigned(r_index), RESULT_INDEX_WIDTH));
                r_seq_len_mem_read  <= '1';
                -- Memory Operation Guard
                if (r_seq_len_mem_ready_in = '1') then -- AND all 'ready_in' signals
                    -- NOTE: We are toggling the 'valid_in' signal only after we make sure all memories are ready
                    r_seq_len_mem_valid_in  <= '1';
                    stage_next  <= READ_RESULT;
                end if;
                -- ###GENERATED END###
            when READ_RESULT =>
                assert (r_sel = '1') severity FAILURE;
                
                -- ###GENERATED START###
                if (r_seq_len_mem_valid_out = '1') then -- AND all 'valid_out' Signals
                    stage_next      <= PUSH_SEQUENCE;
                    cnt_next        <= 0;
                    seq_cnt_next    <= 0;
                end if;
                -- ###GENERATED END###
            -- ###GENERATED START###
            when PUSH_SEQUENCE =>
                assert (r_sel = '1') severity FAILURE;
                
                case (cnt) is
                    -- GET
                    when 0 =>
                        r_seq_mem_addr(to_integer(unsigned(r_index)))       <= std_logic_vector(to_unsigned(seq_cnt,R_RR_SEQ_ADDR_WIDTH));
                        r_seq_mem_valid_in(to_integer(unsigned(r_index)))   <= '1';
                        r_seq_mem_read(to_integer(unsigned(r_index)))       <= '1';
                        -- Memory Operation Guard
                        if (r_seq_mem_ready_in(to_integer(unsigned(r_index))) = '1') then
                            cnt_next    <= cnt + 1;
                        end if;
                    -- WRITE
                    when 1 =>
                        -- Memory Operation Guard
                        if (seq_ready_sig = '1' and r_seq_mem_valid_out(to_integer(unsigned(r_index))) = '1') then
                            seq_addr_sig    <= std_logic_vector(to_unsigned(seq_cnt,R_RR_SEQ_ADDR_WIDTH));
                            seq_wen_sig     <= '1';
                            seq_w_sig       <= r_seq_mem_data_out(to_integer(unsigned(r_index)));
                            r_seq_mem_ready_out(to_integer(unsigned(r_index)))  <= '1';
                            
                            -- All Elements processed
                            if (seq_cnt = unsigned(r_seq_len_mem_data_out)-1) then
                                -- DONE
                                stage_next  <= PASSTHROUGH;
                            else
                                seq_cnt_next    <= seq_cnt + 1;
                                cnt_next        <= 0;
                            end if;
                        end if;
                    when others => 
                end case;
            -- ###GENERATED END###
            when PASSTHROUGH =>
                r_sel_ack   <= '1';
                
                -- ###GENERATED START###
                if (unsigned(r_index) = MAX_GOALS) then
                    seq_len_sig <= (others => '0');
                else
                    seq_len_sig <= r_seq_len_mem_data_out;
                end if;
                -- ###GENERATED END###
                
                if (r_sel = '0') then
                    stage_next  <= IDLE;
                    -- Reset
                    abort_mem   <= '1';
                end if;
        end case;
    end process;
    
    sync_prc : process(clk)
    begin
        if rising_edge(clk) then
            if (reset = '1') then
                stage               <= IDLE;
                cnt                 <= 0;
                seq_cnt             <= 0;
                result_index_latch  <= (others => '0');
            else
                stage               <= stage_next;
                cnt                 <= cnt_next;
                seq_cnt             <= seq_cnt_next;
                result_index_latch  <= result_index_latch_next;
            end if;
        end if;
    end process;
    
end architecture;
