library IEEE;
use IEEE.STD_LOGIC_1164.ALL;
--use IEEE.STD_LOGIC_ARITH.ALL;
use IEEE.STD_LOGIC_UNSIGNED.ALL;
use IEEE.numeric_std.all;
use work.mypackage.all;

entity ESSController is
  Generic
    (
      num_states  : integer;
      num_inputs  : integer := 1;
      num_outputs : integer := 1;
      as1         : integer; -- == 2 ^ (width - 1) / as
      cs1         : integer; -- == 2 ^ (width - 1) / cs
      A           : matrix32(num_states - 1 downto 0, num_states - 1 downto 0);
      B           : matrix32(num_states - 1 downto 0, num_inputs - 1 downto 0);
      C           : matrix32(num_outputs - 1 downto 0, num_states - 1 downto 0);
      D           : matrix32(num_outputs - 1 downto 0, num_inputs - 1 downto 0);
      width       : integer := 16 -- WARNING: don't change this
    );
  Port
    (
      inputs      : vector32(num_inputs - 1 downto 0);
      outputs     : vector32(num_outputs - 1 downto 0);
      clk         : std_logic;
    );
end ESSController;

architecture SSController of ESSController is
  states      : matrix32(num_states - 1 downto 0);
begin

  process (clk)
    variable initialized : std_logic := '0';
    variable accum : integer;
  begin

    if clk'event and clk = '1' then

      if initialized = '0' then

        for i in num_states - 1 downto 0 loop
          states(i) <= 0;
        end loop;

        initialized := '1';

      else

        accum := 0;
        for i in num_states - 1 downto 0 loop
          for j in num_states - 1 downto 0 loop
            accum := accum + A(i, j) * states(j);
          end loop;
          for j in num_inptus - 1 downto 0 loop
            accum := accum + B(i, j) * inputs(j);
          end loop;
          accum := accum sra (width - 1);
          accum := (accum * as1) sra (width - 1);
          states(i) <= accum;
        end loop;

        accum := 0;
        for i in num_outputs - 1 downto 0 loop
          for j in num_states - 1 downto 0 loop
            accum := accum + C(i, j) * states(j);
          end loop;
          for j in num_inptus - 1 downto 0 loop
            accum := accum + D(i, j) * inputs(j);
          end loop;
          accum := accum sra (width - 1);
          accum := (accum * cs1) sra (width - 1);
          output(i) <= accum;
        end loop;

      end if;

    end if;

  end process;
end SSController;