#include <stdint.h>
#include "panic.h"
#include <stdbool.h>
#include "drive.h"

void memcpy(void *to, void *from, uint32_t n) {
  uint32_t *tp = to, *fp = from;
  while (n >= 4) {
    *(tp++) = *(fp++);
    n -= 4;
  }
  uint8_t *tpp = (uint8_t *)tp, *fpp = (uint8_t *)fp;
  while (n--)
    *(tpp++) = *(fpp++);
}

void fmcpy(void *to, struct drive *d, drive_file_id_t f, uint32_t from, uint32_t n) {
  uint8_t buf[512];
  d->load_sector(d, f, from >> 9, buf);
  uint16_t from_low = from & 511;
  if (!((from_low + n) & ~511)) {
    memcpy(to, buf + from_low, n);
    return;
  }

  uint32_t i = 512 - from_low;
  memcpy(to, buf + from_low, i);

  n -= i;
  uint32_t fsi = (from + i) >> 9;

  while (n & ~511) {
    d->load_sector(d, f, fsi, buf);
    memcpy(to + i, buf, 512);
    i += 512;
    n -= 512;
    ++fsi;
  }

  d->load_sector(d, f, fsi, buf);
  memcpy(to + i, buf, n);
}

void u32_dec(uint32_t n, char *b) {
  if (!n) {
    *(uint16_t *)b = (uint16_t)'0';
    return;
  }
  bool zero = false;
  for (uint32_t m = 1000000000; m; m /= 10) {
    uint8_t d = (n / m) % 10;
    if (zero)
      *(b++) = d + '0';
    else if (d) {
      zero = true;
      *(b++) = d + '0';
    }
  }
  *b = '\0';
}

void u16_dec(uint16_t n, char *b) {
  if (!n) {
    *(uint16_t *)b = (uint16_t)'0';
    return;
  }
  bool zero = false;
  for (uint32_t m = 10000; m; m /= 10) {
    uint8_t d = (n / m) % 10;
    if (zero)
      *(b++) = d + '0';
    else if (d) {
      zero = true;
      *(b++) = d + '0';
    }
  }
  *b = '\0';
}

void u8_dec(uint8_t n, char *b) {
  if (!n) {
    *(uint16_t *)b = (uint16_t)'0';
    return;
  }
  bool zero = false;
  for (uint32_t m = 100; m; m /= 10) {
    uint8_t d = (n / m) % 10;
    if (zero)
      *(b++) = d + '0';
    else if (d) {
      zero = true;
      *(b++) = d + '0';
    }
  }
  *b = '\0';
}

void u32_hex(uint32_t n, char *b) {
  uint8_t m = 28;
  while (1) {
    uint8_t d = (n >> m) & 0xf;
    *(b++) = d >= 10 ? 'a' + d - 10 : '0' + d;
    if (!m) {
      *b = '\0';
      return;
    }
    m -= 4;
  }
}

void u16_hex(uint16_t n, char *b) {
  uint8_t m = 12;
  while (1) {
    uint8_t d = (n >> m) & 0xf;
    *(b++) = d >= 10 ? 'a' + d - 10 : '0' + d;
    if (!m) {
      *b = '\0';
      return;
    }
    m -= 4;
  }
}

void u8_hex(uint8_t n, char *b) {
  uint8_t m = 4;
  while (1) {
    uint8_t d = (n >> m) & 0xf;
    *(b++) = d >= 10 ? 'a' + d - 10 : '0' + d;
    if (!m) {
      *b = '\0';
      return;
    }
    m -= 4;
  }
}