#include <stdarg.h>
#include <stdint.h>
#include <lib/textMode/stdio.h>
#include <lib/ports.h>
#include <lib/textMode/cursor.h>
#include <lib/task/message.h>
#include <lib/task/task.h>
#include <lib/task/osTasks.h>
#include <lib/memory/alloc.h>

uint16_t* framebuffer = (uint16_t*) FRAMEBUFFER_LOCATION;
char format = 0x0F;

void putCharWithFormatAtOffset(uint16_t c, uint16_t position) {
    *(framebuffer + position) = c;
}

void putCharWithFormatAtPosition(uint8_t x, uint8_t y, uint32_t c) {
    putCharWithFormatAtOffset(c, (x + y * VIDEO_WIDTH));
}

void putCharAtOffset(char c, uint16_t offset) {
    putCharWithFormatAtOffset(c | format << 8, offset);
}

void putCharAt(char c, uint8_t x, uint8_t y) {
    putCharAtOffset(c, (x + y * VIDEO_WIDTH));
}

uint16_t getCharWithFormatAt(uint8_t x, uint8_t y) {
    return *(framebuffer + x + y * VIDEO_WIDTH);
}

void putCharsAt(char* string, uint8_t x, uint8_t y) {
    uint16_t position = 0;
    char c = *string;
    while (c != 0x00) {
        putCharAt(c, x + position, y);
        position++;
        c = *(string + position);
    }
}

void shiftUp() {
    for (int y = 1; y < VIDEO_HEIGHT; y++) {
        for (int x = 0; x < VIDEO_WIDTH; x++) {
            putCharWithFormatAtPosition(x, y-1, getCharWithFormatAt(x, y));
        }
    }
    for (int x = 0; x < VIDEO_WIDTH; x++) {
        putCharWithFormatAtPosition(x, VIDEO_HEIGHT-1, 0x00);
    }
}

void newLine() {
    uint16_t offset = getCursorOffset();
    uint8_t x = offset % VIDEO_WIDTH;
    uint8_t y = offset / VIDEO_WIDTH;
    x = 0;
    y++;
    if (y >= VIDEO_HEIGHT) {
        shiftUp();
        y--;
    }
    setCursorPosition(x, y);
}

void putChar(char c) {
    if (c == '\n') {
        newLine();
        return;
    }
    uint16_t offset = getCursorOffset();
    putCharAtOffset(c, offset);
    setCursorOffset(offset + 1);
}

void puts(char* string) {
    while (*string != 0x00) {
        putChar(*string);
        string++;
    }
}

char HEX_PREFIX[] = "0x";
char HEX_CHARS[] = "0123456789ABCDEF";

void putHex(char** write, uint32_t x) {
    uint8_t alreadyWriting = 0;
    for (uint8_t position = 3; position >= 0; position--) {
        uint8_t byte = (x >> (position * 8)) & 0xFF;
        if (byte != 0x00) {
            alreadyWriting = 0x01;
        }
        if (alreadyWriting) {
            **write = HEX_CHARS[byte % 256];
            *write++;
            **write = HEX_CHARS[byte / 256];
            *write++;
        }
    }
}

uint32_t getInsertLength(char insertType, uint32_t x) {
    switch (insertType)
    {
    case 's':
        return strlen((char*) x);
    case 'x':
        for (uint8_t position = 3; position >= 0; position--) {
            uint8_t byte = (x >> (position * 8)) & 0xFF;
            if (byte != 0x00) {
                return 2*position;
            }
        }
        return 0;
    }
    return 0;
}

void handleInsert(char** write, char insertType, uint32_t x) {
    if (insertType == 's') {
        char* string = (char*) x;
        uint32_t length = strlen(string);
        for (uint32_t position = 0; position < length; position++) {
            **write = string[position];
            *write++;
        }
        return;
    }
    if (insertType == 'x') {
        putHex(write, x);
    }
}

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 = {
        message.data = data,
        message.size = size,
        message.next = 0x00
    };
    sendMessage(getPrinterTask(), &message);
    schedule(getPrinterTask());
    yields();
}

void clearScreen() {
    setCursorOffset(0);
    for (int y = 0; y < VIDEO_HEIGHT; y++) {
        for (int x = 0; x < VIDEO_WIDTH; x++) {
            putCharAt(' ', x, y);
        }
    }
}

void setTextStyle(uint8_t style) {
    format = style;
}

void putChars(char* string, uint32_t length) {
    for (uint32_t i = 0; i < length; i++) {
        if (string[i] == '\n') {
            newLine();
            continue;
        }
        putChar(string[i]);
    }
}

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