-- 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.rtps_package.all;
use work.ros_package.all;

entity AddTwoInts is
    port (
        -- SYSTEM
        clk             : in std_logic;
        reset           : in std_logic;
        -- SERVICE SERVER
        start           : out std_logic;
        ack             : in std_logic;
        opcode          : out ROS_SERVICE_OPCODE_TYPE;
        service_info    : in SERVICE_INFO_TYPE;
        request_id      : out REQUEST_ID_TYPE;
        taken           : in std_logic;
        data_available  : in std_logic;
        a               : in std_logic_vector(CDR_LONG_LONG_WIDTH-1 downto 0);
        b               : in std_logic_vector(CDR_LONG_LONG_WIDTH-1 downto 0);
        sum             : out std_logic_vector(CDR_LONG_LONG_WIDTH-1 downto 0);
        done            : in std_logic;
        return_code     : in std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0)
    );
end entity;

architecture arch of AddTwoInts is
    
    --*****TYPE DECLARATION*****
    type STAGE_TYPE is (IDLE,A1,B2,C3,D4);
    
    --*****SIGNAL DECLARATION*****
    signal stage, stage_next : STAGE_TYPE;
    signal sum_sig, sum_sig_next : std_logic_vector(CDR_LONG_LONG_WIDTH-1 downto 0);
    signal request_id_sig, request_id_sig_next : REQUEST_ID_TYPE;
    
begin
    
    sum         <= sum_sig;
    request_id  <= request_id_sig;
    
    main_prc : process(all)
    begin
        -- DEFAULT
        stage_next          <= stage;
        sum_sig_next        <= sum_sig;
        request_id_sig_next <= request_id_sig;
        -- DEFAULT Unregistered
        start               <= '0';
        opcode              <= NOP;
        
        case (stage) is
            when IDLE =>
                -- Reader has Available Data
                if (data_available = '1') then
                    stage_next  <= A1;
                end if;
            when A1 =>
                start   <= '1';
                opcode  <= TAKE_REQUEST;
                if (ack = '1') then
                    stage_next  <= B2;
                end if;
            when B2 =>
                if (done = '1') then
                    case (return_code) is
                        when ROS_RET_OK =>
                            if (taken = '1') then
                                sum_sig_next        <= std_logic_vector(unsigned(a) + unsigned(b));
                                request_id_sig_next <= service_info.request_id;
                                
                                stage_next  <= C3;
                            else
                                stage_next  <= IDLE;
                            end if;
                        when others =>
                            report "AddTwoInts: Error taking request" severity FAILURE;
                            stage_next  <= IDLE;
                    end case;
                end if;
            when C3 =>
                start   <= '1';
                opcode  <= SEND_RESPONSE;
                if (ack = '1') then
                    stage_next  <= D4;
                end if;
            when D4 =>
                if (done = '1') then
                    case (return_code) is
                        when ROS_RET_OK =>
                            -- DONE
                            stage_next  <= IDLE;
                        when others =>
                            report "AddTwoInts: Error sending response" severity FAILURE;
                            stage_next  <= IDLE;
                    end case;
                end if;
        end case;
    end process;
    
    sync_prc : process(clk)
    begin
        if rising_edge(clk) then
            if (reset = '1') then
                stage           <= IDLE;
                request_id_sig  <= EMPTY_REQUEST_ID;
                sum_sig         <= (others => '0');
            else
                stage           <= stage_next;
                request_id_sig  <= request_id_sig_next;
                sum_sig         <= sum_sig_next;
            end if;
        end if;
    end process;
    
end architecture;