#include <stdint.h>

#include <kernel/heap.h>
#include <debugging.h>

#define HEAP_SIZE (4096)

uint8_t* global_heap_data[HEAP_SIZE + sizeof(struct Heap_Block)];
struct Heap_Metadata global_heap;

void heap_init() 
{
    global_heap.size = HEAP_SIZE;
    global_heap.start = (struct Heap_Block*) global_heap_data;

    struct Heap_Block* start = global_heap.start;
    start->size = HEAP_SIZE;
    start->used = false;
    start->data = start + 1;
    start->next = 0;
}

void* malloc(size_t size)
{
    struct Heap_Block* current = global_heap.start;
    
    while (true) {
        if (current->used || current->size < size) { 
            current = current->next; 
            continue; 
        }

        if (current == 0) {
            return 0;
        }

        struct Heap_Block* next = current->data + size;

        next->next = current->next;
        next->size = current->size - size - sizeof(struct Heap_Block);
        next->data = next + 1;

        current->next = next;
        current->size = size;
        current->used = true;

        return current->data;
    }
}

void free(void *ptr)
{
    struct Heap_Block* current = global_heap.start;

    struct Heap_Block* first_unused;
    bool found_unused = false;

    while (current->data != ptr) { 
        if (!current->used && !found_unused) {
            first_unused = current;
            found_unused = true;
        } else {
            found_unused = false;
        }
        current = current->next;
    }

    if (!found_unused) {
        first_unused = current;
    }

    current->used = false;

    if (current->next == 0) {
        return;
    }

    struct Heap_Block* start = first_unused;
    current = start;

    while (current->used == false && current->next != 0) {
        current = current->next;
    }

    start->next = current;
    start->size = (uint32_t)start->next - (uint32_t)start->data;
}