-- 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;
use work.ros_config;
use work.GoalStatus_package;
use work.Fibonacci_package.all;

entity Fibonacci is
    port (
        -- SYSTEM
        clk                     : in std_logic;
        reset                   : in std_logic;
        time                    : in ROS_TIME_TYPE;
        -- SERVICE SERVER
        start                   : out std_logic;
        opcode                  : out ROS_ACTION_OPCODE_TYPE;
        ack                     : in std_logic;
        done                    : in std_logic;
        return_code             : in std_logic_vector(ROS_RETCODE_WIDTH-1 downto 0);
        goal_handle_in          : out std_logic_vector(GOAL_HANDLE_WIDTH-1 downto 0);
        goal_handle_out         : in std_logic_vector(GOAL_HANDLE_WIDTH-1 downto 0);
        goal_state_in           : out std_logic_vector(CDR_INT8_WIDTH-1 downto 0);
        goal_state_out          : in std_logic_vector(CDR_INT8_WIDTH-1 downto 0);
        goal_id                 : in std_logic_vector(UUID_WIDTH-1 downto 0);
        goal_result_index       : in std_logic_vector(WORD_WIDTH-1 downto 0);
        goal_stamp              : in ROS_TIME_TYPE;
        -- GOAL
        new_goal_request        : in std_logic;
        new_goal_handle         : in std_logic_vector(GOAL_HANDLE_WIDTH-1 downto 0);
        new_goal_result_index   : in std_logic_vector(WORD_WIDTH-1 downto 0);
        new_goal_order          : in std_logic_vector(CDR_LONG_WIDTH-1 downto 0);
        new_goal_accepted       : out std_logic;
        new_goal_response       : out std_logic;
        -- CANCEL
        cancel_request          : in std_logic;
        cancel_request_handle   : in std_logic_vector(GOAL_HANDLE_WIDTH-1 downto 0);
        cancel_accepted         : out std_logic;
        cancel_response         : out std_logic;
        -- RESULT SIGNALS
        index_result            : out std_logic_vector(WORD_WIDTH-1 downto 0);
        index_result_wen        : out std_logic;
        index_result_ready      : in std_logic;
        result_seq_len          : out std_logic_vector(R_RR_SEQ_ADDR_WIDTH-1 downto 0);
        result_seq_addr         : out std_logic_vector(R_RR_SEQ_ADDR_WIDTH-1 downto 0);
        result_seq_ready        : in std_logic;
        result_seq_ren          : out std_logic;
        result_seq_wen          : out std_logic;
        result_seq_valid        : in std_logic;
        result_seq_ack          : out std_logic;
        result_seq_r            : in std_logic_vector(CDR_LONG_WIDTH-1 downto 0);
        result_seq_w            : out std_logic_vector(CDR_LONG_WIDTH-1 downto 0);
        -- FEEDBACK SIGNALS
        feedback_seq_len        : out std_logic_vector(F_SEQ_ADDR_WIDTH-1 downto 0);
        feedback_seq_addr       : out std_logic_vector(F_SEQ_ADDR_WIDTH-1 downto 0);
        feedback_seq_ready      : in std_logic;
        feedback_seq_ren        : out std_logic;
        feedback_seq_wen        : out std_logic;
        feedback_seq_valid      : in std_logic;
        feedback_seq_ack        : out std_logic;
        feedback_seq_r          : in std_logic_vector(CDR_LONG_WIDTH-1 downto 0);
        feedback_seq_w          : out std_logic_vector(CDR_LONG_WIDTH-1 downto 0)
    );
end entity;

architecture arch of Fibonacci is
    
    --*****TYPE DECLARATION*****
    type STAGE_TYPE is (IDLE,DELAY_START,START_EXECUTION,INITIALIZE_EXECUTION,EXECUTE,FEEDBACK,TIMER,FINALIZE,CANCEL);
    
    --*****SIGNAL DECLARATION*****
    signal stage, stage_next : STAGE_TYPE;
    signal i, i_next : unsigned(R_RR_SEQ_ADDR_WIDTH-1 downto 0);
    signal order, order_next, a, a_next, b, b_next : unsigned(CDR_LONG_WIDTH-1 downto 0);
    signal handle, handle_next : std_logic_vector(GOAL_HANDLE_WIDTH-1 downto 0);
    signal ind, ind_next : std_logic_vector(WORD_WIDTH-1 downto 0);
    signal cnt, cnt_next : natural range 0 to 2;
    signal deadline, deadline_next : ROS_TIME_TYPE;
    
