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

entity 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         : in UNSIGNED (15 downto 0);
       t_mod_4	 : in STD_LOGIC_VECTOR (1 downto 0);
       FDBK_EN   : in STD_LOGIC;   -- feedback enable
       RESET     : in STD_LOGIC;
       CLK       : in STD_LOGIC;
     --XOR_SIGN  : in STD_LOGIC;
       ERR3      : out SIGNED (11 downto 0);
       ERR4      : out SIGNED (11 downto 0)
       );
end ERROR3; 

architecture ERROR3_arch of ERROR3 is
-- components declaration
component KCM is
  port (
       VAR     : in SIGNED (9 downto 0);
       CONST_K : in SIGNED (11 downto 0);    -- signed
       LOAD_K  : in STD_LOGIC;	             -- load constant, starts 16 cycle action
       CLK     : in STD_LOGIC;
       BUSY    : out STD_LOGIC;
       PRODUCT : out SIGNED (21 downto 0)
       );
end component;
component llrf_kcm is
  port (
       dataa   : in SIGNED (9 downto 0);
       signeda : in std_logic;
       datab   : in SIGNED (11 downto 0);    -- signed
       loadb   : in STD_LOGIC;	             -- load constant, starts 16 cycle action
       c       : in STD_LOGIC;
       busy    : out STD_LOGIC;
       product : out SIGNED (21 downto 0)
       );
end component;

-- internal signals declaration
signal high    : std_logic;
signal ERR1    : SIGNED (9 downto 0);  	-- registered saturated error
signal ERR2    : SIGNED (9 downto 0);  	-- delay version of ERR1
signal ERR3_loc   : SIGNED (12 downto 0);  -- proportional error term, before passing to port
--signal ERR3_trunc : SIGNED (9 downto 0);
signal PROD1   : SIGNED (21 downto 0); 	-- internal signals to avoid reading
signal PROD2   : SIGNED (21 downto 0); 	-- directly from the out port PRODUCT
signal PROD3   : SIGNED (21 downto 0);
signal SUM     : SIGNED (12 downto 0);
signal ADD1    : SIGNED (11 downto 0);  -- inputs to 
signal ADD2    : SIGNED (11 downto 0);  -- the adder
signal XOR_SIGN: STD_LOGIC;		-- to adjust the sign of feedback output
signal BUSY_real : STD_LOGIC;
signal BUSY_imag : STD_LOGIC;
signal BUSY_intg : STD_LOGIC;
signal KCM_load_real_sync, KCM_load_imag_sync, KCM_load_intg_sync: std_logic;

begin
  high <= '1';
--  REG1: FD10R port map (			-- 10-bits D register with synchronous RESET
--                  	D => SATURATED,
--                  	R => RESET,
--                  	C => CLK,
--                  	Q => ERR1
--                       );
--  REG2: FD10R port map (
--                        D => ERR1,
--                        R => RESET,
--                        C => CLK,
--                        Q => ERR2
--                       );

  pipeline: process (CLK) begin
    if (CLK'event and CLK = '1') then
        ERR1 <= SATURATED;
        ERR2 <= ERR1;
    end if;
  end process pipeline;
  
  MUL1: KCM port map (				-- loadable Constant Coefficient Multiplier (KCM)
                      VAR     => ERR1,
                      CONST_K => KCM_load_data,
                      LOAD_K  => KCM_load_real_sync,
                      CLK     => CLK,
                      BUSY    => BUSY_real,
                      PRODUCT => PROD1
                     );                       
  MUL2: KCM port map (
                      VAR     => ERR2,
                      CONST_K => KCM_load_data,
                      LOAD_K  => KCM_load_imag_sync,
                      CLK     => CLK,
                      BUSY    => BUSY_imag,
                      PRODUCT => PROD2
                     );
  MUL3: KCM port map (
		      VAR     => ERR3_loc(12 downto 3),
		      CONST_K => KCM_load_data,
		      LOAD_K  => KCM_load_intg_sync,
		      CLK     => CLK,
		      BUSY    => BUSY_intg,
		      PRODUCT => PROD3
		     );

  -- feedback output sign  
  -- I at input when t_mod_4 = "00", Q when "01", -I when "10", -Q when "11"
  XOR_SIGN <= t_mod_4(1);
  
  -- to avoid wrap-around after addition, arguments are resized one bit larger before addition is performed.
  SUM <= (ADD1(11) & ADD1) + (ADD2(11) & ADD2);

  -- we want the feedback output to be I, Q, I, Q, I, Q, ... 
  fdbk_out: process (CLK) begin
    if (CLK'event and CLK = '1') then
      KCM_load_real_sync <= KCM_load_real;
      KCM_load_imag_sync <= KCM_load_imag;
      KCM_load_intg_sync <= KCM_load_intg;
      if (FDBK_EN = '1') then	   	-- t >= initial ramp time
        if (XOR_SIGN = '1') then  	-- when both M and SETP has -I    or   both M and SETP has -Q
          ERR3_loc <= -SUM;			-- flip the sign
        else 
          ERR3_loc <= SUM;
        end if;
      else
        ERR3_loc <= (others => '0');		-- t < intial ramp time ... feedback not enabled 
      end if;
      -- slice and register upper 12 bits from KCM output
      ADD1 <= PROD1(21 downto 10);
      ADD2 <= PROD2(21 downto 10); 
      ERR4 <= PROD3(21 downto 10);
    end if;
  end process fdbk_out;

  ERR3 <= ERR3_loc(12 downto 1);
  --ERR3_trunc <= ERR3_loc(11 downto 2);      -- 10 bit input to KCM. Could we round instead of truncate?
  KCM_load_busy <= BUSY_real or BUSY_imag or BUSY_intg;
    
end ERROR3_arch;