#include "PROJECT_INCLUDES.H"

#include <stdarg.h>
void (*xfunc_out)(unsigned char);
static char *outptr;

	
void xputc(char c)
{
	// CR->CRLF
	if(_CR_CRLF && c == '\n') Uart0_Putchar('\r');

	if(outptr)
	{
		*outptr++ = (unsigned char)c;
		return;
	}
	Uart0_Putchar(c);
	//if(xfunc_out) xfunc_out((unsigned char)c);
}
/*  org
void xputc(char c)
{
	// CR->CRLF
	if(_CR_CRLF && c == '\n') xputc('\r');

	if(outptr)
	{
		*outptr++ = (unsigned char)c;
		return;
	}

	if(xfunc_out) xfunc_out((unsigned char)c);
}
*/
void xputs(const char* str)
{
	while (*str)
		xputc(*str++);
}

void xfputs(void(*func)(unsigned char),	const char*	str)
{
	void (*pf)(unsigned char);

	pf = xfunc_out;
	xfunc_out = func;
	while (*str)
		xputc(*str++);
	xfunc_out = pf;
}


//----------------------------------------------
// Formatted string output                      
//----------------------------------------------
//  xprintf("%d", 1234);			"1234"
//  xprintf("%6d,%3d%%", -200, 5);	"  -200,  5%"
//  xprintf("%-6u", 100);			"100   "
//  xprintf("%ld", 12345678L);		"12345678"
//  xprintf("%04x", 0xA3);			"00a3"
//  xprintf("%08LX", 0x123ABC);		"00123ABC"
//  xprintf("%016b", 0x550F);		"0101010100001111"
//  xprintf("%s", "String");		"String"
//  xprintf("%-4s", "abc");			"abc "
//  xprintf("%4s", "abc");			" abc"
//  xprintf("%c", 'a');				"a"
//  xprintf("%f", 10.0);            <xprintf lacks floating point support>
void xvprintf (const char* fmt, va_list arp)
{
	unsigned int r, i, j, w, f;
	unsigned long v;
	char s[16], c, d, *p;


	for(;;)
	{
		c = *fmt++;
		if(!c) 
			break;

		if(c != '%')
		{
			xputc(c); 
			continue;
		}
		f = 0;
		c = *fmt++;	
		if(c == '0')
		{
			f = 1; 
			c = *fmt++;
		} else 
		{
			if (c == '-')
			{	
				f = 2; 
				c = *fmt++;
			}
		}
		for(w = 0; c >= '0' && c <= '9'; c = *fmt++)
			w = w * 10 + c - '0';

		if(c == 'l' || c == 'L')
		{
			f |= 4; c = *fmt++;
		}

		if(!c)
			break;

		d = c;
		if (d >= 'a') d -= 0x20;
		switch(d) 
		{
			case 'S' :
				p = va_arg(arp, char*);
				for(j = 0; p[j]; j++);
				while(!(f & 2) && j++ < w)xputc(' ');
				xputs(p);
				while(j++ < w)xputc(' ');
				continue;
			case 'C' :
				xputc((char)va_arg(arp, int)); continue;
			case 'B' : r = 2;  break;
			case 'O' : r = 8;  break;
			case 'D' :
			case 'U' : r = 10; break;
			case 'X' : r = 16; break;
			default:
				xputc(c); continue;
		}

		v = (f & 4) ? va_arg(arp, long) : ((d == 'D') ? \
			(long)va_arg(arp, int) : (long)va_arg(arp, unsigned int));

		if (d == 'D' && (v & 0x80000000))
		{
			v = 0 - v;
			f |= 8;
		}
		i = 0;
		do 
		{
			d = (char)(v % r); v /= r;
			if (d > 9) d += (c == 'x') ? 0x27 : 0x07;
			s[i++] = d + '0';

		}while (v && i < sizeof(s));

		if(f & 8) 
			s[i++] = '-';

		j = i; 
		d = (f & 1) ? '0' : ' ';

		while(!(f & 2) && j++ < w) 
			xputc(d);

		do xputc(s[--i]); while(i);
		while(j++ < w) xputc(' ');
	}
}

void xprintf(const char* fmt, ...)
{
	va_list arp;

	va_start(arp, fmt);
	xvprintf(fmt, arp);
	va_end(arp);
}

void xsprintf(char* buff, const char*fmt, ...)
{
	va_list arp;

	outptr = buff;

	va_start(arp, fmt);
	xvprintf(fmt, arp);
	va_end(arp);

	*outptr = 0;
	outptr = 0;
}

