/* -*- Mode: C++; tab-width: 2; indent-tabs-mode: nil; c-basic-offset: 2 -*- */
/***************************************************************************
 *            cli.c
 *
 *  Thu Jun  6 20:20:20 CEST 2013
 *  Copyright 2013 Bent Bisballe Nyeng
 *  deva@aasimon.org
 ****************************************************************************/

/*
 *  This file is part of Pedal2Metal.
 *
 *  Pedal2Metal is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  Pedal2Metal is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with Pedal2Metal; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA.
 */
#include "cli.h"

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

#include "uart.h"

void cli_init()
{
  UARTInit(0, 115200);	/* baud rate setting */
}

static int cli_strlen(char *p0)
{
  char *p = p0;
  while(*p) p++;
  return p0 - p;
}

static inline void cli_write_int(int i)
{
	if(i < 0) {
		UARTSend(0, "-", 1);
		i *= -1;
	}

	char buf[] = "\0\0";
	int pos = 1000000000;
	int tvaersum = 0;
	while(pos) {
		int res = i / pos;
		i -= res * pos;

		tvaersum += res;
		if(tvaersum || pos == 1) {
			buf[0] = res + '0';
			UARTSend(0, buf, 1);
		}
		pos /= 10;
	}
}

static inline char *cli_to_hex(char i)
{
	static const char hex[] = { '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', 'a', 'b', 'c', 'd', 'e', 'f' };
	static char buf[] = { 'f', 'f', 0 };
	buf[0] = hex[i / 16];
	buf[1] = hex[i % 16];
	return buf;
}

static int cli_write_format(const char *fmt, va_list ap)
{
	char buf[] = "\0\0";

	const char *p = fmt;
	while(*p) {
		if(*p != '%') {
			buf[0] = *p;

      UARTSend(0, buf, 1);
		} else {
			p++;
			if(!*p) {
				return 1; // Error: Invalid format string
			}
			if(*p == '\n') UARTSend(0, "\n\r", 2);
			if(*p == '%') UARTSend(0, "%", 1);
			if(*p == 'd') cli_write_int(va_arg(ap, int));
			if(*p == 'c') {
				buf[0] = va_arg(ap, int);
        UARTSend(0, buf, 1);
			}
			if(*p == 'x') {
				char *c = va_arg(ap, char *);
				size_t sz = va_arg(ap, int);
				while(sz) {
          UARTSend(0, cli_to_hex(*c), 2);
					c++; sz--;
				}
			}
			if(*p == 's') {
				char *str = va_arg(ap, char *);
        UARTSend(0, str, cli_strlen(str));
			}
		}
		p++;
	}

	// If last character wasn't a newline, print one.
	if(p > fmt && *(p - 1) != '\n') UARTSend(0, "\n\r", 2);

	return 0;
}

int cli_write(const char *fmt, ...)
{
  va_list ap;
	va_start(ap, fmt);
	cli_write_format(fmt, ap);
	va_end(ap);

	return 0;
}