/*
   Copyright (c) 2007-2013, John Ryland
 */
#include <oslayer.h>
#include <corelayer.h>


void memCopy(char *dst, const char *src, int len)
{
    int i;
    if (dst && src) {
        // TODO: Could step this up to do 64bits at a time
        if (((long)dst & 3) == ((long)src & 3)) {
            // Bring dst and src in to 4 byte alignment
            while (((long)dst & 3) != 0 && len) {
                *dst = *src;
                dst++;
                src++;
                len--;
            }
            for (i = 0; i < len/4; i++)
                ((int*)dst)[i] = ((int*)src)[i];
            for (i = i*4; i < len; i++)          // addresses for both dst and src in general
                dst[i] = src[i];
        } else {
            for (i = 0; i < len; i++)
                dst[i] = src[i];
        }
    }
}


void* memcpy(char *dst, const char *src, int len)
{
  memCopy(dst, src, len);
  return dst;
}


// Some GCC versions appear to generate code which calls this
void* memmove(char *dst, const char *src, int len)
{
  int i;
  if (dst == src)
    return dst;
  if (dst > src) {
    memCopy(dst, src, len);
  } else {
	if (dst && src) {
      // TODO: Could optimize this to copy more than a byte at a time
      for (i = 0; i < len; i++)
        dst[len-1-i] = src[len-1-i];
    }
  }
  strPrint("memmove called!\n");
  return 0;
}


int strCompare(const char *str1, const char *str2)
{
	int i = 0;
	if (!str1 || !str2)
		return -1;
	while (str1[i] && str2[i]) {
		if (str1[i] != str2[i])
			return str1[i] - str2[i];
		i++;
	}
	return str1[i] - str2[i];
}


int strLength(const char *str)
{
	int i = 0;
	if (str)
		while (str[i])
			i++;
	return i;
}


// GCC required code
unsigned __udivmodsi4(unsigned num, unsigned den, unsigned *rem_p)
{
	unsigned quot = 0, qbit = 1;

	if ( den == 0 ) {
		strPrint("Divide by 0\n");
		//__divide_error();
		return 0;
	}

#define UNTESTED_CODE_PATH
#ifdef UNTESTED_CODE_PATH
	// XXX we are assuming den is 32bits here
	unsigned int shift;
	qbit = 0;
	shift  = (den & 0xffff0000) ? 0 : 16; den <<= shift; qbit |= shift;
	shift  = (den & 0xff000000) ? 0 :  8; den <<= shift; qbit |= shift;
	shift  = (den & 0xc0000000) ? 0 :  2; den <<= shift; qbit |= shift;
	shift  = (den & 0x80000000) ? 0 :  1; den <<= shift; qbit |= shift;
	qbit = 1 << qbit;
	
#else
	// No assumption about den size, but assuming 2s complement
	while ( (int)den >= 0 ) {
		den <<= 1;
		qbit <<= 1;
	}
#endif

	while ( qbit ) {
		if ( den <= num ) {
			num -= den;
			quot += qbit;
		}
		den >>= 1;
		qbit >>= 1;
	}

	if ( rem_p )
		*rem_p = num;

	return quot;
}


// GCC required code
unsigned __udivsi3(unsigned num, unsigned den)
{
	return __udivmodsi4(num, den, 0);
}


// GCC required code
unsigned __umodsi3(unsigned num, unsigned den)
{
	unsigned v;
	(void) __udivmodsi4(num, den, &v);
	return v;
}


// GCC required code
void __stack_chk_fail(void)
{
	sysExit(-1);
}


// GCC required code (on MacOSX)
void __stack_chk_guard(void)
{
    // No idea what this function ought to do
}


// Bad, not reentrant
// What if base is > 16 ?
char strTmpBuf[64];
char *strInteger(unsigned num, unsigned base)
{
	const char *lookup = "0123456789ABCDEF";
	int i = 1;
	strTmpBuf[63] = 0;
	do {
		strTmpBuf[63-i] = lookup[num % base];
		num /= base;
		i++;
	} while (num);
	return &(strTmpBuf[63-i+1]);
}


