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

package user_config is
    
    --*****USER CONFIG*****
    
    -- Period of system clock
    constant CLOCK_PERIOD               : time := ROS_CLOCK_PERIOD;
    -- Maximum number of supported remote Participants (Affects generated Memory Size)
    constant MAX_REMOTE_PARTICIPANTS    : natural := 50;
    -- Maximum number of supported remote Endpoints (Affects generated Memory Size)
    constant MAX_REMOTE_ENDPOINTS       : natural := 50;
    -- If Multicast Locators should be used instead of Unicast Locators
    constant PREFER_MULTICAST_LOCATORS  : boolean := FALSE;
    
    -- Unicast IPv4 Address used by all RTPS Entities [Default 192.168.0.128]
    constant DEFAULT_IPv4_ADDRESS   : std_logic_vector(IPv4_ADDRESS_WIDTH-1 downto 0) := ROS_ADDRESS;
    -- Number of RTPS Writer Endpoints
    constant NUM_WRITERS            : natural := gen_num_writers(ROS_PUBLICATIONS,ROS_SERVICES,ROS_ACTIONS); -- Deferred to Package Body
    -- Number of RTPS Reader Endpoints
    constant NUM_READERS            : natural := gen_num_readers(ROS_SUBSCRIPTIONS,ROS_SERVICES,ROS_ACTIONS); -- Deferred to Package Body
    -- Number of RTPS Endpoints (Do not modify)
    constant NUM_ENDPOINTS          : natural := NUM_READERS + NUM_WRITERS; -- Deferred to Package Body
    -- PB Value of Default Port Generation (see DDSI-RTPS 2.3 Section 9.6.1)
    constant PORT_CONFIG_PB         : natural := 7400;
    -- DG Value of Default Port Generation (see DDSI-RTPS 2.3 Section 9.6.1)    
    constant PORT_CONFIG_DG         : natural := 250;
    -- PG Value of Default Port Generation (see DDSI-RTPS 2.3 Section 9.6.1)
    constant PORT_CONFIG_PG         : natural := 2; 
    -- D0 Value of Default Port Generation (see DDSI-RTPS 2.3 Section 9.6.1)
    constant PORT_CONFIG_D0         : natural := 0; 
    -- D1 Value of Default Port Generation (see DDSI-RTPS 2.3 Section 9.6.1)
    constant PORT_CONFIG_D1         : natural := 10;
    -- D2 Value of Default Port Generation (see DDSI-RTPS 2.3 Section 9.6.1)    
    constant PORT_CONFIG_D2         : natural := 1; 
    -- D3 Value of Default Port Generation (see DDSI-RTPS 2.3 Section 9.6.1)
    constant PORT_CONFIG_D3         : natural := 11;
    -- MAC Address of underlying network stack (Used to generate GUIDs)
    constant MAC_ADDRESS            : std_logic_vector(47 downto 0) := ROS_RAND_KEY;
    -- Domain ID
    constant USER_DOMAIN_ID         : natural := get_domain_id(ROS_NODES);
    -- Domain TAG
    constant USER_DOMAIN_TAG        : USER_STRING_TYPE  := gen_user_string("");
    
    -- *TIMING CHARACTERISTICS*
    -- Timing Characteristics for Participant
    constant PARTICIPANT_ANNOUNCEMENT_PERIOD            : DURATION_TYPE;
    constant PARTICIPANT_LEASE_DURATION                 : DURATION_TYPE;
    -- Denotes how much faster then the deadline/period we schedule in order to account for transport delay.
    constant DURATION_DELTA                             : DURATION_TYPE;
    -- Timing Characteristics for built-in Endpoints (Discovery Module)
    constant PARTICIPANT_HEARTBEAT_PERIOD               : DURATION_TYPE;
    constant PARTICIPANT_HEARTBEAT_RESPONSE_DELAY       : DURATION_TYPE;
    constant PARTICIPANT_HEARTBEAT_SUPPRESSION_DELAY    : DURATION_TYPE;
    constant PARTICIPANT_ACKNACK_RESPONSE_DELAY         : DURATION_TYPE;
    constant PARTICIPANT_ACKNACK_SUPPRESSION_DELAY      : DURATION_TYPE;
    
    -- *ENDPOINT CONFIG*
    constant ENDPOINT_CONFIG : CONFIG_ARRAY_TYPE(0 to NUM_ENDPOINTS-1); -- Deferred to Package Body
    
    -- Set to TRUE for Simulation Testing (Extra Code generated)
    constant SIMULATION_FLAG : boolean := SIMULATION_TIMING;
