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

use work.typedef_package.all;

-- Architecture
--   ---      ----------      ------      -------      ---
--->|ADC|--->|Delay Line|--->|Scaler|--->|ADD/SUB|--->|DAC|--->
--   ---      ----------      ------      -------      ---
--                                           ^
--   ---                       -----    ---  |
--->|ADC|-------------------->|Latch|->|   | |
--   ---                       -----   |MUX|--
--                              GND--->|   |
--                                      ---

-- Feedback Loop
-- This entity implements the feedback loop as defined above.
-- In addition to that, this entity latches and provides various max values that can be used for debug
-- purposes.

entity feedback_loop is
    port (
        clk             : in std_logic;
        reset           : in std_logic;
        adc_data_in1    : in std_logic;     -- PMOD-AD1
        adc_data_in2    : in std_logic;     -- PMOD-AD1
        adc_cs_n        : out std_logic;    -- PMOD-AD1
        adc_sclk        : out std_logic;    -- PMOD-AD1
        dac_data_out    : out std_logic;    -- PMOD-DA3
        dac_cs_n        : out std_logic;    -- PMOD-DA3
        dac_ldac        : out std_logic;    -- PMOD-DA3
        dac_sclk        : out std_logic;    -- PMOD-DA3
        -- DYNAMIC CONFIGURATION
        addsub_mode     : in std_logic;                                 -- (1=ADD, 0=SUB)
        add_input_mux   : in std_logic;                                 -- (1=Double Input 2, 0=Single Input)
        delay           : in std_logic_vector(DELAY_WIDTH-1 downto 0);  -- Unsigned delay clock count
        factor1         : in std_logic_vector(FACTOR_WIDTH-1 downto 0); -- Q1.x Fixed Point
        factor2         : in std_logic_vector(FACTOR_WIDTH-1 downto 0); -- Q1.x Fixed Point
        -- DEBUG
        reset_debug     : in std_logic;
        adc_data1_max   : out std_logic_vector(ADC_DATA_WIDTH-1 downto 0);
        adc_data2_max   : out std_logic_vector(ADC_DATA_WIDTH-1 downto 0);
        scaler_max      : out std_logic_vector(DAC_DATA_WIDTH-1 downto 0);
        dac_max         : out std_logic_vector(DAC_DATA_WIDTH-1 downto 0)
    );
end entity;

architecture arch of feedback_loop is
    
    --*****COMPONENT DECLARATION*****
    component pmod_ad1_ctrl is
        generic(
            TRANSFER_CLK_COUNT  : integer := 16;
            DELAY_CLK_CNT       : integer := 2;
            DATA_WIDTH          : integer := 12
        );
        port (
            clk     : in std_logic;
            reset   : in std_logic;
            enable  : in std_logic;
            sdata1  : in std_logic; -- PMOD-AD1
            sdata2  : in std_logic; -- PMOD-AD1
            sclk    : out std_logic;-- PMOD-AD1
            cs_n    : out std_logic;-- PMOD-AD1
            data1   : out std_logic_vector(DATA_WIDTH-1 downto 0);
            data2   : out std_logic_vector(DATA_WIDTH-1 downto 0);
            done    : out std_logic
        );
    end component;
    
    component pmod_da3_ctrl is
        generic(
            TRANSFER_CLK_COUNT  : integer := 16;
            DATA_WIDTH          : integer := 16
        );
        port (
            clk     : in std_logic;
            reset   : in std_logic;
            start   : in std_logic;
            data    : in std_logic_vector(DATA_WIDTH-1 downto 0);
            cs_n    : out std_logic;-- PMOD-DA3
            sdata   : out std_logic;-- PMOD-DA3
            ldac    : out std_logic;-- PMOD-DA3
            sclk    : out std_logic;-- PMOD-DA3
            done    : out std_logic
        );
    end component;
    
    component scaler is
        generic (
            DATA_WIDTH      : integer := 12;
            FACTOR_WIDTH    : integer := 4;
            PIPELINE_STAGES : integer := 1
        );
        port (
            clk         : in std_logic;  
            data_in     : in std_logic_vector(DATA_WIDTH-1 downto 0);
            factor      : in std_logic_vector(FACTOR_WIDTH-1 downto 0);
            data_out    : out std_logic_vector(DATA_WIDTH+FACTOR_WIDTH-1 downto 0)
        );
    end component;
    
    component delay_line is
            generic (
                DATA_WIDTH  : integer := 12;
                DELAY_WIDTH : integer := 8;
                MAX_DELAY   : integer := 200
            );
            port (
                clk         : in std_logic;
                reset       : in std_logic;
                delay       : in std_logic_vector(DELAY_WIDTH-1 downto 0);
                data_in     : in std_logic_vector(DATA_WIDTH-1 downto 0);
                data_out    : out std_logic_vector(DATA_WIDTH-1 downto 0)
            );
    end component;
    
    --*****CONSTANT DECLARATION*****
    constant CONST_MAX  : unsigned(DAC_DATA_WIDTH-1 downto 0) := (others => '1');
    constant CONST_HALF : unsigned(DAC_DATA_WIDTH-1 downto 0) := (DAC_DATA_WIDTH-1 => '1', others => '0');
    constant FACTOR_ONE : unsigned(FACTOR_WIDTH-1 downto 0) := (FACTOR_WIDTH-1 => '1', others => '0');
    
    --*****SIGNAL DECLARATION*****
    signal adc_data1, adc_data2, adc_data1_latch : std_logic_vector(ADC_DATA_WIDTH-1 downto 0) := (others => '0');
    signal adc_done : std_logic := '0';
    signal data1_A : std_logic_vector(ADC_DATA_WIDTH-1 downto 0) := (others => '0');
    signal data2_A : std_logic_vector(ADC_DATA_WIDTH downto 0) := (others => '0');
    signal data2_C, data1_C : std_logic_vector(DAC_DATA_WIDTH downto 0) := (others => '0');
    signal data_out, data2_D, data1_D : std_logic_vector(DAC_DATA_WIDTH-1 downto 0) := (others => '0');
    signal data1_B, data2_B, scaler_offset_1, scaler_offset_2 : std_logic_vector(DAC_DATA_WIDTH downto 0) := (others => '0');
    signal done_II, done_IV : std_logic := '0';
    signal offset_factor1, offset_factor2, factor1_latch, factor2_latch : std_logic_vector(FACTOR_WIDTH-1 downto 0) := (others => '0');
    signal add_input_mux_latch, addsub_mode_latch : std_logic := '0';
    
