#include <msp430.h>
#include "dconsole.h"

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

volatile char dconsole_buffer[DCONSOLE_BUFLEN] = { COMMAND_NONE };

__attribute__((noinline)) void dconsole_breakpoint() {
    dconsole_buffer[0] = COMMAND_NONE;
}

void debug_print(char *str) {
    int i = 0;

    for (; *str; str++) {
        i++;
        dconsole_buffer[i] = *str;

        if (i == DCONSOLE_BUFLEN - 1) {
            dconsole_buffer[0] = COMMAND_PRINT | (DCONSOLE_BUFLEN - 1);
            dconsole_breakpoint();
            i = 0;
        }
    }

    if (i > 0) {
        dconsole_buffer[0] = COMMAND_PRINT | i;
        dconsole_breakpoint();
    }
}

void debug_printc(char num) {
    dconsole_buffer[0] = COMMAND_PRINTC;
    dconsole_buffer[1] = num;
    dconsole_breakpoint();
}

void debug_prints(short num) {
    dconsole_buffer[0] = COMMAND_PRINTS;
    dconsole_buffer[1] = num & 0xFF;
    dconsole_buffer[2] = (num >> 8) & 0xFF;
    dconsole_breakpoint();
}