#include "hos_types.h"
#include "hos_defines.h"
#include "kio.h"
#include "string.h"
#include "portio.h"

#include <limits.h>
#include <stdarg.h>                 /* va_*() */

static void fmt_d2a(char * buf, int val);
static void fmt_u2a(char * buf, unsigned int val);
static void fmt_ll2a(char * buf, long long val);
static void fmt_ull2a(char * buf, unsigned long long val);
static void fmt_x2a(char * buf, unsigned int val);
static void fmt_xl2a(char * buf, unsigned long long val);
static void fmt_o2a(char * buf, unsigned int val);

static int cursor_x, cursor_y;

static void writeCursorPosition(int x, int y)
{
    u16_t pos = 80 * y + x;
    outportb(0x3D4, 0x0E);
    outportb(0x3D5, pos >> 8);
    outportb(0x3D4, 0x0F);
    outportb(0x3D5, pos);
}

extern "C" {

void kio_bootstrap()
{
    cursor_x = 0;
    cursor_y = 9;
    writeCursorPosition(cursor_x, cursor_y);
}

void kprintf(const char * fmt, ...)
{
    va_list args;
    va_start(args, fmt);

    kvprintf(fmt, args);

    va_end(args);
}

void kvprintf(const char * fmt, va_list args)
{
    char tmpbuf[25];
    for ( ; *fmt; fmt++)
    {
        if (*fmt == '%')
        {
            fmt++;
            if (*fmt)
            {
                int width = 0;
                char pad_char = ' ';
                bool pad_right = false;
                if (*fmt == '0')
                {
                    pad_char = '0';
                    fmt++;
                }
                else if (*fmt == '-')
                {
                    pad_right = true;
                    fmt++;
                }
                for ( ; '0' <= *fmt && *fmt <= '9'; fmt++)
                {
                    if (width == 0)
                    {
                        width = *fmt - '0';
                    }
                    else
                    {
                        width *= 10;
                        width += *fmt - '0';
                    }
                }
                switch (*fmt)
                {
                    case 'c':
                        kputc(va_arg(args, int));
                        break;
                    case 'd':
                        fmt_d2a(tmpbuf, va_arg(args, int));
                        kputs_pad(tmpbuf, width, pad_char, pad_right);
                        break;
                    case 'l':
                        fmt_ll2a(tmpbuf, va_arg(args, long long));
                        kputs_pad(tmpbuf, width, pad_char, pad_right);
                        break;
                    case 'L':
                        fmt_ull2a(tmpbuf, va_arg(args, unsigned long long));
                        kputs_pad(tmpbuf, width, pad_char, pad_right);
                        break;
                    case 'o':
                        fmt_o2a(tmpbuf, va_arg(args, unsigned int));
                        kputs_pad(tmpbuf, width, pad_char, pad_right);
                        break;
                    case 's':
                        kputs_pad(va_arg(args, char *),
                                  width, pad_char, pad_right);
                        break;
                    case 'u':
                        fmt_u2a(tmpbuf, va_arg(args, unsigned int));
                        kputs_pad(tmpbuf, width, pad_char, pad_right);
                        break;
                    case 'x':
                        fmt_x2a(tmpbuf, va_arg(args, unsigned int));
                        kputs_pad(tmpbuf, width, pad_char, pad_right);
                        break;
                    case 'X':
                        fmt_xl2a(tmpbuf, va_arg(args, unsigned long long));
                        kputs_pad(tmpbuf, width, pad_char, pad_right);
                        break;
                    case '%':
                        kputc('%');
                        break;
                }
            }
        }
        else
        {
            kputc(*fmt);
        }
        if (!*fmt)
        {
            break;
        }
    }
}

void kputc(char c)
{
    u16_t * console_memory = (u16_t *) CONSOLE_MEMORY;
    console_memory += 80 * cursor_y + cursor_x;
    switch (c)
    {
        case '\t':
            {
                int to_advance = 8 - (cursor_x & 0x3);
                while (to_advance--)
                {
                    *console_memory++ = 0x0720;
                }
            }
            break;
        case '\n':
            cursor_x = 0;
            cursor_y++;
            break;
        default:
            *console_memory = 0x0700 | (c & 0xFF);
            cursor_x++;
            break;
    }
    if (cursor_x >= 80)
    {
        cursor_x = 0;
        cursor_y++;
    }
    if (cursor_y >= 25)
    {
        memcpy((u8_t *) CONSOLE_MEMORY,
               (u8_t *) (CONSOLE_MEMORY + 80 * 2),
               2 * 80 * 24);
        memsetw((u16_t *) (CONSOLE_MEMORY + 2 * 80 * 24), 0x0720, 80);
        cursor_y = 24;
    }
    writeCursorPosition(cursor_x, cursor_y);
}

void kputs(const char * s)
{
    while (*s)
    {
        kputc(*s);
        s++;
    }
}

void kputs_pad(const char * s, int width, char pad_char, bool pad_right)
{
    int len = strlen(s);
    if (pad_right)
    {
        kputs(s);
    }
    int num_pad_chars = width - len;
    for (int i = 0; i < num_pad_chars; i++)
    {
        kputc(pad_char);
    }
    if (!pad_right)
    {
        kputs(s);
    }
}

} /* extern "C" */

static void fmt_d2a(char * buf, int val)
{
    if (val < 0)
    {
        *buf++ = '-';
        val = -val;
    }
    fmt_u2a(buf, (unsigned int) val);
}

static void fmt_u2a(char * buf, unsigned int val)
{
    bool printing = false;
    for (unsigned int div = 1000000000; div >= 1; div /= 10)
    {
        unsigned int n = val / div;
        if (n || div == 1)
        {
            printing = true;
        }
        if (printing)
        {
            *buf++ = '0' + n;
        }
        val -= n * div;
    }
    *buf = '\0';
}

static void fmt_ll2a(char * buf, long long val)
{
    if (val < 0)
    {
        *buf++ = '-';
        val = -val;
    }
    fmt_ull2a(buf, (unsigned long long) val);
}

static void fmt_ull2a(char * buf, unsigned long long val)
{
    bool printing = false;
    for (unsigned long long div = 10000000000000000000ull; div >= 1; div /= 10)
    {
        unsigned long long n = val / div;
        if (n || div == 1)
        {
            printing = true;
        }
        if (printing)
        {
            *buf++ = '0' + n;
        }
        val -= n * div;
    }
    *buf = '\0';
}

static void fmt_x2a(char * buf, unsigned int val)
{
    bool printing = false;
    for (int s = 28; s >= 0; s -= 4)
    {
        unsigned int n = (val >> s) & 0xF;
        if (n || s == 0)
        {
            printing = true;
        }
        if (printing)
        {
            *buf++ = "0123456789abcdef"[n];
        }
    }
    *buf = '\0';
}

static void fmt_xl2a(char * buf, unsigned long long val)
{
    bool printing = false;
    for (int s = 60; s >= 0; s -= 4)
    {
        unsigned int n = (val >> s) & 0xF;
        if (n || s == 0)
        {
            printing = true;
        }
        if (printing)
        {
            *buf++ = "0123456789abcdef"[n];
        }
    }
    *buf = '\0';
}

static void fmt_o2a(char * buf, unsigned int val)
{
    bool printing = false;
    for (int s = 30; s >= 0; s -= 3)
    {
        unsigned int n = (val >> s) & 0x7;
        if (n || s == 0)
        {
            printing = true;
        }
        if (printing)
        {
            *buf++ = "01234567"[n];
        }
    }
    *buf = '\0';
}