use std.textio.all;

package body bv_arithmetic is
  
  -- Arithmetic on bit_vectors.

  -- Bit vectors are treated as either unsigned or signed (2's complement) numbers
  -- bv'first is the msb and bv'last is the lsb

  -- Dyadic operations assume the two bit vectors are the same length and index range.
  -- Functions assume the result is the same length and index range as arguments.

  function bv_to_natural(bv : in bit_vector) return natural is
    variable result : natural := 0;
  begin
    for index in bv'range loop
      result := result * 2 + bit'pos(bv(index));
    end loop;
    return result;
  end bv_to_natural;

  function natural_to_bv(nat : in natural;
      	      	      	 length : in natural) return bit_vector is
    variable temp : natural := nat;
    variable result : bit_vector(0 to length-1);
  begin
    for index in result'reverse_range loop
      result(index) := bit'val(temp rem 2);
      temp := temp / 2;
    end loop;
    return result;
  end natural_to_bv;

  function bv_to_integer(bv : in bit_vector) return integer is
    variable temp : bit_vector(bv'range);
    variable result : integer := 0;
  begin
    if bv(bv'left) = '1' then	  -- negative number
      temp := not bv;
    else
      temp := bv;
    end if;
    for index in bv'range loop	  -- sign bit of temp = '0'
      result := result * 2 + bit'pos(temp(index));
    end loop;
    if bv(bv'left) = '1' then
      result := (-result) - 1;
    end if;
    return result;
  end bv_to_integer;

  function integer_to_bv(int : in integer;
      	      	      	 length : in natural) return bit_vector is

    variable temp : integer;
    variable result : bit_vector(0 to length-1);
  begin
    if int < 0 then
      temp := -(int+1);
    else
      temp := int;
    end if;
    for index in result'reverse_range loop
      result(index) := bit'val(temp rem 2);
      temp := temp / 2;
    end loop;
    if int < 0 then
      result := not result;
      result(result'left) := '1';
    end if;
    return result;
  end integer_to_bv;

  procedure bv_add (bv1, bv2 : in bit_vector;
      	       	    bv_result : out bit_vector;
		    overflow : out boolean) is
    variable result : bit_vector(bv1'range);
    variable carry_in : bit;
    variable carry_out : bit := '0';
  begin
    for index in result'reverse_range loop
      carry_in := carry_out;  -- of previous bit
      result(index) := bv1(index) xor bv2(index) xor carry_in;
      carry_out := (bv1(index) and bv2(index))
      	      	   or (carry_in and (bv1(index) xor bv2(index)));
    end loop;
    bv_result := result;
    overflow := carry_out /= carry_in;
  end bv_add;

  procedure bv_sub (bv1, bv2 : in bit_vector;
      	       	    bv_result : out bit_vector;
		    overflow : out boolean) is
    -- subtraction implemented by adding ((not bv2) + 1), ie -bv2
    variable result : bit_vector(bv1'range);
    variable carry_in : bit;
    variable carry_out : bit := '1';
  begin
    for index in result'reverse_range loop
      carry_in := carry_out;  -- of previous bit
      result(index) := bv1(index) xor (not bv2(index)) xor carry_in;
      carry_out := (bv1(index) and (not bv2(index)))
      	      	   or (carry_in and (bv1(index) xor (not bv2(index))));
    end loop;
    bv_result := result;
    overflow := carry_out /= carry_in;
  end bv_sub;

  procedure bv_addu (bv1, bv2 : in bit_vector;
      	       	     bv_result : out bit_vector;
		     overflow : out boolean) is
    variable result : bit_vector(bv1'range);
    variable carry : bit := '0';
  begin
    for index in result'reverse_range loop
      result(index) := bv1(index) xor bv2(index) xor carry;
      carry := (bv1(index) and bv2(index)) or (carry and (bv1(index) xor bv2(index)));
    end loop;
    bv_result := result;
    overflow := carry = '1';
  end bv_addu;

  procedure bv_subu (bv1, bv2 : in bit_vector;
      	       	     bv_result : out bit_vector;
		     overflow : out boolean) is
    variable result : bit_vector(bv1'range);
    variable borrow : bit := '0';
  begin
    for index in result'reverse_range loop
      result(index) := bv1(index) xor bv2(index) xor borrow;
      borrow := (not bv1(index) and bv2(index))
      	      	or (borrow and not (bv1(index) xor bv2(index)));
    end loop;
    bv_result := result;
    overflow := borrow = '1';
  end bv_subu;

  function bv_sll (bv : in bit_vector;
       	      	   shift_count : in natural) return bit_vector is
    variable result : bit_vector(bv'range) := (others => '0');
  begin
    if bv'left = bv'low then -- ascending range
      result(result'left to
      	       result'left + result'length-1 - (shift_count mod result'length)) :=
      	bv(bv'left + (shift_count mod bv'length) to bv'right);
    else -- descending range
      result(result'left downto 
      	       result'left - (result'length-1) + (shift_count mod result'length)) :=
      	bv(bv'left - (shift_count mod bv'length) downto bv'right);
    end if;
    return result;
  end bv_sll;

  function bv_srl (bv : in bit_vector;
      	      	   shift_count : in natural) return bit_vector is
    variable result : bit_vector(bv'range) := (others => '0');
  begin
    if bv'left = bv'low then -- ascending range
      result(result'left + (shift_count mod result'length) to result'right) :=
      	bv(bv'left to bv'left + bv'length-1 - (shift_count mod bv'length));
    else -- descending range
      result(result'left - (shift_count mod result'length) downto result'right) :=
      	bv(bv'left downto bv'left - (bv'length-1) + (shift_count mod bv'length));
    end if;
    return result;
  end bv_srl;

  function bv_sra (bv : in bit_vector;
      	      	   shift_count : in natural) return bit_vector is
    variable result : bit_vector(bv'range) := (others => bv(bv'left));
  begin
    if bv'left = bv'low then -- ascending range
      result(result'left + (shift_count mod result'length) to result'right) :=
      	bv(bv'left to bv'left + bv'length-1 - (shift_count mod bv'length));
    else -- descending range
      result(result'left - (shift_count mod result'length) downto result'right) :=
      	bv(bv'left downto bv'left - (bv'length-1) + (shift_count mod bv'length));
    end if;
    return result;
  end bv_sra;


  -- this function is here to work around a Mentor bug in 
  -- "<" (bit_vector, bit_vector)
  
  function cmp_lt (bv1, bv2 : in bit_vector) return boolean is
    variable i1, i2 : natural;
    variable bv1_is_ascending, bv2_is_ascending : boolean;
  begin
    i1 := bv1'left;
    i2 := bv2'left;
    bv1_is_ascending := bv1'left = bv1'low;
    bv2_is_ascending := bv2'left = bv2'low;
    loop
      if (bv1_is_ascending and i1 > bv1'right) or ((not bv1_is_ascending) and i1 < bv1'right) then
      	-- off the end of bv1, so bv1 < bv2 iff not (off end of bv2)
	return not ((bv2_is_ascending and i2 > bv2'right) 
	      	    or ((not bv2_is_ascending) and i2 < bv2'right));
      elsif (bv2_is_ascending and i2 > bv2'right) or ((not bv2_is_ascending) and i2 < bv2'right) then
      	-- not off end of bv1, and off end of bv2
	return false;
      elsif bv1(i1) /= bv2(i2) then
      	return bv1(i1) < bv2(i2);
      else
      	if bv1_is_ascending then
	  i1 := i1 + 1;
	else
	  i1 := i1 - 1;
	end if;
      	if bv2_is_ascending then
	  i2 := i2 + 1;
	else
	  i2 := i2 - 1;
	end if;
      end if;
    end loop;
  end cmp_lt;

  function bv_lt (bv1, bv2 : in bit_vector) return boolean is
    variable tmp1 : bit_vector(bv1'range) := bv1;
    variable tmp2 : bit_vector(bv2'range) := bv2;
  begin
    tmp1(tmp1'left) := not tmp1(tmp1'left);
    tmp2(tmp2'left) := not tmp2(tmp2'left);
    return tmp1 < tmp2;
  end bv_lt;
  
  function bv_le (bv1, bv2 : in bit_vector) return boolean is
    variable tmp1 : bit_vector(bv1'range) := bv1;
    variable tmp2 : bit_vector(bv2'range) := bv2;
  begin
    tmp1(tmp1'left) := not tmp1(tmp1'left);
    tmp2(tmp2'left) := not tmp2(tmp2'left);
    return (tmp1 < tmp2) or (tmp1 = tmp2);
  end bv_le;
  
  function bv_gt (bv1, bv2 : in bit_vector) return boolean is
    variable tmp1 : bit_vector(bv1'range) := bv1;
    variable tmp2 : bit_vector(bv2'range) := bv2;
  begin
    tmp1(tmp1'left) := not tmp1(tmp1'left);
    tmp2(tmp2'left) := not tmp2(tmp2'left);
    return tmp2 < tmp1;
  end bv_gt;
  
  function bv_ge (bv1, bv2 : in bit_vector) return boolean is
    variable tmp1 : bit_vector(bv1'range) := bv1;
    variable tmp2 : bit_vector(bv2'range) := bv2;
  begin
    tmp1(tmp1'left) := not tmp1(tmp1'left);
    tmp2(tmp2'left) := not tmp2(tmp2'left);
    return (tmp2 < tmp1) or (tmp1 = tmp2);
  end bv_ge;
  
  function bv_sext (bv : in bit_vector;
      	      	    length : in natural) return bit_vector is
    variable result : bit_vector(0 to length - 1) := (others => bv(bv'left));
    variable src_length : natural := bv'length;
  begin
    if src_length > length then
      src_length := length;
    end if;
    if bv'left < bv'right then -- ascending range
      result(length - src_length to length - 1) :=
      	  bv(bv'right - (src_length - 1) to bv'right);
    else -- ascending range
      result(length - src_length to length - 1) :=
      	  bv(bv'right + (src_length - 1) downto bv'right);
    end if;
    return result;
  end bv_sext;
  
  function bv_zext (bv : in bit_vector;
      	      	    length : in natural) return bit_vector is
    variable result : bit_vector(0 to length - 1) := (others => '0');
    variable src_length : natural := bv'length;
  begin
    if src_length > length then
      src_length := length;
    end if;
    if bv'left < bv'right then -- ascending range
      result(length - src_length to length - 1) :=
      	  bv(bv'right - (src_length - 1) to bv'right);
    else -- ascending range
      result(length - src_length to length - 1) :=
      	  bv(bv'right + (src_length - 1) downto bv'right);
    end if;
    return result;
  end bv_zext;

  procedure write_hex (L : inout line; b : in bit_vector) is
      constant nr_digits : natural := (b'length + 3) / 4;
      variable bits : bit_vector(0 to 4*nr_digits-1) := (others => '0');
      variable four_bits : bit_vector(0 to 3);
      variable digit : character;
    begin
      write(L, string'("X"""));
      bits(bits'right - b'length + 1 to bits'right) := b;
      for i in 0 to nr_digits - 1 loop
	four_bits := bits(4*i to 4*i + 3);
	case four_bits is
	  when b"0000" =>
	    digit := '0';
	  when b"0001" =>
	    digit := '1';
	  when b"0010" =>
	    digit := '2';
	  when b"0011" =>
	    digit := '3';
	  when b"0100" =>
	    digit := '4';
	  when b"0101" =>
	    digit := '5';
	  when b"0110" =>
	    digit := '6';
	  when b"0111" =>
	    digit := '7';
	  when b"1000" =>
	    digit := '8';
	  when b"1001" =>
	    digit := '9';
	  when b"1010" =>
	    digit := 'A';
	  when b"1011" =>
	    digit := 'B';
	  when b"1100" =>
	    digit := 'C';
	  when b"1101" =>
	    digit := 'D';
	  when b"1110" =>
	    digit := 'E';
	  when b"1111" =>
	    digit := 'F';
	end case;
	write(L, digit);
      end loop;
      write(L, '"');
    end write_hex;

end bv_arithmetic;