#include <stdio.h>
#include <stdarg.h>
#include <stdbool.h>
#include <stdlib.h>
#include <string.h>
#include "core_uart_apb.h"
#include "osal.h"
extern UART_instance_t g_uart;
#define PRINT_BUF_SZ (512)
void uart_transmit_char(uint8_t uartchar) {

	UART_send(&g_uart, &uartchar, 1);
}


void uart_transmit(uint8_t * in)
{
    int32_t i;
    int32_t str_len = strnlen((char *)in, PRINT_BUF_SZ-1);
    for(i = 0; i < str_len; i++) {
    uart_transmit_char(in[i]);
    }
}


int32_t g5_vsnprintf(char * out, size_t n, const char * s, va_list vl)
{
  bool format = false;
  bool longarg = false;
  size_t pos = 0;
  for( ; *s; s++)
  {
    if(format)
    {
      switch(*s)
      {
        case 'l':
          longarg = true;
          break;
        case 'p':
          longarg = true;
          if (++pos < n) out[pos-1] = '0';
          if (++pos < n) out[pos-1] = 'x';
        case 'x':
        {
          long num = longarg ? va_arg(vl, long) : va_arg(vl, int);
          for(int i = 2*(longarg ? sizeof(long) : sizeof(int))-1; i >= 0; i--) {
            int d = (num >> (4*i)) & 0xF;
            if (++pos < n) out[pos-1] = (d < 10 ? '0'+d : 'a'+d-10);
          }
          longarg = false;
          format = false;
          break;
        }
        case 'd':
        {
          long num = longarg ? va_arg(vl, long) : va_arg(vl, int);
          if (num < 0) {
            num = -num;
            if (++pos < n) out[pos-1] = '-';
          }
          long digits = 1;
          for (long nn = num; nn /= 10; digits++)
            ;
          for (int i = digits-1; i >= 0; i--) {
            if (pos + i + 1 < n) out[pos + i] = '0' + (num % 10);
            num /= 10;
          }
          pos += digits;
          longarg = false;
          format = false;
          break;
        }
        case 's':
        {
          const char* s2 = va_arg(vl, const char*);
          while (*s2) {
            if (++pos < n)
              out[pos-1] = *s2;
            s2++;
          }
          longarg = false;
          format = false;
          break;
        }
        case 'c':
        {
          if (++pos < n) out[pos-1] = (char)va_arg(vl,int);
          longarg = false;
          format = false;
          break;
        }
        default:
          break;
      }
    }
    else if(*s == '%')
      format = true;
    else
      if (++pos < n) out[pos-1] = *s;
  }
  if (pos < n)
    out[pos] = 0;
  else if (n)
    out[n-1] = 0;
  return pos;
}
extern osSemaphore_t       g_print_sem ;

void ts_printf(const char * s, ...)
{
  osSemaphoreTake(g_print_sem,0);
  va_list vl;
  va_start(vl, s);
  uint8_t printbuf[PRINT_BUF_SZ] = { 0 };
  memset(printbuf, 0, PRINT_BUF_SZ);

  UART_send(&g_uart, (const uint8_t *)"\n\r", 2);
  g5_vsnprintf((char *)printbuf, sizeof printbuf, s, vl);
  va_end(vl);
  //taskENTER_CRITICAL();
  uart_transmit(printbuf);
  osSemaphoreGive(g_print_sem);
  //taskEXIT_CRITICAL();



}
