// This is NEW CODE replacing the old printf with one based on my libc
#include "types.h"
#include "param.h"
#include "riscv.h"
#include "defs.h"
#include "kprintf.h"
#include "sched.h"

volatile int kprintf_shouldlock = 0;
sched_spinlock_t kprintf_spin;

// These are non-standard, but the other *printf functions need to be implemented somehow,
// so fnprintf/vfnprintf just use a callback function for output of n bytes of string output.
typedef int(*_libc_fnprintf_fn_t)(const char* str, int n, void* udata);
int _LIBC_PRINTF_CALLCONV _libc_fnprintf(_libc_fnprintf_fn_t fn, void* udata, const char* fmt, ...);
int _LIBC_PRINTF_CALLCONV _libc_vfnprintf(_libc_fnprintf_fn_t fn, void* udata, const char* fmt, va_list list);

// These are non-standard, but the other *printf functions need to be implemented somehow,
// so fnprintf/vfnprintf just use a callback function for output of n bytes of string output.
int _LIBC_PRINTF_CALLCONV _libc_fnprintf(_libc_fnprintf_fn_t fn, void* udata, const char* fmt, ...) {
  va_list varargs;
  va_start(varargs, fmt);
  uintptr_t result = _libc_vfnprintf(fn, udata, fmt, varargs);
  va_end(varargs);
  return result;
}

#define DUMPTOFN() \
  if (dayswithoutincident) { \
    if (fn(f-dayswithoutincident, dayswithoutincident, udata) != dayswithoutincident) { \
      return EOF; \
    } \
    written += dayswithoutincident; \
    dayswithoutincident = 0; \
  }

volatile int kprintf_inpanic = 0;

void kprintf_panic(const char* reason) {
  kprintf_shouldlock = 0;
  kprintf("KERNEL PANIC: %s\n", reason);
  kprintf_inpanic = 1;
  
  // TODO: Disable interrupts?
  
  while (1) {
    // Hot loop
  }
}

