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

-- TODO: Adding LIFESPAN Duration in the stored Endpoint Metatraffic Data would allow us to not expect in-line QoS (Which could be a significant overhead)


entity rtps_reader is
    generic (
        ENTITYID                    : std_logic_vector(ENTITYID_WIDTH-1 downto 0);
        RELIABILTY_QOS              : std_logic_vector(CDR_ENUMERATION_WIDTH-1 downto 0);
        LIVELINESS_QOS              : std_logic_vector(CDR_ENUMERATION_WIDTH-1 downto 0);
        DURABILITY_QOS              : std_logic_vector(CDR_ENUMERATION_WIDTH-1 downto 0);
        HEARTBEAT_RESPONSE_DELAY    : DURATION_TYPE;
        HEARTBEAT_SUPPRESSION_DELAY : DURATION_TYPE;
        LEASE_DURATION              : DURATION_TYPE;
        WITH_KEY                    : boolean;
        MAX_REMOTE_ENDPOINTS        : natural := 50
    );
    port (
        -- SYSTEM
        clk                 : in std_logic;
        reset               : in std_logic;
        time                : in TIME_TYPE;
        -- FROM RTPS_HANDLER (USER TRAFFIC)
        empty_user          : in std_logic;
        rd_user             : out std_logic;
        data_in_user        : in std_logic_vector(WORD_WIDTH-1 downto 0);
        last_word_in_user   : in std_logic;
        -- FROM RTPS_BUILTIN_ENDPOINT (META TRAFFIC)
        empty_meta          : in std_logic;
        rd_meta             : out std_logic;
        data_in_meta        : in std_logic_vector(WORD_WIDTH-1 downto 0);
        last_word_in_meta   : in std_logic;
        -- RTPS OUTPUT
        wr_rtps             : out std_logic;
        full_rtps           : in std_logic;
        last_word_out_rtps  : out std_logic;
        data_out_rtps       : out std_logic_vector(WORD_WIDTH-1 downto 0);
        -- TO HISTORY CACHE
        start_hc            : out std_logic;
        opcode_hc           : out HISTORY_CACHE_OPCODE_TYPE;
        ack_hc              : in std_logic;
        done_hc             : in std_logic;
        ret_hc              : in HISTORY_CACHE_RESPONSE_TYPE;
        data_out_hc         : out std_logic_vector(WORD_WIDTH-1 downto 0);
        valid_out_hc        : out std_logic;
        ready_out_hc        : in std_logic;
        last_word_out_hc    : out std_logic
    );
end entity;

architecture arch of rtps_reader is
    
    --*****CONSTANT DECLARATION*****
    -- *ENDPOINT MEMORY*
    -- 4-Byte Word Size of a Remote Endpoint Entry in Memory
    function gen_frame_size(qos : std_logic_vector(CDR_ENUMERATION_WIDTH-1 downto 0)) return natural is
        variable ret : natural := 0;
    begin
        ret := 12 when (qos = RELIABLE_RELIABILITY_QOS) else 8;
        return ret;
    end function;
    constant ENDPOINT_FRAME_SIZE            : natural := gen_frame_size(RELIABILTY_QOS);
    -- Endpoint Memory Size in 4-Byte Words
    constant ENDPOINT_MEMORY_SIZE           : natural := MAX_REMOTE_ENDPOINTS * ENDPOINT_FRAME_SIZE;
    -- Endpoint Memory Address Width
    constant ENDPOINT_MEMORY_ADDR_WIDTH     : natural := log2c(ENDPOINT_MEMORY_SIZE);
    -- Highest Endpoint Memory Address
    constant ENDPOINT_MEMORY_MAX_ADDRESS    : unsigned(ENDPOINT_MEMORY_ADDR_WIDTH-1 downto 0) := to_unsigned(ENDPOINT_MEMORY_SIZE-1, ENDPOINT_MEMORY_ADDR_WIDTH);
    -- Highest Endpoint Frame Address
    constant MAX_ENDPOINT_ADDRESS           : unsigned(ENDPOINT_MEMORY_ADDR_WIDTH-1 downto 0) := ENDPOINT_MEMORY_MAX_ADDRESS - ENDPOINT_FRAME_SIZE + 1;
    -- Address pointing to the beginning of the first Endpoint Data Frame
    constant FIRST_ENDPOINT_ADDRESS         : unsigned(ENDPOINT_MEMORY_ADDR_WIDTH-1 downto 0) := (others => '0');
    
    -- *ENDPOINT MEMORY FORMAT FORMAT FLAGS*
    -- Flags mapping to the respective Endpoint Memory Frame Fields
    constant EMF_FLAG_WIDTH             : natural := 7;
    constant EMF_ENTITYID_FLAG          : std_logic_vector(0 to EMF_FLAG_WIDTH-1) := (0 => '1', others => '0');
    constant EMF_GUIDPREFIX_FLAG        : std_logic_vector(0 to EMF_FLAG_WIDTH-1) := (1 => '1', others => '0');
    constant EMF_IPV4_ADDR_FLAG         : std_logic_vector(0 to EMF_FLAG_WIDTH-1) := (2 => '1', others => '0');
    constant EMF_UDP_PORT_FLAG          : std_logic_vector(0 to EMF_FLAG_WIDTH-1) := (3 => '1', others => '0');
    constant EMF_NEXT_SEQ_NR_FLAG       : std_logic_vector(0 to EMF_FLAG_WIDTH-1) := (4 => '1', others => '0');
    constant EMF_LEASE_DEADLINE_FLAG    : std_logic_vector(0 to EMF_FLAG_WIDTH-1) := (5 => '1', others => '0');
    constant EMF_RES_TIME_FLAG          : std_logic_vector(0 to EMF_FLAG_WIDTH-1) := (6 => '1', others => '0');
    
    -- *ENDPOINT MEMORY FRAME FORMAT*
    -- 4-Byte Word Offsets to Beginning of Respective Fields in the Endpoint Memory Frame
    constant EMF_ENTITYID_OFFSET        : natural := 0;
    constant EMF_GUIDPREFIX_OFFSET      : natural := 1;
    constant EMF_IPV4_ADDR_OFFSET       : natural := 4;
    constant EMF_UDP_PORT_OFFSET        : natural := 5;
    function gen_emf_udp_port_offset(qos : std_logic_vector(CDR_ENUMERATION_WIDTH-1 downto 0)) return natural is
        variable ret : natural := 0;
    begin
        ret := 6 when (qos = RELIABLE_RELIABILITY_QOS) else EMF_GUIDPREFIX_OFFSET + 3;
        return ret;
    end function;
    constant EMF_NEXT_SEQ_NR_OFFSET     : natural := gen_emf_udp_port_offset(RELIABILTY_QOS);
    constant EMF_LEASE_DEADLINE_OFFSET  : natural := EMF_NEXT_SEQ_NR_OFFSET + 2;
    constant EMF_RES_TIME_OFFSET        : natural := EMF_LEASE_DEADLINE_OFFSET + 2;
    
    
    --*****TYPE DECLARATION*****
    -- FSM states. Explained below in detail
    type STAGE_TYPE is (IDLE, LATCH_GUIDPREFIX, LATCH_ENTITYID, INITIATE_ENDPOINT_SEARCH, LATCH_ENDPOINT_DATA, METATRAFFIC_OPERATION, LATCH_SRC_ADDR, 
        LATCH_EXTRA_DATA, LATCH_HEARTBEAT, PROCESS_HEARTBEAT, LATCH_GAP, PROCESS_GAP, FIND_NEXT_VALID_IN_BITMAP, PROCESS_INLINE_QOS, LATCH_LIFESPAN, 
        LATCH_KEY_HASH, LATCH_STATUS_INFO, INITIATE_ADD_CACHE_CHANGE_REQUEST, ADD_CACHE_CHANGE, PUSH_PAYLOAD, FINALIZE_ADD_CACHE_CHANGE_REQUEST, 
        ENDPOINT_STALE_CHECK, SEND_HEADER, SEND_ACKNACK, SKIP_PARAMETER, SKIP_PACKET, SKIP_META_OPERATION);
    -- Memory FSM states. Explained below in detail
    type MEM_STAGE_TYPE is (IDLE, SEARCH_ENDPOINT, GET_ENDPOINT_DATA, INSERT_ENDPOINT, UPDATE_ENDPOINT, REMOVE_ENDPOINT, FIND_EMPTY_SLOT, 
        RESET_MAX_POINTER, GET_NEXT_ENDPOINT, RESET_MEMORY);
    -- *Memory FSM Opcodes*
    --  OPCODE              DESCRIPTION
    --  SEARCH_ENDPOINT     Find Endpoint with specified GUID in memory
    --  INSERT_ENDPOINT     Insert Endpoint to first available empty_user slot in memory
    --  UPDATE_ENDPOINT     Update Endpoint pointed by mem_addr_base. (mem_field_flags specifies which Fields to update)
    --  REMOVE_ENDPOINT     Remove Endpoint pointed by mem_addr_base
    --  GET_FIRST_ENDPOINT  Get Endpoint Data of first Endpoint stored in Memory. (mem_field_flags specifies which Fields to get)
    --  GET_NEXT_ENDPOINT   Get Endpoint Data of next Endpoint (from the Endpoint pointed by mem_addr_base) stored in Memory. (mem_field_flags specifies which Fields to get)
    --  GET_ENDPOINT        Get Endpoint Data from Endpoint currently pointed by mem_addr_base. (mem_field_flags specifies which Fields to get)
    type MEM_OPCODE_TYPE is (NOP, SEARCH_ENDPOINT, INSERT_ENDPOINT, UPDATE_ENDPOINT, REMOVE_ENDPOINT, GET_FIRST_ENDPOINT, GET_NEXT_ENDPOINT, 
        GET_ENDPOINT);
    -- Record of Endpoint Data
    type ENDPOINT_DATA_TYPE is record
        guid                : GUID_TYPE;
        addr                : std_logic_vector(IPv4_ADDRESS_WIDTH-1 downto 0);
        portn               : std_logic_vector(UDP_PORT_WIDTH-1 downto 0);
        next_seq_nr         : SEQUENCENUMBER_TYPE;
        lease_deadline      : TIME_TYPE;
        res_time            : TIME_TYPE;
    end record;
    -- Zero initialized Endpoint Data
    constant ZERO_ENDPOINT_DATA : ENDPOINT_DATA_TYPE := (
        guid                => GUID_UNKNOWN,
        addr                => IPv4_ADDRESS_INVALID,
        portn               => UDP_PORT_INVALID,
        next_seq_nr         => SEQUENCENUMBER_UNKNOWN,
        lease_deadline      => TIME_INVALID,
        res_time            => TIME_INVALID
    );
    -- Endpoint Data Latch used as temporal cache by Memory Process
    type ENDPOINT_LATCH_DATA_TYPE is record
        guid                : GUID_TYPE;
        addr                : std_logic_vector(IPv4_ADDRESS_WIDTH-1 downto 0);
        portn               : std_logic_vector(UDP_PORT_WIDTH-1 downto 0);
        lease_deadline      : TIME_TYPE;
        res_time            : TIME_TYPE;
        next_seq_nr         : SEQUENCENUMBER_TYPE;
        field_flag          : std_logic_vector(0 to EMF_FLAG_WIDTH-1);
    end record;
    -- Zero initialized Endpoint Data Latch
    constant ZERO_ENDPOINT_LATCH_DATA : ENDPOINT_LATCH_DATA_TYPE := (
        guid                => GUID_UNKNOWN,
        addr                => IPv4_ADDRESS_INVALID,
        portn               => UDP_PORT_INVALID,
        lease_deadline      => TIME_INVALID,
        res_time            => TIME_INVALID,
        next_seq_nr         => SEQUENCENUMBER_UNKNOWN,
        field_flag          => (others => '0')
    );
    
    
    --*****SIGNAL DECLARATION*****
    -- *MAIN PROCESS*
    -- FSM state
    signal stage, stage_next : STAGE_TYPE := IDLE;
    -- FSM state latch. Used to transition dynamically to different states from the same state.
    signal return_stage, return_stage_next : STAGE_TYPE := IDLE;
    -- Intermediate input read signal. (Read from output port not allowed)
    signal rd_sig : std_logic := '0';
    -- Signal used to reset the word counter
    signal reset_read_cnt : std_logic;
    -- Word (4-Byte) counter (Counts words read from input fifo)
    signal read_cnt : unsigned(SUBMESSAGE_LENGTH_WIDTH-3 downto 0) := (others => '0');
    -- Word aligned End of Parameter
    signal parameter_end, parameter_end_next : unsigned(PARAMETER_LENGTH_WIDTH-1 downto 0) := (others => '0');
    -- General Purpose Counter
    signal cnt, cnt_next : natural range 0 to 9 := 0;
    -- Packet Opcode Latch (RTPS Message ID)
    signal opcode, opcode_next : std_logic_vector(SUBMESSAGE_ID_WIDTH-1 downto 0) := (others => '0');
    -- Metatraffic Opcode Latch
    signal meta_opcode, meta_opcode_next : std_logic_vector(WORD_WIDTH-1 downto 0) := (others => '0');
    -- Signifies if the received packet is a metatraffic operation
    signal is_meta, is_meta_next : std_logic := '0';
    -- Source GUID Latch
    signal guid, guid_next : GUID_TYPE := (others => (others => '0'));
    -- Source IPv4 Address Latch
    signal addr, addr_next : std_logic_vector(IPv4_ADDRESS_WIDTH-1 downto 0) := (others => '0');
    -- UDP Port Latch
    signal portn, portn_next : std_logic_vector(UDP_PORT_WIDTH-1 downto 0) := (others => '0');
    -- RTPS Header Flags Latch
    signal flags, flags_next : std_logic_vector(SUBMESSAGE_FLAGS_WIDTH-1 downto 0) := (others => '0');
    -- Source Timestamp Latch
    signal ts, ts_next : TIME_TYPE := TIME_INVALID;
    -- Key Hash Latch
    signal key_hash, key_hash_next  : KEY_HASH_TYPE := (others => (others => '0'));
    -- Signifies if a Key Hash was received
    signal key_hash_rcvd, key_hash_rcvd_next : std_logic := '0';
    -- Status Info Latch
    signal status_info, status_info_next : std_logic_vector(STATUS_INFO_WIDTH-1 downto 0) := (others => '0');
    -- Lifespan Latch
    signal lifespan, lifespan_next : TIME_TYPE := TIME_INVALID;
    -- RTPS Sequence Number Latch
    signal seq_nr, seq_nr_next : SEQUENCENUMBER_TYPE := SEQUENCENUMBER_UNKNOWN;
    -- Signifies the next expected Sequence Number
    signal next_seq_nr, next_seq_nr_next : SEQUENCENUMBER_TYPE := SEQUENCENUMBER_UNKNOWN;
    -- Generic Sequence Number Latch
    signal sn_latch_1, sn_latch_1_next : SEQUENCENUMBER_TYPE := SEQUENCENUMBER_UNKNOWN;
    -- Generic Sequence Number Latch
    signal sn_latch_2, sn_latch_2_next : SEQUENCENUMBER_TYPE := SEQUENCENUMBER_UNKNOWN;
    -- Generic Sequence Number Latch
    signal sn_latch_3, sn_latch_3_next : SEQUENCENUMBER_TYPE := SEQUENCENUMBER_UNKNOWN;
    -- Toggle latching the "last_word_in_user" signal until reset
    signal last_word_in_latch, last_word_in_latch_next : std_logic := '0';
    -- Time of next Stale Endpoint Check
    signal check_time, check_time_next : TIME_TYPE := TIME_INVALID;
    -- Signifies if a Stale Endpoint Check is in progress
    signal stale_check, stale_check_next : std_logic := '0';
    -- Signal containing the RTPS ACKNACK Count Field
    signal count, count_next : unsigned(COUNT_WIDTH-1 downto 0) := (others => '0');
    -- Data in represented in Big Endian
    signal data_in_swapped : std_logic_vector(WORD_WIDTH-1 downto 0) := (others => '0');
    -- Denotes the number of valid Bitmap longs (4-Byte words)
    signal bitmap_cnt, bitmap_cnt_next : unsigned(log2c(MAX_BITMAP_WIDTH/CDR_LONG_WIDTH)-1 downto 0) := (others => '0');
    -- NumberSet Bitmap Latch
    signal bitmap_latch, bitmap_latch_next : BITMAP_TYPE := (others => (others => '0'));
    -- Counter used to read out Bitmaps
    signal cnt2, cnt2_next : natural range 0 to BITMAP_TYPE'length := 0;
    -- Signal used to iterate through Bitmaps
    signal bitmap_pos, bitmap_pos_next : natural range 0 to MAX_BITMAP_WIDTH-1 := 0;
    -- Signals the start of a Memory Operation
    signal mem_op_start : std_logic := '0';
    -- Opcode of the Memory Operation (Valid only when mem_op_start is high)
    signal mem_opcode : MEM_OPCODE_TYPE := NOP;
    -- Signals the end of a Memory Operation
    signal mem_op_done : std_logic := '0';
    -- Signal containing the relevant Endpoint Memory Format Fields of the Memory Operation
    signal mem_field_flags : std_logic_vector(0 to EMF_FLAG_WIDTH-1) := (others => '0');
    -- Signal used to pass Lease Deadlines from main to memory process
    signal lease_deadline : TIME_TYPE := TIME_INVALID;
    -- Signal used to pass Response Deadlines from main to memory process
    signal res_time : TIME_TYPE := TIME_INVALID;
    -- Test signal used for testbench synchronisation
    signal idle_sig : std_logic := '0';
    
    -- *MEMORY PROCESS*
    -- Memory FSM state
    signal mem_stage, mem_stage_next : MEM_STAGE_TYPE := IDLE;
    -- Pointer to current relevant Endpoint Address
    signal mem_addr_base, mem_addr_base_next : unsigned(ENDPOINT_MEMORY_ADDR_WIDTH-1 downto 0) := (others => '0');
    -- Help signal used to reset the MAX Endpoint Memory Pointer
    signal last_addr, last_addr_next : unsigned(ENDPOINT_MEMORY_ADDR_WIDTH-1 downto 0) := (others => '0');
    -- General Memory Address Latch
    signal mem_addr_latch, mem_addr_latch_next : unsigned(ENDPOINT_MEMORY_ADDR_WIDTH-1 downto 0) := (others => '0');
    -- Highest Endpoint Memory Address (Points to first Address of last occupied Endpoint Frame)
    signal max_endpoint_addr, max_endpoint_addr_next : unsigned(ENDPOINT_MEMORY_ADDR_WIDTH-1 downto 0) := (others => '0');
    -- General Purpose Couter
    signal mem_cnt, mem_cnt_next : natural range 0 to 23 := 0;
    -- Latch for Endpoint Data from Memory
    signal mem_endpoint_data, mem_endpoint_data_next : ENDPOINT_DATA_TYPE := ZERO_ENDPOINT_DATA;
    -- Latch for Endpoint Data from main process
    signal mem_endpoint_latch_data, mem_endpoint_latch_data_next : ENDPOINT_LATCH_DATA_TYPE := ZERO_ENDPOINT_LATCH_DATA;
    -- Position (In Endpoint Memory Frame Granularity) of current relevant Endpoint
    signal mem_pos, mem_pos_next : natural range 0 to MAX_REMOTE_ENDPOINTS-1 := 0;
    -- Signifies an abort of the currently initiated read transaction
    signal abort_read : std_logic := '0';
    
    -- *MEMORY CONTROL CONNECTION SIGNALS*
    signal mem_addr : unsigned(ENDPOINT_MEMORY_ADDR_WIDTH-1 downto 0) := (others => '0');
    signal mem_read_data, mem_write_data : std_logic_vector(WORD_WIDTH-1 downto 0) := (others => '0');
    signal mem_ready_in, mem_valid_in : std_logic := '0';
    signal mem_ready_out, mem_valid_out : std_logic := '0';
    signal mem_read : std_logic := '0';
    
    
    --*****ALIAS DECLARATION*****
    -- ENDPOINT FRAME HEADER
    alias header_opcode             : std_logic_vector(7 downto 0) is data_in_user(31 downto 24);
    alias header_flags              : std_logic_vector(7 downto 0) is data_in_user(23 downto 16);
    alias header_udp_port           : std_logic_vector(15 downto 0) is data_in_user(15 downto 0);
    -- RTPS PARAMETER LIST HEADER
    alias parameter_id              : std_logic_vector(15 downto 0) is data_in_user(31 downto 16);
    alias parameter_length          : std_logic_vector(15 downto 0) is data_in_user(15 downto 0);
    alias must_understand           : std_logic is parameter_id(14);
    -- RTPS DATA PAYLOAD HEADER
    alias representation_id         : std_logic_vector(15 downto 0) is data_in_user(31 downto 16);
    alias representation_options    : std_logic_vector(15 downto 0) is data_in_user(15 downto 0);
    -- RTPS SUBMESSAGE FLAGS
    alias endian_flag               : std_logic is flags(0);
    alias qos_flag                  : std_logic is flags(1);
    alias data_flag                 : std_logic is flags(2);
    alias key_flag                  : std_logic is flags(3);
    alias final_flag                : std_logic is flags(1);
    alias payload_flag              : std_logic is flags(4);
    alias liveliness_flag           : std_logic is flags(2);
    -- HEARTBEAT
    alias first_seq_nr              : SEQUENCENUMBER_TYPE is sn_latch_1;
    alias first_seq_nr_next         : SEQUENCENUMBER_TYPE is sn_latch_1_next;
    alias last_seq_nr               : SEQUENCENUMBER_TYPE is sn_latch_2;
    alias last_seq_nr_next          : SEQUENCENUMBER_TYPE is sn_latch_2_next;
    -- GAP
    alias gap_start                 : SEQUENCENUMBER_TYPE is sn_latch_1;
    alias gap_start_next            : SEQUENCENUMBER_TYPE is sn_latch_1_next;
    alias gap_list_base             : SEQUENCENUMBER_TYPE is sn_latch_2;
    alias gap_list_base_next        : SEQUENCENUMBER_TYPE is sn_latch_2_next;
    alias gap_list_end              : SEQUENCENUMBER_TYPE is sn_latch_3;
    alias gap_list_end_next         : SEQUENCENUMBER_TYPE is sn_latch_3_next;
    
    --*****FUNCTION DECLARATION*****
    -- Helper function to convert BITMAP_TYPE to std_logic_vector
    function to_slv_bitmap (input : BITMAP_TYPE) return std_logic_vector is
        variable ret : std_logic_vector(0 to MAX_BITMAP_WIDTH-1) := (others => '0');
    begin
        for i in 0 to BITMAP_TYPE'length-1 loop
            ret(i*WORD_WIDTH to ((i+1)*WORD_WIDTH)-1) := input(i);
        end loop;
        return ret;
    end function;
    
