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

#define BITS_PER_WORD       (8 * sizeof(bitmap_word_t))
#define DIV_ROUND_UP(n, d)  (((n) + (d) - 1) / (d))
#define BITMAP_WORDS(nbits) DIV_ROUND_UP(nbits, BITS_PER_WORD)

/*** PRIVATE DATA *************************************************************/

typedef unsigned long bitmap_word_t;

struct b_bitmap_p {
	bitmap_word_t *map_words;
	size_t map_nr_words, map_nr_bits;
};

/*** PRIVATE FUNCTIONS ********************************************************/

static void bitmap_set_bit(struct b_bitmap_p *map, size_t 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->map_words[index] |= mask;
}

static void bitmap_clear_bit(struct b_bitmap_p *map, size_t bit)
{
	unsigned long index = bit / BITS_PER_WORD;
	unsigned long offset = bit & (BITS_PER_WORD - 1);
	unsigned long mask = 1ul << offset;

	map->map_words[index] &= ~mask;
}

static void bitmap_set_range(struct b_bitmap_p *map, size_t first_bit, size_t nbits)
{
}

static void bitmap_clear_range(struct b_bitmap_p *map, size_t first_bit, size_t nbits)
{
}

static void bitmap_set_all(struct b_bitmap_p *map)
{
	memset(map->map_words, 0xFF, map->map_nr_words * sizeof(bitmap_word_t));
}

static void bitmap_clear_all(struct b_bitmap_p *map)
{
	memset(map->map_words, 0x00, map->map_nr_words * sizeof(bitmap_word_t));
}

static bool bitmap_check_bit(const struct b_bitmap_p *map, size_t 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->map_words[index] & mask) != 0;
}

static size_t bitmap_count_set_bits(const struct b_bitmap_p *map)
{
	size_t set_bits = 0;

	for (size_t i = 0; i < map->map_nr_words; i++) {
		set_bits += b_popcountl(map->map_words[i]);
	}

	if (set_bits > map->map_nr_bits) {
		set_bits = map->map_nr_bits;
	}

	return set_bits;
}

static size_t bitmap_count_clear_bits(const struct b_bitmap_p *map)
{
	size_t clear_bits = 0;

	for (size_t i = 0; i < map->map_nr_words; i++) {
		clear_bits += b_popcountl(~map->map_words[i]);
	}

	if (clear_bits > map->map_nr_bits) {
		clear_bits = map->map_nr_bits;
	}

	return clear_bits;
}

