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=ADC Input 2, 0=GND)
        delay           : in std_logic_vector(DELAY_WIDTH-1 downto 0);  -- unsigned delay clock count
        factor          : in std_logic_vector(FACTOR_WIDTH-1 downto 0); -- 1Q3 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
    
    --*****SIGNAL DECLARATION*****
    signal adc_data1, adc_data2 : std_logic_vector(ADC_DATA_WIDTH-1 downto 0) := (others => '0');
    signal adc_done : std_logic := '0';
    
    --*****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 addsub is
        generic (
            PIPELINE_STAGES : integer := 1;
            DATA_WIDTH      : integer := 16
        );
        port (
            clk     : in std_logic;
            reset   : in std_logic;
            mode    : in std_logic;
            cap     : in std_logic;
            A       : in std_logic_vector(DATA_WIDTH-1 downto 0);
            B       : in std_logic_vector(DATA_WIDTH-1 downto 0);
            RES     : out std_logic_vector(DATA_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 delay_out : std_logic_vector(ADC_DATA_WIDTH downto 0) := (others => '0');
    signal tmp2 : std_logic_vector(ADC_DATA_WIDTH-1 downto 0) := (others => '0');
    signal inputA_wide, inputB_wide : std_logic_vector(DAC_DATA_WIDTH downto 0) := (others => '0');
    signal addsub_out, inputA, inputB : std_logic_vector(DAC_DATA_WIDTH-1 downto 0) := (others => '0');
    signal scaler_1_out, scaler_2_out, scaler_offset_1, scaler_offset_2 : std_logic_vector(DAC_DATA_WIDTH downto 0) := (others => '0');
    signal scaler_done, addsub_done : std_logic := '0';
    signal offset_factor, tmp : std_logic_vector(FACTOR_WIDTH-1 downto 0) := (others => '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*****
    delay_line_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_data1),
            data_out    => delay_out
        );
    
    latch_prc : process (all)
    begin
        if (delay = (delay'range => '0')) then
            tmp2    <= adc_data2;
        elsif rising_edge(clk) then
            if (adc_done = '1') then
                tmp2    <= adc_data2;
            end if;
        end if;
    end process;
    
    addsub_offset_inst : addsub
        generic map(
            PIPELINE_STAGES => 0,
            DATA_WIDTH      => FACTOR_WIDTH
        )
        port map(
            clk     => clk,
            reset   => reset,
            mode    => '0',
            cap     => '0',
            A       => (FACTOR_WIDTH-1 => '1', others => '0'),
            B       => factor,
            RES     => tmp
        );
    
    offset_factor   <= tmp when (factor(FACTOR_WIDTH-1) = '0') else ("0" & factor(FACTOR_WIDTH-2 downto 0));
    
    --*****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     => delay_out(ADC_DATA_WIDTH-1 downto 0),
            factor      => factor,
            data_out    => scaler_1_out
        );
    
    scaler_2_inst : scaler
        generic map(
            DATA_WIDTH      => ADC_DATA_WIDTH,
            FACTOR_WIDTH    => FACTOR_WIDTH,
            PIPELINE_STAGES => 1
        )
        port map(
            clk         => clk,
            data_in     => tmp2,
            factor      => "10101", --1.32
            data_out    => scaler_2_out
        );
    
    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_factor,
            data_out    => scaler_offset_1
        );
    
    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      => "00101", --0.32
            data_out    => scaler_offset_2
        );
    
    process(clk)
    begin
        if (rising_edge(clk)) then
            if (reset = '1') then
                scaler_done <= '0';
            else
                scaler_done <= delay_out(ADC_DATA_WIDTH);
            end if;
        end if;
    end process;
    
    addsub_1_inst : addsub
        generic map(
            PIPELINE_STAGES => 0,
            DATA_WIDTH      => DAC_DATA_WIDTH+1
        )
        port map(
            clk     => clk,
            reset   => reset,
            mode    => not factor(FACTOR_WIDTH-1),
            cap     => '0',
            A       => scaler_1_out,
            B       => scaler_offset_1,
            RES     => inputB_wide
        );
    
    addsub_2_inst : addsub
        generic map(
            PIPELINE_STAGES => 0,
            DATA_WIDTH      => DAC_DATA_WIDTH+1
        )
        port map(
            clk     => clk,
            reset   => reset,
            mode    => '0',
            cap     => '0',
            A       => scaler_2_out,
            B       => scaler_offset_2,
            RES     => inputA_wide
        );
    
    cap_B_prc : process(all)
    begin
        if (inputB_wide(DAC_DATA_WIDTH) = '1') then
            if (factor(FACTOR_WIDTH-1) = '1' and scaler_1_out(DAC_DATA_WIDTH) = '0') then
                inputB  <= (others => '0');
            else
                inputB  <= (others => '1');
            end if;
        else
            inputB  <= inputB_wide(DAC_DATA_WIDTH-1 downto 0);
        end if;
    end process;
    
    cap_A_prc : process(all)
    begin
        if (inputA_wide(DAC_DATA_WIDTH) = '1') then
            --if (factor(FACTOR_WIDTH-1) = '1' and scaler_2_out(DAC_DATA_WIDTH) = '0') then
            if (scaler_2_out(DAC_DATA_WIDTH) = '0') then
                inputA  <= (others => '0');
            else
                inputA  <= (others => '1');
            end if;
        else
            inputA  <= inputA_wide(DAC_DATA_WIDTH-1 downto 0);
        end if;
    end process;
    
    --*****STAGE IV*****
    
    --mux: process(all)
    --begin
    --    if (add_input_mux = '1') then
    --        if (inputA_wide(DAC_DATA_WIDTH) = '1') then
    --            --TODO: CAP Needed?
    --            inputA  <= (others => '0');
    --        else
    --            inputA  <= inputA_wide(DAC_DATA_WIDTH-1 downto 0);
    --        end if;
    --    else
    --        if (addsub_mode = '1') then
    --            inputA  <= (others => '0');
    --        else
    --            inputA  <= (others => '1');
    --        end if;
    --    end if;
    --end process;
    
    --add_sub_prc : process (all)
    --    variable tmp_res : unsigned(inputB'length-1 downto 0) := (others => '0');
    --begin
    --    if rising_edge(clk) then
    --        if (reset = '1') then
    --            addsub_out  <= (others => '0');
    --        else
    --            -- Both Inputs
    --            if (add_input_mux = '1') then
    --                -- ADD
    --                if (addsub_mode = '1') then
    --                    tmp_res  := unsigned(inputB) + unsigned(inputA);
    --                -- SUB
    --                else
    --                    tmp_res  := unsigned(inputB) - unsigned(inputA);
    --                end if;
    --                addsub_out  <= std_logic_vector(tmp_res + CONST_HALF);
    --            -- Single Input
    --            else
    --                -- ADD
    --                if (addsub_mode = '1') then
    --                    addsub_out  <= inputB;
    --                -- SUB
    --                else
    --                    addsub_out  <= not inputB;--std_logic_vector(CONST_MAX - unsigned(inputB));
    --                end if;
    --            end if;
    --        end if;
    --    end if;
    --end process;
    
    add_sub_prc : process (all)
        variable tmp_res : unsigned(DAC_DATA_WIDTH+1 downto 0) := (others => '0');
    begin
        if rising_edge(clk) then
            if (reset = '1') then
                addsub_out  <= (others => '0');
            else
                -- Both Inputs
                if (add_input_mux = '1') then
                    -- ADD
                    if (addsub_mode = '1') then
                        tmp_res := unsigned("00" & inputB) + unsigned("00" & inputA);
                        tmp_res := tmp_res + ("00" & CONST_HALF);
                        -- Overflow
                        if (tmp_res(DAC_DATA_WIDTH+1) = '1') then
                            addsub_out  <= (others => '1');
                        -- Underflow
                        elsif (tmp_res(DAC_DATA_WIDTH+1 downto DAC_DATA_WIDTH) = "00") then
                            addsub_out  <= (others => '0');
                        else
                            addsub_out  <= std_logic_vector(tmp_res(DAC_DATA_WIDTH-1 downto 0));
                        end if;
                    -- SUB
                    else
                        tmp_res  := unsigned("00" & inputB) - unsigned("00" & inputA);
                        tmp_res := tmp_res + ("00" & CONST_HALF);
                        -- Underflow
                        if (tmp_res(DAC_DATA_WIDTH+1 downto DAC_DATA_WIDTH) = "11") then
                            addsub_out  <= (others => '0');
                        -- Overflow
                        elsif (tmp_res(DAC_DATA_WIDTH) = '1') then
                            addsub_out  <= (others => '1');
                        else
                            addsub_out  <= std_logic_vector(tmp_res(DAC_DATA_WIDTH-1 downto 0));
                        end if;
                    end if;
                -- Single Input
                else
                    -- ADD
                    if (addsub_mode = '1') then
                        addsub_out  <= inputB;
                    -- SUB
                    else
                        addsub_out  <= not inputB;--std_logic_vector(CONST_MAX - unsigned(inputB));
                    end if;
                end if;
            end if;
        end if;
    end process;
    
--    addsub_instA : addsub
--        generic map(
--            PIPELINE_STAGES => 1,
--            DATA_WIDTH      => DAC_DATA_WIDTH
--        )
--        port map(
--            clk     => clk,
--            reset   => reset,
--            mode    => addsub_mode,
--            cap     => add_input_mux,
--            A       => inputA,
--            B       => inputB,
--            RES     => addsub_out
--        );
    
    process(clk)
    begin
        if (rising_edge(clk)) then
            if (reset = '1') then
                addsub_done <= '0';
            else
                addsub_done <= scaler_done;
            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   => addsub_done,
            data    => addsub_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;
            -- DAC MAX VALUES
            elsif (addsub_done = '1') then
                if (to_integer(unsigned(addsub_out)) >= to_integer(unsigned(dac_max))) then
                    dac_max <= addsub_out;
                end if;
            -- SCALER MAX VALUES
            elsif (scaler_done = '1') then
                if (to_integer(unsigned(scaler_1_out)) >= to_integer(unsigned(scaler_max))) then
                    scaler_max  <= scaler_1_out(DAC_DATA_WIDTH-1 downto 0);
                end if;
            end if;
        end if;
    end process;
    
    
    
end architecture;
