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

library desy;
use desy.math_signed;

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_ACCU-1 downto 0);
        matmult_valid   : in std_logic;
        matmult_seq     : in std_logic_vector(C_W_SEQ-1 downto 0);

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

        reset_accu      : in std_logic;
        enable_corr     : in std_logic;

        -- Corr output
        corrout_valid   : out std_logic;
        corrout_seq     : out std_logic_vector(C_W_SEQ-1 downto 0);
        corrout         : out signed_array(0 to C_N_MM_PSC-1)(C_W_COR_OUT-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 downto 2)(C_W_SEQ-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)        <= matmul_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_ACCU-1 downto 0);
        signal accu_pre     : signed(C_W_MM_ACCU-1 downto 0);
        signal accu_post    : signed(C_W_COR_ACCU-1 downto 0);
        signal ki_mult      : signed(C_W_COR_KI+C_W_COR_ACCU-1 downto 0);
        signal kp_mult      : signed(C_W_COR_KP+C_W_COR_ACCU-1 downto 0);
        signal ki_mult_rnd  : signed(ki_mult'left-C_N_COR_KIRND downto 0);
        signal corr_sum     : signed(ki_mult_rnd'left downto 0);
        signal corr_sum_rnd : signed(C_W_COR_OUT-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');
                corrout       <= (others => (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 rst_accu = '1' then
                    accu_post   <= (others => '0');
                else
                    if ena_accu = '1' then
                        accu_post <= f_sum_sat(f_sum_sat(accu_post, matmult), accu_pre);
                    end if;
                end if;

                -- Delay
                r_matmult  <= matmult(I);

                ----------------------
                -- COEFF MULTIPLIER --
                ----------------------
                ki_mult <= accu_post * corr_ki;
                kp_mult <= r_matmult * corr_kp;

                --------------------------
                -- FINAL ADDER REGISTER --
                --------------------------
                corrout(I) <= f_resize_sat(ki_mult_rnd, C_W_COR_OUT);


            end if;
        end process;

        ----------------------
        -- KI MULT ROUNDING --
        ----------------------
        ki_mult_rnd <= ki_mult(ki_mult'left downto C_N_COR_KIRND) when ki_mult(C_N_COR_KIRND-1) = '1' else
                       f_sum_sat(ki_mult(ki_mult'left 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    <= corr_sum(corr_sum'left downto C_N_COR_RND) when corr_sum(C_N_COR_RND-1) = '1' else
                           f_sum_sat(corr_sum(corr_sum'left downto C_N_COR_RND), to_signed(1,1));


    end generate;




end architecture;