void strPrint(const char *str)
{
	//if (str)
	//	fsWrite(STDOUT_FILENO, str, strLength(str));

	if (str) {
		char strBuf[1024];
		int i = 0;
		while (*str) {
			if ( *str == '\n' )
				strBuf[i++] = '\r';
			strBuf[i++] = *str++;
			if ( i == 1022 )
				fsWrite(STDOUT_FILENO, strBuf, 1024), i = 0;
		}
		fsWrite(STDOUT_FILENO, strBuf, i);
	}
}


void strNPrint(const char *str, int n)
{
	//if (str && n)
	//	fsWrite(STDOUT_FILENO, str, n);

	if (str && n) {
		char strBuf[1024];
		int i = 0;
		while (n--) {
			if ( *str == '\n' )
				strBuf[i++] = '\r';
			strBuf[i++] = *str++;
			if ( i == 1022 )
				fsWrite(STDOUT_FILENO, strBuf, 1024), i = 0;
		}
		fsWrite(STDOUT_FILENO, strBuf, i);
	}
}


/*
	// Currently supported format identifiers

		%d %i     Decimal signed integer.
		%o        Octal integer.
		%x %X     Hex integer.
		%u        Unsigned integer.
		%c        Character.
		%s        String. See below.
		//    %f        double
		//    %e %E     double.
		//    %g %G     double.
		%p        pointer.
		%n        Number of characters written by this printf.
		No argument expected.
		%%        %. No argument expected.
 */
#define PRINTF_BUF_SIZE	    2048


#ifdef ARCH_MacOSX64
#  include <stdarg.h>
#  define my_va_list                    va_list
#  define my_va_start(args, first)      va_start(args, first)
#  define my_va_arg(args, type)         va_arg(args, type)
#else
#  define my_va_list                    char*
#  define my_va_start(args, first)      args = (char*)&first + sizeof(first)
#  define my_va_arg(args, type)         *(type*)(args); args += sizeof(type)
#endif


char outputBuffer[PRINTF_BUF_SIZE]; // For some reason this needs to be out here
// Some how when it is on the stack it goes out of scope before it should. Weird
void strPrintf(const char *str, ...)
{
	int i = 0, x = 0;
    my_va_list ap;
    my_va_start(ap, str);

	while (str[i]) {
		if (str[i] == '%') {
			i++;
			if (str[i] == '%') {
				outputBuffer[x] = str[i];
				x++;
			} else if (str[i] == 'i' || str[i] == 'd' || str[i] == 'u' || str[i] == 'o' ||
					str[i] == 'x' || str[i] == 'X' || str[i] == 'p' || str[i] == 'n' ||
					str[i] == 's')
			{
				int num, base = 10, t = 0;
				char *tmp;
				if (str[i] != 's') {
					if (str[i] == 'n') {
						num = x;
					} else {
						num = my_va_arg(ap, int);
					}
					if (str[i] == 'o') {
						outputBuffer[x] = '0'; x++;
						base = 8;
					}
					if (str[i] == 'x' || str[i] == 'X') {
						outputBuffer[x] = '0'; x++;
						outputBuffer[x] = str[i]; x++;
						base = 16;
					}
					if (str[i] == 'p') {
						outputBuffer[x] = '('; x++;
						outputBuffer[x] = '0'; x++;
						outputBuffer[x] = 'X'; x++;
						base = 16;
					}
					tmp = strInteger(num, base);
				} else {
				    tmp = my_va_arg(ap, char*);
				}
				while (tmp[t]) {
					outputBuffer[x] = tmp[t];
					t++;
					x++;
				}
				if (str[i] == 'p') {
					outputBuffer[x] = ')'; x++;
				}
			} else if (str[i] == 'c') {
				int ch = my_va_arg(ap, int);
				outputBuffer[x] = (char)ch;
				x++;
			} else {
				// unrecognized / unsupported format identifier
			}
		} else {
			outputBuffer[x] = str[i];
			x++;
		}

		// Flush what is buffered so far
		if ( x >= (PRINTF_BUF_SIZE / 2) ) {
			strNPrint(outputBuffer, x);
			x = 0;
		}

		i++;
	}

	strNPrint(outputBuffer, x);
}