#include "AT91RM9200.h"
#include "def.h"
#include "config.h"
#include "console.h"
#include "utils.h"

extern volatile U32 StTick;
void delay(U32 ms)
{
	U16 i;
	
	i = AT91C_BASE_TC0->TC_RC>>1;	//1000us/2
	
	AT91C_BASE_TC0->TC_CCR = 4;	//復位為0,加計數
	
	while(ms--) {
		while(AT91C_BASE_TC0->TC_CV<=i);
		while(AT91C_BASE_TC0->TC_CV>i);
	}
}

void putchar(char c)
{	
	if(c=='\n') {
		while(!AT91F_US_TxReady((AT91PS_USART)AT91C_BASE_DBGU));
		AT91F_US_PutChar((AT91PS_USART)AT91C_BASE_DBGU, '\r');
		delay(1);
	}
	while(!AT91F_US_TxReady((AT91PS_USART)AT91C_BASE_DBGU));
	AT91F_US_PutChar((AT91PS_USART)AT91C_BASE_DBGU, c);
//	delay(1);
}

int getchar(void)
{
	while(!kbhit());
	return AT91F_US_GetChar((AT91PS_USART)AT91C_BASE_DBGU);
}

//*----------------------------------------------------------------------------
//* \fn    puts
//* \brief This function is used to send a string through the DBGU channel (Very low level debugging)
//*----------------------------------------------------------------------------
void puts(char *buffer) // \arg pointer to a string ending by \0
{	
	while(*buffer != '\0') {		
		putch(*buffer++);
	}
}

int getyorn(void)
{
	char c;
	
	puts(" [y/n] ");
	while(1) {		
		c = getch();
		if((c=='y')||(c=='Y')||(c=='n')||(c=='N'))
			break;
	}
	putch(c);
	putch('\n');
	return c&1;	//y&Y are odd, n&N are even
}

#if	1	//#ifdef	SELF_BOOT
/*---------------------printf and support routines ---------------------*/
/* print c count times */
void PutRepChar(char c, int count)
{
  while (count--) putchar(c);
}

/* put string reverse */
void PutStringReverse(char *s, int index)
{
  while ((index--) > 0) putchar(s[index]);
}
/*
typedef struct {
	U32 q;
	U16 r;
}u_div_e;

static u_div_e u_div(U32 value, U16 radix)
{
	u_div_e e = {0, 0};
	
	while(value>=radix) {
		value -= radix;
		e.q++;
	}
	e.r = value;
	
	return e;	
}*/
/*-------------------------------------------------------------------------*/
/* 
   prints value in radix, in a field width width, with fill
   character fill
   if radix is negative, print as signed quantity
   if width is negative, left justify
   if width is 0, use whatever is needed
   if fill is 0, use ' '
 */

static void PutNumber(int value, int radix, int width, char fill)
{
  char buffer[40];
  int bi = 0;
  int unsigned uvalue;
  short int digit;
  short int left = FALSE;
  short int negative = FALSE;

  if (fill == 0) fill = ' ';

  if (width < 0) {
    width = -width;
    left = TRUE;
  }
  if (width < 0 || width > 80) width = 0;
  
  if (radix < 0) {
    radix = -radix;
    if (value < 0) {
      negative = TRUE;
      value = -value;
    }
  }
  uvalue = value;
  do {
    if (radix != 16) {
      digit = uvalue % radix ;
      uvalue = uvalue / radix ;
//      u_div_e result;
      
//      result = u_div(uvalue, radix);
//      digit  = result.r;
//      uvalue = result.q;
    }
    else {
      digit = uvalue & 0xf;
      uvalue = uvalue >> 4;
    }
    buffer[bi] = digit + ((digit <= 9) ? '0' : ('A' - 10));
    bi++;

    if (uvalue != 0) {
      if ((radix==10)&&((bi==3)||(bi==7)||(bi==11)|(bi==15))) {
	buffer[bi++]=',';
      }
    }
  } while (uvalue != 0);

  if (negative) {
    buffer[bi] = '-';
    bi += 1;
  }
  if (width <= bi) PutStringReverse(buffer, bi);
  else {
    width -= bi;
    if (!left) PutRepChar(fill, width);
    PutStringReverse(buffer, bi);
    if (left) PutRepChar(fill, width);
  } 
}

/*-------------------------------------------------------------------------*/
static char *FormatItem(char *f, int a)
{
  char c;
  int fieldwidth = 0;
  int leftjust = FALSE;
  int radix = 0;
  char fill = ' ';

  if (*f == '0') fill = '0';
  while ((c = *f++)!=0) {
    if (c >= '0' && c <= '9') {
      fieldwidth = (fieldwidth * 10) + (c - '0');
    }
    else switch (c) {
    case '\000': return(--f);
    case '%': putchar('%');
      return(f);
    case '-': leftjust = TRUE;
      break;
    case 'c': {
      if (leftjust) putchar(a & 0x7f);
      if (fieldwidth > 0) PutRepChar(fill, fieldwidth - 1);
      if (!leftjust) putchar(a & 0x7f);
      return(f);
    }
    case 's': {
      if (leftjust) PutString((char *) a);
      if (fieldwidth > strlen((char *) a))
	PutRepChar(fill, fieldwidth - strlen((char *)a));
      if (!leftjust) PutString((char *) a);
      return(f);
    }
    case 'd': 
    case 'i': radix = -10;break;
    case 'u': radix = 10;break;
    case 'x': radix = 16;break;
    case 'X': radix = 16;break;
    case 'o': radix = 8;break;
    default : radix = 3;break;/* unknown switch! */
    }
    if (radix) break;
  }
  if (leftjust) fieldwidth = -fieldwidth;
  PutNumber(a, radix, fieldwidth, fill);
  return(f);
}

#define vaStart(list, param) list = (char*)((int)&param + sizeof(param))
#define vaArg(list, type) ((type *)(list += sizeof(type)))[-1]
#define vaEnd(list)

void printf(char *f, ...)       /* variable arguments */
{
//  U32 mode ;
  char *argP;

  /* disable IRQs and FIQs */
//  mode = uHALir_ReadMode() ;
//  uHALir_WriteMode(mode | NoFIQ | NoIRQ) ;

  vaStart(argP,f);		/* point at the end of the format string */
  while (*f) {			/* this works because args are all ints */
    if (*f == '%') f = FormatItem(f + 1, vaArg(argP, int));
    else putchar(*f++);
  }
  vaEnd(argP);

  /* restore the previous mode */
//  uHALir_WriteMode(mode) ;
}

#else

#include <stdarg.h>
#include <string.h>
#include <stdlib.h>

//If you don't use vsprintf(), the code size is reduced very much.
typedef int *__va_list[1];
int vsprintf(char * /*s*/, const char * /*format*/, __va_list /*arg*/);

void printf(char *fmt,...)
{
	va_list ap;
	char string[256];
	
	va_start(ap,fmt);
	vsprintf(string,fmt,ap);
	puts(string);
	va_end(ap);
}

#endif