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

use work.typedef_package.all;
use work.sine_package.all;

entity feedback_loop_tb is
end entity;

architecture beh of feedback_loop_tb is
    
    --*****COMPONENT DECLARATION*****
    component 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 component;
    
    type ADC_STAGE_TYPE is (ZERO, DATA);
    
    constant ZERO_SIGNAL : SIGNAL_RECORD_TYPE := (
        sine    => (others => (others => '0')),
        len     => 1
    );
    constant HALF_SIGNAL : SIGNAL_RECORD_TYPE := (
        sine    => (others => x"800"),
        len     => 1
    );
    
    --*****SIGNAL DECLARATION*****
    signal clk, reset : std_logic := '0';
    signal adc_data_in1, adc_data_in2, adc_cs_n : std_logic := '0';
    signal factor1, factor2 : std_logic_vector(FACTOR_WIDTH-1 downto 0) := (others => '0');
    signal addsub_mode, add_input_mux : std_logic := '0';
    signal delay : std_logic_vector(DELAY_WIDTH-1 downto 0) := (others => '0');
    signal adc_stage : ADC_STAGE_TYPE := ZERO;
    signal cnt1, cnt2, cnt3 : integer := 0;
    signal input1, input2 : std_logic_vector(ADC_DATA_WIDTH-1 downto 0) := (others => '0');
    signal output : std_logic_vector(DAC_DATA_WIDTH-1 downto 0) := (others => '0');
    signal sig1, sig2 : SIGNAL_RECORD_TYPE := ZERO_SIGNAL;
    
    procedure wait_samples( num : in natural) is
    begin
        -- Num * Sampling Cadence * Clock Period
        wait for num*18*50 ns;
        wait until rising_edge(clk);
    end procedure;
    
