#include <string.h>
#include <blue/object/bitmap.h>
#include <blue/core/bitop.h>

void b_bitmap_zero(b_bitmap_word *map, unsigned long nbits)
{
	unsigned long words = B_BITMAP_WORDS(nbits);
	memset(map, 0x00, words * sizeof *map);
}

void b_bitmap_fill(b_bitmap_word *map, unsigned long nbits)
{
	unsigned long words = B_BITMAP_WORDS(nbits);
	memset(map, 0xFF, words * sizeof *map);
}

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

	map[index] |= mask;
}

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

	map[index] &= ~mask;
}

bool b_bitmap_check(const b_bitmap_word *map, unsigned long bit)
{
	unsigned long index = bit / Z__B_BITS_PER_WORD;
	unsigned long offset = (Z__B_BITS_PER_WORD - bit - 1) & (Z__B_BITS_PER_WORD - 1);
	unsigned long mask = 1ul << offset;

	return (map[index] & mask) != 0;
}

unsigned int b_bitmap_count_set(const b_bitmap_word *map, unsigned long nbits)
{
	unsigned long words = B_BITMAP_WORDS(nbits);
	unsigned int set_bits = 0;

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

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

	return set_bits;
}

unsigned int b_bitmap_count_clear(const b_bitmap_word *map, unsigned long nbits)
{
	unsigned long words = B_BITMAP_WORDS(nbits);
	unsigned int clear_bits = 0;

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

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

	return clear_bits;
}

unsigned int b_bitmap_highest_set(const b_bitmap_word *map, unsigned long nbits)
{
	unsigned long words = B_BITMAP_WORDS(nbits);
	unsigned long bit_index = 0;
	b_bitmap_word last_word = 0;

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

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

	return bit_index + (Z__B_BITS_PER_WORD - b_ctzl(last_word) - 1);
}

unsigned int b_bitmap_highest_clear(const b_bitmap_word *map, unsigned long nbits)
{
	unsigned long words = B_BITMAP_WORDS(nbits);
	unsigned long bit_index = 0;
	b_bitmap_word last_word = ~(b_bitmap_word)0;

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

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

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

	return bit_index + (Z__B_BITS_PER_WORD - b_ctzl(~last_word)) - 1;
}

unsigned int b_bitmap_lowest_set(const b_bitmap_word *map, unsigned long nbits)
{
	unsigned long words = B_BITMAP_WORDS(nbits);
	unsigned long bit_index = 0;
	b_bitmap_word last_word = 0;

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

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

	return bit_index + b_clzl(last_word);
}

unsigned int b_bitmap_lowest_clear(const b_bitmap_word *map, unsigned long nbits)
{
	unsigned long words = B_BITMAP_WORDS(nbits);
	unsigned long bit_index = 0;
	b_bitmap_word 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 * Z__B_BITS_PER_WORD;
			break;
		}
	}

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

	if (last_word == (~(b_bitmap_word)0)) {
		return B_BITMAP_NPOS;
	}

	return bit_index + b_clzl(~last_word);
}