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

library std;
use std.textio.all;

--------------------------------------------------------------------------------

entity mcc_master is
  generic (
    MCC_W     : integer := 5;
    MUX_W     : integer := 2;
    IRF_ADR_W : integer := 5);
  port (
    -- Primary slave intefrace
    ACK_O        : out std_logic := '0';
    CLK_I        : in  std_logic;
    RST_I        : in  std_logic;
    STB_I        : in  std_logic;
    -- Motion Control Chain
    MCC_STB_O    : out std_logic_vector (MCC_W-1 downto 0) := (others => '0');
    MCC_ACK_I    : in  std_logic_vector (MCC_W-1 downto 0);
    MCC_MUX_CODE : out std_logic_vector (MUX_W-1 downto 0) := (others => '0');
    MCC_MUX_EN   : out std_logic := '0';
    -- Shared dual-port memory
    IRF_ACK_I    : in  std_logic;
    IRF_ADR_O    : out std_logic_vector (IRF_ADR_W-1 downto 0);
    IRF_DAT_I    : in  std_logic_vector (15 downto 0);
    IRF_DAT_O    : out std_logic_vector (15 downto 0);
    IRF_STB_O    : out std_logic := '0';
    IRF_WE_O     : out std_logic);
end entity mcc_master;

--------------------------------------------------------------------------------

architecture behavioral of mcc_master is

  type state_t is (ready, read_mask, do_mcc, done);

  signal state : state_t := ready;

  signal mcc_mask       : std_logic_vector (MCC_W-1 downto 0);
  signal mcc_ack_inner  : std_logic_vector (MCC_W downto 0) := (others => '0');
  signal mcc_stb_inner  : std_logic_vector (MCC_W-1 downto 0) := (others => '0');
  signal mux_code_inner : std_logic_vector (MUX_W-1 downto 0);
  signal mcc_exec       : std_logic := '0';
  
--------------------------------------------------------------------------------

begin

  IRF_ADR_O <= conv_std_logic_vector(0, IRF_ADR_W);
  IRF_DAT_O <= (others => '-');
  IRF_WE_O  <= '0';


  priority_encoder_1: entity work.priority_encoder
    generic map (
      SEL_W  => MCC_W,
      CODE_W => MUX_W)
    port map (
      sel  => mcc_stb_inner,
      code => mux_code_inner);
  

  MCC_EXEC_LOGIC : process (RST_I, CLK_I) is
  begin
    if rising_edge(CLK_I) then
      if RST_I = '1' then
        mcc_ack_inner <= (others => '0');
        mcc_stb_inner <= (others => '0');

      else
        if mcc_exec = '0' then
          mcc_ack_inner <= (others => '0');
          mcc_stb_inner <= (others => '0');
          
        else
          mcc_ack_inner (0) <= mcc_exec;
          
          for i in 0 to MCC_W-1 loop
            if mcc_mask (i) = '1' then
              mcc_ack_inner (i+1) <= MCC_ACK_I (i);
              mcc_stb_inner (i)   <= mcc_ack_inner (i);
            else
              mcc_ack_inner (i+1) <= mcc_ack_inner (i);
              mcc_stb_inner (i)   <= '0';
            end if;
          end loop;
        end if;
      end if;
    end if;
  end process;

  
  LATCHES : process (RST_I, CLK_I) is
  begin
    if RST_I = '1' then
      MCC_STB_O    <= (others => '0');
      MCC_MUX_CODE <= (others => '0');

    elsif rising_edge(CLK_I) then
      MCC_STB_O    <= mcc_stb_inner;
      MCC_MUX_CODE <= mux_code_inner;
    end if;
  end process;

  
  FSM : process (CLK_I, RST_I) is
  begin
    if RST_I = '1' then
      state      <= ready;
      ACK_O      <= '0';
      mcc_exec   <= '0';
      MCC_MUX_EN <= '0';
      IRF_STB_O  <= '0';
      
    elsif rising_edge(CLK_I) then
      case state is
        when ready =>
          if STB_I = '1' then
            state     <= read_mask;
            IRF_STB_O <= '1';
          end if;

        when read_mask =>
          if IRF_ACK_I = '1' then
            state      <= do_mcc;
            mcc_mask   <= IRF_DAT_I (mcc_mask'RANGE);
            MCC_MUX_EN <= '1';
            mcc_exec   <= '1';
          end if;

        when do_mcc =>
          if mcc_ack_inner (MCC_W) = '1' then
            state      <= done;
            ACK_O      <= '1';
            IRF_STB_O  <= '0';
            MCC_MUX_EN <= '0';
            mcc_exec   <= '0';
          end if;

        when done =>
          if STB_I = '0' then
            state <= ready;
            ACK_O <= '0';
          end if;
      end case;      
    end if;
  end process;
  
end architecture behavioral;