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

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

entity rtps_out is
    generic (
        MAX_BUFFER_SIZE : natural := 65508/4
    );
    port (
        clk         : in std_logic;
        reset       : in std_logic;
        data_in     : in RTPS_OUT_DATA_TYPE;
        last_word_in: in std_logic_vector(0 to NUM_ENDPOINTS);
        rd          : out std_logic_vector(0 to NUM_ENDPOINTS);
        empty       : in std_logic_vector(0 to NUM_ENDPOINTS);
        data_out    : out std_logic_vector(WORD_WIDTH-1 downto 0);
        wr          : out std_logic;
        full        : in std_logic
    );
end entity;

architecture arch of rtps_out is
    
    -- *TYPE DECLARATION*
    type BUFFER_TYPE is array (0 to MAX_BUFFER_SIZE-1) of std_logic_vector(WORD_WIDTH-1 downto 0);
    type INPUT_STAGE_TYPE is (IDLE, SRC_ADDR_HEADER, DEST_ADDR_HEADER, PORT_HEADER, READ, SKIP);
    type OUTPUT_STAGE_TYPE is (IDLE, SRC_ADDR_HEADER, DEST_ADDR_HEADER, PORT_HEADER, PACKET_LENGTH, WRITE);
    
    -- *SIGNAL DECLARATION*
    signal selector, selector_next : natural range 0 to NUM_ENDPOINTS := 0;
    signal buff, buff_next : BUFFER_TYPE := (others => (others => '0'));
    signal in_pntr, in_pntr_next : natural range 0 to MAX_BUFFER_SIZE := MAX_BUFFER_SIZE;
    signal out_pntr, out_pntr_next : natural range 0 to MAX_BUFFER_SIZE := MAX_BUFFER_SIZE;
    signal length, length_next : unsigned(WORD_WIDTH-1 downto 0) := (others => '0');
    signal packet_end, packet_end_next : unsigned(WORD_WIDTH-1 downto 0) := (others => '0');
    signal input_stage, input_stage_next : INPUT_STAGE_TYPE := IDLE;
    signal output_stage, output_stage_next : OUTPUT_STAGE_TYPE := IDLE;
    signal filled ,reset_filled, set_filled: std_logic := '0';
    signal src_addr, src_addr_next : std_logic_vector(WORD_WIDTH-1 downto 0 ) := (others => '0');
    signal dest_addr, dest_addr_next : std_logic_vector(WORD_WIDTH-1 downto 0 ) := (others => '0');
    signal ports, ports_next : std_logic_vector(WORD_WIDTH-1 downto 0) := (others => '0');
    
    
