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

use work.typedef_package.all;

-- Feedback Controller
-- This entity reads from the config memory and applies the resepctive configuration when the relevant 
-- timestamp is reached or exceeded. The timer starts counting when a high level is detected in the 'sync_pulse'
-- signal after a reset.
-- NOTE: This entity is continuosly reading from the memory, except when the 'reset' signal is held high.

-- Config Mapping:
-- mem_data(63)     : Slot Enable
-- mem_data(62)     : Addsub_mode
-- mem_data(61)     : Add_input_mux
-- mem_data(47-40)  : delay
-- mem_data(36-32)  : factor
-- mem_data(31-0)   : timestamp


entity feedback_controller is
    port (
        clk             : in std_logic;
        reset           : in std_logic;
        
        sync_pulse      : in std_logic;
        
        mem_addr        : out std_logic_vector(CONFIG_MEM_ADDR_WIDTH-1 downto 0);
        mem_ren         : out std_logic;
        mem_data        : in std_logic_vector(CONFIG_DATA_WIDTH-1 downto 0);
        
        addsub_mode     : out std_logic;
        add_input_mux   : out std_logic;
        delay           : out std_logic_vector(DELAY_WIDTH-1 downto 0);
        factor          : out std_logic_vector(FACTOR_WIDTH-1 downto 0)
    );
end entity;

architecture arch of feedback_controller is
    --*****CONSTANT DECLARATION*****
    constant inc : unsigned(CONFIG_MEM_ADDR_WIDTH-1 downto 0) := to_unsigned(1,CONFIG_MEM_ADDR_WIDTH);
    
    --*****SIGNAL DECLARATION*****
    signal slot_nr, slot_nr_next : std_logic_vector(CONFIG_MEM_ADDR_WIDTH-1 downto 0) := (others => '0');
    signal timer : std_logic_vector(TIMESTAMP_WIDTH-1 downto 0) := (others => '0');
    signal sync_pulse_arrived : std_logic;
    signal addsub_mode_next, addsub_mode_sig, add_input_mux_next, add_input_mux_sig : std_logic := '0';
    signal delay_next, delay_sig : std_logic_vector(DELAY_WIDTH-1 downto 0) := (others => '0');
    signal factor_next, factor_sig : std_logic_vector(FACTOR_WIDTH-1 downto 0) := (others => '0');
    
begin
    
    --OUTPUT SIGNALS
    -- Intermediate "_sig" signals needed because we cannot read from output pins
    mem_ren         <= not reset;
    mem_addr        <= slot_nr_next; -- Use 'next' signal to "asynchronously" set addr (Save 1 clock cycle)
    addsub_mode     <= addsub_mode_sig;
    add_input_mux   <= add_input_mux_sig;
    delay           <= delay_sig;
    factor          <= factor_sig;
    
    ctrl : process(all)
    begin
        --DEFAULT
        slot_nr_next        <= slot_nr;
        addsub_mode_next    <= addsub_mode_sig;
        add_input_mux_next  <= add_input_mux_sig;
        delay_next          <= delay_sig;
        factor_next         <= factor_sig;
        
        if(mem_data(CONFIG_DATA_WIDTH-1) = '1') then
            -- If timestamp is reached (or exceeded), apply configuration and fetch next configuration 
            -- slot
            if (unsigned(timer) >= unsigned(mem_data(TIMESTAMP_WIDTH-1 downto 0))) then
                addsub_mode_next    <= mem_data(CONFIG_DATA_WIDTH-2);
                add_input_mux_next  <= mem_data(CONFIG_DATA_WIDTH-3);
                delay_next          <= mem_data(CONFIG_DATA_WIDTH-17 downto CONFIG_DATA_WIDTH-16-DELAY_WIDTH);
                factor_next         <= mem_data(TIMESTAMP_WIDTH+FACTOR_WIDTH-1 downto TIMESTAMP_WIDTH);
                
                slot_nr_next    <= std_logic_vector(unsigned(slot_nr) + inc);
            end if;
        end if;
    end process;
    
    sync : process(clk) 
    begin
        if rising_edge(clk) then
            if (reset = '1') then
                slot_nr             <= (others => '0');
                addsub_mode_sig     <= '0';
                add_input_mux_sig   <= '0';
                delay_sig           <= (others => '0');
                factor_sig          <= (others => '0');
            else
                slot_nr             <= slot_nr_next;
                addsub_mode_sig     <= addsub_mode_next;
                add_input_mux_sig   <= add_input_mux_next;
                delay_sig           <= delay_next;
                factor_sig          <= factor_next;
            end if;
        end if;
    end process;
    
    timer_prc : process(clk) 
    begin
        if rising_edge(clk) then
            if (reset = '1') then
                timer           <= (others => '0');
                sync_pulse_arrived  <= '0';
            else
                if (sync_pulse_arrived = '0') then
                    -- Wait for rising edge of sync pulse
                    if (sync_pulse = '1') then
                        sync_pulse_arrived  <= '1';
                    end if;
                else
                    timer   <= std_logic_vector(unsigned(timer) + inc);
                end if;
            end if;
        end if;
    end process;
    
end architecture;