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

entity Current_controller_v1_0 is
	generic (
		-- Users to add parameters here
        Fvz : integer := 20;
        Kp_d : integer := 1000;
        Ki_d : integer := 1000;
        Kp_q : integer := 1000;
        Ki_q : integer := 1000;
        Ext_par : integer := 0;
		-- User parameters ends
		-- Do not modify the parameters beyond this line


		-- Parameters of Axi Slave Bus Interface S00_AXI
		C_S00_AXI_DATA_WIDTH	: integer	:= 32;
		C_S00_AXI_ADDR_WIDTH	: integer	:= 4
	);
	port (
		-- Users to add ports here
        run : in std_logic;
        clk : in std_logic;
        sync_in : in std_logic;
        w_d : in std_logic_vector(15 downto 0);
        w_q : in std_logic_vector(15 downto 0);
        y_d : in std_logic_vector(15 downto 0);
        y_q : in std_logic_vector(15 downto 0);
        x_d : out std_logic_vector(15 downto 0);
        x_q : out std_logic_vector(15 downto 0);
        Kp_d_ext : in std_logic_vector (31 downto 0);
        Ki_d_ext : in std_logic_vector (31 downto 0);
        Kp_q_ext : in std_logic_vector (31 downto 0);
        Ki_q_ext : in std_logic_vector (31 downto 0);
        sync_out : out std_logic;
		-- User ports ends
		-- Do not modify the ports beyond this line


		-- Ports of Axi Slave Bus Interface S00_AXI
		s00_axi_aclk	: in std_logic;
		s00_axi_aresetn	: in std_logic;
		s00_axi_awaddr	: in std_logic_vector(C_S00_AXI_ADDR_WIDTH-1 downto 0);
		s00_axi_awprot	: in std_logic_vector(2 downto 0);
		s00_axi_awvalid	: in std_logic;
		s00_axi_awready	: out std_logic;
		s00_axi_wdata	: in std_logic_vector(C_S00_AXI_DATA_WIDTH-1 downto 0);
		s00_axi_wstrb	: in std_logic_vector((C_S00_AXI_DATA_WIDTH/8)-1 downto 0);
		s00_axi_wvalid	: in std_logic;
		s00_axi_wready	: out std_logic;
		s00_axi_bresp	: out std_logic_vector(1 downto 0);
		s00_axi_bvalid	: out std_logic;
		s00_axi_bready	: in std_logic;
		s00_axi_araddr	: in std_logic_vector(C_S00_AXI_ADDR_WIDTH-1 downto 0);
		s00_axi_arprot	: in std_logic_vector(2 downto 0);
		s00_axi_arvalid	: in std_logic;
		s00_axi_arready	: out std_logic;
		s00_axi_rdata	: out std_logic_vector(C_S00_AXI_DATA_WIDTH-1 downto 0);
		s00_axi_rresp	: out std_logic_vector(1 downto 0);
		s00_axi_rvalid	: out std_logic;
		s00_axi_rready	: in std_logic
	);
end Current_controller_v1_0;

architecture arch_imp of Current_controller_v1_0 is

    signal w_d_signal : signed (15 downto 0);
    signal y_d_signal : signed (15 downto 0);
    signal x_d_signal : signed (15 downto 0);
    signal Kp_d_signal : signed (31 downto 0);
    signal Ki_d_signal : signed (31 downto 0);
    
    signal w_q_signal : signed (15 downto 0);
    signal y_q_signal : signed (15 downto 0);
    signal x_q_signal : signed (15 downto 0);
    signal Kp_q_signal : signed (31 downto 0);
    signal Ki_q_signal : signed (31 downto 0);
    
    signal done_d : std_logic;
    signal done_q : std_logic;
    signal done_sqrt : std_logic;
    
    signal limit_q : signed (15 downto 0);
    signal limit_q_squared : signed (31 downto 0);
    
    signal AXI_DATA0 : std_logic_vector (C_S00_AXI_DATA_WIDTH-1 downto 0);
    signal AXI_DATA1 : std_logic_vector (C_S00_AXI_DATA_WIDTH-1 downto 0);

	-- component declaration
	component Current_controller_v1_0_S00_AXI is
		generic (
		C_S_AXI_DATA_WIDTH	: integer	:= 32;
		C_S_AXI_ADDR_WIDTH	: integer	:= 4
		);
		port (
		AXI_DATA0 : in std_logic_vector (C_S_AXI_DATA_WIDTH-1 downto 0);
        AXI_DATA1 : in std_logic_vector (C_S_AXI_DATA_WIDTH-1 downto 0);
		S_AXI_ACLK	: in std_logic;
		S_AXI_ARESETN	: in std_logic;
		S_AXI_AWADDR	: in std_logic_vector(C_S_AXI_ADDR_WIDTH-1 downto 0);
		S_AXI_AWPROT	: in std_logic_vector(2 downto 0);
		S_AXI_AWVALID	: in std_logic;
		S_AXI_AWREADY	: out std_logic;
		S_AXI_WDATA	: in std_logic_vector(C_S_AXI_DATA_WIDTH-1 downto 0);
		S_AXI_WSTRB	: in std_logic_vector((C_S_AXI_DATA_WIDTH/8)-1 downto 0);
		S_AXI_WVALID	: in std_logic;
		S_AXI_WREADY	: out std_logic;
		S_AXI_BRESP	: out std_logic_vector(1 downto 0);
		S_AXI_BVALID	: out std_logic;
		S_AXI_BREADY	: in std_logic;
		S_AXI_ARADDR	: in std_logic_vector(C_S_AXI_ADDR_WIDTH-1 downto 0);
		S_AXI_ARPROT	: in std_logic_vector(2 downto 0);
		S_AXI_ARVALID	: in std_logic;
		S_AXI_ARREADY	: out std_logic;
		S_AXI_RDATA	: out std_logic_vector(C_S_AXI_DATA_WIDTH-1 downto 0);
		S_AXI_RRESP	: out std_logic_vector(1 downto 0);
		S_AXI_RVALID	: out std_logic;
		S_AXI_RREADY	: in std_logic
		);
	end component Current_controller_v1_0_S00_AXI;
	
	component sqrt is
        port(
            sync_in : in std_logic;
            clk : in std_logic;
            number : in signed (31 downto 0);
            sqrt : out signed (15 downto 0);
            sync_out : out std_logic);
    end component;
    
    component 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 component;

