/**
 * @brief   Reads a whole line from the input channel.
 *
 * @param[in] chp       pointer to a @p BaseSequentialStream object
 * @param[in] line      pointer to the line buffer
 * @param[in] size      buffer maximum length
 * @return              The operation status.
 * @retval true         the channel was reset or CTRL-D pressed.
 * @retval false        operation successful.
 *
 * @api
 */
bool shellGetLine(BaseSequentialStream *chp, char *line, unsigned size) {
  char *p = line;

  while (true) {
    char c;

    if (chSequentialStreamRead(chp, (uint8_t *)&c, 1) == 0)
      return true;
    if (c == 4) {
      chprintf(chp, "^D");
      return true;
    }
    if ((c == 8) || (c == 127)) {
      if (p != line) {
        chSequentialStreamPut(chp, c);
        chSequentialStreamPut(chp, 0x20);
        chSequentialStreamPut(chp, c);
        p--;
      }
      continue;
    }
    if (c == '\r') {
      chprintf(chp, "\r\n");
      *p = 0;
      return false;
    }
    if (c < 0x20)
      continue;
    if (p < line + size - 1) {
      chSequentialStreamPut(chp, c);
      *p++ = (char)c;
    }
  }
}

static void printn(uint32_t n) {
  char buf[16], *p;

  if (!n)
    chSequentialStreamPut(&SD1, '0');
  else {
    p = buf;
    while (n)
      *p++ = (n % 10) + '0', n /= 10;
    while (p > buf)
      chSequentialStreamPut(&SD1, *--p);
  }
}

static void print_line(void) {
  unsigned i;

  for (i = 0; i < 76; i++)
    chSequentialStreamPut(chp, '-');
  chSequentialStreamWrite(chp, (const uint8_t *)"\r\n", 2);
}

static THD_FUNCTION(thSerEcho, arg)
{
  (void)arg;
  chRegSetThreadName("SerEcho");
  event_listener_t elSerData;
  eventflags_t flags;
  chEvtRegisterMask((event_source_t *)chnGetEventSource(&SD1), &elSerData, EVENT_MASK(1));

  while (!chThdShouldTerminateX())
  {
     chEvtWaitOneTimeout(EVENT_MASK(1), MS2ST(10));
     flags = chEvtGetAndClearFlags(&elSerData);
     if (flags & CHN_INPUT_AVAILABLE)
     {
        msg_t charbuf;
        do
        {
           charbuf = chnGetTimeout(&SD1, TIME_IMMEDIATE);
           if ( charbuf != Q_TIMEOUT )
           {
             chSequentialStreamPut(&SD1, charbuf);
           }
        }
        while (charbuf != Q_TIMEOUT);
     }
  }
}

static void cmd_gps_passthrough(BaseSequentialStream *chp, int argc, char *argv[]) {
    (void)argc;
    (void)argv;
    static const SerialConfig sc = {
        9600, 0, USART_CR2_STOP1_BITS | USART_CR2_LINEN, 0};
    sdStart(&SD1, &sc);
    EventListener elSerData;
    flagsmask_t flags;
    chEvtRegisterMask(chnGetEventSource(&SD1), &elSerData, EVENT_MASK(1));

    while (TRUE)
    {
       chEvtWaitOneTimeout(EVENT_MASK(1), MS2ST(10));
       flags = chEvtGetAndClearFlags(&elSerData);
       if (flags & CHN_INPUT_AVAILABLE)
       {
          msg_t charbuf;
          do
          {
             charbuf = chnGetTimeout(&SD1, TIME_IMMEDIATE);
             if ( charbuf != Q_TIMEOUT )
             {
               chSequentialStreamPut(chp, charbuf);
             }
          }
          while (charbuf != Q_TIMEOUT);
       }
    }
}

static void println(char *p) {

  while (*p) {
    chSequentialStreamPut(&SD1, *p++);
  }
  chSequentialStreamWrite(&SD1, (uint8_t *)"\r\n", 2);
}

/**
 * @brief   System formatted output function.
 * @details This function implements a minimal @p vprintf()-like functionality
 *          with output on a @p BaseSequentialStream.
 *          The general parameters format is: %[-][width|*][.precision|*][l|L]p.
 *          The following parameter types (p) are supported:
 *          - <b>x</b> hexadecimal integer.
 *          - <b>X</b> hexadecimal long.
 *          - <b>o</b> octal integer.
 *          - <b>O</b> octal long.
 *          - <b>d</b> decimal signed integer.
 *          - <b>D</b> decimal signed long.
 *          - <b>u</b> decimal unsigned integer.
 *          - <b>U</b> decimal unsigned long.
 *          - <b>c</b> character.
 *          - <b>s</b> string.
 *          .
 *
 * @param[in] chp       pointer to a @p BaseSequentialStream implementing object
 * @param[in] fmt       formatting string
 * @param[in] ap        list of parameters
 * @return              The number of bytes that would have been
 *                      written to @p chp if no stream error occurs
 *
 * @api
 */
