LIBRARY IEEE; 
USE IEEE.std_logic_1164.ALL;
PACKAGE math IS
  TYPE st16 IS ARRAY(0 TO 15) OF std_logic;

  FUNCTION add(a, b: IN st16) RETURN st16;
  FUNCTION sub(a, b: IN st16) RETURN st16;

END math;

PACKAGE BODY math IS

  FUNCTION vect_to_int(S : st16) RETURN INTEGER IS
    VARIABLE result : INTEGER := 0;
  BEGIN
    FOR i IN 0 TO 7 LOOP
      result := result * 2;

      IF S(i) = '1' THEN
        result := result + 1;
      END IF;
    END LOOP;

    RETURN result;
  END vect_to_int;

  FUNCTION int_to_st16(s : INTEGER) RETURN st16 IS
    VARIABLE result : st16;
    VARIABLE digit : INTEGER := 2**15;
    VARIABLE local : INTEGER;
  BEGIN
    local := s;
    FOR i IN 15 DOWNTO 0 LOOP
      IF local/digit >= 1 THEN
        result(i) := '1';
        local := local - digit;
      ELSE
        result(i) := '0';
      END IF;

      digit := digit/2;

    END LOOP;
    RETURN result;
  END int_to_st16;

  FUNCTION add(a, b: IN st16) RETURN st16 IS
    VARIABLE result : INTEGER;
  BEGIN
    result := vect_to_int(a) + vect_to_int(b);
    RETURN int_to_st16(result);
  END add;

  FUNCTION sub(a, b: IN st16) RETURN st16 IS
    VARIABLE result : INTEGER;
  BEGIN
    result := vect_to_int(a) - vect_to_int(b);
    RETURN int_to_st16(result);
  END sub;

END math;




