#include "hos_printf.h"
#include <stdint.h>
#include <stdbool.h>
#include "string.h"

static size_t format_dec(char * buffer, int v)
{
    size_t sz = 0u;
    bool printing = false;
    if (v < 0)
    {
        buffer[sz++] = '-';
        v = -v;
    }
    for (int div = 1000000000; div >= 1; div /= 10)
    {
        int digit = v / div;
        v %= div;
        if ((digit != 0) || (div == 1))
        {
            printing = true;
        }
        if (printing)
        {
            buffer[sz++] = digit + '0';
        }
    }
    return sz;
}

static size_t format_udec(char * buffer, unsigned int v)
{
    size_t sz = 0u;
    for (unsigned int div = 1000000000u; div >= 1u; div /= 10u)
    {
        unsigned int digit = v / div;
        v %= div;
        if ((digit != 0) || (sz > 0u) || (div == 1))
        {
            buffer[sz++] = digit + '0';
        }
    }
    return sz;
}

static size_t format_hex(char * buffer, int v, bool upper)
{
    const char upper_hex[] = "0123456789ABCDEF";
    const char lower_hex[] = "0123456789abcdef";
    size_t sz = 0u;
    for (int i = 28; i >= 0; i -= 4)
    {
        uint8_t n = (v >> i) & 0xFu;
        if ((sz > 0u) || (n != 0u) || (i == 0))
        {
            buffer[sz] = upper ? upper_hex[n] : lower_hex[n];
            sz++;
        }
    }
    return sz;
}

static void pad_write(const stream_t * stream, const char * data, size_t length, bool leading_zero, bool left_just, size_t width)
{
    if (left_just)
    {
        stream->write(data, length);
        while (length < width)
        {
            stream->write1(' ');
            length++;
        }
    }
    else
    {
        char fill = leading_zero ? '0' : ' ';
        size_t l = length;
        while (l < width)
        {
            stream->write1(fill);
            l++;
        }
        stream->write(data, length);
    }
}

void hos_vprintf(const stream_t * stream, const char * fmt, va_list va)
{
    bool in_conv = false;
    char c;
    char buffer[12];
    size_t width;
    bool leading_zero;
    bool left_just;
    while ((c = *fmt))
    {
        if (in_conv)
        {
            switch (c)
            {
            case '%':
                stream->write1('%');
                break;
            case 'X':
                {
                    unsigned int v = va_arg(va, unsigned int);
                    size_t length = format_hex(buffer, v, true);
                    pad_write(stream, buffer, length, leading_zero, left_just, width);
                }
                break;
            case 'c':
                {
                    char ch = va_arg(va, int);
                    pad_write(stream, &ch, 1u, leading_zero, left_just, width);
                }
                break;
            case 'd':
                {
                    int v = va_arg(va, int);
                    size_t length = format_dec(buffer, v);
                    pad_write(stream, buffer, length, leading_zero, left_just, width);
                }
                break;
            case 's':
                {
                    const char * s = va_arg(va, const char *);
                    pad_write(stream, s, strlen(s), leading_zero, left_just, width);
                }
                break;
            case 'u':
                {
                    unsigned int v = va_arg(va, unsigned int);
                    size_t length = format_udec(buffer, v);
                    pad_write(stream, buffer, length, leading_zero, left_just, width);
                }
                break;
            case 'x':
                {
                    unsigned int v = va_arg(va, unsigned int);
                    size_t length = format_hex(buffer, v, false);
                    pad_write(stream, buffer, length, leading_zero, left_just, width);
                }
                break;
            }
            in_conv = false;
        }
        else if (c == '%')
        {
            in_conv = true;
            width = 0u;
            leading_zero = false;
            left_just = false;
            if (fmt[1] == '-')
            {
                left_just = true;
                fmt++;
            }
            if (fmt[1] == '0')
            {
                leading_zero = true;
                fmt++;
            }
            while (('0' <= fmt[1]) && (fmt[1] <= '9'))
            {
                width *= 10u;
                width += (fmt[1] - '0');
                fmt++;
            }
        }
        else
        {
            stream->write1(c);
        }
        fmt++;
    }
}

void hos_printf(const stream_t * stream, const char * fmt, ...)
{
    va_list va;
    va_start(va, fmt);
    hos_vprintf(stream, fmt, va);
    va_end(va);
}