#include <msp430.h>
#include <stdint.h>
#include <stdarg.h>
#include "mspdebugconsole.h"

// low 7 bits = length
#define COMMAND_NONE 0x00
#define COMMAND_PRINT 0x80
#define COMMAND_PRINTC 0x01
#define COMMAND_PRINTS 0x02

volatile char mspdebug_console_buffer[MSPDEBUGCONSOLE_BUFLEN] = { COMMAND_NONE };

__attribute__((noinline)) void mspdebug_console_breakpoint() {
    mspdebug_console_buffer[0] = COMMAND_NONE;
}

static int buf_pos = 0;

static void d_putchar(char c) {
    buf_pos++;
    mspdebug_console_buffer[buf_pos] = c;

    if (buf_pos == MSPDEBUGCONSOLE_BUFLEN - 1) {
        mspdebug_console_buffer[0] = buf_pos;
        mspdebug_console_breakpoint();
        buf_pos = 0;
    }
}

static void d_flush() {
    if (buf_pos > 0) {
        mspdebug_console_buffer[0] = buf_pos;
        mspdebug_console_breakpoint();
        buf_pos = 0;
    }
}

static void d_puts(char *str) {
    for (; *str; str++)
        d_putchar(*str);
}

static void d_put_decimal(int16_t val) {
    char buffer[6];
    char *p = &buffer[sizeof(buffer) - 1];

    *p = '\0';

    if (val == 0) {
        d_putchar('0');
        return;
    }

    if (val < 0) {
        d_putchar('-');
        val = -val;
    }

    uint16_t uval = val;

    while (uval > 0) {
        uint16_t res = uval / 10;
        uint16_t rem = uval % 10;

        *--p = rem + '0';
        uval = res;
    }

    d_puts(p);
}

//static void d_put_hex(uint16_t val) {
//  
//}

void debug_printf(const char *fmt, ...) {
    va_list argp;
    va_start(argp, fmt);

    for (const char *p = fmt; *p; p++) {
        if (*p != '%') {
            d_putchar(*p);
        } else {
            switch (*++p) {
                case 'c':
                    d_putchar(va_arg(argp, int));
                    break;
                case 'd':
                    d_put_decimal(va_arg(argp, int));
                    break;
//                case 'x':
//                    d_put_hex(va_arg(argp, int));
//                    break;
                case '%':
                    d_putchar('&');
                    break;
            }
        }
    }

    d_flush();

    va_end(argp);
}