Espresso/lib/printf.c

#include <stdarg.h>
#include <stddef.h>
#include <stdbool.h>
#include <stdint.h>

#include <printf.h>

void printwc(const char* str, uint8_t color)
{
  uint8_t c = terminal_getcolor();
  terminal_setcolor(color);
  printf(str);
  terminal_setcolor(c);
}

void printd(const char* str)
{
  terminal_debug_writestring(str);
}

void printdc(const char* str, uint8_t color)
{
  uint8_t c = terminal_getcolor();
  terminal_setcolor(color);
  printd(str);
  terminal_setcolor(c);
}

void printf(const char* format, ...) {
    va_list args;
    va_start(args, format);

    for (size_t i = 0; format[i] != '\0'; ++i) {
        if (format[i] == '%' && format[i + 1] != '\0') {
            ++i;

            // Parse width (only supports zero-padding like %04x)
            int width = 0;
            if (format[i] == '0') {
                ++i;
                while (format[i] >= '0' && format[i] <= '9') {
                    width = width * 10 + (format[i] - '0');
                    ++i;
                }
            }

            switch (format[i]) {
                case 's': {
                    const char* str = va_arg(args, const char*);
                    terminal_writestring(str ? str : "(null)");
                    break;
                }
                case 'c': {
                    char c = (char) va_arg(args, int);
                    terminal_putchar(c);
                    break;
                }
                case 'd':
                case 'i': {
                    int32_t val = va_arg(args, int32_t);
                    print_int(val);
                    break;
                }
                case 'u': {
                    uint32_t val = va_arg(args, uint32_t);
                    print_uint(val);
                    break;
                }
                case 'x': {
                    uint32_t val = va_arg(args, uint32_t);
                    print_hex(val, width, false);
                    break;
                }
                case 'X': {
                    uint32_t val = va_arg(args, uint32_t);
                    print_hex(val, width, true);
                    break;
                }
                case 'p': {
                    void* ptr = va_arg(args, void*);
                    terminal_writestring("0x");
                    print_hex((uint32_t)(uintptr_t)ptr, 8, true); // assumes 32-bit pointer
                    break;
                }
                case 'f':
                case 'F': {
                    double val = va_arg(args, double); // double is promoted from float
                    print_double(val, 2); // 2 decimal places
                    break;
                }
                case '%': {
                    terminal_putchar('%');
                    break;
                }
                default: {
                    terminal_putchar('%');
                    terminal_putchar(format[i]);
                    break;
                }
            }
        } else {
            terminal_putchar(format[i]);
        }
    }

    va_end(args);
}

void print_double(double value, int precision)
{
    // Handle the integer part
    int32_t integer_part = (int32_t)value;
    double fractional_part = value - integer_part;

    // Print the integer part
    print_int(integer_part);

    // Print the decimal point
    terminal_putchar('.');

    // Print the fractional part (scaled up)
    fractional_part *= 1;
    for (int i = 0; i < precision; i++) {
        fractional_part *= 10;
    }

    int32_t frac_int = (int32_t)fractional_part;
    print_int(frac_int);
}

void print_uint(uint32_t value) {
    char buffer[11]; // Enough for 32-bit unsigned int
    int i = 0;

    do {
        buffer[i++] = '0' + (value % 10);
        value /= 10;
    } while (value > 0);

    while (i--) {
        terminal_putchar(buffer[i]);
    }
}

void print_int(int32_t value) {
    char buffer[12]; // Enough for 32-bit signed int (-2147483648)
    int i = 0;
    uint32_t u;

    if (value < 0) {
        terminal_putchar('-');
        u = (uint32_t)(-value);
    } else {
        u = (uint32_t)value;
    }

    // Convert to string in reverse
    do {
        buffer[i++] = '0' + (u % 10);
        u /= 10;
    } while (u > 0);

    // Print in correct order
    while (i--) {
        terminal_putchar(buffer[i]);
    }
}

void print_hex(uint32_t value, int width, bool uppercase)
{
    const char* hex_chars = uppercase ? "0123456789ABCDEF" : "0123456789abcdef";
    char buffer[9]; // 8 hex digits max for 32-bit
    int i = 0;

    do {
        buffer[i++] = hex_chars[value & 0xF];
        value >>= 4;
    } while (value || i < width); // ensure at least 'width' digits

    while (i--) {
        terminal_putchar(buffer[i]);
    }
}