-- altera vhdl_input_version vhdl_2008
-- XXX: QSYS Fix (https://www.intel.com/content/www/us/en/support/programmable/articles/000079458.html)

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.ros_config_package.all;

entity ros_static_discovery_writer is
    port (
        -- SYSTEM
        clk                     : in std_logic;
        reset                   : in std_logic;
        -- TO/FROM RTPS ENDPOINT
        start                   : in std_logic;
        opcode                  : in HISTORY_CACHE_OPCODE_TYPE;
        ack                     : out std_logic;
        done                    : out std_logic;
        ret                     : out HISTORY_CACHE_RESPONSE_TYPE;
        seq_nr                  : in SEQUENCENUMBER_TYPE;
        get_data                : in std_logic;
        data_out                : out std_logic_vector(WORD_WIDTH-1 downto 0);
        valid_out               : out std_logic;
        ready_out               : in std_logic;
        last_word_out           : out std_logic; 
        liveliness_assertion    : out std_logic;
        data_available          : out std_logic;
        -- Cache Change
        cc_instance_handle      : out INSTANCE_HANDLE_TYPE;
        cc_kind                 : out CACHE_CHANGE_KIND_TYPE;
        cc_source_timestamp     : out TIME_TYPE;
        cc_seq_nr               : out SEQUENCENUMBER_TYPE
    );
end entity;

architecture arch of ros_static_discovery_writer is
    
    constant SN : SEQUENCENUMBER_TYPE := FIRST_SEQUENCENUMBER;
    
    --*****TYPE DECLARATION*****
    type STAGE_TYPE is (IDLE,CACHE_CHANGE,DATA,RET_SN,RET_RTPS);
    
    --*****SIGNAL DECLARATION*****
    signal stage, stage_next : STAGE_TYPE;
    signal ret_code, ret_code_next : HISTORY_CACHE_RESPONSE_TYPE;
    signal data_available_sig, data_available_sig_next : std_logic;
    signal cnt, cnt_next : integer range 0 to ROS_DISCOVERY_DATA.length;
    
begin
    
    liveliness_assertion    <= '0';
    data_available          <= data_available_sig;
    
    main_prc : process(all)
    begin
        -- DEFAULT
        stage_next              <= stage;
        cnt_next                <= cnt;
        ret_code_next           <= ret_code;
        data_available_sig_next <= data_available_sig;
        -- DEFAULT Unregistered
        ack                 <= '0';
        done                <= '0';
        ret                 <= OK;
        data_out            <= (others => '0');
        valid_out           <= '0';
        last_word_out       <= '0';
        cc_instance_handle  <= HANDLE_NIL;
        cc_kind             <= ALIVE;
        cc_source_timestamp <= TIME_INVALID;
        cc_seq_nr           <= SEQUENCENUMBER_UNKNOWN;
        
        
        case (stage) is
            when IDLE =>
                if (start = '1') then
                    ack <= '1';
                    case (opcode) is
                        when GET_MIN_SN =>
                            stage_next  <= RET_SN;
                        when GET_MAX_SN =>
                            -- Reset Data Available
                            data_available_sig_next <= '0';
                            
                            stage_next  <= RET_SN;
                        when GET_CACHE_CHANGE =>
                            cnt_next    <= 0;
                            stage_next  <= CACHE_CHANGE;
                        when ACK_CACHE_CHANGE =>
                            ret_code_next   <= OK;
                            stage_next      <= RET_RTPS;
                        when NACK_CACHE_CHANGE =>
                            ret_code_next   <= OK;
                            stage_next      <= RET_RTPS;
                        when REMOVE_CACHE_CHANGE =>
                            ret_code_next   <= OK;
                            stage_next      <= RET_RTPS;
                        when others =>
                    end case;
                end if;
            when CACHE_CHANGE =>
                done        <= '1';
                ret         <= OK;
                cc_seq_nr   <= SN;
                
                if (get_data = '1') then
                    stage_next  <= DATA;
                else
                    stage_next  <= IDLE;
                end if;
            when DATA =>
                valid_out   <= '1';
                data_out    <= ROS_DISCOVERY_DATA.data(cnt);
                if (cnt = ROS_DISCOVERY_DATA.length-1) then
                    last_word_out   <= '1';
                end if;
                
                if (ready_out = '1') then
                    if (cnt = ROS_DISCOVERY_DATA.length-1) then
                        stage_next  <= IDLE;
                    else
                        cnt_next    <= cnt + 1;
                    end if;
                end if;
            when RET_SN =>
                done        <= '1';
                ret         <= OK;
                cc_seq_nr   <= SN;
                
                stage_next  <= IDLE;
            when RET_RTPS =>
                done    <= '1';
                ret     <= ret_code;
                
                stage_next  <= IDLE;
        end case;
    end process;
    
    sync_prc : process(clk)
    begin
        if rising_edge(clk) then
            if (reset = '1') then
                stage               <= IDLE;
                cnt                 <= 0;
                ret_code            <= ERROR;
                data_available_sig  <= '1';
            else
                stage               <= stage_next;
                cnt                 <= cnt_next;
                ret_code            <= ret_code_next;
                data_available_sig  <= data_available_sig_next;
            end if;
        end if;
    end process;
    
end architecture;