--------------------------------------------------------------------------------
--
-- Kalman Filter Benchmark
--
-- Source: Adapted from the paper
--        "A Synthesis Process applied to the Kalman Filter BEnchmark"
--         by Cleland.O.Newton, DRA Malvern, UK
--         HLSW-92
--
-- VHDL Benchmark author: Champaka Ramachandran on Aug 18th 1992 
--
-- Verification Information:
--
--                  Verified     By whom?           Date         Simulator
--                  --------   ------------        --------     ------------
--  Syntax            yes   Champaka Ramachandran  18th Aug 92    ZYCAD
--  Functionality     yes   Champaka Ramachandran  18th Aug 92    ZYCAD
--------------------------------------------------------------------------------

use work.types.all;


entity kalman is
   port (Input_Vector   : in  nat8;
         Addr           : in  nat4;
         Cexec          : in  bit;
         Vector_type    : in  nat2;
         Output_Vector0 : out nat8;
         Output_Vector1 : out nat8;
         Output_Vector2 : out nat8; 
         Output_Vector3 : out nat8;
         CLK            : in bit;
         Reset          : in bit
        ); 
end kalman;

--VSS: design_style BEHAVIORAL

architecture kalman of kalman is

begin
main : process 

   variable A : Memory (15 downto 0);
   variable K : Memory (15 downto 0);
   variable G : Memory (15 downto 0);   -- Constant
   variable X : Memory ( 3 downto 0);   -- State vector
   variable Y : Memory ( 3 downto 0);   -- Input vector
   variable V : Memory ( 3 downto 0);   -- output vector 
   variable i : nat8;
   variable j : nat8;
   variable index : nat8;
   variable temp : nat8;

begin
   wait until (CLK = '1'); 
   if (Reset = '1') then
   end if;

  --  Loading coefficient array A, G and K and input vector Y
  RESET_LOOP: loop

      wait until (CLK = '1'); exit RESET_LOOP when (Reset = '1');
      case Vector_type is 
         --  Load A matrix which is 16x16 and is upper diagonal
         when 0 =>            A(Addr) := Input_Vector;
         --  Load K matrix which is 16x13 , but is padded with 0s to make it 16x16
         when 1 =>            K(Addr) := Input_Vector;
         --  Load G matrix which is 4x16
         when 2 =>            G(Addr) := Input_Vector;
         --  Load Y matrix which is 1x13 and is the input vector and is padded with 0s
         --  to make it 1x16
         when 3 =>            Y(Addr) := Input_Vector;
         when others =>
      end case;

      wait until (CLK = '1'); exit RESET_LOOP when (Reset = '1');
      if (Cexec = '1') then
         -- Computing state Vector X 
         i := 0;
         while (i < 4) loop
             j := 0; 
             temp := 0;
             while (j<4) loop
                index := i * 4 + j;
                temp := A(index) * X(j) + K(index) * Y(j) + temp;
                j := j + 1;
                wait until (CLK = '1'); exit RESET_LOOP when (Reset = '1');
             end loop;
             X(i) := temp;
             i := i + 1;
             wait until (CLK = '1'); exit RESET_LOOP when (Reset = '1');
         end loop;

         wait until (CLK = '1'); exit RESET_LOOP when (Reset = '1');
         -- Computing output Vector V
         i := 0;
         while (i<4) loop
             j := 0; 
             temp := 0;
             while (j<4) loop
                 index := i * 4 + j;
                 temp := G(index) * X(j) + temp;    
                 j := j + 1;
                 wait until (CLK = '1'); exit RESET_LOOP when (Reset = '1');
             end loop;
             V(i) := temp * Y(i+1);
             i := i + 1;
             wait until (CLK = '1'); exit RESET_LOOP when (Reset = '1');
         end loop;

         wait until (CLK = '1'); exit RESET_LOOP when (Reset = '1');
         -- Output Vector V 
         Output_Vector0 <= V(0);
         Output_Vector1 <= V(1);
         Output_Vector2 <= V(2);
         Output_Vector3 <= V(3);
         wait until (CLK = '1'); exit RESET_LOOP when (Reset = '1');
      end if;
    end loop RESET_LOOP;
end process;
end kalman;