#include <fx/core/bitop.h>
#include <fx/core/stream.h>
#include <fx/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 fx_bitmap_p {
	bitmap_word_t *map_words;
	size_t map_nr_words, map_nr_bits;
};

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

static void bitmap_set_bit(struct fx_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 fx_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 fx_bitmap_p *map, size_t first_bit, size_t nbits)
{
}

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

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

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

static bool bitmap_check_bit(const struct fx_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 fx_bitmap_p *map)
{
	size_t set_bits = 0;

	for (size_t i = 0; i < map->map_nr_words; i++) {
		set_bits += fx_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 fx_bitmap_p *map)
{
	size_t clear_bits = 0;

	for (size_t i = 0; i < map->map_nr_words; i++) {
		clear_bits += fx_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 fx_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 FX_NPOS;
	}

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

static size_t bitmap_highest_clear_bit(const struct fx_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 FX_NPOS;
	}

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

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

static size_t bitmap_lowest_set_bit(const struct fx_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 FX_NPOS;
	}

	return bit_index + fx_clzl(last_word);
}

static size_t bitmap_lowest_clear_bit(const struct fx_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 FX_NPOS;
	}

	return bit_index + fx_clzl(~last_word);
}

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

fx_bitmap *fx_bitmap_create(size_t nr_bits)
{
	fx_bitmap *map = fx_object_create(FX_TYPE_BITMAP);
	if (!map) {
		return NULL;
	}

	struct fx_bitmap_p *p = fx_object_get_private(map, FX_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) {
		fx_bitmap_unref(map);
		return NULL;
	}

	return map;
}

void fx_bitmap_set_bit(fx_bitmap *map, size_t bit)
{
	FX_CLASS_DISPATCH_STATIC_V(FX_TYPE_BITMAP, bitmap_set_bit, map, bit);
}

void fx_bitmap_clear_bit(fx_bitmap *map, size_t bit)
{
	FX_CLASS_DISPATCH_STATIC_V(FX_TYPE_BITMAP, bitmap_clear_bit, map, bit);
}

void fx_bitmap_set_range(fx_bitmap *map, size_t first_bit, size_t nbits)
{
	FX_CLASS_DISPATCH_STATIC_V(
		FX_TYPE_BITMAP, bitmap_set_range, map, first_bit, nbits);
}

void fx_bitmap_clear_range(fx_bitmap *map, size_t first_bit, size_t nbits)
{
	FX_CLASS_DISPATCH_STATIC_V(
		FX_TYPE_BITMAP, bitmap_clear_range, map, first_bit, nbits);
}

void fx_bitmap_set_all(fx_bitmap *map)
{
	FX_CLASS_DISPATCH_STATIC_V0(FX_TYPE_BITMAP, bitmap_set_all, map);
}

void fx_bitmap_clear_all(fx_bitmap *map)
{
	FX_CLASS_DISPATCH_STATIC_V0(FX_TYPE_BITMAP, bitmap_clear_all, map);
}

bool fx_bitmap_check_bit(const fx_bitmap *map, size_t bit)
{
	FX_CLASS_DISPATCH_STATIC(FX_TYPE_BITMAP, bitmap_check_bit, map, bit);
}

size_t fx_bitmap_count_set_bits(const fx_bitmap *map)
{
	FX_CLASS_DISPATCH_STATIC_0(FX_TYPE_BITMAP, bitmap_count_set_bits, map);
}

size_t fx_bitmap_count_clear_bits(const fx_bitmap *map)
{
	FX_CLASS_DISPATCH_STATIC_0(FX_TYPE_BITMAP, bitmap_count_clear_bits, map);
}

size_t fx_bitmap_highest_set_bit(const fx_bitmap *map)
{
	FX_CLASS_DISPATCH_STATIC_0(FX_TYPE_BITMAP, bitmap_highest_set_bit, map);
}

size_t fx_bitmap_highest_clear_bit(const fx_bitmap *map)
{
	FX_CLASS_DISPATCH_STATIC_0(FX_TYPE_BITMAP, bitmap_highest_clear_bit, map);
}

size_t fx_bitmap_lowest_set_bit(const fx_bitmap *map)
{
	FX_CLASS_DISPATCH_STATIC_0(FX_TYPE_BITMAP, bitmap_lowest_set_bit, map);
}

size_t fx_bitmap_lowest_clear_bit(const fx_bitmap *map)
{
	FX_CLASS_DISPATCH_STATIC_0(FX_TYPE_BITMAP, bitmap_lowest_clear_bit, map);
}

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

static void bitmap_init(fx_object *obj, void *priv)
{
	struct fx_bitmap_p *map = priv;
}

static void bitmap_fini(fx_object *obj, void *priv)
{
	struct fx_bitmap_p *map = priv;
}

static void bitmap_to_string(const fx_object *obj, fx_stream *out)
{
	const struct fx_bitmap_p *map = fx_object_get_private(obj, FX_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];
		fx_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++) {

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

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

FX_TYPE_CLASS_DEFINITION_BEGIN(fx_bitmap)
	FX_TYPE_CLASS_INTERFACE_BEGIN(fx_object, FX_TYPE_OBJECT)
		FX_INTERFACE_ENTRY(to_string) = bitmap_to_string;
	FX_TYPE_CLASS_INTERFACE_END(fx_object, FX_TYPE_OBJECT)
FX_TYPE_CLASS_DEFINITION_END(fx_bitmap)

FX_TYPE_DEFINITION_BEGIN(fx_bitmap)
	FX_TYPE_ID(0xea115cef, 0x8a63, 0x445f, 0x9474, 0xba9309d5dde8);
	FX_TYPE_CLASS(fx_bitmap_class);
	FX_TYPE_INSTANCE_PRIVATE(struct fx_bitmap_p);
	FX_TYPE_INSTANCE_INIT(bitmap_init);
	FX_TYPE_INSTANCE_FINI(bitmap_fini);
FX_TYPE_DEFINITION_END(fx_bitmap)

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