#include <stdbool.h>
#include "stivale.h"
#include "heap.h"
#include "textio.h"


struct memchunk_free *heap;


inline static uint64_t size_align(uint64_t num) {
    return ((num + sizeof(struct memchunk_free) - 1) / sizeof(struct memchunk_free)) * sizeof(struct memchunk_free);
}

void heap_init() {
    heap = (struct memchunk_free *)higherhalf.base;
    heap[0].size = 0;
    heap[0].next = heap + 1;
    heap[0].next->size = higherhalf.length - sizeof(struct memchunk_free);
    heap[0].next->next = (void *)0;
}

void *heap_alloc(uint64_t size) {
    // Force-align size
    size = size_align(size);
    // Find first free space
    struct memchunk_free *nextmatch;
    for (nextmatch = heap;
         nextmatch->next->size < size;
         nextmatch = nextmatch->next) {
        print("Searching for heap space...\n");
    }
    struct memchunk_free *baknext = nextmatch->next;
    // Allocate
    nextmatch->next = baknext->next;
    nextmatch->size = baknext - nextmatch;
    // Return allocated memory
    return baknext;
}

void *heap_dealloc(void *memptr) {
    struct memchunk_free *lastchunk;
    for (struct memchunk_free *thischunk = heap;
         (uint64_t)lastchunk + (uint64_t)lastchunk->size < (uint64_t)memptr && (uint64_t)lastchunk->next > (uint64_t)memptr;
         lastchunk = lastchunk->next) {
        print("Was heap deallocation fixed?\n");
    }
    // Dechunk trailing available chunks
    lastchunk->size = lastchunk->next - lastchunk;
}