begin
    
    index_result        <= ind;
    feedback_seq_len    <= std_logic_vector(i);
    result_seq_len      <= std_logic_vector(i);
    
    main_prc : process(all)
    begin
        -- DEFAULT
        stage_next      <= stage;
        order_next      <= order;
        a_next          <= a;
        b_next          <= b;
        handle_next     <= handle;
        ind_next        <= ind;
        cnt_next        <= cnt;
        i_next          <= i;
        deadline_next   <= deadline;
        -- DEFAULT Unregistered
        new_goal_accepted   <= '0';
        new_goal_response   <= '0';
        cancel_accepted     <= '0';
        cancel_response     <= '0';
        start               <= '0';
        index_result_wen    <= '0';
        opcode              <= NOP;
        goal_handle_in      <= GOAL_HANDLE_UNKNOWN;
        goal_state_in       <= GoalStatus_package.STATUS_UNKNOWN;
        feedback_seq_ren    <= '0';
        result_seq_ren      <= '0';
        feedback_seq_wen    <= '0';
        result_seq_wen      <= '0';
        feedback_seq_ack    <= '0';
        result_seq_ack      <= '0';
        feedback_seq_addr   <= (others => '0');
        result_seq_addr     <= (others => '0');
        feedback_seq_w      <= (others => '0');
        result_seq_w        <= (others => '0');
        
        case (stage) is
            when IDLE =>
                -- NOTE: Because of race conditions that can occur with the Cyclone DDS implementation, we wait
                --       2 seconds to make sure that all remote readers have received an initial HEARTBEAT.
                if (new_goal_request = '1' or cancel_request = '1') then
                    -- NOTE: During simulation we essentially skip the timer
                    if (ros_config.SIMULATION_TIMING) then
                        deadline_next   <= time;
                    else
                        deadline_next.sec       <= std_logic_vector(unsigned(time.sec) + 2);
                        deadline_next.nanosec   <= time.nanosec;
                    end if;
                    stage_next  <= DELAY_START;
                end if;
            when DELAY_START =>
                if (deadline <= time) then
                    if (new_goal_request = '1') then
                        new_goal_accepted   <= '1';
                        new_goal_response   <= '1';
                        
                        -- Latch DATA
                        order_next  <= unsigned(new_goal_order);
                        handle_next <= new_goal_handle;
                        ind_next    <= new_goal_result_index;
                        
                        stage_next  <= START_EXECUTION;
                        cnt_next    <= 0;
                    elsif (cancel_request = '1') then
                        cancel_response <= '1';
                        stage_next      <= IDLE;
                    end if;
                end if;
            when START_EXECUTION =>
                case (cnt) is
                    when 0 =>
                        start           <= '1';
                        opcode          <= UPDATE_GOAL_STATE;
                        goal_handle_in  <= handle;
                        goal_state_in   <= GoalStatus_package.STATUS_EXECUTING;
                        if (ack = '1') then
                            cnt_next    <= cnt + 1;
                        end if;
                    when 1 =>
                        if (done = '1') then
                            assert (return_code = ROS_RET_OK) severity FAILURE;
                            stage_next  <= INITIALIZE_EXECUTION;
                            cnt_next    <= 0;
                        end if;
                    when others =>
                        null;
                end case;
            when INITIALIZE_EXECUTION =>
                case (cnt) is
                    when 0 =>
                        feedback_seq_addr   <= std_logic_vector(i);
                        result_seq_addr     <= std_logic_vector(i);
                        feedback_seq_w      <= (others => '0');
                        result_seq_w        <= (others => '0');
                        a_next              <= (others => '0');
                        
                        if (feedback_seq_ready  = '1' and result_seq_ready = '1') then
                            feedback_seq_wen    <= '1';
                            result_seq_wen      <= '1';
                            
                            i_next      <= i + 1;
                            cnt_next    <= cnt + 1;
                        end if;
                    when 1 =>
                        feedback_seq_addr   <= std_logic_vector(i);
                        result_seq_addr     <= std_logic_vector(i);
                        feedback_seq_w      <= (0 => '1', others => '0');
                        result_seq_w        <= (0 => '1', others => '0');
                        b_next              <= (0 => '1', others => '0');
                        
                        if (feedback_seq_ready  = '1' and result_seq_ready = '1') then
                            feedback_seq_wen    <= '1';
                            result_seq_wen      <= '1';
                            
                            i_next      <= i + 1;
                            if (order /= 0) then
                                order_next  <= order - 1;
                            end if;
                            stage_next  <= TIMER;
                            cnt_next    <= 0;
                        end if;
                    when others =>
                        null;
                end case;
            when EXECUTE =>
                case (cnt) is
                    when 0 =>
                        a_next  <= b;
                        b_next  <= a + b;
                        
                        cnt_next    <= cnt + 1;
                    when 1 =>
                        feedback_seq_addr   <= std_logic_vector(i);
                        result_seq_addr     <= std_logic_vector(i);
                        feedback_seq_w      <= std_logic_vector(b);
                        result_seq_w        <= std_logic_vector(b);
                        
                        if (feedback_seq_ready  = '1' and result_seq_ready = '1') then
                            feedback_seq_wen    <= '1';
                            result_seq_wen      <= '1';
                            
                            i_next      <= i + 1;
                            order_next  <= order - 1;
                            stage_next  <= FEEDBACK;
                            cnt_next    <= 0;
                        end if;
                    when others =>
                        null;
                end case;
            when FEEDBACK =>
                case (cnt) is
                    when 0 =>
                        start           <= '1';
                        opcode          <= PUBLISH_FEEDBACK;
                        goal_handle_in  <= handle;
                        if (ack = '1') then
                            cnt_next    <= cnt + 1;
                        end if;
                    when 1 =>
                        if (done = '1') then
                            assert (return_code = ROS_RET_OK) severity FAILURE;
                            stage_next  <= TIMER;
                            cnt_next    <= 0;
                        end if;
                    when others =>
                        null;
                end case;
            when TIMER =>
                case (cnt) is
                    when 0 =>
                        -- NOTE: During simulation we essentially skip the timer
                        if (ros_config.SIMULATION_TIMING) then
                            deadline_next   <= time;
                        else
                            deadline_next.sec       <= std_logic_vector(unsigned(time.sec) + 1);
                            deadline_next.nanosec   <= time.nanosec;
                        end if;
                        
                        cnt_next    <= cnt + 1;
                    when 1 =>
                        if (cancel_request = '1') then
                            cancel_response <= '1';
                            
                            if (cancel_request_handle = handle) then
                                cancel_accepted <= '1';
                                stage_next      <= CANCEL;
                                cnt_next        <= 0;
                            end if;
                        elsif (deadline <= time) then
                            if (order = 0) then
                                stage_next  <= FINALIZE;
                                cnt_next    <= 0;
                            else
                                stage_next  <= EXECUTE;
                                cnt_next    <= 0;
                            end if;
                        end if;
                    when others =>
                        null;
                end case;
            when FINALIZE =>
                case (cnt) is
                    when 0 =>
                        index_result_wen    <= '1';
                        
                        if (index_result_ready = '1') then
                            cnt_next    <= cnt + 1;
                        end if;
                    when 1 =>
                        start           <= '1';
                        opcode          <= UPDATE_GOAL_STATE;
                        goal_handle_in  <= handle;
                        goal_state_in   <= GoalStatus_package.STATUS_SUCCEEDED;
                        if (ack = '1') then
                            cnt_next    <= cnt + 1;
                        end if;
                    when 2 =>
                        if (done = '1') then
                            assert (return_code = ROS_RET_OK) severity FAILURE;
                            stage_next  <= IDLE;
                        end if;
                    when others =>
                        null;
                end case;
            when CANCEL =>
                case (cnt) is
                    when 0 =>
                        start           <= '1';
                        opcode          <= UPDATE_GOAL_STATE;
                        goal_handle_in  <= handle;
                        goal_state_in   <= GoalStatus_package.STATUS_CANCELED;
                        if (ack = '1') then
                            cnt_next    <= cnt + 1;
                        end if;
                    when 1 =>
                        if (done = '1') then
                            assert (return_code = ROS_RET_OK) severity FAILURE;
                            stage_next  <= IDLE;
                            cnt_next    <= 0;
                        end if;
                    when others =>
                        null;
                end case;
        end case;
    end process;
    
    sync_prc : process(clk)
    begin
        if rising_edge(clk) then
            if (reset = '1') then
                stage       <= IDLE;
                handle      <= GOAL_HANDLE_UNKNOWN;
                deadline    <= ROS_TIME_INFINITE;
                cnt         <= 0;
                order       <= (others => '0');
                a           <= (others => '0');
                b           <= (others => '0');
                ind         <= (others => '0');
                i           <= (others => '0');
            else
                stage       <= stage_next;
                handle      <= handle_next;
                deadline    <= deadline_next;
                cnt         <= cnt_next;
                order       <= order_next;
                a           <= a_next;
                b           <= b_next;
                ind         <= ind_next;
                i           <= i_next;
            end if;
        end if;
    end process;
    
end architecture;
