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

#ifdef OSLEVEL
#include <os/heap.h>
#else
#include <prog/svcCalls.h>
#endif

void init_all(u64 __memsize)
{
#ifdef OSLEVEL
	init_io_int();
	init_console();
	heap_init(__memsize);
#endif
}

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 {
#ifdef OSLEVEL
		putline(a, (b - a > CON_LEN)?CON_LEN:(b - a));
#else //problem mode
		printLine(a, (b - a > CON_LEN)?CON_LEN:(b - a));
#endif
		
		a += CON_LEN;
	} while(a < b);
}

char* sGet(char *a, unsigned int n)
{
#ifdef OSLEVEL
	int length = getline(a, n); //TODO supervisor call
#else //problem mode
	int length = readLine(a, n); //TODO supervisor call
#endif

	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(size_t size)
{
	Header *prev, *cur, *temp;
	size_t nUnits = ((size + sizeof(Header)) + sizeof(blockUnit) - 1)/sizeof(blockUnit);
	if(!(prev = allocp)) { //setup malloc if it is the first call to it
		base.size = 0;
		prev = allocp = base.next = &base;
	}
	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*sizeof(blockUnit)/sizeof(Header);
				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...
			cur = moreMem(nUnits);
			if(!cur) return NULL;
		}
	}
}

void* moreMem(size_t blocks)
{
	size_t size = blocks*sizeof(blockUnit);
	size_t allocSize;
	void *block = NULL;
#ifdef OSLEVEL
	block = allocHeap(size, &allocSize);
#else
	size = 0;
	block = NULL;
	sPrint("Program level malloc has not been implemented yet, sorry\n");
#endif
	if(!block) return NULL;
	Header *blockH = block;
	blockH->size = allocSize/sizeof(blockUnit);
	free(blockH + 1);
	return allocp;
}

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;
	}
}