#include <socks/libc/string.h>
#include <socks/bitmap.h>

void bitmap_zero(unsigned long *map, unsigned long nbits)
{
	unsigned long words = BITMAP_WORDS(nbits);
	memset(map, 0x00, words * sizeof *map);
}

void bitmap_fill(unsigned long *map, unsigned long nbits)
{
	unsigned long words = BITMAP_WORDS(nbits);
	memset(map, 0xFF, words * sizeof *map);
}

void bitmap_set(unsigned long *map, unsigned long bit)
{
	unsigned long index = bit / BITS_PER_WORD;
	unsigned long offset = (BITS_PER_WORD - bit - 1) & (BITS_PER_WORD - 1);
	unsigned long mask = 1ul << offset;

	map[index] |= mask;
}

void bitmap_clear(unsigned long *map, unsigned long bit)
{
	unsigned long index = bit / BITS_PER_WORD;
	unsigned long offset = bit & (BITS_PER_WORD - 1);
	unsigned long mask = 1ul << offset;

	map[index] &= ~mask;
}

bool bitmap_check(unsigned long *map, unsigned long bit)
{
	unsigned long index = bit / BITS_PER_WORD;
	unsigned long offset = (BITS_PER_WORD - bit - 1) & (BITS_PER_WORD - 1);
	unsigned long mask = 1ul << offset;

	return (map[index] & mask) != 0 ? true : false;

}

unsigned int bitmap_count_set(unsigned long *map, unsigned long nbits)
{
	unsigned long words = BITMAP_WORDS(nbits);
	unsigned int set_bits = 0;

	for (unsigned long i = 0; i < words; i++) {
		set_bits += __builtin_popcountl(map[i]);
	}

	if (set_bits > nbits) {
		set_bits = nbits;
	}

	return set_bits;
}

unsigned int bitmap_count_clear(unsigned long *map, unsigned long nbits)
{
	unsigned long words = BITMAP_WORDS(nbits);
	unsigned int clear_bits = 0;

	for (unsigned long i = 0; i < words; i++) {
		clear_bits += __builtin_popcountl(~map[i]);
	}

	if (clear_bits > nbits) {
		clear_bits = nbits;
	}

	return clear_bits;
}

unsigned int bitmap_highest_set(unsigned long *map, unsigned long nbits)
{
	unsigned long words = BITMAP_WORDS(nbits);
	unsigned long bit_index = 0;
	unsigned long last_word = 0;

	unsigned long i;
	for (i = 0; i < words; i++) {
		if (map[i] != 0x00) {
			last_word = map[i];
			bit_index = i * BITS_PER_WORD;
		}
	}

	if (last_word == 0x00) {
		return BITMAP_NPOS;
	}

	return bit_index + (BITS_PER_WORD - __builtin_ctzl(last_word) - 1);
}

unsigned int bitmap_highest_clear(unsigned long *map, unsigned long nbits)
{
	unsigned long words = BITMAP_WORDS(nbits);
	unsigned long bit_index = 0;
	unsigned long last_word = ~(unsigned long)0;

	for (unsigned long i = 0; i < words; i++) {
		if (map[i] != (~(unsigned long)0)) {
			last_word = map[i];
			bit_index = i * BITS_PER_WORD;
		}
	}

	if (last_word == ~(unsigned long)0) {
		return BITMAP_NPOS;
	}

	if (last_word == 0) {
		return bit_index + BITS_PER_WORD - 1;
	}

	return bit_index + (BITS_PER_WORD - __builtin_ctzl(~last_word)) - 1;
}

unsigned int bitmap_lowest_set(unsigned long *map, unsigned long nbits)
{
	unsigned long words = BITMAP_WORDS(nbits);
	unsigned long bit_index = 0;
	unsigned long last_word = 0;

	unsigned long i;
	for (i = 0; i < words; i++) {
		if (map[i] != 0x00) {
			last_word = map[i];
			bit_index = i * BITS_PER_WORD;
			break;
		}
	}

	if (last_word == 0x00) {
		return BITMAP_NPOS;
	}

	return bit_index + __builtin_clzl(last_word);
}

unsigned int bitmap_lowest_clear(unsigned long *map, unsigned long nbits)
{
	unsigned long words = BITMAP_WORDS(nbits);
	unsigned long bit_index = 0;
	unsigned long last_word = 0;

	unsigned long i;
	for (i = 0; i < words; i++) {
		if (map[i] != (~(unsigned long)0)) {
			last_word = map[i];
			bit_index = i * BITS_PER_WORD;
			break;
		}
	}

	if (last_word == 0) {
		return bit_index;
	}

	if (last_word == (~(unsigned long)0)) {
		return BITMAP_NPOS;
	}

	return bit_index + __builtin_clzl(~last_word);
}