#include <stdarg.h>
#include <stdint.h>
#include <ports.h>
#include <cursor.h>
#include <message.h>
#include <task.h>
#include <osTasks.h>
#include <alloc.h>
#include <stdbool.h>
#include <_stdio.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_CHARS[] = "0123456789ABCDEF";

void putHex(char** write, uint32_t x) {
    bool alreadyWriting = false;
    for (int position = 3; position >= 0; position--) {
        uint8_t byte = (x >> (position * 8)) & 0xFF;
        if (byte != 0x00) {
            alreadyWriting = true;
        }
        if (alreadyWriting) {
            **write = HEX_CHARS[byte >> 8];
            (*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 = true;
        }
        if (alreadyWriting) {
            size += 2;
        }
    }
    return size;
}

uint32_t getInsertLength(char insertType, uint32_t x) {
    switch (insertType) {
    case 's':
        return strlen((char*) x);
    case 'x':
        return hexLength(x);
    }
    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;
    }
}

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());
}

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;
}