begin

    in_prc : process (all)
    begin
        -- DEFAULT
        input_stage_next    <= input_stage;
        selector_next       <= selector;
        in_pntr_next        <= in_pntr;
        src_addr_next       <= src_addr;
        dest_addr_next      <= dest_addr;
        ports_next          <= ports;
        buff_next           <= buff;
        length_next         <= length;
        rd                  <= (others => '0');
        set_filled          <= '0';
        
        case (input_stage) is
            when IDLE =>
                -- Currently Selected Input FIFO is empty
                if (empty(selector) = '1') then
                    -- Select next input FIFO
                    if (selector = NUM_ENDPOINTS) then
                        selector_next   <= 0;
                    else
                        selector_next   <= selector + 1;
                    end if;
                else
                    -- Wait until Output Pointer is beyond the Header
                    if (filled = '0' and out_pntr /= 0) then
                        -- Read from input FIFO
                        input_stage_next    <= SRC_ADDR_HEADER;
                        in_pntr_next        <= 0;
                    end if;
                end if;
            when SRC_ADDR_HEADER =>
                -- Input FIFO Guard
                if (empty(selector) = '0') then
                    rd(selector)    <= '1';
                    src_addr_next   <= data_in(selector);
                    input_stage_next<= DEST_ADDR_HEADER;
                    
                    -- SANITY CHECK: Skip Packet if last word in before actual packet
                    if (last_word_in(selector) = '1') then
                        -- Skip 
                        input_stage_next    <= IDLE;
                        in_pntr_next        <= MAX_BUFFER_SIZE;
                        -- Select next input FIFO
                        if (selector = NUM_ENDPOINTS) then
                            selector_next   <= 0;
                        else
                            selector_next   <= selector + 1;
                        end if;
                    end if;
                end if;
            when DEST_ADDR_HEADER =>
                -- Input FIFO Guard
                if (empty(selector) = '0') then
                    rd(selector)    <= '1';
                    dest_addr_next  <= data_in(selector);
                    input_stage_next<= PORT_HEADER;
                    
                    -- SANITY CHECK: Skip Packet if last word in before actual packet
                    if (last_word_in(selector) = '1') then
                        -- Skip 
                        input_stage_next    <= IDLE;
                        in_pntr_next        <= MAX_BUFFER_SIZE;
                        -- Select next input FIFO
                        if (selector = NUM_ENDPOINTS) then
                            selector_next   <= 0;
                        else
                            selector_next   <= selector + 1;
                        end if;
                    end if;
                end if;
            when PORT_HEADER =>
                -- Input FIFO Guard
                if (empty(selector) = '0') then
                    rd(selector)    <= '1';
                    ports_next      <= data_in(selector);
                    input_stage_next<= READ;
                    
                    -- SANITY CHECK: Skip Packet if last word in before actual packet
                    if (last_word_in(selector) = '1') then
                        -- Skip 
                        input_stage_next    <= IDLE;
                        in_pntr_next        <= MAX_BUFFER_SIZE;
                        -- Select next input FIFO
                        if (selector = NUM_ENDPOINTS) then
                            selector_next   <= 0;
                        else
                            selector_next   <= selector + 1;
                        end if;
                    end if;
                end if;
            when READ =>
                -- Output pointer past our point (We can safely read into the Buffer)
                if (out_pntr > in_pntr) then
                    -- Input FIFO Guard
                    if (empty(selector) = '0') then
                        rd(selector)        <= '1';
                        buff_next(in_pntr)  <= data_in(selector);
                        in_pntr_next        <= in_pntr + 1;
                        
                        -- Last Input Word
                        if (last_word_in(selector) = '1') then
                            -- Set Length
                            length_next <= to_unsigned(in_pntr, WORD_WIDTH);
                            -- Mark Buffer Ready for Output
                            set_filled  <= '1';
                            -- DONE
                            input_stage_next    <= IDLE;
                            in_pntr_next        <= MAX_BUFFER_SIZE;
                            -- Select next input FIFO
                            if (selector = NUM_ENDPOINTS) then
                                selector_next   <= 0;
                            else
                                selector_next   <= selector + 1;
                            end if;
                        -- Overflow (Packet larger than buffer)
                        elsif (in_pntr = MAX_BUFFER_SIZE-1) then
                            -- SKIP PACKET
                            input_stage_next    <= SKIP;
                        end if;
                    end if;
                end if;
            when SKIP =>
                -- Input FIFO Guard
                if (empty(selector) = '0') then
                    rd(selector)            <= '1';
                    
                    -- Last Input Word
                    if (last_word_in(selector) = '1') then
                        -- DONE
                        input_stage_next    <= IDLE;
                        in_pntr_next        <= MAX_BUFFER_SIZE;
                        -- Select next input FIFO
                        if (selector = NUM_ENDPOINTS) then
                            selector_next   <= 0;
                        else
                            selector_next   <= selector + 1;
                        end if;
                    end if;
                end if;
            when others =>
                null;
        end case;
    end process;
    
    out_prc : process (all)
    begin
        -- DEFAULT
        output_stage_next   <= output_stage;
        out_pntr_next       <= out_pntr;
        wr                  <= '0';
        data_out            <= (others => '0');
        packet_end_next     <= packet_end;
        reset_filled        <= '0';
        
        case (output_stage) is
            when IDLE =>
                -- Wait until Buffer is Ready
                if (filled = '1') then
                    output_stage_next   <= SRC_ADDR_HEADER;
                    out_pntr_next       <= 0;
                    -- Mark Buffer as being processed
                    reset_filled        <= '1';
                end if;
            when SRC_ADDR_HEADER =>
                -- Output FIFO Guard
                if (full = '0') then
                    wr                  <= '1';
                    data_out            <= src_addr;
                    output_stage_next   <= DEST_ADDR_HEADER;
                end if;
            when DEST_ADDR_HEADER =>
                -- Output FIFO Guard
                if (full = '0') then
                    wr                  <= '1';
                    data_out            <= dest_addr;
                    output_stage_next   <= PORT_HEADER;
                end if;
            when PORT_HEADER =>
                -- Output FIFO Guard
                if (full = '0') then
                    wr                  <= '1';
                    data_out            <= ports;
                    output_stage_next   <= PACKET_LENGTH;
                end if;
            when PACKET_LENGTH =>
                -- Output FIFO Guard
                if (full = '0') then
                    wr                  <= '1';
                    data_out            <= std_logic_vector(length + 1);
                    packet_end_next     <= length;
                    output_stage_next   <= WRITE;
                end if;
            when WRITE =>
                -- Output FIFO Guard
                if (full = '0') then
                    wr                  <= '1';
                    out_pntr_next       <= out_pntr + 1;
                    data_out            <= buff(out_pntr);
                    
                    if (out_pntr = packet_end) then
                        -- DONE
                        output_stage_next   <= IDLE;
                        out_pntr_next       <= MAX_BUFFER_SIZE;
                    end if;
                end if;
            when others =>
                null;
        end case;
    end process;
    
    filled_prc : process (all)
    begin
        if rising_edge(clk) then
            if (reset = '1') then
                filled  <= '0';
            else
                -- NOTE: This condition should not be possible due to the additional bound checks of the input and output processes.
                assert(not (reset_filled = '1' and set_filled = '1')) report "Both set and reset Flag set on same clock cycle" severity FAILURE;
                
                if (reset_filled = '1') then
                    filled  <= '0';
                elsif (set_filled = '1') then
                    filled  <= '1';
                end if;
            end if;
        end if;
    end process;
    
    sync_prc : process(all)
    begin
        if rising_edge(clk) then
            if (reset = '1') then
                input_stage     <= IDLE;
                output_stage    <= IDLE;
                selector        <= 0;
                in_pntr         <= MAX_BUFFER_SIZE;
                out_pntr        <= MAX_BUFFER_SIZE;
                src_addr        <= (others => '0');
                dest_addr       <= (others => '0');
                ports           <= (others => '0');
                length          <= (others => '0');
                packet_end      <= (others => '0');
                buff            <= (others => (others => '0'));
            else
                input_stage     <= input_stage_next;
                output_stage    <= output_stage_next;
                selector        <= selector_next;
                in_pntr         <= in_pntr_next;
                out_pntr        <= out_pntr_next;
                src_addr        <= src_addr_next;
                dest_addr       <= dest_addr_next;
                ports           <= ports_next;
                length          <= length_next;
                packet_end      <= packet_end_next;
                buff            <= buff_next;
            end if;
        end if;
    end process;
    
end architecture;