begin
    
    --*****COMPONENT INSTANTIATION*****
    mem_ctrl_inst : entity work.mem_ctrl(arch)
        generic map (
            ADDR_WIDTH          => ENDPOINT_MEMORY_ADDR_WIDTH,
            DATA_WIDTH          => WORD_WIDTH,
            MEMORY_DEPTH        => ENDPOINT_MEMORY_SIZE,
            MAX_BURST_LENGTH    => ENDPOINT_FRAME_SIZE
        )
        port map (
            clk         => clk,
            reset       => reset or abort_read,
            addr        => std_logic_vector(mem_addr),
            read        => mem_read,
            ready_in    => mem_ready_in,
            valid_in    => mem_valid_in,
            data_in     => mem_write_data,
            ready_out   => mem_ready_out,
            valid_out   => mem_valid_out,
            data_out    => mem_read_data
        );
    
    -- Big Endian Representation
    data_in_swapped <= endian_swap(endian_flag, data_in_user);
    rd_user <= rd_sig;
    
    -- *Main State Machine*
    -- STATE                                DESCRIPTION
    -- IDLE                                 Idle State. Initiates Stale Endpoint Checks, Metatraffic Packet Processing, and User Packet Processing, in that priority order.
    -- LATCH_GUIDPREFIX                     Store source GUID Prefix
    -- LATCH_ENTITYID                       Store source Entity ID
    -- INITIATE_ENDPOINT_SEARCH             Initiate Endpoint Search Memory Operation. This state is used to start the Search operation as soon as the required data is available
    -- LATCH_ENDPOINT_DATA                  Store Remote Endpoint Data
    -- METATRAFFIC_OPERATION                State handling the Metatraffic Operations
    -- LATCH_SRC_ADDR                       Store source IPv4 Address
    -- LATCH_EXTRA_DATA                     Store source Timestamp, and source Sequence Number
    -- LATCH_HEARTBEAT                      Store RTPS HEARTBEAT Sequence Numbers
    -- PROCESS_HEARTBEAT                    Parse RTPS HEARTBEAT Message. Update stored Sequence Number if necessary. Set HEARTBEAT response time accordingly.
    -- LATCH_GAP                            Store RTPS GAP Sequence Numbers
    -- PROCESS_GAP                          Parse RTPS GAP Submsessage. Initiates search for next valid Sequence Number if currently expected Sequence Number is in GAP
    -- FIND_NEXT_VALID_IN_BITMAP            Iterate through Bitmap and find the next valid Sequence Number.
    -- PROCESS_INLINE_QOS                   Parse in-line Parameter List QoS
    -- LATCH_LIFESPAN                       Store LIFESPAN_QOS
    -- LATCH_KEY_HASH                       Store KEY_HASH
    -- LATCH_STATUS_INFO                    Store STATUS_INFO
    -- INITIATE_ADD_CACHE_CHANGE_REQUEST    Initiate an ADD_CACHE_CHANGE Operation
    -- ADD_CACHE_CHANGE                     Send CACHE_CHANGE Data
    -- PUSH_PAYLOAD                         Send CACHE_CHANGE Data (Direct Input Passthrough)
    -- FINALIZE_ADD_CACHE_CHANGE_REQUEST    Wait for ADD_CACHE_CHANGE Operation Results. Update Endpoint Data if successfull.
    -- ENDPOINT_STALE_CHECK                 Check remote Endpoint Entries for Liveliness Lease Expiration, and Response Timeouts.
    -- SEND_HEADER                          Send Output Data Header and RTPS Message Header
    -- SEND_ACKNACK                         Send ACKNACK Submessage
    -- SKIP_PARAMETER                       Skip rest of Parameter
    -- SKIP_PACKET                          Skip rest of Packet
    -- SKIP_META_OPERATION                  Skip Metatraffic Operation
    parse_prc : process(all)
        variable tmp_dw     : DOUBLE_WORD_ARRAY := (others => (others => '0'));
        -- NOTE: We convert the bitamp to a slv to make operations easier (The tool should handle both cases equally)
        variable tmp_bitmap : std_logic_vector(0 to MAX_BITMAP_WIDTH-1) := (others => '0');
        variable rd_guard   : std_logic := '0';
        variable tmp_flags  : std_logic_vector(0 to EMF_FLAG_WIDTH-1) := (others => '0');
    begin
        -- DEFAULT Registered
        stage_next                  <= stage;
        meta_opcode_next            <= meta_opcode;
        cnt_next                    <= cnt;
        cnt2_next                   <= cnt2;
        guid_next                   <= guid;
        addr_next                   <= addr;
        portn_next                  <= portn;
        is_meta_next                <= is_meta;
        opcode_next                 <= opcode;
        flags_next                  <= flags;
        seq_nr_next                 <= seq_nr;
        ts_next                     <= ts;
        sn_latch_1_next             <= sn_latch_1;
        sn_latch_2_next             <= sn_latch_2;
        sn_latch_3_next             <= sn_latch_3;
        bitmap_latch                <= bitmap_latch_next;
        bitmap_cnt                  <= bitmap_cnt_next;
        bitmap_pos_next             <= bitmap_pos;
        key_hash_rcvd_next          <= key_hash_rcvd;
        key_hash_next               <= key_hash;
        status_info_next            <= status_info;
        last_word_in_latch_next     <= last_word_in_latch;
        stale_check_next            <= stale_check;
        count_next                  <= count;
        return_stage_next           <= return_stage;
        check_time_next             <= check_time;
        lifespan_next               <= lifespan;
        parameter_end_next          <= parameter_end;
        next_seq_nr_next            <= next_seq_nr;
        -- DEFAULT Unregistered
        mem_opcode                  <= NOP;
        opcode_hc                   <= NOP;
        lease_deadline              <= TIME_INVALID;
        res_time                    <= TIME_INVALID;
        reset_read_cnt              <= '0';
        rd_meta                     <= '0';
        mem_op_start                <= '0';
        start_hc                    <= '0';
        valid_out_hc                <= '0';
        last_word_out_hc            <= '0';
        wr_rtps                     <= '0';
        last_word_out_rtps          <= '0';
        idle_sig                    <= '0';
        rd_guard                    := '0';
        mem_field_flags             <= (others => '0');
        data_out_hc                 <= (others => '0');
        data_out_rtps               <= (others => '0');
        
        
        
        -- Last Word Latch Setter
        if (last_word_in_user = '1') then
            last_word_in_latch_next    <= '1';
        end if;
        
        case (stage) is
            when IDLE =>
                idle_sig    <= '1';
                -- RESET
                lifespan_next               <= TIME_INVALID;
                addr_next                   <= (others => '0');
                portn_next                  <= (others => '0');
                status_info_next            <= (others => '0');
                key_hash_rcvd_next          <= '0';
                is_meta_next                <= '0';
                
                -- Stale Check Timeout
                if (time >= check_time) then
                    -- Memory Operation Guard
                    if (mem_op_done = '1') then
                        stale_check_next    <= '1';
                        stage_next          <= ENDPOINT_STALE_CHECK;
                        cnt_next            <= 1;
                        mem_op_start        <= '1';
                        mem_opcode          <= GET_FIRST_ENDPOINT;
                        mem_field_flags     <= EMF_LEASE_DEADLINE_FLAG or EMF_RES_TIME_FLAG;
                        -- Reset Timeout
                        check_time_next     <= TIME_INFINITE;
                    end if;
                -- Input FIFO Guard
                elsif (empty_meta = '0') then
                    rd_meta     <= '1';
                    
                    -- Mark as METATRAFFIC
                    is_meta_next        <= '1';
                    
                    -- Latch Endpoint Metatraffic Opcode
                    meta_opcode_next    <= data_in_meta;
                    
                    case (data_in_meta) is
                        when OPCODE_ENDPOINT_MATCH =>
                            stage_next  <= LATCH_GUIDPREFIX;
                            cnt_next    <= 0;
                        when OPCODE_ENDPOINT_UNMATCH =>
                            stage_next  <= LATCH_GUIDPREFIX;
                            cnt_next    <= 0;
                        when OPCODE_PARTICIPANT_UNMATCH =>
                            stage_next  <= LATCH_GUIDPREFIX;
                            cnt_next    <= 0;
                        when OPCODE_LIVELINESS_UPDATE =>
                            -- Synthesis Guard
                            if (LIVELINESS_QOS /= MANUAL_BY_TOPIC_LIVELINESS_QOS) then
                                stage_next  <= LATCH_GUIDPREFIX;
                                cnt_next    <= 0;
                            else
                                stage_next  <= SKIP_META_OPERATION;
                            end if;
                        when others =>
                            stage_next  <= SKIP_META_OPERATION;
                    end case;
                -- Input FIFO Guard
                elsif (empty_user = '0') then
                    rd_guard        := '1';
                    
                    -- Latch Opcode
                    opcode_next <= header_opcode;
                    -- Latch Flags
                    flags_next  <= header_flags;
                    -- Latch Source UDP Port
                    portn_next  <= header_udp_port;
                    
                    stage_next  <= LATCH_SRC_ADDR;
                    -- SANITY CHECK: Skip Packet if non-standard Payload
                    if(header_opcode = SID_DATA and header_flags(SUBMESSAGE_NON_STANDARD_PAYLOAD_FLAG_POS) = '1') then
                        stage_next  <= SKIP_PACKET;
                    end if;
                end if;
            when LATCH_GUIDPREFIX =>
                -- Input FIFO Guard
                if ((is_meta = '1' and empty_meta = '0') or (is_meta = '0' and empty_user = '0')) then
                    if (is_meta = '1') then
                        rd_meta     <= '1';
                    else
                        rd_guard    := '1';
                    end if;
                    cnt_next    <= cnt + 1;
                    
                    case (cnt) is
                        -- GUID Prefix 1/3
                        when 0 =>
                            if (is_meta = '1') then
                                guid_next(0)    <= data_in_meta;
                            else
                                guid_next(0)    <= data_in_user;
                            end if;
                        -- GUID Prefix 2/3
                        when 1 =>
                            if (is_meta = '1') then
                                guid_next(1)    <= data_in_meta;
                            else
                                guid_next(1)    <= data_in_user;
                            end if;
                        -- GUID Prefix 3/3
                        when 2 =>
                            if (is_meta = '1') then
                                guid_next(2)    <= data_in_meta;
                            else
                                guid_next(2)    <= data_in_user;
                            end if;
                            
                            if (is_meta = '1' and (meta_opcode = OPCODE_PARTICIPANT_UNMATCH or (LIVELINESS_QOS /= MANUAL_BY_TOPIC_LIVELINESS_QOS and meta_opcode = OPCODE_LIVELINESS_UPDATE))) then
                                --assert (last_word_in_meta = '1') report "last_word_in_meta not set" severity FAILURE;
                                -- DONE Parsing
                                stage_next  <= INITIATE_ENDPOINT_SEARCH;
                            else
                                stage_next  <= LATCH_ENTITYID;
                            end if;
                        when others =>
                            null;
                    end case;
                end if;
            when LATCH_ENTITYID =>
                -- Input FIFO Guard
                if ((is_meta = '1' and empty_meta = '0') or (is_meta = '0' and empty_user = '0')) then
                    if (is_meta = '1') then
                        --assert (meta_opcode /= OPCODE_ENDPOINT_UNMATCH or (meta_opcode = OPCODE_ENDPOINT_UNMATCH and last_word_in_meta = '1')) report "last_word_in_meta not set" severity FAILURE;
                        rd_meta         <= '1';
                        guid_next(3)    <= data_in_meta;
                    -- Memory Operation Guard
                    else
                        rd_guard        := '1';
                        guid_next(3)    <= data_in_user;
                    end if;
                    stage_next  <= INITIATE_ENDPOINT_SEARCH;
                end if;
            when INITIATE_ENDPOINT_SEARCH =>
                -- Memory Operation Guard
                if (mem_op_done = '1') then
                
                    if (is_meta = '1') then
                        
                        case (meta_opcode) is
                            when OPCODE_ENDPOINT_MATCH =>
                                mem_op_start    <= '1';
                                mem_opcode      <= SEARCH_ENDPOINT;
                                mem_field_flags <= (others => '0');
                                -- Synthesis Guard
                                if (RELIABILTY_QOS = RELIABLE_RELIABILITY_QOS) then
                                    stage_next      <= LATCH_ENDPOINT_DATA;
                                    cnt_next        <= 0;
                                else
                                    stage_next  <= METATRAFFIC_OPERATION;
                                end if;
                            when OPCODE_ENDPOINT_UNMATCH =>
                                mem_op_start    <= '1';
                                mem_opcode      <= SEARCH_ENDPOINT;
                                mem_field_flags <= (others => '0');
                                stage_next      <= METATRAFFIC_OPERATION;
                            when OPCODE_PARTICIPANT_UNMATCH =>
                                mem_op_start    <= '1';
                                mem_opcode      <= GET_FIRST_ENDPOINT;
                                mem_field_flags <= EMF_GUIDPREFIX_FLAG;
                                stage_next      <= METATRAFFIC_OPERATION;
                                cnt_next        <= 0;
                            when OPCODE_LIVELINESS_UPDATE =>
                                -- Synthesis Guard
                                if (LIVELINESS_QOS /= MANUAL_BY_TOPIC_LIVELINESS_QOS) then
                                    mem_op_start    <= '1';
                                    mem_opcode      <= GET_FIRST_ENDPOINT;
                                    mem_field_flags <= EMF_GUIDPREFIX_FLAG;
                                    stage_next      <= METATRAFFIC_OPERATION;
                                    cnt_next        <= 0;
                                end if;
                            when others =>
                                null;
                        end case;
                    else
                        mem_op_start    <= '1';
                        mem_opcode      <= SEARCH_ENDPOINT;
                        
                        case (opcode) is
                            when SID_DATA =>
                                stage_next      <= LATCH_EXTRA_DATA;
                                mem_field_flags <= EMF_NEXT_SEQ_NR_FLAG;
                                cnt_next        <= 0;
                            when SID_HEARTBEAT =>
                                stage_next      <= LATCH_HEARTBEAT;
                                mem_field_flags <= EMF_NEXT_SEQ_NR_FLAG or EMF_RES_TIME_FLAG;
                                cnt_next        <= 0;
                            when SID_GAP =>
                                stage_next      <= LATCH_GAP;
                                mem_field_flags <= EMF_NEXT_SEQ_NR_FLAG;
                                cnt_next        <= 0;
                            when others =>
                                stage_next  <= SKIP_PACKET;
                        end case;
                    end if;
                end if;
            when LATCH_ENDPOINT_DATA =>
                -- Synthesis Guard
                if (RELIABILTY_QOS = RELIABLE_RELIABILITY_QOS) then
                    -- Input FIFO Guard
                    if (empty_meta = '0') then
                        rd_meta     <= '1';
                        cnt_next    <= cnt + 1;
                        
                        case (cnt) is
                            -- IPv4 Address
                            when 0 =>
                                addr_next   <= data_in_meta;
                            -- UDP Port
                            when 1 =>
                                --assert (last_word_in_meta = '1') report "last_word_in_meta not set" severity FAILURE;
                                
                                portn_next  <= data_in_meta(WORD_WIDTH-1 downto WORD_WIDTH-UDP_PORT_WIDTH);
                                
                                stage_next  <= METATRAFFIC_OPERATION;
                            when others =>
                                null;
                        end case;
                    end if;
                end if;
            when METATRAFFIC_OPERATION =>
                -- Memory Operation Guard
                if (mem_op_done = '1') then
                    case (meta_opcode) is
                        when OPCODE_ENDPOINT_MATCH =>
                            -- Endpoint already in Memory
                            if (mem_addr_base /= ENDPOINT_MEMORY_MAX_ADDRESS) then
                                -- Synthesis Guard
                                if (RELIABILTY_QOS = RELIABLE_RELIABILITY_QOS) then
                                    -- Update the Endpoint Data
                                    -- NOTE: The Lease Duration is NOT updated in case of an update. That is the responsibility of the Liveliness Update
                                    mem_op_start    <= '1';
                                    mem_opcode      <= UPDATE_ENDPOINT;
                                    mem_field_flags <= EMF_IPV4_ADDR_FLAG or EMF_UDP_PORT_FLAG;
                                end if;
                                -- DONE
                                stage_next      <= IDLE;
                            else
                                -- Insert Matched Remote Endpoint
                                mem_op_start    <= '1';
                                mem_opcode      <= INSERT_ENDPOINT;
                                if (LEASE_DURATION /= DURATION_INFINITE) then
                                    lease_deadline  <= time + LEASE_DURATION;
                                    
                                    -- XXX: Possible Worst Case Path (64-bit addition and comparison in same clock)
                                    -- Update Check Time
                                    if ((time + LEASE_DURATION) < check_time) then
                                        check_time_next <= time + LEASE_DURATION;
                                    end if;
                                else
                                    lease_deadline  <= TIME_INVALID;
                                end if;
                                -- DONE
                                stage_next      <= IDLE;
                            end if;
                        when OPCODE_ENDPOINT_UNMATCH =>
                            -- Endpoint not in Memory
                            if (mem_addr_base = ENDPOINT_MEMORY_MAX_ADDRESS) then
                                -- Ignore
                                stage_next  <= IDLE;
                            else
                                -- Propagate Removal
                                start_hc       <= '1';
                                opcode_hc      <= REMOVE_WRITER;
                                data_out_hc    <= std_logic_vector(to_unsigned(mem_pos, WORD_WIDTH));
                                -- Wait for Operation Acknowledgement
                                if (ack_hc = '1') then
                                    -- Remove Unmatched Remote Endpoint
                                    mem_op_start    <= '1';
                                    mem_opcode      <= REMOVE_ENDPOINT;
                                    -- DONE
                                    stage_next      <= IDLE;
                                end if;
                            end if;
                        when OPCODE_PARTICIPANT_UNMATCH =>
                            case (cnt) is
                                when 0 =>
                                    -- Reached End of Endpoints
                                    if (mem_addr_base = ENDPOINT_MEMORY_MAX_ADDRESS) then
                                        -- DONE
                                        stage_next  <= IDLE;
                                    else
                                        -- Participant Match
                                        if (guid(0) = mem_endpoint_data.guid(0) and guid(1) = mem_endpoint_data.guid(1) and guid(2) = mem_endpoint_data.guid(2)) then
                                            -- Propagate Removal
                                            start_hc       <= '1';
                                            opcode_hc      <= REMOVE_WRITER;
                                            data_out_hc    <= std_logic_vector(to_unsigned(mem_pos, WORD_WIDTH));
                                            -- Wait for Operation Acknowledgement
                                            if (ack_hc = '1') then
                                                -- Remove Unmatched Remote Endpoint
                                                mem_op_start    <= '1';
                                                mem_opcode      <= REMOVE_ENDPOINT;
                                                cnt_next        <= cnt + 1;
                                            end if;
                                        else
                                            cnt_next    <= cnt + 1;
                                        end if;
                                    end if;
                                when 1 =>
                                    -- Continue Search
                                    mem_op_start    <= '1';
                                    mem_opcode      <= GET_NEXT_ENDPOINT;
                                    mem_field_flags <= EMF_GUIDPREFIX_FLAG;
                                    cnt_next        <= 0;
                                when others =>
                                    null;
                            end case;
                        when OPCODE_LIVELINESS_UPDATE =>
                            -- Synthesis Guard
                            if (LIVELINESS_QOS /= MANUAL_BY_TOPIC_LIVELINESS_QOS) then
                                case (cnt) is
                                    when 0 =>
                                        -- No matches in memory
                                        if (mem_addr_base = ENDPOINT_MEMORY_MAX_ADDRESS) then
                                            -- DONE
                                            stage_next  <= IDLE;
                                        else
                                            -- Participant Match
                                            if (guid(0) = mem_endpoint_data.guid(0) and guid(1) = mem_endpoint_data.guid(1) and guid(2) = mem_endpoint_data.guid(2)) then
                                                -- Renew Lease of Remote Endpoint
                                                mem_op_start    <= '1';
                                                mem_opcode      <= UPDATE_ENDPOINT;
                                                mem_field_flags <= EMF_LEASE_DEADLINE_FLAG;
                                                if (LEASE_DURATION /= DURATION_INFINITE) then
                                                    lease_deadline  <= time + LEASE_DURATION;
                                                    
                                                    -- XXX: Possible Worst Case Path (64-bit addition and comparison in same clock)
                                                    -- Update Check Time
                                                    if ((time + LEASE_DURATION) < check_time) then
                                                        check_time_next <= time + LEASE_DURATION;
                                                    end if;
                                                else
                                                    lease_deadline  <= TIME_INVALID;
                                                end if;
                                            end if;
                                            cnt_next    <= 1;
                                        end if;
                                    when 1 =>
                                        -- Continue Search
                                        mem_op_start    <= '1';
                                        mem_opcode      <= GET_NEXT_ENDPOINT;
                                        mem_field_flags <= EMF_GUIDPREFIX_FLAG;
                                        cnt_next        <= 0;
                                    when others =>
                                        null;
                                end case;
                            end if;
                        when others =>
                            null;
                    end case;
                end if;
            when LATCH_SRC_ADDR =>
                -- Input FIFO Guard
                if (empty_user = '0') then
                    rd_guard        := '1';
                    
                    -- Latch Source IP Address
                    addr_next   <= data_in_user;
                    
                    stage_next  <= LATCH_GUIDPREFIX;
                    cnt_next    <= 0;
                end if;
            when LATCH_EXTRA_DATA =>
                -- Input FIFO Guard
                if (empty_user = '0') then
                    rd_guard    := '1';
                    cnt_next    <= cnt + 1;
                    
                    case (cnt) is
                        -- Sequence Number 1/2
                        when 0 =>
                            seq_nr_next(0)  <= unsigned(data_in_user);
                        -- Sequence Number 2/2
                        when 1 =>
                            seq_nr_next(1)  <= unsigned(data_in_user);
                            -- Store Next Sequence Number
                            tmp_dw              := (0 => seq_nr(0), 1 => unsigned(data_in_user));
                            next_seq_nr_next    <= tmp_dw + 1;
                        -- Timestamp 1/2
                        when 2 =>
                            ts_next(0)      <= unsigned(data_in_user);
                        -- Timestamp 2/2
                        when 3 =>
                            ts_next(1)      <= unsigned(data_in_user);
                            
                            if (qos_flag = '1') then
                                stage_next  <= PROCESS_INLINE_QOS;
                            else
                                stage_next  <= INITIATE_ADD_CACHE_CHANGE_REQUEST;
                            end if;
                        when others => 
                            null;
                    end case;
                end if;
            when LATCH_HEARTBEAT =>
                -- Input FIFO Guard
                if (empty_user = '0') then
                    rd_guard    := '1';
                    cnt_next    <= cnt + 1;
                    
                    case (cnt) is
                        -- First Sequence Number 1/2
                        when 0 =>
                            first_seq_nr_next(0)    <= unsigned(data_in_user);
                        -- First Sequence Number 2/2
                        when 1 =>
                            first_seq_nr_next(1)    <= unsigned(data_in_user);
                        -- Last Sequence Number 1/2
                        when 2 =>
                            last_seq_nr_next(0)     <= unsigned(data_in_user);
                        -- Last Sequence Number 2/2
                        when 3 =>
                            last_seq_nr_next(1)     <= unsigned(data_in_user);
                            
                            stage_next  <= PROCESS_HEARTBEAT;
                        when others => 
                            null;
                    end case;
                end if;
            when PROCESS_HEARTBEAT =>
                -- Wait for Endpoint Search to finish
                if (mem_op_done = '1') then
                    -- DEFAULT
                    stage_next  <= SKIP_PACKET;
                    
                    -- Endpoint in Buffer
                    if (mem_addr_base /= ENDPOINT_MEMORY_MAX_ADDRESS) then
                        
                        -- Default
                        tmp_flags   := (others => '0');
                        tmp_dw      := TIME_INFINITE;
                        
                        -- Liveliness Assertion
                        if (liveliness_flag = '1') then
                            mem_op_start    <= '1';
                            mem_opcode      <= UPDATE_ENDPOINT;
                            tmp_flags       := tmp_flags or EMF_LEASE_DEADLINE_FLAG;
                            if (LEASE_DURATION /= DURATION_INFINITE) then
                                lease_deadline  <= time + LEASE_DURATION;
                                tmp_dw          := time + LEASE_DURATION;
                            else
                                lease_deadline  <= TIME_INVALID;
                            end if;
                        end if;
                        
                        -- No scheduled Heartbeat Response
                        if (mem_endpoint_data.res_time = TIME_INVALID) then
                            -- If Reader is Volatile and we have not received anything from the writer yet
                            if (DURABILITY_QOS = VOLATILE_DURABILITY_QOS and mem_endpoint_data.next_seq_nr = SEQUENCENUMBER_UNKNOWN) then
                                -- Mark last available SN as next expected (Ignore historical data)
                                next_seq_nr_next    <= last_seq_nr when (first_seq_nr <= last_seq_nr) else first_seq_nr;
                                mem_op_start        <= '1';
                                mem_opcode          <= UPDATE_ENDPOINT;
                                tmp_flags           := tmp_flags or EMF_NEXT_SEQ_NR_FLAG or EMF_RES_TIME_FLAG;
                                -- NOTE: Only response with ACKNACK if SN is available (Or no Final Flag)
                                if (HEARTBEAT_RESPONSE_DELAY /= DURATION_INFINITE and (first_seq_nr <= last_seq_nr or final_flag = '0')) then
                                    res_time        <= time + HEARTBEAT_RESPONSE_DELAY;
                                    -- NOTE: Last Bit denotes if this is Response or Suppression Delay
                                    res_time(1)(0)  <= '0';
                                    tmp_dw          := (time + HEARTBEAT_RESPONSE_DELAY) when ((time + HEARTBEAT_RESPONSE_DELAY) < tmp_dw);
                                else
                                    res_time    <= TIME_INVALID;
                                end if;
                            -- If current Sequence Number obsolete (removed from source history cache)
                            elsif (first_seq_nr > mem_endpoint_data.next_seq_nr) then
                                -- Store new expected Sequence Number and set Response Dealy
                                next_seq_nr_next    <= first_seq_nr;
                                mem_op_start        <= '1';
                                mem_opcode          <= UPDATE_ENDPOINT;
                                tmp_flags           := tmp_flags or EMF_NEXT_SEQ_NR_FLAG or EMF_RES_TIME_FLAG;
                                -- NOTE: Only response with ACKNACK if SN is available (Or no Final Flag)
                                if (HEARTBEAT_RESPONSE_DELAY /= DURATION_INFINITE and (first_seq_nr <= last_seq_nr or final_flag = '0')) then
                                    res_time        <= time + HEARTBEAT_RESPONSE_DELAY;
                                    -- NOTE: Last Bit denotes if this is Response or Suppression Delay
                                    res_time(1)(0)  <= '0';
                                    tmp_dw          := (time + HEARTBEAT_RESPONSE_DELAY) when ((time + HEARTBEAT_RESPONSE_DELAY) < tmp_dw);
                                else
                                    res_time    <= TIME_INVALID;
                                end if;
                            -- If new Sequence Number is available or Writer expects ACKNACK
                            elsif (last_seq_nr >= mem_endpoint_data.next_seq_nr or final_flag = '0') then
                                -- Set Response Delay
                                mem_op_start    <= '1';
                                mem_opcode      <= UPDATE_ENDPOINT;
                                tmp_flags       := tmp_flags or EMF_RES_TIME_FLAG;
                                if (HEARTBEAT_RESPONSE_DELAY /= DURATION_INFINITE) then
                                    res_time        <= time + HEARTBEAT_RESPONSE_DELAY;
                                    -- NOTE: Last Bit denotes if this is Response or Suppression Delay
                                    res_time(1)(0)  <= '0';
                                    tmp_dw  := (time + HEARTBEAT_RESPONSE_DELAY) when ((time + HEARTBEAT_RESPONSE_DELAY) < tmp_dw);
                                else
                                    res_time        <= TIME_INVALID;
                                end if;
                            end if;
                        -- Currently in Heartbeat Response Delay
                        elsif (mem_endpoint_data.res_time(1)(0) = '0') then
                            -- If current Sequence Number obsolete (removed from source history cache)
                            if (first_seq_nr > mem_endpoint_data.next_seq_nr) then
                                -- NOTE: Even if first_seq_nr > last_seq_nr, meaning that the writer does not have any stored SNs
                                --       we don't cancel the response, because it may have been initiated by the Final Flag.
                                
                                -- Store new expected Sequence Number
                                next_seq_nr_next    <= first_seq_nr;
                                mem_op_start        <= '1';
                                mem_opcode          <= UPDATE_ENDPOINT;
                                tmp_flags           := tmp_flags or EMF_NEXT_SEQ_NR_FLAG;
                            end if;
                        end if;
                        
                        mem_field_flags <= tmp_flags;
                        
                        -- XXX: Possible Worst Case Path (MULTIPLE 64-bit addition and comparison in same clock)
                        -- Update Check Time
                        if (tmp_dw < check_time) then
                            check_time_next <= tmp_dw;
                        end if;
                    end if;
                end if;
            when LATCH_GAP =>
                -- Input FIFO Guard
                if (empty_user = '0') then
                    rd_guard    := '1';
                    cnt_next    <= cnt + 1;
                    
                    case (cnt) is
                        -- GapStart Sequence Number  1/2
                        when 0 =>
                            gap_start_next(0)       <= unsigned(data_in_user);
                        -- GapStart Sequence Number  2/2
                        when 1 =>
                            gap_start_next(1)       <= unsigned(data_in_user);
                        -- GapList.Base 1/2
                        when 2 =>
                            gap_list_base_next(0)   <= unsigned(data_in_user);
                        -- GapList.Base 2/2
                        when 3 =>
                            gap_list_base_next(1)   <= unsigned(data_in_user);
                        -- GapList.NumBits
                        when 4 =>
                            gap_list_end_next   <= gap_list_base + to_integer(unsigned(data_in_user));
                            bitmap_cnt_next     <= unsigned(round_slv(data_in_user(log2c(MAX_BITMAP_WIDTH)-1 downto 0),bitmap_cnt'length));
                            cnt2_next           <= 0;
                        -- GapList.Bitmap
                        when 5 =>
                            -- Read Bitmap
                            if (cnt2 < bitmap_cnt) then
                                cnt2_next <= cnt2 + 1;
                                
                                bitmap_latch_next(cnt2) <= data_in_user;
                                
                                -- Keep Sub-State
                                cnt_next    <= cnt;
                            else
                                stage_next  <= PROCESS_GAP;
                            end if;
                        when others =>
                            null;
                    end case;
                    
                end if;
            when PROCESS_GAP =>
                -- Wait for Endpoint Search
                if (mem_op_done = '1') then
                    -- DEFAULT
                    stage_next  <= SKIP_PACKET;
                    
                    -- Known Remote Endpoint
                    if (mem_addr_base /= ENDPOINT_MEMORY_MAX_ADDRESS) then
                        -- GAP is relevant
                        if (gap_start <= mem_endpoint_data.next_seq_nr and mem_endpoint_data.next_seq_nr <= gap_list_end) then
                            -- Next Expected is in GAP List
                            if (gap_list_base <= mem_endpoint_data.next_seq_nr) then
                                -- Begin searching next valid SN from the current expected in the bitmap list
                                bitmap_pos_next     <= to_integer(next_seq_nr - gap_list_base);
                            else
                                -- Begin searching next valid SN from the beginning of the bitmap list
                                next_seq_nr_next    <= gap_list_base;
                                bitmap_pos_next     <= 0;
                            end if;
                            stage_next  <= FIND_NEXT_VALID_IN_BITMAP;
                        end if;
                    end if;
                end if;
            when FIND_NEXT_VALID_IN_BITMAP =>
                -- Memory Operation Guard
                if (mem_op_done = '1') then
                    tmp_bitmap  := to_slv_bitmap(bitmap_latch);
                    
                    -- TODO: Test GAP that has next expected not marked in GAP
                    
                    -- First valid sequence number found
                    if (tmp_bitmap(bitmap_pos) = '0') then
                        -- Update next sequence number
                        mem_op_start    <= '1';
                        mem_opcode      <= UPDATE_ENDPOINT;
                        mem_field_flags <= EMF_NEXT_SEQ_NR_FLAG;
                        -- DONE
                        stage_next      <= SKIP_PACKET;
                    else
                        -- Continue search
                        bitmap_pos_next     <= bitmap_pos + 1;
                        next_seq_nr_next    <= next_seq_nr + 1;
                    end if;
                end if;
            when PROCESS_INLINE_QOS =>
                -- input FIFO Guard
                if (empty_user = '0') then
                    rd_guard            := '1';
                    -- Reset Word Counter
                    reset_read_cnt      <= '1';
                    -- Latch Parameter End (In order to skip parameters)
                    parameter_end_next  <= unsigned(endian_swap(endian_flag,parameter_length));
                    -- DEFAULT STAGE
                    stage_next          <= SKIP_PARAMETER;
                    
                    case (parameter_id) is
                        when PID_TOPIC_NAME =>
                            -- Ignore
                            null;
                        when PID_DURABILITY =>
                            -- Ignore
                            null;
                        when PID_PRESENTATION =>
                            -- Ignore
                            null;
                        when PID_DEADLINE =>
                            -- Ignore
                            null;
                        when PID_LATENCY_BUDGET =>
                            -- Ignore
                            null;
                        when PID_OWNERSHIP =>
                            -- Ignore
                            null;
                        when PID_OWNERSHIP_STRENGTH =>
                            -- Ignore
                            null;
                        when PID_LIVELINESS =>
                            -- Ignore
                            null;
                        when PID_PARTITION =>
                            -- Ignore
                            null;
                        when PID_RELIABILITY =>
                            -- Ignore
                            null;
                        when PID_DESTINATION_ORDER =>
                            -- Ignore
                            null;
                        when PID_TRANSPORT_PRIORITY =>
                            -- Ignore
                            null;
                        when PID_LIFESPAN =>
                            stage_next  <= LATCH_LIFESPAN;
                            cnt_next    <= 0;    
                        when PID_CONTENT_FILTER_INFO =>
                            -- Ignore
                            null;
                        when PID_COHERENT_SET =>
                            -- Ignore
                            null;
                        when PID_DIRECTED_WRITE =>
                            -- Ignore
                            null;
                        when PID_ORIGINAL_WRITER_INFO =>
                            -- Ignore
                            null;
                        when PID_GROUP_COHERENT_SET =>
                            -- Ignore
                            null;
                        when PID_GROUP_SEQ_NUM =>
                            -- Ignore
                            null;
                        when PID_WRITER_GROUP_INFO =>
                            -- Ignore
                            null;
                        when PID_SECURE_WRITER_GROUP_INFO =>
                            -- Ignore
                            null;
                        when PID_KEY_HASH =>
                            key_hash_rcvd_next  <= '1';
                            stage_next          <= LATCH_KEY_HASH;
                            cnt_next            <= 0;
                        when PID_STATUS_INFO =>
                            stage_next  <= LATCH_STATUS_INFO;
                        when PID_PAD =>
                            -- Ignore
                            null;
                        when PID_SENTINEL =>
                            -- Reset
                            parameter_end_next  <= (others => '1');
                            -- QOS DONE
                            stage_next  <= INITIATE_ADD_CACHE_CHANGE_REQUEST;
                        when others =>
                            -- If MUST_UNDERSTAND Flag is set, we have incompatible communication. Drop Packet
                            if (must_understand = '1') then
                                stage_next  <= SKIP_PACKET;
                            -- Else skip Unknown Parameter
                            else
                                stage_next  <= SKIP_PARAMETER;
                            end if;
                    end case;
                end if;
            when LATCH_LIFESPAN =>
                -- Input FIFO Guard
                if (empty_user = '0') then
                    rd_guard    := '1';
                    cnt_next    <= cnt + 1;
                    
                    case (cnt) is
                        -- Lifespan 1/2
                        when 0 =>
                            -- NOTE: We are misusing the sn_latch_1 as temporal CDR_LONG storage
                            sn_latch_1_next(0)  <= unsigned(data_in_swapped);
                        -- Lifespan 2/2
                        when 1 =>
                            tmp_dw  := (0 => sn_latch_1(0), 1 => unsigned(data_in_swapped));
                            
                            -- TODO: Use source timestamp if clocks with remote synchronized
                            -- Calculate Sample Lifespan Deadline
                            lifespan_next   <= (time + tmp_dw) when (tmp_dw /= DURATION_INFINITE) else TIME_INVALID;
                            
                            -- DONE
                            stage_next  <= SKIP_PARAMETER;
                        when others => 
                            null;
                    end case;
                end if;
            when LATCH_KEY_HASH =>
                -- Input FIFO Guard
                if (empty_user = '0') then
                    rd_guard    := '1';
                    cnt_next    <= cnt + 1;
                    
                    case (cnt) is
                        -- Key Hash 1/4
                        when 0 =>
                            key_hash_next(0)    <= data_in_user;
                        -- Key Hash 2/4
                        when 1 =>
                            key_hash_next(1)    <= data_in_user;
                        -- Key Hash 3/4
                        when 2 =>
                            key_hash_next(2)    <= data_in_user;
                        -- Key Hash 4/4
                        when 3 =>
                            key_hash_next(3)    <= data_in_user;
                            
                            -- DONE
                            stage_next  <= SKIP_PARAMETER;
                        when others => 
                            null;
                    end case;
                end if;
            when LATCH_STATUS_INFO =>
                -- Input FIFO Guard
                if (empty_user = '0') then
                    rd_guard            := '1';
                    
                    status_info_next    <= data_in_user;
                    
                    -- DONE
                    stage_next  <= SKIP_PARAMETER;
                end if;
            when INITIATE_ADD_CACHE_CHANGE_REQUEST =>
                -- Wait for Endpoint Data
                if (mem_op_done = '1') then
                    -- Unknown Endpoint
                    if (mem_addr_base = ENDPOINT_MEMORY_MAX_ADDRESS) then
                        -- Ignore
                        stage_next  <= SKIP_PACKET;
                    else
                        -- Data is Next expected Sequence Number
                        if ((RELIABILTY_QOS = RELIABLE_RELIABILITY_QOS and seq_nr = mem_endpoint_data.next_seq_nr) or (RELIABILTY_QOS = BEST_EFFORT_RELIABILITY_QOS and seq_nr >= mem_endpoint_data.next_seq_nr) or (DURABILITY_QOS = VOLATILE_DURABILITY_QOS and mem_endpoint_data.next_seq_nr = SEQUENCENUMBER_UNKNOWN)) then
                            -- SANITY CHECK: Skip if no Hash Key and no Payload
                            if (WITH_KEY and key_hash_rcvd = '0' and data_flag = '0' and key_flag = '0') then
                                -- Ignore
                                stage_next  <= SKIP_PACKET;
                            else
                                start_hc    <= '1';
                                opcode_hc   <= ADD_CACHE_CHANGE;
                                -- Wait until History Cache acknowledges request
                                if (ack_hc = '1') then
                                    stage_next  <= ADD_CACHE_CHANGE;
                                    cnt_next    <= 0;
                                end if;
                            end if;
                        else
                            -- Ignore
                            stage_next  <= SKIP_PACKET;
                        end if;
                    end if;
                end if;
            when ADD_CACHE_CHANGE =>
                case (cnt) is
                    -- Status Info
                    when 0 =>
                        valid_out_hc    <= '1';
                        data_out_hc                    <= status_info;
                        data_out_hc(SSI_KEY_HASH_FLAG) <= key_hash_rcvd when WITH_KEY;
                        data_out_hc(SSI_PAYLOAD_FLAG)  <= data_flag;
                        -- Output Guard
                        if (ready_out_hc = '1') then
                            cnt_next    <= cnt + 1;
                        end if;
                    -- Timestamp 1/2
                    when 1 =>
                        valid_out_hc   <= '1';
                        data_out_hc    <= std_logic_vector(ts(0));
                        -- Output Guard
                        if (ready_out_hc = '1') then
                            cnt_next    <= cnt + 1;
                        end if;
                    -- Timestamp 2/2
                    when 2 =>
                        valid_out_hc   <= '1';
                        data_out_hc    <= std_logic_vector(ts(1));
                        -- Output Guard
                        if (ready_out_hc = '1') then
                            cnt_next    <= cnt + 1;
                        end if;
                    -- Lifespan Deadline 1/2
                    when 3 =>
                        valid_out_hc   <= '1';
                        data_out_hc    <= std_logic_vector(lifespan(0));
                        -- Output Guard
                        if (ready_out_hc = '1') then
                            cnt_next    <= cnt + 1;
                        end if;
                    -- Lifespan Deadline 2/2
                    when 4 =>
                        valid_out_hc   <= '1';
                        data_out_hc    <= std_logic_vector(lifespan(1));
                        -- Output Guard
                        if (ready_out_hc = '1') then
                            -- Skip Key Hash, if not received
                            if (not WITH_KEY or key_hash_rcvd = '0') then
                                cnt_next    <= cnt + 5;
                            else
                                cnt_next    <= cnt + 1;
                            end if;
                        end if;
                    -- Key hash 1/4
                    when 5 =>
                        valid_out_hc   <= '1';
                        data_out_hc    <= key_hash(0);
                        -- Output Guard
                        if (ready_out_hc = '1') then
                            cnt_next    <= cnt + 1;
                        end if;
                    -- Key Hash 2/4
                    when 6 =>
                        valid_out_hc   <= '1';
                        data_out_hc    <= key_hash(1);
                        -- Output Guard
                        if (ready_out_hc = '1') then
                            cnt_next    <= cnt + 1;
                        end if;
                    -- Key Hash 3/4
                    when 7 =>
                        valid_out_hc   <= '1';
                        data_out_hc    <= key_hash(2);
                        -- Output Guard
                        if (ready_out_hc = '1') then
                            cnt_next    <= cnt + 1;
                        end if;
                    -- Key hash 4/4
                    when 8 =>
                        valid_out_hc   <= '1';
                        data_out_hc    <= key_hash(3);
                        -- Output Guard
                        if (ready_out_hc = '1') then
                            cnt_next    <= cnt + 1;
                        end if;
                    -- Endpoint Memory Position
                    when 9 =>
                        -- Wait for Endpoint Search
                        if (mem_op_done = '1') then
                            valid_out_hc   <= '1';
                            -- TODO: Assert mem_pos range fits in CDR_LONG
                            data_out_hc    <= std_logic_vector(to_unsigned(mem_pos, CDR_LONG_WIDTH));
                            
                            -- Output Guard
                            if (ready_out_hc = '1') then
                                -- NOTE: We only push the Payload if there is Data, or if the Key Hash was not received and we need to push
                                --       the serialized Key.
                                -- Payload exists
                                if (data_flag = '1' or (WITH_KEY and key_hash_rcvd = '0')) then
                                    stage_next      <= PUSH_PAYLOAD;
                                else
                                    -- DONE
                                    last_word_out_hc    <= '1';
                                    stage_next          <= FINALIZE_ADD_CACHE_CHANGE_REQUEST;
                                end if;
                            end if;
                        end if;
                    when others =>
                        null;
                end case;
            when PUSH_PAYLOAD =>
                -- Input Guard
                if (empty_user = '0') then
                    valid_out_hc    <= '1';
                    -- Push Payload to History Cache
                    data_out_hc        <= data_in_user;
                    last_word_out_hc   <= last_word_in_user;
                    
                    -- Output Guard
                    if (ready_out_hc = '1') then
                        rd_guard    := '1';
                        
                        -- Exit Condition
                        if (last_word_in_user = '1') then
                            stage_next          <= FINALIZE_ADD_CACHE_CHANGE_REQUEST;
                        end if;
                    end if;
                end if;
            when FINALIZE_ADD_CACHE_CHANGE_REQUEST =>
                -- NOTE: Memory is already in done state from previous state (ADD_CACHE_CHANGE)
                assert (mem_op_done = '1') report "FINALIZE_ADD_CACHE_CHANGE_REQUEST precondition not met. mem_op_done /= '1'" severity FAILURE;
                -- Wait for History Cache Response
                if (done_hc = '1') then
                    
                    -- NOTE: The Lease Duration is also updated if the Cache Change is not accepted. This in effect "skews" the
                    --       "correctness" of the Writer Liveliness Protocol until the reader has no pending request from the Writer.
                    
                    -- Update Endpoint
                    mem_op_start    <= '1';
                    mem_opcode      <= UPDATE_ENDPOINT;
                    mem_field_flags <= EMF_LEASE_DEADLINE_FLAG;
                    if (LEASE_DURATION /= DURATION_INFINITE) then
                        lease_deadline  <= time + LEASE_DURATION;
                        
                        -- XXX: Possible Worst Case Path (64-bit addition and comparison in same clock)
                        -- Update Check Time
                        if ((time + LEASE_DURATION) < check_time) then
                            check_time_next <= time + LEASE_DURATION;
                        end if;
                    else
                        lease_deadline  <= TIME_INVALID;
                    end if;
                    
                    -- NOTE: In case the operation was unsucessfull (e.g. reached Resource Limits), the Sequence Number is not updated
                    --       and thus not "acknowledged".
                    -- Operation was Accepted
                    if (ret_hc = OK) then
                        -- Update also next sequence number
                        mem_field_flags <= EMF_NEXT_SEQ_NR_FLAG or EMF_LEASE_DEADLINE_FLAG;
                    end if;
                    
                    -- DONE
                    stage_next  <= SKIP_PACKET;
                end if;
            when ENDPOINT_STALE_CHECK =>
                -- Memory Operation Guard
                if (mem_op_done = '1') then
                    case (cnt) is
                        -- Get Next Endpoint
                        when 0 =>
                            mem_op_start    <= '1';
                            mem_opcode      <= GET_NEXT_ENDPOINT;
                            mem_field_flags <= EMF_LEASE_DEADLINE_FLAG or EMF_RES_TIME_FLAG;
                            cnt_next        <= 1;
                        -- Check Endpoint
                        when 1 =>
                            -- End of Endpoints
                            if (mem_addr_base = ENDPOINT_MEMORY_MAX_ADDRESS) then
                                -- Reset
                                stale_check_next    <= '0';
                                -- DONE
                                stage_next          <= IDLE;
                            else
                                -- Endpoint Lease Expired
                                if (mem_endpoint_data.lease_deadline /= TIME_INVALID and mem_endpoint_data.lease_deadline <= time) then
                                    -- Propagate Removal
                                    start_hc       <= '1';
                                    opcode_hc      <= REMOVE_WRITER;
                                    data_out_hc    <= std_logic_vector(to_unsigned(mem_pos, WORD_WIDTH));
                                    -- Wait for Operation Acknowledgement
                                    if (ack_hc = '1') then
                                        -- Remove Endpoint
                                        mem_op_start    <= '1';
                                        mem_opcode      <= REMOVE_ENDPOINT;
                                        -- Continue Search
                                        cnt_next    <= 0;
                                    end if;
                                -- Synthesis Guard/Response Time Reached
                                elsif (RELIABILTY_QOS = RELIABLE_RELIABILITY_QOS and mem_endpoint_data.res_time /= TIME_INVALID and mem_endpoint_data.res_time <= time) then
                                    -- If Suppression Delay passed, zero the time
                                    if(mem_endpoint_data.res_time(1)(0) = '1') then
                                        -- Disable Suppression
                                        mem_op_start    <= '1';
                                        mem_opcode      <= UPDATE_ENDPOINT;
                                        res_time        <= TIME_INVALID;
                                        mem_field_flags <= EMF_RES_TIME_FLAG;
                                        -- Continue Search
                                        cnt_next    <= 0;
                                    -- If Response Delay Passed
                                    else
                                        -- Get Additional Data
                                        mem_op_start    <= '1';
                                        mem_opcode      <= GET_ENDPOINT;
                                        mem_field_flags <= EMF_ENTITYID_FLAG or EMF_IPV4_ADDR_FLAG or EMF_UDP_PORT_FLAG or EMF_NEXT_SEQ_NR_FLAG;
                                        cnt_next        <= 2;
                                    end if;
                                    
                                    -- Update Check Time
                                    if (mem_endpoint_data.lease_deadline /= TIME_INVALID and mem_endpoint_data.lease_deadline < check_time) then
                                        check_time_next <= mem_endpoint_data.lease_deadline;
                                    end if;
                                else
                                    -- Update Check Time
                                    if (RELIABILTY_QOS = RELIABLE_RELIABILITY_QOS and mem_endpoint_data.res_time /= TIME_INVALID and mem_endpoint_data.lease_deadline /= TIME_INVALID and mem_endpoint_data.res_time < mem_endpoint_data.lease_deadline) then
                                        if (mem_endpoint_data.res_time < check_time) then
                                            check_time_next <= mem_endpoint_data.res_time;
                                        end if;
                                    elsif (mem_endpoint_data.lease_deadline /= TIME_INVALID) then
                                        if (mem_endpoint_data.lease_deadline < check_time) then
                                            check_time_next <= mem_endpoint_data.lease_deadline;
                                        end if;
                                    elsif (RELIABILTY_QOS = RELIABLE_RELIABILITY_QOS and mem_endpoint_data.res_time /= TIME_INVALID) then
                                        if (mem_endpoint_data.res_time < check_time) then
                                            check_time_next <= mem_endpoint_data.res_time;
                                        end if;
                                    end if;
                                    
                                    -- Continue Search
                                    cnt_next    <= 0;
                                end if;
                            end if;
                        when 2 =>
                            -- Synthesis Guard
                            if (RELIABILTY_QOS = RELIABLE_RELIABILITY_QOS) then
                                -- Set Heartbeat Suppression Time
                                if (HEARTBEAT_SUPPRESSION_DELAY /= DURATION_INFINITE and HEARTBEAT_SUPPRESSION_DELAY /= DURATION_ZERO) then
                                    -- Set Heartbeat Suppression Time
                                    res_time        <= time + HEARTBEAT_SUPPRESSION_DELAY;
                                    -- NOTE: Last Bit denotes if this is Response or Suppression Delay
                                    res_time(1)(0)  <= '1';
                                    
                                    -- XXX: Possible Worst Case Path (64-bit addition and comparison in same clock)
                                    -- Update Check Time
                                    if ((time + HEARTBEAT_SUPPRESSION_DELAY) < check_time) then
                                        -- NOTE: Strictly speaking the check time may not EXACTLY represent the res_time (Since the last bit is ovverriden for a different functionality)
                                        --       Since the difference is at most 1/10^(-32) seconds, this will never practically impose any problem (since the time will most certainly have
                                        --       progressed beyond that difference from the check_time trigger to the actual res_time check).
                                        --       Nevertheless this has to be taken into account in testbenches that use "static" time.
                                        check_time_next <= time + HEARTBEAT_SUPPRESSION_DELAY;
                                    end if;
                                else
                                    -- Disable Suppression
                                    res_time    <= TIME_INVALID;
                                end if;
                                mem_op_start    <= '1';
                                mem_opcode      <= UPDATE_ENDPOINT;
                                mem_field_flags <= EMF_RES_TIME_FLAG;
                                
                                -- Send ACKNACK
                                -- Increment Acknack Counter
                                count_next          <= count + 1;
                                stage_next          <= SEND_HEADER;
                                return_stage_next   <= SEND_ACKNACK;
                                cnt_next            <= 0;
                            end if;
                        when others =>
                            null;
                    end case;
                end if;
            when SEND_HEADER =>
                -- Synthesis Guard
                if (RELIABILTY_QOS = RELIABLE_RELIABILITY_QOS) then
                    -- Output FIFO Guard
                    if (full_rtps = '0') then
                        wr_rtps     <= '1';
                        cnt_next    <= cnt + 1;
                        
                        case (cnt) is
                            -- OUTPUT HEADER
                            -- Src IPv4 Address
                            when 0 =>
                                data_out_rtps  <= DEFAULT_IPv4_ADDRESS;
                            -- Dest IPv4 Address
                            when 1 =>
                                data_out_rtps  <= mem_endpoint_data.addr;
                            -- Src and Dest UDPv4 Ports
                            when 2 =>
                                data_out_rtps  <= USER_IPv4_UNICAST_PORT & mem_endpoint_data.portn;
                            -- RTPS MESSAGE HEADER
                            when 3 =>
                                data_out_rtps  <= PROTOCOL_RTPS;
                            when 4 =>
                                data_out_rtps  <= PROTOCOLVERSION_2_4 & VENDORID;
                            when 5 =>
                                data_out_rtps  <= GUIDPREFIX(0);
                            when 6 =>
                                data_out_rtps  <= GUIDPREFIX(1);
                            when 7 =>
                                data_out_rtps  <= GUIDPREFIX(2);
                                -- Continue with respective RTPS Submessage
                                stage_next  <= return_stage;
                                cnt_next    <= 0;
                            when others =>
                                null;
                        end case;
                    end if;
                end if;
            when SEND_ACKNACK =>
                -- Synthesis Guard
                if (RELIABILTY_QOS = RELIABLE_RELIABILITY_QOS) then
                    -- Output FIFO Guard
                    if (full_rtps = '0') then
                        wr_rtps     <= '1';
                        cnt_next    <= cnt + 1;
                        
                        case (cnt) is
                            -- ACKNACK RTPS SUBMESSAGE
                            -- RTPS Submessage Header
                            when 0 =>
                                data_out_rtps  <= SID_ACKNACK & "00000010" & std_logic_vector(to_unsigned(28, SUBMESSAGE_LENGTH_WIDTH));
                            -- Reader Entity ID
                            when 1 =>
                                data_out_rtps  <= ENTITYID;
                            -- Writer Entity ID
                            when 2 =>
                                data_out_rtps  <= mem_endpoint_data.guid(3);
                            -- Sequence Number Set (Bitmap Base 1/2)
                            when 3 =>
                                data_out_rtps  <= std_logic_vector(mem_endpoint_data.next_seq_nr(0));
                            -- Sequence Number Set (Bitmap Base 2/2)
                            when 4 =>
                                data_out_rtps  <= std_logic_vector(mem_endpoint_data.next_seq_nr(1));
                            -- Sequence Number Set (NumBits)
                            when 5 =>
                                data_out_rtps  <= std_logic_vector(to_unsigned(CDR_LONG_WIDTH, CDR_LONG_WIDTH));
                            -- Sequence Number Set (Bitmap)
                            when 6 =>
                                -- NOTE: In order to avoid having to generate a variable sized bitmap, we always request the next 32 sequence numbers, even if they do not exist (yet)
                                -- XXX: Assumes correct implementation of the RTPS Protocol (i.e. Writer ignores requested SNs that do not exist)
                                data_out_rtps  <= (others => '1');
                            -- Count
                            when 7 =>
                                data_out_rtps        <= std_logic_vector(count);
                                last_word_out_rtps   <= '1';
                                
                                -- Stale Check in Progress
                                if (stale_check = '1') then
                                    -- Continue Search
                                    stage_next  <= ENDPOINT_STALE_CHECK;
                                    cnt_next    <= 0;
                                else
                                    -- DONE
                                    stage_next  <= IDLE;
                                end if;
                            when others =>
                                null;
                        end case;
                    end if;
                end if;
            when SKIP_PARAMETER =>
                -- Consumed last word of Packet
                if (last_word_in_latch = '1' and last_word_in_user = '0') then
                    -- Reset Last Word In Latch
                    last_word_in_latch_next <= '0';
                    -- Continue parsing next Packet
                    stage_next              <= IDLE;
                    -- Reset Parameter End
                    parameter_end_next      <= (others => '1');
                -- End of Parameter
                elsif ((read_cnt & "00" ) >= parameter_end) then
                    -- Parse Next Parameter
                    -- NOTE: data_in_user is already showing the next parameter
                    stage_next          <= PROCESS_INLINE_QOS;
                    -- Reset Parameter End
                    parameter_end_next  <= (others => '1');
                -- Input FIFO Guard
                elsif (empty_user = '0') then
                    -- Skip-Read
                    rd_guard    := '1';
                end if;
            when SKIP_PACKET =>
                -- NOTE: At the end of a Stale Entry Removal this stage is entered, without having started reading a Packet from input.
                -- Reset Parameter End
                parameter_end_next  <= (others => '1');
                
                -- Consumed last word of Packet
                if (last_word_in_latch = '1' and last_word_in_user = '0') then
                    -- Reset Last Word In Latch
                    last_word_in_latch_next <= '0';
                    -- DONE
                    stage_next              <= IDLE;
                -- Input FIFO Guard
                elsif (empty_user = '0') then
                    -- Skip-Read
                    rd_guard    := '1';
                end if;
            when SKIP_META_OPERATION =>
                -- Input Guard
                if (empty_meta = '0') then
                    -- Skip-Read
                    rd_meta <= '1';
                    
                    -- Exit Condition
                    if (last_word_in_meta = '1') then
                        stage_next  <= IDLE;
                    end if;
                end if;
            when others =>
                null;
        end case;
        
        -- OVERREAD GUARD
        -- Read outside of packet Length
        -- NOTE: If the Packet Length is smaller than expected there will be a read from input FIFO while
        --       the Packet Length has been reached and will be caught by this clause.
        --       The SKIP_PACKET clause prevents a read signal from occuring in this situation, and thus prevents from entering this state.
        if ((last_word_in_latch = '1' and last_word_in_user = '0') and rd_guard = '1') then
            -- Force rd_sig low
            rd_sig                  <= '0';
            -- Continue parsing next Packet
            stage_next              <= IDLE;
            -- Reset Last Word In Latch
            last_word_in_latch_next <= '0';
            -- Reset Parameter End
            parameter_end_next      <= (others => '1');
        -- Read outside of Parameter Length
        -- NOTE: If the Parameter Length is smaller than expected for a particular parameter, there will be a read from input FIFO while
        --       the Parameter Length has been reached and will be caught by this clause.
        --       The SKIP_PARAMETER clause prevents a read signal from occuring in this situation, and thus prevents from entering this state.
        elsif ((read_cnt & "00") >= parameter_end and rd_guard = '1') then
            -- Force rd_sig low
            rd_sig              <= '0';
            -- Invalid Parameter Length, Skip Packet
            stage_next          <= SKIP_PACKET;
            -- Reset Parameter End
            parameter_end_next  <= (others => '1');
        -- DEFAULT
        else
            rd_sig  <= rd_guard;
        end if;
    end process;
    
    -- *Memory State Machine*
    -- STATE                DESCRIPTION
    -- IDLE                 Idle State. Done Signal is pulled high and Memory FSM accepts new memory operations
    -- SEARCH_ENDPOINT      See Memory OPCODE Description
    -- GET_ENDPOINT_DATA    Latch specified Endpoint Data for use by main FSM
    -- INSERT_ENDPOINT      See Memory OPCODE Description
    -- UPDATE_ENDPOINT      See Memory OPCODE Description
    -- REMOVE_ENDPOINT      See Memory OPCODE Description
    -- FIND_EMPTY_SLOT      Find first empty_user slot in memory.
    -- RESET_MAX_POINTER    Reset the max_endpoint_addr pointer to last occupied slot in memory.
    -- GET_NEXT_ENDPOINT    See Memory OPCODE Description
    -- RESET_MEMORY         Reset Endpoint Memory to Empty State
    mem_ctrl_prc : process(all)
    begin
        -- DEFAULT Registered
        mem_stage_next                  <= mem_stage;
        mem_addr_base_next              <= mem_addr_base;
        mem_cnt_next                    <= mem_cnt;
        last_addr_next                  <= last_addr;
        mem_addr_latch_next             <= mem_addr_latch;
        mem_endpoint_data_next          <= mem_endpoint_data;
        max_endpoint_addr_next          <= max_endpoint_addr;
        mem_endpoint_latch_data_next    <= mem_endpoint_latch_data;
        mem_pos_next                    <= mem_pos;
        -- DEFAULT Unregistered
        mem_addr        <= (others => '0');
        mem_write_data  <= (others => '0');
        mem_read        <= '0';
        mem_valid_in    <= '0';
        mem_ready_out   <= '0';
        mem_op_done     <= '0';
        abort_read      <= '0';
        
        
        case (mem_stage) is
            when IDLE =>
                mem_op_done             <= '1';
                
                if (mem_op_start = '1') then
                    -- Latch Signals needed for Mermory Operation (Use _next signals, because some signals are set in same clk)
                    mem_endpoint_latch_data_next      <= (
                        guid                => guid_next,
                        addr                => addr_next,
                        portn               => portn_next,
                        lease_deadline      => lease_deadline,
                        res_time            => res_time,
                        next_seq_nr         => next_seq_nr_next,
                        field_flag          => mem_field_flags
                    );
                    
                    case(mem_opcode) is
                        when SEARCH_ENDPOINT =>
                            mem_addr_base_next  <= FIRST_ENDPOINT_ADDRESS;
                            mem_pos_next        <= 0;
                            mem_stage_next      <= SEARCH_ENDPOINT;
                            mem_cnt_next        <= 0;
                        when INSERT_ENDPOINT =>
                            mem_addr_base_next  <= FIRST_ENDPOINT_ADDRESS;
                            mem_pos_next        <= 0;
                            mem_stage_next      <= FIND_EMPTY_SLOT;
                            mem_cnt_next        <= 0;
                            -- Set Endpoint Data
                            mem_endpoint_data_next                  <= ZERO_ENDPOINT_DATA;
                            mem_endpoint_data_next.guid             <= guid_next;
                            mem_endpoint_data_next.addr             <= addr_next;
                            mem_endpoint_data_next.portn            <= portn_next;
                            mem_endpoint_data_next.next_seq_nr      <= SEQUENCENUMBER_UNKNOWN when (DURABILITY_QOS = VOLATILE_DURABILITY_QOS) else FIRST_SEQUENCENUMBER;
                            mem_endpoint_data_next.lease_deadline   <= lease_deadline;
                            mem_endpoint_data_next.res_time         <= TIME_INVALID;
                        when UPDATE_ENDPOINT =>
                            mem_stage_next  <= UPDATE_ENDPOINT;
                            if (RELIABILTY_QOS = RELIABLE_RELIABILITY_QOS and check_mask(mem_field_flags,EMF_IPV4_ADDR_FLAG)) then
                                mem_cnt_next    <= 0;
                            elsif (RELIABILTY_QOS = RELIABLE_RELIABILITY_QOS and check_mask(mem_field_flags,EMF_UDP_PORT_FLAG)) then
                                mem_cnt_next    <= 1;
                            elsif check_mask(mem_field_flags,EMF_NEXT_SEQ_NR_FLAG) then
                                mem_cnt_next    <= 2;
                            elsif check_mask(mem_field_flags,EMF_LEASE_DEADLINE_FLAG) then
                                mem_cnt_next    <= 4;
                            elsif (RELIABILTY_QOS = RELIABLE_RELIABILITY_QOS and check_mask(mem_field_flags,EMF_RES_TIME_FLAG)) then
                                mem_cnt_next    <= 6;
                            else
                                -- DONE
                                mem_stage_next  <= IDLE;
                            end if;
                        when REMOVE_ENDPOINT =>
                            mem_stage_next      <= REMOVE_ENDPOINT;
                            mem_cnt_next        <= 0;
                        when GET_FIRST_ENDPOINT =>
                            mem_addr_base_next  <= FIRST_ENDPOINT_ADDRESS;
                            mem_pos_next        <= 0;
                            mem_stage_next      <= GET_NEXT_ENDPOINT;
                            mem_cnt_next        <= 0;
                        when GET_NEXT_ENDPOINT =>
                            -- Memory Bound Guard
                            if (mem_addr_base >= max_endpoint_addr) then
                                mem_addr_base_next  <= ENDPOINT_MEMORY_MAX_ADDRESS;
                            else
                                -- Reached End of Memory, No match
                                mem_addr_base_next  <= mem_addr_base + ENDPOINT_FRAME_SIZE;
                                mem_pos_next        <= mem_pos + 1;
                                mem_stage_next      <= GET_NEXT_ENDPOINT;
                                mem_cnt_next        <= 0;
                            end if;
                        when GET_ENDPOINT =>
                            -- Fetch Endpoint Data
                            mem_stage_next          <= GET_ENDPOINT_DATA;
                            mem_endpoint_data_next  <= ZERO_ENDPOINT_DATA;
                            if check_mask(mem_field_flags,EMF_ENTITYID_FLAG) then
                                mem_cnt_next    <= 0;
                            elsif check_mask(mem_field_flags,EMF_GUIDPREFIX_FLAG) then
                                mem_cnt_next    <= 1;
                            elsif (RELIABILTY_QOS = RELIABLE_RELIABILITY_QOS and check_mask(mem_field_flags,EMF_IPV4_ADDR_FLAG)) then
                                mem_cnt_next    <= 4;
                            elsif (RELIABILTY_QOS = RELIABLE_RELIABILITY_QOS and check_mask(mem_field_flags,EMF_UDP_PORT_FLAG)) then
                                mem_cnt_next    <= 5;
                            elsif check_mask(mem_field_flags,EMF_NEXT_SEQ_NR_FLAG) then
                                mem_cnt_next    <= 6;
                            elsif check_mask(mem_field_flags,EMF_LEASE_DEADLINE_FLAG) then
                                mem_cnt_next    <= 8;
                            elsif (RELIABILTY_QOS = RELIABLE_RELIABILITY_QOS and check_mask(mem_field_flags,EMF_RES_TIME_FLAG)) then
                                mem_cnt_next    <= 10;
                            else
                                -- DONE
                                mem_stage_next  <= IDLE;
                            end if;
                        when others =>
                            null;
                    end case;
                end if;
            when SEARCH_ENDPOINT =>
                case (mem_cnt) is
                    -- GET Entity ID
                    when 0 =>
                        mem_addr        <= mem_addr_base + EMF_ENTITYID_OFFSET;
                        mem_read        <= '1';
                        mem_valid_in    <= '1';
                        -- Memory Flow Control Guard
                        if (mem_ready_in = '1') then
                            mem_cnt_next    <= mem_cnt + 1;
                        end if;
                    -- GET GUID Prefix 1/3
                    when 1 =>
                        mem_addr        <= mem_addr_base + EMF_GUIDPREFIX_OFFSET;
                        mem_read        <= '1';
                        mem_valid_in    <= '1';
                        -- Memory Flow Control Guard
                        if (mem_ready_in = '1') then
                            mem_cnt_next    <= mem_cnt + 1;
                        end if;
                    -- GET GUID Prefix 2/3
                    when 2 =>
                        mem_addr        <= mem_addr_base + EMF_GUIDPREFIX_OFFSET + 1;
                        mem_read        <= '1';
                        mem_valid_in    <= '1';
                        -- Memory Flow Control Guard
                        if (mem_ready_in = '1') then
                            mem_cnt_next    <= mem_cnt + 1;
                        end if;
                    -- GET GUID Prefix 3/3
                    when 3 =>
                        mem_addr        <= mem_addr_base + EMF_GUIDPREFIX_OFFSET + 2;
                        mem_read        <= '1';
                        mem_valid_in    <= '1';
                        -- Memory Flow Control Guard
                        if (mem_ready_in = '1') then
                            mem_cnt_next    <= mem_cnt + 1;
                        end if;
                    -- READ Entity ID
                    when 4 =>
                        mem_ready_out   <= '1';
                        -- Memory Flow Control Guard
                        if (mem_valid_out = '1') then
                            -- No Match
                            if (mem_read_data /= mem_endpoint_latch_data.guid(3)) then
                                abort_read  <= '1';
                                -- Reached End of Memory, No Match
                                if (mem_addr_base = max_endpoint_addr) then
                                    mem_addr_base_next  <= ENDPOINT_MEMORY_MAX_ADDRESS; --No match
                                    -- DONE
                                    mem_stage_next      <= IDLE;
                                else
                                    -- Continue Search
                                    mem_addr_base_next  <= mem_addr_base + ENDPOINT_FRAME_SIZE;
                                    mem_pos_next        <= mem_pos + 1;
                                    mem_cnt_next        <= 0;
                                end if;
                            else
                                mem_cnt_next    <= mem_cnt + 1;
                            end if;
                        end if;
                    -- READ GUID Prefix 1/3
                    when 5 =>
                        mem_ready_out   <= '1';
                        -- Memory Flow Control Guard
                        if (mem_valid_out = '1') then
                            -- No  Match
                            if (mem_read_data /= mem_endpoint_latch_data.guid(0)) then
                                abort_read  <= '1';
                                -- Reached End of Memory, No Match
                                if (mem_addr_base = max_endpoint_addr) then
                                    mem_addr_base_next  <= ENDPOINT_MEMORY_MAX_ADDRESS; --No match
                                    -- DONE
                                    mem_stage_next      <= IDLE;
                                else
                                    -- Continue Search
                                    mem_addr_base_next  <= mem_addr_base + ENDPOINT_FRAME_SIZE;
                                    mem_pos_next        <= mem_pos + 1;
                                    mem_cnt_next        <= 0;
                                end if;
                            else
                                mem_cnt_next    <= mem_cnt + 1;
                            end if;
                        end if;
                    -- READ GUID Prefix 2/3
                    when 6 =>
                        mem_ready_out   <= '1';
                        -- Memory Flow Control Guard
                        if (mem_valid_out = '1') then
                            -- No  Match
                            if (mem_read_data /= mem_endpoint_latch_data.guid(1)) then
                                abort_read  <= '1';
                                -- Reached End of Memory, No Match
                                if (mem_addr_base = max_endpoint_addr) then
                                    mem_addr_base_next  <= ENDPOINT_MEMORY_MAX_ADDRESS; --No match
                                    -- DONE
                                    mem_stage_next      <= IDLE;
                                else
                                    -- Continue Search
                                    mem_addr_base_next  <= mem_addr_base + ENDPOINT_FRAME_SIZE;
                                    mem_pos_next        <= mem_pos + 1;
                                    mem_cnt_next        <= 0;
                                end if;
                            else
                                mem_cnt_next    <= mem_cnt + 1;
                            end if;
                        end if;
                    -- READ GUID Prefix 3/3
                    when 7 =>
                        mem_ready_out   <= '1';
                        -- Memory Flow Control Guard
                        if (mem_valid_out = '1') then
                            -- No  Match
                            if (mem_read_data /= mem_endpoint_latch_data.guid(2)) then
                                abort_read  <= '1';
                                -- Reached End of Memory, No Match
                                if (mem_addr_base = max_endpoint_addr) then
                                    mem_addr_base_next  <= ENDPOINT_MEMORY_MAX_ADDRESS; --No match
                                    -- DONE
                                    mem_stage_next      <= IDLE;
                                else
                                    -- Continue Search
                                    mem_addr_base_next  <= mem_addr_base + ENDPOINT_FRAME_SIZE;
                                    mem_pos_next        <= mem_pos + 1;
                                    mem_cnt_next        <= 0;
                                end if;
                            -- Match
                            else
                                -- Fetch Endpoint Data
                                mem_stage_next          <= GET_ENDPOINT_DATA;
                                mem_endpoint_data_next  <= ZERO_ENDPOINT_DATA;
                                if check_mask(mem_endpoint_latch_data.field_flag,EMF_ENTITYID_FLAG) then
                                    mem_cnt_next    <= 0;
                                elsif check_mask(mem_endpoint_latch_data.field_flag,EMF_GUIDPREFIX_FLAG) then
                                    mem_cnt_next    <= 1;
                                elsif (RELIABILTY_QOS = RELIABLE_RELIABILITY_QOS and check_mask(mem_endpoint_latch_data.field_flag,EMF_IPV4_ADDR_FLAG)) then
                                    mem_cnt_next    <= 4;
                                elsif (RELIABILTY_QOS = RELIABLE_RELIABILITY_QOS and check_mask(mem_endpoint_latch_data.field_flag,EMF_UDP_PORT_FLAG)) then
                                    mem_cnt_next    <= 5;
                                elsif check_mask(mem_endpoint_latch_data.field_flag,EMF_NEXT_SEQ_NR_FLAG) then
                                    mem_cnt_next    <= 6;
                                elsif check_mask(mem_endpoint_latch_data.field_flag,EMF_LEASE_DEADLINE_FLAG) then
                                    mem_cnt_next    <= 8;
                                elsif (RELIABILTY_QOS = RELIABLE_RELIABILITY_QOS and check_mask(mem_endpoint_latch_data.field_flag,EMF_RES_TIME_FLAG)) then
                                    mem_cnt_next    <= 10;
                                else
                                    -- DONE
                                    mem_stage_next  <= IDLE;
                                end if;
                            end if;
                        end if;
                    when others =>
                        null;
                end case;
            when GET_ENDPOINT_DATA =>
                
                case (mem_cnt) is
                    -- GET Entity ID
                    when 0 =>
                        mem_valid_in    <= '1';
                        mem_addr        <= mem_addr_base + EMF_ENTITYID_OFFSET;
                        mem_read        <= '1';
                        -- Memory Flow Control Guard
                        if (mem_ready_in = '1') then
                            if check_mask(mem_endpoint_latch_data.field_flag,EMF_GUIDPREFIX_FLAG) then
                                mem_cnt_next    <= 1;
                            elsif (RELIABILTY_QOS = RELIABLE_RELIABILITY_QOS and check_mask(mem_endpoint_latch_data.field_flag,EMF_IPV4_ADDR_FLAG)) then
                                mem_cnt_next    <= 4;
                            elsif (RELIABILTY_QOS = RELIABLE_RELIABILITY_QOS and check_mask(mem_endpoint_latch_data.field_flag,EMF_UDP_PORT_FLAG)) then
                                mem_cnt_next    <= 5;
                            elsif check_mask(mem_endpoint_latch_data.field_flag,EMF_NEXT_SEQ_NR_FLAG) then
                                mem_cnt_next    <= 6;
                            elsif check_mask(mem_endpoint_latch_data.field_flag,EMF_LEASE_DEADLINE_FLAG) then
                                mem_cnt_next    <= 8;
                            elsif (RELIABILTY_QOS = RELIABLE_RELIABILITY_QOS and check_mask(mem_endpoint_latch_data.field_flag,EMF_RES_TIME_FLAG)) then
                                mem_cnt_next    <= 10;
                            else
                                mem_cnt_next    <= 12;
                            end if;
                        end if;
                    -- GET GUID Prefix 1/3
                    when 1 =>
                        mem_valid_in    <= '1';
                        mem_addr        <= mem_addr_base + EMF_GUIDPREFIX_OFFSET;
                        mem_read        <= '1';
                        -- Memory Flow Control Guard
                        if (mem_ready_in = '1') then
                            mem_cnt_next    <= mem_cnt + 1;
                        end if;
                    -- GET GUID Prefix 2/3
                    when 2 =>
                        mem_valid_in    <= '1';
                        mem_addr        <= mem_addr_base + EMF_GUIDPREFIX_OFFSET + 1;
                        mem_read        <= '1';
                        -- Memory Flow Control Guard
                        if (mem_ready_in = '1') then
                            mem_cnt_next    <= mem_cnt + 1;
                        end if;
                    -- GET GUID Prefix 3/3
                    when 3 =>
                        mem_valid_in    <= '1';
                        mem_addr        <= mem_addr_base + EMF_GUIDPREFIX_OFFSET + 2;
                        mem_read        <= '1';
                        -- Memory Flow Control Guard
                        if (mem_ready_in = '1') then
                            if (RELIABILTY_QOS = RELIABLE_RELIABILITY_QOS and check_mask(mem_endpoint_latch_data.field_flag,EMF_IPV4_ADDR_FLAG)) then
                                mem_cnt_next    <= 4;
                            elsif (RELIABILTY_QOS = RELIABLE_RELIABILITY_QOS and check_mask(mem_endpoint_latch_data.field_flag,EMF_UDP_PORT_FLAG)) then
                                mem_cnt_next    <= 5;
                            elsif check_mask(mem_endpoint_latch_data.field_flag,EMF_NEXT_SEQ_NR_FLAG) then
                                mem_cnt_next    <= 6;
                            elsif check_mask(mem_endpoint_latch_data.field_flag,EMF_LEASE_DEADLINE_FLAG) then
                                mem_cnt_next    <= 8;
                            elsif (RELIABILTY_QOS = RELIABLE_RELIABILITY_QOS and check_mask(mem_endpoint_latch_data.field_flag,EMF_RES_TIME_FLAG)) then
                                mem_cnt_next    <= 10;
                            else
                                if check_mask(mem_endpoint_latch_data.field_flag,EMF_ENTITYID_FLAG) then
                                    mem_cnt_next    <= 12;
                                else
                                    mem_cnt_next    <= 13;
                                end if;
                            end if;
                        end if;
                    -- GET IPv4 Address
                    when 4 =>
                        -- Synthesis Guard
                        if (RELIABILTY_QOS = RELIABLE_RELIABILITY_QOS) then
                            mem_valid_in    <= '1';
                            mem_addr        <= mem_addr_base + EMF_IPV4_ADDR_OFFSET;
                            mem_read        <= '1';
                            -- Memory Flow Control Guard
                            if (mem_ready_in = '1') then
                                if (RELIABILTY_QOS = RELIABLE_RELIABILITY_QOS and check_mask(mem_endpoint_latch_data.field_flag,EMF_UDP_PORT_FLAG)) then
                                    mem_cnt_next    <= 5;
                                elsif check_mask(mem_endpoint_latch_data.field_flag,EMF_NEXT_SEQ_NR_FLAG) then
                                    mem_cnt_next    <= 6;
                                elsif check_mask(mem_endpoint_latch_data.field_flag,EMF_LEASE_DEADLINE_FLAG) then
                                    mem_cnt_next    <= 8;
                                elsif ((RELIABILTY_QOS = RELIABLE_RELIABILITY_QOS) and check_mask(mem_endpoint_latch_data.field_flag,EMF_RES_TIME_FLAG)) then
                                    mem_cnt_next    <= 10;
                                else
                                    if check_mask(mem_endpoint_latch_data.field_flag,EMF_ENTITYID_FLAG) then
                                        mem_cnt_next    <= 12;
                                    elsif check_mask(mem_endpoint_latch_data.field_flag,EMF_GUIDPREFIX_FLAG) then
                                        mem_cnt_next    <= 13;
                                    else
                                        mem_cnt_next    <= 16;
                                    end if;
                                end if;
                            end if;
                        end if;
                    -- GET UDP Port/ Flags
                    when 5 =>
                        -- Synthesis Guard
                        if (RELIABILTY_QOS = RELIABLE_RELIABILITY_QOS) then
                            mem_valid_in    <= '1';
                            mem_addr        <= mem_addr_base + EMF_UDP_PORT_OFFSET;
                            mem_read        <= '1';
                            -- Memory Flow Control Guard
                            if (mem_ready_in = '1') then
                                if check_mask(mem_endpoint_latch_data.field_flag,EMF_NEXT_SEQ_NR_FLAG) then
                                    mem_cnt_next    <= 6;
                                elsif check_mask(mem_endpoint_latch_data.field_flag,EMF_LEASE_DEADLINE_FLAG) then
                                    mem_cnt_next    <= 8;
                                elsif (RELIABILTY_QOS = RELIABLE_RELIABILITY_QOS and check_mask(mem_endpoint_latch_data.field_flag,EMF_RES_TIME_FLAG)) then
                                    mem_cnt_next    <= 10;
                                else
                                    if check_mask(mem_endpoint_latch_data.field_flag,EMF_ENTITYID_FLAG) then
                                        mem_cnt_next    <= 12;
                                    elsif check_mask(mem_endpoint_latch_data.field_flag,EMF_GUIDPREFIX_FLAG) then
                                        mem_cnt_next    <= 13;
                                    elsif (RELIABILTY_QOS = RELIABLE_RELIABILITY_QOS and check_mask(mem_endpoint_latch_data.field_flag,EMF_IPV4_ADDR_FLAG)) then
                                        mem_cnt_next    <= 16;
                                    else
                                        mem_cnt_next    <= 17;
                                    end if;
                                end if;
                            end if;
                        end if;
                    -- GET Next Sequence Number 1/2
                    when 6 =>
                        mem_valid_in    <= '1';
                        mem_addr        <= mem_addr_base + EMF_NEXT_SEQ_NR_OFFSET;
                        mem_read        <= '1';
                        -- Memory Flow Control Guard
                        if (mem_ready_in = '1') then
                            mem_cnt_next    <= mem_cnt + 1;
                        end if;
                    -- GET Next Sequence Number 2/2
                    when 7 =>
                        mem_valid_in    <= '1';
                        mem_addr        <= mem_addr_base + EMF_NEXT_SEQ_NR_OFFSET + 1;
                        mem_read        <= '1';
                        -- Memory Flow Control Guard
                        if (mem_ready_in = '1') then
                            if check_mask(mem_endpoint_latch_data.field_flag,EMF_LEASE_DEADLINE_FLAG) then
                                mem_cnt_next    <= 8;
                            elsif (RELIABILTY_QOS = RELIABLE_RELIABILITY_QOS and check_mask(mem_endpoint_latch_data.field_flag,EMF_RES_TIME_FLAG)) then
                                mem_cnt_next    <= 10;
                            else
                                if check_mask(mem_endpoint_latch_data.field_flag,EMF_ENTITYID_FLAG) then
                                    mem_cnt_next    <= 12;
                                elsif check_mask(mem_endpoint_latch_data.field_flag,EMF_GUIDPREFIX_FLAG) then
                                    mem_cnt_next    <= 13;
                                elsif (RELIABILTY_QOS = RELIABLE_RELIABILITY_QOS and check_mask(mem_endpoint_latch_data.field_flag,EMF_IPV4_ADDR_FLAG)) then
                                    mem_cnt_next    <= 16;
                                elsif (RELIABILTY_QOS = RELIABLE_RELIABILITY_QOS and check_mask(mem_endpoint_latch_data.field_flag,EMF_UDP_PORT_FLAG)) then
                                    mem_cnt_next    <= 17;
                                else
                                    mem_cnt_next    <= 18;
                                end if;
                            end if;
                        end if;
                    -- GET Lease Deadline 1/2
                    when 8 =>
                        mem_valid_in    <= '1';
                        mem_addr        <= mem_addr_base + EMF_LEASE_DEADLINE_OFFSET;
                        mem_read        <= '1';
                        -- Memory Flow Control Guard
                        if (mem_ready_in = '1') then
                            mem_cnt_next    <= mem_cnt + 1;
                        end if;
                    -- GET Lease Deadline 2/2
                    when 9 =>
                        mem_valid_in    <= '1';
                        mem_addr        <= mem_addr_base + EMF_LEASE_DEADLINE_OFFSET + 1;
                        mem_read        <= '1';
                        -- Memory Flow Control Guard
                        if (mem_ready_in = '1') then
                            if (RELIABILTY_QOS = RELIABLE_RELIABILITY_QOS and check_mask(mem_endpoint_latch_data.field_flag,EMF_RES_TIME_FLAG)) then
                                mem_cnt_next    <= 10;
                            else
                                if check_mask(mem_endpoint_latch_data.field_flag,EMF_ENTITYID_FLAG) then
                                    mem_cnt_next    <= 12;
                                elsif check_mask(mem_endpoint_latch_data.field_flag,EMF_GUIDPREFIX_FLAG) then
                                    mem_cnt_next    <= 13;
                                elsif (RELIABILTY_QOS = RELIABLE_RELIABILITY_QOS and check_mask(mem_endpoint_latch_data.field_flag,EMF_IPV4_ADDR_FLAG)) then
                                    mem_cnt_next    <= 16;
                                elsif (RELIABILTY_QOS = RELIABLE_RELIABILITY_QOS and check_mask(mem_endpoint_latch_data.field_flag,EMF_UDP_PORT_FLAG)) then
                                    mem_cnt_next    <= 17;
                                elsif check_mask(mem_endpoint_latch_data.field_flag,EMF_NEXT_SEQ_NR_FLAG) then
                                    mem_cnt_next    <= 18;
                                else
                                    mem_cnt_next    <= 20;
                                end if;
                            end if;
                        end if;
                    -- GET Response Time 1/2
                    when 10 =>
                        -- Synthesis Guard
                        if (RELIABILTY_QOS = RELIABLE_RELIABILITY_QOS) then
                            mem_valid_in    <= '1';
                            mem_addr        <= mem_addr_base + EMF_RES_TIME_OFFSET;
                            mem_read        <= '1';
                            -- Memory Flow Control Guard
                            if (mem_ready_in = '1') then
                                mem_cnt_next    <= mem_cnt + 1;
                            end if;
                        end if;
                    -- GET Response Time 2/2
                    when 11 =>
                        -- Synthesis Guard
                        if (RELIABILTY_QOS = RELIABLE_RELIABILITY_QOS) then
                            mem_valid_in    <= '1';
                            mem_addr        <= mem_addr_base + EMF_RES_TIME_OFFSET + 1;
                            mem_read        <= '1';
                            -- Memory Flow Control Guard
                            if (mem_ready_in = '1') then
                                if check_mask(mem_endpoint_latch_data.field_flag,EMF_ENTITYID_FLAG) then
                                    mem_cnt_next    <= 12;
                                elsif check_mask(mem_endpoint_latch_data.field_flag,EMF_GUIDPREFIX_FLAG) then
                                    mem_cnt_next    <= 13;
                                elsif (RELIABILTY_QOS = RELIABLE_RELIABILITY_QOS and check_mask(mem_endpoint_latch_data.field_flag,EMF_IPV4_ADDR_FLAG)) then
                                    mem_cnt_next    <= 16;
                                elsif (RELIABILTY_QOS = RELIABLE_RELIABILITY_QOS and check_mask(mem_endpoint_latch_data.field_flag,EMF_UDP_PORT_FLAG)) then
                                    mem_cnt_next    <= 17;
                                elsif check_mask(mem_endpoint_latch_data.field_flag,EMF_NEXT_SEQ_NR_FLAG) then
                                    mem_cnt_next    <= 18;
                                elsif check_mask(mem_endpoint_latch_data.field_flag,EMF_LEASE_DEADLINE_FLAG) then
                                    mem_cnt_next    <= 20;
                                else
                                    mem_cnt_next    <= 22;
                                end if;
                            end if;
                        end if;
                    -- READ Entity ID
                    when 12 =>
                        mem_ready_out   <= '1';
                        -- Memory Flow Control Guard
                        if (mem_valid_out = '1') then
                            mem_endpoint_data_next.guid(3)  <= mem_read_data;
                            
                            if check_mask(mem_endpoint_latch_data.field_flag,EMF_GUIDPREFIX_FLAG) then
                                mem_cnt_next    <= 13;
                            elsif (RELIABILTY_QOS = RELIABLE_RELIABILITY_QOS and check_mask(mem_endpoint_latch_data.field_flag,EMF_IPV4_ADDR_FLAG)) then
                                mem_cnt_next    <= 16;
                            elsif (RELIABILTY_QOS = RELIABLE_RELIABILITY_QOS and check_mask(mem_endpoint_latch_data.field_flag,EMF_UDP_PORT_FLAG)) then
                                mem_cnt_next    <= 17;
                            elsif check_mask(mem_endpoint_latch_data.field_flag,EMF_NEXT_SEQ_NR_FLAG) then
                                mem_cnt_next    <= 18;
                            elsif check_mask(mem_endpoint_latch_data.field_flag,EMF_LEASE_DEADLINE_FLAG) then
                                mem_cnt_next    <= 20;
                            elsif (RELIABILTY_QOS = RELIABLE_RELIABILITY_QOS and check_mask(mem_endpoint_latch_data.field_flag,EMF_RES_TIME_FLAG)) then
                                mem_cnt_next    <= 22;
                            else
                                -- DONE
                                mem_stage_next  <= IDLE;
                            end if;
                        end if;
                    -- READ GUID Prefix 1/3
                    when 13 =>
                        mem_ready_out   <= '1';
                        -- Memory Flow Control Guard
                        if (mem_valid_out = '1') then
                            mem_endpoint_data_next.guid(0)  <= mem_read_data;
                            
                            mem_cnt_next    <= mem_cnt + 1;
                        end if;
                    -- READ GUID Prefix 2/3
                    when 14 =>
                        mem_ready_out   <= '1';
                        -- Memory Flow Control Guard
                        if (mem_valid_out = '1') then
                            mem_endpoint_data_next.guid(1)  <= mem_read_data;
                            
                            mem_cnt_next    <= mem_cnt + 1;
                        end if;
                    -- READ GUID Prefix 3/3
                    when 15 =>
                        mem_ready_out   <= '1';
                        -- Memory Flow Control Guard
                        if (mem_valid_out = '1') then
                            mem_endpoint_data_next.guid(2)  <= mem_read_data;
                            
                            if (RELIABILTY_QOS = RELIABLE_RELIABILITY_QOS and check_mask(mem_endpoint_latch_data.field_flag,EMF_IPV4_ADDR_FLAG)) then
                                mem_cnt_next    <= 16;
                            elsif (RELIABILTY_QOS = RELIABLE_RELIABILITY_QOS and check_mask(mem_endpoint_latch_data.field_flag,EMF_UDP_PORT_FLAG)) then
                                mem_cnt_next    <= 17;
                            elsif check_mask(mem_endpoint_latch_data.field_flag,EMF_NEXT_SEQ_NR_FLAG) then
                                mem_cnt_next    <= 18;
                            elsif check_mask(mem_endpoint_latch_data.field_flag,EMF_LEASE_DEADLINE_FLAG) then
                                mem_cnt_next    <= 20;
                            elsif (RELIABILTY_QOS = RELIABLE_RELIABILITY_QOS and check_mask(mem_endpoint_latch_data.field_flag,EMF_RES_TIME_FLAG)) then
                                mem_cnt_next    <= 22;
                            else
                                -- DONE
                                mem_stage_next  <= IDLE;
                            end if;
                        end if;
                    -- READ IPv4 Address
                    when 16 =>
                        -- Synthesis Guard
                        if (RELIABILTY_QOS = RELIABLE_RELIABILITY_QOS) then
                            mem_ready_out   <= '1';
                            -- Memory Flow Control Guard
                            if (mem_valid_out = '1') then
                                mem_endpoint_data_next.addr     <= mem_read_data;
                                
                                if (RELIABILTY_QOS = RELIABLE_RELIABILITY_QOS and check_mask(mem_endpoint_latch_data.field_flag,EMF_UDP_PORT_FLAG)) then
                                    mem_cnt_next    <= 17;
                                elsif check_mask(mem_endpoint_latch_data.field_flag,EMF_NEXT_SEQ_NR_FLAG) then
                                    mem_cnt_next    <= 18;
                                elsif check_mask(mem_endpoint_latch_data.field_flag,EMF_LEASE_DEADLINE_FLAG) then
                                    mem_cnt_next    <= 20;
                                elsif (RELIABILTY_QOS = RELIABLE_RELIABILITY_QOS and check_mask(mem_endpoint_latch_data.field_flag,EMF_RES_TIME_FLAG)) then
                                    mem_cnt_next    <= 22;
                                else
                                    -- DONE
                                    mem_stage_next  <= IDLE;
                                end if;
                            end if;
                        end if;
                    -- READ UDP Port
                    when 17 =>
                        -- Synthesis Guard
                        if (RELIABILTY_QOS = RELIABLE_RELIABILITY_QOS) then
                            mem_ready_out   <= '1';
                            -- Memory Flow Control Guard
                            if (mem_valid_out = '1') then
                                mem_endpoint_data_next.portn    <= mem_read_data(WORD_WIDTH-1 downto WORD_WIDTH-UDP_PORT_WIDTH);
                                
                                if check_mask(mem_endpoint_latch_data.field_flag,EMF_NEXT_SEQ_NR_FLAG) then
                                    mem_cnt_next    <= 18;
                                elsif check_mask(mem_endpoint_latch_data.field_flag,EMF_LEASE_DEADLINE_FLAG) then
                                    mem_cnt_next    <= 20;
                                elsif (RELIABILTY_QOS = RELIABLE_RELIABILITY_QOS and check_mask(mem_endpoint_latch_data.field_flag,EMF_RES_TIME_FLAG)) then
                                    mem_cnt_next    <= 22;
                                else
                                    -- DONE
                                    mem_stage_next  <= IDLE;
                                end if;
                            end if;
                        end if;
                    -- READ Next Sequence Number 1/2
                    when 18 =>
                        mem_ready_out   <= '1';
                        -- Memory Flow Control Guard
                        if (mem_valid_out = '1') then
                            mem_endpoint_data_next.next_seq_nr(0)   <= unsigned(mem_read_data);
                            
                            mem_cnt_next    <= mem_cnt + 1;
                        end if;
                    -- READ Next Sequence Number 2/2
                    when 19 =>
                        mem_ready_out   <= '1';
                        -- Memory Flow Control Guard
                        if (mem_valid_out = '1') then
                            mem_endpoint_data_next.next_seq_nr(1)   <= unsigned(mem_read_data);
                            
                            if check_mask(mem_endpoint_latch_data.field_flag,EMF_LEASE_DEADLINE_FLAG) then
                                mem_cnt_next    <= 20;
                            elsif (RELIABILTY_QOS = RELIABLE_RELIABILITY_QOS and check_mask(mem_endpoint_latch_data.field_flag,EMF_RES_TIME_FLAG)) then
                                mem_cnt_next    <= 22;
                            else
                                -- DONE
                                mem_stage_next  <= IDLE;
                            end if;
                        end if;
                    -- READ Lease Deadline 1/2
                    when 20 =>
                        mem_ready_out   <= '1';
                        -- Memory Flow Control Guard
                        if (mem_valid_out = '1') then
                            mem_endpoint_data_next.lease_deadline(0)    <= unsigned(mem_read_data);
                            
                            mem_cnt_next    <= mem_cnt + 1;
                        end if;
                    -- READ Lease Deadline 2/2
                    when 21 =>
                        mem_ready_out   <= '1';
                        -- Memory Flow Control Guard
                        if (mem_valid_out = '1') then
                            mem_endpoint_data_next.lease_deadline(1)    <= unsigned(mem_read_data);
                            
                            if (RELIABILTY_QOS = RELIABLE_RELIABILITY_QOS and check_mask(mem_endpoint_latch_data.field_flag,EMF_RES_TIME_FLAG)) then
                                mem_cnt_next    <= 22;
                            else
                                -- DONE
                                mem_stage_next  <= IDLE;
                            end if;
                        end if;
                    -- READ Response Time 1/2
                    when 22 =>
                        -- Synthesis Guard
                        if (RELIABILTY_QOS = RELIABLE_RELIABILITY_QOS) then
                            mem_ready_out   <= '1';
                            -- Memory Flow Control Guard
                            if (mem_valid_out = '1') then
                                mem_endpoint_data_next.res_time(0)  <= unsigned(mem_read_data);
                                
                                mem_cnt_next    <= mem_cnt + 1;
                            end if;
                        end if;
                    -- READ Response Time 2/2
                    when 23 =>
                        -- Synthesis Guard
                        if (RELIABILTY_QOS = RELIABLE_RELIABILITY_QOS) then
                            mem_ready_out   <= '1';
                            -- Memory Flow Control Guard
                            if (mem_valid_out = '1') then
                                mem_endpoint_data_next.res_time(1)  <= unsigned(mem_read_data);
                                
                                -- DONE
                                mem_stage_next  <= IDLE;
                            end if;
                        end if;
                    when others =>
                        null;
                end case;
            when INSERT_ENDPOINT =>
                case (mem_cnt) is
                    -- Entity ID
                    when 0 =>
                        mem_valid_in    <= '1';
                        mem_addr        <= mem_addr_base + EMF_ENTITYID_OFFSET;
                        mem_write_data  <= mem_endpoint_latch_data.guid(3);
                        if (mem_ready_in = '1') then
                            mem_cnt_next    <= mem_cnt + 1;
                        end if;
                    -- GUID Prefix 1/3
                    when 1 =>
                        mem_valid_in    <= '1';
                        mem_addr        <= mem_addr_base + EMF_GUIDPREFIX_OFFSET;
                        mem_write_data  <= mem_endpoint_latch_data.guid(0);
                        if (mem_ready_in = '1') then
                            mem_cnt_next    <= mem_cnt + 1;
                        end if;
                    -- GUID Prefix 2/3
                    when 2 =>
                        mem_valid_in    <= '1';
                        mem_addr        <= mem_addr_base + EMF_GUIDPREFIX_OFFSET + 1;
                        mem_write_data  <= mem_endpoint_latch_data.guid(1);
                        if (mem_ready_in = '1') then
                            mem_cnt_next    <= mem_cnt + 1;
                        end if;
                    -- GUID Prefix 3/3
                    when 3 =>
                        mem_valid_in    <= '1';
                        mem_addr        <= mem_addr_base + EMF_GUIDPREFIX_OFFSET + 2;
                        mem_write_data  <= mem_endpoint_latch_data.guid(2);
                        if (mem_ready_in = '1') then
                            mem_cnt_next    <= mem_cnt + 1 when (RELIABILTY_QOS = RELIABLE_RELIABILITY_QOS) else 6;
                        end if;
                    -- IPv4 Address
                    when 4 =>
                        -- Synthesis Guard
                        if (RELIABILTY_QOS = RELIABLE_RELIABILITY_QOS) then
                            mem_valid_in    <= '1';
                            mem_addr        <= mem_addr_base + EMF_IPV4_ADDR_OFFSET;
                            mem_write_data  <= mem_endpoint_latch_data.addr;
                            if (mem_ready_in = '1') then
                                mem_cnt_next    <= mem_cnt + 1;
                            end if;
                        end if;
                    -- UDPv4 Ports
                    when 5 =>
                        -- Synthesis Guard
                        if (RELIABILTY_QOS = RELIABLE_RELIABILITY_QOS) then
                            mem_valid_in    <= '1';
                            mem_addr        <= mem_addr_base + EMF_UDP_PORT_OFFSET;
                            mem_write_data  <= mem_endpoint_latch_data.portn & ((mem_write_data'length-mem_endpoint_latch_data.portn'length-1) downto 0 => '0');
                            if (mem_ready_in = '1') then
                                mem_cnt_next    <= mem_cnt + 1;
                            end if;
                        end if;
                    -- Next Sequence Number 1/2
                    when 6 =>
                        mem_valid_in    <= '1';
                        mem_addr        <= mem_addr_base + EMF_NEXT_SEQ_NR_OFFSET;
                        mem_write_data  <= std_logic_vector(SEQUENCENUMBER_UNKNOWN(0)) when (DURABILITY_QOS = VOLATILE_DURABILITY_QOS) else std_logic_vector(FIRST_SEQUENCENUMBER(0));
                        if (mem_ready_in = '1') then
                            mem_cnt_next    <= mem_cnt + 1;
                        end if;
                    -- Next Sequence Number 2/2
                    when 7 =>
                        mem_write_data  <= (others => '0');
                        mem_valid_in    <= '1';
                        mem_addr        <= mem_addr_base + EMF_NEXT_SEQ_NR_OFFSET + 1;
                        mem_write_data  <= std_logic_vector(SEQUENCENUMBER_UNKNOWN(1)) when (DURABILITY_QOS = VOLATILE_DURABILITY_QOS) else std_logic_vector(FIRST_SEQUENCENUMBER(1));
                        if (mem_ready_in = '1') then
                            mem_cnt_next    <= mem_cnt + 1;
                        end if;
                    -- Lease Deadline 1/2
                    when 8 =>
                        mem_valid_in    <= '1';
                        mem_addr        <= mem_addr_base + EMF_LEASE_DEADLINE_OFFSET;
                        mem_write_data  <= std_logic_vector(mem_endpoint_latch_data.lease_deadline(0));
                        if (mem_ready_in = '1') then
                            mem_cnt_next    <= mem_cnt + 1;
                        end if;
                    -- Lease Deadline 2/2
                    when 9 =>
                        mem_valid_in    <= '1';
                        mem_addr        <= mem_addr_base + EMF_LEASE_DEADLINE_OFFSET + 1;
                        mem_write_data  <= std_logic_vector(mem_endpoint_latch_data.lease_deadline(1));
                        if (mem_ready_in = '1') then
                            if (RELIABILTY_QOS = RELIABLE_RELIABILITY_QOS) then
                                mem_cnt_next    <= mem_cnt + 1;
                            else
                                -- DONE
                                mem_stage_next  <= IDLE;
                            end if;
                        end if;
                    -- Response Time 1/2
                    when 10 =>
                        -- Synthesis Guard
                        if (RELIABILTY_QOS = RELIABLE_RELIABILITY_QOS) then
                            mem_valid_in    <= '1';
                            mem_addr        <= mem_addr_base + EMF_RES_TIME_OFFSET;
                            mem_write_data  <= std_logic_vector(TIME_INVALID(0));
                            if (mem_ready_in = '1') then
                                mem_cnt_next    <= mem_cnt + 1;
                            end if;
                        end if;
                    -- Response Time 2/2
                    when 11 =>
                        if (RELIABILTY_QOS = RELIABLE_RELIABILITY_QOS) then
                            mem_valid_in    <= '1';
                            mem_addr        <= mem_addr_base + EMF_RES_TIME_OFFSET + 1;
                            mem_write_data  <= std_logic_vector(TIME_INVALID(1));
                            if (mem_ready_in = '1') then
                                -- DONE
                                mem_stage_next  <= IDLE;
                            end if;
                        end if;
                    when others => 
                        null;
                end case;
            when UPDATE_ENDPOINT =>
                case (mem_cnt) is
                    -- IPv4 Address
                    when 0 =>
                        -- Synthesis Guard
                        if (RELIABILTY_QOS = RELIABLE_RELIABILITY_QOS) then
                            mem_valid_in    <= '1';
                            mem_addr        <= mem_addr_base + EMF_IPV4_ADDR_OFFSET;
                            mem_write_data  <= mem_endpoint_latch_data.addr;
                            mem_endpoint_data_next.addr  <= mem_endpoint_latch_data.addr;
                            -- Memory Flow Control Guard
                            if (mem_ready_in = '1') then
                                if (RELIABILTY_QOS = RELIABLE_RELIABILITY_QOS and check_mask(mem_endpoint_latch_data.field_flag,EMF_UDP_PORT_FLAG)) then
                                    mem_cnt_next    <= 1;
                                elsif check_mask(mem_endpoint_latch_data.field_flag,EMF_NEXT_SEQ_NR_FLAG) then
                                    mem_cnt_next    <= 2;
                                elsif check_mask(mem_endpoint_latch_data.field_flag,EMF_LEASE_DEADLINE_FLAG) then
                                    mem_cnt_next    <= 4;
                                elsif (RELIABILTY_QOS = RELIABLE_RELIABILITY_QOS and check_mask(mem_endpoint_latch_data.field_flag,EMF_RES_TIME_FLAG)) then
                                    mem_cnt_next    <= 6;
                                else
                                    -- DONE
                                    mem_stage_next  <= IDLE;
                                end if;
                            end if;
                        end if;
                    -- UDPv4 Ports
                    when 1 =>
                        -- Synthesis Guard
                        if (RELIABILTY_QOS = RELIABLE_RELIABILITY_QOS) then
                            mem_valid_in    <= '1';
                            mem_addr        <= mem_addr_base + EMF_UDP_PORT_OFFSET;
                            mem_write_data  <= mem_endpoint_latch_data.portn & ((mem_write_data'length-mem_endpoint_latch_data.portn'length-1) downto 0 => '0');
                            mem_endpoint_data_next.portn <= mem_endpoint_latch_data.portn;
                            -- Memory Flow Control Guard
                            if (mem_ready_in = '1') then
                                if check_mask(mem_endpoint_latch_data.field_flag,EMF_NEXT_SEQ_NR_FLAG) then
                                    mem_cnt_next    <= 2;
                                elsif check_mask(mem_endpoint_latch_data.field_flag,EMF_LEASE_DEADLINE_FLAG) then
                                    mem_cnt_next    <= 4;
                                elsif (RELIABILTY_QOS = RELIABLE_RELIABILITY_QOS and check_mask(mem_endpoint_latch_data.field_flag,EMF_RES_TIME_FLAG)) then
                                    mem_cnt_next    <= 6;
                                else
                                    -- DONE
                                    mem_stage_next  <= IDLE;
                                end if;
                            end if;
                        end if;
                    -- Next Sequence Number 1/2
                    when 2 =>
                        mem_valid_in    <= '1';
                        mem_addr        <= mem_addr_base + EMF_NEXT_SEQ_NR_OFFSET;
                        mem_write_data  <= std_logic_vector(mem_endpoint_latch_data.next_seq_nr(0));
                        -- Memory Flow Control Guard
                        if (mem_ready_in = '1') then
                            mem_cnt_next    <= mem_cnt + 1;
                        end if;
                    -- Next Sequence Number 2/2
                    when 3 =>
                        mem_valid_in    <= '1';
                        mem_addr        <= mem_addr_base + EMF_NEXT_SEQ_NR_OFFSET + 1;
                        mem_write_data  <= std_logic_vector(mem_endpoint_latch_data.next_seq_nr(1));
                        mem_endpoint_data_next.next_seq_nr   <= mem_endpoint_latch_data.next_seq_nr;
                        -- Memory Flow Control Guard
                        if (mem_ready_in = '1') then
                            if check_mask(mem_endpoint_latch_data.field_flag,EMF_LEASE_DEADLINE_FLAG) then
                                mem_cnt_next    <= 4;
                            elsif (RELIABILTY_QOS = RELIABLE_RELIABILITY_QOS and check_mask(mem_endpoint_latch_data.field_flag,EMF_RES_TIME_FLAG)) then
                                mem_cnt_next    <= 6;
                            else
                                -- DONE
                                mem_stage_next  <= IDLE;
                            end if;
                        end if;
                    -- Lease Deadline 1/2
                    when 4 =>
                        mem_valid_in    <= '1';
                        mem_addr        <= mem_addr_base + EMF_LEASE_DEADLINE_OFFSET;
                        mem_write_data  <= std_logic_vector(mem_endpoint_latch_data.lease_deadline(0));
                        -- Memory Flow Control Guard
                        if (mem_ready_in = '1')  then
                            mem_cnt_next    <= mem_cnt + 1;
                        end if;
                    -- Lease Deadline 2/2
                    when 5 =>
                        mem_valid_in    <= '1';
                        mem_addr        <= mem_addr_base + EMF_LEASE_DEADLINE_OFFSET + 1;
                        mem_write_data  <= std_logic_vector(mem_endpoint_latch_data.lease_deadline(1));
                        mem_endpoint_data_next.lease_deadline    <= mem_endpoint_latch_data.lease_deadline;
                        -- Memory Flow Control Guard
                        if (mem_ready_in = '1')  then
                            if (RELIABILTY_QOS = RELIABLE_RELIABILITY_QOS and check_mask(mem_endpoint_latch_data.field_flag,EMF_RES_TIME_FLAG)) then
                                mem_cnt_next    <= 6;
                            else
                                -- DONE
                                mem_stage_next  <= IDLE;
                            end if;
                        end if;
                    -- Response Time 1/2
                    when 6 =>
                        -- Synthesis Guard
                        if (RELIABILTY_QOS = RELIABLE_RELIABILITY_QOS) then
                            mem_valid_in    <= '1';
                            mem_addr        <= mem_addr_base + EMF_RES_TIME_OFFSET;
                            mem_write_data  <= std_logic_vector(mem_endpoint_latch_data.res_time(0));
                            -- Memory Flow Control Guard
                            if (mem_ready_in = '1')  then
                                mem_cnt_next    <= mem_cnt + 1;
                            end if;
                        end if;
                    -- Response Time 2/2
                    when 7 =>
                        -- Synthesis Guard
                        if (RELIABILTY_QOS = RELIABLE_RELIABILITY_QOS) then
                            mem_valid_in    <= '1';
                            mem_addr        <= mem_addr_base + EMF_RES_TIME_OFFSET + 1;
                            mem_write_data  <= std_logic_vector(mem_endpoint_latch_data.res_time(1));
                            mem_endpoint_data_next.res_time  <= mem_endpoint_latch_data.res_time;
                            -- Memory Flow Control Guard
                            if (mem_ready_in = '1')  then
                                -- DONE
                                mem_stage_next  <= IDLE;
                            end if;
                        end if;
                    when others =>
                        null;
                end case;
            when REMOVE_ENDPOINT =>
                -- Mark with ENTITYID_UNKNOWN to mark slot empty_user
                mem_valid_in    <= '1';
                mem_addr        <= mem_addr_base + EMF_ENTITYID_OFFSET;
                mem_write_data  <= ENTITYID_UNKNOWN;
                
                -- Memory Flow Control Guard
                if (mem_ready_in = '1') then
                    -- Reset MAX Endpoint Pointer
                    mem_addr_base_next      <= FIRST_ENDPOINT_ADDRESS;
                    mem_pos_next            <= 0;
                    last_addr_next          <= (others => '0');
                    mem_stage_next          <= RESET_MAX_POINTER;
                    mem_cnt_next            <= 0;
                    -- Save Current Memory Position
                    mem_addr_latch_next     <= mem_addr_base;
                end if;
            when FIND_EMPTY_SLOT =>
                case (mem_cnt) is
                    -- *READ ADDRESS*
                    -- Entity ID
                    when 0 =>
                        mem_addr        <= mem_addr_base + EMF_ENTITYID_OFFSET;
                        mem_read        <= '1';
                        mem_valid_in    <= '1';
                        
                        -- Memory Control Flow Guard
                        if (mem_ready_in = '1') then
                            mem_cnt_next    <= 1;
                        end if;
                    -- *READ DATA*
                    -- Entity ID
                    when 1 =>
                        mem_ready_out   <= '1';
                        
                        -- Memory Control Flow Guard
                        if (mem_valid_out = '1') then
                            -- Slot Occupied
                            if (mem_read_data /= ENTITYID_UNKNOWN) then
                                -- Reached end of Endpoint Memory Area
                                if (mem_addr_base = max_endpoint_addr) then
                                    -- MEMORY FULL
                                    if (max_endpoint_addr = MAX_ENDPOINT_ADDRESS) then
                                        report "Memory Full, Ignoring Endpoint Data" severity NOTE;
                                        -- Ignore Insertion
                                        mem_stage_next      <= IDLE;
                                        mem_addr_base_next  <= ENDPOINT_MEMORY_MAX_ADDRESS;
                                    else
                                        -- Extend Endpoint Memory Area
                                        -- NOTE: "max_endpoint_addr" points to the first address of last Endpoint Frame
                                        max_endpoint_addr_next   <= mem_addr_base + ENDPOINT_FRAME_SIZE;
                                        -- Populate Endpoint Slot
                                        mem_stage_next      <= INSERT_ENDPOINT;
                                        mem_addr_base_next  <= mem_addr_base + ENDPOINT_FRAME_SIZE;
                                        mem_pos_next        <= mem_pos + 1;
                                        mem_cnt_next        <= 0;
                                    end if;
                                else
                                    -- Continue Search
                                    mem_addr_base_next  <= mem_addr_base + ENDPOINT_FRAME_SIZE;
                                    mem_pos_next        <= mem_pos + 1;
                                    mem_cnt_next        <= 0;
                                end if;
                            -- Slot Empty
                            else
                                -- Populate Endpoint Slot
                                mem_stage_next      <= INSERT_ENDPOINT;
                                mem_cnt_next        <= 0;
                            end if;
                        end if;
                    when others =>
                        null;
                end case;
            when RESET_MAX_POINTER =>
                case (mem_cnt) is
                    -- GET Entity ID
                    when 0 =>
                        mem_addr        <= mem_addr_base + EMF_ENTITYID_OFFSET;
                        mem_read        <= '1';
                        mem_valid_in    <= '1';
                        
                        -- Memory Control Flow Guard
                        if (mem_ready_in = '1') then
                            mem_cnt_next    <= 1;
                        end if;
                    -- READ Entity ID
                    when 1 =>
                        mem_ready_out   <= '1';
                        
                        -- Memory Control Flow Guard
                        if (mem_valid_out = '1') then
                            -- Slot Occupied
                            if (mem_read_data /= ENTITYID_UNKNOWN) then
                                -- Reached end of Endpoint Memory Area
                                if (mem_addr_base = max_endpoint_addr) then                                    
                                    -- No Change
                                    mem_stage_next      <= IDLE;
                                    -- Restore Memory Position
                                    mem_addr_base_next  <= mem_addr_latch;
                                else
                                    -- Latch last occupied Endpoint Slot
                                    last_addr_next      <= mem_addr_base;
                                    -- Continue Search
                                    mem_addr_base_next  <= mem_addr_base + ENDPOINT_FRAME_SIZE;
                                    mem_cnt_next        <= 0;
                                end if;
                            -- Slot Empty
                            else
                                -- Make sure to iterate through complete Endpoint Area
                                if (mem_addr_base = max_endpoint_addr) then
                                    -- Reset Pointer to last occupied Endpoint Slot
                                    max_endpoint_addr_next  <= last_addr;
                                    -- DONE
                                    mem_stage_next      <= IDLE;
                                    -- Restore Memory Position
                                    mem_addr_base_next  <= mem_addr_latch;
                                else
                                    -- Continue Search
                                    mem_addr_base_next  <= mem_addr_base + ENDPOINT_FRAME_SIZE;
                                    mem_cnt_next        <= 0;
                                end if;                                
                            end if;
                        end if;
                    when others =>
                        null;
                end case;
            when GET_NEXT_ENDPOINT =>
                case (mem_cnt) is
                    -- GET Entity ID
                    when 0 =>
                        mem_valid_in    <= '1';
                        mem_read        <= '1';
                        mem_addr        <= mem_addr_base + EMF_ENTITYID_OFFSET;
                        
                        -- Memory Flow Control Guard
                        if (mem_ready_in = '1') then
                            mem_cnt_next    <= 1;
                        end if;
                    -- READ Entity ID
                    when 1 =>
                        mem_ready_out   <= '1';
                        
                        -- Memory Flow Control Guard
                        if (mem_valid_out = '1') then
                            -- Slot Occupied
                            if (mem_read_data /= ENTITYID_UNKNOWN) then
                                -- Fetch Endpoint Data
                                mem_stage_next          <= GET_ENDPOINT_DATA;
                                mem_endpoint_data_next  <= ZERO_ENDPOINT_DATA;
                                if check_mask(mem_endpoint_latch_data.field_flag,EMF_ENTITYID_FLAG) then
                                    mem_cnt_next    <= 0;
                                elsif check_mask(mem_endpoint_latch_data.field_flag,EMF_GUIDPREFIX_FLAG) then
                                    mem_cnt_next    <= 1;
                                elsif (RELIABILTY_QOS = RELIABLE_RELIABILITY_QOS and check_mask(mem_endpoint_latch_data.field_flag,EMF_IPV4_ADDR_FLAG)) then
                                    mem_cnt_next    <= 4;
                                elsif (RELIABILTY_QOS = RELIABLE_RELIABILITY_QOS and check_mask(mem_endpoint_latch_data.field_flag,EMF_UDP_PORT_FLAG)) then
                                    mem_cnt_next    <= 5;
                                elsif check_mask(mem_endpoint_latch_data.field_flag,EMF_NEXT_SEQ_NR_FLAG) then
                                    mem_cnt_next    <= 6;
                                elsif check_mask(mem_endpoint_latch_data.field_flag,EMF_LEASE_DEADLINE_FLAG) then
                                    mem_cnt_next    <= 8;
                                elsif (RELIABILTY_QOS = RELIABLE_RELIABILITY_QOS and check_mask(mem_endpoint_latch_data.field_flag,EMF_RES_TIME_FLAG)) then
                                    mem_cnt_next    <= 10;
                                else
                                    -- DONE
                                    mem_stage_next  <= IDLE;
                                end if;
                            -- Slot Empty
                            else
                                -- Reached End of Memory, No Match
                                if (mem_addr_base = max_endpoint_addr) then
                                    mem_addr_base_next   <= ENDPOINT_MEMORY_MAX_ADDRESS; --No match
                                    -- DONE
                                    mem_stage_next  <= IDLE;
                                else
                                    -- Continue Search
                                    mem_addr_base_next  <= mem_addr_base + ENDPOINT_FRAME_SIZE;
                                    mem_pos_next        <= mem_pos + 1;
                                    mem_cnt_next        <= 0;
                                end if;
                            end if;
                        end if;
                    when others =>
                        null;
                end case;
            when RESET_MEMORY =>
                case (mem_cnt) is
                    -- Initiate Reset
                    when 0 =>
                        mem_addr_base_next  <= FIRST_ENDPOINT_ADDRESS;
                        mem_cnt_next        <= mem_cnt + 1;
                    -- Reset Memory
                    when 1 =>
                        mem_valid_in    <= '1';
                        mem_addr        <= mem_addr_base + EMF_ENTITYID_OFFSET;
                        mem_write_data  <= ENTITYID_UNKNOWN;
                        
                        -- Memory Flow Control Guard
                        if (mem_ready_in = '1') then
                            -- End of Memory
                            if (mem_addr_base = MAX_ENDPOINT_ADDRESS) then
                                -- DONE
                                mem_stage_next  <= IDLE;
                            else
                                -- Next Endpoint Frame
                                mem_addr_base_next  <= mem_addr_base + ENDPOINT_FRAME_SIZE;
                            end if;
                        end if;
                    when others =>
                        null;
                end case;
            when others =>
                null;
        end case;
    end process;
    
    -- Process responsible for counting read words
    -- This process uses the actual FIFO read signals to determine reads
    word_counter_prc : process(clk, reset)
    begin
        if rising_edge(clk) then
            -- Reset Read counter
            if (reset = '1' or reset_read_cnt = '1') then
                read_cnt    <= (others => '0');
            -- Increment read counter each time rd_sig is high
            elsif (rd_sig = '1') then
                read_cnt    <= read_cnt + 1;
            end if;
        end if;
    end process;
    
    sync_prc : process(clk)
    begin
        if rising_edge(clk) then
            if (reset = '1') then
                stage                   <= IDLE;
                return_stage            <= IDLE;
                mem_stage               <= RESET_MEMORY;
                seq_nr                  <= SEQUENCENUMBER_UNKNOWN;
                next_seq_nr             <= SEQUENCENUMBER_UNKNOWN;
                sn_latch_1              <= SEQUENCENUMBER_UNKNOWN;
                sn_latch_2              <= SEQUENCENUMBER_UNKNOWN;
                sn_latch_3              <= SEQUENCENUMBER_UNKNOWN;
                ts                      <= TIME_INVALID;
                check_time              <= TIME_INVALID;
                lifespan                <= TIME_INVALID;
                guid                    <= GUID_UNKNOWN;
                addr                    <= IPv4_ADDRESS_INVALID;
                portn                   <= UDP_PORT_INVALID;
                mem_endpoint_data       <= ZERO_ENDPOINT_DATA;
                mem_endpoint_latch_data <= ZERO_ENDPOINT_LATCH_DATA;
                cnt                     <= 0;
                cnt2                    <= 0;
                bitmap_pos              <= 0;
                mem_cnt                 <= 0;
                mem_pos                 <= 0;
                is_meta                 <= '0';
                key_hash_rcvd           <= '0';
                last_word_in_latch      <= '0';
                stale_check             <= '0';
                parameter_end           <= (others => '1');
                bitmap_cnt              <= (others => '0');
                meta_opcode             <= (others => '0');
                status_info             <= (others => '0');
                mem_addr_base           <= (others => '0');
                last_addr               <= (others => '0');
                mem_addr_latch          <= (others => '0');
                max_endpoint_addr       <= (others => '0');
                flags                   <= (others => '0');
                opcode                  <= (others => '0');
                count                   <= (others => '0');
                key_hash                <= (others => (others => '0'));
                bitmap_latch            <= (others => (others => '0'));
            else
                stage                   <= stage_next;
                return_stage            <= return_stage_next;
                mem_stage               <= mem_stage_next;
                seq_nr                  <= seq_nr_next;
                next_seq_nr             <= next_seq_nr_next;
                sn_latch_1              <= sn_latch_1_next;
                sn_latch_2              <= sn_latch_2_next;
                sn_latch_3              <= sn_latch_3_next;
                ts                      <= ts_next;
                check_time              <= check_time_next;
                lifespan                <= lifespan_next;
                guid                    <= guid_next;
                addr                    <= addr_next;
                portn                   <= portn_next;
                mem_endpoint_data       <= mem_endpoint_data_next;
                mem_endpoint_latch_data <= mem_endpoint_latch_data_next;
                cnt                     <= cnt_next;
                cnt2                    <= cnt2_next;
                bitmap_pos              <= bitmap_pos_next;
                mem_cnt                 <= mem_cnt_next;
                mem_pos                 <= mem_pos_next;
                is_meta                 <= is_meta_next;
                key_hash_rcvd           <= key_hash_rcvd_next;
                last_word_in_latch      <= last_word_in_latch_next;
                parameter_end           <= parameter_end_next;
                bitmap_cnt              <= bitmap_cnt_next;
                stale_check             <= stale_check_next;
                meta_opcode             <= meta_opcode_next;
                status_info             <= status_info_next;
                mem_addr_base           <= mem_addr_base_next;
                last_addr               <= last_addr_next;
                mem_addr_latch          <= mem_addr_latch_next;
                max_endpoint_addr       <= max_endpoint_addr_next;
                flags                   <= flags_next;
                opcode                  <= opcode_next;
                count                   <= count_next;
                key_hash                <= key_hash_next;
                bitmap_latch            <= bitmap_latch_next;
            end if;
        end if;
    end process;
end architecture;