#include <os/heap.h>
#include <memHead.h>
#include <os/storageKeys.h>

static Header base;
static Header *allocp = NULL;

void heap_init(size_t memSize)
{
	allocp = &base;
	allocp->size = 0;
	allocp->next = (void*)HEAP_START;
	allocp->next->next = &base;
	allocp->next->size = (memSize - HEAP_START)/PageSize;
}

//sets ammt to the amount of space that was allocated
void* allocHeap(size_t size, size_t *ammt)
{ 
	size_t nUnits = ((size + sizeof(Header)) + PageSize - 1)/PageSize;
	Header *prev = allocp;
	Header *cur, *temp;
	for(cur = prev->next;; prev = cur, cur = cur->next) {
		if(cur->size >= nUnits) {
			if(cur->size == nUnits) {
				prev->next = cur->next;
			} else {
				temp = cur + nUnits*PageSize/sizeof(Header);
				temp->size = cur->size - nUnits;
				temp->next = cur->next;
				prev->next = temp;
				cur->size = nUnits;
				*ammt = cur->size*PageSize - sizeof(Header);
			}
			allocp = prev;
			intptr_t ptr;
			for(ptr = (intptr_t)cur; (ptr - (intptr_t)cur)/PageSize < nUnits; ptr += PageSize) {
				setStorageKey(ptr, PROGSK, 1);
			}
			return (void*)(cur + 1);
		} else if(cur == allocp) { //We went back to the start...
			return NULL;
		}
	}
}

void freeHeap(void *ptr)
{
	Header *toFree = (Header *)ptr - 1;
	Header *scan;
	for(scan = allocp; !(toFree > scan && toFree < scan->next); scan = scan->next)
		if(scan->next <= scan && (toFree > scan || toFree < scan->next)) break;
	toFree->next = scan->next;
	scan->next = toFree;
	if(scan + scan->size == toFree) {
		scan->size += toFree->size;
		scan->next = toFree->next;
		toFree = scan;
	}
	if(toFree + toFree->size == toFree->next) {
		toFree->size += toFree->next->size;
		toFree->next = toFree->next->next;
	}
}