begin

    --*****COMPONENT INSTANTIATION*****
    uut : feedback_loop
    port map(
        clk             => clk,
        reset           => reset,
        adc_data_in1    => adc_data_in1,
        adc_data_in2    => adc_data_in2,
        adc_cs_n        => adc_cs_n,
        adc_sclk        => open,
        dac_data_out    => open,
        dac_cs_n        => open,
        dac_ldac        => open,
        dac_sclk        => open,
        addsub_mode     => addsub_mode,
        add_input_mux   => add_input_mux,
        delay           => delay,
        factor1         => factor1,
        factor2         => factor2,
        reset_debug     => '0',
        adc_data1_max   => open,
        adc_data2_max   => open,
        scaler_max      => open,
        dac_max         => open
    );

    clk_prc : process
    begin
        clk <= '1';
        wait for 25 ns;
        clk <= '0';
        wait for 25 ns;
    end process;
    
    process
    begin
        --INITIALISE SIGNALS
        reset   <= '1';
        wait until rising_edge(clk);
        wait until rising_edge(clk);
        reset   <= '0';
        report "Single Input, Signal 1 [MAX Amplitude, 10kHz], Positive Feedback, Scale 1";
        sig1            <= sine1;
        sig2            <= ZERO_SIGNAL;
        add_input_mux   <= '0';
        addsub_mode     <= '1';
        factor1         <= "10000"; --1.0
        factor2         <= "10000"; --1.0
        delay           <= std_logic_vector(to_unsigned(0,DELAY_WIDTH));
        wait_samples(sine1.len);
        report "Single Input, Signal 1 [MAX Amplitude, 10kHz], Negative Feedback, Scale 1";
        sig1            <= sine1;
        sig2            <= ZERO_SIGNAL;
        add_input_mux   <= '0';
        addsub_mode     <= '0';
        factor1         <= "10000"; --1.0
        factor2         <= "10000"; --1.0
        delay           <= std_logic_vector(to_unsigned(0,DELAY_WIDTH));
        wait_samples(sine1.len);
        report "Single Input, Signal 1 [DAC Adjusted MAX Amplitude, 10kHz], Positive Feedback, Scale 1.32";
        sig1            <= sine3;
        sig2            <= ZERO_SIGNAL;
        add_input_mux   <= '0';
        addsub_mode     <= '1';
        factor1         <= "10101"; --1.32
        factor2         <= "10000"; --1.0
        delay           <= std_logic_vector(to_unsigned(0,DELAY_WIDTH));
        wait_samples(sine3.len);
        report "Single Input, Signal 1 [MAX Amplitude, 10kHz], Positive Feedback, Scale 1.32";
        sig1            <= sine1;
        sig2            <= ZERO_SIGNAL;
        add_input_mux   <= '0';
        addsub_mode     <= '1';
        factor1         <= "10101"; --1.32
        factor2         <= "10000"; --1.0
        delay           <= std_logic_vector(to_unsigned(0,DELAY_WIDTH));
        wait_samples(sine1.len);
        report "Single Input, Signal 1 [MAX Amplitude, 10kHz], Positive Feedback, Scale 0.5";
        sig1            <= sine1;
        sig2            <= ZERO_SIGNAL;
        add_input_mux   <= '0';
        addsub_mode     <= '1';
        factor1         <= "01000"; --0.5
        factor2         <= "10000"; --1.0
        delay           <= std_logic_vector(to_unsigned(0,DELAY_WIDTH));
        wait_samples(sine1.len);
        report "Double Input, Signal 1 [HALF Amplitude, 10kHz], Signal 2 [Static HALF], Positive Feedback, Scale1 1, Scale2 1, Delay 0";
        sig1            <= sine2;
        sig2            <= HALF_SIGNAL;
        add_input_mux   <= '1';
        addsub_mode     <= '1';
        factor1         <= "10000"; --1.0
        factor2         <= "10000"; --1.0
        delay           <= std_logic_vector(to_unsigned(0,DELAY_WIDTH));
        wait_samples(sine2.len);
        report "Double Input, Signal 1 [HALF Amplitude, 10kHz], Signal 2 [HALF Amplitude, 10kHz], Positive Feedback, Scale1 1, Scale2 1, Delay 0";
        sig1            <= sine2;
        sig2            <= sine2;
        add_input_mux   <= '1';
        addsub_mode     <= '1';
        factor1         <= "10000"; --1.0
        factor2         <= "10000"; --1.0
        delay           <= std_logic_vector(to_unsigned(0,DELAY_WIDTH));
        wait_samples(sine2.len);
        report "Double Input, Signal 1 [MAX Amplitude, 10kHz], Signal 2 [HALF Amplitude, 10kHz], Positive Feedback, Scale1 1, Scale2 1, Delay 0";
        sig1            <= sine1;
        sig2            <= sine2;
        add_input_mux   <= '1';
        addsub_mode     <= '1';
        factor1         <= "10000"; --1.0
        factor2         <= "10000"; --1.0
        delay           <= std_logic_vector(to_unsigned(0,DELAY_WIDTH));
        wait_samples(sine1.len);
        report "Double Input, Signal 1 [MAX Amplitude, 10kHz], Signal 2 [MAX Amplitude, 10kHz], Positive Feedback, Scale1 0.75, Scale2 0.25, Delay 0";
        sig1            <= sine1;
        sig2            <= sine1;
        add_input_mux   <= '1';
        addsub_mode     <= '1';
        factor1         <= "01100"; --0.75
        factor2         <= "00100"; --0.25
        delay           <= std_logic_vector(to_unsigned(0,DELAY_WIDTH));
        wait_samples(sine1.len);
        report "Double Input, Signal 1 [HALF Amplitude, 10kHz], Signal 2 [HALF Amplitude, 10kHz], Negative Feedback, Scale1 1, Scale2 1, Delay 0";
        sig1            <= sine2;
        sig2            <= sine2;
        add_input_mux   <= '1';
        addsub_mode     <= '0';
        factor1         <= "10000"; --1.0
        factor2         <= "10000"; --1.0
        delay           <= std_logic_vector(to_unsigned(0,DELAY_WIDTH));
        wait_samples(sine2.len);
        report "Double Input, Signal 1 [MAX Amplitude, 10kHz], Signal 2 [HALF Amplitude, 10kHz], Negative Feedback, Scale1 1, Scale2 1, Delay 0";
        sig1            <= sine1;
        sig2            <= sine2;
        add_input_mux   <= '1';
        addsub_mode     <= '0';
        factor1         <= "10000"; --1.0
        factor2         <= "10000"; --1.0
        delay           <= std_logic_vector(to_unsigned(0,DELAY_WIDTH));
        wait_samples(sine1.len);
        report "Double Input, Signal 1 [Static HALF], Signal 2 [MAX Amplitude, 10kHz], Negative Feedback, Scale1 1, Scale2 1, Delay 0";
        sig1            <= HALF_SIGNAL;
        sig2            <= sine1;
        add_input_mux   <= '1';
        addsub_mode     <= '0';
        factor1         <= "10000"; --1.0
        factor2         <= "10000"; --1.0
        delay           <= std_logic_vector(to_unsigned(0,DELAY_WIDTH));
        wait_samples(sine1.len);
        report "Double Input, Signal 1 [MAX Amplitude, 10kHz], Signal 2 [HALF Amplitude, 10kHz], Negative Feedback, Scale1 1.32, Scale2 1, Delay 0";
        sig1            <= sine1;
        sig2            <= sine2;
        add_input_mux   <= '1';
        addsub_mode     <= '0';
        factor1         <= "10101"; --1.32
        factor2         <= "10000"; --1.0
        delay           <= std_logic_vector(to_unsigned(0,DELAY_WIDTH));
        wait_samples(sine1.len);
        report "Double Input, Signal 1 [MAX Amplitude, 10kHz], Signal 2 [MAX Amplitude, 10kHz], Positive Feedback, Scale1 1, Scale2 1, Delay 1008";
        sig1            <= sine1;
        sig2            <= sine1;
        add_input_mux   <= '1';
        addsub_mode     <= '1';
        factor1         <= "10000"; --1.0
        factor2         <= "10000"; --1.0
        delay           <= std_logic_vector(to_unsigned(1008,DELAY_WIDTH));
        wait_samples(sine1.len+56);
        report "Double Input, Signal 1 [HALF Amplitude, 10kHz], Signal 2 [MAX Amplitude, 10kHz], Negative Feedback, Scale1 1, Scale2 1, Delay 504";
        sig1            <= sine2;
        sig2            <= sine1;
        add_input_mux   <= '1';
        addsub_mode     <= '0';
        factor1         <= "10000"; --1.0
        factor2         <= "10000"; --1.0
        delay           <= std_logic_vector(to_unsigned(504,DELAY_WIDTH));
        wait_samples(sine1.len+28);
        wait;
    end process;
    
    adc_prc : process (all)
    begin
        if rising_edge(clk) then
            case (adc_stage) is
                when ZERO =>
                    if (adc_cs_n = '0') then
                        cnt1            <= cnt1 + 1;
                        if (cnt1 = 3) then
                            cnt1        <= 11;
                            adc_stage   <= DATA;
                        end if;
                    end if;
                when DATA =>
                    if (adc_cs_n = '0') then
                        cnt1    <= cnt1 - 1;
                        if (cnt1 = 0) then
                            -- Signal 1
                            if (cnt2 = sig1.len-1) then
                                cnt2    <= 0;
                            else
                                cnt2    <= cnt2 + 1;
                            end if;
                            -- Signal 2
                            if (cnt3 = sig2.len-1) then
                                cnt3    <= 0;
                            else
                                cnt3    <= cnt3 + 1;
                            end if;
                            cnt1        <= 0;
                            adc_stage   <= ZERO;
                        end if;
                    end if;
            end case;
        end if;
        
        case (adc_stage) is
            when ZERO =>
                adc_data_in1    <= '0';
                adc_data_in2    <= '0';
            when DATA =>
                adc_data_in1    <= sig1.sine(cnt2)(cnt1);
                adc_data_in2    <= sig2.sine(cnt3)(cnt1);
        end case;
    end process;
    
    io_prc : process (all)
        alias in_done is <<signal uut.adc_done : std_logic>>;
        alias in1 is <<signal uut.data1_A : std_logic_vector(ADC_DATA_WIDTH-1 downto 0)>>;
        alias in2 is <<signal uut.data2_A : std_logic_vector(ADC_DATA_WIDTH downto 0)>>;
        alias out1 is <<signal uut.data_out : std_logic_vector(DAC_DATA_WIDTH-1 downto 0)>>;
        alias out_done is <<signal uut.done_IV : std_logic>>;
    begin
        if rising_edge(clk) then
            -- Signle Input Mode
            if (add_input_mux = '0') then
                if (in_done = '1') then
                    input1  <= in1;
                    input2  <= in2(ADC_DATA_WIDTH-1 downto 0);
                end if;
            else
                if (in2(ADC_DATA_WIDTH) = '1') then
                    input1  <= in1;
                    input2  <= in2(ADC_DATA_WIDTH-1 downto 0);
                end if;
            end if;
            if (out_done = '1') then
                output  <= out1;
            end if;
        end if;
    end process;
    
end architecture;