//
// si/pak.c: Controller pak routines
//
// CEN64: Cycle-Accurate Nintendo 64 Emulator.
// Copyright (C) 2016, Mike Ryan.
//
// This file is subject to the terms and conditions defined in
// 'LICENSE', which is part of this source code package.
//

#ifdef _WIN32
#include <wchar.h>
#endif
#include "pak.h"
#include "pak_transfer.h"

static uint8_t controller_pak_crc(uint8_t *data);

int controller_pak_read(struct controller *controller,
    uint8_t *send_buf, uint8_t send_bytes,
    uint8_t *recv_buf, uint8_t recv_bytes) {

  uint16_t address = send_buf[1] << 8 | send_buf[2];
  address &= ~0x1F; // lower 5 bits are address CRC
  // printf("read from %04x\n", address);

  if (controller->pak == PAK_MEM) {
    if (address <= MEMPAK_SIZE - 0x20)
      memcpy(recv_buf, (uint8_t *) (controller->mempak_save.ptr) + address, 0x20);
    else
      assert(0 && "invalid mempak address");
  }

  else if (controller->pak == PAK_TRANSFER)
    transfer_pak_read(controller, send_buf, send_bytes, recv_buf, recv_bytes);

  else {
    uint8_t peripheral = 0x00;
    if (controller->pak == PAK_RUMBLE)
      peripheral = 0x80;
    memset(recv_buf, peripheral, 0x20);
  }

  recv_buf[0x20] = controller_pak_crc(recv_buf);

  return 0;
}

int controller_pak_write(struct controller *controller,
    uint8_t *send_buf, uint8_t send_bytes,
    uint8_t *recv_buf, uint8_t recv_bytes) {

  uint16_t address = send_buf[1] << 8 | send_buf[2];
  address &= ~0x1F; // lower 5 bits are a checksum
  // printf("write to %04x\n", address);

  if (address == 0x8000) {
    // maybe only for transfer pak, unclear
    if (send_buf[3] == 0xfe)
      controller->pak_enabled = 0;
    else if (send_buf[3] == 0x84)
      controller->pak_enabled = 1;
  }

  else if (controller->pak == PAK_MEM) {
    if (address <= MEMPAK_SIZE - 0x20)
      memcpy((uint8_t *) (controller->mempak_save.ptr) + address, send_buf + 3, 0x20);
    else
      assert(0 && "Attempt to write past end of mempak");
  }

  else if (controller->pak == PAK_RUMBLE) {
    if (address == 0xC000) {
      if (send_buf[3] == 0x01) {
        // printf("Enable rumble\n");
      } else {
        // printf("Disable rumble\n");
      }
    }
    else {
      // printf("Unknown rumble address\n");
    }
  }

  else if (controller->pak == PAK_TRANSFER)
    transfer_pak_write(controller, send_buf, send_bytes, recv_buf, recv_bytes);

  recv_buf[0] = controller_pak_crc(send_buf+3);

  return 0;
}

uint8_t controller_pak_crc(uint8_t *data) {
  size_t i;
  int mask;
  uint8_t crc = 0;

  for (i = 0; i <= 0x20; ++i) {
    for (mask = 0x80; mask >= 1; mask >>= 1) {
      uint8_t xor_tap = (crc & 0x80) ? 0x85 : 0x00;
      crc <<= 1;
      if (i < 0x20 && (data[i] & mask))
        crc |= 1;
      crc ^= xor_tap;
    }
  }

  return crc;
}

void controller_pak_format(uint8_t *ptr) {
  off_t pos;

  static uint8_t init[] = {
    0x81, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08, 0x09, 0x0A, 0x0B, 0x0C, 0x0D, 0x0E, 0x0F,
    0x10, 0x11, 0x12, 0x13, 0x14, 0x15, 0x16, 0x17, 0x18, 0x19, 0x1A, 0x1B, 0x1C, 0x1D, 0x1E, 0x1F,
    0xFF, 0xFF, 0xFF, 0xFF, 0x05, 0x1A, 0x5F, 0x13, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x01, 0xFF, 0x66, 0x25, 0x99, 0xCD,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0xFF, 0xFF, 0xFF, 0xFF, 0x05, 0x1A, 0x5F, 0x13, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x01, 0xFF, 0x66, 0x25, 0x99, 0xCD,
    0xFF, 0xFF, 0xFF, 0xFF, 0x05, 0x1A, 0x5F, 0x13, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x01, 0xFF, 0x66, 0x25, 0x99, 0xCD,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0xFF, 0xFF, 0xFF, 0xFF, 0x05, 0x1A, 0x5F, 0x13, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x01, 0xFF, 0x66, 0x25, 0x99, 0xCD,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x71, 0x00, 0x03, 0x00, 0x03, 0x00, 0x03, 0x00, 0x03, 0x00, 0x03, 0x00, 0x03, 0x00, 0x03,
  };

  memcpy(ptr, init, sizeof(init));
  for (pos = sizeof(init); pos < MEMPAK_SIZE; pos += 2) {
    ptr[pos+0] = 0x00;
    ptr[pos+1] = 0x03;
  }
}