int _LIBC_PRINTF_CALLCONV _libc_vfnprintf(_libc_fnprintf_fn_t fn, void* udata, const char* fmt, va_list list) {
  int dayswithoutincident = 0;
  int written = 0;
  const char* f = fmt;
  int c;
  while ((c = *f)) {
    if (c == '%') {
      DUMPTOFN();
      //int has_decimals = 0;
      int decimals = 0;
      nextctrl:
      int tc = *(f+1);
      int longcount = 0; // for counting %ld/%lld
      switch (tc) {
        case '%': {
          f++;
          dayswithoutincident++; // Just let the second '%' be printed with the next string part
        } break;
        case '0':
        case '1':
        case '2':
        case '3':
        case '4':
        case '5':
        case '6':
        case '7':
        case '8':
        case '9': {
          decimals = decimals * 10 + (tc - '0');
          f++;
        } goto nextctrl;
        #ifdef UNUSED
        case '.': {
          has_decimals = 1;
          f++;
        } goto nextctrl;
        case 'f': {
          char buf[20];
          int i = 20;
          double fval = va_arg(list, double);
          //fval = -543.210987654321;
          long ival = (long) fval;
          int isneg = ival < 0;
          unsigned long uval = isneg ? -ival : ival;
          do {
            buf[--i] = (char)('0' + uval % 10);
            uval /= 10;
          } while (uval > 0);
          if (ival < 0) {
            buf[--i] = '-';
          }
          int nwr = fn(buf+i, 20-i, udata);
          if (nwr != 20-i) {
            return EOF;
          }
          written += nwr;
          if (!has_decimals) {
            decimals = 6;
          }
          double x = fabs(fval - (double) ival);
          if (fn(".", 1, udata) != 1) {
            return EOF;
          }
          written++;
          while (decimals > 0) {
            x *= 10;
            decimals--;
            int ix = (int) x;
            char xch = (char) ('0'+(ix%10));
            if (fn(&xch, 1, udata) != 1) {
              return EOF;
            }
            written++;
          }
          f++;
        } break;
        #endif
        case 'S': { // Formatted substring, I thought there was a standard % for it
          char* s = va_arg(list, char*);
          if (!s) {
            s = "(null)";
          }
          int nwr = _libc_vfnprintf(fn, udata, s, list);
          if (nwr == EOF) {
            return EOF;
          }
          written += nwr;
          f++;
        } break;
        
        case 's': {
          char* s = va_arg(list, char*);
          if (!s) {
            s = "(null)";
          }
          int len = strlen(s);
          int nwr = fn(s, len, udata);
          if (nwr != len) {
            return EOF;
          }
          written += nwr;
          f++;
        } break;
        
        case 'c': {
          int chr = va_arg(list, int);
          char chrc = (char) chr;
          if (fn(&chrc, 1, udata) != 1) {
            return EOF;
          }
          written++;
          f++;
        } break;
        
        case 'l': // %ld/%lx is just handled as a special case of %d/%x
          longcount++;
          f++;
          goto nextctrl;
        
        case 'u': // Unsigned is just handled as a special case of %d
        case 'd': {
          // TODO: Testing/edge case for negative maximum?
          char buf[20];
          int i = 20;
          if (longcount) {
            long val = va_arg(list, long);
            int isneg = ((tc == 'd') && (val < 0));
            unsigned long uval = isneg ? -val : val;
            // Not actually needed, unless you want a full string: buf[i--] = '0';
            do {
              buf[--i] = (char)('0' + uval % 10);
              uval /= 10;
            } while (uval > 0);
            if (isneg) {
              buf[--i] = '-';
            }
          } else {
            int val = va_arg(list, int);
            int isneg = ((tc == 'd') && (val < 0));
            unsigned int uval = isneg ? -val : val;
            // Not actually needed, unless you want a full string: buf[i--] = '0';
            do {
              buf[--i] = (char)('0' + uval % 10);
              uval /= 10;
            } while (uval > 0);
            if (isneg) {
              buf[--i] = '-';
            }
          }
          int nwr = fn(buf+i, 20-i, udata);
          if (nwr != 20-i) {
            return EOF;
          }
          written += nwr;
          f++;
        } break;
        
        case 'p': // Pointer is treated as %lx
          longcount++;
        case 'x':
        case 'X': { // Hex is handled a separate case to %d because the loop is slightly slower and sign is never used
          const char* digits = (tc == 'x') ? "0123456789abcdef" : "0123456789ABCDEF";
          char buf[16]; // Size is easier to predict for hex, two characters of digits per byte
          int i = 16;
          if (longcount) {
            unsigned long uval = va_arg(list, unsigned long);
            // Not actually needed, unless you want a full string: buf[i--] = '0';
            do {
              buf[--i] = digits[uval % 16];
              uval /= 16;
            } while (uval > 0);
          } else {
            unsigned int uval = va_arg(list, unsigned int);
            // Not actually needed, unless you want a full string: buf[i--] = '0';
            do {
              buf[--i] = digits[uval % 16];
              uval /= 16;
            } while (uval > 0);
          }
          int nwr = fn(buf+i, 16-i, udata);
          if (nwr != 16-i) {
            return EOF;
          }
          written += nwr;
          f++;
        } break;
        
        default: { // For now just print %x??? when 'x' is unknown, but this is probably unsafe and an error should be reported properly instead (TODO)
          // Also skip the argument (assume all arguments are word-sized for now)
          /* void* _ignored = */ va_arg(list, void*);
          if (fn(f, 2, udata) != 2) {
            return EOF;
          }
          if (fn("???", 3, udata) != 3) {
            return EOF;
          }
          written+=5;
          f++;
        }
      }
    } else {
      dayswithoutincident++;
    }
    f++;
  }
  DUMPTOFN();
  return written;
}

int _kprintf_backend(const char* str, int n, void* udata) {
  for (int i = 0; i < n; i++) {
    consputc(str[i]);
  }
  return n;
}

int _LIBC_PRINTF_CALLCONV kprintf(const char* fmt, ...) {
  //void* udata = (void*)((uintptr_t)1); // stdout
  int shouldlock = kprintf_shouldlock;
  if (shouldlock) {
    acquire(&kprintf_spin);
  }
  va_list varargs;
  va_start(varargs, fmt);
  uintptr_t result = _libc_vfnprintf(&_kprintf_backend, NULL, fmt, varargs);
  va_end(varargs);
  if (shouldlock) {
    release(&kprintf_spin);
  }
  return result;
}

void kprintf_init() {
  initlock(&kprintf_spin, "kprintf_spin");
  kprintf_shouldlock = 1;
}