#pragma once
#include <stdlib.h>
#include <stdcountof.h>
#include <sys/syscall.h>
#include <unistd.h>
#include <stdint.h>
#include <string.h>

#define xstr(s) str(s)
#define str(s) #s

typedef uint64_t bcdint;

static
void printns(size_t len, unsigned char chars[len]) {
    syscall(SYS_write, STDOUT_FILENO, chars, len);
}

static 
void printptr(uintptr_t v) {
    char buf[sizeof(uintptr_t)*2] = { };
    char *p = buf+countof(buf);
    for (size_t i = 0; i < countof(buf); i++) {
        p--;
        if ((v & 0x0F) < 0xA) {
            *p = '0' + (v & 0x0F);
        } else {
            *p = 'A' - 0xA + (v & 0x0F);
        }
        v >>= 4;
    }
    syscall(SYS_write, STDOUT_FILENO, p, countof(buf));
}

static
void print_many_char(size_t len, char chars[len]) {
    syscall(SYS_write, STDOUT_FILENO, chars, len);
}

#define print(string) print_many_char(sizeof(string)-1, string)

static
void printd(unsigned long v) {
    if (!v) {
        print("0");
        return;
    }
    char buf[32];
    char *p = buf+32;
    unsigned count = 0;
    while (v) {
        p--;
        *p = '0' + (v % 10);
        v /= 10;
        count++;
    }
    syscall(SYS_write, STDOUT_FILENO, p, count);
}

static
void printh(unsigned long v) {
    if (!v) {
        print("0");
        return;
    }
    char buf[32];
    char *p = buf+32;
    unsigned count = 0;
    while (v) {
        p--;
        if ((v & 0x0F) < 0xA) {
            *p = '0' + (v & 0x0F);
        } else {
            *p = 'A' - 0xA + (v & 0x0F);
        }
        v >>= 4;
        count++;
    }
    syscall(SYS_write, STDOUT_FILENO, p, count);
}

static
void print_hex_char(unsigned char v) {
    char buf[2];

    if ((v >> 4) < 0x0A) {
        buf[0] = '0' + (v >> 4);
    } else {
        buf[0] = 'A' - 0xA + (v >> 4);
    }

    if ((v & 0x0F) < 0xA) {
        buf[1] = '0' + (v & 0x0F);
    } else {
        buf[1] = 'A' - 0xA + (v & 0x0F);
    }

    syscall(SYS_write, STDOUT_FILENO, buf, sizeof(buf));
}

static
void print_pre_hex(size_t len, unsigned char prefix[len], unsigned long v) {
    printns(len, prefix);
    printh(v);
}

static
void print_pre_ptr(size_t len, unsigned char prefix[len], void *ptr) {
    printns(len, prefix);
    printptr((uintptr_t)ptr);
}

#define print_watch_ptr(expr) print_pre_ptr(\
        sizeof(#expr ": ")-1, (unsigned char*)(#expr ": "), expr)
#define print_watch_hex(expr) print_pre_hex(\
        sizeof(#expr ": ")-1, (unsigned char*)(#expr ": "), expr)

static
bcdint grabbcd(unsigned char *s, unsigned char **end) {
    bcdint r = 0;
    for (; *s >= '0' && *s <= '9'; s++)
        r = (r << 4) + (*s - '0');
    if (end) *end = s;
    return r;
}

static
unsigned grabnum(const char *s, const char **end) {
    unsigned r = 0;
    for (; *s >= '0' && *s <= '9'; s++)
        r = r * 10 + (*s - '0');
    *end = s;
    return r;
}

// from wikipedia, the free encyclopedia
// https://en.wikipedia.org/wiki/Binary-coded_decimal
static bcdint bcdadd(bcdint a, bcdint b) {
    bcdint t1, t2;
    t1 = a + 0x0666666666666666;
    t2 = t1 ^ b;
    t1 = t1 + b;
    t2 = t1 ^ t2;
    t2 = ~t2 & 0x1111111111111110;
    t2 = (t2 >> 2) | (t2 >> 3);
    return t1 - t2;
}