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

use work.rtps_package.all;
use work.user_config.all;
use work.rtps_config_package.all;

-- TODO: Use TIME_INVALID instead of zero. (Backport to builtin_endpoint) [Pay atention to (0) bit of ACKNACK and HEARTBEAT Deadlines, if compatible]
-- TODO: Check for special values of time/duration?
-- TODO: Remove last_word flag from metattraffic operations
-- TODO: Replace all ('range => 0) checks with defined constants, where possible
-- 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)
-- TODO: Is mem_addr_base needed? Isn't it a direct mirror of addr_mem_base? mem_addr_base is only used in the same clock cycle as mem_op_done is pulled high.


entity rtps_endpoint is
    generic (
        ID  : natural;
    );
    port (
        clk             : in std_logic;
        reset           : in std_logic;
        time            : in TIME_TYPE;
        alive           : out std_logic;
        
        empty           : in std_logic;
        rd              : out std_logic;
        last_word_in    : in std_logic;
        data_in         : in std_logic_vector(WORD_WIDTH-1 downto 0);
        
        meta_empty      : in std_logic;
        meta_data_in    : in std_logic_vector(WORD_WIDTH-1 downto 0);
        meta_rd         : out std_logic;
        
        rtps_wr         : out std_logic;
        rtps_full       : in std_logic;
        last_word_out   : out std_logic;
        data_out        : out std_logic_vector(WORD_WIDTH-1 downto 0);
        
        hc_start        : out std_logic;
        hc_opcode       : out HISTORY_CACHE_OPCODE_TYPE;
        hc_res          : in HISTORY_CACHE_RESPOSNE_TYPE;
        
        hc_data_out     : in std_logic_vector(WORD_WIDTH-1 downto 0);
        hc_valid_out    : in std_logic;
        hc_ready_out    : out std_logic;
        hc_last_word_out: in std_logic;
        
        hc_data_in      : out std_logic_vector(WORD_WIDTH-1 downto 0);
        hc_valid_in     : out std_logic;
        hc_ready_in     : in std_logic;
        hc_last_word_in : out std_logic
    );
end entity;

