#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>

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