#include "buddymap.h"
#include "lib/string.h"
#include "boot/boot.h"
#include "io/log.h"
#include <stdbool.h>

#define ENTRY_BITS (sizeof(uintmax_t) * 8)
#define ENTRY_SIZE (ENTRY_BITS * PAGESIZE)
#define ENTRY_COUNT (MEMMAP_BUDDY_LIMIT / ENTRY_SIZE)
#define BUDDY_LAYERS 4

uintmax_t s_buddy_l0[ENTRY_COUNT];
uintmax_t s_buddy_l1[ENTRY_COUNT << 1];
uintmax_t s_buddy_l2[ENTRY_COUNT << 2];
uintmax_t s_buddy_l3[ENTRY_COUNT << 3];
uintmax_t *s_buddies[BUDDY_LAYERS] = {
    s_buddy_l0,
    s_buddy_l1,
    s_buddy_l2,
    s_buddy_l3
};
size_t s_buddies_lastfree[BUDDY_LAYERS] = { 1, 1, 1, 1 };

static bool
s_buddy_test(size_t l, size_t i)
{
    return (s_buddies[l][i / (ENTRY_BITS)] & (1ULL << (i % (ENTRY_BITS)))) > 0;
}

static void
s_buddy_set(size_t l, size_t i)
{
    size_t j = i << l;
    size_t w = 1 << l;
    for(int layer = 0; layer < BUDDY_LAYERS; layer++)
    {
        if(w == 0) w = 1;
        for(size_t bit = 0; bit < w; bit++) {
            size_t entry = (j + bit) / ENTRY_BITS;
            s_buddies[layer][entry] |= (1ULL << (j + bit % ENTRY_BITS));
        }
        j >>= 1;
        w >>= 1;
    }
}

static void
s_buddy_unset(size_t l, size_t i)
{
    size_t j = i << l;
    size_t w = 1 << l;
    bool free_upper = false;
    for(int layer = 0; layer < BUDDY_LAYERS; layer++)
    {
        if(w == 0) {
            size_t lower = (j << 1) % ENTRY_BITS;
            size_t other = (lower + 1);
            size_t entry = (j << 1) / ENTRY_BITS;
            if((s_buddies[layer-1][entry] & (1ULL << lower)) > 0 &&
               (s_buddies[layer-1][entry] & (1ULL << other)) > 0)
            s_buddies[layer][entry >> 1] &= ~(1ULL << (j % ENTRY_BITS));
        }

        for(size_t bit = 0; bit < w; bit++) {
            size_t entry = j / ENTRY_BITS;
            s_buddies[layer][entry] |= (1ULL << bit);
        }
        j >>= 1;
        w >>= 1;
    }
}

void
mem_buddy_set_range(uintptr_t base, size_t length)
{
    for(int l = 0; l < BUDDY_LAYERS; l++) {
        size_t bits = (length / PAGESIZE) >> l;
        size_t biti = (base / PAGESIZE) >> l;

        if(bits == 0) bits = 1;
        for(size_t i = 0; i < bits; i++) {
            size_t entry = (biti + i) / ENTRY_BITS;
            s_buddies[l][entry] |= 1ULL << ((biti + i) % ENTRY_BITS);
        }
    }
}

void
mem_buddy_free_range(uintptr_t base, size_t length)
{
    for(int l = 0; l < BUDDY_LAYERS; l++) {
        size_t bits = (length / PAGESIZE) >> l;
        size_t biti = (base / PAGESIZE) >> l;
        size_t bitbase = (biti * PAGESIZE) << l;

        if(bits == 0) continue;
        for(size_t i = 0; i < bits; i++, bitbase += (PAGESIZE << l)) {
            if(bitbase < base) continue;
            size_t entry = (biti + i) / ENTRY_BITS;
            s_buddies[l][entry] &= ~(1ULL << ((biti+ i) % ENTRY_BITS));
        }
    }
}


uintptr_t
mem_buddy_takefree(size_t l)
{
    uintmax_t *layer = s_buddies[l];
    size_t lastfree = s_buddies_lastfree[l];
    if(s_buddy_test(l, lastfree)) lastfree = 0;
    size_t entries = ENTRY_COUNT >> l;
    for(size_t i = lastfree / ENTRY_BITS; i < entries; i++) {
        uintmax_t entry = layer[i];
        if(entry == (uintmax_t)-1LL) continue;
        for(size_t j = 0; j < ENTRY_BITS; j++) {
            if((entry & (1ULL << j)) == 0) {
                size_t bit = (i * ENTRY_BITS) + j;
                s_buddies_lastfree[l] = bit + 1;
                s_buddy_set(l, bit);
                return bit * (PAGESIZE << l);
            }
        }
    }
    return 0;
}

void 
mem_buddy_setup()
{
    memset(s_buddy_l0, 0xFF, sizeof(s_buddy_l0));
    memset(s_buddy_l1, 0xFF, sizeof(s_buddy_l1));
    memset(s_buddy_l2, 0xFF, sizeof(s_buddy_l2));
    memset(s_buddy_l3, 0xFF, sizeof(s_buddy_l3));

    for(int i = 0; i < boot_memorymap.count; i++) {
        struct MemoryMapEntry *entry = &boot_memorymap.entries[i];
        klogf("%2i\t%#016X -> %#016X (%i)\n",
                i, entry->base, entry->base + entry->length, entry->usable);
        if(entry->base > MEMMAP_BUDDY_LIMIT) continue;
        size_t length = entry->length;
        if(entry->base + length > MEMMAP_BUDDY_LIMIT) length = MEMMAP_BUDDY_LIMIT - (entry->base + length);
        if(entry->usable)
            mem_buddy_free_range(entry->base, entry->length);
    }

    s_buddies[0][0] |= 1;
    s_buddies[1][0] |= 1;
    s_buddies[2][0] |= 1;
    s_buddies[3][0] |= 1;
}