architecture arch of rtps_endpoint is
    
    --*****CONSTANT DECLARATION*****
    -- Endpoint Memory Size in 4-Byte Words
    constant ENDPOINT_MEMORY_SIZE       : natural := TODO;
    -- Endpoint Memory Address Width
    constant ENDPOINT_MEMORY_ADDR_WIDTH : natural := log2c(ENDPOINT_MEMORY_SIZE);
    -- Highest Memory Address
    constant 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) := 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');
    
    -- *UPDATE PARTICIPANT FLAG POSITIONS*
    constant UPDATE_ENDPOINT_FLAG_WIDTH : natural := 4;
    -- Signifies that the main Endpoint Data are updated
    constant ENDPOINT_DATA_FLAG         : std_logic_vector(0 to UPDATE_ENDPOINT_FLAG_WIDTH-1) := (0 => 1, others => '0');
    -- Signifies that the Lease Deadline of the Endpoint Data is updated
    constant LEASE_DEADLINE_FLAG        : std_logic_vector(0 to UPDATE_ENDPOINT_FLAG_WIDTH-1) := (1 => 1, others => '0');
    -- Signifies that the last Sequence Number of the Endpoint Data is updated
    constant NEXT_SEQ_NR_FLAG           : std_logic_vector(0 to UPDATE_ENDPOINT_FLAG_WIDTH-1) := (2 => 1, others => '0');
    -- Signifies that the HEARTBEAT/ACKNACK Timeout Time of the Endpoint Data is updated
    constant RES_TIME_FLAG              : std_logic_vector(0 to UPDATE_ENDPOINT_FLAG_WIDTH-1) := (3 => 1, others => '0');
    
    --*****TYPE DECLARATION*****
    -- FSM states. Explained below in detail
    type STAGE_TYPE is (IDLE, TODO);
    -- Memory FSM Opcodes
    type MEM_OPCODE_TYPE is (NOP, TODO);
    type MEM_STAGE_TYPE is (IDLE, TODO);
    -- Record of all Participant Data stored in memory
    type ENDPOINT_DATA_TYPE is record
        addr                : std_logic_vector(IPv4_ADDRESS_WIDTH-1 downto 0);
        portn               : std_logic_vector(UDP_PORT_WIDTH-1 downto 0);
        expects_inline_qos  : std_logic;
        lease_deadline      : TIME_TYPE;
        res_time            : TIME_TYPE;
        next_seq_nr         : SEQUENCENUMBER_TYPE;
    end record;
    constant ZERO_ENDPOINT_DATA : ENDPOINT_DATA_TYPE := (
        addr                => IPv4_ADDRESS_INVALID,
        portn               => UDP_PORT_INVALID,
        expects_inline_qos  => '0',
        lease_deadline      => TIME_INVALID,
        res_time            => TIME_INVALID,
        next_seq_nr         => SEQUENCENUMBER_UNKNOWN
    );
    type MEM_CTRL_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);
        expects_inline_qos  : std_logic;
        deadline            : TIME_TYPE;
        next_seq_nr         : SEQUENCENUMBER_TYPE;
        update_flags        : std_logic_vector(0 to UPDATE_ENDPOINT_FLAG_WIDTH-1);
    end record;
    constant ZERO_MEM_CTRL_DATA : MEM_CTRL_DATA_TYPE := (
        guid                => (others => (others => '0')),
        addr                => (others => '0'),
        portn               => (others => '0'),
        expects_inline_qos  => '0',
        deadline            => TIME_INVALID,
        next_seq_nr         => SEQUENCENUMBER_UNKNOWN,
        update_flags        => (others => '0'),
    );
    
    
    --*****SIGNAL DECLARATION*****
    -- FSM state
    signal stage, stage_next : STAGE_TYPE := IDLE;
    signal meta_opcode, meta_opcode_next : std_logic_vector(WORD_WIDTH-1 downto 0) := (others => '0');
    signal cnt, cnt_next : natural range TODO := 0;
    signal guid, guid_next : GUID_TYPE := (others => (others => '0'));
    signal mem_op_done : std_logic := '0';
    signal mem_opcode : MEM_OPCODE_TYPE := NOP;
    signal mem_op_start : std_logic := '0';
    signal addr, addr_next : std_logic_vector(IPv4_ADDRESS_WIDTH-1 downto 0) := (others => '0');
    signal portn, portn_next : std_logic_vector(UDP_PORT_WIDTH-1 downto 0) := (others => '0');
    signal expects_inline_qos, expects_inline_qos_next : std_logic := '0';
    signal is_meta, is_meta_next : std_logic := '0';
    signal update_endpoint_flags : std_logic_vector(0 to UPDATE_ENDPOINT_FLAG_WIDTH-1) := (others => '0');
    signal opcode, opcode_next : std_logic_vector(SUBMESSAGE_ID_WIDTH-1 downto 0) := (others => '0');
    signal flags, flags_next : std_logic_vector(SUBMESSAGE_FLAGS_WIDTH-1 downto 0) := (others => '0');
    signal seq_nr, seq_nr_next : SEQUENCENUMBER_TYPE := SEQUENCENUMBER_UNKNOWN;
    signal next_seq_nr, next_seq_nr_next : SEQUENCENUMBER_TYPE := SEQUENCENUMBER_UNKNOWN;
    signal ts, ts_next : TIME_TYPE := TIME_INVALID;
    signal deadline, deadline_next : DURATION_TYPE := DURATION_ZERO;
    signal sn_latch_1, sn_latch_1_next : SEQUENCENUMBER_TYPE := SEQUENCENUMBER_UNKNOWN;
    signal sn_latch_2, sn_latch_2_next : SEQUENCENUMBER_TYPE := SEQUENCENUMBER_UNKNOWN;
    signal sn_latch_3, sn_latch_3_next : SEQUENCENUMBER_TYPE := SEQUENCENUMBER_UNKNOWN;
    signal bitmap_cnt, bitmap_cnt_next : unsigned(log2c(MAX_BITMAP_WIDTH/CDR_LONG_WIDTH)-1 downto 0) := (others => '0');
    signal bitmap_latch, bitmap_latch_next : BITMAP_TYPE := (others => (others => '0'));
    signal cnt2, cnt2_next : natural range 0 to BITMAP_TYPE'length := 0;
    signal bitmap_pos, bitmap_pos_next : natural range 0 to MAX_BITMAP_WIDTH-1 := 0;
    signal key_hash, key_hash_next  : KEY_HASH_TYPE := (others => (others => '0'));
    signal key_hash_rcvd, key_hash_rcvd_next : std_logic := '0';
    signal status_info, status_info_next : std_logic_vector(STATUS_INFO_WIDTH-1 downto 0) := (others => '0');
    signal last_word_in_latch, last_word_in_latch_next : 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');
    
    signal data_in_swapped : std_logic_vector(WORD_WIDTH-1 downto 0) := (others => '0');
    
    signal mem_stage, mem_stage_next : MEM_STAGE_TYPE := IDLE;
    signal mem_wr, mem_rd : std_logic := '0';
    signal mem_wr_data, mem_rd_data : std_logic_vector(WORD_WIDTH-1 downto 0) := (others => '0');
    signal mem_addr, mem_addr_next : unsigned(ENDPOINT_MEMORY_ADDR_WIDTH-1 downto 0) := (others => '0');
    signal mem_addr_base, mem_addr_base_next : unsigned(ENDPOINT_MEMORY_ADDR_WIDTH-1 downto 0) := (others => '0');
    signal mem_addr_base, mem_addr_base_next : unsigned(ENDPOINT_MEMORY_ADDR_WIDTH-1 downto 0) := (others => '0');
    signal last_addr, last_addr_next : unsigned(ENDPOINT_MEMORY_ADDR_WIDTH-1 downto 0) := (others => '0');
    signal max_endpoint_addr, max_endpoint_addr_next : unsigned(ENDPOINT_MEMORY_ADDR_WIDTH-1 downto 0) := (others => '0');
    signal mem_cnt, mem_cnt_next : natural range TODO := 0;
    signal mem_endpoint_data, mem_endpoint_data_next : ENDPOINT_DATA_TYPE := ZERO_ENDPOINT_DATA;
    signal reset_max_pointer, reset_max_pointer_next : std_logic := '0';
    signal mem_long_latch, mem_long_latch_next : std_logic_vector(CDR_LONG_WIDTH-1 downto 0) := (others => '0');
    signal mem_ctrl_data, mem_ctrl_data_next : MEM_CTRL_DATA_TYPE := ZERO_MEM_CTRL_DATA;
    signal mem_pos, mem_pos_next : natural range TODO := 0;
    signal is_psearch, is_psearch_next : std_logic := '0';
    
    signal stale_check, stale_check_next : std_logic := '0';
    signal count, count_next : unsigned(COUNT_WIDTH-1 downto 0) := (others => '0');
    signal return_stage, return_stage_next : STAGE_TYPE := IDLE;
    
    --*****ALIAS DECLARATION*****
    -- ENDPOINT FRAME HEADER
    alias header_opcode             : std_logic_vector(7 downto 0) is data_in(31 downto 24);
    alias header_flags              : std_logic_vector(7 downto 0) is data_in(23 downto 16);
    alias header_udp_port           : std_logic_vector(15 downto 0) is data_in(15 downto 0);
    -- RTPS PARAMETER LIST HEADER
    alias parameter_id              : std_logic_vector(15 downto 0) is data_in(31 downto 16);
    alias parameter_length          : std_logic_vector(15 downto 0) is data_in(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(31 downto 16);
    alias representation_options    : std_logic_vector(15 downto 0) is data_in(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);
    -- 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
    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*****
    ram_inst : single_port_ram
        generic map (
            ADDR_WIDTH      => ENDPOINT_MEMORY_ADDR_WIDTH,
            DATA_WIDTH      => WORD_WIDTH,
            MEMORY_DEPTH    => ENDPOINT_MEMORY_SIZE
        )
        port map (
            clk         => clk,
            addr        => std_logic_vector(end_mem_addr),
            wen         => mem_wr,
            ren         => mem_rd,
            wr_data     => mem_wr_data,
            rd_data     => mem_rd_data
        );
    
    -- Big Endian Representation
    data_in_swapped <= endian_swap(endian_flag, data_in);
    
    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';
    begin
        -- DEFAULT
        stage_next                  <= stage;
        meta_opcode_next            <= meta_opcode;
        cnt_next                    <= cnt;
        guid_next                   <= guid;
        addr_next                   <= addr;
        portn_next                  <= portn;
        expects_inline_qos_next     <= expects_inline_qos;
        is_meta_next                <= is_meta;
        opcode_next                 <= opcode;
        flags_next                  <= flags;
        seq_nr_next                 <= seq_nr;
        ts_next                     <= ts;
        deadline_next               <= deadline;
        sn_latch_1_next             <= sn_latch_1;
        sn_latch_2_next             <= sn_latch_2;
        sn_latch_3_next             <= sn_latch_3;
        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;
        meta_rd                     <= '0';
        rd                          <= '0';
        mem_opcode                  <= NOP;
        mem_op_start                <= '0';
        rd_guard                    := '0';
        update_endpoint_flags       <= (others => '0');
        hc_start                    <= '0';
        hc_opcode                   <= ADD_CACHE_CHANGE;
        hc_data_in                  <= (others => '0');
        hc_ready_out                <= '0';
        hc_valid_in                 <= '0';
        hc_last_word_in             <= '0';
        
        
        -- Last Word Latch Setter
        if (last_word_in = '1') then
            last_word_in_latch_next    <= '1';
        end if;
        
        case (meta_stage) is
            -- OPCODE
            when IDLE =>
                -- RESET
                deadline_next               <= TIME_INVALID;
                addr_next                   <= (others => '0');
                portn_next                  <= (others => '0');
                status_info_next            <= (others => '0');
                key_hash_rcvd_next          <= '0';
                expects_inline_qos_next     <= '0';
                is_meta_next                <= '0';
                
                -- Stale Entry search is initiated between every packet processing and when there is no packet to process
                if (stale_check = '0' or (empty = '1' and meta_empty = '1')) then
                    if (mem_op_done = '1') then
                        mem_opcode          <= FIND_STALE_ENDPOINT;
                        mem_op_start        <= '1';
                        
                        -- Stale Check Toggle (Setter)
                        stale_check_next    <= '1';
                        
                        stage_next          <= ENDPOINT_STALE_CHECK;
                    end if;
                -- Input FIFO Guard
                elsif (meta_empty = '0') then
                    meta_rd <= '1';
                    
                    -- Stale Check Toggle (Resetter)
                    stale_check_next    <= '0';
                    
                    -- Mark as METATRAFFIC
                    is_meta_next        <= '1';
                    
                    -- Latch Endpoint Metatraffic Opcode
                    meta_opcode_next    <= meta_data_in;
                    
                    stage_next  <= LATCH_GUIDPREFIX;
                    cnt_next    <= 0;
                -- Input FIFO Guard
                elsif (empty = '0') then
                    rd_guard        := '1';
                    
                    -- Stale Check Toggle (Resetter)
                    stale_check_next    <= '0';
                    
                    -- 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 payload_flag = '1') then
                        stage_next  <= SKIP_PACKET;
                    end if;
                end if;
            when LATCH_GUIDPREFIX =>
                -- Input FIFO Guard
                if ((is_meta = '1' and meta_empty = '0') or (is_meta = '0' and empty = '0')) then
                    if (is_meta = '1') then
                        meta_rd     <= '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)    <= meta_data_in;
                            else
                                guid_next(0)    <= data_in;
                            end if;
                        -- GUID Prefix 2/3
                        when 1 =>
                            if (is_meta = '1') then
                                guid_next(1)    <= meta_data_in;
                            else
                                guid_next(1)    <= data_in;
                            end if;
                        -- GUID Prefix 3/3
                        when 2 =>
                            if (is_meta = '1') then
                                guid_next(2)    <= meta_data_in;
                            else
                                guid_next(2)    <= data_in;
                            end if;
                            
                            if (is_meta = '1' and (meta_opcode = OPCODE_PARTICIPANT_UNMATCH or meta_opcode = OPCODE_LIVELINESS_UPDATE)) then
                                -- DONE Parsing
                                stage_next  <= INITIATE_ENDPOINT_SEARCH;
                            else
                                stage_next  <= LATCH_ENTITYID;
                            end if;
                    end case;
                end if;
            when LATCH_ENTITYID =>
                -- Input FIFO Guard
                if ((is_meta = '1' and meta_empty = '0') or (is_meta = '0' and empty = '0')) then
                    if (is_meta = '1') then
                        meta_rd         <= '1';
                        guid_next(3)    <= meta_data_in;
                    -- Memory Operation Guard
                    else
                        rd_guard        := '1';
                        guid_next(3)    <= data_in;
                    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;
                                stage_next      <= LATCH_ENDPOINT_DATA;
                            when OPCODE_ENDPOINT_UNMATCH =>
                                mem_op_start    <= '1';
                                mem_opcode      <= SEARCH_ENDPOINT;
                                stage_next      <= METATRAFFIC_OPERATION;
                            when OPCODE_PARTICIPANT_UNMATCH =>
                                mem_op_start    <= '1';
                                mem_opcode      <= FIND_FIRST_PARTICIPANT_ENDPOINT;
                                stage_next      <= METATRAFFIC_OPERATION;
                            when OPCODE_LIVELINESS_UPDATE =>
                                mem_op_start    <= '1';
                                mem_opcode      <= FIND_FIRST_PARTICIPANT_ENDPOINT;
                                stage_next      <= METATRAFFIC_OPERATION;
                            when others =>
                                assert FALSE report "Uknown metatraffic endpoint frame opcode." severity FAILURE;
                                null;
                        end case;
                    else
                        mem_op_start    <= '1';
                        mem_opcode      <= SEARCH_ENDPOINT;
                        
                        case (opcode) is
                            when SID_DATA =>
                                stage_next  <= LATCH_EXTRA_DATA;
                                cnt_next    <= 0;
                            when SID_HEARTBEAT =>
                                stage_next  <= LATCH_HEARTBEAT;
                                cnt_next    <= 0;
                            when SID_GAP =>
                                stage_next  <= LATCH_GAP;
                                cnt_next    <= 0;
                            when others =>
                                stage_next  <= SKIP_PACKET;
                        end case;
                    end if;
                end if;
            when LATCH_ENDPOINT_DATA =>
                -- Input FIFO Guard
                if (meta_empty = '0') then
                    meta_rd     <= '1';
                    cnt_next    <= cnt + 1;
                    
                    case (cnt) is
                        -- IPv4 Address
                        when 0 =>
                            addr_next  <= meta_data_in;
                        -- UDP Port & EXTRA Flags (Expects InLine QoS)
                        when 1 =>
                            portn_next              <= meta_data_in(WORD_WIDTH-1 downto WORD_WIDTH-UDP_PORT_WIDTH-1);
                            expects_inline_qos_next <= meta_data_in(0);
                            -- Calculate Lease Deadline
                            deadline_next           <= time + ENDPOINT_LEASE_DURATION(ID);
                            
                            stage_next  <= METATRAFFIC_OPERATION;
                        when others =>
                            null;
                    end case;
                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 /= MAX_ADDRESS) 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
                                update_endpoint_flags   <= ENDPOINT_DATA_FLAG; 
                                mem_opcode      <= UDPATE_ENDPOINT;
                                mem_op_start    <= '1';
                                stage_next      <= IDLE;
                            else
                                -- Insert Matched Remote Endpoint
                                mem_opcode      <= INSERT_ENDPOINT;
                                mem_op_start    <= '1';
                                stage_next      <= IDLE;
                            end if;
                        when OPCODE_ENDPOINT_UNMATCH =>
                            -- Endpoint not in Memory
                            if (mem_addr_base = MAX_ADDRESS) then
                                -- Ignore
                                stage_next  <= IDLE;
                            else
                                -- Output Guard
                                if (hc_ready_in = '1') then
                                    -- Propagate Removal
                                    hc_start        <= '1';
                                    hc_opcode       <= REMOVE_WRITER;
                                    hc_valid_in     <= '1';
                                    hc_data_in      <= std_logic_vector(to_unsigned(mem_pos, WORD_WIDTH));
                                    -- Wait until HC Acknowledgement
                                    if (hc_res = ACK) then
                                        -- Remove Unmatched Remote Endpoint
                                        mem_opcode      <= REMOVE_ENDPOINT;
                                        mem_op_start    <= '1';
                                        stage_next      <= IDLE;
                                    end if;
                                end if;
                            end if;
                        when OPCODE_PARTICIPANT_UNMATCH =>
                            -- No matches in memory
                            if (mem_addr_base = MAX_ADDRESS) then
                                -- DONE
                                stage_next  <= IDLE;
                            else
                                -- Output Guard
                                if (hc_ready_in = '1') then
                                    -- Propagate Removal
                                    hc_start        <= '1';
                                    hc_opcode       <= REMOVE_WRITER;
                                    hc_valid_in     <= '1';
                                    hc_data_in      <= std_logic_vector(to_unsigned(mem_pos, WORD_WIDTH));
                                    -- Wait until HC Acknowledgement
                                    if (hc_res = ACK) then
                                        -- Remove Unmatched Remote Endpoint
                                        mem_opcode      <= REMOVE_ENDPOINT;
                                        mem_op_start    <= '1';
                                        stage_next      <= INITIATE_NEXT_ENDPOINT_SEARCH;
                                    end if;
                                end if;
                            end if;
                        when OPCODE_LIVELINESS_UPDATE =>
                            -- No matches in memory
                            if (mem_addr_base = MAX_ADDRESS) then
                                -- DONE
                                stage_next  <= IDLE;
                            else
                                -- Renew Lease of Remote Endpoint
                                update_endpoint_flags   <= LEASE_DEADLINE_FLAG;
                                mem_opcode              <= UDPATE_ENDPOINT;
                                mem_op_start            <= '1';
                                stage_next              <= INITIATE_NEXT_ENDPOINT_SEARCH;
                            end if;
                        when others =>
                            assert FALSE report "Uknown metatraffic endpoint frame opcode." severity FAILURE;
                            null;
                    end case;
                    -- DONE
                    stage_next  <= SKIP_PACKET;
                end if;
            when INITIATE_NEXT_ENDPOINT_SEARCH =>
                -- Memory Operation Guard
                if (mem_op_done = '1') then
                    mem_opcode      <= FIND_NEXT_PARTICIPANT_ENDPOINT;
                    mem_op_start    <= '1';
                    stage_next      <= METATRAFFIC_OPERATION;
                end if;
            when LATCH_SRC_ADDR =>
                -- Input FIFO Guard
                if (empty = '0') then
                    rd_guard        := '1';
                    
                    -- Latch Source IP Address
                    addr_next   <= data_in;
                    
                    stage_next  <= LATCH_GUIDPREFIX;
                    cnt_next    <= 0;
                end if;
            when LATCH_EXTRA_DATA =>
                -- Input FIFO Guard
                if (empty = '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);
                        -- Sequence Number 2/2
                        when 1 =>
                            seq_nr_next(1)  <= unsigned(data_in);
                            -- Store Next Sequence Number
                            tmp_dw              := (0 => seq_nr(0), 1 => unsigned(data_in));
                            next_seq_nr_next    <= tmp_dw + 1;
                        -- Timestamp 1/2
                        when 2 =>
                            ts_next(0)      <= unsigned(data_in);
                        -- Timestamp 2/2
                        when 3 =>
                            ts_next(1)      <= unsigned(data_in);
                            
                            if (qos_flag = '1') then
                                stage_next  <= PROCESS_INLINE_QOS;
                            else
                                stage_next  <= INITIATE_HISTORY_CACHE_REQUEST;
                            end if;
                        when others => 
                            null;
                    end case;
                end if;
            when LATCH_HEARTBEAT =>
                -- Input FIFO Guard
                if (empty = '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);
                        -- First Sequence Number 2/2
                        when 1 =>
                            first_seq_nr_next(1)    <= unsigned(data_in);
                        -- Last Sequence Number 1/2
                        when 2 =>
                            last_seq_nr_next(0)     <= unsigned(data_in);
                        -- Last Sequence Number 2/2
                        when 3 =>
                            last_seq_nr_next(1)     <= unsigned(data_in);
                            
                            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 /= MAX_ADDRESS) then
                        -- No scheduled Heartbeat Response
                        if (mem_endpoint_data.res_time = 0) then
                            -- If current Sequence Number obsolete (removed from source history cache)
                            if (first_seq_nr > mem_endpoint_data.next_seq_nr and first_seq_nr <= last_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;
                                -- XXX: Assumes lease deadline will not be updated concurrently (Since they use the same latch)
                                deadline_next           <= time + ENDPOINT_HEARTBEAT_RESPONSE_DELAY(ID);
                                -- NOTE: Last Bit denotes if this is Response or Suppression Delay
                                deadline_next(1)(0)     <= '0';
                                update_endpoint_flags   <= NEXT_SEQ_NR_FLAG or RES_TIME_FLAG;
                            -- 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_opcode                  <= UPDATE_ENDPOINT;
                                mem_op_start                <= '1';
                                deadline_next               <= time + ENDPOINT_HEARTBEAT_RESPONSE_DELAY(ID);
                                -- NOTE: Last Bit denotes if this is Response or Suppression Delay
                                deadline_next(1)(0)         <= '0';
                                update_endpoint_flags   <= RES_TIME_FLAG;
                            end if;
                        -- Currently in Heartbeat Response Delay
                        elsif (mem_participant_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 and first_seq_nr <= last_seq_nr) then
                                -- Store new expected Sequence Number
                                next_seq_nr_next    <= first_seq_nr;
                                mem_op_start        <= '1';
                                mem_opcode          <= UPDATE_ENDPOINT;
                                update_endpoint_flags   <= NEXT_SEQ_NR_FLAG;
                            end if;
                        end if;
                    end if;
                end if;
            when LATCH_GAP =>
                -- Input FIFO Guard
                if (empty = '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);
                        -- GapStart Sequence Number  2/2
                        when 1 =>
                            gap_start_next(1)       <= unsigned(data_in);
                        -- GapList.Base 1/2
                        when 2 =>
                            gap_list_base_next(0)   <= unsigned(data_in);
                        -- GapList.Base 2/2
                        when 3 =>
                            gap_list_base_next(1)   <= unsigned(data_in);
                        -- ReaderSNState.NumBits
                        when 4 =>
                            gap_list_end_next   <= gap_list_base + to_integer(unsigned(data_in));
                            bitmap_cnt_next     <= unsigned(round_slv(data_in(log2c(MAX_BITMAP_WIDTH)-1 downto 0),bitmap_cnt'length));
                            cnt2_next           <= 0;
                        -- ReaderSNState.Bitmap
                        when 5 =>
                            -- Read Bitmap
                            if (cnt2 < bitmap_cnt) then
                                cnt2_next <= cnt2 + 1;
                                
                                bitmap_latch_next(cnt2) <= data_in_swapped;
                                
                                -- 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 /= 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;
                        update_endpoint_flags   <= 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 = '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 =>
                            -- QOS DONE
                            stage_next  <= INITIATE_HISTORY_CACHE_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 = '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
                            deadline_next   <= time + tmp_dw;
                            
                            -- DONE
                            stage_next  <= SKIP_PARAMETER;
                        when others => 
                            null;
                    end case;
                end if;
            when LATCH_KEY_HASH =>
                -- Input FIFO Guard
                if (empty = '0') then
                    rd_guard    := '1';
                    cnt_next    <= cnt + 1;
                    
                    case (cnt) is
                        -- Key Hash 1/4
                        when 0 =>
                            key_hash_next(0)    <= data_in;
                        -- Key Hash 2/4
                        when 1 =>
                            key_hash_next(1)    <= data_in;
                        -- Key Hash 3/4
                        when 2 =>
                            key_hash_next(2)    <= data_in;
                        -- Key Hash 4/4
                        when 3 =>
                            key_hash_next(3)    <= data_in;
                            
                            -- DONE
                            stage_next  <= SKIP_PARAMETER;
                        when others => 
                            null;
                    end case;
                end if;
            when LATCH_STATUS_INFO =>
                -- Input FIFO Guard
                if (empty = '0') then
                    rd_guard            := '1';
                    
                    status_info_next    <= data_in;
                    
                    -- DONE
                    stage_next  <= SKIP_PARAMETER;
                end if;
            when INITIATE_HISTORY_CACHE_REQUEST =>
                hc_start    <= '1';
                hc_opcode   <= ADD_CACHE_CHANGE;
                -- Wait until History Cache acknowledges request
                if (hc_res = ACK) then
                    stage_next  <= ADD_CACHE_CHANGE;
                    cnt_next    <= 0;
                end if;
            when ADD_CACHE_CHANGE =>
                if (hc_ready_in = '1') then
                    hc_valid_in    <= '1';
                    cnt_next        <= cnt + 1;
                    
                    case (cnt) is
                        -- Status Info
                        when 0 =>
                            hc_data_in     <= status_info;
                            hc_data_in(KEY_HASH_FLAG)  <= key_hash_rcvd;
                            hc_data_in(PAYLOAD_FLAG)   <= data_flag;
                        -- Timestamp 1/2
                        when 1 =>
                            hc_data_in     <= ts(0);
                        -- Timestamp 2/2
                        when 2 =>
                            hc_data_in     <= ts(1);
                        -- Lifespan Deadline 1/2
                        when 3 =>
                            hc_data_in     <= deadline(0);
                        -- Lifespan Deadline 2/2
                        when 4 =>
                            hc_data_in     <= deadline(1);
                            -- Skip Key Hash, if not received
                            if (key_hash_rcvd = '0') then
                                cnt_next    <= 9;
                            end if;
                        -- Key hash 1/4
                        when 5 =>
                            hc_data_in     <= key_hash(0);
                        -- Key Hash 2/4
                        when 6 =>
                            hc_data_in     <= key_hash(1);
                        -- Key Hash 3/4
                        when 7 =>
                            hc_data_in     <= key_hash(2);
                        -- Key hash 4/4
                        when 8 =>
                            hc_data_in     <= key_hash(3);
                        -- Endpoint Memory Position
                        when 9 =>
                            -- Wait for Endpoint Search
                            if (mem_op_done = '1') then
                                
                                -- TODO: Assert mem_pos range fits in CDR_LONG
                                hc_data_in  <= std_logic_vector(to_unsigned(mem_pos, CDR_LONG_WIDTH));
                                
                                -- Payload exists
                                if (data_flag = '1' or key_flag = '1') then
                                    stage_next      <= PUSH_PAYLOAD;
                                else
                                    -- DONE
                                    hc_last_word_in <= '1';
                                    stage_next      <= FINALIZE_HISTORY_CACHE_REQUEST;
                                end if;
                            else
                                -- Keep State
                                hc_valid_in <= '0';
                                cnt_next    <= cnt;
                            end if;
                        when others =>
                            null;
                    end case;
                end if;
            when PUSH_PAYLOAD =>
                -- Input/Output Guard
                if (empty = '0' and hc_ready_in = '1') then
                    rd_guard        := '1';
                    hc_valid_in    <= '1';
                    
                    -- Push Payload to History Cache
                    hc_data_in     <= data_in;
                    
                    -- Exit Condition
                    if (last_word_in = '1') then
                        hc_last_word_in <= '1';
                        stage_next      <= FINALIZE_HISTORY_CACHE_REQUEST;
                    end if;
                end if;
            when FINALIZE_HISTORY_CACHE_REQUEST =>
                -- NOTE: Memory is already in done state from previous state (ADD_CACHE_CHANGE)
                assert (mem_op_done = '1') report "FINALIZE_HISTORY_CACHE_REQUEST precondition not met. mem_op_done /= '1'" severity FAILURE;
                -- Wai for History Cache Response
                if (hc_res /= UNDEFINED) then
                    -- Operation was Accepted
                    if (hc_res = ACCEPTED) then
                        -- Update next sequence number and renew Lease
                        mem_op_start            <= '1';
                        mem_opcode              <= UPDATE_ENDPOINT;
                        deadline_next           <= time + ENDPOINT_LEASE_DURATION(ID);
                        update_endpoint_flags   <= NEXT_SEQ_NR_FLAG or LEASE_DEADLINE_FLAG;
                    end if;
                    -- NOTE: In case the operation was unsucessfull (e.g. reached Resource Limits), the endpoint is not updated and the 
                    --       Sequence Number is thus not "acknowledged".
                    -- DONE
                    stage_next  <= SKIP_PACKET;
                end if;
            when ENDPOINT_STALE_CHECK =>
                -- Wait for Stale Search to finish
                if (mem_op_done = '1') then
                    -- Found Stale Entry
                    if (mem_addr_base /= MAX_ADDRESS) then
                        -- Endpoint Lease Expired
                        -- NOTE:    The mem_endpoint_data is zero initialized on lease expiration, so we check if the address is set
                        if (mem_endpoint_data.addr = IPv4_ADDRESS_INVALID) then
                            -- Remove Participant
                            mem_opcode      <= REMOVE_ENDPOINT;
                            mem_op_start    <= '1';
                            stage_next      <= IDLE;
                        -- Response Time Reached
                        else
                            -- If Suppression Delay passed, zero the time
                            if(mem_endpoint_data.res_time(1)(0) = '1') then
                                -- Zero Response Time
                                mem_opcode                  <= UPDATE_ENDPOINT;
                                deadline_next               <= TIME_ZERO;
                                update_participant_flags    <= RES_TIME_FLAG;
                                mem_op_start                <= '1';
                                -- DONE
                                stage_next  <= IDLE;
                            -- If Response Delay Passed
                            else
                                 -- Set Heartbeat Suppression Time
                                if (ENDPOINT_HEARTBEAT_SUPPRESSION_DELAY(ID) /= 0) then
                                    -- Set Heartbeat Suppression Time
                                    deadline_next       <= time + ENDPOINT_HEARTBEAT_SUPPRESSION_DELAY(ID);
                                    -- NOTE: Last Bit denotes if this is Response or Suppression Delay
                                    deadline_next(1)(0) <= '1';
                                else
                                    -- Zero Heartbeat Response Time
                                    deadline_next       <= TIME_ZERO;
                                end if;
                                mem_opcode                      <= UPDATE_PARTICIPANT;
                                update_participant_flags_next   <= RES_TIME_FLAG;
                                mem_op_start                    <= '1';
                                -- Send ACKNACK
                                -- Increment Heartbeat/Acknack Counter
                                count_next                      <= count + 1;
                                stage_next                      <= SEND_HEADER;
                                return_stage_next               <= SEND_ACKNACK;
                                cnt_next                        <= 0;
                            end if;
                        end if;
                    -- No Stale Entry Found
                    else
                        -- DONE
                        stage_next  <= IDLE;
                    end if;
                end if;
            when SEND_HEADER =>
                if (rtps_full = '0') then
                    wr_sig      <= '1';
                    cnt_next    <= cnt + 1;
                    
                    case (cnt) is
                        -- OUTPUT HEADER
                        -- Src IPv4 Address
                        when 0 =>
                            data_out  <= DEFAULT_IPv4_ADDRESS;
                        -- Dest IPv4 Address
                        when 1 =>
                            data_out  <= mem_endpoint_data.addr;
                        -- Src and Dest UDPv4 Ports
                        when 2 =>
                            data_out  <= USER_IPv4_UNICAST_PORT & mem_participant_data.meta_port;
                        -- RTPS MESSAGE HEADER
                        when 3 =>
                            data_out  <= PROTOCOL_RTPS;
                        when 4 =>
                            data_out  <= PROTOCOLVERSION_2_4 & VENDORID;
                        when 5 =>
                            data_out  <= GUIDPREFIX(0);
                        when 6 =>
                            data_out  <= GUIDPREFIX(1);
                        when 7 =>
                            data_out  <= GUIDPREFIX(2);
                            -- Continue with respective RTPS Submessage
                            stage_next  <= return_stage;
                            cnt_next    <= 0;
                        when others =>
                            null;
                    end case;
                end if;
            when SEND_ACKNACK =>
                if (rtps_full = '0') then
                    wr_sig      <= '1';
                    cnt_next    <= cnt + 1;
                    
                    case (cnt) is
                        -- ACKNACK RTPS SUBMESSAGE
                        -- RTPS Submessage Header
                        when 0 =>
                            data_out  <= SID_ACKNACK & "00000010" & std_logic_vector(to_unsigned(28, SUBMESSAGE_LENGTH_WIDTH));
                        -- Reader Entity ID
                        when 1 =>
                            data_out  <= ENTITYID(ID);
                        -- Writer Entity ID
                        when 2 =>
                            data_out  <= ENTITYID_UNKNOWN;
                        -- Sequence Number Set (Bitmap Base 1/2)
                        when 3 =>
                            data_out  <= std_logic_vector(mem_endpoint_data.next_seq_nr(0));
                        -- Sequence Number Set (Bitmap Base 2/2)
                        when 4 =>
                            data_out  <= std_logic_vector(mem_endpoint_data.next_seq_nr(1));
                        -- Sequence Number Set (NumBits)
                        when 5 =>
                            data_out  <= 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  <= (others => '1');
                        -- Count
                        when 7 =>
                            data_out  <= std_logic_vector(count);
                            last_word_out   <= '1';
                            
                            -- DONE
                            stage_next      <= IDLE;
                        when others =>
                            null;
                    end case;
                end if;
            when SKIP_PARAMETER =>
                -- Consumed last word of Packet
                if (last_word_in_latch = '1' and last_word_in = '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 is already showing the next parameter
                    stage_next          <= PROCESS_PL;
                    -- Reset Parameter End
                    parameter_end_next  <= (others => '1');
                -- Input FIFO Guard
                elsif (empty = '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');
                
                -- Stale Check Exit
                if (stale_check = '1') then
                    -- DONE
                    stage_next  <= IDLE;
                -- Consumed last word of Packet
                elsif (last_word_in_latch = '1' and last_word_in = '0') then
                    -- Reset Last Word In Latch
                    last_word_in_latch_next <= '0';
                    -- DONE
                    stage_next              <= IDLE;
                -- Input FIFO Guard
                elsif (empty = '0') then
                    -- Skip-Read
                    rd_guard    := '1';
                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 = '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;
    
    mem_ctrl_prc : process(all)
        variable tmp    : unsigned(mem_addr_base'range) := (others => '0');
        variable tmp2   : unsigned(mem_addr_base'range) := (others => '0');
        variable tmp3   : unsigned(mem_addr_base'range) := (others => '0');
        variable tmp_dw : DOUBLE_WORD_ARRAY := (others => (others => '0'));
    begin
        -- DEFAULT Registered
        mem_stage_next              <= mem_stage;
        mem_addr_base_next          <= mem_addr_base;
        mem_addr_next               <= mem_addr;
        mem_cnt_next                <= mem_cnt;
        last_addr_next              <= last_addr;
        mem_endpoint_data_next      <= mem_endpoint_data;
        mem_guidprefix_next         <= mem_guidprefix;
        max_endpoint_addr_next      <= max_endpoint_addr;
        reset_max_pointer_next      <= reset_max_pointer;
        mem_long_latch_next         <= mem_long_latch;
        mem_ctrl_data_next          <= mem_ctrl_data;
        mem_pos_next                <= mem_pos;
        -- DEFAULT Unregistered
        mem_write_data              <= (others => '0');
        mem_op_done                 <= '0';
        mem_rd                      <= '0';
        mem_wr                      <= '0';
        
        
        case (mem_stage) is
            when IDLE =>
                mem_op_done             <= '1';
                reset_max_pointer_next  <= '0';
                is_psearch_next         <= '0';
                
                if (mem_op_start = '1') then
                    -- Latch Signals needed for Mermory Operation (Use _next signals, because some signals are set in same clk)
                    mem_ctrl_data_next      <= (
                        guid                => guid_next,
                        addr                => addr_next,
                        portn               => portn_next,
                        expects_inline_qos  => expects_inline_qos_next,
                        deadline            => deadline_next,
                        next_seq_nr         => next_seq_nr_next,
                        update_flags        => update_endpoint_flags_next
                    );
                    
                    case(mem_opcode) is
                        when SEARCH_ENDPOINT =>
                            mem_addr_base_next  <= FIRST_ENDPOINT_ADDRESS;
                            mem_addr_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_addr_next       <= FIRST_ENDPOINT_ADDRESS;
                            mem_pos_next        <= 0;
                            mem_stage_next      <= FIND_ENDPOINT_SLOT;
                            mem_cnt_next        <= 0;
                        when UPDATE_ENDPOINT =>
                            if ((update_endpoint_flags and ENDPOINT_DATA_FLAG) = ENDPOINT_DATA_FLAG) then
                                mem_stage_next  <= UPDATE_ENDPOINT;
                                mem_addr_next   <= mem_addr_base + 4;
                                mem_cnt_next    <= 0;
                            elsif ((update_endpoint_flags and LEASE_DEADLINE_FLAG) = LEASE_DEADLINE_FLAG) then
                                mem_stage_next  <= UPDATE_ENDPPOINT;
                                mem_addr_next   <= mem_addr_base + 6;
                                mem_cnt_next    <= 2;
                            elsif ((update_endpoint_flags and RES_TIME_FLAG) = RES_TIME_FLAG) then
                                mem_stage_next  <= UPDATE_ENDPOINT;
                                mem_addr_next   <= mem_addr_base + 8;
                                mem_cnt_next    <= 4;
                            elsif ((update_endpoint_flags and NEXT_SEQ_NR_FLAG) = NEXT_SEQ_NR_FLAG) then
                                mem_stage_next  <= UPDATE_ENDPOINT;
                                mem_addr_next   <= mem_addr_base + 10;
                                mem_cnt_next    <= 6;
                            end if;
                        when REMOVE_ENDPOINT =>
                            mem_addr_next       <= mem_addr_base;
                            mem_stage_next      <= REMOVE_ENDPOINT;
                            mem_cnt_next        <= 0;
                        when FIND_STALE_ENDPOINT =>
                            mem_addr_base_next  <= FIRST_ENDPOINT_ADDRESS;
                            mem_addr_next       <= FIRST_ENDPOINT_ADDRESS;
                            mem_pos_next        <= 0;
                            mem_stage_next      <= FIND_STALE_ENDPOINT;
                            mem_cnt_next        <= 0;
                        when GET_FIRST_ENDPOINT =>
                            mem_addr_base_next  <= FIRST_ENDPOINT_ADDRESS;
                            mem_addr_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
                                -- Reached End of Memory, No match
                                tmp                 := mem_addr_base + ENDPOINT_FRAME_SIZE;
                                mem_addr_base_next  <= tmp;
                                mem_addr_next       <= tmp;
                                mem_pos_next        <= mem_pos + 1;
                                mem_stage_next      <= GET_NEXT_ENDPOINT;
                                mem_cnt_next        <= 0;
                            else
                                mem_addr_base_next       <= MAX_ADDRESS;
                            end if;
                        when FIND_FIRST_PARTICIPANT_ENDPOINT =>
                            mem_addr_base_next  <= FIRST_ENDPOINT_ADDRESS;
                            mem_addr_next       <= FIRST_ENDPOINT_ADDRESS;
                            mem_pos_next        <= 0;
                            mem_stage_next      <= SEARCH_ENDPOINT;
                            mem_cnt_next        <= 0;
                            is_psearch_next     <= '1';
                        when FIND_NEXT_PARTICIPANT_ENDPOINT =>
                            -- Memory Bound Guard
                            if (mem_addr_base /= max_endpoint_addr) then
                                -- Reached End of Memory, No match
                                tmp                 := mem_addr_base + ENDPOINT_FRAME_SIZE;
                                mem_addr_base_next  <= tmp;
                                mem_addr_next       <= tmp;
                                mem_pos_next        <= mem_pos + 1;
                                mem_stage_next      <= SEARCH_ENDPOINT;
                                mem_cnt_next        <= 0;
                                is_psearch_next     <= '1';
                            else
                                mem_addr_base_next  <= MAX_ADDRESS;
                            end if;
                        when others =>
                            null;
                    end case;
                end if;
            when SEARCH_ENDPOINT =>
                mem_rd          <= '1';
                mem_cnt_next    <= mem_cnt + 1;
                mem_addr_next   <= mem_addr + 1;
                
                -- Next Endpoint Frame Address
                tmp := mem_addr_base + ENDPOINT_FRAME_SIZE;
                
                case (mem_cnt) is
                    -- Preload
                    when 0 =>
                        null;
                    -- Entity ID
                    when 1 =>
                        -- No Match (Ignore Entity ID match on Participant Endpoint Search, but skip empty Slot)
                        if ((mem_read_data /= mem_ctrl_data.guid(3) and is_psearch = '0') or (mem_read_data = ENTITYID_UNKNOWN)) then
                            -- Reached End of Memory, No Match
                            if (mem_addr_base = max_endpoint_addr) then
                                mem_addr_base_next  <= MAX_ADDRESS; --No match
                                -- DONE
                                mem_stage_next      <= IDLE;
                            else
                                -- Continue Search
                                mem_addr_next       <= tmp;
                                mem_addr_base_next  <= tmp;
                                mem_pos_next        <= mem_pos + 1;
                                mem_cnt_next        <= 0;
                            end if;
                        end if;
                    -- GUID Prefix 1/3
                    when 2 =>
                        -- No Match
                        if (mem_read_data /= mem_ctrl_data.guid(0)) then                            
                            -- Reached End of Memory, No Match
                            if (mem_addr_base = max_endpoint_addr) then
                                mem_addr_base_next  <= MAX_ADDRESS; --No match
                                -- DONE
                                mem_stage_next      <= IDLE;
                            else
                                -- Continue Search
                                mem_addr_next       <= tmp;
                                mem_addr_base_next  <= tmp;
                                mem_pos_next        <= mem_pos + 1;
                                mem_cnt_next        <= 0;
                            end if;
                        end if;
                    -- GUID Prefix 2/3
                    when 3 =>
                        -- No Match
                        if (mem_read_data /= mem_ctrl_data.guid(1)) then
                            -- Reached End of Memory, No Match
                            if (mem_addr_base = max_endpoint_addr) then
                                mem_addr_base_next  <= MAX_ADDRESS; --No match
                                -- DONE
                                mem_stage_next      <= IDLE;
                            else
                                -- Continue Search
                                mem_addr_next       <= tmp;
                                mem_addr_base_next  <= tmp;
                                mem_pos_next        <= mem_pos + 1;
                                mem_cnt_next        <= 0;
                            end if;
                        end if;
                    -- GUID Prefix 3/3
                    when 4 =>
                        -- No Match
                        if (mem_read_data /= mem_ctrl_data.guid(2)) then
                            -- Reached End of Memory, No Match
                            if (mem_addr_base = max_endpoint_addr) then
                                mem_addr_base_next  <= MAX_ADDRESS; --No match
                                -- DONE
                                mem_stage_next      <= IDLE;
                            else
                                -- Continue Search
                                mem_addr_next       <= tmp;
                                mem_addr_base_next  <= tmp;
                                mem_pos_next        <= mem_pos + 1;
                                mem_cnt_next        <= 0;
                            end if;
                        -- Match
                        else
                            mem_addr_base_next   <= mem_addr_base;
                            -- Fetch Endpoint Data
                            mem_stage_next  <= GET_ENDPOINT_DATA;
                            mem_cnt_next    <= 1; -- No preload needed
                        end if;
                    when others =>
                        null;
                end case;
            when GET_ENDPOINT_DATA =>
                mem_rd          <= '1';
                mem_cnt_next    <= mem_cnt + 1;
                mem_addr_next   <= mem_addr + 1;
                
                case (mem_cnt) is
                    -- Memory Preload
                    when 0 =>
                        null;
                    -- IPv4 Address
                    when 1 =>
                        mem_endpoint_data_next.addr                     <= mem_read_data;
                    -- UDPv4 Ports
                    when 2 =>
                        mem_endpoint_data_next.portn                    <= mem_read_data(31 downto 16);
                        mem_endpoint_data_next.expects_inline_qos       <= mem_read_data(0);
                    -- Lease Deadline 1/2
                    when 3 =>
                        mem_endpoint_data_next.lease_deadline(0)        <= unsigned(mem_read_data);
                    -- Lease Deadline 2/2
                    when 4 =>
                        mem_endpoint_data_next.lease_deadline(1)        <= unsigned(mem_read_data);
                    -- Response/Suppression Time 1/2
                    when 5 =>
                        mem_endpoint_data_next.res_time(0)              <= unsigned(mem_read_data);
                    -- Response/Suppression Time 2/2
                    when 6 =>
                        mem_endpoint_data_next.res_time(1)              <= unsigned(mem_read_data);
                    -- Next Sequence Number 1/2
                    when 7 =>
                        mem_endpoint_data_next.next_seq_nr(0)           <= unsigned(mem_read_data);
                    -- Next Sequence Number 2/2
                    when 8 =>
                        mem_endpoint_data_next.next_seq_nr(1)           <= unsigned(mem_read_data);
                        -- DONE
                        mem_stage_next  <= IDLE;
                    when others =>
                        null;
                end case;
            when INSERT_ENDPOINT =>
                mem_wr          <= '1';
                mem_addr_next   <= mem_addr + 1;
                mem_cnt_next    <= mem_cnt + 1;
                
                case (mem_cnt) is
                    -- Entity ID
                    when 0 =>
                        mem_write_data  <= mem_ctrl_data.guid(3);
                    -- GUID Prefix 1/3
                    when 1 =>
                        mem_write_data  <= mem_ctrl_data.guid(0);
                    -- GUID Prefix 2/3
                    when 2 =>
                        mem_write_data  <= mem_ctrl_data.guid(1);
                    -- GUID Prefix 3/3
                    when 3 =>
                        mem_write_data  <= mem_ctrl_data.guid(2);
                    -- IPv4 Address
                    when 4 =>
                        mem_write_data  <= mem_ctrl_data.addr;
                    -- UDPv4 Ports
                    when 5 =>
                        mem_write_data  <= mem_ctrl_data.portn & (0 => mem_ctrl_data.expects_inline_qos, 16 downto 1 => '0');
                    -- Lease Deadline 1/2
                    when 6 =>
                        mem_write_data  <= std_logic_vector(mem_ctrl_data.deadline(0));
                    -- Lease Deadline 2/2
                    when 7 =>
                        mem_write_data  <= std_logic_vector(mem_ctrl_data.deadline(1));
                    -- Response/Suppression Time 1/2
                    when 8 =>
                        mem_write_data  <= (others => '0');
                    -- Response/Suppression Time 2/2
                    when 9 =>
                        mem_write_data  <= (others => '0');
                    -- Next Sequence Number 1/2
                    when 10 =>
                        mem_write_data  <= (others => '0');
                    -- Next Sequence Number 2/2
                    when 11 =>
                        mem_write_data  <= to_unsigned(1,CDR_LONG_WIDTH);
                        -- DONE
                        mem_stage_next  <= IDLE;
                    when others => 
                        null;
                end case;
            when UPDATE_ENDPOINT =>
                mem_cnt_next    <= mem_cnt + 1;
                mem_addr_next   <= mem_addr + 1;
                
                case (mem_cnt) is
                    -- IPv4 Address
                    when 0 =>
                        mem_write_data  <= mem_ctrl_data.addr;
                        if ((mem_ctrl_data.update_flags and ENDPOINT_DATA_FLAG) = ENDPOINT_DATA_FLAG) then
                            mem_wr  <= '1';
                        end if;
                    -- UDPv4 Ports
                    when 1 =>
                        mem_write_data  <= mem_ctrl_data.portn & (0 => mem_ctrl_data.expects_inline_qos, 16 downto 1 => '0');
                        if ((mem_ctrl_data.update_flags and ENDPOINT_DATA_FLAG) = ENDPOINT_DATA_FLAG) then
                            mem_wr  <= '1';
                        end if;
                        -- If nothing else to update
                        if ((mem_ctrl_data.update_flags and (LEASE_DEADLINE_FLAG or NEXT_SEQ_NR_FLAG or RES_TIME_FLAG)) = (mem_ctrl_data.update_flags'range => '0')) then
                            -- DONE
                            mem_stage_next  <= IDLE;
                        end if;
                    -- Lease Deadline 1/2
                    when 2 =>
                        mem_write_data  <= std_logic_vector(mem_ctrl_data.deadline(0));
                        if ((mem_ctrl_data.update_flags and LEASE_DEADLINE_FLAG) = LEASE_DEADLINE_FLAG) then
                            mem_wr  <= '1';
                        end if;
                    -- Lease Deadline 2/2
                    when 3 =>
                        mem_write_data  <= std_logic_vector(mem_ctrl_data.deadline(1));
                        if ((mem_ctrl_data.update_flags and LEASE_DEADLINE_FLAG) = LEASE_DEADLINE_FLAG) then
                            mem_wr  <= '1';
                        end if;
                        -- If nothing else to update
                        if ((mem_ctrl_data.update_flags and (NEXT_SEQ_NR_FLAG or RES_TIME_FLAG)) = (mem_ctrl_data.update_flags'range => '0')) then
                            -- DONE
                            mem_stage_next  <= IDLE;
                        end if;
                    -- Response/Suppression Time 1/2
                    when 4 =>
                        mem_write_data  <= std_logic_vector(mem_ctrl_data.deadline(0));
                        if ((mem_ctrl_data.update_flags and RES_TIME_FLAG) = RES_TIME_FLAG) then
                            mem_wr  <= '1';
                        end if;
                    -- Response/Suppression Time 2/2
                    when 5 =>
                        mem_write_data  <= std_logic_vector(mem_ctrl_data.deadline(1));
                        if ((mem_ctrl_data.update_flags and RES_TIME_FLAG) = RES_TIME_FLAG) then
                            mem_wr  <= '1';
                        end if;
                        -- If nothing else to update
                        if ((mem_ctrl_data.update_flags and (NEXT_SEQ_NR_FLAG)) = (mem_ctrl_data.update_flags'range => '0')) then
                            -- DONE
                            mem_stage_next  <= IDLE;
                        end if;
                    -- Next Sequence Number 1/2
                    when 6 =>
                        mem_write_data  <= std_logic_vector(mem_ctrl_data.next_seq_nr(0));
                        if ((mem_ctrl_data.update_flags and NEXT_SEQ_NR_FLAG) = NEXT_SEQ_NR_FLAG) then
                            mem_wr  <= '1';
                        end if;
                    -- Next Sequence Number 2/2
                    when 7 =>
                        mem_write_data  <= std_logic_vector(mem_ctrl_data.next_seq_nr(1));
                        if ((mem_ctrl_data.update_flags and NEXT_SEQ_NR_FLAG) = NEXT_SEQ_NR_FLAG) then
                            mem_wr  <= '1';
                        end if;
                        -- DONE
                        mem_stage_next  <= IDLE;
                    when others =>
                        null;
                end case;
            when REMOVE_ENDPOINT =>                
                -- Mark with ENTITYID_UNKNOWN to mark slot empty
                mem_wr          <= '1';
                mem_write_data  <= ENTITYID_UNKNOWN;
                
                -- Reset MAX Endpoint Pointer
                mem_addr_base_next      <= FIRST_ENDPOINT_ADDRESS;
                mem_addr_next           <= FIRST_ENDPOINT_ADDRESS;
                mem_pos_next            <= 0;
                reset_max_pointer_next  <= '1';
                last_addr_next          <= (others => '0');
                mem_stage_next          <= FIND_ENDPOINT_SLOT;
                mem_cnt_next            <= 0;
            when FIND_ENDPOINT_SLOT =>
                mem_rd  <= '1';
                mem_addr_next   <= mem_addr + 1;
                mem_cnt_next    <= mem_cnt + 1;
                
                -- Next Endpoint Frame Address
                tmp := mem_addr_base + ENDPOINT_FRAME_SIZE;
                
                case (mem_cnt) is
                    -- Preload
                    when 0 =>
                        null;
                    -- Entity ID
                    when 1 =>
                        -- Slot Occupied
                        if (mem_read_data /= ENTITYID_UNKNOWN) then
                            -- Reached end of Endpoint Memory Area
                            if (mem_addr_base = max_endpoint_addr) then
                                -- We are in the middle of resetting the MAX Endpoint Pointer
                                if (reset_max_pointer = '1') then
                                    -- No Change
                                    mem_stage_next  <= IDLE;
                                -- MEMORY FULL
                                elsif (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   <= MAX_ADDRESS;
                                else
                                    -- Extend Endpoint Memory Area
                                    -- NOTE: "max_endpoint_addr" points to the first address of last Endpoint Frame
                                    max_endpoint_addr_next   <= tmp;
                                    -- Populate Endpoint Slot
                                    mem_stage_next      <= INSERT_ENDPOINT;
                                    mem_addr_base_next  <= tmp;
                                    mem_addr_next       <= tmp;
                                    mem_pos_next        <= mem_pos + 1;
                                    mem_addr_base_next  <= tmp;
                                    mem_cnt_next        <= 0;
                                end if;
                            else
                                -- Latch last occupied Endpoint Slot
                                last_addr_next      <= mem_addr_base;
                                -- Continue Search
                                mem_addr_next       <= tmp;
                                mem_addr_base_next  <= tmp;
                                mem_pos_next        <= mem_pos + 1;
                                mem_cnt_next        <= 0;
                            end if;
                        -- Slot Empty
                        else
                            -- We are in the middle of resetting the MAX Endpoint Pointer
                            if (reset_max_pointer = '1') then
                                -- 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;
                                else
                                    -- Continue Search
                                    mem_addr_next       <= tmp;
                                    mem_addr_base_next  <= tmp;
                                    mem_pos_next        <= mem_pos + 1;
                                    mem_cnt_next        <= 0;
                                end if;                                
                            else
                                -- Populate Endpoint Slot
                                mem_stage_next      <= INSERT_ENDPOINT;
                                mem_addr_next       <= mem_addr_base;
                                mem_cnt_next        <= 0;
                            end if;
                        end if;
                    when others =>
                        null;
                end case;
            when GET_NEXT_ENDPOINT =>
                mem_rd          <= '1';
                mem_cnt_next    <= mem_cnt + 1;
                mem_addr_next   <= mem_addr + 1;
                
                -- Next Endpoint Frame Address
                tmp     := mem_addr_base + ENDPOINT_FRAME_SIZE;
                -- Beginning of Endpoint Data
                tmp2    := mem_addr_base + 4;
                
                case (mem_cnt) is
                    -- Preload
                    when 0 =>
                        null;
                    -- Entity ID
                    when 1 =>
                        -- Slot Occupied
                        if (mem_read_data /= ENTITYID_UNKNOWN) then
                            -- Get Endpoint Data
                            mem_addr_next       <= tmp2;
                            mem_stage_next      <= GET_ENDPOINT_DATA;
                            mem_cnt_next        <= 0;
                        -- Slot Empty
                        else
                            -- Reached End of Memory, No Match
                            if (mem_addr_base = max_endpoint_addr) then
                                mem_addr_base_next   <= MAX_ADDRESS; --No match
                                -- DONE
                                mem_stage_next  <= IDLE;
                            else
                                -- Continue Search
                                mem_addr_next       <= tmp;
                                mem_addr_base_next  <= tmp;
                                mem_pos_next        <= mem_pos + 1;
                                mem_cnt_next        <= 0;
                            end if;
                        end if;
                    when others =>
                        null;
                end case;
            when FIND_STALE_ENDPOINT =>
                mem_rd          <= '1';
                mem_cnt_next    <= mem_cnt + 1;
                mem_addr_next   <= mem_addr + 1;
                
                -- Next Endpoint Frame Address
                tmp             := mem_addr_base + ENDPOINT_FRAME_SIZE;
                -- Beginning of Endpoint Data
                tmp2            := mem_addr_base + 4;
                -- Beginning of Lease Deadline
                tmp3            := mem_addr_base + 6;
                
                case (mem_cnt) is
                    -- Preload
                    when 0 =>
                        null;
                    -- Entity ID
                    when 1 =>
                        -- Slot Occupied
                        if (mem_read_data /= ENTITYID_UNKNOWN) then
                            -- Jump to Stale Check
                            mem_addr_next   <= tmp3;
                            mem_cnt_next    <= 2;
                        -- Slot Empty
                        else
                            -- Reached End of Memory, No Match
                            if (mem_addr_base = max_endpoint_addr) then
                                mem_addr_base_next   <= MAX_ADDRESS; --No match
                                -- DONE
                                mem_stage_next  <= IDLE;
                            else
                                -- Continue Search
                                mem_addr_next       <= tmp;
                                mem_addr_base_next  <= tmp;
                                mem_pos_next        <= mem_pos + 1;
                                mem_cnt_next        <= 0;
                            end if;
                        end if;
                    -- Preload
                    when 2 =>
                        null;
                    -- Lease Deadline 1/2
                    when 3 =>
                        mem_long_latch_next <= mem_read_data;
                    -- Lease Deadline 2/2
                    when 4 =>
                        tmp_dw  := (0 => unsigned(mem_long_latch), 1 => unsigned(mem_read_data));
                        -- Lease Deadline passed
                        if (tmp_dw < time) then
                            -- Mark Endpoint as stale
                            mem_endpoint_data_next  <= ZERO_ENDPOINT_DATA;
                            -- DONE
                            mem_stage_next          <= IDLE;
                        end if;
                    -- Response/Suppression Time 1/2
                    when 5 =>
                        mem_long_latch_next <= mem_read_data;
                    -- Response/Suppression Time 2/2
                    when 6 =>
                        tmp_dw  := (0 => unsigned(mem_long_latch), 1 => unsigned(mem_read_data));
                        -- Response/Suppression Time passed
                        if (tmp_dw /= 0 and tmp_dw < time) then
                            -- Mark Endpoint and get Endpoint Data
                            mem_addr_next   <= tmp2;
                            mem_stage_next  <= GET_ENDPOINT_DATA;
                            mem_cnt_next    <= 0;
                        -- Endpoint not Stale
                        else
                            -- Reached End of Memory, No Match
                            if (mem_addr_base = max_endpoint_addr) then
                                mem_addr_base_next  <= MAX_ADDRESS; --No match
                                -- DONE
                                mem_stage_next      <= IDLE;
                            else
                                -- Continue Search
                                mem_addr_next       <= tmp;
                                mem_addr_base_next  <= tmp;
                                mem_pos_next        <= mem_pos + 1;
                                mem_cnt_next        <= 0;
                            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 is high
            elsif (rd_sig = '1') then
                read_cnt    <= read_cnt + 1;
            end if;
        end if;
    end process;
    
end architecture;
