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

library desy;
use desy.math_signed.all;

use work.pkg_corr_matrixpi.all;

entity corr_pi is
    port(
        clk             : in std_logic;
        rst_n           : in std_logic;

        -- matmult input
        matmult         : in signed_array(0 to C_N_MM_PSC-1)(C_W_MM-1 downto 0);
        matmult_valid   : in std_logic;
        matmult_seq     : in std_logic_vector(C_W_BPMSEQ-1 downto 0);

        -- Corr coefs
        corr_kp         : std_logic_vector(C_W_COR_KP-1 downto 0);
        corr_ki         : std_logic_vector(C_W_COR_KI-1 downto 0);

        reset_accu      : in std_logic;
        enable_accu     : in std_logic;
        enable_corr     : in std_logic;

        -- Corr output
        corrout_valid   : out std_logic;
        corrout_seq     : out std_logic_vector(C_W_BPMSEQ-1 downto 0);
        corrout         : out signed_array(0 to C_N_MM_PSC-1)(C_W_COR-1 downto 0)
    );
end entity corr_pi;


architecture rtl of corr_pi is


    type arr_slv is array (natural range <>) of std_logic_vector;
    ------------------------
    -- SIGNAL DECLARATION --
    ------------------------
    signal r_valid     : std_logic_vector(2 downto 0);
    signal r_seq      : arr_slv(0 to 2)(C_W_BPMSEQ-1 downto 0);


begin


    --------------------
    -- DELAY REGISTER --
    --------------------
    p_delay:process(clk,rst_n)
    begin
        if rst_n = '0' then
            r_valid <= (others => '0');
            r_seq         <= (others => (others => '0'));

        elsif rising_edge(clk) then
            r_valid <= r_valid(r_valid'left-1 downto 0) & matmult_valid;
            r_seq(0)        <= matmult_seq;
            for I in 1 to r_seq'right loop
                r_seq(I) <= r_seq(I-1);
            end loop;
        end if;
    end process;


    ----------------------
    -- OUTPUT CONNEXION --
    ----------------------
    corrout_valid   <= r_valid(2);
    corrout_seq     <= r_seq(2);

    --------------------
    -- CORRECTOR LINE --
    --------------------
    G_CORR:for I in 0 to C_N_MM_PSC-1 generate

        signal r_matmult    : signed(C_W_MM-1 downto 0);
        signal accu_pre     : signed(C_W_MM-1 downto 0);
        signal accu_post    : signed(C_W_COR_ACCU-1 downto 0);
        signal ki_mult      : signed(C_W_COR_MI-1 downto 0);
        signal kp_mult      : signed(C_W_COR_MP-1 downto 0);
        signal ki_mult_rnd  : signed(C_W_COR_MI-C_N_COR_MIRND-1 downto 0);
        signal corr_sum     : signed(C_W_COR_SUM-1 downto 0);
        signal corr_sum_rnd : signed(C_W_COR_SUM-C_N_COR_RND-1 downto 0);
        signal corr         : signed(C_W_COR-1 downto 0);

    begin

        p_acc:process(clk, rst_n)
        begin
            if rst_n = '0' then
                accu_pre      <= (others => '0');
                accu_post     <= (others => '0');
                ki_mult       <= (others => '0');
                kp_mult       <= (others => '0');
                corr          <= (others => '0');
            elsif rising_edge(clk) then

                -----------------
                -- ACCUMULATOR --
                -----------------
                -- This is a bilinear integrator !
                if matmult_valid = '1' then
                    accu_pre  <= matmult(I);
                end if;
                if reset_accu = '1' then
                    accu_post   <= (others => '0');
                else
                    if enable_accu = '1' and matmult_valid = '1' then
                        accu_post <= f_sum_sat(f_sum_sat(accu_post, matmult(I)), accu_pre);
                    end if;
                end if;

                -- Delay
                r_matmult  <= matmult(I);

                ----------------------
                -- COEFF MULTIPLIER --
                ----------------------
                ki_mult <= accu_post * signed('0'&corr_ki);
                kp_mult <= r_matmult * signed('0'&corr_kp);

                ----------------------
                -- FINAL SATURATION --
                ----------------------
                if enable_corr = '1' then
                    corr <= f_resize_sat(corr_sum_rnd, C_W_COR);
                else
                    corr <= (others => '0');
                end if;


            end if;
        end process;

        ----------------------
        -- KI MULT ROUNDING --
        ----------------------
        ki_mult_rnd <= f_resize_lsb(ki_mult, C_W_COR_MI-C_N_COR_MIRND);
        --ki_mult_rnd <= ki_mult(C_W_COR_MI-1 downto C_N_COR_MIRND) when ki_mult(C_N_COR_MIRND-1) = '1' else
        --               f_sum_sat(ki_mult(C_W_COR_MI-1 downto C_N_COR_KIRND), to_signed(1,1));


        -----------------------
        -- FINAL ADDER LOGIC --
        -----------------------
        corr_sum <= f_sum_sat(ki_mult_rnd, kp_mult);

        --------------------
        -- FINAL ROUNDING --
        --------------------
        corr_sum_rnd    <= f_resize_lsb(corr_sum, C_W_COR_SUM-C_N_COR_RND) when corr_sum(C_N_COR_RND-1) = '0' else
                           f_sum_sat(f_resize_lsb(corr_sum, C_W_COR_SUM-C_N_COR_RND), to_signed(1,C_W_COR_SUM-C_N_COR_RND));

        -------------
        -- MAPPING --
        -------------
        corrout(I) <= corr;

    end generate;


end architecture;