#include "archinc.h"
#include "uart0.h"
#include "logmsg.h"

static char msgQueue[CONFIG_LOGMSG_QUEUE_SIZE];
static int msgHead = 0;
static int msgTail = 0;
static int msgLen = 0;
static int msgMaxLen = 0;

char bcdToHex[16] = {'0','1','2','3','4','5','6','7','8','9','a','b','c','d','e','f'};

int logmsgInit(void) 
{
  return 0;
}


static inline int putOne(char putCh)
{
  if (msgLen == CONFIG_LOGMSG_QUEUE_SIZE)
    return -1;

  msgQueue[msgHead] = putCh;
  msgLen++;
  msgMaxLen = ((msgLen > msgMaxLen) ? msgLen : msgMaxLen);
  circInc(msgHead, 0, (CONFIG_LOGMSG_QUEUE_SIZE-1));


  return 0;
}

int logmsgPrintBcd8(char * pPrefixStr, char bcdCh)
{
  int i;
  int retStat;
  char * pCh = pPrefixStr;
  char upperNib;

  if (pPrefixStr != NULL) {
    // output the prefix string
    retStat = putOne(*pCh++);
    while((retStat != -1) && (*pCh != '\0')) {
      putOne(*pCh++);
      } 
    }

  upperNib = ((bcdCh >> 4) & 0x0f);
  bcdCh &= 0x0f;

  if (putOne('0') == -1)
    return  -1;

  if (putOne('x') == -1)
    return  -1;

  if (putOne(bcdToHex[upperNib]) == -1)
    return  -1;

  if (putOne(bcdToHex[bcdCh]) == -1)
    return  -1;

  if (putOne('\r') == -1)
    return  -1;

  if (putOne('\n') == -1)
    return  -1;

  return 0;
}

int logmsgPut(char * pPutMsg, int numChars)
{
  int retVal;

  if (pPutMsg == NULL)
    return -1;

  if (numChars == -1) {
    numChars = CONFIG_LOGMSG_QUEUE_SIZE - msgLen;
    }

  retVal = 0;

  while ((retVal != -1) && (*pPutMsg != '\0') && (numChars > 0)) {
    retVal = putOne(*pPutMsg++);
    numChars--;
    }

  return retVal;
}

void logmsgPoll(void)
{
  while (msgLen > 0) {
    uart0TxOne(msgQueue[msgTail]);
    circInc(msgTail, 0, (CONFIG_LOGMSG_QUEUE_SIZE-1));
    msgLen--;
    }
}