#include <std.h>
#include <string.h>

void init_all(u64 __memsize)
{
	init_io_int();
	init_console();
	malloc_init(__memsize - HEAP_START);
}

double abs(double num)
{
	if(num < 0) return num*-1;
	else return num;
}

char* itoa(s64 n, char *a, unsigned short base)
{
	char *ret = a;
	if(n < 0) {
		*a++ = '-';
		n *= -1;
	}
	char *b = a;
	if(!n) *b++ = '0';
	for(; n; b++) {
		int temp = n%base;
		if(temp < 10) *b = '0' + temp;
		else *b = 'a' + temp - 10;
		n = n/base;
	}
	*b-- = '\0';
	for(; a < b; a++, b--) { //reverse
		char temp = *b;
		*b = *a;
		*a = temp;
	}
	return ret;
}

char* ftoa(double x, char *a, unsigned int prec)
{
	char *ret = a;
	if(x == INF || x == -INF) {
		strcpy(a, "INF");
	} else if(x != x) { //NAN != NAN
		strcpy(a, "NAN");
	} else {
		s64 n = (s64) x; //integer part
		double d = abs(x - (double) n); //fractional part;
		itoa(n, a, 10);
		if(prec) { //only do the decimal part if decimal parts were asked for
			while(*a && *++a); //get to the null character from itoa
			int i; //counter variable for the for loop
			*a++ = '.'; //put the decimal in place
			for(i = 0; i < prec; i++) {
				d *= 10; //the integer part is the decimal digit
				*a++ = ((int) d) + '0'; //add the integer part of d to the string
				d -= (int) d; //chop off the integer part
			} a--; //move back to the last digit
			while(*a != '.') {
				if(*a == '0') {
					a--;
					continue;
				} else if(*a > '0' && *a <= '9') {
					a++;
					break;
				} else {
					sPrint("ERROR: SOMETHING IS VERY WRONG\n");
					break;
				}
			}
			*a = '\0';
		}
	}
	return ret;
}

s64 atoi(char *a)
{
	short neg = 0;
	s64 n = 0;
	if(*a == '-') {
		neg = 1;
		a++;
	} else if(*a == '+') a++;
	while(*a >= '0' && *a <= '9')
		n = n*10 + (*a++ - '0');
	if(neg) n *= -1;
	return n;
}

double atof(char *a)
{
	s64 n = atoi(a);
	double x = 0;
	double dec = .1;
	short neg = 0;
	if(*a == '-') {
		neg = 1;
		a++;
	}
	while(*a != '.') {
		if(!(*a >= '0' && *a <= '9') && *a != '-') return n;
		a++;
	} a++; //a will be immediately after the decimal point
	while(*a >= '0' && *a <= '9') { //goes through the decimal part
		x += (*a - '0')*dec;
		dec *= .1;
		a++;
	}
	if(neg) x*=-1;
	return n + x;
}

void sPrint(char *a)
{
	char *b = a;
	while(*b && *++b);
	do {
		putline(a, (b - a > CON_LEN)?CON_LEN:(b - a));
		a += CON_LEN;
	} while(a < b);
}

char* sGet(char *a, unsigned int n)
{
	int length = getline(a, n);
	a[(length < n)?length:n - 1] = '\0';
	return a;
}

//DYNAMIC MEMORY

static Header base;
static Header *allocp = NULL; //the location of the last known free block

void malloc_init(size_t memSize)
{
	allocp = &base;
	allocp->size = 0;
	allocp->next = (void*)HEAP_START;
	allocp->next->next = &base;
	allocp->next->size = memSize/sizeof(blockUnit);
	if((sizeof(blockUnit)%sizeof(u64))) {
		sPrint("WARNING: MEMORY NOT 8-BYTE ALIGNED\n");
	}
}

void* malloc(size_t size)
{
	if(allocp == NULL) return NULL;
	size_t nUnits = ((size + sizeof(Header)) + sizeof(blockUnit) - 1)/sizeof(blockUnit);
	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; //This requires sizeof(blockUnit) == sizeof(Header).  TODO fix
				temp->size = cur->size - nUnits;
				temp->next = cur->next;
				prev->next = temp;
				cur->size = nUnits;
			}
			allocp = prev;
			return (void*)(cur + 1);
		} else if(cur == allocp) { //We went back to the start...
			return NULL;
		}
	}
}

void free(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;
	}
}

void printHeader(Header* head)
{
	char buffer[20];
	sPrint("Memory address is: ");
	sPrint(itoa((intptr_t)head, buffer, 16));
	sPrint("\n");
	sPrint("The size of the block is: ");
	sPrint(itoa(head->size, buffer, 10));
	sPrint("\n");
	sPrint("Next memory address is: ");
	sPrint(itoa((intptr_t)head->next, buffer, 16));
	sPrint("\n\n");
}

void printMem()
{
	Header *cur = allocp;
	do {
		printHeader(cur);
		cur = cur->next;
	} while(cur != allocp);
	sPrint("PRINTOUT DONE\n\n");
}