#include <stdarg.h>
#include <stdbool.h>

static void append_char(char **buf, size_t *remaining, char c) {
    if (*remaining > 1) { // Leave space for null terminator
        **buf = c;
        (*buf)++;
        (*remaining)--;
    }
}

static void append_str(char **buf, size_t *remaining, const char *str) {
    while (*str) {
        append_char(buf, remaining, *str++);
    }
}

static void append_int(char **buf, size_t *remaining, int value, int base) {
    char        tmp[32];
    const char *digits = "0123456789abcdef";
    bool        neg = false;
    int         i = 0;

    if (base == 10 && value < 0) {
        neg = true;
        value = -value;
    }

    do {
        tmp[i++] = digits[value % base];
        value /= base;
    } while (value && i < (int)sizeof(tmp));

    if (neg) {
        tmp[i++] = '-';
    }

    while (i--) {
        append_char(buf, remaining, tmp[i]);
    }
}

int kvsnprintf(char *buf, size_t size, const char *fmt, va_list args) {
    char  *p = buf;
    size_t remaining = size;

    while (*fmt) {
        if (*fmt != '%') {
            append_char(&p, &remaining, *fmt++);
            continue;
        }
        fmt++; // skip '%'

        switch (*fmt) {
        case 's':
            append_str(&p, &remaining, va_arg(args, const char *));
            break;
        case 'd':
            append_int(&p, &remaining, va_arg(args, int), 10);
            break;
        case 'x':
            append_int(&p, &remaining, va_arg(args, unsigned int), 16);
            break;
        case 'c':
            append_char(&p, &remaining, (char)va_arg(args, int));
            break;
        case '%':
            append_char(&p, &remaining, '%');
            break;
        default:
            append_char(&p, &remaining, '?');
            break;
        }
        fmt++;
    }

    if (remaining > 0) {
        *p = '\0';
    } else {
        buf[size - 1] = '\0';
    }

    return (int)(p - buf);
}

int ksnprintf(char *buf, size_t size, const char *fmt, ...) {
    va_list args;
    va_start(args, fmt);
    int ret = kvsnprintf(buf, size, fmt, args);
    va_end(args);
    return ret;
}

void kprintf(const char *fmt, ...) {
    char    buffer[256];
    va_list args;
    va_start(args, fmt);
    kvsnprintf(buffer, sizeof(buffer), fmt, args);
    va_end(args);

    // Now send buffer to your console/serial output
    console_write(buffer);
}