begin

-- Instantiation of Axi Bus Interface S00_AXI
Current_controller_v1_0_S00_AXI_inst : Current_controller_v1_0_S00_AXI
	generic map (
		C_S_AXI_DATA_WIDTH	=> C_S00_AXI_DATA_WIDTH,
		C_S_AXI_ADDR_WIDTH	=> C_S00_AXI_ADDR_WIDTH
	)
	port map (
	    AXI_DATA0 => AXI_DATA0,
        AXI_DATA1 => AXI_DATA1,
		S_AXI_ACLK	=> s00_axi_aclk,
		S_AXI_ARESETN	=> s00_axi_aresetn,
		S_AXI_AWADDR	=> s00_axi_awaddr,
		S_AXI_AWPROT	=> s00_axi_awprot,
		S_AXI_AWVALID	=> s00_axi_awvalid,
		S_AXI_AWREADY	=> s00_axi_awready,
		S_AXI_WDATA	=> s00_axi_wdata,
		S_AXI_WSTRB	=> s00_axi_wstrb,
		S_AXI_WVALID	=> s00_axi_wvalid,
		S_AXI_WREADY	=> s00_axi_wready,
		S_AXI_BRESP	=> s00_axi_bresp,
		S_AXI_BVALID	=> s00_axi_bvalid,
		S_AXI_BREADY	=> s00_axi_bready,
		S_AXI_ARADDR	=> s00_axi_araddr,
		S_AXI_ARPROT	=> s00_axi_arprot,
		S_AXI_ARVALID	=> s00_axi_arvalid,
		S_AXI_ARREADY	=> s00_axi_arready,
		S_AXI_RDATA	=> s00_axi_rdata,
		S_AXI_RRESP	=> s00_axi_rresp,
		S_AXI_RVALID	=> s00_axi_rvalid,
		S_AXI_RREADY	=> s00_axi_rready
	);

	-- Add user logic here
	
	sqrt_inst : sqrt
    port map (
        sync_in => done_d,
        clk => clk,
        number => limit_q_squared,
        sqrt => limit_q,
        sync_out => done_sqrt
    );
        
    PI_inst_d : PI_core
     generic map(
       Fvz => Fvz
     )
       port map(
       run => run,
       clk => clk,
       sync_in => sync_in,
       lim => x"7FFF",
       Kp => Kp_d_signal,
       Ki => Ki_d_signal,
       w => w_d_signal,
       y => y_d_signal,
       x => x_d_signal,
       sync_out => done_d  
       );
       
    PI_inst_q : PI_core
     generic map(
       Fvz => Fvz
     )
       port map(
       run => run,
       clk => clk,
       sync_in => done_sqrt,
       lim => limit_q,
       Kp => Kp_q_signal,
       Ki => Ki_q_signal,
       w => w_q_signal,
       y => y_q_signal,
       x => x_q_signal,
       sync_out => done_q  
       );       
	
	Kp_d_signal <= signed(Kp_d_ext) when Ext_par=1 else to_signed(Kp_d,32);
	Ki_d_signal <= signed(Ki_d_ext) when Ext_par=1 else to_signed(Ki_d,32);
	Kp_q_signal <= signed(Kp_q_ext) when Ext_par=1 else to_signed(Kp_q,32);
	Ki_q_signal <= signed(Ki_q_ext) when Ext_par=1 else to_signed(Ki_q,32);
	
	w_d_signal <= signed(w_d);
    y_d_signal <= signed(y_d);
    w_q_signal <= signed(w_q);
    y_q_signal <= signed(y_q);
    limit_q_squared <= x"3FFF0001" - x_d_signal * x_d_signal;
    
    process (clk) begin
      if (clk'event and clk='1') then
        if done_q='1' then
            AXI_DATA0 <= std_logic_vector(resize(x_d_signal,C_S00_AXI_DATA_WIDTH));
            AXI_DATA1 <= std_logic_vector(resize(x_q_signal,C_S00_AXI_DATA_WIDTH));
            x_d <= std_logic_vector(x_d_signal);
            x_q <= std_logic_vector(x_q_signal);
            sync_out <= '1';
        else
            sync_out <= '0';
        end if; 
      end if;
    end process;
	-- User logic ends

end arch_imp;
