library IEEE;
use IEEE.std_logic_1164.all;
use IEEE.std_logic_arith.all;
use IEEE.std_logic_unsigned.all;

entity FDBK_LOOP is
  port (
    	M        : in SIGNED (11 downto 0);
    	SET_I    : in SIGNED (13 downto 0);
    	SET_Q    : in SIGNED (13 downto 0);
    	dynamic_setpoint : in STD_LOGIC;
    	KCM_load_data : in SIGNED (11 downto 0);
    	KCM_load_real : in STD_LOGIC;
    	KCM_load_imag : in STD_LOGIC;
    	KCM_load_intg : in STD_LOGIC;
    	KCM_load_rise : in STD_LOGIC;
    	KCM_load_busy : out STD_LOGIC;
    	t_mod_4  : in STD_LOGIC_VECTOR (1 downto 0);
    	feedback_enable  : in STD_LOGIC;
    	integrate_enable: in STD_LOGIC;
    	feedforward_data: in  STD_LOGIC_VECTOR(7 downto 0);
    	RESET    : in STD_LOGIC;
    	CLK      : in STD_LOGIC;
    	FDBK_ERR : out SIGNED (11 downto 0)    
       );
end FDBK_LOOP;

architecture FDBK_LOOP_arch of FDBK_LOOP is
-- component declarations

component ERROR3 is
    port (
        SATURATED : in SIGNED (9 downto 0);
        KCM_load_data : in SIGNED (11 downto 0);
        KCM_load_real : in STD_LOGIC;
        KCM_load_imag : in STD_LOGIC;
        KCM_load_intg : in STD_LOGIC;
        KCM_load_busy : out STD_LOGIC;
        t_mod_4	 : in STD_LOGIC_VECTOR (1 downto 0);
        FDBK_EN   : in STD_LOGIC;   
        RESET     : in STD_LOGIC;
        CLK       : in STD_LOGIC;
        ERR3      : out SIGNED (11 downto 0);
        ERR4      : out SIGNED (11 downto 0)
    );
end component;
-- end of component declarations

-- internal signal declarations
SIGNAL AVG_M     : SIGNED (13 downto 0);
signal ACCUM     : SIGNED (13 downto 0);
signal iqset     : SIGNED (13 downto 0);
signal iqiqset   : SIGNED (13 downto 0);
signal set_wave  : SIGNED (13 downto 0);
signal target    : SIGNED (14 downto 0);
SIGNAL iERROR    : SIGNED (15 downto 0);
SIGNAL ERROR_sat : SIGNED (9 downto 0);
SIGNAL ERROR_sat_latched: SIGNED (9 downto 0);
SIGNAL PROP_ERR: SIGNED (11 downto 0);
SIGNAL scaled_PROP_ERR: SIGNED (11 downto 0);
signal integrate_input: SIGNED (12 downto 0);
signal integrate_sum:   SIGNED (16 downto 0);
signal integrate_out:   SIGNED (15 downto 0);
signal integrate_sat:   SIGNED (15 downto 0);
signal fdbk_err_wide:   SIGNED (12 downto 0);

-- design_top_arch begins
begin
-- component instantiations

u4: ERROR3 port map (
		 SATURATED => ERROR_sat_latched,
 		 KCM_load_data => KCM_load_data,
 	   	 KCM_load_real => KCM_load_real,
 	   	 KCM_load_imag => KCM_load_imag,
 	   	 KCM_load_intg => KCM_load_intg,
 	   	 KCM_load_busy => KCM_load_busy,
		 t_mod_4   => t_mod_4,
		 FDBK_EN   => feedback_enable,
		 RESET     => RESET,
		 CLK       => CLK,
		 ERR3      => PROP_ERR,
		 ERR4      => scaled_PROP_ERR
		);

four_cycle_accum: process (CLK) begin
    if (CLK'event and CLK = '1') then
        if (t_mod_4 = "00") then
            AVG_M <= ACCUM;
            ACCUM <= M(11) & M(11) & M;
        else
            ACCUM <= ACCUM + (M(11) & M(11) & M);
        end if;
    end if;
end process four_cycle_accum;

iqset  <= SET_I when t_mod_4(0) = '1' else SET_Q;
iqiqset <= iqset when t_mod_4(1) = '1' else -iqset;
target <= (AVG_M(13) & AVG_M) + (set_wave(13) & set_wave);
iERROR <= (M(11) & M(11) & M & "00") - (target(14) & target);

general: process (CLK) begin
    if (CLK'event and CLK = '1') then
        set_wave <= iqiqset;
        ERROR_sat_latched <= ERROR_sat;
    end if;
end process general;

saturate: process (iERROR) begin
    if ((iERROR(15 downto 9)=0        ) or (iERROR(15 downto 9)=-1       )) then
--  if ((iERROR(15 downto 9)="0000000") or (iERROR(15 downto 9)="1111111")) then
        ERROR_sat <= iERROR(9 downto 0);
    elsif (iERROR(15)='1') then
        ERROR_sat <= "1000000000";
    else
        ERROR_sat <= "0111111111";
    end if;
end process saturate;

integrate_saturation: process (integrate_sum) begin
    if ((integrate_sum(16 downto 15)=0) or (integrate_sum(16 downto 15)=-1)) then
        integrate_sat <= integrate_sum(15 downto 0);
    elsif (integrate_sum(16)='1') then
        integrate_sat <= "1000000000000000";
    else
        integrate_sat <= "0111111111111111";
    end if;
end process integrate_saturation;

-- Integrator works on interleaved I and Q.
integrator: process (CLK) begin
    if (CLK'event and CLK = '1') then
	if (integrate_enable = '0') then
	    integrate_out <= (others => '0');
	else
	    integrate_out <= integrate_sat;
	end if;
	-- to avoid wrap-around, arguments are resized 1 bit larger (sign extended) 
	-- before addition or subtraction is performed      
	integrate_sum <= (integrate_out(15) & integrate_out)
		       + (integrate_input(12) & integrate_input(12) & integrate_input(12)
		        & integrate_input(12) & integrate_input);
	integrate_input <= SIGNED(feedforward_data(7) & feedforward_data(7) & feedforward_data & "000")
			 + (scaled_PROP_ERR(11) & scaled_PROP_ERR);
    end if;
end process integrator;

-- Combine low-latency linear feedback with the integration results
-- This doesn't saturate or deal with the carry yet.
fdbk_err_wide <= (integrate_out(15) & integrate_out(15 downto 4)) + (PROP_ERR(11) & PROP_ERR);
output_sat: process (fdbk_err_wide) begin
    if ((fdbk_err_wide(12 downto 11)=0) or (fdbk_err_wide(12 downto 11)=-1)) then
        fdbk_err <= fdbk_err_wide(11 downto 0);
    elsif (fdbk_err_wide(12)='1') then
        fdbk_err <= "100000000000";
    else
        fdbk_err <= "011111111111";
    end if;
end process output_sat;

end FDBK_LOOP_arch;