#include "printf.h"
#include "fb.h"
#include "util.h"

static void print_dec(char **buf, size_t *pos, size_t size, uint64_t val, int sign) {
    char tmp[32];
    int i = 0;
    if(sign && (int64_t)val < 0) {
        if(*pos < size) (*buf)[(*pos)++] = '-';
        val = -(int64_t)val;
    }
    if(val == 0) {
        if(*pos < size) (*buf)[(*pos)++] = '0';
        return;
    }
    while(val > 0 && i < 32) {
        tmp[i++] = '0' + (val % 10);
        val /= 10;
    }
    while(i > 0) {
        if(*pos < size) (*buf)[(*pos)++] = tmp[--i];
    }
}

static void print_hex(char **buf, size_t *pos, size_t size, uint64_t val, int upper) {
    char *digits = upper ? "0123456789ABCDEF" : "0123456789abcdef";
    char tmp[32];
    int i = 0;
    if(val == 0) {
        if(*pos < size) (*buf)[(*pos)++] = '0';
        return;
    }
    while(val > 0 && i < 32) {
        tmp[i++] = digits[val & 0xF];
        val >>= 4;
    }
    while(i > 0) {
        if(*pos < size) (*buf)[(*pos)++] = tmp[--i];
    }
}

int vsnprintf(char *buf, size_t size, const char *fmt, void *vargs) {
    size_t pos = 0;
    char *arg_ptr = (char*)vargs;

    for(int i = 0; fmt[i]; i++) {
        if(fmt[i] != '%') {
            if(pos < size) buf[pos++] = fmt[i];
            continue;
        }
        i++;
        if(!fmt[i]) break;

        while(fmt[i] == '0' || fmt[i] == '-' || fmt[i] == '+' ||
              fmt[i] == ' ' || fmt[i] == '#') i++;
        while(fmt[i] >= '1' && fmt[i] <= '9') i++;
        if(fmt[i] == '.') { i++; while(fmt[i] >= '0' && fmt[i] <= '9') i++; }

        int lng = 0;
        while(fmt[i] == 'l') { lng++; i++; }

        if(fmt[i] == '%') {
            if(pos < size) buf[pos++] = '%';
        } else if(fmt[i] == 's') {
            char *s = *(char**)arg_ptr; arg_ptr += 8;
            if(!s) s = "(null)";
            while(*s) { if(pos < size) buf[pos++] = *s++; }
        } else if(fmt[i] == 'd' || fmt[i] == 'i') {
            uint64_t val;
            if(lng >= 2) { val = *(int64_t*)arg_ptr; arg_ptr += 8; }
            else { val = *(int32_t*)arg_ptr; arg_ptr += 8; }
            print_dec(&buf, &pos, size, val, 1);
        } else if(fmt[i] == 'u') {
            uint64_t val;
            if(lng >= 2) { val = *(uint64_t*)arg_ptr; arg_ptr += 8; }
            else { val = *(uint32_t*)arg_ptr; arg_ptr += 8; }
            print_dec(&buf, &pos, size, val, 0);
        } else if(fmt[i] == 'x' || fmt[i] == 'X') {
            uint64_t val;
            if(lng >= 2) { val = *(uint64_t*)arg_ptr; arg_ptr += 8; }
            else { val = *(uint32_t*)arg_ptr; arg_ptr += 8; }
            print_hex(&buf, &pos, size, val, fmt[i] == 'X');
        } else if(fmt[i] == 'p') {
            uint64_t val = *(uint64_t*)arg_ptr; arg_ptr += 8;
            if(pos + 2 < size) { buf[pos++] = '0'; buf[pos++] = 'x'; }
            if(val == 0) { if(pos < size) buf[pos++] = '0'; }
            else print_hex(&buf, &pos, size, val, 0);
        } else if(fmt[i] == 'c') {
            int c = *(int*)arg_ptr; arg_ptr += 8;
            if(pos < size) buf[pos++] = (char)c;
        } else {
            if(pos < size) buf[pos++] = '%';
            if(pos < size) buf[pos++] = fmt[i];
        }
    }

    if(pos < size) buf[pos] = '\0';
    else if(size > 0) buf[size-1] = '\0';
    return (int)pos;
}

int snprintf(char *buf, size_t size, const char *fmt, ...) {
    __builtin_va_list args;
    __builtin_va_start(args, fmt);
    int r = vsnprintf(buf, size, fmt, args);
    __builtin_va_end(args);
    return r;
}

void printf(const char *fmt, ...) {
    char buf[4096];
    __builtin_va_list args;
    __builtin_va_start(args, fmt);
    vsnprintf(buf, sizeof(buf), fmt, args);
    __builtin_va_end(args);
    fb_puts(buf);
}
