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

#include "log.h"
#define ACROSS 0x04004e60

void memcpy(void *to, const void *from, uint32_t n) {
  if (((uint32_t)to <= ACROSS) && ((uint32_t)(to + n) > ACROSS)) {
    logf(LOG_WARN, "memcpy across 0x%h!", ACROSS);
    logf(LOG_INFO, "was: memcpy(0x%h, 0x%h, %d)", to, from, n);
  }

  uint32_t *tp = to;
  const uint32_t *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, const struct drive *d, 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 mfcpy(const struct drive *d, file_id_t f, uint32_t to, const void *from, uint32_t n) {
  uint8_t buf[512];

  const uint16_t first_batch = to % 512 + n >= 512 ? 512 - to % 512 : n;
  if (first_batch) {
    d->load_sector(d, f, to / 512, buf);
    memcpy(buf + to % 512, from, first_batch);
    d->save_sector(d, f, to / 512, buf);
    n -= first_batch;
    from += first_batch;
  }

  uint32_t sector_on = to / 512 + 1;
  while (n >= 512) {
    memcpy(buf, from, 512);
    d->save_sector(d, f, sector_on, buf);
    n -= 512;
    from += 512;
    ++sector_on;
  }

  if (n) {
    d->load_sector(d, f, sector_on, buf);
    memcpy(buf, from, n);
    d->save_sector(d, f, sector_on, buf);
  }
}