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

package rtps_package is
    
    --*****USER CONFIG*****
    -- Unicast IPv4 Address used by all RTPS Entities [Default 192.168.0.80]
    constant IPv4_UNICAST_ADDRESS   : std_logic_vector(31 downto 0) := x"C0A80080";
    -- Number of RTPS Writer Endpoints
    constant NUM_WRITERS            : integer := 0;
    -- Number of RTPS Reader Endpoints
    constant NUM_READERS            : integer := 1; 
    -----------------------------------------------------------------------------------------------------
    -- *DO NOT MODIFY BEGIN*
    constant MAX_ENDPOINTS          : integer := NUM_READERS+NUM_WRITERS;
    -- *DO NOT MODIFY END*
    -----------------------------------------------------------------------------------------------------
    -- PB Value of Default Port Generation (see DDSI-RTPS 2.3 Section 9.6.1)
    constant PORT_CONFIG_PB         : integer := 7400;
    -- DG Value of Default Port Generation (see DDSI-RTPS 2.3 Section 9.6.1)    
    constant PORT_CONFIG_DG         : integer := 250;
    -- PG Value of Default Port Generation (see DDSI-RTPS 2.3 Section 9.6.1)
    constant PORT_CONFIG_PG         : integer := 2; 
    -- D0 Value of Default Port Generation (see DDSI-RTPS 2.3 Section 9.6.1)
    constant PORT_CONFIG_D0         : integer := 0; 
    -- D1 Value of Default Port Generation (see DDSI-RTPS 2.3 Section 9.6.1)
    constant PORT_CONFIG_D1         : integer := 10;
    -- D2 Value of Default Port Generation (see DDSI-RTPS 2.3 Section 9.6.1)    
    constant PORT_CONFIG_D2         : integer := 1; 
    -- D3 Value of Default Port Generation (see DDSI-RTPS 2.3 Section 9.6.1)
    constant PORT_CONFIG_D3         : integer := 11;
    -- Number of Domains    
    constant NUM_DOMAIN             : integer := 1;
    -- MAC Address of underlying network stack (Used to generate GUIDs)
    constant MAC_ADDRESS            : std_logic_vector(47 downto 0) := x"97917E0BA8CF";
    -----------------------------------------------------------------------------------------------------
    -- *DO NOT MODIFY BEGIN*
    type USER_DOMAIN_ID_TYPE is array (NUM_DOMAIN-1 downto 0) of integer;
    -- *DO NOT MODIFY END*
    -----------------------------------------------------------------------------------------------------
    -- Array of Domain IDs
    constant USER_DOMAIN_ID     : USER_DOMAIN_ID_TYPE := (0 => 1);
    
    -----------------------------------------------------------------------------------------------------
    -- *DO NOT MODIFY BEGIN*
    type ENDPOINT_DOMAIN_MAP_TYPE is array (MAX_ENDPOINTS-1 downto 0) of integer;
    type ENDPOINT_WITH_KEY_TYPE is array (MAX_ENDPOINTS-1 downto 0) of boolean;
    -- *DO NOT MODIFY END*
    -----------------------------------------------------------------------------------------------------
    
    --***RTPS ENDPOINTS***
    -- Array mapping the RTPS Endpoints to their respective Domain IDs
    -- The index of this array denotes the Endpoint, and the element value the index of the Domain in the 'USER_DOMAIN_ID'.
    constant ENDPOINT_DOMAIN_MAP: ENDPOINT_DOMAIN_MAP_TYPE := (0 => 0);
    -- Array denoting if Endpoints use Keyed Topics
    constant ENDPOINT_WITH_KEY  : ENDPOINT_WITH_KEY_TYPE := (0 => FALSE);
    
    
    --*****DDSI-RTPS 2.3*****

    -- Default Multicast Ipv4 Address (239.255.0.1)
    constant DEFAULT_IPv4_MULTICAST_ADDRESS : std_logic_vector(31 downto 0) := x"EFFF0001";

    constant GUIDPREFIX_WIDTH       : integer := 96;
    constant PROTOCOLVERSION_WIDTH  : integer := 16;
    constant VENDORID_WIDTH         : integer := 16;
    constant SUBMESSAGE_ID_WIDTH    : integer := 8;
    constant DOMAIN_ID_WIDTH        : integer := 32;
    constant UDP_PORT_WIDTH         : integer := 16;
    constant ENTITYID_WIDTH         : integer := 32;
    constant PROTOCOL_WIDTH         : integer := 32;
    
    -- 'RTPS' in Ascii code 
    constant PROTOCOL_RTPS          : std_logic_vector(PROTOCOL_WIDTH-1 downto 0) := x"52545053";
    constant PROTOCOLVERSION_2_4    : std_logic_vector(PROTOCOLVERSION_WIDTH-1 downto 0) := x"0204";
    constant VENDORID_UNKNOWN       : std_logic_vector(VENDORID_WIDTH-1 downto 0) := (others => '0');
    constant VENDORID               : std_logic_vector(VENDORID_WIDTH-1 downto 0) := VENDORID_UNKNOWN;
    -- Submessage IDs
    constant SID_PAD            : std_logic_vector(SUBMESSAGE_ID_WIDTH-1 downto 0) := x"01";
    constant SID_ACKNACK        : std_logic_vector(SUBMESSAGE_ID_WIDTH-1 downto 0) := x"06";
    constant SID_HEARTBEAT      : std_logic_vector(SUBMESSAGE_ID_WIDTH-1 downto 0) := x"07";
    constant SID_GAP            : std_logic_vector(SUBMESSAGE_ID_WIDTH-1 downto 0) := x"08";
    constant SID_INFO_TS        : std_logic_vector(SUBMESSAGE_ID_WIDTH-1 downto 0) := x"09";
    constant SID_INFO_SRC       : std_logic_vector(SUBMESSAGE_ID_WIDTH-1 downto 0) := x"0c";
    constant SID_INFO_REPLY_IP4 : std_logic_vector(SUBMESSAGE_ID_WIDTH-1 downto 0) := x"0d";
    constant SID_INFO_DST       : std_logic_vector(SUBMESSAGE_ID_WIDTH-1 downto 0) := x"0e";
    constant SID_INFO_REPLY     : std_logic_vector(SUBMESSAGE_ID_WIDTH-1 downto 0) := x"0f";
    constant SID_NACK_FRAG      : std_logic_vector(SUBMESSAGE_ID_WIDTH-1 downto 0) := x"12";
    constant SID_HEARTBEAT_FRAG : std_logic_vector(SUBMESSAGE_ID_WIDTH-1 downto 0) := x"13";
    constant SID_DATA           : std_logic_vector(SUBMESSAGE_ID_WIDTH-1 downto 0) := x"15";
    constant SID_DATA_FRAG      : std_logic_vector(SUBMESSAGE_ID_WIDTH-1 downto 0) := x"16";
    
    
    
    type DOMAIN_ID_TYPE is array (NUM_DOMAIN-1 downto 0) of std_logic_vector(DOMAIN_ID_WIDTH-1 downto 0);
    constant DOMAIN_ID : DOMAIN_ID_TYPE; -- Deferred to Package Body
    
    type DOMAIN_ENDPOINT_MAP_TYPE is array (NUM_DOMAIN-1 downto 0) of std_logic_vector(MAX_ENDPOINTS-1 downto 0);
    constant DOMAIN_ENDPOINT_MAP : DOMAIN_ENDPOINT_MAP_TYPE; -- Deferred to Package Body
    
    -- Since this implementation runs on the same network stack and the RTPS Endpoints (Readers & Writers) 
    -- can be differentiated based on their Entity ID, it makes no sense to have multiple IP Addresses
    -- (and hence multiple Participants). 
    -- We generate just single participant for every Domain, and later match the Endpoints to their respective
    -- Domain (and thus also to their respective RTPS Participant).
    
    type IPv4_PORT_TYPE is array (NUM_DOMAIN-1 downto 0) of std_logic_vector(UDP_PORT_WIDTH-1 downto 0);
    constant META_IPv4_MULTICAST_PORT: IPv4_PORT_TYPE; -- Deferred to Package Body
    constant META_IPv4_UNICAST_PORT  : IPv4_PORT_TYPE; -- Deferred to Package Body
    constant USER_IPv4_MULTICAST_PORT: IPv4_PORT_TYPE; -- Deferred to Package Body
    constant USER_IPv4_UNICAST_PORT  : IPv4_PORT_TYPE; -- Deferred to Package Body
    
    type GUIDPREFIX_TYPE is array (NUM_DOMAIN-1 downto 0) of std_logic_vector(GUIDPREFIX_WIDTH-1 downto 0);
    constant GUIDPREFIX         : GUIDPREFIX_TYPE; -- Deferred to Package Body
    constant GUIDPREFIX_UNKNOWN : std_logic_vector(GUIDPREFIX_WIDTH-1 downto 0) := (others => '0');
    
    subtype ENTITY_KIND_H is std_logic_vector(1 downto 0);
    subtype ENTITY_KIND_L is std_logic_vector(5 downto 0);
    --FOLLOWING MAP TO ENTITY_KIND_H
    constant USER_DEFINED_ENTITY    : ENTITY_KIND_H := "00";
    constant BUILT_IN_ENTITY        : ENTITY_KIND_H := "11";
    constant VENDOR_SPECIFIC_ENTITY : ENTITY_KIND_H := "01";
    --FOLLOWING MAP TO ENTITY_KIND_L
    constant WRITER_WITH_KEY    : ENTITY_KIND_L := "000010";
    constant WRITER_NO_KEY      : ENTITY_KIND_L := "000011";
    constant READER_NO_KEY      : ENTITY_KIND_L := "000100";
    constant READER_WITH_KEY    : ENTITY_KIND_L := "000111";
    constant WRITER_GROUP       : ENTITY_KIND_L := "001000";
    constant READER_GROUP       : ENTITY_KIND_L := "001001";
    
    -- DDSI-RTPS 2.3 states that Entity IDs have to be unique within each Participant.
    -- For simplicity and ease of mapping we make the Entity IDs unique across all Participant and Domains on this node.
    type ENTITYID_TYPE is array (MAX_ENDPOINTS-1 downto 0) of std_logic_vector(ENTITYID_WIDTH-1 downto 0);
    constant ENTITYID : ENTITYID_TYPE; -- Deferred to Package Body
    constant ENTITYID_UNKNOWN                               : std_logic_vector(ENTITYID_WIDTH-1 downto 0) := (others => '0');
    constant ENTITYID_PARTICIPANT                           : std_logic_vector(ENTITYID_WIDTH-1 downto 0) := (x"000001c1");
    constant ENTITYID_SEDP_BUILTIN_TOPICS_ANNOUNCER         : std_logic_vector(ENTITYID_WIDTH-1 downto 0) := (x"000002c2");
    constant ENTITYID_SEDP_BUILTIN_TOPICS_DETECTOR          : std_logic_vector(ENTITYID_WIDTH-1 downto 0) := (x"000002c7");
    constant ENTITYID_SEDP_BUILTIN_PUBLICATIONS_ANNOUNCER   : std_logic_vector(ENTITYID_WIDTH-1 downto 0) := (x"000003c2");
    constant ENTITYID_SEDP_BUILTIN_PUBLICATIONS_DETECTOR    : std_logic_vector(ENTITYID_WIDTH-1 downto 0) := (x"000003c7");
    constant ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_ANNOUNCER  : std_logic_vector(ENTITYID_WIDTH-1 downto 0) := (x"000004c2");
    constant ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_DETECTOR   : std_logic_vector(ENTITYID_WIDTH-1 downto 0) := (x"000004c7");
    constant ENTITYID_SPDP_BUILTIN_PARTICIPANT_ANNOUNCER    : std_logic_vector(ENTITYID_WIDTH-1 downto 0) := (x"000100c2");
    constant ENTITYID_SPDP_BUILTIN_PARTICIPANT_DETECTOR     : std_logic_vector(ENTITYID_WIDTH-1 downto 0) := (x"000100c7");
    constant ENTITYID_P2P_BUILTIN_PARTICIPANT_MESSAGE_WRITER: std_logic_vector(ENTITYID_WIDTH-1 downto 0) := (x"000200c2");
    constant ENTITYID_P2P_BUILTIN_PARTICIPANT_MESSAGE_READER: std_logic_vector(ENTITYID_WIDTH-1 downto 0) := (x"000200c7");
    
    constant LOCATOR_KIND_WIDTH     : integer := 32;
    constant LOCATOR_KIND_INVALID   : std_logic_vector := std_logic_vector(to_signed(-1,LOCATOR_KIND_WIDTH));
    constant LOCATOR_KIND_RESERVERD : std_logic_vector := std_logic_vector(to_signed(0,LOCATOR_KIND_WIDTH));
    constant LOCATOR_KIND_UDPv4     : std_logic_vector := std_logic_vector(to_signed(1,LOCATOR_KIND_WIDTH));
    constant LOCATOR_KIND_UDPv6     : std_logic_vector := std_logic_vector(to_signed(2,LOCATOR_KIND_WIDTH));
    
    --*****CUSTOM*****
    
    --****************
    
    type USER_ENDPOINT_OUTPUT is array (MAX_ENDPOINTS-1 downto 0) of std_logic_vector(31 downto 0);
    type BUILTIN_ENDPOINT_TYPE is array (NUM_DOMAIN-1 downto 0) of std_logic_vector(31 downto 0);
    
