#include "DebugPrint.h"
#include "buffer.h"
#include "Application.h"
//#include "rs232DB.h"

#ifdef NCI_DEBUG_OUTPUT
  #include "CommunicationHandler.h"
#endif

#ifdef _DEBUG_PRINT_ON
  #ifdef NCI_DEBUG_OUTPUT
    static Array<258> printfbuf;
  #else
    static Buffer<word,2150> printfbuf;
  #endif

  #include "MinimalLibrary.h"
#else
  static Buffer<word, 1> printfbuf;
#endif //_DEBUG_PRINT_ON


bool DebugPrint::hexDump = false;

void DebugPrint::Init()
{
  #ifdef NCI_DEBUG_OUTPUT
    printfbuf.Init();
  #else
    printfbuf.flush();
  #endif
}

word DebugPrint::Read(byte *buffer, word len)
{
#ifndef NCI_DEBUG_OUTPUT
  word rlen =  printfbuf.Read((void *)buffer,len);
  return rlen;
#else
  return 0;
#endif
}

void _putc(char x) //exports to printf
{
  #ifdef NCI_DEBUG_OUTPUT
    printfbuf.data[printfbuf.len++] = x;
  #else
    //Rs232DB::putc(x);
    //printfbuf.Write((byte *)&x,1);
  #endif
}

#ifdef NCI_DEBUG_OUTPUT
void DebugPrint::setNtf()
{
  printfbuf.data[2] = printfbuf.len - 3; //set the payload length
  CommunicationHandler::sendCommand(printfbuf.data, printfbuf.len);
  printfbuf.Init();
}
#endif

void format(char *buf, unsigned short x, unsigned char base, unsigned char digits );
unsigned char strlen(char *s);

extern "C"
{
#include "stdarg.h"

  void DebugPrint::Write(char *fmt,...)
  {
#ifdef _DEBUG_PRINT_ON

#ifdef NCI_DEBUG_OUTPUT
if(hexDump == false) {
  _putc(0x60); //Header
  _putc(0x3E); //Parsing indication: 0x3F -> String, 0x3E -> HexDump
  _putc(0x00); //length, will be set at setNtf
}
#endif


    static char buf[17]; //length needed by 16 bit binary output
    short int x;
    bool zeropadding;
    unsigned char width;
    va_list va;
    va_start(va,fmt);
    while(1)
    {
      char c;
      c=*fmt++;
      if(c==0) break;
      if(c != '%') {_putc(c); continue;} //format string output
      //number output
      c=*fmt++;
      zeropadding=false; width=0;
      if(c=='0') {zeropadding=true; c=*fmt++;}; //zero padding
      if((c <= '9')&&(c >= '0')) {width=c-'0'; c=*fmt++;}; //field width
      switch(c)   //format specifieres
      {
      case 'i': c='d';  //integer output
      case 'd':
      case 'u':
        x=va_arg(va,short int); //get 16 bit argument
        if((c=='d')&&(x < 0)) {_putc('-'); x=-x;} //signed number ?
        format(buf,x,10,5);    // format as a decimal; 5 digit for 16 bit number
print_number:
        {
          unsigned char i, w;
          i=0;
          while(buf[i]=='0') i++; //remove leading zeros
          if(buf[i]==0) i--; //if x==0 do not remove 0
          //adjust width and zero padding
          w=strlen(buf)-i;
          if(width > w) width-=w;
          else width=0;
          if(zeropadding) while(width-- !=0) _putc('0');
          else while(width-- !=0) _putc(' ');
          //output the number
          while(buf[i]) _putc(buf[i++]);
          break;
        }
#ifdef long_printf
      case 'l': //long
        {
          unsigned long x;
          c=*fmt++;  // expecting d,u or x
          x=va_arg(va,long int); //get 32 bit argument
          if(c=='d') if(x&0x80000000) {_putc('-'); x=~x; x++; } //2s complement if sign bit is set
          if((c=='d')||(c=='u'))
          {
            format(buf, (word)(x/1000000L),10,4);  // upper 4 digits
            x%=1000000L;
            format(buf+4,(word)(x/1000L),10,3); //mid 3 digits
            format(buf+7,(word)(x%1000L),10,3); //low 3 digits
            goto print_number;
          }
          if(c=='x') //long hex
          {
            format(buf,(word)(x>>16),16,4); //upper 4 digits
            format(buf+4,(word)(x & 0xFFFF),16,4); //lower 4 digits
            goto print_number;
          }
          if(c=='b')  //long binary; no zero padding or with spec supported
          {
            byte i;
            format(buf,(word)(x>>16),2,16); //upper 16 digits
            i=0; while(buf[i]) _putc(buf[i++]);
            format(buf,(word)(x & 0xFFFF),2,16); //lower 16 digits
            i=0; while(buf[i]) _putc(buf[i++]);
            break;
          }
        }
#endif //long_printf
      case 'x':  //hexadecimal output
      case 'X':
        x=va_arg(va,short int);
        format(buf,x,16,4);  //format as hexadecimal; 4 digit for 16 bit number
        goto print_number;
      case 'b':  //binary output
        x=va_arg(va,short int);
        format(buf,x,2,16);  //format as binary; 16 digit for 16 bit number
        goto print_number;
      case 's':  //string output
        {
          char *p;
          p=va_arg(va,char *);
          while(*p) _putc(*p++);
          break;
        }
      case 'c': //character output
        _putc(va_arg(va,short int));
        break;
      }
    }
    va_end(va);

  #ifdef NCI_DEBUG_OUTPUT
    if(hexDump == false) {
      setNtf();
    }
  #endif

#endif //_DEBUG_PRINT_ON
  }


} //extern "C"


//convert unsigned to string, number base f.e. 2, 10 or 16
void format(char *buf, unsigned short x, unsigned char base, unsigned char digits )
{
  unsigned char k;
  loop(k,digits)
  {
    char c;
    c='0'+x%base;
    if(c > '9') c+='A'-'9'-1;
    buf[digits-k-1]=c;
    x/=base;
  }
  buf[digits]=0;  //append zero
}


unsigned char strlen(char *s) //length of 0 terminated string
{
  unsigned char i=0;
  while(*s++) i++;
  return i;
}

void DebugPrint::Hex(byte *buf, word len)
{
#ifdef NCI_DEBUG_OUTPUT
  if(len > 0xFF) { return; }

  _putc(0x60); //Header
  _putc(0x3F); //Parsing indication: 0x3F -> String, 0x3E -> HexDump
  _putc(0x00); //length, will set at setNtf()
  hexDump = true;

  while(len--) {
    _putc(*buf++);
  }

  setNtf();
  hexDump = false;
  return;
#else
  while(len--) Write("%02x ",*buf++);
  Write("\n");
  return;
#endif
}

bool DebugPrint::overflow() {
#ifndef NCI_DEBUG_OUTPUT
  return printfbuf.overflow();
#else
  return false;
#endif
}
