#include <_stdio.h>
#include <alloc.h>
#include <cursor.h>
#include <message.h>
#include <osTasks.h>
#include <ports.h>
#include <stdarg.h>
#include <stdbool.h>
#include <stdint.h>
#include <task.h>
#include <util.h>

char HEX_CHARS[] = "0123456789ABCDEF";

void putHex(char **write, uint32_t x) {
    if (x == 0) {
        **write = HEX_CHARS[x];
        (*write)++;
        **write = HEX_CHARS[x];
        (*write)++;
        return;
    }
    bool alreadyWriting = false;
    for (int position = 3; position >= 0; position--) {
        uint8_t byte = (x >> (position * 8)) & 0xFF;
        if (byte != 0x00 && !alreadyWriting) {
            alreadyWriting = true;
        }
        if (alreadyWriting) {
            **write = HEX_CHARS[byte >> 4];
            (*write)++;
            **write = HEX_CHARS[byte & 0x0F];
            (*write)++;
        }
    }
}

uint8_t hexLength(uint32_t x) {
    bool alreadyWriting = false;
    uint8_t size = 0;
    for (int position = 3; position >= 0; position--) {
        uint8_t byte = (x >> (position * 8)) & 0xFF;
        if (byte != 0x00 && !alreadyWriting) {
            alreadyWriting = true;
        }
        if (alreadyWriting) {
            size += 2;
        }
    }
    return max(size, 2);
}

uint32_t getInsertLength(char insertType, uint32_t x) {
    switch (insertType) {
    case 's':
        return strlen((char *)x);
    case 'x':
        return hexLength(x);
    case 'c':
        return 1;
    }
    return 0;
}

void stringInsert(char **write, uint32_t x) {
    char *string = (char *)x;
    uint32_t length = strlen(string);
    for (uint32_t position = 0; position < length; position++) {
        **write = string[position];
        (*write)++;
    }
}

void handleInsert(char **write, char insertType, uint32_t x) {
    switch (insertType) {
    case 's':
        stringInsert(write, x);
        return;
    case 'x':
        putHex(write, x);
        return;
    case 'c':
        **write = x;
        return;
    }
}

void printf(const char *format, ...) {
    uint32_t size = 0;
    va_list valist;
    va_start(valist, format);
    for (int i = 0; format[i] != 0; i++) {
        if (format[i] == '%') {
            size += getInsertLength(format[++i], va_arg(valist, uint32_t));
            continue;
        }
        size++;
    }
    va_start(valist, format);

    char *data = malloc(size);
    char *write = data;
    for (int i = 0; format[i] != 0; i++) {
        if (format[i] == '%') {
            handleInsert(&write, format[++i], va_arg(valist, uint32_t));
            continue;
        }
        *write = format[i];
        write++;
    }
    va_end(valist);

    Message *message = malloc(sizeof(Message));

    message->data = data;
    message->size = size;
    message->next = 0x00;
    sendMessage(getPrinterTask(), message);
    schedule(getPrinterTask());
}

uint32_t strlen(char *string) {
    uint32_t length = 0;
    while (string[length] != 0) {
        length++;
    }
    return length;
}