#include "std/stdio.h"
#include "io/Serial.h"
#include "std/stdlib.h"
#include <string.h>

typedef long int ssize_t;

template <typename PutString>
static int internal_printf(const char* format, PutString put_string_callback, ssize_t max, va_list ap)
{
    char buffer[1025]; // 1024 with null terminator
    size_t format_size = strlen(format);
    size_t format_index = 0;
    size_t buffer_insert_index = 0;
    ssize_t max_remaining = max;
    size_t written = 0;

    auto flush_buffer = [&]() {
        size_t buffer_length = buffer_insert_index;
        written += buffer_length;
        buffer_insert_index = 0;
        if (max_remaining < 0)
        {
            buffer[buffer_length] = 0;
            put_string_callback(buffer);
            return;
        }
        if (max_remaining == 0) { return; }
        if (buffer_length <= (size_t)max_remaining)
        {
            max_remaining -= buffer_length;
            buffer[buffer_length] = 0;
            put_string_callback(buffer);
        }
        else
        {
            buffer[max_remaining] = 0;
            max_remaining = 0;
            put_string_callback(buffer);
        }
    };

    while (format_index < format_size)
    {
        char current_char = format[format_index];
        if (current_char == '%')
        {
            if (format_index + 1 == format_size) // end of format string
            {
                continue;
            }
            else
            {
                format_index++;
                current_char = format[format_index];
                switch (current_char)
                {
                case 'c': {
                    buffer[buffer_insert_index++] = va_arg(ap, int);
                    if (buffer_insert_index == 1024) flush_buffer();
                    break;
                }
                case '%': {
                    buffer[buffer_insert_index++] = '%';
                    if (buffer_insert_index == 1024) flush_buffer();
                    break;
                }
                case 'd': {
                    char result[41];
                    itoa(va_arg(ap, int), result, 10);
                    if (buffer_insert_index + strlen(result) > 1024) flush_buffer();
                    memcpy(buffer + buffer_insert_index, result, strlen(result));
                    buffer_insert_index += strlen(result);
                    if (buffer_insert_index == 1024) flush_buffer();
                    break;
                }
                case 'x': {
                    char result[50];
                    utoa(va_arg(ap, unsigned int), result, 16);
                    if (buffer_insert_index + strlen(result) > 1024) flush_buffer();
                    memcpy(buffer + buffer_insert_index, result, strlen(result));
                    buffer_insert_index += strlen(result);
                    if (buffer_insert_index == 1024) flush_buffer();
                    break;
                }
                case 's': {
                    const char* str = va_arg(ap, const char*);
                    while (strlen(str) > 1024)
                    {
                        flush_buffer();
                        memcpy(buffer, str, 1024);
                        str += 1024;
                        buffer_insert_index = 1024;
                    }
                    if (buffer_insert_index + strlen(str) > 1024) flush_buffer();
                    memcpy(buffer + buffer_insert_index, str, strlen(str));
                    buffer_insert_index += strlen(str);
                    if (buffer_insert_index == 1024) flush_buffer();
                    break;
                }
                default: {
                    buffer[buffer_insert_index++] = '%';
                    if (buffer_insert_index == 1024) flush_buffer();
                    buffer[buffer_insert_index++] = current_char;
                    if (buffer_insert_index == 1024) flush_buffer();
                    break;
                }
                }
            }
        }
        else
        {
            buffer[buffer_insert_index++] = current_char;
            if (buffer_insert_index == 1024) flush_buffer();
        }
        format_index++;
    }

    if (buffer_insert_index > 0) flush_buffer();
    return written;
}

int printf(const char* fmt, ...)
{
    va_list ap;
    va_start(ap, fmt);
    int written = internal_printf(
        fmt, [](const char* s) { Serial::print(s); }, -1, ap);
    va_end(ap);
    return written;
}

int sprintf(char* __s, const char* fmt, ...)
{
    va_list ap;
    va_start(ap, fmt);
    int written = internal_printf(
        fmt,
        [&](const char* s) {
            if (__s) { strncat(__s, s, 1025); }
        },
        -1, ap);
    va_end(ap);
    return written;
}

int snprintf(char* __s, size_t max, const char* fmt, ...)
{
    va_list ap;
    va_start(ap, fmt);
    int written = internal_printf(
        fmt,
        [&](const char* s) {
            if (__s) { strncat(__s, s, 1025); }
        },
        max == 0 ? 0 : max - 1, ap);
    va_end(ap);
    return written;
}

int vprintf(const char* fmt, va_list ap)
{
    return internal_printf(
        fmt, [](const char* s) { Serial::print(s); }, -1, ap);
}

int vsprintf(char* __s, const char* fmt, va_list ap)
{
    return internal_printf(
        fmt,
        [&](const char* s) {
            if (__s) { strncat(__s, s, 1025); }
        },
        -1, ap);
}

int vsnprintf(char* __s, size_t max, const char* fmt, va_list ap)
{
    return internal_printf(
        fmt,
        [&](const char* s) {
            if (__s) { strncat(__s, s, 1025); }
        },
        max == 0 ? 0 : max - 1, ap);
}