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

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

-- TODO: Initialise RAM to zeroes
-- TODO: Skip Packet while we are waiting for memory operation to complete
-- FIXME: ACNACK response does not nack, which is necessary to get the missing SNs.
-- TODO: Could we overwrite the data (pre-GUIDPrefix) while the memory is using them before the first memory guard? (Add test case)
-- TODO: Parse inlineQOS HASH_KEY.

entity rtps_builtin_endpoint is
    generic (
        MAX_REMOTE_PARTICIPANTS : natural := 50
    );
    port (
        clk             : in std_logic;                                             -- Input Clock
        reset           : in std_logic;                                             -- Synchronous Reset
        empty           : in std_logic;                                             -- Input FIFO empty flag
        rd              : out std_logic;                                            -- Input FIFO read signal
        data_in         : in std_logic_vector(WORD_WIDTH-1 downto 0);               -- Input FIFO data signal
        data_out        : out std_logic_vector(WORD_WIDTH-1 downto 0);
        last_word_in    : in std_logic;
        time            : in TIME_TYPE;
        endpoint_full   : in std_logic_vector(0 to NUM_ENDPOINTS-1);
        endpoint_wr     : out std_logic_vector(0 to NUM_ENDPOINTS-1);
        rtps_wr         : out std_logic;
        rtps_full       : in std_logic;
        last_word_out   : out std_logic;
        alive           : in std_logic_vector(0 to NUM_ENDPOINTS-1)
    );
end entity;