end package;

package body user_config is
    
    function duration_mux(a : DURATION_TYPE; b : DURATION_TYPE; sel : boolean) return DURATION_TYPE is
    begin
        if (sel) then
            return a;
        else
            return b;
        end if;
    end function;
    
    constant PARTICIPANT_ANNOUNCEMENT_PERIOD            : DURATION_TYPE := duration_mux(gen_duration(500 us), gen_duration(30 sec), SIMULATION_TIMING);
    constant PARTICIPANT_LEASE_DURATION                 : DURATION_TYPE := duration_mux(DEFAULT_PARTICIPANT_LEASE_DURATION, DEFAULT_PARTICIPANT_LEASE_DURATION, SIMULATION_TIMING);
    constant DURATION_DELTA                             : DURATION_TYPE := duration_mux(gen_duration(50 ns), gen_duration(100 ms), SIMULATION_TIMING);
    constant PARTICIPANT_HEARTBEAT_PERIOD               : DURATION_TYPE := duration_mux(gen_duration(100 us), gen_duration(1 sec), SIMULATION_TIMING);
    constant PARTICIPANT_HEARTBEAT_RESPONSE_DELAY       : DURATION_TYPE := duration_mux(gen_duration(500 ns), gen_duration(500 ms), SIMULATION_TIMING);
    constant PARTICIPANT_HEARTBEAT_SUPPRESSION_DELAY    : DURATION_TYPE := duration_mux(gen_duration(100 ns), gen_duration(0 ms), SIMULATION_TIMING);
    constant PARTICIPANT_ACKNACK_RESPONSE_DELAY         : DURATION_TYPE := duration_mux(gen_duration(500 ns), gen_duration(200 ms), SIMULATION_TIMING);
    constant PARTICIPANT_ACKNACK_SUPPRESSION_DELAY      : DURATION_TYPE := duration_mux(gen_duration(100 ns), gen_duration(0 ms), SIMULATION_TIMING);
    
    procedure set_sim_timing (ref : inout CONFIG_TYPE; sel : in boolean) is
    begin
        if (sel) then
            ref.HEARTBEAT_PERIOD              := gen_duration(100 us);
            ref.HEARTBEAT_RESPONSE_DELAY      := gen_duration(500 ns);
            ref.HEARTBEAT_SUPPRESSION_DELAY   := gen_duration(100 ns);
            ref.ACKNACK_RESPONSE_DELAY        := gen_duration(500 ns);
            ref.ACKNACK_SUPPRESSION_DELAY     := gen_duration(100 ns);
        end if;
    end procedure;
    
    -- NOTE: This function sets also the 
    function gen_config return CONFIG_ARRAY_TYPE is
        variable ret : CONFIG_ARRAY_TYPE(0 to NUM_ENDPOINTS-1);
        variable index : natural;
        variable tmp : USER_STRING_TYPE;
    begin
        
        check_node_mapping(ROS_NODES, ROS_PUBLICATIONS, ROS_SUBSCRIPTIONS, ROS_SERVICES, ROS_ACTIONS);
        index := 0;
        
        -- SUBSCRIPTIONS
        for i in 0 to ROS_SUBSCRIPTIONS'length-1 loop
            ret(index) := DEFAULT_READER_CONFIG;
            set_from_qos_profile(ROS_SUBSCRIPTIONS(i).QOS, ret(index));
            set_sim_timing(ret(index),SIMULATION_TIMING);
            ret(index).TYPENAME         := ROS_SUBSCRIPTIONS(i).TYPENAME;
            ret(index).MAX_PAYLOAD_SIZE := ROS_SUBSCRIPTIONS(i).MAX_SIZE + 4; -- (+ PAYLOAD HEADER)
            tmp := gen_fqn(ROS_NODES(ROS_SUBSCRIPTIONS(i).node_id).namespace, ROS_NODES(ROS_SUBSCRIPTIONS(i).node_id).name, ROS_SUBSCRIPTIONS(i).TOPICNAME);
            if (not ROS_SUBSCRIPTIONS(i).QOS.AVOID_ROS_NAMESPACE_CONVENTION) then
                ret(index).TOPICNAME := concat("rt",tmp);
            else
                ret(index).TOPICNAME := tmp;
            end if;
            index := index + 1;
        end loop;
        -- SERVICE SUBS
        for i in 0 to ROS_SERVICES'length-1 loop
            ret(index) := DEFAULT_READER_CONFIG;
            set_from_qos_profile(ROS_SERVICES(i).QOS, ret(index));
            set_sim_timing(ret(index),SIMULATION_TIMING);
            tmp := gen_fqn(ROS_NODES(ROS_SERVICES(i).node_id).namespace, ROS_NODES(ROS_SERVICES(i).node_id).name, ROS_SERVICES(i).SERVICENAME);
            if (ROS_SERVICES(i).is_client) then
                ret(index).TOPICNAME        := concat("rr",concat(tmp,"Reply"));
                ret(index).MAX_PAYLOAD_SIZE := ROS_SERVICES(i).MAX_RR_SIZE + 20; -- (+ PAYLOAD HEADER + REQUEST HEADER)
                ret(index).TYPENAME         := ROS_SERVICES(i).RR_TYPENAME;
            else
                ret(index).TOPICNAME        := concat("rq",concat(tmp,"Request"));
                ret(index).MAX_PAYLOAD_SIZE := ROS_SERVICES(i).MAX_RQ_SIZE + 20; -- (+ PAYLOAD HEADER + REQUEST HEADER)
                ret(index).TYPENAME         := ROS_SERVICES(i).RQ_TYPENAME;
            end if;
            index := index + 1;
        end loop;
        -- ACTION SUBS
        for i in 0 to ROS_ACTIONS'length-1 loop
            tmp := gen_fqn(ROS_NODES(ROS_ACTIONS(i).node_id).namespace, ROS_NODES(ROS_ACTIONS(i).node_id).name, ROS_ACTIONS(i).ACTIONNAME);
            -- GOAL SERVICE
            ret(index) := DEFAULT_READER_CONFIG;
            set_from_qos_profile(ROS_ACTIONS(i).GOAL_QOS, ret(index));
            set_sim_timing(ret(index),SIMULATION_TIMING);
            if (ROS_ACTIONS(i).is_client) then
                ret(index).TOPICNAME        := concat("rr",concat(tmp,"/_action/send_goalReply"));
                ret(index).MAX_PAYLOAD_SIZE := ROS_ACTIONS(i).MAX_GOAL_RR_SIZE + 20; -- (+ PAYLOAD HEADER + REQUEST HEADER)
                ret(index).TYPENAME         := ROS_ACTIONS(i).GOAL_RR_TYPENAME;
            else
                ret(index).TOPICNAME        := concat("rq",concat(tmp,"/_action/send_goalRequest"));
                ret(index).MAX_PAYLOAD_SIZE := ROS_ACTIONS(i).MAX_GOAL_RQ_SIZE + 20; -- (+ PAYLOAD HEADER + REQUEST HEADER)
                ret(index).TYPENAME         := ROS_ACTIONS(i).GOAL_RQ_TYPENAME;
            end if;
            index := index + 1;
            -- RESULT SERVICE
            ret(index) := DEFAULT_READER_CONFIG;
            set_from_qos_profile(ROS_ACTIONS(i).RESULT_QOS, ret(index));
            set_sim_timing(ret(index),SIMULATION_TIMING);
            if (ROS_ACTIONS(i).is_client) then
                ret(index).TOPICNAME        := concat("rr",concat(tmp,"/_action/get_resultReply"));
                ret(index).MAX_PAYLOAD_SIZE := ROS_ACTIONS(i).MAX_RESULT_RR_SIZE + 20; -- (+ PAYLOAD HEADER + REQUEST HEADER)
                ret(index).TYPENAME         := ROS_ACTIONS(i).RESULT_RR_TYPENAME;
            else
                ret(index).TOPICNAME        := concat("rq",concat(tmp,"/_action/get_resultRequest"));
                ret(index).MAX_PAYLOAD_SIZE := ROS_ACTIONS(i).MAX_RESULT_RQ_SIZE + 20; -- (+ PAYLOAD HEADER + REQUEST HEADER)
                ret(index).TYPENAME         := ROS_ACTIONS(i).RESULT_RQ_TYPENAME;
            end if;
            index := index + 1;
            -- CANCEL SERVICE
            ret(index) := DEFAULT_READER_CONFIG;
            set_from_qos_profile(ROS_ACTIONS(i).CANCEL_QOS, ret(index));
            set_sim_timing(ret(index),SIMULATION_TIMING);
            if (ROS_ACTIONS(i).is_client) then
                ret(index).TOPICNAME        := concat("rr",concat(tmp,"/_action/cancel_goalReply"));
                ret(index).MAX_PAYLOAD_SIZE := CancelGoal_package.RR_MAX_CANCELGOAL_SIZE + 20; -- (+ PAYLOAD HEADER + REQUEST HEADER)
                ret(index).TYPENAME         := gen_user_string("action_msgs::srv::dds_::CancelGoal_Response_");
            else
                ret(index).TOPICNAME        := concat("rq",concat(tmp,"/_action/cancel_goalRequest"));
                ret(index).MAX_PAYLOAD_SIZE := CancelGoal_package.RQ_MAX_CANCELGOAL_SIZE + 20; -- (+ PAYLOAD HEADER + REQUEST HEADER)
                ret(index).TYPENAME         := gen_user_string("action_msgs::srv::dds_::CancelGoal_Request_");
            end if;
            index := index + 1;
            -- FEEDBACK TOPIC
            if (ROS_ACTIONS(i).enable_feedback and ROS_ACTIONS(i).is_client) then
                ret(index) := DEFAULT_READER_CONFIG;
                set_from_qos_profile(ROS_ACTIONS(i).FEEDBACK_QOS, ret(index));
                set_sim_timing(ret(index),SIMULATION_TIMING);
                ret(index).TOPICNAME        := concat("rt",concat(tmp,"/_action/feedback"));
                ret(index).MAX_PAYLOAD_SIZE := ROS_ACTIONS(i).MAX_FEEDBACK_SIZE + 4; -- (+ PAYLOAD HEADER)
                ret(index).TYPENAME         := ROS_ACTIONS(i).FEEDBACK_TYPENAME;
                index := index + 1;
            end if;
        end loop;
        -- PUBLICATIONS
        for i in 0 to ROS_PUBLICATIONS'length-1 loop
            ret(index) := DEFAULT_WRITER_CONFIG;
            set_from_qos_profile(ROS_PUBLICATIONS(i).QOS, ret(index));
            set_sim_timing(ret(index),SIMULATION_TIMING);
            ret(index).TYPENAME         := ROS_PUBLICATIONS(i).TYPENAME;
            ret(index).MAX_PAYLOAD_SIZE := ROS_PUBLICATIONS(i).MAX_SIZE + 4; -- (+ PAYLOAD HEADER)
            tmp := gen_fqn(ROS_NODES(ROS_PUBLICATIONS(i).node_id).namespace, ROS_NODES(ROS_PUBLICATIONS(i).node_id).name, ROS_PUBLICATIONS(i).TOPICNAME);
            if (not ROS_PUBLICATIONS(i).QOS.AVOID_ROS_NAMESPACE_CONVENTION) then
                ret(index).TOPICNAME := concat("rt",tmp);
            else
                ret(index).TOPICNAME := tmp;
            end if;
            index := index + 1;
        end loop;
        -- SERVICE PUBS
        for i in 0 to ROS_SERVICES'length-1 loop
            ret(index) := DEFAULT_WRITER_CONFIG;
            set_from_qos_profile(ROS_SERVICES(i).QOS, ret(index));
            set_sim_timing(ret(index),SIMULATION_TIMING);
            tmp := gen_fqn(ROS_NODES(ROS_SERVICES(i).node_id).namespace, ROS_NODES(ROS_SERVICES(i).node_id).name, ROS_SERVICES(i).SERVICENAME);
            if (ROS_SERVICES(i).is_client) then
                ret(index).TOPICNAME        := concat("rq",concat(tmp,"Request"));
                ret(index).MAX_PAYLOAD_SIZE := ROS_SERVICES(i).MAX_RQ_SIZE + 20; -- (+ PAYLOAD HEADER + REQUEST HEADER)
                ret(index).TYPENAME         := ROS_SERVICES(i).RQ_TYPENAME;
            else
                ret(index).TOPICNAME    := concat("rr",concat(tmp,"Reply"));
                ret(index).MAX_PAYLOAD_SIZE := ROS_SERVICES(i).MAX_RR_SIZE + 20; -- (+ PAYLOAD HEADER + REQUEST HEADER)
                ret(index).TYPENAME         := ROS_SERVICES(i).RR_TYPENAME;
            end if;
            index := index + 1;
        end loop;
        -- ACTION PUBS
        for i in 0 to ROS_ACTIONS'length-1 loop
            tmp := gen_fqn(ROS_NODES(ROS_ACTIONS(i).node_id).namespace, ROS_NODES(ROS_ACTIONS(i).node_id).name, ROS_ACTIONS(i).ACTIONNAME);
            -- GOAL SERVICE
            ret(index) := DEFAULT_WRITER_CONFIG;
            set_from_qos_profile(ROS_ACTIONS(i).GOAL_QOS, ret(index));
            set_sim_timing(ret(index),SIMULATION_TIMING);
            if (ROS_ACTIONS(i).is_client) then
                ret(index).TOPICNAME        := concat("rq",concat(tmp,"/_action/send_goalRequest"));
                ret(index).MAX_PAYLOAD_SIZE := ROS_ACTIONS(i).MAX_GOAL_RQ_SIZE + 20; -- (+ PAYLOAD HEADER + REQUEST HEADER)
                ret(index).TYPENAME         := ROS_ACTIONS(i).GOAL_RQ_TYPENAME;
            else
                ret(index).TOPICNAME        := concat("rr",concat(tmp,"/_action/send_goalReply"));
                ret(index).MAX_PAYLOAD_SIZE := ROS_ACTIONS(i).MAX_GOAL_RR_SIZE + 20; -- (+ PAYLOAD HEADER + REQUEST HEADER)
                ret(index).TYPENAME         := ROS_ACTIONS(i).GOAL_RR_TYPENAME;
            end if;
            index := index + 1;
            -- RESULT SERVICE
            ret(index) := DEFAULT_WRITER_CONFIG;
            set_from_qos_profile(ROS_ACTIONS(i).RESULT_QOS, ret(index));
            set_sim_timing(ret(index),SIMULATION_TIMING);
            if (ROS_ACTIONS(i).is_client) then
                ret(index).TOPICNAME        := concat("rq",concat(tmp,"/_action/get_resultRequest"));
                ret(index).MAX_PAYLOAD_SIZE := ROS_ACTIONS(i).MAX_RESULT_RQ_SIZE + 20; -- (+ PAYLOAD HEADER + REQUEST HEADER)
                ret(index).TYPENAME         := ROS_ACTIONS(i).RESULT_RQ_TYPENAME;
            else
                ret(index).TOPICNAME        := concat("rr",concat(tmp,"/_action/get_resultReply"));
                ret(index).MAX_PAYLOAD_SIZE := ROS_ACTIONS(i).MAX_RESULT_RR_SIZE + 20; -- (+ PAYLOAD HEADER + REQUEST HEADER)
                ret(index).TYPENAME         := ROS_ACTIONS(i).RESULT_RR_TYPENAME;
            end if;
            index := index + 1;
            -- CANCEL SERVICE
            ret(index) := DEFAULT_WRITER_CONFIG;
            set_from_qos_profile(ROS_ACTIONS(i).CANCEL_QOS, ret(index));
            set_sim_timing(ret(index),SIMULATION_TIMING);
            if (ROS_ACTIONS(i).is_client) then
                ret(index).TOPICNAME        := concat("rq",concat(tmp,"/_action/cancel_goalRequest"));
                ret(index).MAX_PAYLOAD_SIZE := CancelGoal_package.RQ_MAX_CANCELGOAL_SIZE + 20; -- (+ PAYLOAD HEADER + REQUEST HEADER)
                ret(index).TYPENAME         := gen_user_string("action_msgs::srv::dds_::CancelGoal_Request_");
            else
                ret(index).TOPICNAME        := concat("rr",concat(tmp,"/_action/cancel_goalReply"));
                ret(index).MAX_PAYLOAD_SIZE := CancelGoal_package.RR_MAX_CANCELGOAL_SIZE + 20; -- (+ PAYLOAD HEADER + REQUEST HEADER)
                ret(index).TYPENAME         := gen_user_string("action_msgs::srv::dds_::CancelGoal_Response_");
            end if;
            index := index + 1;
            -- FEEDBACK TOPIC
            if (ROS_ACTIONS(i).enable_feedback and (not ROS_ACTIONS(i).is_client)) then
                ret(index) := DEFAULT_WRITER_CONFIG;
                set_from_qos_profile(ROS_ACTIONS(i).FEEDBACK_QOS, ret(index));
                set_sim_timing(ret(index),SIMULATION_TIMING);
                ret(index).TOPICNAME        := concat("rt",concat(tmp,"/_action/feedback"));
                ret(index).MAX_PAYLOAD_SIZE := ROS_ACTIONS(i).MAX_FEEDBACK_SIZE + 4; -- (+ PAYLOAD HEADER)
                ret(index).TYPENAME         := ROS_ACTIONS(i).FEEDBACK_TYPENAME;
                index := index + 1;
            end if;
            -- STATUS TOPIC
            if (not ROS_ACTIONS(i).is_client) then
                ret(index) := DEFAULT_WRITER_CONFIG;
                set_from_qos_profile(ROS_QOS_PROFILE_ACTION_STATUS_DEFAULT, ret(index));
                set_sim_timing(ret(index),SIMULATION_TIMING);
                ret(index).TOPICNAME        := concat("rt",concat(tmp,"/_action/status"));
                ret(index).MAX_PAYLOAD_SIZE := GoalStatusArray_package.MAX_GOALSTATUSARRAY_SIZE + 4; -- (+ PAYLOAD HEADER)
                ret(index).TYPENAME         := gen_user_string("action_msgs::msg::dds_::GoalStatusArray_");
                index := index + 1;
            end if;
        end loop;
        -- ROS DISCOVERY PUB
        ret(index)                          := DEFAULT_WRITER_CONFIG;
        set_sim_timing(ret(index),SIMULATION_TIMING);
        ret(index).RELIABILITY_QOS          := RELIABLE_RELIABILITY_QOS;
        ret(index).DURABILITY_QOS           := TRANSIENT_LOCAL_DURABILITY_QOS;
        ret(index).HISTORY_QOS              := KEEP_LAST_HISTORY_QOS;
        ret(index).HISTORY_DEPTH            := std_logic_vector(to_unsigned(1, CDR_LONG_WIDTH));
        ret(index).MAX_SAMPLES              := std_logic_vector(to_unsigned(1, CDR_LONG_WIDTH));
        ret(index).MAX_INSTANCES            := std_logic_vector(to_unsigned(1, CDR_LONG_WIDTH));
        ret(index).MAX_SAMPLES_PER_INSTANCE := std_logic_vector(to_unsigned(1, CDR_LONG_WIDTH));
        ret(index).TOPICNAME                := gen_user_string("ros_discovery_info");
        ret(index).TYPENAME                 := gen_user_string("rmw_dds_common::msg::dds_::ParticipantEntitiesInfo_");
        
        return ret;
    end function;
    
    constant ENDPOINT_CONFIG : CONFIG_ARRAY_TYPE(0 to NUM_ENDPOINTS-1) := gen_config;
    
end package body;