end package;

package body rtps_package is

    function gen_domain_ids (user_id : USER_DOMAIN_ID_TYPE) return DOMAIN_ID_TYPE is
        variable ret : DOMAIN_ID_TYPE;
    begin
        ret := (others => (others => '0'));
        for i in 0 to user_id'length-1 loop
            -- Check if User provided Domain ID fits
            -- NOTE: Cannot assert due to vhdl integer overflow.
            --assert (user_id(i) < (2**DOMAIN_ID_WIDTH-1 - PORT_CONFIG_PB) / PORT_CONFIG_DG) report "Domain ID range exceeded" severity failure;
            ret(i) := std_logic_vector(to_unsigned(user_id(i), ret(i)'length));
        end loop;
        return ret;
    end function;
    
    constant DOMAIN_ID : DOMAIN_ID_TYPE := gen_domain_ids(USER_DOMAIN_ID);
    
    function gen_domain_endpoint_map (end_id : ENDPOINT_DOMAIN_MAP_TYPE) return DOMAIN_ENDPOINT_MAP_TYPE is
        variable ret : DOMAIN_ENDPOINT_MAP_TYPE;
    begin
        ret := (others => (others => '0'));
        for i in 0 to NUM_DOMAIN-1 loop
            for j in 0 to MAX_ENDPOINTS-1 loop
                if (i = end_id(j)) then
                    ret(i)(j) := '1';
                end if;
            end loop;
        end loop;
        return ret;
    end function;
    
    constant DOMAIN_ENDPOINT_MAP : DOMAIN_ENDPOINT_MAP_TYPE := gen_domain_endpoint_map(ENDPOINT_DOMAIN_MAP);
    
    function gen_meta_multicast_ports (domain_id : DOMAIN_ID_TYPE) return IPv4_PORT_TYPE is
        variable ret : IPv4_PORT_TYPE;
    begin
        ret := (others => (others => '0'));
        for i in 0 to domain_id'length-1 loop
            -- (see DDSI-RTPS 2.3 Section 9.6.1)
            -- PB + DG * domain_id + d0
            ret(i) := std_logic_vector(to_unsigned(PORT_CONFIG_PB + PORT_CONFIG_D0 + PORT_CONFIG_DG*to_integer(unsigned(domain_id(i))), ret(i)'length));
        end loop;
        return ret;
    end function;
    
    function gen_meta_unicast_ports (domain_id : DOMAIN_ID_TYPE) return IPv4_PORT_TYPE is
        variable ret : IPv4_PORT_TYPE;
    begin
        ret := (others => (others => '0'));
        for i in 0 to domain_id'length-1 loop
            -- (see DDSI-RTPS 2.3 Section 9.6.1)
            -- PB + DG * domainId + d1 + PG * participant_id
            -- participant_id=0
            ret(i) := std_logic_vector(to_unsigned(PORT_CONFIG_PB + PORT_CONFIG_D1 + PORT_CONFIG_DG*to_integer(unsigned(domain_id(i))), ret(i)'length));
        end loop;
        return ret;
    end function;
    
    function gen_user_multicast_ports (domain_id : DOMAIN_ID_TYPE) return IPv4_PORT_TYPE is
        variable ret : IPv4_PORT_TYPE;
    begin
        ret := (others => (others => '0'));
        for i in 0 to domain_id'length-1 loop
            -- (see DDSI-RTPS 2.3 Section 9.6.1)
            -- PB + DG * domainId + d2
            ret(i) := std_logic_vector(to_unsigned(PORT_CONFIG_PB + PORT_CONFIG_D2 + PORT_CONFIG_DG*to_integer(unsigned(domain_id(i))), ret(i)'length));
        end loop;
        return ret;
    end function;
    
    function gen_user_unicast_ports (domain_id : DOMAIN_ID_TYPE) return IPv4_PORT_TYPE is
        variable ret : IPv4_PORT_TYPE;
    begin
        ret := (others => (others => '0'));
        for i in 0 to domain_id'length-1 loop
            -- (see DDSI-RTPS 2.3 Section 9.6.1)
            -- PB + DG * domainId + d3 + PG * participant_id
            -- participant_id=0
            ret(i) := std_logic_vector(to_unsigned(PORT_CONFIG_PB + PORT_CONFIG_D3 + PORT_CONFIG_DG*to_integer(unsigned(domain_id(i))), ret(i)'length));
        end loop;
        return ret;
    end function;
    
    constant META_IPv4_MULTICAST_PORT: IPv4_PORT_TYPE := gen_meta_multicast_ports(DOMAIN_ID);
    constant META_IPv4_UNICAST_PORT  : IPv4_PORT_TYPE := gen_meta_unicast_ports(DOMAIN_ID);
    constant USER_IPv4_MULTICAST_PORT: IPv4_PORT_TYPE := gen_user_multicast_ports(DOMAIN_ID);
    constant USER_IPv4_UNICAST_PORT  : IPv4_PORT_TYPE := gen_user_unicast_ports(DOMAIN_ID);
    
    function gen_guid_prefix (domain_id : DOMAIN_ID_TYPE) return GUIDPREFIX_TYPE is
        variable ret : GUIDPREFIX_TYPE;
    begin
        ret := (others => (others => '0'));
        for i in 0 to ret'length-1 loop
            -- First to bytes have to be Vendor ID (see DDSI-RTPS 2.3 Section 9.3.1.5)
            ret(i)(ret(i)'length-1 downto ret(i)'length-VENDORID_WIDTH) := VENDORID;
            -- Next we insert the MAC address for uniqueness
            ret(i)(ret(i)'length-VENDORID_WIDTH-1 downto ret(i)'length-VENDORID_WIDTH-48) := MAC_ADDRESS;
            -- Next we insert the Domain ID
            -- NOTE: If the widths of signals change in the future, this will overwrite part of the MAC Address.
            ret(i)(DOMAIN_ID_WIDTH-1 downto 0) := domain_id(i);
        end loop;
        return ret;
    end function;
    
    constant GUIDPREFIX         : GUIDPREFIX_TYPE := gen_guid_prefix(DOMAIN_ID);
    
    function gen_entyid return ENTITYID_TYPE is
        variable ret : ENTITYID_TYPE;
    begin
        ret := (others => (others => '0'));
        for i in 0 to ret'length-1 loop
            -- (see DDSI-RTPS 2.3 Section 9.3.1.2)
            -- Entity Kind Mapping 
            ret(i)(7 downto 6)  := USER_DEFINED_ENTITY;
            if (i <= NUM_READERS-1) then
                if (ENDPOINT_WITH_KEY(i)) then
                    ret(i)(5 downto 0)  := READER_WITH_KEY;
                else
                    ret(i)(5 downto 0)  := READER_NO_KEY;
                end if;
            else
                if (ENDPOINT_WITH_KEY(i)) then
                    ret(i)(5 downto 0)  := WRITER_WITH_KEY;
                else
                    ret(i)(5 downto 0)  := WRITER_NO_KEY;
                end if;
            end if;
            -- ID Mapping
            ret(i)(ENTITYID_WIDTH-1 downto 8)  := std_logic_vector(to_unsigned(i,ENTITYID_WIDTH-8));
        end loop;
        return ret;
    end function;
    
    constant ENTITYID : ENTITYID_TYPE := gen_entyid;
    
end package body;