#include "stdio.hpp"
#include "drivers/memtext.hpp"
#include "math.hpp"
#include "string.hpp"

void (*__putchar)(char)=memtext_putchar;

void printf(char *str,...) {
  u64 rsi,rdx,rcx,r8,r9;
  u64* rbp;
  asm( "mov %%rsi, %0": "=a"(rsi));
  asm( "mov %%rdx, %0": "=a"(rdx));
  asm( "mov %%rcx, %0": "=a"(rcx));
  asm( "mov %%r8, %0": "=a"(r8));
  asm( "mov %%r9, %0": "=a"(r9));
  asm( "mov %%rbp, %0": "=a"(rbp));

  // Init informations
  int len=strlen(str);
  int i=0; // Pointer to the current character
  int p=1; // Pointer to the current parameter

  while (i!=len) {
    char c=str[i];
    // Check if special char is comming
    if(str[i]=='%'){
      char c2=str[i+1];
      char c3=str[i+2];
      // First just consider the data as a void pointer
      u64 data;
      switch (p) {
      case 1:
        data=rsi;
        break;
      case 2:
        data=rdx;
        break;
      case 3:
        data=rcx;
        break;
      case 4:
        data=r8;
        break;
      case 5:
        data=r9;
        break;
      default:
        data=*(rbp+2+p-6);
      }
      
      if(c2=='%'){
        __putchar('%');
        i++;
      }
      else if (c2=='d') {
        int data_int=(int)data;
        printi(data_int);
        i++;
        p++;
      }
      else if (c2=='s') {
        print((char*)data);
        i++;
        p++;
      }
      else if (c2=='x') {
        print("0x");
        printh(data);
        i++;
        p++;
      }
      else if (c2=='l' && c3=='l') {
        printi(data); // TODO: Print 64bit number
        i+=2;
        p++;
      }
    }
    else{
      __putchar(c);
    }
    i++;
  }
}

void print(char *s){
  int i=0;
  while(s[i]!='\0'){
    __putchar(s[i]);
    i++;
  }
}

void printi(int i) {
  char str[50];
  itoa(i, str);
  print(str);
}

void printh(u64 h) {
  char str[17];
  itoh(h, str);
  u8 i=0;
  while(str[i]=='0')
    i++;
  print(&str[i]);
}