static size_t bitmap_highest_set_bit(const struct b_bitmap_p *map)
{
	unsigned long bit_index = 0;
	bitmap_word_t last_word = 0;

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

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

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

static size_t bitmap_highest_clear_bit(const struct b_bitmap_p *map)
{
	unsigned long bit_index = 0;
	bitmap_word_t last_word = ~(bitmap_word_t)0;

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

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

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

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

static size_t bitmap_lowest_set_bit(const struct b_bitmap_p *map)
{
	unsigned long bit_index = 0;
	bitmap_word_t last_word = 0;

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

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

	return bit_index + b_clzl(last_word);
}

static size_t bitmap_lowest_clear_bit(const struct b_bitmap_p *map)
{
	unsigned long bit_index = 0;
	bitmap_word_t last_word = 0;

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

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

	if (last_word == (~(bitmap_word_t)0)) {
		return B_NPOS;
	}

	return bit_index + b_clzl(~last_word);
}

/*** PUBLIC FUNCTIONS *********************************************************/

b_bitmap *b_bitmap_create(size_t nr_bits)
{
	b_bitmap *map = b_object_create(B_TYPE_BITMAP);
	if (!map) {
		return NULL;
	}

	struct b_bitmap_p *p = b_object_get_private(map, B_TYPE_BITMAP);
	p->map_nr_bits = nr_bits;
	p->map_nr_words = BITMAP_WORDS(nr_bits);
	p->map_words = calloc(p->map_nr_words, sizeof(bitmap_word_t));
	if (!p->map_words) {
		b_bitmap_unref(map);
		return NULL;
	}

	return map;
}

void b_bitmap_set_bit(b_bitmap *map, size_t bit)
{
	B_CLASS_DISPATCH_STATIC_V(B_TYPE_BITMAP, bitmap_set_bit, map, bit);
}

void b_bitmap_clear_bit(b_bitmap *map, size_t bit)
{
	B_CLASS_DISPATCH_STATIC_V(B_TYPE_BITMAP, bitmap_clear_bit, map, bit);
}

void b_bitmap_set_range(b_bitmap *map, size_t first_bit, size_t nbits)
{
	B_CLASS_DISPATCH_STATIC_V(
		B_TYPE_BITMAP, bitmap_set_range, map, first_bit, nbits);
}

void b_bitmap_clear_range(b_bitmap *map, size_t first_bit, size_t nbits)
{
	B_CLASS_DISPATCH_STATIC_V(
		B_TYPE_BITMAP, bitmap_clear_range, map, first_bit, nbits);
}

void b_bitmap_set_all(b_bitmap *map)
{
	B_CLASS_DISPATCH_STATIC_V0(B_TYPE_BITMAP, bitmap_set_all, map);
}

void b_bitmap_clear_all(b_bitmap *map)
{
	B_CLASS_DISPATCH_STATIC_V0(B_TYPE_BITMAP, bitmap_clear_all, map);
}

bool b_bitmap_check_bit(const b_bitmap *map, size_t bit)
{
	B_CLASS_DISPATCH_STATIC(B_TYPE_BITMAP, bitmap_check_bit, map, bit);
}

size_t b_bitmap_count_set_bits(const b_bitmap *map)
{
	B_CLASS_DISPATCH_STATIC_0(B_TYPE_BITMAP, bitmap_count_set_bits, map);
}

size_t b_bitmap_count_clear_bits(const b_bitmap *map)
{
	B_CLASS_DISPATCH_STATIC_0(B_TYPE_BITMAP, bitmap_count_clear_bits, map);
}

size_t b_bitmap_highest_set_bit(const b_bitmap *map)
{
	B_CLASS_DISPATCH_STATIC_0(B_TYPE_BITMAP, bitmap_highest_set_bit, map);
}

size_t b_bitmap_highest_clear_bit(const b_bitmap *map)
{
	B_CLASS_DISPATCH_STATIC_0(B_TYPE_BITMAP, bitmap_highest_clear_bit, map);
}

size_t b_bitmap_lowest_set_bit(const b_bitmap *map)
{
	B_CLASS_DISPATCH_STATIC_0(B_TYPE_BITMAP, bitmap_lowest_set_bit, map);
}

size_t b_bitmap_lowest_clear_bit(const b_bitmap *map)
{
	B_CLASS_DISPATCH_STATIC_0(B_TYPE_BITMAP, bitmap_lowest_clear_bit, map);
}

/*** PUBLIC ALIAS FUNCTIONS ***************************************************/
/*** VIRTUAL FUNCTIONS ********************************************************/

static void bitmap_init(b_object *obj, void *priv)
{
	struct b_bitmap_p *map = priv;
}

static void bitmap_fini(b_object *obj, void *priv)
{
	struct b_bitmap_p *map = priv;
}

static void bitmap_to_string(const b_object *obj, b_stream *out)
{
	const struct b_bitmap_p *map = b_object_get_private(obj, B_TYPE_BITMAP);

	unsigned char *bytes = (unsigned char *)map->map_words;
	size_t nr_bytes = map->map_nr_words * sizeof(bitmap_word_t);
	unsigned char c = 0;

	for (size_t i = 0; i < nr_bytes - 1; i++) {
		c = bytes[i];
		b_stream_write_fmt(
			out, NULL, "%c%c%c%c%c%c%c%c", c & 0x80 ? '1' : '0',
			c & 0x40 ? '1' : '0', c & 0x20 ? '1' : '0',
			c & 0x10 ? '1' : '0', c & 0x08 ? '1' : '0',
			c & 0x04 ? '1' : '0', c & 0x02 ? '1' : '0',
			c & 0x01 ? '1' : '0');
	}

	unsigned char mask = 0x80;
	size_t remaining_bits = map->map_nr_bits - ((nr_bytes - 1) * 8);
	c = bytes[nr_bytes - 1];

	for (size_t i = 0; i < remaining_bits; i++) {

		b_stream_write_fmt(out, NULL, "%c", (c & mask) ? '1' : '0');
	}
}

/*** CLASS DEFINITION *********************************************************/

B_TYPE_CLASS_DEFINITION_BEGIN(b_bitmap)
	B_TYPE_CLASS_INTERFACE_BEGIN(b_object, B_TYPE_OBJECT)
		B_INTERFACE_ENTRY(to_string) = bitmap_to_string;
	B_TYPE_CLASS_INTERFACE_END(b_object, B_TYPE_OBJECT)
B_TYPE_CLASS_DEFINITION_END(b_bitmap)

B_TYPE_DEFINITION_BEGIN(b_bitmap)
	B_TYPE_ID(0xea115cef, 0x8a63, 0x445f, 0x9474, 0xba9309d5dde8);
	B_TYPE_CLASS(b_bitmap_class);
	B_TYPE_INSTANCE_PRIVATE(struct b_bitmap_p);
	B_TYPE_INSTANCE_INIT(bitmap_init);
	B_TYPE_INSTANCE_FINI(bitmap_fini);
B_TYPE_DEFINITION_END(b_bitmap)

/*** ITERATOR FUNCTIONS *******************************************************/