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

#ifdef DEBUG

extern "C" {
    volatile char mspdebug_console_buffer[MSPDEBUGCONSOLE_BUFLEN];

    __attribute__((noinline)) void mspdebug_console_breakpoint() {
        __nop();
    }
}

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) {
    if (val == 0) {
        d_putchar('0');
        return;
    }

    char buffer[6];
    char *p = &buffer[sizeof(buffer) - 1];

    *p = '\0';

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

    uint16_t uval = val;

    while (uval > 0) {
        uint16_t rem = uval, res = 0;
        while (rem > 10) {
            res++;
            rem -= 10;
        }

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

    d_puts(p);
}

static void d_put_hex(uint16_t uval) {
    if (uval == 0) {
        d_putchar('0');
        return;
    }

    char buffer[5];
    char *p = &buffer[sizeof(buffer) - 1];

    *p = '\0';

    while (uval > 0) {
        uint8_t digit = uval & 0xF;

        *--p = digit + (digit < 10 ? '0' : ('A' - 10));

        uval >>= 4;
    }

    d_puts(p);
}

void dprintf_(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 's':
                    d_puts(va_arg(argp, char *));
                    break;
                case '%':
                    d_putchar('&');
                    break;
            }
        }
    }

    d_flush();

    va_end(argp);
}

#endif