-- 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.ros_config.all;
use work.user_config.all;
use work.rtps_config_package.all;

package ros_config_package is
    constant ENDPOINT_ROS_NODE_MAPPING : ENDPOINT_ROS_NODE_MAPPING_ARRAY_TYPE(0 to NUM_ENDPOINTS-1); -- Deferred to Package Body
    constant ROS_DISCOVERY_DATA : OUTPUT_DATA_TYPE; --Deferred to package body
end package;

package body ros_config_package is
    
    -- NOTE: This function has to mirror the behaviour of the 'gen_config' function of the user_config package
    function gen_endpoint_node_mapping return ENDPOINT_ROS_NODE_MAPPING_ARRAY_TYPE is
        variable ret : ENDPOINT_ROS_NODE_MAPPING_ARRAY_TYPE(0 to NUM_ENDPOINTS-1) := (others => 0);
        variable index : natural;
    begin
        ret := (others => NUM_NODES);
        index   := 0;
        
        -- SUBSCRIPTIONS
        for i in 0 to ROS_SUBSCRIPTIONS'length-1 loop
            ret(index) := ROS_SUBSCRIPTIONS(i).node_id;
            index := index + 1;
        end loop;
        -- SERVICE SUBS
        for i in 0 to ROS_SERVICES'length-1 loop
            ret(index) := ROS_SERVICES(i).node_id;
            index := index + 1;
        end loop;
        -- PUBLICATIONS
        for i in 0 to ROS_PUBLICATIONS'length-1 loop
            ret(index) := ROS_PUBLICATIONS(i).node_id;
            index := index + 1;
        end loop;
        -- SERVICE PUBS
        for i in 0 to ROS_SERVICES'length-1 loop
            ret(index) := ROS_SERVICES(i).node_id;
            index := index + 1;
        end loop;
        
        return ret;
    end function;
    
    constant ENDPOINT_ROS_NODE_MAPPING : ENDPOINT_ROS_NODE_MAPPING_ARRAY_TYPE(0 to NUM_ENDPOINTS-1) := gen_endpoint_node_mapping;
    
    function gen_ros_discovery_data return OUTPUT_DATA_TYPE is
        variable ret    : OUTPUT_DATA_TYPE;
        variable gid    : GID_TYPE;
        variable tmp    : natural;
    begin
        ret.data    := (others => (others => '0'));
        ret.length  := 0;
        
        -- Sanity Check
        if (NUM_NODES = 0) then
            return ret;
        end if;
        
        -- Serialized Payload Header
        ret.data(ret.length)    := CDR_BE & x"0000";
        ret.length              := ret.length + 1;
        -- ParticipantEntitiesInfo.gid
        gid := to_gid(to_guid(GUIDPREFIX,ENTITYID_PARTICIPANT));
        for i in 0 to GID'length-1 loop
            ret.data(ret.length)    := gid(i);
            ret.length              := ret.length + 1;
        end loop;
        -- ParticipantEntitiesInfo.node_entities_info_seq [LENGTH]
        ret.data(ret.length)    := std_logic_vector(to_unsigned(NUM_NODES,WORD_WIDTH));
        ret.length              := ret.length + 1;
        for i in 0 to NUM_NODES-1 loop
            -- ParticipantEntitiesInfo.node_entities_info_seq[i].node_namespace
            ret.data(ret.length)    := std_logic_vector(to_unsigned(string_len(ROS_NODES(i).namespace),WORD_WIDTH));
            ret.length              := ret.length + 1;
            for j in 0 to round_div(string_len(ROS_NODES(i).namespace),WORD_WIDTH/BYTE_WIDTH)-1 loop
                ret.data(ret.length)    := convert_string(ROS_NODES(i).namespace)(j);
                ret.length              := ret.length + 1;
            end loop;
            -- ParticipantEntitiesInfo.node_entities_info_seq[i].node_name
            ret.data(ret.length)    := std_logic_vector(to_unsigned(string_len(ROS_NODES(i).name),WORD_WIDTH));
            ret.length              := ret.length + 1;
            for j in 0 to round_div(string_len(ROS_NODES(i).name),WORD_WIDTH/BYTE_WIDTH)-1 loop
                ret.data(ret.length)    := convert_string(ROS_NODES(i).name)(j);
                ret.length              := ret.length + 1;
            end loop;
            -- ParticipantEntitiesInfo.node_entities_info_seq[i].reader_gid_seq [LENGTH]
            tmp := 0;
            for j in 0 to NUM_READERS-1 loop
                if (i = ENDPOINT_ROS_NODE_MAPPING(j)) then
                    tmp := tmp + 1;
                end if;
            end loop;
            ret.data(ret.length)    := std_logic_vector(to_unsigned(tmp,WORD_WIDTH));
            ret.length              := ret.length + 1;
            -- ParticipantEntitiesInfo.node_entities_info_seq[i].reader_gid_seq[j]
            for j in 0 to NUM_READERS-1 loop
                if (i = ENDPOINT_ROS_NODE_MAPPING(j)) then
                    gid := to_gid(to_guid(GUIDPREFIX,ENTITYID(j)));
                    for k in 0 to GID'length-1 loop
                        ret.data(ret.length)    := gid(k);
                        ret.length              := ret.length + 1;
                    end loop;
                end if;
            end loop;
            -- ParticipantEntitiesInfo.node_entities_info_seq[i].writer_gid_seq [LENGTH]
            tmp := 0;
            for j in NUM_READERS to NUM_ENDPOINTS-1 loop
                if (i = ENDPOINT_ROS_NODE_MAPPING(j)) then
                    tmp := tmp + 1;
                end if;
            end loop;
            ret.data(ret.length)    := std_logic_vector(to_unsigned(tmp,WORD_WIDTH));
            ret.length              := ret.length + 1;
            -- ParticipantEntitiesInfo.node_entities_info_seq[i].writer_gid_seq[j]
            for j in NUM_READERS to NUM_ENDPOINTS-1 loop
                if (i = ENDPOINT_ROS_NODE_MAPPING(j)) then
                    gid := to_gid(to_guid(GUIDPREFIX,ENTITYID(j)));
                    for k in 0 to GID'length-1 loop
                        ret.data(ret.length)    := gid(k);
                        ret.length              := ret.length + 1;
                    end loop;
                end if;
            end loop;
        end loop;
        
        return ret;
    end function;
    
    constant ROS_DISCOVERY_DATA : OUTPUT_DATA_TYPE := gen_ros_discovery_data;
    
end package body;