begin

    --*****STAGE I*****    
    adc_inst : pmod_ad1_ctrl
        generic map(
            TRANSFER_CLK_COUNT  => ADC_TRANSFER_CLK_COUNT,
            DELAY_CLK_CNT       => ADC_DELAY_CLK_CNT,
            DATA_WIDTH          => ADC_DATA_WIDTH
        )
        port map(
            clk     => clk,
            reset   => reset,
            enable  => '1',
            sdata1  => adc_data_in1,
            sdata2  => adc_data_in2,
            sclk    => adc_sclk,
            cs_n    => adc_cs_n,
            data1   => adc_data1,
            data2   => adc_data2,
            done    => adc_done
        );
    
    --*****STAGE II*****
    
    -- NOTE: The Input1 Signal has to be latched if the delay line is used, so that 
    --       there is a valid Signal to do operations with after the delay line.
    latch_1_prc : process (clk)
    begin
        if rising_edge(clk) then
            if (adc_done = '1') then
                adc_data1_latch <= adc_data1;
            end if;
        end if;
    end process;
    
    data1_prc : process (all)
    begin
        -- If delay_line disabled or Signle Input Mode
        if (delay = (delay'range => '0') or add_input_mux = '0') then
            data1_A    <= adc_data1;
        else
            data1_A    <= adc_data1_latch;
        end if;
    end process;
    
    delay_line_2_inst : delay_line
        generic map(
            DATA_WIDTH  => ADC_DATA_WIDTH+1,
            DELAY_WIDTH => DELAY_WIDTH,
            MAX_DELAY   => MAX_DELAY
        )
        port map(
            clk         => clk,
            reset       => reset,
            delay       => delay,
            data_in     => (adc_done & adc_data2),
            data_out    => data2_A
        );
    
    offset_factor_1_prc : process (all)
        variable tmp_res : unsigned(FACTOR_WIDTH-1 downto 0) := (others => '0');
    begin
        if (factor1(FACTOR_WIDTH-1) = '1') then
            offset_factor1  <= "0" & std_logic_vector(factor1(FACTOR_WIDTH-2 downto 0));
        else
            tmp_res := FACTOR_ONE - unsigned(factor1);
            offset_factor1  <= "0" & std_logic_vector(tmp_res(FACTOR_WIDTH-2 downto 0));
        end if;
    end process;
    
    offset_factor_2_prc : process (all)
        variable tmp_res : unsigned(FACTOR_WIDTH-1 downto 0) := (others => '0');
    begin
        if (factor2(FACTOR_WIDTH-1) = '1') then
            offset_factor2  <= "0" & std_logic_vector(factor2(FACTOR_WIDTH-2 downto 0));
        else
            tmp_res := FACTOR_ONE - unsigned(factor2);
            offset_factor2  <= "0" & std_logic_vector(tmp_res(FACTOR_WIDTH-2 downto 0));
        end if;
    end process;
    
    -- NOTE: We delay the config signals to synchronize them with the data flow and
    --       prevent glitches in the case that the config is changed between the stages.
    config_latch : process (clk)
    begin
        if rising_edge(clk) then
            factor1_latch       <= factor1;
            factor2_latch       <= factor2;
            add_input_mux_latch <= add_input_mux;
            addsub_mode_latch   <= addsub_mode;    
        end if;
    end process;
    
    --*****STAGE III*****
    
    scaler_1_inst : scaler
        generic map(
            DATA_WIDTH      => ADC_DATA_WIDTH,
            FACTOR_WIDTH    => FACTOR_WIDTH,
            PIPELINE_STAGES => 1
        )
        port map(
            clk         => clk,
            data_in     => data1_A,
            factor      => factor1,
            data_out    => data1_B
        );
    
    scaler_offset_1_inst : scaler
        generic map(
            DATA_WIDTH      => ADC_DATA_WIDTH,
            FACTOR_WIDTH    => FACTOR_WIDTH,
            PIPELINE_STAGES => 1
        )
        port map(
            clk         => clk,
            data_in     => (ADC_DATA_WIDTH-1 => '0', others => '1'),
            factor      => offset_factor1,
            data_out    => scaler_offset_1
        );
    
    addsub_1_prc : process (all)
    begin
        if (factor1_latch(FACTOR_WIDTH-1) = '1') then
            data1_C <= std_logic_vector(unsigned(data1_B) - unsigned(scaler_offset_1));
        else
            data1_C <= std_logic_vector(unsigned(data1_B) + unsigned(scaler_offset_1));
        end if;
    end process;
    
    cap_1_prc : process(all)
    begin
        if (data1_C(DAC_DATA_WIDTH) = '1') then
            if (factor1_latch(FACTOR_WIDTH-1) = '1' and data1_B(DAC_DATA_WIDTH) = '0') then
                data1_D  <= (others => '0');
            else
                data1_D  <= (others => '1');
            end if;
        else
            data1_D  <= data1_C(DAC_DATA_WIDTH-1 downto 0);
        end if;
    end process;
    
    scaler_2_inst : scaler
        generic map(
            DATA_WIDTH      => ADC_DATA_WIDTH,
            FACTOR_WIDTH    => FACTOR_WIDTH,
            PIPELINE_STAGES => 1
        )
        port map(
            clk         => clk,
            data_in     => data2_A(ADC_DATA_WIDTH-1 downto 0),
            factor      => factor2,
            data_out    => data2_B
        );
    
    scaler_offset_2_inst : scaler
        generic map(
            DATA_WIDTH      => ADC_DATA_WIDTH,
            FACTOR_WIDTH    => FACTOR_WIDTH,
            PIPELINE_STAGES => 1
        )
        port map(
            clk         => clk,
            data_in     => (ADC_DATA_WIDTH-1 => '0', others => '1'),
            factor      => offset_factor2,
            data_out    => scaler_offset_2
        );
    
    addsub_2_prc : process (all)
    begin
        if (factor2_latch(FACTOR_WIDTH-1) = '1') then
            data2_C <= std_logic_vector(unsigned(data2_B) - unsigned(scaler_offset_2));
        else
            data2_C <= std_logic_vector(unsigned(data2_B) + unsigned(scaler_offset_2));
        end if;
    end process;
    
    cap_2_prc : process(all)
    begin
        if (data2_C(DAC_DATA_WIDTH) = '1') then
            if (factor2_latch(FACTOR_WIDTH-1) = '1' and data2_B(DAC_DATA_WIDTH) = '0') then
                data2_D  <= (others => '0');
            else
                data2_D  <= (others => '1');
            end if;
        else
            data2_D  <= data2_C(DAC_DATA_WIDTH-1 downto 0);
        end if;
    end process;
    
    done_II_prc : process(clk)
    begin
        if (rising_edge(clk)) then
            if (reset = '1') then
                done_II <= '0';
            else
                -- Signle Input Mode
                if (add_input_mux_latch = '0') then
                    -- Synchronize on ADC Output
                    done_II <= adc_done;
                else
                    -- Synchronize on Delay Line Output
                    done_II <= data2_A(ADC_DATA_WIDTH);
                end if;
            end if;
        end if;
    end process;
    
    --*****STAGE IV*****
    
    add_sub_prc : process (clk)
        variable tmp_res : unsigned(DAC_DATA_WIDTH+1 downto 0) := (others => '0');
    begin
        if rising_edge(clk) then
            if (reset = '1') then
                data_out    <= (others => '0');
            else
                -- Double Input Mode
                if (add_input_mux_latch = '1') then
                    -- ADD
                    if (addsub_mode_latch = '1') then
                        tmp_res := unsigned("00" & data1_D) + unsigned("00" & data2_D);
                        tmp_res := tmp_res + ("00" & CONST_HALF);
                        -- Overflow
                        if (tmp_res(DAC_DATA_WIDTH+1) = '1') then
                            data_out  <= (others => '1');
                        -- Underflow
                        elsif (tmp_res(DAC_DATA_WIDTH+1 downto DAC_DATA_WIDTH) = "00") then
                            data_out  <= (others => '0');
                        else
                            data_out  <= std_logic_vector(tmp_res(DAC_DATA_WIDTH-1 downto 0));
                        end if;
                    -- SUB
                    else
                        tmp_res := unsigned("00" & data1_D) - unsigned("00" & data2_D);
                        tmp_res := tmp_res + ("00" & CONST_HALF);
                        -- Underflow
                        if (tmp_res(DAC_DATA_WIDTH+1 downto DAC_DATA_WIDTH) = "11") then
                            data_out  <= (others => '0');
                        -- Overflow
                        elsif (tmp_res(DAC_DATA_WIDTH) = '1') then
                            data_out  <= (others => '1');
                        else
                            data_out  <= std_logic_vector(tmp_res(DAC_DATA_WIDTH-1 downto 0));
                        end if;
                    end if;
                -- Single Input Mode
                else
                    -- ADD
                    if (addsub_mode_latch = '1') then
                        data_out  <= data1_D;
                    -- SUB
                    else
                        data_out  <= not data1_D;
                    end if;
                end if;
            end if;
        end if;
    end process;
    
    done_IV_prc : process(clk)
    begin
        if (rising_edge(clk)) then
            if (reset = '1') then
                done_IV <= '0';
            else
                done_IV <= done_II;
            end if;
        end if;
    end process;
    
    --*****STAGE V*****
    
    dac_inst : pmod_da3_ctrl
        generic map(
            TRANSFER_CLK_COUNT  => DAC_TRANSFER_CLK_COUNT,
            DATA_WIDTH          => DAC_DATA_WIDTH
        )
        port map(
            clk     => clk,
            reset   => reset,
            start   => done_IV,
            data    => data_out,
            cs_n    => dac_cs_n,
            sdata   => dac_data_out,
            ldac    => dac_ldac,
            sclk    => dac_sclk,
            done    => open
        );
    
    --*****DEBUG*****
    process(clk)
    begin
        if (rising_edge(clk)) then
            if (reset = '1' or reset_debug = '1') then
                adc_data1_max   <= (others => '0');
                adc_data2_max   <= (others => '0');
                dac_max         <= (others => '0');
                scaler_max      <= (others => '0');
            -- ADC MAX VALUES
            elsif (adc_done = '1') then
                if (to_integer(unsigned(adc_data1)) >= to_integer(unsigned(adc_data1_max))) then
                    adc_data1_max   <= adc_data1;
                elsif (to_integer(unsigned(adc_data2)) >= to_integer(unsigned(adc_data2_max))) then
                    adc_data2_max   <= adc_data2;
                end if;
            -- SCALER MAX VALUES
            elsif (done_II = '1') then
                if (to_integer(unsigned(data1_B)) >= to_integer(unsigned(scaler_max))) then
                    scaler_max  <= data1_B(DAC_DATA_WIDTH-1 downto 0);
                end if;
            -- DAC MAX VALUES
            elsif (done_IV = '1') then
                if (to_integer(unsigned(data_out)) >= to_integer(unsigned(dac_max))) then
                    dac_max <= data_out;
                end if;
            end if;
        end if;
    end process;
    
    
    
end architecture;
