library ieee;
use ieee.std_logic_1164.all;
USE IEEE.numeric_std.ALL;

entity PI_core is
  generic(
    Fvz : integer := 20
  );
	port (
    run : in std_logic;
    clk : in std_logic;
    sync_in : in std_logic;
    lim : in signed (15 downto 0);
    Kp : signed (31 downto 0);
    Ki : signed (31 downto 0);
    w : in signed(15 downto 0);
    y : in signed(15 downto 0);
    x : out signed(15 downto 0);
    sync_out : out std_logic  
	);
end PI_core;

architecture Behavioral of PI_core is
   
   signal sync : std_logic; 
    
   signal e_signed : signed (15 downto 0);
   signal Kp_norm : signed (23 downto 0);
   signal Ki_norm : signed (23 downto 0);
   
   signal limit : signed (31 downto 0);
   
   signal prop : signed (39 downto 0);
   signal prop_limited : signed (31 downto 0);
   
   signal inc : signed (39 downto 0);
   signal inc_limited : signed (31 downto 0);
   
   signal sum_signal : signed (31 downto 0);
   
   signal x_large : signed (32 downto 0);
   signal x_limited : signed (15 downto 0);    
  
begin
    
    Kp_norm <= resize(Kp*to_signed(65,8),24);
    Ki_norm <= resize(Ki/(to_signed(Fvz*15,12)),24);
    e_signed <= w - y;
    limit <= signed(std_logic_vector(lim) & x"FFFF");
    
    process (clk) begin
      if (clk'event and clk='1') then
        
        prop <= e_signed * Kp_norm;
        if prop > resize(limit,40) then
           prop_limited <= limit;
        elsif prop < resize(-limit,40) then
           prop_limited <= -limit;
        else
           prop_limited <= resize(prop,32);    
        end if;   
      end if;
    end process;
    
    process (clk) begin
      if (clk'event and clk='1') then
        inc <= e_signed * Ki_norm;
        if inc > resize(limit,40) then
           inc_limited <= limit;
        elsif inc < resize(-limit,40) then
           inc_limited <= -limit;
        else
           inc_limited <= resize(inc,32);    
        end if;   
      end if;
    end process;
    
    process (clk) 
        variable sum : signed (31 downto 0); 
    begin       
      if (clk'event and clk='1') then
        if (run='0') then
          sum := (others => '0');   
        elsif (sync_in='1') then
          if sum >= to_signed(0,32) then
            if inc_limited < limit - sum then              
              sum := sum + inc_limited;
            else
              sum := limit;
            end if;
          else
            if inc_limited > -limit - sum then              
              sum := sum + inc_limited;
            else
              sum := -limit;
            end if;    
          end if;
          sync <= '1';
          sum_signal <= sum;
        else
          sync <= '0';    
        end if;
      end if;
    end process;   
    
    process (clk) begin
      if (clk'event and clk='1') then
        if (sync='1') then
            x_large <= resize(prop_limited,33) + resize(sum_signal,33);
            if x_large > limit then
               x_limited <= limit(31 downto 16);
            elsif x_large < -limit then
               x_limited <= -limit(31 downto 16);
            else
               x_limited(15) <= x_large(32);
               x_limited(14 downto 0) <= x_large(30 downto 16);   
            end if;
            sync_out <= '1';
        else
            sync_out <= '0';  
        end if;         
      end if;
    end process;
    
    x <= x_limited;
  
end Behavioral;