int chvprintf(BaseSequentialStream *chp, const char *fmt, va_list ap) {
  char *p, *s, c, filler;
  int i, precision, width;
  int n = 0;
  bool is_long, left_align;
  long l;
#if CHPRINTF_USE_FLOAT
  float f;
  char tmpbuf[2*MAX_FILLER + 1];
#else
  char tmpbuf[MAX_FILLER + 1];
#endif

  while (TRUE) {
    c = *fmt++;
    if (c == 0)
      return n;
    if (c != '%') {
      chSequentialStreamPut(chp, (uint8_t)c);
      n++;
      continue;
    }
    p = tmpbuf;
    s = tmpbuf;
    left_align = FALSE;
    if (*fmt == '-') {
      fmt++;
      left_align = TRUE;
    }
    filler = ' ';
    if (*fmt == '0') {
      fmt++;
      filler = '0';
    }
    width = 0;
    while (TRUE) {
      c = *fmt++;
      if (c >= '0' && c <= '9')
        c -= '0';
      else if (c == '*')
        c = va_arg(ap, int);
      else
        break;
      width = width * 10 + c;
    }
    precision = 0;
    if (c == '.') {
      while (TRUE) {
        c = *fmt++;
        if (c >= '0' && c <= '9')
          c -= '0';
        else if (c == '*')
          c = va_arg(ap, int);
        else
          break;
        precision *= 10;
        precision += c;
      }
    }

static size_t chstream_write(void *ip, const void *data, size_t size) {
	BaseSequentialStream *chp = (BaseSequentialStream *) ip;
	int cr_send = 0;
	int counter = 0;
	while (counter < size) {
		char *ptr = (char*) data;
#if 1
		if (ptr[counter] == '\r')
			cr_send = 1;
		else if (ptr[counter] == '\n' && !cr_send)
			chSequentialStreamPut(chp, '\r');
		else
			cr_send = 0;
#endif
		if (chSequentialStreamPut(chp, ptr[counter]) != Q_OK)
			return counter;
		counter++;
	}
	return counter;
}

int _write_r(struct _reent *r, int file, char * ptr, int len)
{
    (void)r;
    (void)file;
    (void)ptr;
    while(len) {
        chSequentialStreamPut(&SDU2, *ptr);
        ptr++;
        len--;
    }
    return len;
}

void LogTextMessage(const char* format, ...) {
  if ((usbGetDriverStateI(BDU1.config->usbp) == USB_ACTIVE) && (connected)) {
    int h = 0x546F7841; // "AxoT"
    chSequentialStreamWrite((BaseSequentialStream * )&BDU1,
                            (const unsigned char* )&h, 4);

    va_list ap;
    va_start(ap, format);
    chvprintf((BaseSequentialStream *)&BDU1, format, ap);
    va_end(ap);
    chSequentialStreamPut((BaseSequentialStream * )&BDU1, 0);
  }
}

static void print(char *p) {

  while (*p) {
    chSequentialStreamPut(&SD1, *p++);
  }
}

static void print_tokens(void) {
  char *cp = tokens_buffer;

  while (cp < tokp)
    chSequentialStreamPut(chp, *cp++);
}

/**
 * @brief   Prints a line without final end-of-line.
 *
 * @param[in] msgp      the message
 */
void test_print(const char *msgp) {

  while (*msgp)
    chSequentialStreamPut(chp, *msgp++);
}

/**
 * @brief Function required to properly using format library
 * @param c char
 * @return
 */
int fm_putchar(int c)
{
    chThdSleepMilliseconds(2);
    chSequentialStreamPut(&SDU1, (uint8_t)c);
    return 1;
}

void chvprintf(BaseSequentialStream *chp, const char *fmt, va_list ap) {
  char *p, *s, c, filler;
  int i, precision, width;
  bool is_long, left_align;
  long l;
#if CHPRINTF_USE_FLOAT
  double d;
  char tmpbuf[2*MAX_FILLER + 1];
  int fprec=0;
#else
  char tmpbuf[MAX_FILLER + 1];
#endif

  while (TRUE) {
    c = *fmt++;
    if (c == 0) {
      return;
    }
    if (c != '%') {
      chSequentialStreamPut(chp, (uint8_t)c);
      continue;
    }
    p = tmpbuf;
    s = tmpbuf;
    left_align = false;
    if (*fmt == '-') {
      fmt++;
      left_align = true;
    }
    filler = ' ';
    if (*fmt == '.') {
      fmt++;
      filler = '0';
#if CHPRINTF_USE_FLOAT
      fprec = intPow (10, (*fmt)-'0');
#endif
    }
    width = 0;
    while (TRUE) {
      c = *fmt++;
      if (c >= '0' && c <= '9')
        c -= '0';
      else if (c == '*')
        c = va_arg(ap, int);
      else
        break;
      width = width * 10 + c;
    }
    precision = 0;
    if (c == '.') {
      while (TRUE) {
        c = *fmt++;
        if (c >= '0' && c <= '9') {
          c -= '0';
#if CHPRINTF_USE_FLOAT
          fprec = intPow (10, c);
#endif
        }
        else if (c == '*')
          c = va_arg(ap, int);
        else
          break;
        precision *= 10;
        precision += c;
      }
    }

void send_command( char *p )
{
    while (*p) chSequentialStreamPut( &SD6, *p++ );
}

void spicallback(SPIDriver *spip){
  chSequentialStreamPut(&SD1, '.');
  spip->txbuf = 0; // terminating null
}

void consolePutChar(int x) {
	chSequentialStreamPut(getConsoleChannel(), (uint8_t )(x));
}