architecture arch of rtps_builtin_endpoint is
    
    --*****COMPONENT DECLARATION******
    -- The RAM is used for storing remote Participant and Endpoint Data.
    -- Participant Data is stored from low to high and Endpoint Data is stored from high to low.
    -- The RAM should have a READ and WRITE latency of 1 clock cycle.
    component single_port_ram is
        generic (
            ADDR_WIDTH      : natural := 8;
            DATA_WIDTH      : natural := 12;
            MEMORY_DEPTH    : natural := 256
            
        );
        port (
            clk         : in std_logic;
            addr        : in std_logic_vector(ADDR_WIDTH-1 downto 0);
            wen         : in std_logic;
            ren         : in std_logic;
            wr_data     : in std_logic_vector(DATA_WIDTH-1 downto 0);
            rd_data     : out std_logic_vector(DATA_WIDTH-1 downto 0)
        );
    end component;
    
    --*****CONSTANT DECLARATION*****
    -- Width of extra_flags signal
    constant EXTRA_FLAGS_WIDTH : natural := 4;
    
    --*****TYPE DECLARATION*****
    -- FSM states. Explained below in detail
    type STAGE_TYPE is (IDLE, PACKET_HEADER, PACKET_SRC_ADDR, PACKET_SRC_ENTITYID, PACKET_SRC_GUIDPREFIX, PACKET_DEST_ENTITYID, 
        CHECK_SRC_ENTITYID, LATCH_SEQ_NR, PROCESS_DATA, PROCESS_MESSAGE, PROCESS_GAP, PROCESS_GAP_SEQUENCE_NUMBERS, PROCESS_PL, CHECK_DEFAULT,
        LATCH_STRING_LENGTH, COMPARE_STRING, RXO_DURABILITY, RXO_DEADLINE, RXO_LIVELINESS, RXO_LEASE_DURATION, LATCH_LEASE_DURATION, 
        RXO_RELIABILITY, RXO_DESTINATION_ORDER, RXO_OWNERSHIP, RXO_PRESENTATION, RXO_PARTITION, RXO_LATENCY_BUDGET, CHECK_MAX_SIZE_SERIALIZED, 
        MATCH_DOMAIN_ID, MATCH_PROTOCOL_VERSION, LATCH_LOCATOR, LATCH_EXPECTS_INLINE_QOS, MATCH_GUID, CHECK_REMOTE_BUILTIN_ENDPOINTS, 
        PARTICIPANT_MATCH_STAGE, INFORM_ENDPOINTS_MATCH, INFORM_ENDPOINTS_UNMATCH, INFORM_ENDPOINTS_PARTICIPANT_UNMATCH, PARTICIPANT_STALE_CHECK, 
        LATCH_REMOVED_GUIDPREFIX, PROCESS_HEARTBEAT, PROCESS_HEARTBEAT_SEQUENCE_NUMBERS, SEND_ACKNACK, SEND_HEARTBEAT, PROCESS_ACKNACK, 
        PROCESS_ACKNACK_SEQUENCE_NUMBERS, FIND_PARTICIPANT_DEST, SEND_HEADER, SEND_PARTICIPANT_ANNOUNCEMENT, SEND_PUB_DATA, SEND_SUB_DATA, 
        SEND_MES_MAN_LIVE, SEND_MES_GAP, SEND_MES_AUTO_LIVE, LIVELINESS_UPDATE, SKIP_PARAMETER, SKIP_PACKET);
    -- Memory FSM states. Explained below in detail
    type MEM_STAGE_TYPE is (IDLE, SEARCH_PARTICIPANT, GET_PARTICIPANT_DATA, REMOVE_PARTICIPANT, FIND_PARTICIPANT_SLOT, 
        INSERT_PARTICIPANT, FIND_NEXT_PARTICIPANT, FIND_STALE_PARTICIPANT, UPDATE_PARTICIPANT);
    -- Memory FSM Opcodes
    -- OPCODE                   DESCRIPTION
    -- SEARCH_PARTICIPANT       Search memory for Participant Entry with GUID Prefix equal to "guid" signal. 
    --                          Set "addr_res" signal to Base Address of found Participant Entry or MAX Address if nothing found.
    --                          "mem_participant_data" contains memory Participant Data.
    -- INSERT_PARTICIPANT       Write Participant Data (contained in latches) to first empty Participant Slot.
    -- UPDATE_PARTICIPANT       Update the Participant Data of the Participant Entry pointed in "addr_res" according to the "update_participant_flags" flags.
    -- REMOVE_PARTICIPANT       Remove the Participant Entry pointed in "addr_res".
    -- FIND_STALE_PARTICIPANT   Find first Participant Entry with expired times (Lease Deadline, HEARTBEAT/ACKNACK Response/Suppression Timeout). 
    --                          Set "addr_res" signal to Base Address of found Participant Entry or MAX Address if nothing found.
    -- FIND_FIRST_PATICIPANT    Find first occupied Participant Entry in memory. 
    --                          Set "addr_res" signal to Base Address of found Participant Entry or MAX Address if nothing found.
    -- FIND_NEXT_PARTICIPANT    Find next occupied Participant Entry in memory from the Participant Entry pointed by "addr_res". 
    --                          Set "addr_res" signal to Base Address of found Participant Entry or MAX Address if nothing found.
    type MEM_OPCODE_TYPE is (NOP, SEARCH_PARTICIPANT, SEARCH_ENDPOINT, INSERT_PARTICIPANT, INSERT_ENDPOINT, UPDATE_PARTICIPANT, 
        REMOVE_PARTICIPANT, REMOVE_ENDPOINT, FIND_STALE_PARTICIPANT, FIND_FIRST_PATICIPANT, FIND_NEXT_PARTICIPANT);
    -- RTPS Data Submessage Content:
    --  * PDP       Simple Participant Discovery Protocol Data
    --  * EDP       Simple Endpoint Discovery Protocol Data
    --  * MESSAGE   Participant Message Data
    type MESSAGE_TYPE_TYPE is (NONE, PDP, EDP, MESSAGE);
    -- String Content
    type STRING_CONTENT_TYPE is (TOPIC_NAME_TYPE, TYPE_NAME_TYPE, DOMAIN_TAG_TYPE);
    -- Record of all Participant Data stored in memory
    type PARTICIPANT_DATA_TYPE is record
        meta_addr           : std_logic_vector(IPv4_ADDRESS_WIDTH-1 downto 0);
        def_addr            : std_logic_vector(IPv4_ADDRESS_WIDTH-1 downto 0);
        meta_port           : std_logic_vector(UDP_PORT_WIDTH-1 downto 0);
        def_port            : std_logic_vector(UDP_PORT_WIDTH-1 downto 0);
        extra_flags         : std_logic_vector(EXTRA_FLAGS_WIDTH-1 downto 0);
        lease_duration      : DURATION_TYPE;
        lease_deadline      : TIME_TYPE;
        heartbeat_res_time  : TIME_TYPE;
        acknack_res_time    : TIME_TYPE;
        spdp_seq_nr         : SEQUENCENUMBER_TYPE;
        pub_seq_nr          : SEQUENCENUMBER_TYPE;
        sub_seq_nr          : SEQUENCENUMBER_TYPE;
        mes_seq_nr          : SEQUENCENUMBER_TYPE;
    end record;
    
    --*****CONSTANT DECLARATION*****
    -- Max Serialized Payload Size in a UDP Stream (Bytes) [MAX_PAYLAOD(65536) - IPv4_HEADER(20) - UDP_HEADER(8) - RTPS_HEADER(20) - DATA_HEADER(24)]
    constant UDP_MAX_SIZE_SERIALIZED    : natural := 65464;
    -- 4-Byte Word Size of a Participant Entry in Memory
    constant PARTICIPANT_FRAME_SIZE     : natural := 23;
    -- Memory Size in 4-Byte Words
    constant BUILTIN_BUFFER_SIZE        : natural := MAX_REMOTE_PARTICIPANTS*PARTICIPANT_FRAME_SIZE;
    -- Memory Address Width
    constant BUILTIN_BUFFER_ADDR_WIDTH  : natural := log2c(BUILTIN_BUFFER_SIZE);
    -- Highest Memory Address
    constant MAX_ADDRESS                : unsigned(BUILTIN_BUFFER_ADDR_WIDTH-1 downto 0) := to_unsigned(BUILTIN_BUFFER_SIZE-1, BUILTIN_BUFFER_ADDR_WIDTH);
    -- Highest participant Frame Address
    constant MAX_PARTICIPANT_ADDRESS    : unsigned(BUILTIN_BUFFER_ADDR_WIDTH-1 downto 0) := MAX_ADDRESS - PARTICIPANT_FRAME_SIZE + 1;
    -- Address pointing to the beginning of the first Participant Data Frame
    constant FIRST_PARTICIPANT_ADDRESS  : unsigned(BUILTIN_BUFFER_ADDR_WIDTH-1 downto 0) := (others => '0');
    -- *UPDATE PARTICIPANT FLAG POSITIONS*
    -- Width of the Update Participant Flag Signal
    constant UPDATE_PARTICIPANT_FLAG_WIDTH  : natural := 6;
    -- Signifies that the main Participant Data are updated
    constant PARTICIPANT_DATA_FLAG          : natural := 0;
    -- Signifies that the Lease Deadline of the Participant Data is updated
    constant LEASE_DEADLINE_FLAG            : natural := 1;
    -- Signifies that the Extra Flags of the Participant Data are updated
    constant EXTRA_FLAGS_FLAG               : natural := 2;
    -- Signifies that the ACKNACK Timeout Time of the Participant Data is updated
    constant ACKNACK_RES_TIME_FLAG          : natural := 3;
    -- Signifies that the HEARTBEAT Timeout Time of the Participant Data is updated
    constant HEARTBEAT_RES_TIME_FLAG        : natural := 4;
    -- Signifies that the remote Endpoint (Publisher/Subscriber/Message) Sequence Number of the Participant Data is updated
    constant EDP_SEQ_NR_FLAG                : natural := 5;
    -- *EXTRA FLAG POSITIONS*
    -- Signifies that the Publisher Data should be sent
    constant PUB_DATA_FLAG              : natural := 3;
    -- Signifies that the Subscriber Data should be sent
    constant SUB_DATA_FLAG              : natural := 2;
    -- Signifies that the Message Data (Liveliness) should be sent
    constant MES_DATA_FLAG              : natural := 1;
    -- Signifies if the Reader Endpoint expects in-line QoS
    constant EXPECTS_INLINE_QOS_FLAG    : natural := 0;
    -- Highest Sequence Number of Publisher Data
    constant PUB_SEQUENCENUMBER         : SEQUENCENUMBER_TYPE := convert_to_double_word(to_unsigned(NUM_WRITERS, 64));
    -- Highest Sequence Number of Subscriber Data
    constant SUB_SEQUENCENUMBER         : SEQUENCENUMBER_TYPE := convert_to_double_word(to_unsigned(NUM_READERS, 64));
    -- Heartbeat/Liveliness Assertion Period
    constant HEARTBEAT_PERIOD           : DURATION_TYPE := work.rtps_package.min(MIN_ENDPOINT_LEASE_DURATION, PARTICIPANT_HEARTBEAT_PERIOD) - DURATION_DELTA;
    -- *RECV FLAGS*
    -- Width of the received array
    constant RCVD_WIDTH                         : natural := 9;
    -- Signifies that the DOMAIN_TAG was received
    constant DOMAIN_TAG_RCVD                    : natural := 0;
    -- Signifies that the DURABILITY QoS was received
    constant DURABILITY_QOS_RCVD_FLAG           : natural := 1;
    -- Signifies that the PRESENTATION QoS was received
    constant PRESENTATION_QOS_RCVD_FLAG         : natural := 2;
    -- Signifies that the DEADLINE QoS was received
    constant DEADLINE_QOS_RCVD_FLAG             : natural := 3;
    -- Signifies that the LATENCY_BUDGET QoS was received
    constant LATENCY_BUDGET_QOS_RCVD_FLAG       : natural := 4;
    -- Signifies that the OWNERSHIP QoS was received
    constant OWNERSHIP_QOS_RCVD_FLAG            : natural := 5;
    -- Signifies that the LIVELINESS QoS was received
    constant LIVELINESS_QOS_RCVD_FLAG           : natural := 6;
    -- Signifies that the RELIABILITY QoS was received
    constant RELIABILITY_QOS_RCVD_FLAG          : natural := 7;
    -- Signifies that the DESTINATION_ORDER QoS was received
    constant DESTINATION_ORDER_QOS_RCVD_FLAG    : natural := 8;
    
    
    -- Constant for zero Participant Data
    constant ZERO_PARTICIPANT_DATA      : PARTICIPANT_DATA_TYPE := (
        meta_addr           => IPv4_ADDRESS_INVALID,
        def_addr            => IPv4_ADDRESS_INVALID,
        meta_port           => UDP_PORT_INVALID,
        def_port            => UDP_PORT_INVALID,
        extra_flags         => (others => '0'),
        lease_duration      => DURATION_ZERO,
        lease_deadline      => TIME_INVALID,
        heartbeat_res_time  => TIME_INVALID,
        acknack_res_time    => TIME_INVALID,
        spdp_seq_nr         => SEQUENCENUMBER_UNKNOWN,
        pub_seq_nr          => SEQUENCENUMBER_UNKNOWN,
        sub_seq_nr          => SEQUENCENUMBER_UNKNOWN,
        mes_seq_nr          => SEQUENCENUMBER_UNKNOWN
    );
    
    
    --*****SIGNAL DECLARATION*****
    -- FSM state
    signal stage, stage_next : STAGE_TYPE := IDLE;
    -- FSM state latch. Used to transition dynamically to different states from the same state.
    signal return_stage, return_stage_next : STAGE_TYPE := IDLE;
    -- Memory FSM State
    signal mem_stage, mem_stage_next : MEM_STAGE_TYPE := IDLE;
    -- Intermediate input read signal. (Read from output port not allowed)
    signal rd_sig : std_logic := '0';
    -- Signal used to reset the word counter
    signal reset_read_cnt : std_logic;
    -- Word (4-Byte) counter (Counts words read from input fifo)
    signal read_cnt : unsigned(SUBMESSAGE_LENGTH_WIDTH-3 downto 0) := (others => '0');
    -- RTPS Submessage ID Latch
    signal opcode, opcode_next : std_logic_vector(SUBMESSAGE_ID_WIDTH-1 downto 0) := (others => '0');
    -- RTPS Submessage Flags Latch
    signal flags, flags_next : std_logic_vector(SUBMESSAGE_FLAGS_WIDTH-1 downto 0) := (others => '0');
    -- UDPv4 Source Port Latch
    signal src_port, src_port_next : std_logic_vector(UDP_PORT_WIDTH-1 downto 0) := (others => '0');
    -- IPv4 Source Address Latch
    signal src_addr, src_addr_next : std_logic_vector(IPv4_ADDRESS_WIDTH-1 downto 0) := (others => '0');
    -- Source Entity ID Latch
    signal src_entityid, src_entityid_next : std_logic_vector(ENTITYID_WIDTH-1 downto 0) := (others => '0');
    -- Destination Entity ID Latch
    signal dest_entityid, dest_entityid_next : std_logic_vector(ENTITYID_WIDTH-1 downto 0) := (others => '0');
    -- Source GUID Latch
    signal guid, guid_next : GUID_TYPE := (others => (others => '0'));
    -- RTPS DATA Submessage Sequence Number Latch
    signal seq_nr, seq_nr_next : SEQUENCENUMBER_TYPE := (others => (others => '0'));
    -- Word aligned End of Parameter
    signal parameter_end, parameter_end_next : unsigned(PARAMETER_LENGTH_WIDTH-1 downto 0) := (others => '0');
    -- RTPS DATA Submessage Content Type
    signal message_type, message_type_next : MESSAGE_TYPE_TYPE := NONE;
    -- Data in represented in Big Endian
    signal data_in_swapped : std_logic_vector(WORD_WIDTH-1 downto 0) := (others => '0');
    -- Byte Length of string
    signal string_length, string_length_next : unsigned(WORD_WIDTH-1 downto 0) := (others => '0');
    -- Counter of compared string words (4-Byte)
    signal compare_length, compare_length_next : unsigned(WORD_WIDTH-3 downto 0) := (others => '0');
    -- Bitmask of local Endpoint Matches
    signal endpoint_mask, endpoint_mask_next : std_logic_vector(0 to NUM_ENDPOINTS-1) := (others => '0');
    -- Signifies if the source of the Participant Data is compatible with our Participant
    signal participant_match, participant_match_next : std_logic := '0';
    -- Signifies if the Packet is comming from a Subscriber Endpoint
    signal is_subscriber, is_subscriber_next : std_logic := '0';
    -- Signal storing times for memory operations (Participant Lease Deadline, HEARTBEAT/ACKNACK Response/Suppression Time)
    signal deadline, deadline_next : TIME_TYPE := (others => (others => '0'));
    -- Signifies that the read Locator is a Metatraffic Locator
    signal is_meta_addr, is_meta_addr_next : std_logic := '0';
    -- General Purpose counter
    signal cnt, cnt_next : natural range 0 to max(23, STRING_WORD_ARRAY_TYPE'length) := 0;
    -- NOTE: In order to avoid synthesizing indexing for the full range of the OUTPUT_DATA_TYPE, we explicitly use counters constrained to the actual size
    -- Counter used to index the Participant Data
    signal participant_data_cnt, participant_data_cnt_next : natural range 0 to PARTICIPANT_DATA.length-1 := 0;
    -- Counter used to index the Publisher Data
    signal publisher_data_cnt, publisher_data_cnt_next : natural range 0 to work.math_pkg.max(WRITER_ENDPOINT_DATA.length-1, 0) := 0;
    -- Counter used to index the Subscriber Data
    signal subscriber_data_cnt, subscriber_data_cnt_next : natural range 0 to work.math_pkg.max(READER_ENDPOINT_DATA.length-1, 0) := 0;
    -- Signals the start of a Memory Operation (Should be pulled high only when mem_op_done is high)
    signal mem_op_start : std_logic := '0';
    -- Signals the end of a Memory Operation
    signal mem_op_done : std_logic := '0';
    -- Opcode of the Memory Operation (Valid only when mem_op_start is high)
    signal mem_opcode : MEM_OPCODE_TYPE := NOP;
    -- Current Memory Address
    signal mem_addr, mem_addr_next : unsigned(BUILTIN_BUFFER_ADDR_WIDTH-1 downto 0) := (others => '0');
    -- Base Memory Address of current Participant/Endpoint Frame
    signal mem_addr_base, mem_addr_base_next : unsigned(BUILTIN_BUFFER_ADDR_WIDTH-1 downto 0) := (others => '0');
    -- Result Base Memory Address of Memory Operation
    signal addr_res, addr_res_next : unsigned(BUILTIN_BUFFER_ADDR_WIDTH-1 downto 0) := (others => '0');
    -- Help signal used to reset the MAX Participant/Endpoint Memory Pointers
    signal last_addr, last_addr_next : unsigned(BUILTIN_BUFFER_ADDR_WIDTH-1 downto 0) := (others => '0');
    -- Highest Participant Memory Address (Points to first Word of last occupied Participant Frame)
    signal max_participant_addr, max_participant_addr_next : unsigned(BUILTIN_BUFFER_ADDR_WIDTH-1 downto 0) := (others => '0');
    -- Memory Data Read and Write Signals
    signal mem_read_data, mem_write_data : std_logic_vector(WORD_WIDTH-1 downto 0) := (others => '0');
    -- Memory Read and Write Enable Signals
    signal mem_rd, mem_wr : std_logic := '0';
    -- General Purpose Counter (Memory FSM)
    signal mem_cnt, mem_cnt_next : natural range 0 to 22 := 0;
    -- Signifies the next expected Sequence Number of the current relevant Message Type (Participant/Publisher/Subscriber/Message Data)
    signal next_seq_nr, next_seq_nr_next : SEQUENCENUMBER_TYPE := (others => (others => '0'));
    -- Latch used to store the first Sequence Number in ACKNACK/HEARTBEAT/GAP Messages
    signal first_seq_nr, first_seq_nr_next : SEQUENCENUMBER_TYPE := (others => (others => '0'));
    -- Latch used to store the last Sequence Number in HEARTBEAT/GAP Messages
    signal last_seq_nr, last_seq_nr_next : SEQUENCENUMBER_TYPE := (others => (others => '0'));
    -- Intermediate write enable signal.
    signal wr_sig : std_logic := '0';
    -- Signifies if we currently are resetting the MAX Participant/Endpoint Pointer
    signal reset_max_pointer, reset_max_pointer_next : std_logic := '0';
    -- Signifies if we currently are doing a Participant Stale Entry Check (Used to start Stale Checks between packet handling)
    signal stale_check, stale_check_next : std_logic := '0';
    -- Latch containing the GUID Prefix of the removed Participant from the memory, and the Enity ID of the last Orphan Endpoint found
    signal mem_guidprefix, mem_guidprefix_next : GUIDPREFIX_TYPE := (others => (others => '0'));
    -- Toggle latching the "last_word_in" signal until reset
    signal last_word_in_latch, last_word_in_latch_next : std_logic := '0';
    -- Flags signifying which parts of the participant Data stored in memory to update
    signal update_participant_flags, update_participant_flags_next : std_logic_vector(UPDATE_PARTICIPANT_FLAG_WIDTH-1 downto 0) := (others => '0');
    -- Latch for the Participant Data stored in memory
    signal mem_participant_data, mem_participant_data_next : PARTICIPANT_DATA_TYPE := ZERO_PARTICIPANT_DATA;
    -- Signifies if the marked Stale Participant had a Hertbeat Response/Suppression Delay Timeout
    signal is_heartbeat_res, is_heartbeat_res_next : std_logic := '0';
    -- Signal containing the HEARTBEAT/ACKNACK count of all built-in Endpoints
    signal count, count_next : unsigned(COUNT_WIDTH-1 downto 0) := (others => '0');
    -- Toggle set when at least one Endpoint has asserted the "alive" signal, until reset
    signal endpoint_alive : std_logic := '0';
    -- Resets the "endpoint_alive" signal
    signal reset_endpoint_alive : std_logic := '0';
    -- NOTE: The "auto_live_seq_nr" is always higher than "man_live_seq_nr"
    -- Contains the highest Sequence Number for automatic liveliness updates
    signal auto_live_seq_nr, auto_live_seq_nr_next : SEQUENCENUMBER_TYPE := (others => (others => '0'));
    -- Contains the highest Sequence Number for manual by participant liveliness updates
    signal man_live_seq_nr, man_live_seq_nr_next : SEQUENCENUMBER_TYPE := (others => (others => '0'));
    -- Points to the first Sequence Number after "man_live_seq_nr" (Signifies the start of the GAP between "man_live_seq_nr" and "auto_live_seq_nr")
    signal live_gap_start, live_gap_start_next : SEQUENCENUMBER_TYPE := (others => (others => '0'));
    -- Points to the first Sequence Number before "auto_live_seq_nr" (Signifies the end of the GAP between "man_live_seq_nr" and "auto_live_seq_nr")
    signal live_gap_end, live_gap_end_next : SEQUENCENUMBER_TYPE := (others => (others => '0'));
    -- Participant Announcement Timeout Time
    signal announcement_time, announcement_time_next : TIME_TYPE := (others => (others => '0'));
    -- Heartbeat/Liveliness Assertion Timeout Time
    signal heartbeat_time, heartbeat_time_next : TIME_TYPE := (others => (others => '0'));
    -- Signifies if we are currently in a Liveliness Assertion
    signal is_live_assert, is_live_assert_next : std_logic := '0';
    -- Signifies the content of the string read from input
    signal string_content, string_content_next : STRING_CONTENT_TYPE := TOPIC_NAME_TYPE;
    -- Reader Flags of remote Reader
    signal reader_flags, reader_flags_next : std_logic_vector(READER_FLAGS_WIDTH-1 downto 0) := (others => '0');
    -- Endpoint Locator IPv4 Address Latch
    signal def_addr, def_addr_next : std_logic_vector(IPv4_ADDRESS_WIDTH-1 downto 0) := (others => '0');
    -- Metatraffic Locator IPv4 Address Latch
    signal meta_addr, meta_addr_next : std_logic_vector(IPv4_ADDRESS_WIDTH-1 downto 0) := (others => '0');
    -- Endpoint UDPv4 Port Latch
    signal def_port, def_port_next : std_logic_vector(UDP_PORT_WIDTH-1 downto 0) := (others => '0');
    -- Metatraffic UDPv4 Port Latch
    signal meta_port, meta_port_next : std_logic_vector(UDP_PORT_WIDTH-1 downto 0) := (others => '0');
    -- Participant Lease Duration Latch
    signal lease_duration, lease_duration_next : DURATION_TYPE := (others => (others => '0'));
    -- Extra Flags Latch
    signal extra_flags, extra_flags_next : std_logic_vector(EXTRA_FLAGS_WIDTH-1 downto 0) := (others => '0');
    -- General Purpose Long latch
    signal long_latch, long_latch_next  : std_logic_vector(CDR_LONG_WIDTH-1 downto 0) := (others => '0');
    -- General Purpose Long latch (For Memory FSM)
    signal mem_long_latch, mem_long_latch_next  : std_logic_vector(CDR_LONG_WIDTH-1 downto 0) := (others => '0');
    -- Contains flags that signify which PIDs where received. This is done in order to use the default value for
    -- not received elements.
    signal rcvd, rcvd_next : std_logic_vector(RCVD_WIDTH-1 downto 0) := (others => '0');
    
    
    
    --*****ALIAS DEFINATION*****
    -- 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 endian_flag_next          : std_logic is flags_next(0);
    alias qos_flag                  : std_logic is flags(1);
    alias qos_flag_next             : std_logic is flags_next(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);
    
    --*****FUNCTION DECLARATION*****
begin
    
    --*****COMPONENT INSTANTIATION*****
    ram_inst : single_port_ram
        generic map (
            ADDR_WIDTH      => BUILTIN_BUFFER_ADDR_WIDTH,
            DATA_WIDTH      => WORD_WIDTH,
            MEMORY_DEPTH    => BUILTIN_BUFFER_SIZE
        )
        port map (
            clk         => clk,
            addr        => std_logic_vector(mem_addr),
            wen         => mem_wr,
            ren         => mem_rd,
            wr_data     => mem_write_data,
            rd_data     => mem_read_data
        );
    
    
    rd  <= rd_sig;
    
    -- Big Endian Representation
    data_in_swapped <= endian_swap(endian_flag, data_in);
    
    -- This process is responsible for toggling the "endpoint_alive" signal
    -- The signal is set high when at least one bit of the "alive" signal is high for at least one clock cycle,
    -- and is set low when "reset_endpoint_alive" signal is high for at least one clock cycle.
    endpoint_alive_prc: process(all)
    begin
        if rising_edge(clk) then
            -- Reset Endpoint Alive Signal
            if (reset = '1' or reset_endpoint_alive = '1') then
                endpoint_alive  <= '0';
            -- Set Endpoint Alive Signal, if at least one endpoint asserts liveliness
            elsif (alive /= (alive'range => '0')) then
                endpoint_alive  <= '1';
            end if;
        end if;
    end process;
    
    -- This process connects the Intermediate Output Signals to the actual output FIFOs
    output_prc : process(all)
    begin
        endpoint_wr <= (others => '0');
        rtps_wr     <= '0';
        case (stage) is
            when INFORM_ENDPOINTS_MATCH =>
                if (wr_sig = '1') then
                    endpoint_wr <= endpoint_mask;
                end if;
            when INFORM_ENDPOINTS_UNMATCH =>
                if (wr_sig = '1') then
                    endpoint_wr <= not endpoint_mask;
                end if;
            when INFORM_ENDPOINTS_PARTICIPANT_UNMATCH =>
                if (wr_sig = '1') then
                    endpoint_wr <= (others => '1');
                end if;
            when LIVELINESS_UPDATE =>
                if (wr_sig = '1') then
                    endpoint_wr <= endpoint_mask;
                end if;
            when others =>
                    rtps_wr <= wr_sig;
        end case;
    end process;
    
    -- Main State Machine
    -- STATE                                DESCRIPTION
    -- IDLE                                 Idle state. Initiates Participant Announcements, Heartbeat/Liveliness Assertions, Stale Participant Entry Checks ,and Packet Processing, in that priority order.
    -- PACKET_HEADER                        Read the Endpoint FIFO Packet Format Header (RTPS Submessage ID/Opcode, Submessage Flags, Source UDPv4 Port)
    -- PACKET_SRC_ADDR                      Read Source IPv4 Address    
    -- PACKET_SRC_GUIDPREFIX                Read Source GUID Prefix
    -- PACKET_SRC_ENTITYID                  Read Source Entity ID
    -- PACKET_DEST_ENTITYID                 Read Destination Entity ID. Determine content of Packet (Participant Data, Publisher/Subscriber/Message Data/GAP/HEARTBEAT/ACKNACK) and initiate processing.
    -- CHECK_SRC_ENTITYID                   Read Source Entity ID. Determine content of Packet (Participant Data, Publisher/Subscriber/Message Data/GAP/HEARTBEAT/ACKNACK) and initiate processing.
    --                                      This state is taken if the Packet content could not be determined in the PACKET_DEST_ENTITYID state.
    -- LATCH_SEQ_NR                         Store RTPS Data Submessage Sequence Number
    -- PROCESS_DATA                         Parse RTPS Data Submessage Payload Header. Determine content of Data (Parameter List, Participant Message CDR Data) or if Data Message contains in-line QoS and initiate respective handling.
    -- PROCESS_MESSAGE                      Parse Participant Data Message and extract liveliness assertion of remote Participant
    -- LIVELINESS_UPDATE                    Propagate Liveliness Assertion of remote Participant to respective local Reader Endpoints
    -- PROCESS_GAP                          Parse RTPS GAP Submessage
    -- PROCESS_GAP_SEQUENCE_NUMBERS         Process GAP Sequence Numbers. Update stored Sequence Numbers if necessary
    -- PROCESS_HEARTBEAT                    Parse RTPS HEARTBEAT Submessage
    -- PROCESS_HEARTBEAT_SEQUENCE_NUMBERS   Process HEARTBEAT Sequence Numbers. Update stored Sequence Numbers if necessary. Set HEARTBEAT response timeout accordingly.
    -- PROCESS_ACKNACK                      Parse RTPS ACKNACK Submessage
    -- PROCESS_ACKNACK_SEQUENCE_NUMBERS     Process ACKNACK Sequence Numbers. Update stored Sequence Numbers if necessary. Set ACKNACK response timeout accordingly.
    -- FIND_PARTICIPANT_DEST                Find next stored Participant to send Participant Announcement and Liveliness Assertion to.
    -- PARTICIPANT_STALE_CHECK              Check memory for remote stale Participant Entries (Lease Duration Exceeded, HEARTBEAT/ACKNACK timeout passed)
    -- LATCH_REMOVED_GUIDPREFIX             Store Participant GUID Prefix of removed Participant for Orphan Endpoint Search
    -- PROCESS_PL                           Parse RTPS Parameter List
    -- CHECK_DEFAULT                        Match local endpoints against non-received QoS (default values) of remote
    -- LATCH_STRING_LENGTH                  Store String Length
    -- COMPARE_STRING                       Compare string to constants depending on string contents.
    -- MATCH_GUID                           Compare GUID Prefix to source GUID Prefix of Packet
    -- MATCH_DOMAIN_ID                      Compare Domain ID
    -- MATCH_PROTOCOL_VERSION               Compare RTPS Protocol Version
    -- LATCH_LOCATOR                        Store Locator. NOTE: Only Metatraffic and User Locators are differentiated. Unicast and Multicast are overwriting each other and the last parsed is the one to be used.
    -- LATCH_EXPECTS_INLINE_QOS             Store 'expectsInlineQoS' Flag
    -- LATCH_LEASE_DURATION                 Store remote Participant Lease Duration
    -- CHECK_REMOTE_BUILTIN_ENDPOINTS       Check if the remote Participant has the required built-in Endpoints
    -- RXO_DURABILITY                       Check Compatibility of Durability QoS
    -- RXO_DEADLINE                         Check Compatibility of Deadline QoS
    -- RXO_LIVELINESS                       Check Compatibility of Liveliness QoS
    -- RXO_LEASE_DURATION                   Check Compatibility of Liveliness QoS
    -- RXO_RELIABILITY                      Check Compatibility of Reliability QoS
    -- RXO_DESTINATION_ORDER                Check Compatibility of Destination Order QoS
    -- RXO_OWNERSHIP                        Check Compatibility of Ownership QoS
    -- RXO_PRESENTATION                     Check Compatibility of Presentation QoS
    -- RXO_PARTITION                        Check Compatibility of Partition QoS
    -- RXO_LATENCY_BUDGET                   Check Compatibility of Latency Budget QoS
    -- CHECK_MAX_SIZE_SERIALIZED            Check if Publishers Max sent Payload Size is Compatible
    -- PARTICIPANT_MATCH_STAGE              Add compatible remote and remove incompatible remote Participants to and from memory. Update stored Sequence Numbers respectively (also for EDP)
    -- INFORM_ENDPOINTS_MATCH               Propagate local Endpoint matches
    -- INFORM_ENDPOINTS_UNMATCH             Propagate local Endpoint unmatches
    -- INFORM_ENDPOINTS_PARTICIPANT_UNMATCH Propagate remote Participant unmatch
    -- SEND_HEADER                          Send Output Data Header and RTPS Message Header
    -- SEND_PARTICIPANT_ANNOUNCEMENT        Send Participant Announcement Data Submessage
    -- SEND_ACKNACK                         Send ACKNACK Submessages
    -- SEND_HEARTBEAT                       Send HEARTBEAT Submessages
    -- SEND_PUB_DATA                        Send Endpoint Discovery Publication Data Submessage
    -- SEND_SUB_DATA                        Send Endpoint Discovery Subscriber Data Submessage
    -- SEND_MES_MAN_LIVE                    Send Participant Message Automatic Liveliness Assertion Data Submessage
    -- SEND_MES_GAP                         Send Participant Message GAP Submessages
    -- SEND_MES_AUTO_LIVE                   Send Participant Message Manual-by-Participant Liveliness Assertion Data Submessage
    -- SKIP_PARAMETER                       Skip rest of Parameter
    -- SKIP_PACKET                          Skip rest of Packet
    parse_prc: process(all)
        variable tmp_endpoint_mask      : std_logic_vector(0 to NUM_ENDPOINTS-1) := (others => '0');
        variable rd_guard               : std_logic := '0';
        variable tmp_default_qos_match  : DEFAULT_QOS_MATCH_TYPE := READER_DEFAULT_QOS_MATCH;
        variable tmp_dw                 : DOUBLE_WORD_ARRAY := (others => (others => '0'));
        variable mem_seq_nr             : SEQUENCENUMBER_TYPE := SEQUENCENUMBER_UNKNOWN;
    begin
        --DEFAULT Registered
        stage_next                      <= stage;
        return_stage_next               <= return_stage;
        opcode_next                     <= opcode;
        flags_next                      <= flags;
        src_port_next                   <= src_port;
        src_addr_next                   <= src_addr;
        guid_next                       <= guid;
        src_entityid_next               <= src_entityid;
        dest_entityid_next              <= dest_entityid;
        parameter_end_next              <= parameter_end;
        message_type_next               <= message_type;
        string_length_next              <= string_length;
        compare_length_next             <= compare_length;
        endpoint_mask_next              <= endpoint_mask;
        participant_match_next          <= participant_match;
        is_subscriber_next              <= is_subscriber;
        lease_duration_next             <= lease_duration;
        def_addr_next                   <= def_addr;
        meta_addr_next                  <= meta_addr;
        def_port_next                   <= def_port;
        meta_port_next                  <= meta_port;
        is_meta_addr_next               <= is_meta_addr;
        cnt_next                        <= cnt;
        reader_flags_next               <= reader_flags;
        extra_flags_next                <= extra_flags;
        stale_check_next                <= stale_check;
        first_seq_nr_next               <= first_seq_nr;
        last_seq_nr_next                <= last_seq_nr;
        update_participant_flags_next   <= update_participant_flags;
        deadline_next                   <= deadline;
        auto_live_seq_nr_next           <= auto_live_seq_nr;
        man_live_seq_nr_next            <= man_live_seq_nr;
        live_gap_start_next             <= live_gap_start;
        live_gap_end_next               <= live_gap_end;
        announcement_time_next          <= announcement_time;
        heartbeat_time_next             <= heartbeat_time;
        is_live_assert_next             <= is_live_assert;
        string_content_next             <= string_content;
        participant_data_cnt_next       <= participant_data_cnt;
        publisher_data_cnt_next         <= publisher_data_cnt;
        subscriber_data_cnt_next        <= subscriber_data_cnt;
        seq_nr_next                     <= seq_nr;
        next_seq_nr_next                <= next_seq_nr;
        last_word_in_latch_next         <= last_word_in_latch;
        count_next                      <= count;
        long_latch_next                 <= long_latch;
        rcvd_next                       <= rcvd;
        -- DEFAULT Unregistered
        rd_sig                          <= '0';
        rd_guard                        := '0';
        reset_read_cnt                  <= '0';
        wr_sig                          <= '0';
        mem_opcode                      <= NOP;
        mem_op_start                    <= '0';
        last_word_out                   <= '0';
        reset_endpoint_alive            <= '0';
        data_out                        <= (others => '0');
        
        -- Last Word Latch Setter
        if (last_word_in = '1') then
            last_word_in_latch_next <= '1';
        end if;
        
        -- Set mem_seq_nr to correct sequence number
        -- NOTE: The stored Sequence Numbers are the next expected
        case (message_type) is
            -- Participant Data Sequence Number
            when PDP =>
                mem_seq_nr      := mem_participant_data.spdp_seq_nr;
            when EDP =>
                -- Subscriber Data Sequence Number
                if (is_subscriber = '1') then
                    mem_seq_nr  := mem_participant_data.sub_seq_nr;
                -- Publisher Data Sequence Number
                else
                    mem_seq_nr  := mem_participant_data.pub_seq_nr;
                end if;
            -- Participant Message Data Sequence Number
            when MESSAGE =>
                mem_seq_nr      := mem_participant_data.mes_seq_nr;
            when others =>
                mem_seq_nr      := SEQUENCENUMBER_UNKNOWN;
        end case;
        
        case(stage) is
            -- Initial/Idle State
            when IDLE =>
                -- Participant Announcement Time Trigger
                if (time > announcement_time) then
                    announcement_time_next  <= time + PARTICIPANT_ANNOUNCEMENT_PERIOD;
                    stage_next              <= SEND_HEADER;
                    return_stage_next       <= SEND_PARTICIPANT_ANNOUNCEMENT;
                    cnt_next                <= 0;
                -- Heartbeat Time Trigger
                -- NOTE: Liveliness assertion and Heartbeat sending is done together
                elsif (time > heartbeat_time) then
                    -- Memory Operation Guard
                    if (mem_op_done = '1') then
                        -- NOTE:    The sequence number of the automatic liveliness is always higher than that of the manual liveliness
                        -- If Manual By Participant Liveliness asserted, send both manual and automatic liveliness
                        if (endpoint_alive = '1') then
                            -- Reset the endpoint alive toggle signal
                            reset_endpoint_alive    <= '1';
                            man_live_seq_nr_next    <= auto_live_seq_nr + 1;
                            auto_live_seq_nr_next   <= auto_live_seq_nr + 2;
                            -- GAP Start is always man_live_seq_nr+1
                            live_gap_start_next     <= auto_live_seq_nr + 2;
                            -- GAP End is always auto_live_seq_nr-1
                            live_gap_end_next       <= auto_live_seq_nr + 1;
                        -- Else send only automatic liveliness
                        else
                            auto_live_seq_nr_next   <= auto_live_seq_nr + 1;
                            live_gap_end_next       <= live_gap_end + 1;
                        end if;
                        heartbeat_time_next <= time + HEARTBEAT_PERIOD;
                        -- Send Heartbeat and Liveliness Assertions to all stored Participants
                        mem_opcode          <= FIND_FIRST_PATICIPANT;
                        mem_op_start        <= '1';
                        stage_next          <= FIND_PARTICIPANT_DEST;
                        -- Increment the counter for the new Heartbeat Messages
                        count_next          <= count + 1;
                        -- Mark this as liveliness assertion (Allows the "SEND_MES_AUTO_LIVE" stage to decide on return stage)
                        is_live_assert_next <= '1';
                    end if;
                -- Stale Entry search is initiated between every packet processing and when there is no packet to process
                elsif (stale_check = '0' or empty = '1') then
                    if (mem_op_done = '1') then
                        mem_opcode          <= FIND_STALE_PARTICIPANT;
                        mem_op_start        <= '1';
                        -- Stale Check Toggle (Setter)
                        stale_check_next    <= '1';
                        stage_next          <= PARTICIPANT_STALE_CHECK;
                    end if;
                -- Process Packet
                else
                    -- Stale Check Toggle (Resetter)
                    stale_check_next    <= '0';
                    stage_next          <= PACKET_HEADER;
                    
                    -- Reset Latches
                    def_addr_next               <= (others => '0');
                    meta_addr_next              <= DEFAULT_IPv4_META_ADDRESS;
                    def_port_next               <= (others => '0');
                    meta_port_next              <= META_IPv4_MULTICAST_PORT;
                    lease_duration_next         <= DEFAULT_PARTICIPANT_LEASE_DURATION;
                    extra_flags_next            <= (EXPECTS_INLINE_QOS_FLAG => DEFAULT_EXPECTS_INLINE_QOS, others => '0');
                    reader_flags_next           <= (READER_EXPECTS_INLINE_QOS_FLAG => DEFAULT_EXPECTS_INLINE_QOS, READER_EXPECTS_HISTORICAL_DATA_FLAG => DEFAULT_EXPECTS_HISTORICAL_DATA_FLAG, READER_IS_BEST_EFFORT_FLAG => DEFAULT_IS_BEST_EFFORT_FLAG, others => '0');
                    rcvd_next                   <= (others => '0');
                    -- NOTE: We work with a "mark by default, and unmark on first missmatch" System.
                    --       This assumes that each RxO QoS parameter occur only once in the list, else the behavior is undefined.
                    participant_match_next      <= '1';
                    endpoint_mask_next          <= (others => '1');
                end if;
            when PACKET_HEADER =>
                -- Input FIFO Guard
                if (empty = '0') then
                    rd_guard        := '1';
                    -- Latch Opcode
                    opcode_next     <= header_opcode;
                    -- Latch Flags
                    flags_next      <= header_flags;
                    -- Latch Source UDP Port
                    src_port_next   <= header_udp_port;
                    
                    stage_next      <= PACKET_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 PACKET_SRC_ADDR =>
                -- Input FIFO Guard
                if (empty = '0') then
                    rd_guard        := '1';
                    -- Latch Source IP Address
                    src_addr_next   <= data_in;
                    
                    stage_next      <= PACKET_SRC_GUIDPREFIX;
                    cnt_next        <= 0;
                end if;
            when PACKET_SRC_GUIDPREFIX =>
                -- Input FIFO Guard and Memory Operation Guard
                if (empty = '0' and mem_op_done = '1') then
                    rd_guard    := '1';
                    cnt_next    <= cnt + 1;
                    
                    -- Latch Src GUIDPrefix
                    case (cnt) is
                        when 0 =>
                            guid_next(0)    <= data_in;
                        when 1 =>
                            guid_next(1)    <= data_in;
                        when 2 =>
                            guid_next(2)    <= data_in;
                            guid_next(3)    <= (others => '0');
                            -- Start Participant Search
                            mem_opcode      <= SEARCH_PARTICIPANT;
                            mem_op_start    <= '1';
                            
                            stage_next      <= PACKET_SRC_ENTITYID;
                        when others =>
                            null;
                    end case;
                end if;
            when PACKET_SRC_ENTITYID =>
                -- Input FIFO Guard
                if (empty = '0') then
                    rd_guard        := '1';
                    -- Latch Source Entity ID
                    src_entityid_next   <= data_in;
                    
                    stage_next  <= PACKET_DEST_ENTITYID;
                end if;
            when PACKET_DEST_ENTITYID =>
                -- Input FIFO Guard
                if (empty = '0') then
                    rd_guard    := '1';
                    
                    -- DEFAULT
                    stage_next          <= SKIP_PACKET;
                    is_subscriber_next  <= '0';
                    
                    -- *Check Destination Entity ID*
                    case (data_in) is
                        when ENTITYID_UNKNOWN =>
                            -- Wildcard Destination
                            -- Content has to be determined through Source Entity ID
                            stage_next  <= CHECK_SRC_ENTITYID;
                        when ENTITYID_SPDP_BUILTIN_PARTICIPANT_DETECTOR =>
                            -- Only DATA relevant
                            if (opcode = SID_DATA) then
                                -- Packet Contains Participant Data
                                message_type_next       <= PDP;
                                
                                stage_next              <= LATCH_SEQ_NR;
                                cnt_next                <= 0;
                            end if;
                        when ENTITYID_SEDP_BUILTIN_PUBLICATIONS_ANNOUNCER =>
                            -- SANITY CHECK: Ignore if no Writers
                            -- Only ACKNACKs are relevant
                            if (NUM_WRITERS /= 0 and opcode = SID_ACKNACK) then
                                message_type_next   <= EDP;
                                is_subscriber_next  <= '1';
                                
                                stage_next          <= PROCESS_ACKNACK; --ACKNACK Processing
                                cnt_next            <= 0;
                            end if;
                        when ENTITYID_SEDP_BUILTIN_PUBLICATIONS_DETECTOR =>
                            -- SANITY CHECK: Ignore if no Readers
                            if (NUM_READERS /= 0) then
                                message_type_next   <= EDP;
                                
                                
                                -- Only HEARTBEATs, GAPs and DATA are relevant
                                case (opcode) is
                                    when SID_HEARTBEAT =>
                                        stage_next          <= PROCESS_HEARTBEAT; -- HEARTBEAT Processing
                                        cnt_next            <= 0;
                                    when SID_GAP =>
                                        stage_next          <= PROCESS_GAP; -- GAP Processing
                                        cnt_next            <= 0;
                                    when SID_DATA =>
                                        stage_next          <= LATCH_SEQ_NR; -- DATA Processing
                                        cnt_next            <= 0;
                                        
                                        -- Unmark all Writers
                                        endpoint_mask_next  <= ENDPOINT_READERS;
                                    when others =>
                                        null;
                                end case;
                            end if;
                        when ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_ANNOUNCER =>
                            -- SANITY CHECK: Ignore if no Readers
                            -- Only ACKNACKs are relevant
                            if (NUM_READERS /= 0 and opcode = SID_ACKNACK) then
                                message_type_next   <= EDP;
                                
                                stage_next          <= PROCESS_ACKNACK; --ACKNACK Processing
                                cnt_next            <= 0;
                            end if;
                        when ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_DETECTOR =>
                            -- SANITY CHECK: Ignore if no Writers
                            if (NUM_WRITERS /= 0) then
                                message_type_next   <= EDP;
                                is_subscriber_next  <= '1';
                                
                                -- Only HEARTBEATs, GAPs and DATA are relevant
                                case (opcode) is
                                    when SID_HEARTBEAT =>
                                        stage_next          <= PROCESS_HEARTBEAT; -- HEARTBEAT Processing
                                        cnt_next            <= 0;
                                    when SID_GAP =>
                                        stage_next          <= PROCESS_GAP; -- GAP Processing
                                        cnt_next            <= 0;
                                    when SID_DATA =>
                                        stage_next          <= LATCH_SEQ_NR; -- DATA Processing
                                        cnt_next            <= 0;
                                        
                                        -- Unmark all Readers
                                        endpoint_mask_next  <= not ENDPOINT_READERS;
                                    when others =>
                                        null;
                                end case;
                            end if;
                        when ENTITYID_P2P_BUILTIN_PARTICIPANT_MESSAGE_WRITER =>
                            -- SANITY CHECK: Ignore if no Writers
                            if (NUM_WRITERS /= 0) then
                                -- Only ACKNACKs are relevant
                                if (opcode = SID_ACKNACK) then
                                    message_type_next   <= MESSAGE;
                                    
                                    stage_next          <= PROCESS_ACKNACK; --ACKNACK Processing
                                    cnt_next            <= 0;
                                end if;
                            end if;
                        when ENTITYID_P2P_BUILTIN_PARTICIPANT_MESSAGE_READER =>
                            -- SANITY CHECK: Ignore if no Readers
                            if (NUM_READERS /= 0) then
                                message_type_next   <= MESSAGE;
                                
                                -- Only HEARTBEATs, GAPs and DATA are relevant
                                case (opcode) is
                                    when SID_HEARTBEAT =>
                                        stage_next          <= PROCESS_HEARTBEAT; -- HEARTBEAT Processing
                                        cnt_next            <= 0;
                                    when SID_GAP =>
                                        stage_next          <= PROCESS_GAP; -- GAP Processing
                                        cnt_next            <= 0;
                                    when SID_DATA =>
                                        stage_next          <= LATCH_SEQ_NR; -- DATA Processing
                                        cnt_next            <= 0;
                                        
                                        -- Unmark all Writers
                                        endpoint_mask_next  <= ENDPOINT_READERS;
                                    when others =>
                                        null;
                                end case;
                            end if;
                        when others =>
                            -- Skip Packet
                            null;
                    end case;
                end if;
            when CHECK_SRC_ENTITYID =>
                -- DEFAULT
                stage_next          <= SKIP_PACKET;
                is_subscriber_next  <= '0';
                
                -- *Check Dest Entity ID*
                case (src_entityid) is
                    when ENTITYID_SPDP_BUILTIN_PARTICIPANT_ANNOUNCER =>
                        -- Only DATA relevant
                        if (opcode = SID_DATA) then
                            -- Packet Contains Participant Data
                            message_type_next   <= PDP;
                            
                            stage_next          <= LATCH_SEQ_NR;
                            cnt_next            <= 0;
                        end if;
                    when ENTITYID_SEDP_BUILTIN_PUBLICATIONS_DETECTOR =>
                        -- SANITY CHECK: Ignore if no Writers
                        -- Only ACKNACKs are relevant
                        if (NUM_WRITERS /= 0 and opcode = SID_ACKNACK) then
                            message_type_next   <= EDP;
                            is_subscriber_next  <= '1';
                            
                            stage_next          <= PROCESS_ACKNACK; --ACKNACK Processing
                            cnt_next            <= 0;
                        end if;
                    when ENTITYID_SEDP_BUILTIN_PUBLICATIONS_ANNOUNCER =>
                        -- SANITY CHECK: Ignore if no Readers
                        if (NUM_READERS /= 0) then
                            message_type_next   <= EDP;
                            
                            -- Only HEARTBEATs, GAPs and DATA are relevant
                            case (opcode) is
                                when SID_HEARTBEAT =>
                                    stage_next          <= PROCESS_HEARTBEAT; -- HEARTBEAT Processing
                                    cnt_next            <= 0;
                                when SID_GAP =>
                                    stage_next          <= PROCESS_GAP; -- GAP Processing
                                    cnt_next            <= 0;
                                when SID_DATA =>
                                    stage_next          <= LATCH_SEQ_NR; -- DATA Processing
                                    cnt_next            <= 0;
                                    
                                    -- Unmark all Writers
                                    endpoint_mask_next  <= ENDPOINT_READERS;
                                when others =>
                                    null;
                            end case;
                        end if;
                    when ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_DETECTOR =>
                        -- SANITY CHECK: Ignore if no Readers
                        -- Only ACKNACKs are relevant
                        if (NUM_READERS /= 0 and opcode = SID_ACKNACK) then
                            message_type_next   <= EDP;
                            
                            stage_next          <= PROCESS_ACKNACK; --ACKNACK Processing
                            cnt_next            <= 0;
                        end if;
                    when ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_ANNOUNCER =>
                        -- SANITY CHECK: Ignore if no Writers
                        if (NUM_WRITERS /= 0) then
                            message_type_next   <= EDP;
                            is_subscriber_next  <= '1';
                            
                            -- Only HEARTBEATs, GAPs and DATA are relevant
                            case (opcode) is
                                when SID_HEARTBEAT =>
                                    stage_next          <= PROCESS_HEARTBEAT; -- HEARTBEAT Processing
                                    cnt_next            <= 0;
                                when SID_GAP =>
                                    stage_next          <= PROCESS_GAP; -- GAP Processing
                                    cnt_next            <= 0;
                                when SID_DATA =>
                                    stage_next          <= LATCH_SEQ_NR; -- DATA Processing
                                    cnt_next            <= 0;
                                    
                                    -- Unmark all Readers
                                    endpoint_mask_next  <= not ENDPOINT_READERS;
                                when others =>
                                    null;
                            end case;
                        end if;
                    when ENTITYID_P2P_BUILTIN_PARTICIPANT_MESSAGE_READER =>
                        -- SANITY CHECK: Ignore if no Writers
                        if (NUM_WRITERS /= 0) then
                            -- Only ACKNACKs are relevant
                            if (opcode = SID_ACKNACK) then
                                message_type_next   <= MESSAGE;
                                
                                stage_next          <= PROCESS_ACKNACK; --ACKNACK Processing
                                cnt_next            <= 0;
                            end if;
                        end if;
                    when ENTITYID_P2P_BUILTIN_PARTICIPANT_MESSAGE_WRITER =>
                        -- SANITY CHECK: Ignore if no Readers
                        if (NUM_READERS /= 0) then
                            message_type_next       <= MESSAGE;
                            
                            -- Only HEARTBEATs, GAPs and DATA are relevant
                            case (opcode) is
                                when SID_HEARTBEAT =>
                                    stage_next          <= PROCESS_HEARTBEAT; -- HEARTBEAT Processing
                                    cnt_next            <= 0;
                                when SID_GAP =>
                                    stage_next          <= PROCESS_GAP; -- GAP Processing
                                    cnt_next            <= 0;
                                when SID_DATA =>
                                    stage_next          <= LATCH_SEQ_NR; -- DATA Processing
                                    cnt_next            <= 0;
                                    
                                    -- Unmark all Writers
                                    endpoint_mask_next  <= ENDPOINT_READERS;
                                when others =>
                                    null;
                            end case;
                        end if;
                    when others =>
                        null;
                end case;
            when LATCH_SEQ_NR =>
                -- Input FIFO Guard
                if (empty = '0') then
                    rd_guard    := '1';
                    cnt_next    <= cnt + 1;
                    
                    -- Latch Sequence Number
                    case (cnt) is
                        when 0 =>
                            seq_nr_next(0)  <= unsigned(data_in_swapped);
                        when 1 =>
                            seq_nr_next(1)  <= unsigned(data_in_swapped);
                            -- Store Next Sequence Number
                            tmp_dw              := (0 => seq_nr(0), 1 => unsigned(data_in_swapped));
                            next_seq_nr_next    <= tmp_dw + 1;
                            
                            stage_next      <= PROCESS_DATA;
                        when others =>
                            null;
                    end case;
                end if;
            when PROCESS_DATA =>
                -- Data Message contains inline QoS
                if (qos_flag = '1') then
                    stage_next  <= PROCESS_PL;
                -- Data Message contains Data Payload
                elsif (data_flag = '1') then
                    -- Input FIFO Guard
                    if (empty = '0') then
                        rd_guard    := '1';
                        -- DEFAULT STAGE
                        stage_next  <= SKIP_PACKET;
                        
                        -- Process Payload Header
                        case(representation_id) is
                            when PL_CDR_BE =>
                                -- Override Endian Flag
                                endian_flag_next    <= '0';
                                
                                stage_next          <= PROCESS_PL;
                            when PL_CDR_LE =>
                                -- Override Endian Flag
                                endian_flag_next    <= '1';
                                
                                stage_next          <= PROCESS_PL;
                            when CDR_BE =>
                                -- Accept CDR Representation only for Participant Messages
                                if (message_type = MESSAGE) then
                                    -- Override Endian Flag
                                    endian_flag_next    <= '0';
                                    
                                    stage_next          <= PROCESS_MESSAGE;
                                    cnt_next            <= 0;
                                end if;
                            when CDR_LE =>
                                -- Accept CDR Representation only for Participant Messages
                                if (message_type = MESSAGE) then
                                    -- Override Endian Flag
                                    endian_flag_next    <= '1';
                                    
                                    stage_next          <= PROCESS_MESSAGE;
                                    cnt_next            <= 0;
                                end if;
                            when others =>
                                -- Skip Packet
                                null;
                        end case;
                    end if;
                end if;
            when PROCESS_MESSAGE =>
                -- Input FIFO Guard
                if (empty = '0') then
                    rd_guard    := '1';
                    cnt_next    <= cnt + 1;
                    
                    case (cnt) is
                        -- Check if GUID Prefix valid (Should be the same as the GUID Prefix of the Packet)
                        -- GUID Prefix 1/3
                        when 0 =>
                            if (data_in /= guid(0)) then
                                stage_next  <= SKIP_PACKET;
                            end if;
                        -- GUID Prefix 2/3
                        when 1 =>
                            if (data_in /= guid(1)) then
                                stage_next  <= SKIP_PACKET;
                            end if;
                        -- GUID Prefix 3/3
                        when 2 =>
                            if (data_in /= guid(2)) then
                                stage_next  <= SKIP_PACKET;
                            end if;
                        -- Participant Mesasge Kind
                        when 3 =>
                            -- NOTE: Rest of Participant Message is ignored
                            -- XXX: The Participant Message Data may contain additional data, and according to DDSI-RTPS 2.3 
                            --      implementations must be able to support up to 128 Bytes of additional data. Due to the unnecessary 
                            --      high complexity this would add this is not supported.
                            case (data_in) is
                                -- Automatic Liveliness Assertion
                                when PARTICIPANT_MESSAGE_DATA_KIND_AUTOMATIC_LIVELINESS_UPDATE =>
                                    -- Mark all readers with automatic liveliness QoS
                                    endpoint_mask_next  <= AUTOMATIC_LIVELINESS_READERS;
                                    stage_next          <= LIVELINESS_UPDATE;
                                    cnt_next            <= 0;
                                -- Manual by Participant Liveliness Assertion
                                when PARTICIPANT_MESSAGE_DATA_KIND_MANUAL_LIVELINESS_UPDATE =>
                                    -- Mark all readers with manual by participant liveliness QoS
                                    endpoint_mask_next  <= MANUAL_BY_PARTICIPANT_LIVELINESS_READERS;
                                    stage_next          <= LIVELINESS_UPDATE;
                                    cnt_next            <= 0;
                                -- No Liveliness Update, skip packet
                                when others =>
                                    stage_next  <= SKIP_PACKET;
                            end case;
                        when others =>
                            null;
                    end case;
                end if;
            when LIVELINESS_UPDATE =>
                -- If no Endpoint interested for Liveliness Assertion, skip
                if (endpoint_mask = (endpoint_mask'range => '0')) then
                    -- DONE
                    stage_next  <= SKIP_PACKET;
                -- Output FIFO Guard
                elsif ((endpoint_mask and endpoint_full) = (endpoint_full'range => '0')) then
                    wr_sig      <= '1';
                    cnt_next    <= cnt + 1;
                    
                    case (cnt) is
                        -- Liveliness Assertion Opcode
                        when 0 =>
                            data_out  <= OPCODE_LIVELINESS_UPDATE;
                        -- GUID Prefix 1/3
                        when 1 =>
                            data_out  <= guid(0);
                        -- GUID Prefix 2/3
                        when 2 =>
                            data_out  <= guid(1);
                        -- GUID Prefix 3/3
                        when 3 =>
                            data_out        <= guid(2);
                            last_word_out   <= '1';
                            
                            -- DONE
                            stage_next  <= SKIP_PACKET;
                        when others =>
                            null;
                    end case;
                end if;
            when PROCESS_GAP =>
                -- Input FIFO Guard
                if (empty = '0') then
                    rd_guard    := '1';
                    cnt_next    <= cnt + 1;
                    
                    -- Latch Sequence Numbers
                    case (cnt) is
                        when 0 =>
                            first_seq_nr_next(0)    <= unsigned(data_in_swapped);
                        when 1 =>
                            first_seq_nr_next(1)    <= unsigned(data_in_swapped);
                        when 2 =>
                            last_seq_nr_next(0)     <= unsigned(data_in_swapped);
                        when 3 =>
                            last_seq_nr_next(1)     <= unsigned(data_in_swapped);
                            -- NOTE: Rest of GAP Message is ignored
                            stage_next              <= PROCESS_GAP_SEQUENCE_NUMBERS;
                        when others =>
                            null;
                    end case;
                end if;
            when PROCESS_GAP_SEQUENCE_NUMBERS =>
                -- Wait for Sequence Number to be fetched from buffer
                if (mem_op_done = '1') then
                    -- Sender known
                    if (addr_res /= MAX_ADDRESS) then
                        -- XXX: This logic relies on the sender marking continuous GAPs with the GAP Start Sequence Number and GAP End Sequence Number Set Base (i.e. the first bit in the bitmask should be 0/not in GAP)
                        -- GAP only relevant if next expected sequence number in GAP
                        if (first_seq_nr <= mem_seq_nr and last_seq_nr >= mem_seq_nr) then
                            -- Store GAP end as last sequence number
                            next_seq_nr_next                <= last_seq_nr + 1;
                            mem_opcode                      <= UPDATE_PARTICIPANT;
                            update_participant_flags_next   <= (EDP_SEQ_NR_FLAG => '1', others => '0');
                            mem_op_start                    <= '1';
                        end if;
                    end if;
                    -- DONE
                    stage_next  <= SKIP_PACKET;
                end if;
            when PROCESS_HEARTBEAT =>
                -- Input FIFO Guard
                if (empty = '0') then
                    rd_guard    := '1';
                    cnt_next    <= cnt + 1;
                    
                    -- Latch Sequence Numbers
                    case (cnt) is
                        when 0 =>
                            first_seq_nr_next(0)    <= unsigned(data_in_swapped);
                        when 1 =>
                            first_seq_nr_next(1)    <= unsigned(data_in_swapped);
                        when 2 =>
                            last_seq_nr_next(0)     <= unsigned(data_in_swapped);
                        when 3 =>
                            last_seq_nr_next(1)     <= unsigned(data_in_swapped);
                            stage_next              <= PROCESS_HEARTBEAT_SEQUENCE_NUMBERS;
                        when others =>
                            null;
                    end case;
                end if;
            when PROCESS_HEARTBEAT_SEQUENCE_NUMBERS =>
                -- Wait for Participant Search to finish
                if (mem_op_done = '1') then
                    -- Participant in Buffer
                    if (addr_res /= MAX_ADDRESS) then
                        -- No scheduled Heartbeat Response
                        if (mem_participant_data.heartbeat_res_time = 0) then
                            -- If current Sequence Number obsolete (removed from source history cache)
                            if (first_seq_nr > mem_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_opcode                      <= UPDATE_PARTICIPANT;
                                deadline_next                   <= time + PARTICIPANT_HEARTBEAT_RESPONSE_DELAY;
                                -- NOTE: Last Bit denotes if this is Response or Suppression Delay
                                deadline_next(1)(0)             <= '0';
                                update_participant_flags_next   <= (EDP_SEQ_NR_FLAG => '1', HEARTBEAT_RES_TIME_FLAG => '1', others => '0');
                                mem_op_start                    <= '1';
                            -- If new Sequence Number is available or Writer expects ACKNACK
                            elsif (last_seq_nr >= mem_seq_nr or final_flag = '0') then
                                -- Set Response Delay
                                mem_opcode                      <= UPDATE_PARTICIPANT;
                                deadline_next                   <= time + PARTICIPANT_HEARTBEAT_RESPONSE_DELAY;
                                -- NOTE: Last Bit denotes if this is Response or Suppression Delay
                                deadline_next(1)(0)             <= '0';
                                update_participant_flags_next   <= (HEARTBEAT_RES_TIME_FLAG => '1', others => '0');
                                mem_op_start                    <= '1';
                            end if;
                        -- Currently in Heartbeat Response Delay
                        elsif (mem_participant_data.heartbeat_res_time(1)(0) = '0') then
                            -- If current Sequence Number obsolete (removed from source history cache)
                            if (first_seq_nr > mem_seq_nr and first_seq_nr <= last_seq_nr) then
                                -- Store new expected Sequence Number
                                next_seq_nr_next                <= first_seq_nr;
                                mem_opcode                      <= UPDATE_PARTICIPANT;
                                update_participant_flags_next   <= (EDP_SEQ_NR_FLAG => '1', others => '0');
                                mem_op_start                    <= '1';
                            end if;
                        end if;
                    end if;
                    -- DONE
                    stage_next  <= SKIP_PACKET;
                end if;
            when PROCESS_ACKNACK =>
                if (empty = '0') then
                    rd_guard    := '1';
                    cnt_next    <= cnt + 1;
                    
                    -- Latch Sequence Number
                    -- NOTE: Because we always sent the entire history cache, we only need to look at the SequenceNumberSetBase to determine if we need to sent data or not
                    case (cnt) is
                        when 0 =>
                            first_seq_nr_next(0)    <= unsigned(data_in_swapped);
                        when 1 =>
                            first_seq_nr_next(1)    <= unsigned(data_in_swapped);
                            
                            stage_next              <= PROCESS_ACKNACK_SEQUENCE_NUMBERS;
                        when others =>
                            null;
                    end case;
                end if;
            when PROCESS_ACKNACK_SEQUENCE_NUMBERS =>
                -- Wait for Participant Search to finish
                if (mem_op_done = '1') then
                    -- Participant in Buffer
                    if (addr_res /= MAX_ADDRESS) then
                        -- No scheduled Acknack Response
                        if (mem_participant_data.acknack_res_time = 0) then
                            case (message_type) is
                                when EDP =>
                                    -- Subscriber Acknack
                                    if (is_subscriber = '1') then
                                        -- If Reader has not ACKed all Publisher History Cache
                                        if (first_seq_nr <= PUB_SEQUENCENUMBER) then
                                            -- Set Acknack Response Time and Publisher Data as Acknack Response
                                            mem_opcode                      <= UPDATE_PARTICIPANT;
                                            deadline_next                   <= time + PARTICIPANT_ACKNACK_RESPONSE_DELAY;
                                            -- NOTE: Last Bit denotes if this is Response or Suppression Delay
                                            deadline_next(1)(0)             <= '0';
                                            extra_flags_next                <= mem_participant_data.extra_flags;
                                            extra_flags_next(PUB_DATA_FLAG) <= '1';
                                            update_participant_flags_next   <= (EXTRA_FLAGS_FLAG => '1', ACKNACK_RES_TIME_FLAG => '1', others => '0');
                                            mem_op_start                    <= '1';
                                        end if;
                                    -- Publisher Acknack
                                    else
                                        -- If Reader has not ACKed all Subscriber History Cache
                                        if (first_seq_nr <= SUB_SEQUENCENUMBER) then
                                            -- Set Acknack Response Time and set Subscriber Data as Acknack Response
                                            mem_opcode                      <= UPDATE_PARTICIPANT;
                                            deadline_next                   <= time + PARTICIPANT_ACKNACK_RESPONSE_DELAY;
                                            -- NOTE: Last Bit denotes if this is Response or Suppression Delay
                                            deadline_next(1)(0)             <= '0';
                                            extra_flags_next                <= mem_participant_data.extra_flags;
                                            extra_flags_next(SUB_DATA_FLAG) <= '1';
                                            update_participant_flags_next   <= (EXTRA_FLAGS_FLAG => '1', ACKNACK_RES_TIME_FLAG => '1', others => '0');
                                            mem_op_start                    <= '1';
                                        end if;
                                    end if;
                                -- Message Acknack
                                when MESSAGE =>
                                    -- NOTE:    "auto_live_seq_nr" always has the higher sequence number by design, so we just need to check against that
                                    -- If Reader has not ACKed all Message History Cache
                                    if (first_seq_nr <= auto_live_seq_nr) then
                                        -- Set Acknack Response Time and set Message Data as Acknack Response
                                        mem_opcode                      <= UPDATE_PARTICIPANT;
                                        deadline_next                   <= time + PARTICIPANT_ACKNACK_RESPONSE_DELAY;
                                        -- NOTE: Last Bit denotes if this is Response or Suppression Delay
                                        deadline_next(1)(0)             <= '0';
                                        extra_flags_next                <= mem_participant_data.extra_flags;
                                        extra_flags_next(MES_DATA_FLAG) <= '1';
                                        update_participant_flags_next   <= (EXTRA_FLAGS_FLAG => '1', ACKNACK_RES_TIME_FLAG => '1', others => '0');
                                        mem_op_start                    <= '1';
                                    end if;
                                when others =>
                                    null;
                            end case;
                        -- Currently in Acknack Response Delay
                        elsif (mem_participant_data.acknack_res_time(1)(0) = '0') then
                            case (message_type) is
                                when EDP =>
                                    -- Subscriber Acknack
                                    if (is_subscriber = '1') then
                                        -- Publisher Data not scheduled for response
                                        if (mem_participant_data.extra_flags(PUB_DATA_FLAG) = '0') then
                                            -- If Reader has not ACKed all Publisher History Cache
                                            if (first_seq_nr <= PUB_SEQUENCENUMBER) then
                                                -- Set Publisher Data as Acknack Response
                                                mem_opcode                      <= UPDATE_PARTICIPANT;
                                                extra_flags_next                <= mem_participant_data.extra_flags;
                                                extra_flags_next(PUB_DATA_FLAG) <= '1';
                                                update_participant_flags_next   <= (EXTRA_FLAGS_FLAG => '1', others => '0');
                                                mem_op_start                    <= '1';
                                            end if;
                                        end if;
                                    -- Publisher Acknack
                                    else
                                        -- Subscriber Data not scheduled for response
                                        if (mem_participant_data.extra_flags(SUB_DATA_FLAG) = '0') then
                                            -- If Reader has not ACKed all Subscriber History Cache
                                            if (first_seq_nr <= SUB_SEQUENCENUMBER) then
                                                -- Set Subscriber Data as Acknack Response
                                                mem_opcode                      <= UPDATE_PARTICIPANT;
                                                extra_flags_next                <= mem_participant_data.extra_flags;
                                                extra_flags_next(SUB_DATA_FLAG) <= '1';
                                                update_participant_flags_next   <= (EXTRA_FLAGS_FLAG => '1', others => '0');
                                                mem_op_start                    <= '1';
                                            end if;
                                        end if;
                                    end if;
                                -- Message Acknack
                                when MESSAGE =>
                                    -- Message Data not scheduled for response
                                    if (mem_participant_data.extra_flags(MES_DATA_FLAG) = '0') then
                                        -- NOTE:    "auto_live_seq_nr" always has the higher sequence number by design, so we just need to
                                        --          check against that
                                        -- If Reader has not ACKed all Message History Cache
                                        if (first_seq_nr <= auto_live_seq_nr) then
                                            -- Set Message Data as Acknack Response
                                            mem_opcode                      <= UPDATE_PARTICIPANT;
                                            extra_flags_next                <= mem_participant_data.extra_flags;
                                            extra_flags_next(MES_DATA_FLAG) <= '1';
                                            update_participant_flags_next   <= (EXTRA_FLAGS_FLAG => '1', others => '0');
                                            mem_op_start                    <= '1';
                                        end if;
                                    end if;
                                when others =>
                                    null;
                            end case;
                        end if;
                    end if;
                    -- DONE
                    stage_next  <= SKIP_PACKET;
                end if;
            when FIND_PARTICIPANT_DEST =>
                -- Wait for Next Participant to be Fetched
                if (mem_op_done = '1') then
                    -- No more Participants
                    if (addr_res = MAX_ADDRESS) then
                        -- DONE
                        stage_next  <= IDLE;
                    else
                        stage_next          <= SEND_HEADER;
                        return_stage_next   <= SEND_HEARTBEAT;
                        cnt_next            <= 0;
                    end if;
                end if;
            when PARTICIPANT_STALE_CHECK =>
                -- Wait for Stale Search to finish
                if (mem_op_done = '1') then
                    -- Found Stale Entry
                    if (addr_res /= MAX_ADDRESS) then
                        -- Participant Lease Expired
                        -- NOTE:    The mem_participant_data is zero initialized on lease expiration, so we check the default address for zero 
                        --          (since the default address should always be set)
                        if (mem_participant_data.def_addr = IPv4_ADDRESS_INVALID) then
                            -- Remove Participant
                            mem_opcode      <= REMOVE_PARTICIPANT;
                            mem_op_start    <= '1';
                            stage_next      <= LATCH_REMOVED_GUIDPREFIX;
                        -- Response Time Reached
                        else
                            -- Heartbeat Response
                            if (is_heartbeat_res = '1') then
                                -- If Suppression Delay passed, zero the time
                                if(mem_participant_data.heartbeat_res_time(1)(0) = '1') then
                                    -- Zero Heartbeat Response Time
                                    mem_opcode                      <= UPDATE_PARTICIPANT;
                                    deadline_next                   <= (others => (others => '0'));
                                    update_participant_flags_next   <= (HEARTBEAT_RES_TIME_FLAG => '1', others => '0');
                                    mem_op_start                    <= '1';
                                    -- DONE
                                    stage_next                      <= IDLE;
                                -- If Response Delay Passed
                                else
                                     -- Set Heartbeat Suppression Time
                                    mem_opcode                      <= UPDATE_PARTICIPANT;
                                    if (PARTICIPANT_HEARTBEAT_SUPPRESSION_DELAY /= 0) then
                                        -- Set Heartbeat Suppression Time
                                        deadline_next                   <= time + PARTICIPANT_HEARTBEAT_SUPPRESSION_DELAY;
                                        -- NOTE: Last Bit denotes if this is Response or Suppression Delay
                                        deadline_next(1)(0)             <= '1';
                                    else
                                        -- Zero Heartbeat Response Time
                                        deadline_next                   <= (others => (others => '0'));
                                    end if;
                                    update_participant_flags_next   <= (HEARTBEAT_RES_TIME_FLAG => '1', others => '0');
                                    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;
                            -- Acknack Response
                            else
                                -- If Suppression Delay passed, zero the time
                                if(mem_participant_data.acknack_res_time(1)(0) = '1') then
                                    -- Zero Acknack Response Time
                                    mem_opcode                      <= UPDATE_PARTICIPANT;
                                    deadline_next                   <= (others => (others => '0'));
                                    update_participant_flags_next   <= (ACKNACK_RES_TIME_FLAG => '1', others => '0');
                                    mem_op_start                    <= '1';
                                    -- DONE
                                    stage_next                      <= IDLE;
                                -- If Response Delay Passed
                                else
                                    mem_opcode                      <= UPDATE_PARTICIPANT;
                                    if (PARTICIPANT_ACKNACK_SUPPRESSION_DELAY /= 0) then
                                        -- Set Acknack Suppression Time
                                        deadline_next                   <= time + PARTICIPANT_ACKNACK_SUPPRESSION_DELAY;
                                        -- NOTE: Last Bit denotes if this is Response or Suppression Delay
                                        deadline_next(1)(0)             <= '1';
                                    else
                                        -- Zero Acknack Response Time
                                        deadline_next                   <= (others => (others => '0'));
                                    end if;
                                    -- Zero Data Response Flags
                                    extra_flags_next                <= mem_participant_data.extra_flags;
                                    extra_flags_next(PUB_DATA_FLAG) <= '0';
                                    extra_flags_next(SUB_DATA_FLAG) <= '0';
                                    extra_flags_next(MES_DATA_FLAG) <= '0';
                                    update_participant_flags_next   <= (EXTRA_FLAGS_FLAG => '1', ACKNACK_RES_TIME_FLAG => '1', others => '0');
                                    mem_op_start                    <= '1';
                                    -- Send Requested Data
                                    stage_next                      <= SEND_HEADER;
                                    return_stage_next               <= SEND_PUB_DATA;
                                    cnt_next                        <= 0;
                                end if;
                            end if;
                        end if;
                    -- No Stale Entry Found
                    else
                        -- DONE
                        stage_next  <= IDLE;
                    end if;
                end if;
            when LATCH_REMOVED_GUIDPREFIX =>
                if (mem_op_done = '1') then
                    -- Help Stage needed to latch the GUID Prefix of the removed staled participant
                    guid_next(0)    <= mem_guidprefix(0);
                    guid_next(1)    <= mem_guidprefix(1);
                    guid_next(2)    <= mem_guidprefix(2);
                    stage_next      <= INFORM_ENDPOINTS_PARTICIPANT_UNMATCH;
                    cnt_next        <= 0;
                end if;
            when PROCESS_PL =>
                -- 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;
                    
                    -- TODO: DDS-XTYPES: When reading data, implementations of this specification shall be robust to any setting of the FLAG_MUST_UNDERSTAND bit and accept the parameter nevertheless.
                    
                    -- NOTE: In-line QoS is completely ignored
                    
                    case (parameter_id) is
                        when PID_PARTICIPANT_GUID =>
                            -- Ignore in-line QoS
                            -- Only relevant for Participant Discovery Protocol
                            if(qos_flag = '0' and message_type = PDP) then
                                stage_next  <= MATCH_GUID;
                                cnt_next    <= 0;
                            end if;
                        when PID_DOMAIN_ID =>
                            -- Ignore in-line QoS
                            -- Only relevant for Participant Discovery Protocol
                            if(qos_flag = '0' and message_type = PDP) then
                                stage_next <= MATCH_DOMAIN_ID;
                            end if;
                        when PID_DOMAIN_TAG =>
                            -- Ignore in-line QoS
                            -- Only relevant for Participant Discovery Protocol
                            if(qos_flag = '0' and message_type = PDP) then
                                stage_next          <= LATCH_STRING_LENGTH;
                                -- Mark String contents (Needed for string comparison)
                                string_content_next <= DOMAIN_TAG_TYPE;
                                -- Mark reception of DOMAIN_TAG
                                rcvd_next(DOMAIN_TAG_RCVD) <= '1';
                            end if;
                        when PID_PROTOCOL_VERSION =>
                            -- Ignore in-line QoS
                            -- Only relevant for Participant Discovery Protocol
                            if(qos_flag = '0' and message_type = PDP) then
                                stage_next <= MATCH_PROTOCOL_VERSION;
                            end if;
                        when PID_DEFAULT_UNICAST_LOCATOR =>
                            -- Ignore in-line QoS
                            -- Only relevant for Participant Discovery Protocol
                            if(qos_flag = '0' and message_type = PDP) then
                                stage_next              <= LATCH_LOCATOR;
                                cnt_next                <= 0;
                                is_meta_addr_next       <= '0';
                            end if;
                        when PID_DEFAULT_MULTICAST_LOCATOR =>
                            -- Ignore in-line QoS
                            -- Only relevant for Participant Discovery Protocol
                            if(qos_flag = '0' and message_type = PDP) then
                                stage_next              <= LATCH_LOCATOR;
                                cnt_next                <= 0;
                                is_meta_addr_next       <= '0';
                            end if;
                        when PID_METATRAFFIC_UNICAST_LOCATOR =>
                            -- Ignore in-line QoS
                            -- Only relevant for Participant Discovery Protocol
                            if(qos_flag = '0' and message_type = PDP) then
                                stage_next              <= LATCH_LOCATOR;
                                cnt_next                <= 0;
                                is_meta_addr_next       <= '1';
                            end if;
                        when PID_METATRAFFIC_MULTICAST_LOCATOR =>
                            -- Ignore in-line QoS
                            -- Only relevant for Participant Discovery Protocol
                            if(qos_flag = '0' and message_type = PDP) then
                                stage_next              <= LATCH_LOCATOR;
                                cnt_next                <= 0;
                                is_meta_addr_next       <= '1';
                            end if;
                        when PID_PARTICIPANT_LEASE_DURATION =>
                            -- Ignore in-line QoS
                            -- Only relevant for Participant Discovery Protocol
                            if(qos_flag = '0' and message_type = PDP) then
                                stage_next  <= LATCH_LEASE_DURATION;
                                cnt_next    <= 0;
                            end if;
                        when PID_BUILTIN_ENDPOINT_SET =>
                            -- Ignore in-line QoS
                            -- Only relevant for Participant Discovery Protocol
                            if(qos_flag = '0' and message_type = PDP) then
                                stage_next  <= CHECK_REMOTE_BUILTIN_ENDPOINTS;
                            end if;
                        when PID_BUILTIN_ENDPOINT_QOS =>
                            -- Only relevant for Participant Discovery Protocol
                            -- NOTE:    As of DDSI-RTPS 2.3 the PID_BUILTIN_ENDPOINT_QOS only contains the BEST_EFFORT_PARTICIPANT_MESSAGE_DATA_READER bit
                            --          Due to how the built-in Endpoint is behaving, this bit as no impact in our case and we can safely ignore it.
                            -- Ignore
                            null;
                        when PID_TOPIC_NAME =>
                            -- Ignore in-line QoS
                            -- Only relevant for Endpoint Discovery Protocol
                            if(qos_flag = '0' and message_type = EDP) then
                                stage_next <= LATCH_STRING_LENGTH;
                                -- Mark String contents (Needed for string comparison)
                                string_content_next  <= TOPIC_NAME_TYPE;
                            end if;
                        when PID_TYPE_NAME =>
                            -- Ignore in-line QoS
                            -- Only relevant for Endpoint Discovery Protocol
                            if(qos_flag = '0' and message_type = EDP) then
                                stage_next <= LATCH_STRING_LENGTH;
                                -- Mark String contents (Needed for string comparison)
                                string_content_next  <= TYPE_NAME_TYPE;
                            end if;
                        when PID_DURABILITY =>
                            -- Ignore in-line QoS
                            -- Only relevant for Endpoint Discovery Protocol
                            if(qos_flag = '0' and message_type = EDP) then
                                stage_next <= RXO_DURABILITY;
                                rcvd_next(DURABILITY_QOS_RCVD_FLAG) <= '1';
                            end if;
                        when PID_DEADLINE =>
                            -- Ignore in-line QoS
                            -- Only relevant for Endpoint Discovery Protocol
                            if(qos_flag = '0' and message_type = EDP) then
                                stage_next  <= RXO_DEADLINE;
                                cnt_next    <= 0;
                                rcvd_next(DEADLINE_QOS_RCVD_FLAG)    <= '1';
                            end if;
                        when PID_LIVELINESS =>
                            -- Ignore in-line QoS
                            -- Only relevant for Endpoint Discovery Protocol
                            if(qos_flag = '0' and message_type = EDP) then
                                stage_next <= RXO_LIVELINESS;
                                rcvd_next(LIVELINESS_QOS_RCVD_FLAG) <= '1';
                            end if;
                        when PID_RELIABILITY =>
                            -- Ignore in-line QoS
                            -- Only relevant for Endpoint Discovery Protocol
                            if(qos_flag = '0' and message_type = EDP) then
                                stage_next  <= RXO_RELIABILITY;
                                cnt_next    <= 0;
                                rcvd_next(RELIABILITY_QOS_RCVD_FLAG)    <= '1';
                            end if;
                        when PID_DESTINATION_ORDER =>
                            -- Ignore in-line QoS
                            -- Only relevant for Endpoint Discovery Protocol
                            if(qos_flag = '0' and message_type = EDP) then
                                stage_next <= RXO_DESTINATION_ORDER;
                                rcvd_next(DESTINATION_ORDER_QOS_RCVD_FLAG)  <= '1';
                            end if;
                        when PID_OWNERSHIP =>
                            -- Ignore in-line QoS
                            -- Only relevant for Endpoint Discovery Protocol
                            if(qos_flag = '0' and message_type = EDP) then
                                stage_next  <= RXO_OWNERSHIP;
                                rcvd_next(OWNERSHIP_QOS_RCVD_FLAG)  <= '1';
                            end if;
                        when PID_PRESENTATION =>
                            -- Ignore in-line QoS
                            -- Only relevant for Endpoint Discovery Protocol
                            if(qos_flag = '0' and message_type = EDP) then
                                stage_next  <= RXO_PRESENTATION;
                                cnt_next    <= 0;
                                rcvd_next(PRESENTATION_QOS_RCVD_FLAG)   <= '1';
                            end if;
                        when PID_PARTITION =>
                            -- Ignore in-line QoS
                            -- Only relevant for Endpoint Discovery Protocol
                            if(qos_flag = '0' and message_type = EDP) then
                                stage_next <= RXO_PARTITION;
                            end if;
                        when PID_LATENCY_BUDGET =>
                            -- Ignore in-line QoS
                            -- Only relevant for Endpoint Discovery Protocol
                            if(qos_flag = '0' and message_type = EDP) then
                                stage_next  <= RXO_LATENCY_BUDGET;
                                cnt_next    <= 0;
                                rcvd_next(LATENCY_BUDGET_QOS_RCVD_FLAG) <= '1';
                            end if;
                        when PID_UNICAST_LOCATOR =>
                            -- Ignore in-line QoS
                            -- Only relevant for Endpoint Discovery Protocol
                            if(qos_flag = '0' and message_type = EDP) then
                                stage_next              <= LATCH_LOCATOR;
                                cnt_next                <= 0;
                                -- Mark Latch 1 for storing
                                is_meta_addr_next       <= '0';
                            end if;
                        when PID_MULTICAST_LOCATOR =>
                            -- Ignore in-line QoS
                            -- Only relevant for Endpoint Discovery Protocol
                            if(qos_flag = '0' and message_type = EDP) then
                                stage_next              <= LATCH_LOCATOR;
                                cnt_next                <= 0;
                                -- Mark Latch 1 for storing
                                is_meta_addr_next       <= '0';
                            end if;
                        when PID_ENDPOINT_GUID =>
                            -- Ignore in-line QoS
                            -- Only relevant for Endpoint Discovery Protocol
                            if(qos_flag = '0' and message_type = EDP) then
                                stage_next  <= MATCH_GUID;
                                cnt_next    <= 0;
                            end if;
                        when PID_DATA_MAX_SIZE_SERIALIZED =>
                            -- Ignore in-line QoS
                            -- Only relevant for Endpoint Discovery Protocol
                            if(qos_flag = '0' and message_type = EDP) then
                                stage_next  <= CHECK_MAX_SIZE_SERIALIZED;
                            end if;
                        when PID_EXPECTS_INLINE_QOS =>
                            stage_next  <= LATCH_EXPECTS_INLINE_QOS;
                        when PID_PARTICIPANT_MANUAL_LIVELINESS_COUNT =>
                            -- Ignore
                            null;
                        when PID_CONTENT_FILTER_PROPERTY =>
                            -- Ignore
                            null;
                        when PID_GROUP_GUID =>
                            -- Ignore
                            null;
                        when PID_PROPERTY_LIST =>
                            -- Ignore
                            null;
                        when PID_ENTITY_NAME =>
                            -- Ignore
                            null;
                        when PID_TIME_BASED_FILTER =>
                            -- Ignore
                            null;
                        when PID_TRANSPORT_PRIORITY =>
                            -- Ignore
                            null;
                        when PID_VENDORID =>
                            -- Ignore
                            null;
                        when PID_OWNERSHIP_STRENGTH =>
                            -- Ignore
                            null;
                        when PID_HISTORY =>
                            -- Ignore
                            null;
                        when PID_RESOURCE_LIMITS =>
                            -- Ignore
                            null;
                        when PID_LIFESPAN =>
                            -- Ignore
                            -- NOTE: Lifespan is requested via in-line QoS and handled directly by the endpoint
                            null;
                        when PID_DURABILITY_SERVICE =>
                            -- Ignore
                            null;
                        when PID_USER_DATA =>
                            -- Ignore
                            null;
                        when PID_GROUP_DATA =>
                            -- Ignore
                            null;
                        when PID_TOPIC_DATA =>
                            -- Ignore
                            null;
                        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 =>
                            -- Ignore
                            null;
                        when PID_STATUS_INFO =>
                            -- Ignore
                            null;
                        when PID_PAD =>
                            -- Ignore
                            null;
                        when PID_EXTENDED =>
                            -- TODO
                            null;
                        when PID_IGNORE =>
                            -- Ignore
                            null;
                        when PID_SENTINEL =>
                            parameter_end_next  <= (others => '1');
                            
                            -- If Processing in-line QoS until now, start processing data
                            if(qos_flag = '1') then
                                -- Override QoS Flag
                                qos_flag_next   <= '0';
                                stage_next      <= PROCESS_DATA;
                            -- Else check Defaults
                            else
                                stage_next  <= CHECK_DEFAULT;
                            end if;
                        when PID_LIST_END =>
                            -- TODO
                            null;
                        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 Uknown Parameter
                            else
                                stage_next  <= SKIP_PARAMETER;
                            end if;
                    end case;
                end if;
            when CHECK_DEFAULT =>
                -- DEFAULT
                stage_next      <= PARTICIPANT_MATCH_STAGE;
                
                case (message_type) is
                    when PDP =>
                        -- Check Domain Tag against Default if necessary
                        if (rcvd(DOMAIN_TAG_RCVD) = '0' and DOMAIN_TAG /= DEFAULT_DOMAIN_TAG) then
                            participant_match_next  <= '0';
                        end if;
                    when EDP =>
                        -- Defaults
                        tmp_endpoint_mask   := (others => '1');
                        if (is_subscriber = '1') then
                            tmp_default_qos_match   := READER_DEFAULT_QOS_MATCH;
                        else
                            tmp_default_qos_match   := WRITER_DEFAULT_QOS_MATCH;
                        end if;
                        
                        if (rcvd(DURABILITY_QOS_RCVD_FLAG) = '0') then
                            tmp_endpoint_mask   := tmp_endpoint_mask and tmp_default_qos_match.DURABILITY_QOS;
                        end if;
                        if (rcvd(PRESENTATION_QOS_RCVD_FLAG) = '0') then
                            tmp_endpoint_mask   := tmp_endpoint_mask and tmp_default_qos_match.PRESENTATION_QOS;
                        end if;
                        if (rcvd(DEADLINE_QOS_RCVD_FLAG) = '0') then
                            tmp_endpoint_mask   := tmp_endpoint_mask and tmp_default_qos_match.DEADLINE_QOS;
                        end if;
                        if (rcvd(LATENCY_BUDGET_QOS_RCVD_FLAG) = '0') then
                            tmp_endpoint_mask   := tmp_endpoint_mask and tmp_default_qos_match.LATENCY_BUDGET_QOS;
                        end if;
                        if (rcvd(OWNERSHIP_QOS_RCVD_FLAG) = '0') then
                            tmp_endpoint_mask   := tmp_endpoint_mask and tmp_default_qos_match.OWNERSHIP_QOS;
                        end if;
                        if (rcvd(LIVELINESS_QOS_RCVD_FLAG) = '0') then
                            tmp_endpoint_mask   := tmp_endpoint_mask and tmp_default_qos_match.LIVELINESS_QOS;
                        end if;
                        if (rcvd(RELIABILITY_QOS_RCVD_FLAG) = '0') then
                            tmp_endpoint_mask   := tmp_endpoint_mask and tmp_default_qos_match.RELIABILITY_QOS;
                        end if;
                        if (rcvd(DESTINATION_ORDER_QOS_RCVD_FLAG) = '0') then
                            tmp_endpoint_mask   := tmp_endpoint_mask and tmp_default_qos_match.DESTINATION_ORDER_QOS;
                        end if;
                        
                        endpoint_mask_next  <= endpoint_mask and tmp_endpoint_mask;
                    when others =>
                        null;
                end case;
            when LATCH_STRING_LENGTH =>
                -- Input FIFO Guard
                if (empty = '0') then
                    rd_guard            := '1';
                    string_length_next  <= unsigned(data_in_swapped);
                    stage_next          <= COMPARE_STRING;
                    compare_length_next <= to_unsigned(1,compare_length'length);
                    cnt_next            <= 0;
                end if;
            when COMPARE_STRING =>
                -- Input FIFO Guard
                if (empty = '0') then
                    rd_guard            := '1';
                    compare_length_next <= compare_length + 1;
                    cnt_next            <= cnt + 1;
                    
                    -- NOTE: This assumes that the Parameter is padded with zeroes (Since we also compare the padding)
                    --       XTypes 7.4.1.1 requires padding bytes for "PLAIN_CDR" to be zero, but does not specify the value for "PL_CDR"
                    
                    -- Unmark matches on string comparison missmatch
                    case (string_content) is
                        when TOPIC_NAME_TYPE =>
                            for i in 0 to NUM_ENDPOINTS-1 loop
                                if (data_in /= ENDPOINT_TOPIC(i)(cnt)) then
                                    endpoint_mask_next(i)   <= '0';
                                end if;
                            end loop;
                        when TYPE_NAME_TYPE =>
                            for i in 0 to NUM_ENDPOINTS-1 loop
                                if (data_in /= ENDPOINT_TYPE(i)(cnt)) then
                                    endpoint_mask_next(i)   <= '0';
                                end if;
                            end loop;
                        when DOMAIN_TAG_TYPE =>
                            if (data_in /= DOMAIN_TAG(cnt)) then
                                    participant_match_next  <= '0';
                                end if;
                        when others =>
                            null;
                    end case;
                    
                    -- NOTE: "compare_length" counts the 4-Byte words, while "string_length" contains the total string octets/bytes
                    -- End of String (Exit Condition)
                    if ((compare_length & "00") >= string_length) then
                        -- DONE
                        stage_next  <= SKIP_PARAMETER;
                    end if;
                end if;
            when MATCH_GUID =>
                -- Input FIFO Guard
                if (empty = '0') then
                    rd_guard    := '1';
                    cnt_next    <= cnt + 1;
                    
                    -- Check if GUID Prefix valid (Should be the same as the GUID Prefix of the Packet) and update the Entity ID
                    case (cnt) is
                        when 0 =>
                            if (data_in /= guid(0)) then
                                stage_next  <= SKIP_PACKET;
                            end if;
                        when 1 =>
                            if (data_in /= guid(1)) then
                                stage_next  <= SKIP_PACKET;
                            end if;
                        when 2 =>
                            if (data_in /= guid(2)) then
                                stage_next  <= SKIP_PACKET;
                            end if;
                        when 3 =>
                            -- NOTE: Even though the mem_ctrl_prc is currently using the guid signal, it only uses the first 
                            --          3 words (GUIDPrefix) for the SEARCH_PARTICIPANT
                            guid_next(3)    <= data_in;
                            -- DONE
                            stage_next      <= SKIP_PARAMETER;
                        when others =>
                            null;
                    end case;
                end if;
            when MATCH_DOMAIN_ID =>
                -- Input FIFO Guard
                if (empty = '0') then
                    rd_guard    := '1';
                    
                    -- Match Domain ID
                    if (data_in_swapped /= DOMAIN_ID) then
                        participant_match_next  <= '0';
                    end if;
                    -- DONE
                    stage_next  <= SKIP_PARAMETER;
                end if;
            when MATCH_PROTOCOL_VERSION =>
                -- Input FIFO Guard
                if (empty = '0') then
                    rd_guard    := '1';
                    
                    -- Match major Protocol Version
                    if(data_in(31 downto 24) /= PROTOCOLVERSION_2_4(15 downto 8)) then
                        participant_match_next  <= '0';
                    end if;
                    -- DONE
                    stage_next  <= SKIP_PARAMETER;
                end if;
            when LATCH_LOCATOR =>
                -- Input FIFO Guard
                if (empty = '0') then
                    rd_guard    := '1';
                    cnt_next    <= cnt + 1;
                    
                    case (cnt) is
                        -- Locator Kind
                        when 0 =>
                            -- Check if UDPv4 Locator
                            if (data_in_swapped /= LOCATOR_KIND_UDPv4) then
                                -- Skip
                                stage_next  <= SKIP_PARAMETER;
                            end if;
                        -- Locator Port
                        when 1 =>
                            -- Latch Source Port
                            -- SANITY CHECK: Check if UDP Port valid
                            if (data_in_swapped(UDP_PORT_INVALID'range) /= UDP_PORT_INVALID) then
                                -- NOTE: We have to make sure that the address is also valid
                                -- Temporal Latch
                                long_latch_next <= data_in_swapped;
                            else
                                -- Ignore
                                stage_next      <= SKIP_PARAMETER;
                            end if;
                        -- Locator Address 1/4
                        when 2 =>
                            null;
                        -- Locator Address 2/4
                        when 3 =>
                            null;
                        -- Locator Address 3/4
                        when 4 =>
                            null;
                        -- Locator Address 4/4 (IPv4)
                        when 5 =>
                            -- Latch Src Addr
                            -- SANITY CHECK: Check if IPv4 Address valid
                            if (data_in_swapped /= IPv4_ADDRESS_INVALID) then
                                if (is_meta_addr = '0') then
                                    def_addr_next   <= data_in;
                                    def_port_next   <= long_latch(def_port'length-1 downto 0);
                                else
                                    meta_addr_next  <= data_in;
                                    meta_port_next  <= long_latch(meta_port'length-1 downto 0);
                                end if;
                            end if;
                            -- DONE
                            stage_next          <= SKIP_PARAMETER;
                        when others =>
                            null;
                    end case;
                end if;
            when LATCH_EXPECTS_INLINE_QOS =>
                -- Input FIFO Guard
                if (empty = '0') then
                    rd_guard    := '1';
                    
                    -- Latch 'expectsInlineQoS'
                    extra_flags_next(EXPECTS_INLINE_QOS_FLAG)           <= data_in(24);
                    reader_flags_next(READER_EXPECTS_INLINE_QOS_FLAG)   <= data_in(24);
                    
                    -- DONE
                    stage_next  <= SKIP_PARAMETER;
                end if;
            when LATCH_LEASE_DURATION =>
                -- Input FIFO Guard
                if (empty = '0') then
                    rd_guard    := '1';
                    cnt_next    <= cnt + 1;
                    
                    -- Latch Lease Duration
                    case (cnt) is
                        when 0 =>
                            lease_duration_next(0)  <= unsigned(data_in_swapped);
                        when 1 =>
                            lease_duration_next(1)  <= unsigned(data_in_swapped);
                            -- DONE
                            stage_next              <= SKIP_PARAMETER;
                        when others =>
                            null;
                    end case;
                end if;
            when CHECK_REMOTE_BUILTIN_ENDPOINTS =>
                -- Input FIFO Guard
                if (empty = '0') then
                    rd_guard    := '1';
                    
                    -- SANITY CHECK (Builtin Participant and Participant Message Endpoints have to be always available)
                    if (    data_in_swapped(DISC_BUILTIN_ENDPOINT_PARTICIPANT_ANNOUNCER) = '0' or
                            data_in_swapped(DISC_BUILTIN_ENDPOINT_PARTICIPANT_DETECTOR) = '0' or
                            data_in_swapped(BUILTIN_ENDPOINT_PARTICIPANT_MESSAGE_DATA_WRITER) = '0' or
                            data_in_swapped(BUILTIN_ENDPOINT_PARTICIPANT_MESSAGE_DATA_READER) = '0') then
                        participant_match_next  <= '0';
                    end if;
                    -- Check for necessary Builtin Endpoints for Readers and Writers
                    if (    (NUM_READERS > 0 and (data_in_swapped(DISC_BUILTIN_ENDPOINT_PUBLICATIONS_ANNOUNCER) = '0' or data_in_swapped(DISC_BUILTIN_ENDPOINT_SUBSCRIPTIONS_DETECTOR) = '0')) and
                            (NUM_WRITERS > 0 and (data_in_swapped(DISC_BUILTIN_ENDPOINT_SUBSCRIPTIONS_ANNOUNCER) = '0' or data_in_swapped(DISC_BUILTIN_ENDPOINT_PUBLICATIONS_DETECTOR) = '0'))
                    ) then
                        participant_match_next  <= '0';
                    end if;
                    
                    -- DONE
                    stage_next  <= SKIP_PARAMETER;
                end if;
            when RXO_DURABILITY =>
                -- Input FIFO Guard
                if (empty = '0') then
                    rd_guard    := '1';
                    
                    -- Check QoS Compatibility (Unmark match on incompatibility)
                    -- COMPATIBLE (DDS v1.4): offered >= requested, with VOLATILE < TRANSIENT_LOCAL < TRANSIENT < PERSISTENT
                    for i in 0 to NUM_ENDPOINTS-1 loop
                        if (not check_qos_compatibility(is_subscriber, '1', unsigned(data_in_swapped), unsigned(ENDPOINT_DURABILITY_QOS(i)))) then
                            endpoint_mask_next(i) <= '0';
                        end if;
                    end loop;
                    
                    -- Set Reader Flag
                    if (is_subscriber = '1') then
                        reader_flags_next(READER_EXPECTS_HISTORICAL_DATA_FLAG)  <= '0' when (data_in_swapped = VOLATILE_DURABILITY_QOS) else '1';
                    end if;
                    
                    -- DONE
                    stage_next  <= SKIP_PARAMETER;
                end if;
            when RXO_DEADLINE =>
                -- Input FIFO Guard
                if (empty = '0') then
                    rd_guard    := '1';
                    cnt_next    <= cnt + 1;
                    
                    case (cnt) is
                        when 0 =>
                            long_latch_next <= data_in_swapped;
                        when 1 =>
                            tmp_dw  := (0 => unsigned(long_latch), 1 => unsigned(data_in_swapped));
                            -- Check QoS Compatibility (Unmark match on incompatibility)
                            -- COMPATIBLE (DDS v1.4): offered <= requested
                            for i in 0 to NUM_ENDPOINTS-1 loop
                                if (not check_qos_compatibility(is_subscriber, '0', tmp_dw, ENDPOINT_DEADLINE_QOS(i))) then
                                    endpoint_mask_next(i) <= '0';
                                end if;
                            end loop;
                            -- DONE
                            stage_next  <= SKIP_PARAMETER;
                        when others =>
                            null;
                    end case;
                end if;
            when RXO_LIVELINESS =>
                -- Input FIFO Guard
                if (empty = '0') then
                    rd_guard    := '1';
                    
                    -- Check QoS Compatibility (Unmark match on incompatibility)
                    -- COMPATIBLE (DDS v1.4): offered >= requested, with AUTOMATIC < MANUAL_BY_PARTICIPANT < MANUAL_BY_TOPIC
                    for i in 0 to NUM_ENDPOINTS-1 loop
                        if (not check_qos_compatibility(is_subscriber, '1', unsigned(data_in_swapped), unsigned(ENDPOINT_LIVELINESS_QOS(i)))) then
                            endpoint_mask_next(i) <= '0';
                        end if;
                    end loop;
                    
                    stage_next  <= RXO_LEASE_DURATION;
                    cnt_next    <= 0;
                end if;
            when RXO_LEASE_DURATION =>
                -- Input FIFO Guard
                if (empty = '0') then
                    rd_guard    := '1';
                    cnt_next    <= cnt + 1;
                    
                    case (cnt) is
                        when 0 =>
                            -- Temporal Latch
                            long_latch_next <= data_in_swapped;
                        when 1 =>
                            tmp_dw  := (0 => unsigned(long_latch), 1 => unsigned(data_in_swapped));
                            -- Check QoS Compatibility (Unmark match on incompatibility)
                            -- COMPATIBLE (DDS v1.4): offered <= requested
                            for i in 0 to NUM_ENDPOINTS-1 loop
                                if (not check_qos_compatibility(is_subscriber, '0', tmp_dw, ENDPOINT_LEASE_DURATION(i))) then
                                    endpoint_mask_next(i) <= '0';
                                end if;
                            end loop;
                            -- DONE
                            stage_next  <= SKIP_PARAMETER;
                        when others =>
                            null;
                    end case;
                end if;
            when RXO_RELIABILITY =>
                -- Input FIFO Guard
                if (empty = '0') then
                    rd_guard    := '1';
                    cnt_next    <= cnt + 1;
                    
                    case (cnt) is
                        when 0 =>
                            -- Check QoS Compatibility (Unmark match on incompatibility)
                            -- COMPATIBLE (DDS v1.4): offered >= requested, with BEST_EFFORT < RELIABLE
                            for i in 0 to NUM_ENDPOINTS-1 loop
                                if (not check_qos_compatibility(is_subscriber, '1', unsigned(data_in_swapped), unsigned(ENDPOINT_RELIABILITY_QOS(i)))) then
                                    endpoint_mask_next(i) <= '0';
                                end if;
                            end loop;
                            
                            -- Set Reader Flag
                            if (is_subscriber = '1') then
                                reader_flags_next(READER_IS_BEST_EFFORT_FLAG)  <= '1' when (data_in_swapped = BEST_EFFORT_RELIABILITY_QOS) else '0';
                            end if;
                        when 1 =>
                            -- NOTE: The max_blocking_time value is ignored
                            null;
                        when 2 =>
                            -- DONE
                            stage_next  <= SKIP_PARAMETER;
                        when others =>
                            null;
                    end case;
                end if;
            when RXO_DESTINATION_ORDER =>
                -- Input FIFO Guard
                if (empty = '0') then
                    rd_guard    := '1';
                    
                    -- Check QoS Compatibility (Unmark match on incompatibility)
                    -- COMPATIBLE (DDS v1.4): offered >= requested, with BY_RECEPTION_TIMESTAMP < BY_SOURCE_TIMESTAMP
                    for i in 0 to NUM_ENDPOINTS-1 loop
                        if (not check_qos_compatibility(is_subscriber, '1', unsigned(data_in_swapped), unsigned(ENDPOINT_DESTINATION_ORDER_QOS(i)))) then
                            endpoint_mask_next(i) <= '0';
                        end if;
                    end loop;
                    -- DONE
                    stage_next  <= SKIP_PARAMETER;
                end if;
            when RXO_OWNERSHIP =>
                -- Input FIFO Guard
                if (empty = '0') then
                    rd_guard    := '1';
                    
                    -- Check QoS Compatibility (Unmark match on incompatibility)
                    -- COMPATIBLE (DDS v1.4): offered = requested
                    for i in 0 to NUM_ENDPOINTS-1 loop
                        if (data_in_swapped /= ENDPOINT_OWNERSHIP_QOS(i)) then
                            endpoint_mask_next(i) <= '0';
                        end if;
                    end loop;
                    -- DONE
                    stage_next  <= SKIP_PARAMETER;
                end if;
            when RXO_PRESENTATION =>
                -- Input FIFO Guard
                if (empty = '0') then
                    rd_guard    := '1';
                    cnt_next    <= cnt + 1;
                    
                    case (cnt) is
                        when 0 =>
                            -- Check QoS Compatibility (Unmark match on incompatibility)
                            -- COMPATIBLE (DDS v1.4): offered >= requested, with INSTANCE < TOPIC < GROUP
                            for i in 0 to NUM_ENDPOINTS-1 loop
                                if (not check_qos_compatibility(is_subscriber, '1', unsigned(data_in_swapped), unsigned(ENDPOINT_PRESENTATION_QOS(i)))) then
                                    endpoint_mask_next(i) <= '0';
                                end if;
                            end loop;
                        when 1 =>
                            -- Check QoS Compatibility (Unmark match on incompatibility)
                            for i in 0 to NUM_ENDPOINTS-1 loop
                                -- data-in is Subscriber-Requested
                                -- COMPATIBLE (DDS v1.4): requested=FALSE or requested=offered=TRUE
                                if (is_subscriber = '1') then
                                    if (data_in(24) = '1' and not ENDPOINT_COHERENT_ACCESS(i)) then
                                        endpoint_mask_next(i) <= '0';
                                    end if;
                                    if (data_in(16) = '1' and not ENDPOINT_ORDERED_ACCESS(i)) then
                                        endpoint_mask_next(i) <= '0';
                                    end if;
                                -- data-in is Publisher-Offered
                                else
                                    if (data_in(24) = '0' and ENDPOINT_COHERENT_ACCESS(i)) then
                                        endpoint_mask_next(i) <= '0';
                                    end if;
                                    if (data_in(16) = '0' and ENDPOINT_ORDERED_ACCESS(i)) then
                                        endpoint_mask_next(i) <= '0';
                                    end if;
                                end if;
                            end loop;
                            -- DONE
                            stage_next  <= SKIP_PARAMETER;
                        when others =>
                            null;
                    end case;
                end if;
            when RXO_PARTITION =>
                -- Input FIFO Guard
                if (empty = '0') then
                    rd_guard    := '1';
                    
                    -- NOTE: Endpoints are only matched against the default empty partition.
                    -- Match Partition Names
                    if (data_in_swapped /= (data_in_swapped'reverse_range => '0')) then
                        endpoint_mask_next <= (others => '0');
                    end if;
                    -- DONE
                    stage_next  <= SKIP_PARAMETER;
                end if;
            when RXO_LATENCY_BUDGET =>
                -- Input FIFO Guard
                if (empty = '0') then
                    rd_guard    := '1';
                    cnt_next    <= cnt + 1;
                    
                    case (cnt) is
                        when 0 =>
                            long_latch_next <= data_in_swapped;
                        when 1 =>
                            tmp_dw  := (0 => unsigned(long_latch), 1 => unsigned(data_in_swapped));
                            -- Check QoS Compatibility (Unmark match on incompatibility)
                            -- COMPATIBLE (DDS v1.4): offered <= requested
                            for i in 0 to NUM_ENDPOINTS-1 loop
                                if (not check_qos_compatibility(is_subscriber, '0', tmp_dw, ENDPOINT_LATENCY_BUDGET_QOS(i))) then
                                    endpoint_mask_next(i) <= '0';
                                end if;
                            end loop;
                            -- DONE
                            stage_next  <= SKIP_PARAMETER;
                        when others =>
                            null;
                    end case;
                end if;
            when CHECK_MAX_SIZE_SERIALIZED =>
                -- Input FIFO Guard
                if (empty = '0') then
                    rd_guard    := '1';
                    
                    -- Ignore Publishers that may need Fragmentation
                    if (is_subscriber = '0' and unsigned(data_in_swapped) > to_unsigned(UDP_MAX_SIZE_SERIALIZED, CDR_LONG_WIDTH)) then
                        endpoint_mask_next  <= (others => '0');
                    end if;
                    -- DONE
                    stage_next  <= SKIP_PARAMETER;
                end if;
            when PARTICIPANT_MATCH_STAGE =>
                -- Wait for Participant Search to finish
                if (mem_op_done = '1') then
                    -- Participant not in Buffer
                    if (addr_res = MAX_ADDRESS) then
                        -- Participant Match
                        if (message_type = PDP and participant_match = '1') then
                            -- Add participant in buffer
                            deadline_next   <= time + lease_duration;
                            mem_opcode      <= INSERT_PARTICIPANT;
                            mem_op_start    <= '1';
                            -- DONE
                            stage_next      <= SKIP_PACKET;
                        else
                            -- Ignore all other messages, since there is no corresponding participant in the buffer.
                            stage_next  <= SKIP_PACKET;
                        end if;
                    -- Participant in Buffer
                    else
                        -- Participant Announcement
                        if (message_type = PDP) then
                            -- Newer Sequence Number
                            if (mem_seq_nr <= seq_nr) then
                                -- Participant Unmatch
                                if (participant_match = '0') then
                                    -- Remove participant from buffer
                                    mem_opcode      <= REMOVE_PARTICIPANT;
                                    mem_op_start    <= '1';
                                    -- Inform ENDPOINTS
                                    stage_next      <= INFORM_ENDPOINTS_PARTICIPANT_UNMATCH;
                                    cnt_next        <= 0;
                                -- Participant remains matched
                                else
                                    -- Update Participant Data and Lease
                                    mem_opcode                      <= UPDATE_PARTICIPANT;
                                    deadline_next                   <= time + lease_duration;
                                    update_participant_flags_next   <= (PARTICIPANT_DATA_FLAG => '1', LEASE_DEADLINE_FLAG => '1', EXTRA_FLAGS_FLAG => '1', others => '0');
                                    mem_op_start                    <= '1';
                                    -- DONE
                                    stage_next                      <= SKIP_PACKET;
                                end if;
                            -- Old Sequence Number
                            else
                                -- Update Lease
                                mem_opcode                      <= UPDATE_PARTICIPANT;
                                deadline_next                   <= time + lease_duration;
                                update_participant_flags_next   <= (LEASE_DEADLINE_FLAG => '1', others => '0');
                                mem_op_start                    <= '1';
                                -- DONE
                                stage_next  <= SKIP_PACKET;
                            end if;
                        -- NOTE:    We only process the Sequences in order. This may lead to more traffic being exchanged 
                        --          (since all Data messages with a higher sequence number than the next expected will be ignored), 
                        --          but keeps the logic simple.
                        -- Endpoint (Next Expected Sequence Number)
                        elsif (message_type = EDP and mem_seq_nr = seq_nr) then
                            -- Store new Sequence Number
                            mem_opcode                      <= UPDATE_PARTICIPANT;
                            update_participant_flags_next   <= (EDP_SEQ_NR_FLAG => '1', others => '0');
                            mem_op_start                    <= '1';
                            
                            -- At least one local Endpoint match
                            if (endpoint_mask /= (endpoint_mask'range => '0')) then
                                -- Propagate Matches to local Endpoints
                                stage_next      <= INFORM_ENDPOINTS_MATCH;
                                cnt_next        <= 0;
                            -- No local Endpoint matches
                            else
                                -- Propagate Matches to local Endpoints
                                stage_next      <= INFORM_ENDPOINTS_UNMATCH;
                                cnt_next        <= 0;
                            end if;
                        else
                            -- Ignore (Messages are handled in a dedicated stage)
                            stage_next  <= SKIP_PACKET;
                        end if;
                    end if;
                end if;
            when INFORM_ENDPOINTS_MATCH =>
                -- Output FIFO Guard
                if ((endpoint_mask and endpoint_full) = (endpoint_full'range => '0')) then
                    wr_sig      <= '1';
                    cnt_next    <= cnt + 1;
                    
                    case (cnt) is
                        -- Match Opcode
                        when 0 =>
                            data_out  <= OPCODE_ENDPOINT_MATCH;
                        -- GUID Prefix 1/3
                        when 1 =>
                            data_out  <= guid(0);
                        -- GUID Prefix 2/3
                        when 2 =>
                            data_out  <= guid(1);
                        -- GUID Prefix 3/3
                        when 3 =>
                            data_out  <= guid(2);
                        -- Entity ID
                        when 4 =>
                            data_out  <= guid(3);
                        -- IPv4 Address
                        when 5 =>
                            -- If Endpoint did not set Address, use Participant Default
                            if (def_addr /= (def_addr'reverse_range => '0')) then
                                data_out  <= def_addr;
                            else
                                data_out  <= mem_participant_data.def_addr;
                            end if;
                        -- UDPv4 Port and Reader Flags
                        when 6 =>
                            -- Default
                            data_out        <= (others => '0');
                            last_word_out   <= '1';
                            
                            -- If Endpoint did not set Port, use Participant Default
                            if (def_port /= (def_port'reverse_range => '0')) then
                                data_out(31 downto 16)    <= def_port;
                            else
                                data_out(31 downto 16)    <= mem_participant_data.def_port;
                            end if;
                            
                            if (is_subscriber = '1') then
                                data_out(15 downto 0) <= reader_flags;
                            end if;
                            
                            -- DONE
                            stage_next  <= INFORM_ENDPOINTS_UNMATCH;
                            cnt_next    <= 0;
                        when others =>
                            null;
                    end case;
                end if;
            when INFORM_ENDPOINTS_UNMATCH =>
                -- Output FIFO Guard
                if (((not endpoint_mask) and endpoint_full) = (endpoint_full'range => '0')) then
                    wr_sig      <= '1';
                    cnt_next    <= cnt + 1;
                    
                    case (cnt) is
                        -- Match Opcode
                        when 0 =>
                            data_out  <= OPCODE_ENDPOINT_UNMATCH;
                        -- GUID Prefix 1/3
                        when 1 =>
                            data_out  <= guid(0);
                        -- GUID Prefix 2/3
                        when 2 =>
                            data_out  <= guid(1);
                        -- GUID Prefix 3/3
                        when 3 =>
                            data_out  <= guid(2);
                        -- Entity ID
                        when 4 =>
                            data_out        <= guid(3);
                            last_word_out   <= '1';
                            
                            -- DONE
                            stage_next  <= SKIP_PACKET;
                        when others =>
                            null;
                    end case;
                end if;
            when INFORM_ENDPOINTS_PARTICIPANT_UNMATCH =>
                -- Output FIFO Guard
                if (endpoint_full = (endpoint_full'range => '0')) then
                    wr_sig      <= '1';
                    cnt_next    <= cnt + 1;
                    
                    case (cnt) is
                        -- Match Opcode
                        when 0 =>
                            data_out  <= OPCODE_PARTICIPANT_UNMATCH;
                        -- GUID Prefix 1/3
                        when 1 =>
                            data_out  <= guid(0);
                        -- GUID Prefix 2/3
                        when 2 =>
                            data_out  <= guid(1);
                        -- GUID Prefix 3/3
                        when 3 =>
                            data_out  <= guid(2);
                            last_word_out   <= '1';
                            
                            -- DONE
                            stage_next  <= SKIP_PACKET;
                        when others =>
                            null;
                    end case;
                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 =>
                            -- Set Default Multicast Announce Address if Participant Announcement
                            if (return_stage = SEND_PARTICIPANT_ANNOUNCEMENT) then
                                data_out  <= DEFAULT_IPv4_META_ADDRESS;
                            else
                                data_out  <= mem_participant_data.meta_addr;
                            end if;
                        -- Src and Dest UDPv4 Ports
                        when 2 =>
                            -- Set Default Multicast Announce Port if Participant Announcement
                            if (return_stage = SEND_PARTICIPANT_ANNOUNCEMENT) then
                                data_out  <= META_IPv4_UNICAST_PORT & META_IPv4_MULTICAST_PORT;
                            else
                                data_out  <= META_IPv4_UNICAST_PORT & mem_participant_data.meta_port;
                            end if;
                        -- 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;
                            participant_data_cnt_next   <= 0;
                            publisher_data_cnt_next     <= 0;
                            subscriber_data_cnt_next    <= 0;
                        when others =>
                            null;
                    end case;
                end if;
            when SEND_PARTICIPANT_ANNOUNCEMENT =>
                if (rtps_full = '0') then
                    wr_sig                      <= '1';
                    participant_data_cnt_next   <= participant_data_cnt + 1;
                    
                    -- Send Participant Data
                    data_out  <= PARTICIPANT_DATA.data(participant_data_cnt);
                    
                    -- Exit Condition
                    if (participant_data_cnt = (PARTICIPANT_DATA.length-1)) then
                        last_word_out   <= '1';
                        -- DONE
                        stage_next      <= IDLE;
                    end if;
                end if;
            when SEND_ACKNACK =>
                if (rtps_full = '0') then
                    wr_sig      <= '1';
                    cnt_next    <= cnt + 1;
                    
                    case (cnt) is
                        -- ACKNACK RTPS SUBMESSAGE (Publication)
                        -- RTPS Submessage Header
                        when 0 =>
                            data_out  <= SID_ACKNACK & "00000010" & std_logic_vector(to_unsigned(24, 16));
                        -- Reader Entity ID
                        when 1 =>
                            data_out  <= ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_DETECTOR;
                        -- Writer Entity ID
                        when 2 =>
                            data_out  <= ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_ANNOUNCER;
                        -- Sequence Number Set (Bitmap Base 1/2)
                        when 3 =>
                            data_out  <= std_logic_vector(mem_participant_data.pub_seq_nr(0));
                        -- Sequence Number Set (Bitmap Base 2/2)
                        when 4 =>
                            data_out  <= std_logic_vector(mem_participant_data.pub_seq_nr(1));
                        -- Sequence Number Set (NumBits)
                        when 5 =>
                            data_out  <= (others => '0');
                        -- Count
                        when 6 =>
                            data_out  <= std_logic_vector(count);
                        -- ACKNACK RTPS SUBMESSAGE (Subscription)
                        -- RTPS Submessage Header
                        when 7 =>
                            data_out  <= SID_ACKNACK & "00000010" & std_logic_vector(to_unsigned(24, 16));
                        -- Reader Entity ID
                        when 8 =>
                            data_out  <= ENTITYID_SEDP_BUILTIN_PUBLICATIONS_DETECTOR;
                        -- Writer Entity ID
                        when 9 =>
                            data_out  <= ENTITYID_SEDP_BUILTIN_PUBLICATIONS_ANNOUNCER;
                        -- Sequence Number Set (Bitmap Base 1/2)
                        when 10 =>
                            data_out  <= std_logic_vector(mem_participant_data.sub_seq_nr(0));
                        -- Sequence Number Set (Bitmap Base 2/2)
                        when 11 =>
                            data_out  <= std_logic_vector(mem_participant_data.sub_seq_nr(1));
                        -- Sequence Number Set (NumBits)
                        when 12 =>
                            data_out  <= (others => '0');
                        -- Count
                        when 13 =>
                            data_out  <= std_logic_vector(count);
                        -- ACKNACK RTPS SUBMESSAGE (Message)
                        -- RTPS Submessage Header
                        when 14 =>
                            data_out  <= SID_ACKNACK & "00000010" & std_logic_vector(to_unsigned(24, 16));
                        -- Reader Entity ID
                        when 15 =>
                            data_out  <= ENTITYID_P2P_BUILTIN_PARTICIPANT_MESSAGE_READER;
                        -- Writer Entity ID
                        when 16 =>
                            data_out  <= ENTITYID_P2P_BUILTIN_PARTICIPANT_MESSAGE_WRITER;
                        -- Sequence Number Set (Bitmap Base 1/2)
                        when 17 =>
                            data_out  <= std_logic_vector(mem_participant_data.mes_seq_nr(0));
                        -- Sequence Number Set (Bitmap Base 2/2)
                        when 18 =>
                            data_out  <= std_logic_vector(mem_participant_data.mes_seq_nr(1));
                        -- Sequence Number Set (NumBits)
                        when 19 =>
                            data_out  <= (others => '0');
                        -- Count
                        when 20 =>
                            data_out      <= std_logic_vector(count);
                            last_word_out   <= '1';
                            
                            -- DONE
                            stage_next      <= IDLE;
                        when others =>
                            null;
                    end case;
                end if;
            when SEND_HEARTBEAT =>
                if (rtps_full = '0') then
                    wr_sig      <= '1';
                    cnt_next    <= cnt + 1;
                    
                    case (cnt) is
                        -- HEARTBEAT RTPS SUBMESSAGE (Publication)
                        -- RTPS Submessage Header
                        when 0 =>
                            data_out  <= SID_HEARTBEAT & "00000010" & std_logic_vector(to_unsigned(28, 16));
                        -- Reader Entity ID
                        when 1 =>
                            data_out  <= ENTITYID_SEDP_BUILTIN_PUBLICATIONS_DETECTOR;
                        -- Writer Entity ID
                        when 2 =>
                            data_out  <= ENTITYID_SEDP_BUILTIN_PUBLICATIONS_ANNOUNCER;
                        -- Sequence Number 1/2
                        when 3 =>
                            data_out  <= std_logic_vector(FIRST_SEQUENCENUMBER(0));
                        -- Sequence Number 2/2
                        when 4 =>
                            data_out  <= std_logic_vector(FIRST_SEQUENCENUMBER(1));
                        -- Sequence Number 1/2
                        when 5 =>
                            data_out  <= std_logic_vector(PUB_SEQUENCENUMBER(0));
                        -- Sequence Number 1/2
                        when 6 =>
                            data_out  <= std_logic_vector(PUB_SEQUENCENUMBER(1));
                        -- Count
                        when 7 =>
                            data_out  <= std_logic_vector(count);
                        -- HEARTBEAT RTPS SUBMESSAGE (Subscription)
                        -- RTPS Submessage Header
                        when 8 =>
                            data_out  <= SID_HEARTBEAT & "00000010" & std_logic_vector(to_unsigned(28, 16));
                        -- Reader Entity ID
                        when 9 =>
                            data_out  <= ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_DETECTOR;
                        -- Writer Entity ID
                        when 10 =>
                            data_out  <= ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_ANNOUNCER;
                        -- Sequence Number 1/2
                        when 11 =>
                            data_out  <= std_logic_vector(FIRST_SEQUENCENUMBER(0));
                        -- Sequence Number 2/2
                        when 12 =>
                            data_out  <= std_logic_vector(FIRST_SEQUENCENUMBER(1));
                        -- Sequence Number 1/2
                        when 13 =>
                            data_out  <= std_logic_vector(SUB_SEQUENCENUMBER(0));
                        -- Sequence Number 1/2
                        when 14 =>
                            data_out  <= std_logic_vector(SUB_SEQUENCENUMBER(1));
                        -- Count
                        when 15 =>
                            data_out  <= std_logic_vector(count);
                        -- HEARTBEAT RTPS SUBMESSAGE (Message)
                        -- RTPS Submessage Header
                        when 16 =>
                            data_out  <= SID_HEARTBEAT & "00000010" & std_logic_vector(to_unsigned(28, 16));
                        -- Reader Entity ID
                        when 17 =>
                            data_out  <= ENTITYID_P2P_BUILTIN_PARTICIPANT_MESSAGE_READER;
                        -- Writer Entity ID
                        when 18 =>
                            data_out  <= ENTITYID_P2P_BUILTIN_PARTICIPANT_MESSAGE_WRITER;
                        -- Sequence Number 1/2
                        when 19 =>
                            data_out  <= std_logic_vector(man_live_seq_nr(0));
                        -- Sequence Number 2/2
                        when 20 =>
                            data_out  <= std_logic_vector(man_live_seq_nr(1));
                        -- Sequence Number 1/2
                        when 21 =>
                            data_out  <= std_logic_vector(auto_live_seq_nr(0));
                        -- Sequence Number 1/2
                        when 22 =>
                            data_out  <= std_logic_vector(auto_live_seq_nr(1));
                        -- Count
                        when 23 =>
                            data_out  <= std_logic_vector(count);
                            -- If manual and automatic sequence numbers are not consecutive, we have only asserted the automatic liveliness
                            if (live_gap_start /= auto_live_seq_nr) then
                                stage_next  <= SEND_MES_AUTO_LIVE;
                                cnt_next    <= 0;
                            -- Else we need to send both (manual and automatic)
                            else
                                stage_next  <= SEND_MES_MAN_LIVE;
                                cnt_next    <= 0;
                            end if;
                        when others =>
                            null;
                    end case;
                end if;
            when SEND_PUB_DATA =>
                -- If Publisher Data not scheduled for response or no Writers available, skip
                if (mem_participant_data.extra_flags(PUB_DATA_FLAG) = '0' or NUM_WRITERS = 0) then
                    stage_next  <= SEND_SUB_DATA;
                -- Output FIFO Guard
                elsif (rtps_full = '0') then
                    wr_sig                  <= '1';
                    publisher_data_cnt_next <= publisher_data_cnt + 1;
                    
                    -- Send Publisher Data
                    data_out  <= WRITER_ENDPOINT_DATA.data(publisher_data_cnt);
                    
                    -- Exit Condition
                    if (publisher_data_cnt = (WRITER_ENDPOINT_DATA.length-1)) then
                        last_word_out   <= '1';
                        -- If we still have Data to sent, continue
                        if (mem_participant_data.extra_flags(SUB_DATA_FLAG) = '1' or mem_participant_data.extra_flags(MES_DATA_FLAG) = '1') then
                            stage_next          <= SEND_HEADER;
                            return_stage_next   <= SEND_SUB_DATA;
                            cnt_next            <= 0;
                        else
                            -- DONE
                            stage_next  <= IDLE;
                        end if;
                    end if;
                end if;
            when SEND_SUB_DATA =>
                -- If Subscriber Data not scheduled for response or no Readers available, skip
                if (mem_participant_data.extra_flags(PUB_DATA_FLAG) = '0' or NUM_READERS = 0) then
                    stage_next  <= SEND_MES_MAN_LIVE;
                    cnt_next    <= 0;
                -- Output FIFO Guard
                elsif (rtps_full = '0') then
                    wr_sig                      <= '1';
                    subscriber_data_cnt_next    <= subscriber_data_cnt + 1;
                    
                    -- Send Subscriber Data
                    data_out  <= READER_ENDPOINT_DATA.data(subscriber_data_cnt);
                    
                    -- Exit Condition
                    if (subscriber_data_cnt = (READER_ENDPOINT_DATA.length-1)) then
                        last_word_out   <= '1';
                        -- If we still have Data to sent, continue
                        if (mem_participant_data.extra_flags(MES_DATA_FLAG) = '1') then
                            stage_next          <= SEND_HEADER;
                            return_stage_next   <= SEND_MES_MAN_LIVE;
                            cnt_next            <= 0;
                        else
                            -- DONE
                            stage_next  <= IDLE;
                        end if;
                    end if;
                end if;
            when SEND_MES_MAN_LIVE =>
                -- Output FIFO Guard
                if (rtps_full = '0') then
                    wr_sig      <= '1';
                    cnt_next    <= cnt + 1;
                    
                    case (cnt) is
                        -- DATA RTPS SUBMESSAGE (Participant Message)
                        -- RTPS Submessage Header
                        when 0 =>
                            data_out  <= SID_DATA & "00000100" & std_logic_vector(to_unsigned(44, 16));
                        -- ExtraFlags, octetsToInlineQoS
                        when 1 =>
                            data_out  <= x"0000" & std_logic_vector(to_unsigned(16, 16));
                        -- Reader Entity ID
                        when 2 =>
                            data_out  <= ENTITYID_P2P_BUILTIN_PARTICIPANT_MESSAGE_READER;
                        -- Writer Entity ID
                        when 3 =>
                            data_out  <= ENTITYID_P2P_BUILTIN_PARTICIPANT_MESSAGE_WRITER;
                        -- Sequence Number 1/2
                        when 4 =>
                            data_out  <= std_logic_vector(man_live_seq_nr(0));
                        -- Sequence Number 2/2
                        when 5 =>
                            data_out  <= std_logic_vector(man_live_seq_nr(1));
                        -- Serialized Payload Header
                        when 6 =>
                            data_out  <= CDR_BE & x"0000";
                        -- Serialized Payload BEGIN
                        -- GUID Prefix 1/3
                        when 7 =>
                            data_out  <= GUIDPREFIX(0);
                        -- GUID Prefix 2/3
                        when 8 =>
                            data_out  <= GUIDPREFIX(1);
                        -- GUID Prefix 3/3
                        when 9 =>
                            data_out  <= GUIDPREFIX(2);
                        -- Participant Message Kind
                        when 10 =>
                            data_out  <= PARTICIPANT_MESSAGE_DATA_KIND_MANUAL_LIVELINESS_UPDATE;
                        -- Data Length
                        when 11 =>
                            data_out  <= (others => '0');
                            -- If manual and automatic sequence numbers are not consecutive, we need to send a GAP Message
                            if (live_gap_start /= auto_live_seq_nr) then
                                stage_next  <= SEND_MES_GAP;
                                cnt_next    <= 0;
                            else
                                stage_next  <= SEND_MES_AUTO_LIVE;
                                cnt_next    <= 0;
                            end if;
                        when others =>
                            null;
                    end case;
                end if;
            when SEND_MES_GAP =>
                -- Output FIFO Guard
                if (rtps_full = '0') then
                    wr_sig      <= '1';
                    cnt_next    <= cnt + 1;
                    
                    case (cnt) is
                        -- DATA RTPS SUBMESSAGE (Participant Message)
                        -- RTPS Submessage Header
                        when 0 =>
                            data_out  <= SID_GAP & "00000000" & std_logic_vector(to_unsigned(28, 16));
                        -- Reader Entity ID
                        when 1 =>
                            data_out  <= ENTITYID_P2P_BUILTIN_PARTICIPANT_MESSAGE_READER;
                        -- Writer Entity ID
                        when 2 =>
                            data_out  <= ENTITYID_P2P_BUILTIN_PARTICIPANT_MESSAGE_WRITER;
                        -- GAP Start Sequence Number 1/2
                        when 3 =>
                            data_out  <= std_logic_vector(live_gap_start(0));
                        -- GAP Start Sequence Number 2/2
                        when 4 =>
                            data_out  <= std_logic_vector(live_gap_start(1));
                        -- GAP End Sequence Number Set (Bitmap Base 1/2)
                        when 5 =>
                            data_out  <= std_logic_vector(live_gap_end(0));
                        -- GAP End Sequence Number Set (Bitmap Base 2/2)
                        when 6 =>
                            data_out  <= std_logic_vector(live_gap_end(1));
                        -- GAP End Sequence Number Set (NumBits)
                        when 7 =>
                            data_out  <= (others => '0');
                            stage_next  <= SEND_MES_AUTO_LIVE;
                            cnt_next    <= 0;
                        when others =>
                            null;
                    end case;
                end if;
            when SEND_MES_AUTO_LIVE =>
                -- Output FIFO Guard
                if (rtps_full = '0') then
                    wr_sig      <= '1';
                    cnt_next    <= cnt + 1;
                    
                    case (cnt) is
                        -- DATA RTPS SUBMESSAGE (Participant Message)
                        -- RTPS Submessage Header
                        when 0 =>
                            data_out  <= SID_DATA & "00000100" & std_logic_vector(to_unsigned(44, 16));
                        -- ExtraFlags, octetsToInlineQoS
                        when 1 =>
                            data_out  <= x"0000" & std_logic_vector(to_unsigned(16, 16));
                        -- Reader Entity ID
                        when 2 =>
                            data_out  <= ENTITYID_P2P_BUILTIN_PARTICIPANT_MESSAGE_READER;
                        -- Writer Entity ID
                        when 3 =>
                            data_out  <= ENTITYID_P2P_BUILTIN_PARTICIPANT_MESSAGE_WRITER;
                        -- Sequence Number 1/2
                        when 4 =>
                            data_out  <= std_logic_vector(auto_live_seq_nr(0));
                        -- Sequence Number 2/2
                        when 5 =>
                            data_out  <= std_logic_vector(auto_live_seq_nr(1));
                        -- Serialized Payload Header
                        when 6 =>
                            data_out  <= CDR_BE & x"0000";
                        -- Serialized Payload BEGIN
                        -- GUID Prefix 1/3
                        when 7 =>
                            data_out  <= GUIDPREFIX(0);
                        -- GUID Prefix 2/3
                        when 8 =>
                            data_out  <= GUIDPREFIX(1);
                        -- GUID Prefix 3/3
                        when 9 =>
                            data_out  <= GUIDPREFIX(2);
                        -- Participant Message Kind
                        when 10 =>
                            data_out  <= PARTICIPANT_MESSAGE_DATA_KIND_AUTOMATIC_LIVELINESS_UPDATE;
                        -- Data Length
                        when 11 =>
                            data_out      <= (others => '0');
                            last_word_out   <= '1';
                            -- If we are in the middle of a liveliness assertion, find the next Participant destination
                            if (is_live_assert = '1') then
                                stage_next      <= FIND_PARTICIPANT_DEST;
                                mem_opcode      <= FIND_NEXT_PARTICIPANT;
                                mem_op_start    <= '1';
                            else
                                -- DONE
                                stage_next      <= IDLE;
                            end if;
                        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';
            -- Notify Endpoints of EOP
            last_word_out           <= '1'; -- TODO: Necessary? (We do not pass through input to output, so a EOP should not leave as "stranded" during a write out)
            -- 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';
            -- Notify Endpoints of EOP
            last_word_out       <= '1';
            -- 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;
    
    
    
    -- Main State Machine
    -- STATE                    DESCRIPTION
    -- IDLE                     Idle state. Done Signal is pulled high and Memory FSM accepts new memory operations
    -- SEARCH_PARTICIPANT       See Memory OPCODE Description
    -- GET_PARTICIPANT_DATA     Latch the contents of the Participant Entry for use in the main FSM
    -- INSERT_PARTICIPANT       See Memory OPCODE Description
    -- UPDATE_PARTICIPANT       See Memory OPCODE Description
    -- REMOVE_PARTICIPANT       See Memory OPCODE Description
    -- FIND_PARTICIPANT_SLOT    Find first empty Participant Slot in memory
    -- FIND_NEXT_PARTICIPANT    See Memory OPCODE Description
    -- FIND_STALE_PARTICIPANT   See Memory OPCODE Description
    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;
        addr_res_next               <= addr_res;
        mem_cnt_next                <= mem_cnt;
        last_addr_next              <= last_addr;
        mem_participant_data_next   <= mem_participant_data;
        is_heartbeat_res_next       <= is_heartbeat_res;
        mem_guidprefix_next         <= mem_guidprefix;
        max_participant_addr_next   <= max_participant_addr;
        reset_max_pointer_next      <= reset_max_pointer;
        mem_long_latch_next         <= mem_long_latch;
        -- 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';
                
                if (mem_op_start = '1') then
                    case(mem_opcode) is
                        when SEARCH_PARTICIPANT =>
                            mem_addr_base_next  <= FIRST_PARTICIPANT_ADDRESS;
                            mem_addr_next       <= FIRST_PARTICIPANT_ADDRESS;
                            mem_stage_next      <= SEARCH_PARTICIPANT;
                            mem_cnt_next        <= 0;
                        when INSERT_PARTICIPANT =>
                            mem_addr_base_next  <= FIRST_PARTICIPANT_ADDRESS;
                            mem_addr_next       <= FIRST_PARTICIPANT_ADDRESS;
                            mem_stage_next      <= FIND_PARTICIPANT_SLOT;
                            mem_cnt_next        <= 0;
                        when UPDATE_PARTICIPANT =>
                            -- NOTE: We use the "_next" flag here, because it is not yet latched.
                            if (update_participant_flags_next(PARTICIPANT_DATA_FLAG) = '1') then
                                mem_stage_next  <= UPDATE_PARTICIPANT;
                                mem_addr_next   <= addr_res + 3;
                                mem_cnt_next    <= 0;
                            elsif (update_participant_flags_next(LEASE_DEADLINE_FLAG) = '1') then
                                mem_stage_next  <= UPDATE_PARTICIPANT;
                                mem_addr_next   <= addr_res + 10;
                                mem_cnt_next    <= 7;
                            elsif (update_participant_flags_next(EXTRA_FLAGS_FLAG) = '1') then
                                mem_stage_next  <= UPDATE_PARTICIPANT;
                                mem_addr_next   <= addr_res + 12;
                                mem_cnt_next    <= 9;
                            elsif (update_participant_flags_next(ACKNACK_RES_TIME_FLAG) = '1') then
                                mem_stage_next  <= UPDATE_PARTICIPANT;
                                mem_addr_next   <= addr_res + 13;
                                mem_cnt_next    <= 10;
                            elsif (update_participant_flags_next(HEARTBEAT_RES_TIME_FLAG) = '1') then
                                mem_stage_next  <= UPDATE_PARTICIPANT;
                                mem_addr_next   <= addr_res + 15;
                                mem_cnt_next    <= 12;
                            elsif (update_participant_flags_next(EDP_SEQ_NR_FLAG) = '1') then
                                mem_stage_next  <= UPDATE_PARTICIPANT;
                                mem_addr_next   <= addr_res + 17;
                                mem_cnt_next    <= 14;
                            end if;
                        when REMOVE_PARTICIPANT =>
                            mem_addr_next       <= addr_res;
                            mem_stage_next      <= REMOVE_PARTICIPANT;
                            mem_cnt_next        <= 0;
                        when FIND_STALE_PARTICIPANT =>
                            mem_addr_base_next  <= FIRST_PARTICIPANT_ADDRESS;
                            mem_addr_next       <= FIRST_PARTICIPANT_ADDRESS;
                            mem_stage_next      <= FIND_STALE_PARTICIPANT;
                            mem_cnt_next        <= 0;
                        when FIND_FIRST_PATICIPANT =>
                            mem_addr_base_next  <= FIRST_PARTICIPANT_ADDRESS;
                            mem_addr_next       <= FIRST_PARTICIPANT_ADDRESS;
                            mem_stage_next      <= FIND_NEXT_PARTICIPANT;
                            mem_cnt_next        <= 0;
                        when FIND_NEXT_PARTICIPANT =>
                            -- Memory Bound Guard
                            if (addr_res /= max_participant_addr) then
                                -- Reached End of Memory, No match
                                tmp                 := addr_res + PARTICIPANT_FRAME_SIZE;
                                mem_addr_base_next  <= tmp;
                                mem_addr_next       <= tmp;
                                mem_stage_next      <= FIND_NEXT_PARTICIPANT;
                                mem_cnt_next        <= 0;
                            else
                                addr_res_next       <= MAX_ADDRESS;
                            end if;
                        when others =>
                            null;
                    end case;
                end if;
            when SEARCH_PARTICIPANT =>
                mem_rd          <= '1';
                mem_cnt_next    <= mem_cnt + 1;
                mem_addr_next   <= mem_addr + 1;
                
                -- Next Participant Frame Address
                tmp := mem_addr_base + PARTICIPANT_FRAME_SIZE;
                
                case (mem_cnt) is
                    -- Preload
                    when 0 =>
                        null;
                    -- GUID Prefix 1/3
                    when 1 =>
                        -- No Match
                        if (mem_read_data /= guid(0)) then
                            -- Reached End of Memory, No Match
                            if (mem_addr_base = max_participant_addr) then
                                addr_res_next   <= MAX_ADDRESS; --No match
                                -- DONE
                                mem_stage_next  <= IDLE;
                            else
                                -- Continue Search
                                mem_addr_next       <= tmp;
                                mem_addr_base_next  <= tmp;
                                mem_cnt_next        <= 0;
                            end if;
                        end if;
                    -- GUID Prefix 2/3
                    when 2 =>
                        -- No Match
                        if (mem_read_data /= guid(1)) then
                            -- Reached End of Memory, No Match
                            if (mem_addr_base = max_participant_addr) then
                                addr_res_next   <= MAX_ADDRESS; --No match
                                -- DONE
                                mem_stage_next  <= IDLE;
                            else
                                -- Continue Search
                                mem_addr_next       <= tmp;
                                mem_addr_base_next  <= tmp;
                                mem_cnt_next        <= 0;
                            end if;
                        end if;
                    -- GUID Prefix 3/3
                    when 3 =>
                        -- No Match
                        if (mem_read_data /= guid(2)) then
                            -- Reached End of Memory, No Match
                            if (mem_addr_base = max_participant_addr) then
                                addr_res_next   <= MAX_ADDRESS; --No match
                                -- DONE
                                mem_stage_next  <= IDLE;
                            else
                                -- Continue Search
                                mem_addr_next       <= tmp;
                                mem_addr_base_next  <= tmp;
                                mem_cnt_next        <= 0;
                            end if;
                        -- Match
                        else
                            -- Fetch Participant Data
                            addr_res_next   <= mem_addr_base;
                            mem_stage_next  <= GET_PARTICIPANT_DATA;
                            mem_cnt_next    <= 1; -- No preload needed
                        end if;
                    when others =>
                        null;
                end case;
            when GET_PARTICIPANT_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;
                    -- Metatraffic IPv4 Address
                    when 1 =>
                        mem_participant_data_next.meta_addr             <= mem_read_data;
                    -- Default Endpoint IPv4 Address
                    when 2 =>
                        mem_participant_data_next.def_addr              <= mem_read_data;
                    -- UDPv4 Ports
                    when 3 =>
                        mem_participant_data_next.meta_port             <= mem_read_data(31 downto 16);
                        mem_participant_data_next.def_port              <= mem_read_data(15 downto 0);
                    -- SPDP Sequence Number 1/2
                    when 4 =>
                        mem_participant_data_next.spdp_seq_nr(0)        <= unsigned(mem_read_data);
                    -- SPDP Sequence Number 2/2
                    when 5 =>
                        mem_participant_data_next.spdp_seq_nr(1)        <= unsigned(mem_read_data);
                    -- Lease Duration 1/2
                    when 6 =>
                        mem_participant_data_next.lease_duration(0)     <= unsigned(mem_read_data);
                    -- Lease Duration 2/2
                    when 7 =>
                        mem_participant_data_next.lease_duration(1)     <= unsigned(mem_read_data);
                    -- Lease Deadline 1/2
                    when 8 =>
                        mem_participant_data_next.lease_deadline(0)     <= unsigned(mem_read_data);
                    -- Lease Deadline 2/2
                    when 9 =>
                        mem_participant_data_next.lease_deadline(1)     <= unsigned(mem_read_data);
                    -- Extra Flags
                    when 10 =>
                        mem_participant_data_next.extra_flags           <= mem_read_data(EXTRA_FLAGS_WIDTH-1 downto 0);
                    -- ACKNACK Response/Suppression Time 1/2
                    when 11 =>
                        mem_participant_data_next.acknack_res_time(0)   <= unsigned(mem_read_data);
                    -- ACKNACK Response/Suppression Time 2/2
                    when 12 =>
                        mem_participant_data_next.acknack_res_time(1)   <= unsigned(mem_read_data);
                    -- HEARTBEAT Response/Suppression Time 1/2
                    when 13 =>
                        mem_participant_data_next.heartbeat_res_time(0) <= unsigned(mem_read_data);
                    -- HEARTBEAT Response/Suppression Time 2/2
                    when 14 =>
                        mem_participant_data_next.heartbeat_res_time(1) <= unsigned(mem_read_data);
                    -- Publication Sequence Number 1/2
                    when 15 =>
                        mem_participant_data_next.pub_seq_nr(0)         <= unsigned(mem_read_data);
                    -- Publication Sequence Number 2/2
                    when 16 =>
                        mem_participant_data_next.pub_seq_nr(1)         <= unsigned(mem_read_data);
                    -- Subscription Sequence Number 1/2
                    when 17 =>
                        mem_participant_data_next.sub_seq_nr(0)         <= unsigned(mem_read_data);
                    -- Subscription Sequence Number 2/2
                    when 18 =>
                        mem_participant_data_next.sub_seq_nr(1)         <= unsigned(mem_read_data);
                    -- Participant Message Sequence Number 1/2
                    when 19 =>
                        mem_participant_data_next.mes_seq_nr(0)         <= unsigned(mem_read_data);
                    -- Participant Message Sequence Number 2/2
                    when 20 =>
                        mem_participant_data_next.mes_seq_nr(1)         <= unsigned(mem_read_data);
                        mem_rd  <= '0';
                        -- DONE
                        mem_stage_next  <= IDLE;
                    when others =>
                        null;
                end case;
            when INSERT_PARTICIPANT =>
                mem_wr          <= '1';
                mem_addr_next   <= mem_addr + 1;
                mem_cnt_next    <= mem_cnt + 1;
                
                case (mem_cnt) is
                    -- GUIDPrefix 1/3
                    when 0 =>
                        mem_write_data  <= guid(0);
                    -- GUIDPrefix 2/3
                    when 1 =>
                        mem_write_data  <= guid(1);
                    -- GUIDPrefix 3/3
                    when 2 =>
                        mem_write_data  <= guid(2);
                    -- Metatraffic IPv4 Address
                    when 3 =>
                        mem_write_data  <= meta_addr;
                    -- Default Endpoint IPv4 Address
                    when 4 =>
                        mem_write_data  <= def_addr;
                    -- UDPv4 Ports
                    when 5 =>
                        mem_write_data  <= meta_port & def_port;
                    -- SPDP Sequence Number 1/2
                    when 6 =>
                        mem_write_data  <= std_logic_vector(next_seq_nr(0));
                    -- SPDP Sequence Number 2/2
                    when 7 =>
                        mem_write_data  <= std_logic_vector(next_seq_nr(1));
                    -- Lease Duration 1/2
                    when 8 =>
                        mem_write_data  <= std_logic_vector(lease_duration(0));
                    -- Lease Duration 2/2
                    when 9 =>
                        mem_write_data  <= std_logic_vector(lease_duration(1));
                    -- Lease Deadline 1/2
                    when 10 =>
                        mem_write_data  <= std_logic_vector(deadline(0));
                    -- Lease Deadline 2/2
                    when 11 =>
                        mem_write_data  <= std_logic_vector(deadline(1));
                    -- Extra Flags
                    when 12 =>
                        mem_write_data                                  <= (others => '0');
                        mem_write_data(EXTRA_FLAGS_WIDTH-1 downto 0)    <= extra_flags;
                    -- ACKNACK Response/Suppression Time 1/2
                    when 13 =>
                        mem_write_data  <= (others => '0');
                    -- ACKNACK Response/Suppression Time 2/2
                    when 14 =>
                        mem_write_data  <= (others => '0');
                    -- HEARTBEAT Response/Suppression Time 1/2
                    when 15 =>
                        mem_write_data  <= (others => '0');
                    -- HEARTBEAT Response/Suppression Time 2/2
                    when 16 =>
                        mem_write_data  <= (others => '0');
                    -- Publication Sequence Number 1/2
                    when 17 =>
                        mem_write_data  <= std_logic_vector(FIRST_SEQUENCENUMBER(0));
                    -- Publication Sequence Number 2/2
                    when 18 =>
                        mem_write_data  <= std_logic_vector(FIRST_SEQUENCENUMBER(1));
                    -- Subscription Sequence Number 1/2
                    when 19 =>
                        mem_write_data  <= std_logic_vector(FIRST_SEQUENCENUMBER(0));
                    -- Subscription Sequence Number 2/2
                    when 20 =>
                        mem_write_data  <= std_logic_vector(FIRST_SEQUENCENUMBER(1));
                    -- Participant Message Sequence Number 1/2
                    when 21 =>
                        mem_write_data  <= std_logic_vector(FIRST_SEQUENCENUMBER(0));
                    -- Participant Message Sequence Number 2/2
                    when 22 =>
                        mem_write_data  <= std_logic_vector(FIRST_SEQUENCENUMBER(1));
                        -- DONE
                        mem_stage_next  <= IDLE;
                    when others => 
                        null;
                end case;
            when UPDATE_PARTICIPANT =>
                mem_cnt_next    <= mem_cnt + 1;
                mem_addr_next   <= mem_addr + 1;
                
                case (mem_cnt) is
                    -- Metatraffic IPv4 Address
                    when 0 =>
                        mem_write_data  <= meta_addr;
                        if (update_participant_flags(PARTICIPANT_DATA_FLAG) = '1') then
                            mem_wr  <= '1';
                        end if;
                    -- Default Endpoint IPv4 Address
                    when 1 =>
                        mem_write_data  <= def_addr;
                        if (update_participant_flags(PARTICIPANT_DATA_FLAG) = '1') then
                            mem_wr  <= '1';
                        end if;
                    -- UDPv4 Ports
                    when 2 =>
                        mem_write_data  <= meta_port & def_port;
                        if (update_participant_flags(PARTICIPANT_DATA_FLAG) = '1') then
                            mem_wr  <= '1';
                        end if;
                    -- SPDP Sequence Number 1/2
                    when 3 =>
                        mem_write_data  <= std_logic_vector(next_seq_nr(0));
                        if (update_participant_flags(PARTICIPANT_DATA_FLAG) = '1') then
                            mem_wr  <= '1';
                        end if;
                    -- SPDP Sequence Number 2/2
                    when 4 =>
                        mem_write_data  <= std_logic_vector(next_seq_nr(1));
                        if (update_participant_flags(PARTICIPANT_DATA_FLAG) = '1') then
                            mem_wr  <= '1';
                        end if;
                    -- Lease Duration 1/2
                    when 5 =>
                        mem_write_data  <= std_logic_vector(lease_duration(0));
                        if (update_participant_flags(PARTICIPANT_DATA_FLAG) = '1') then
                            mem_wr  <= '1';
                        end if;
                    -- Lease Duration 2/2
                    when 6 =>
                        mem_write_data  <= std_logic_vector(lease_duration(1));
                        if (update_participant_flags(PARTICIPANT_DATA_FLAG) = '1') then
                            mem_wr  <= '1';
                        end if;
                        -- If nothing else to update
                        if (update_participant_flags(5 downto 1) = (5 downto 1 => '0')) then
                            -- DONE
                            mem_stage_next  <= IDLE;
                        end if;
                    -- Lease Deadline 1/2
                    when 7 =>
                        mem_write_data  <= std_logic_vector(deadline(0));
                        if (update_participant_flags(LEASE_DEADLINE_FLAG) = '1') then
                            mem_wr  <= '1';
                        end if;
                    -- Lease Deadline 2/2
                    when 8 =>
                        mem_write_data  <= std_logic_vector(deadline(1));
                        if (update_participant_flags(LEASE_DEADLINE_FLAG) = '1') then
                            mem_wr  <= '1';
                        end if;
                        -- If nothing else to update
                        if (update_participant_flags(5 downto 2) = (5 downto 2 => '0')) then
                            -- DONE
                            mem_stage_next  <= IDLE;
                        end if;
                    -- Extra Flags
                    when 9 =>
                        mem_write_data                                  <= (others => '0');
                        mem_write_data(EXTRA_FLAGS_WIDTH-1 downto 0)    <= extra_flags;
                        if (update_participant_flags(EXTRA_FLAGS_FLAG) = '1') then
                            mem_wr  <= '1';
                        end if;
                        -- If nothing else to update
                        if (update_participant_flags(5 downto 3) = (5 downto 3 => '0')) then
                            -- DONE
                            mem_stage_next  <= IDLE;
                        end if;
                    -- ACKNACK DEADLINE 1/2
                    when 10 =>
                        mem_write_data  <= std_logic_vector(deadline(0));
                        if (update_participant_flags(ACKNACK_RES_TIME_FLAG) = '1') then
                            mem_wr  <= '1';
                        end if;
                    -- ACKNACK DEADLINE 2/2
                    when 11 =>
                        mem_write_data  <= std_logic_vector(deadline(1));
                        if (update_participant_flags(ACKNACK_RES_TIME_FLAG) = '1') then
                            mem_wr  <= '1';
                        end if;
                        -- If nothing else to update
                        if (update_participant_flags(5 downto 4) = (5 downto 4 => '0')) then
                            -- DONE
                            mem_stage_next  <= IDLE;
                        end if;
                    -- HEARTBEAT DEADLINE 1/2
                    when 12 =>
                        mem_write_data  <= std_logic_vector(deadline(0));
                        if (update_participant_flags(HEARTBEAT_RES_TIME_FLAG) = '1') then
                            mem_wr  <= '1';
                        end if;
                    -- HEARTBEAT DEADLINE 2/2
                    when 13 =>
                        mem_write_data  <= std_logic_vector(deadline(1));
                        if (update_participant_flags(HEARTBEAT_RES_TIME_FLAG) = '1') then
                            mem_wr  <= '1';
                        end if;
                        -- If nothing else to update
                        if (update_participant_flags(5 downto 5) = (5 downto 5 => '0')) then
                            -- DONE
                            mem_stage_next  <= IDLE;
                        end if;
                    -- Publication Sequence Number 1/2
                    when 14 =>
                        mem_write_data  <= std_logic_vector(next_seq_nr(0));
                        if (update_participant_flags(EDP_SEQ_NR_FLAG) = '1' and message_type = EDP and is_subscriber = '0') then
                            mem_wr  <= '1';
                        end if;
                    -- Publication Sequence Number 2/2
                    when 15 =>
                        mem_write_data  <= std_logic_vector(next_seq_nr(1));
                        if (update_participant_flags(EDP_SEQ_NR_FLAG) = '1' and message_type = EDP and is_subscriber = '0') then
                            mem_wr  <= '1';
                        end if;
                    -- Subscription Sequence Number 1/2
                    when 16 =>
                        mem_write_data  <= std_logic_vector(next_seq_nr(0));
                        if (update_participant_flags(EDP_SEQ_NR_FLAG) = '1' and message_type = EDP and is_subscriber = '1') then
                            mem_wr  <= '1';
                        end if;
                    -- Subscription Sequence Number 2/2
                    when 17 =>
                        mem_write_data  <= std_logic_vector(next_seq_nr(1));
                        if (update_participant_flags(EDP_SEQ_NR_FLAG) = '1' and message_type = EDP and is_subscriber = '1') then
                            mem_wr  <= '1';
                        end if;
                    -- Participant Message Sequence Number 1/2
                    when 18 =>
                        mem_write_data  <= std_logic_vector(next_seq_nr(0));
                        if (update_participant_flags(EDP_SEQ_NR_FLAG) = '1' and message_type = MESSAGE) then
                            mem_wr  <= '1';
                        end if;
                    -- Participant Message Sequence Number 2/2
                    when 19 =>
                        mem_write_data  <= std_logic_vector(next_seq_nr(1));
                        if (update_participant_flags(EDP_SEQ_NR_FLAG) = '1' and message_type = MESSAGE) then
                            mem_wr  <= '1';
                        end if;
                        -- DONE
                        mem_stage_next  <= IDLE;
                    when others =>
                        null;
                end case;
            when REMOVE_PARTICIPANT =>
                mem_addr_next   <= mem_addr + 1;
                mem_cnt_next    <= mem_cnt + 1;
                
                -- Latch Participant GUID Prefix, and then overwrite with GUIDPREFIX_UNKNOWN to mark slot empty
                case (mem_cnt) is
                    -- Preload
                    when 0 =>
                        mem_rd  <= '1';
                    -- GUID Prefix 1/3
                    when 1 =>
                        mem_rd  <= '1';
                        mem_guidprefix_next(0)  <= mem_read_data;
                    -- GUID Prefix 2/3
                    when 2 =>
                        mem_rd  <= '1';
                        mem_guidprefix_next(1)  <= mem_read_data;
                    -- GUID Prefix 3/3
                    when 3 =>
                        mem_rd                  <= '1';
                        mem_guidprefix_next(2)  <= mem_read_data;
                        mem_addr_next           <= addr_res;
                    -- GUID Prefix 1/3
                    when 4 =>
                        mem_wr          <= '1';
                        mem_write_data  <= GUIDPREFIX_UNKNOWN(0);
                    -- GUID Prefix 2/3
                    when 5 =>
                        mem_wr          <= '1';
                        mem_write_data  <= GUIDPREFIX_UNKNOWN(1);
                    -- GUID Prefix 3/3
                    when 6 =>
                        mem_wr          <= '1';
                        mem_write_data  <= GUIDPREFIX_UNKNOWN(2);
                        -- Reset MAX Participant Pointer
                        mem_addr_base_next      <= FIRST_PARTICIPANT_ADDRESS;
                        mem_addr_next           <= FIRST_PARTICIPANT_ADDRESS;
                        reset_max_pointer_next  <= '1';
                        last_addr_next          <= (others => '0');
                        mem_stage_next          <= FIND_PARTICIPANT_SLOT;
                        mem_cnt_next            <= 0;
                    when others =>
                        null;
                end case;
            when FIND_PARTICIPANT_SLOT =>
                mem_rd  <= '1';
                mem_addr_next   <= mem_addr + 1;
                mem_cnt_next    <= mem_cnt + 1;
                
                -- Next Participant Frame Address
                tmp := mem_addr_base + PARTICIPANT_FRAME_SIZE;
                
                case (mem_cnt) is
                    -- Preload
                    when 0 =>
                        null;
                    -- GUID Prefix 1/3
                    when 1 =>
                        -- Slot Occupied
                        if (mem_read_data /= GUIDPREFIX_UNKNOWN(0)) then
                            if (mem_addr_base = max_participant_addr) then
                                -- We are in the middle of resetting the MAX Participant Pointer
                                if (reset_max_pointer = '1') then
                                    -- No Change
                                    mem_stage_next  <= IDLE;
                                -- MEMORY FULL
                                elsif (max_participant_addr = MAX_PARTICIPANT_ADDRESS) then
                                    report "Memory Full, Ignoring Participant Data" severity NOTE;
                                    -- Ignore Insertion
                                    mem_stage_next  <= IDLE;
                                    addr_res_next   <= MAX_ADDRESS;
                                else
                                    -- Extend Participant Memory Area
                                    -- NOTE: "max_participant_addr" points to the first address of last participant frame
                                    max_participant_addr_next   <= tmp;
                                    -- Populate Participant Slot
                                    mem_stage_next  <= INSERT_PARTICIPANT;
                                    mem_addr_next   <= tmp;
                                    addr_res_next   <= tmp;
                                    mem_cnt_next    <= 0;
                                end if;
                            else
                                -- Latch last occupied Participant Slot
                                last_addr_next      <= mem_addr_base;
                                -- Continue Search
                                mem_addr_next       <= tmp;
                                mem_addr_base_next  <= tmp;
                                mem_cnt_next        <= 0;
                            end if;
                        end if;
                    -- GUID Prefix 2/3
                    when 2 =>
                        -- Slot Occupied
                        if (mem_read_data /= GUIDPREFIX_UNKNOWN(1)) then
                            if (mem_addr_base = max_participant_addr) then
                                -- We are in the middle of resetting the MAX Participant Pointer
                                if (reset_max_pointer = '1') then
                                    -- No Change
                                    mem_stage_next  <= IDLE;
                                -- MEMORY FULL
                                elsif (max_participant_addr = MAX_PARTICIPANT_ADDRESS) then
                                    report "Memory Full, Ignoring Participant Data" severity NOTE;
                                    -- Ignore Insertion
                                    mem_stage_next  <= IDLE;
                                    addr_res_next   <= MAX_ADDRESS;
                                else
                                    -- Extend Participant Memory Area
                                    -- NOTE: "max_participant_addr" points to the first address of last participant frame
                                    max_participant_addr_next   <= tmp;
                                    -- Populate Participant Slot
                                    mem_stage_next  <= INSERT_PARTICIPANT;
                                    mem_addr_next   <= tmp;
                                    addr_res_next   <= tmp;
                                    mem_cnt_next    <= 0;
                                end if;
                            else
                                -- Latch last occupied Participant Slot
                                last_addr_next      <= mem_addr_base;
                                -- Continue Search
                                mem_addr_next       <= tmp;
                                mem_addr_base_next  <= tmp;
                                mem_cnt_next        <= 0;
                            end if;
                        end if;
                    -- GUID Prefix 3/3
                    when 3 =>
                        -- Slot Occupied
                        if (mem_read_data /= GUIDPREFIX_UNKNOWN(2)) then
                            if (mem_addr_base = max_participant_addr) then
                                -- We are in the middle of resetting the MAX Participant Pointer
                                if (reset_max_pointer = '1') then
                                    -- No Change
                                    mem_stage_next  <= IDLE;
                                -- MEMORY FULL
                                elsif (max_participant_addr = MAX_PARTICIPANT_ADDRESS) then
                                    report "Memory Full, Ignoring Participant Data" severity NOTE;
                                    -- Ignore Insertion
                                    mem_stage_next  <= IDLE;
                                    addr_res_next   <= MAX_ADDRESS;
                                else
                                    -- Extend Participant Memory Area
                                    -- NOTE: "max_participant_addr" points to the first address of last participant frame
                                    max_participant_addr_next   <= tmp;
                                    -- Populate Participant Slot
                                    mem_stage_next  <= INSERT_PARTICIPANT;
                                    mem_addr_next   <= tmp;
                                    addr_res_next   <= tmp;
                                    mem_cnt_next    <= 0;
                                end if;
                            else
                                -- Latch last occupied Participant Slot
                                last_addr_next      <= mem_addr_base;
                                -- Continue Search
                                mem_addr_next       <= tmp;
                                mem_addr_base_next  <= tmp;
                                mem_cnt_next        <= 0;
                            end if;
                        -- Slot Empty
                        else
                            if (reset_max_pointer = '1') then
                                -- Make sure to iterate through complete participant area
                                if (mem_addr_base = max_participant_addr) then
                                    -- Reset Pointer to last occupied participant Slot
                                    max_participant_addr_next   <= last_addr;
                                    -- DONE
                                    mem_stage_next  <= IDLE;
                                else
                                    -- Continue Search
                                    mem_addr_next       <= tmp;
                                    mem_addr_base_next  <= tmp;
                                    mem_cnt_next        <= 0;
                                end if;                                
                            else
                                -- Populate Participant Slot
                                mem_stage_next  <= INSERT_PARTICIPANT;
                                mem_addr_next   <= mem_addr_base;
                                addr_res_next   <= mem_addr_base;
                                mem_cnt_next    <= 0;
                            end if;
                        end if;
                    when others =>
                        null;
                end case;
            when FIND_NEXT_PARTICIPANT =>
                mem_rd          <= '1';
                mem_cnt_next    <= mem_cnt + 1;
                mem_addr_next   <= mem_addr + 1;
                
                -- Next Participant Frame Address
                tmp     := mem_addr_base + PARTICIPANT_FRAME_SIZE;
                -- Beginning of Participant Data
                tmp2    := mem_addr_base + 3;
                
                case (mem_cnt) is
                    -- Preload
                    when 0 =>
                        null;
                    -- GUID Prefix 1/3
                    when 1 =>
                        -- Slot Occupied
                        if (mem_read_data /= GUIDPREFIX_UNKNOWN(0)) then
                            -- Get Participant Data
                            mem_addr_next   <= tmp2;
                            addr_res_next   <= mem_addr_base;
                            mem_stage_next  <= GET_PARTICIPANT_DATA;
                            mem_cnt_next    <= 0;
                        end if;
                    when 2 =>
                        -- Slot Occupied
                        if (mem_read_data /= GUIDPREFIX_UNKNOWN(1)) then
                            -- Get Participant Data
                            mem_addr_next   <= tmp2;
                            addr_res_next   <= mem_addr_base;
                            mem_stage_next  <= GET_PARTICIPANT_DATA;
                            mem_cnt_next    <= 0;
                        end if;
                    when 3 =>
                        -- Slot Occupied
                        if (mem_read_data /= GUIDPREFIX_UNKNOWN(2)) then
                            -- Get Participant Data
                            mem_addr_next   <= tmp2;
                            addr_res_next   <= mem_addr_base;
                            mem_stage_next  <= GET_PARTICIPANT_DATA;
                            mem_cnt_next    <= 0;
                        -- Slot Empty
                        else
                            -- Reached End of Memory, No Match
                            if (mem_addr_base = max_participant_addr) then
                                addr_res_next   <= MAX_ADDRESS; --No match
                                -- DONE
                                mem_stage_next  <= IDLE;
                            else
                                -- Continue Search
                                mem_addr_next       <= tmp;
                                mem_addr_base_next  <= tmp;
                                mem_cnt_next        <= 0;
                            end if;
                        end if;
                    when others =>
                        null;
                end case;
            when FIND_STALE_PARTICIPANT =>
                mem_rd          <= '1';
                mem_cnt_next    <= mem_cnt + 1;
                mem_addr_next   <= mem_addr + 1;
                
                -- Next Participant Frame Address
                tmp             := mem_addr_base + PARTICIPANT_FRAME_SIZE;
                -- Beginning of Lease Deadline
                tmp2            := mem_addr_base + 10;
                -- Beginning of Participant Data
                tmp3            := mem_addr_base + 3;
                
                case (mem_cnt) is
                    -- Preload
                    when 0 =>
                        null;
                    -- GUID Prefix 1/3
                    when 1 =>
                        -- Slot Occupied
                        if (mem_read_data /= GUIDPREFIX_UNKNOWN(0)) then
                            -- Jump to Stale Check
                            mem_addr_next   <= tmp2;
                            mem_cnt_next    <= 4;
                        end if;
                    -- GUID Prefix 2/3
                    when 2 =>
                        -- Slot Occupied
                        if (mem_read_data /= GUIDPREFIX_UNKNOWN(1)) then
                            -- Jump to Stale Check
                            mem_addr_next   <= tmp2;
                            mem_cnt_next    <= 4;
                        end if;
                    -- GUID Prefix 3/3
                    when 3 =>
                        -- Slot Occupied
                        if (mem_read_data /= GUIDPREFIX_UNKNOWN(2)) then
                            -- Jump to Stale Check
                            mem_addr_next   <= tmp2;
                            mem_cnt_next    <= 4;
                        -- Slot Empty
                        else
                            -- Reached End of Memory, No Match
                            if (mem_addr_base = max_participant_addr) then
                                addr_res_next   <= MAX_ADDRESS; --No match
                                -- DONE
                                mem_stage_next  <= IDLE;
                            else
                                -- Continue Search
                                mem_addr_next       <= tmp;
                                mem_addr_base_next  <= tmp;
                                mem_cnt_next        <= 0;
                            end if;
                        end if;
                    -- Preload
                    when 4 =>
                        null;
                    -- Lease Deadline 1/2
                    when 5 =>
                        mem_long_latch_next <= mem_read_data;
                    -- Lease Deadline 2/2
                    when 6 =>
                        tmp_dw  := (0 => unsigned(mem_long_latch), 1 => unsigned(mem_read_data));
                        -- Lease Deadline passed
                        if (tmp_dw < time) then
                            -- Mark Participant as stale
                            addr_res_next               <= mem_addr_base;
                            mem_participant_data_next   <= ZERO_PARTICIPANT_DATA;
                            -- DONE
                            mem_stage_next              <= IDLE;
                        end if;
                    -- Extra Flags
                    when 7 =>
                        null;
                    -- ACKNACK Response/Suppression Time 1/2
                    when 8 =>
                        mem_long_latch_next <= mem_read_data;
                    -- ACKNACK Response/Suppression Time 2/2
                    when 9 =>
                        tmp_dw  := (0 => unsigned(mem_long_latch), 1 => unsigned(mem_read_data));
                        -- Acknack Response/Suppression Time passed
                        if (tmp_dw /= 0 and tmp_dw < time) then
                            -- Mark Participant and get Participant Data
                            addr_res_next           <= mem_addr_base;
                            mem_addr_next           <= tmp3;
                            -- Mark as ACKNACK Trigger
                            is_heartbeat_res_next   <= '0';
                            mem_stage_next          <= GET_PARTICIPANT_DATA;
                            mem_cnt_next            <= 0;
                        end if;
                    -- HEARTBEAT Response/Suppression Time 1/2
                    when 10 =>
                        mem_long_latch_next <= mem_read_data;
                    -- HEARTBEAT Response/Suppression Time 2/2
                    when 11 =>
                        tmp_dw  := (0 => unsigned(mem_long_latch), 1 => unsigned(mem_read_data));
                        -- Heartbeat Response/Suppression Time passed
                        if (tmp_dw /= 0 and tmp_dw < time) then
                            -- Mark Participant and get Participant Data
                            addr_res_next           <= mem_addr_base;
                            mem_addr_next           <= tmp3;
                            -- Mark as HEARTBEAT Trigger
                            is_heartbeat_res_next   <= '1';
                            mem_stage_next          <= GET_PARTICIPANT_DATA;
                            mem_cnt_next            <= 0;
                        -- Participant not Stale
                        else
                            -- Reached End of Memory, No Match
                            if (mem_addr_base = max_participant_addr) then
                                addr_res_next   <= MAX_ADDRESS; --No match
                                -- DONE
                                mem_stage_next  <= IDLE;
                            else
                                -- Continue Search
                                mem_addr_next       <= tmp;
                                mem_addr_base_next  <= tmp;
                                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;
    
    sync: process(clk)
    begin
        if rising_edge(clk) then
            if (reset = '1') then
                stage                       <= IDLE;
                return_stage                <= IDLE;
                message_type                <= NONE;
                string_content              <= DOMAIN_TAG_TYPE;
                mem_stage                   <= IDLE;
                parameter_end               <= (others => '1');
                opcode                      <= (others => '0');
                flags                       <= (others => '0');
                src_port                    <= (others => '0');
                src_addr                    <= (others => '0');
                src_entityid                <= (others => '0');
                dest_entityid               <= (others => '0');
                string_length               <= (others => '0');
                compare_length              <= (others => '0');
                endpoint_mask               <= (others => '0');
                def_addr                    <= (others => '0');
                meta_addr                   <= (others => '0');
                def_port                    <= (others => '0');
                meta_port                   <= (others => '0');
                extra_flags                 <= (others => '0');
                update_participant_flags    <= (others => '0');
                count                       <= (others => '0');
                mem_addr_base               <= (others => '0');
                mem_addr                    <= (others => '0');
                addr_res                    <= (others => '0');
                last_addr                   <= (others => '0');
                long_latch                  <= (others => '0');
                mem_long_latch              <= (others => '0');
                rcvd                        <= (others => '0');
                reader_flags                <= (others => '0');
                max_participant_addr        <= FIRST_PARTICIPANT_ADDRESS;
                guid                        <= (others => (others => '0'));
                mem_guidprefix              <= (others => (others => '0'));
                lease_duration              <= (others => (others => '0'));
                deadline                    <= (others => (others => '0'));
                announcement_time           <= time + PARTICIPANT_ANNOUNCEMENT_PERIOD;
                heartbeat_time              <= time + HEARTBEAT_PERIOD;
                seq_nr                      <= (others => (others => '0'));
                next_seq_nr                 <= (others => (others => '0'));
                first_seq_nr                <= (others => (others => '0'));
                last_seq_nr                 <= (others => (others => '0'));
                auto_live_seq_nr            <= convert_to_double_word(to_unsigned(2, 64));
                man_live_seq_nr             <= convert_to_double_word(to_unsigned(1, 64));
                live_gap_start              <= convert_to_double_word(to_unsigned(2, 64));
                live_gap_end                <= convert_to_double_word(to_unsigned(1, 64));
                mem_participant_data        <= ZERO_PARTICIPANT_DATA;
                cnt                         <= 0;
                participant_data_cnt        <= 0;
                publisher_data_cnt          <= 0;
                subscriber_data_cnt         <= 0;
                mem_cnt                     <= 0;
                participant_match           <= '0';
                is_subscriber               <= '0';
                is_meta_addr                <= '0';
                stale_check                 <= '0';
                is_live_assert              <= '0';
                is_heartbeat_res            <= '0';
                reset_max_pointer           <= '0';
                last_word_in_latch          <= '0';
            else
                stage                       <= stage_next;
                return_stage                <= return_stage_next;
                message_type                <= message_type_next;
                string_content              <= string_content_next;
                mem_stage                   <= mem_stage_next;
                opcode                      <= opcode_next;
                flags                       <= flags_next;
                src_port                    <= src_port_next;
                src_addr                    <= src_addr_next;
                src_entityid                <= src_entityid_next;
                dest_entityid               <= dest_entityid_next;
                parameter_end               <= parameter_end_next;
                string_length               <= string_length_next;
                compare_length              <= compare_length_next;
                endpoint_mask               <= endpoint_mask_next;
                def_addr                    <= def_addr_next;
                meta_addr                   <= meta_addr_next;
                def_port                    <= def_port_next;
                meta_port                   <= meta_port_next;
                extra_flags                 <= extra_flags_next;
                update_participant_flags    <= update_participant_flags_next;
                count                       <= count_next;
                mem_addr_base               <= mem_addr_base_next;
                mem_addr                    <= mem_addr_next;
                addr_res                    <= addr_res_next;
                last_addr                   <= last_addr_next;
                long_latch                  <= long_latch_next;
                mem_long_latch              <= mem_long_latch_next;
                rcvd                        <= rcvd_next;
                reader_flags                <= reader_flags_next;
                max_participant_addr        <= max_participant_addr_next;
                guid                        <= guid_next;
                mem_guidprefix              <= mem_guidprefix_next;
                lease_duration              <= lease_duration_next;
                deadline                    <= deadline_next;
                announcement_time           <= announcement_time_next;
                heartbeat_time              <= heartbeat_time_next;
                seq_nr                      <= seq_nr_next;
                next_seq_nr                 <= next_seq_nr_next;
                first_seq_nr                <= first_seq_nr_next;
                last_seq_nr                 <= last_seq_nr_next;
                auto_live_seq_nr            <= auto_live_seq_nr_next;
                man_live_seq_nr             <= man_live_seq_nr_next;
                live_gap_start              <= live_gap_start_next;
                live_gap_end                <= live_gap_end_next;
                mem_participant_data        <= mem_participant_data_next;
                cnt                         <= cnt_next;
                participant_data_cnt        <= participant_data_cnt_next;
                publisher_data_cnt          <= publisher_data_cnt_next;
                subscriber_data_cnt         <= subscriber_data_cnt_next;
                mem_cnt                     <= mem_cnt_next;
                participant_match           <= participant_match_next;
                is_subscriber               <= is_subscriber_next;
                is_meta_addr                <= is_meta_addr_next;
                stale_check                 <= stale_check_next;
                is_live_assert              <= is_live_assert_next;
                is_heartbeat_res            <= is_heartbeat_res_next;
                reset_max_pointer           <= reset_max_pointer_next;
                last_word_in_latch          <= last_word_in_latch_next;
            end if;
        end if;
    end process;
    
end architecture;
