//******************************************
// NINTENDO POWER
//******************************************

/******************************************
   NP Clock Source
******************************************/
// The clock signal for the Nintendo Power cart
// is generated by the Arduino, the CLK0 jumper
// must be set to the down position.

/******************************************
   Variables
 *****************************************/
// Nintendo Power status
byte NPReady = 0;

/******************************************
  Menu
*****************************************/
// NP flash menu items
const char NPFlashMenuItem1[] PROGMEM = "Read Flash";
const char NPFlashMenuItem2[] PROGMEM = "Write Flash";
const char NPFlashMenuItem3[] PROGMEM = "Print Mapping";
const char NPFlashMenuItem4[] PROGMEM = "Read Mapping";
const char NPFlashMenuItem5[] PROGMEM = "Write Mapping";
const char NPFlashMenuItem6[] PROGMEM = "Back";
const char* const menuOptionsNPFlash[] PROGMEM = {NPFlashMenuItem1, NPFlashMenuItem2, NPFlashMenuItem3, NPFlashMenuItem4, NPFlashMenuItem5, NPFlashMenuItem6};

void NPGameOptions() {}
void NPFlashMenu() {}

void npMenu() {
  // create menu with title and 6 options to choose from
  unsigned char flashSubMenu;
  // Copy menuOptions of of progmem
  convertPgm(menuOptionsNPFlash, 6);
  flashSubMenu = question_box("NP Flash Menu", menuOptions, 6, 0);

  // wait for user choice to come back from the question box menu
  switch (flashSubMenu)
  {
    // Read Flash
    case 0:
      // Clear screen
      display_Clear();

      // Reset to root directory
      sd.chdir("/");

      // Reset to HIROM ALL
      romType = 1;
      print_Msg(F("Switch to HiRom..."));
      display_Update();
      if (send_NP(0x04) == 0x2A) {
        println_Msg(F("OK"));
        display_Update();

        // Reset flash
        resetFlash_NP(0xC0);
        resetFlash_NP(0xE0);

        flashSize = 4194304;
        numBanks = 64;

        // Get name, add extension and convert to char array for sd lib
        EEPROM_readAnything(0, foldern);
        sprintf(fileName, "NP%d", foldern);
        strcat(fileName, ".bin");
        sd.mkdir("NP", true);
        sd.chdir("NP");
        // write new folder number back to eeprom
        foldern = foldern + 1;
        EEPROM_writeAnything(0, foldern);

        // Read flash
        readFlash_NP();
      }
      else {
        print_Error(F("Switch to HiRom failed"), false);
      }
      break;

    // Write Flash
    case 1:
      filePath[0] = '\0';
      sd.chdir("/");
      // Launch file browser
      fileBrowser("Select 4MB file");
      display_Clear();
      sprintf(filePath, "%s/%s", filePath, fileName);
      flashSize = 2097152;
      numBanks = 32;
      println_Msg(F("Writing 1st rom"));
      display_Update();
      // Program 1st flashrom
      write_NP(0xC0, 0);
      display_Clear();
      println_Msg(F("Writing 2nd rom"));
      display_Update();
      // Program 2nd flashrom
      write_NP(0xE0, 2097152);
      break;

    // Print mapping
    case 2:
      // Clear screen
      display_Clear();

      // Reset to root directory
      sd.chdir("/");

      // Reset to HIROM ALL
      romType = 1;
      print_Msg(F("Switch to HiRom..."));
      display_Update();
      if (send_NP(0x04) == 0x2A) {
        println_Msg(F("OK"));
        display_Update();
        idFlash_NP(0xC0);
        if (strcmp(flashid, "c2f3") == 0) {
          idFlash_NP(0xE0);
          if (strcmp(flashid, "c2f3") == 0) {
            // Reset flash
            resetFlash_NP(0xC0);
            resetFlash_NP(0xE0);
            delay_Clock(100);
            // Clear screen
            display_Clear();
            printMapping();
            resetFlash_NP(0xC0);
            resetFlash_NP(0xE0);
          }
          else {
            print_Error(F("Error: Wrong Flash ID"), true);
          }
        }
        else {
          print_Error(F("Error: Wrong Flash ID"), true);
        }
      }
      else {
        print_Error(F("failed"), false);
      }
      break;

    // Read mapping
    case 3:
      // Clear screen
      display_Clear();

      // Reset to root directory
      sd.chdir("/");

      // Reset to HIROM ALL
      romType = 1;
      print_Msg(F("Switch to HiRom..."));
      display_Update();
      if (send_NP(0x04) == 0x2A) {
        println_Msg(F("OK"));
        display_Update();
        idFlash_NP(0xC0);
        if (strcmp(flashid, "c2f3") == 0) {
          idFlash_NP(0xE0);
          if (strcmp(flashid, "c2f3") == 0) {
            // Reset flash
            resetFlash_NP(0xC0);
            resetFlash_NP(0xE0);
            delay_Clock(100);
            readMapping();
            resetFlash_NP(0xC0);
            resetFlash_NP(0xE0);
          }
          else {
            print_Error(F("Error: Wrong Flash ID"), true);
          }
        }
        else {
          print_Error(F("Error: Wrong Flash ID"), true);
        }
      }
      else {
        print_Error(F("failed"), false);
      }
      break;

    // Write mapping
    case 4:
      // Clear screen
      display_Clear();

      // Reset to root directory
      sd.chdir("/");

      // Erase mapping
      eraseMapping(0xD0);
      eraseMapping(0xE0);
      print_Msg(F("Blankcheck..."));
      display_Update();
      if (blankcheckMapping()) {
        println_Msg(F("OK"));
        display_Update();
      }
      else {
        println_Msg(F("Nope"));
        break;
      }

      // Clear screen
      display_Clear();

      // Clear filepath
      filePath[0] = '\0';

      // Reset to root directory
      sd.chdir("/");

      // Launch file browser
      fileBrowser("Select MAP file");
      display_Clear();
      sprintf(filePath, "%s/%s", filePath, fileName);
      display_Update();

      // Write mapping
      writeMapping(0xD0, 0);
      writeMapping(0xE0, 256);
      break;

    // Go back
    case 5:
      mode = mode_NP;
      break;
  }
  if (flashSubMenu != 5) {
    println_Msg(F(""));
    println_Msg(F("Press Button..."));
    display_Update();
    wait();
  }
}

/******************************************
   Setup
 *****************************************/
void setup_NP() {
  // Clock Pin to Output
  DDRE |=  (1 << 3);
  // Clock Pin to Low
  PORTE &= ~(1 << 3);

  // Set cicrstPin(PG1) to Output
  DDRG |= (1 << 1);
  // Output a high signal to disable snesCIC if installed
  PORTG |= (1 << 1);
  // Set cichstPin(PG0) to Input
  DDRG &= ~(1 << 0);

  // Set Address Pins to Output
  //A0-A7
  DDRF = 0xFF;
  //A8-A15
  DDRK = 0xFF;
  //BA0-BA7
  DDRL = 0xFF;

  // Set Control Pins to Output RST(PH0) CS(PH3) WR(PH5) RD(PH6)
  DDRH |= (1 << 0) | (1 << 3) | (1 << 5) | (1 << 6);
  // Output a high signal on all pins, pins are active low therefore everything is disabled now
  PORTH |= (1 << 0) | (1 << 3) | (1 << 5) | (1 << 6);

  // Set IRQ(PH4) to Input
  DDRH &= ~(1 << 4);
  // Activate Internal Pullup Resistors
  PORTH |= (1 << 4);

  // Set Data Pins (D0-D7) to Input
  DDRC = 0x00;
  // Enable Internal Pullups
  PORTC = 0xFF;

  // Wait until all is stable
  delay_Clock(200);

  // Switch to HiRom All
  byte timeout = 0;
  send_NP(0x04);
  delay_Clock(100);
  while (readBank_Clock(0, 0x2400) != 0x2A) {
    delay_Clock(100);
    // Try again
    send_NP(0x04);
    delay_Clock(100);
    timeout++;
    // Abort, something is wrong
    if (timeout == 5) {
      println_Msg(F("Hirom All Timeout"));
      println_Msg(F(""));
      println_Msg(F(""));
      print_Error(F("Please powercycle NP cart"), true);
    }
  }
}

/******************************************
   Clock functions
 *****************************************/
// 500 clock pulses equal 1ms
void delay_Clock(int times) {
  for (int i = 0; i < times * 500; i++) {
    // Switch the clock pin to 0 if it's 1 and 0 if it's 1
    PORTE ^= (1 << 3);
    // Wait 62.5ns
    __asm__("nop\n\t");
  }
}

// Pulses the clock 12 times
void pulse_Clock() {
  for (int i = 0; i < 12; i++) {
    // Switch the clock pin to 0 if it's 1 and 0 if it's 1
    PORTE ^= (1 << 3);
    // Wait 62.5ns
    __asm__("nop\n\t");
  }
}

// Write one byte of data to a location specified by bank and address, 00:0000
void writeBank_Clock(byte myBank, word myAddress, byte myData) {
  PORTL = myBank;
  PORTF = myAddress & 0xFF;
  PORTK = (myAddress >> 8) & 0xFF;
  PORTC = myData;

  // Wait till output is stable
  pulse_Clock();

  // Switch WR(PH5) to LOW
  PORTH &= ~(1 << 5);

  // Leave WR low for at least 60ns
  pulse_Clock();

  // Switch WR(PH5) to HIGH
  PORTH |= (1 << 5);

  // Leave WR high for at least 50ns
  pulse_Clock();
}

// Read one byte of data from a location specified by bank and address, 00:0000
byte readBank_Clock(byte myBank, word myAddress) {
  PORTL = myBank;
  PORTF = myAddress & 0xFF;
  PORTK = (myAddress >> 8) & 0xFF;

  pulse_Clock();

  // Read
  byte tempByte = PINC;
  return tempByte;
}

/******************************************
   29F1601 flashrom functions (NP)
 *****************************************/
// Reset the MX29F1601 flashrom, startbank is 0xC0 for first and 0xE0 for second flashrom
void resetFlash_NP(int startBank) {
  // Configure control pins
  controlOut_SNES();
  // Set data pins to output
  dataOut();

  // Reset command sequence
  if (romType) {
    writeBank_SNES(startBank, 0x5555L * 2, 0xaa);
    writeBank_SNES(startBank, 0x2AAAL * 2, 0x55);
    writeBank_SNES(startBank, 0x5555L * 2, 0xf0);
  }
  else {
    writeBank_SNES(1, 0x8000 + 0x1555L * 2, 0xaa);
    writeBank_SNES(0, 0x8000 + 0x2AAAL * 2, 0x55);
    writeBank_SNES(1, 0x8000 + 0x1555L * 2, 0xf0);
  }
}

// Print flashrom manufacturer and device ID
void idFlash_NP(int startBank) {
  // Configure control pins
  controlOut_SNES();
  // Set data pins to output
  dataOut();

  if (romType) {
    // ID command sequence
    writeBank_SNES(startBank, 0x5555L * 2, 0xaa);
    writeBank_SNES(startBank, 0x2AAAL * 2, 0x55);
    writeBank_SNES(startBank, 0x5555L * 2, 0x90);

    // Set data pins to input again
    dataIn();
    // Set control pins to input
    controlIn_SNES();

    // Read the two id bytes into a string
    sprintf(flashid, "%x%x", readBank_SNES(startBank, 0x00), readBank_SNES(startBank, 0x02));
  }
  else {
    writeBank_SNES(1, 0x8000 + 0x1555L * 2, 0xaa);
    writeBank_SNES(0, 0x8000 + 0x2AAAL * 2, 0x55);
    writeBank_SNES(1, 0x8000 + 0x1555L * 2, 0x90);

    // Set data pins to input again
    dataIn();
    // Set control pins to input
    controlIn_SNES();

    // Read the two id bytes into a string
    sprintf(flashid, "%x%x", readBank_SNES(0, 0x8000), readBank_SNES(0, 0x8000 + 0x02));
  }
}

// Write the flashroms by reading a file from the SD card, pos defines where in the file the reading/writing should start
void writeFlash_NP(int startBank, uint32_t pos) {
  display_Clear();
  print_Msg(F("Writing Bank 0x"));
  print_Msg(startBank, HEX);
  print_Msg(F("..."));
  display_Update();

  // Open file on sd card
  if (myFile.open(filePath, O_READ)) {

    // Seek to a new position in the file
    if (pos != 0)
      myFile.seekCur(pos);

    // Configure control pins
    controlOut_SNES();
    // Set data pins to output
    dataOut();

    if (romType) {
      // Write hirom
      for (int currBank = startBank; currBank < startBank + numBanks; currBank++) {
        // Fill SDBuffer with 1 page at a time then write it repeat until all bytes are written
        for (unsigned long currByte = 0; currByte < 0x10000; currByte += 128) {
          myFile.read(sdBuffer, 128);
          // Write command sequence
          writeBank_SNES(startBank, 0x5555L * 2, 0xaa);
          writeBank_SNES(startBank, 0x2AAAL * 2, 0x55);
          writeBank_SNES(startBank, 0x5555L * 2, 0xa0);

          for (byte c = 0; c < 128; c++) {

            // Write one byte of data
            writeBank_SNES(currBank, currByte + c, sdBuffer[c]);

            if (c == 127) {
              // Write the last byte twice or else it won't write at all
              writeBank_SNES(currBank, currByte + c, sdBuffer[c]);
            }
          }
          // Wait until write is finished
          busyCheck_NP(startBank);
        }
      }
    }
    else {
      // Write lorom
      for (int currBank = 0; currBank < numBanks; currBank++) {
        for (unsigned long currByte = 0x8000; currByte < 0x10000; currByte += 128) {
          myFile.read(sdBuffer, 128);
          // Write command sequence
          writeBank_SNES(1, 0x8000 + 0x1555L * 2, 0xaa);
          writeBank_SNES(0, 0x8000 + 0x2AAAL * 2, 0x55);
          writeBank_SNES(1, 0x8000 + 0x1555L * 2, 0xa0);

          for (byte c = 0; c < 128; c++) {
            // Write one byte of data
            writeBank_SNES(currBank, currByte + c, sdBuffer[c]);

            if (c == 127) {
              // Write the last byte twice or else it won't write at all
              writeBank_SNES(currBank, currByte + c, sdBuffer[c]);
            }
          }
          // Wait until write is finished
          busyCheck_NP(startBank);
        }
      }
    }
    // Close the file:
    myFile.close();
    println_Msg("");
  }
  else {
    print_Error(F("Can't open file on SD"), true);
  }
}

// Delay between write operations based on status register
void busyCheck_NP(byte startBank) {
  // Set data pins to input
  dataIn();
  // Set control pins to input and therefore pull CE low and latch status register content
  controlIn_SNES();

  // Read register
  readBank_SNES(startBank, 0x0000);

  // Read D7 while D7 = 0
  //1 = B00000001, 1 << 7 = B10000000, PINC = B1XXXXXXX (X = don't care), & = bitwise and
  while (!(PINC & (1 << 7))) {
    // CE or OE must be toggled with each subsequent status read or the
    // completion of a program or erase operation will not be evident.
    // Switch RD(PH6) to HIGH
    PORTH |= (1 << 6);

    // one nop ~62.5ns
    __asm__("nop\n\t");

    // Switch RD(PH6) to LOW
    PORTH &= ~(1 << 6);

    // one nop ~62.5ns
    __asm__("nop\n\t");
  }

  // Configure control pins
  controlOut_SNES();
  // Set data pins to output
  dataOut();
}

// Erase the flashrom to 0xFF
void eraseFlash_NP(int startBank) {

  // Configure control pins
  controlOut_SNES();
  // Set data pins to output
  dataOut();

  if (romType) {
    // Erase command sequence
    writeBank_SNES(startBank, 0x5555L * 2, 0xaa);
    writeBank_SNES(startBank, 0x2AAAL * 2, 0x55);
    writeBank_SNES(startBank, 0x5555L * 2, 0x80);
    writeBank_SNES(startBank, 0x5555L * 2, 0xaa);
    writeBank_SNES(startBank, 0x2AAAL * 2, 0x55);
    writeBank_SNES(startBank, 0x5555L * 2, 0x10);
  }
  else {
    writeBank_SNES(1, 0x8000 + 0x1555L * 2, 0xaa);
    writeBank_SNES(0, 0x8000 + 0x2AAAL * 2, 0x55);
    writeBank_SNES(1, 0x8000 + 0x1555L * 2, 0x80);
    writeBank_SNES(1, 0x8000 + 0x1555L * 2, 0xaa);
    writeBank_SNES(0, 0x8000 + 0x2AAAL * 2, 0x55);
    writeBank_SNES(1, 0x8000 + 0x1555L * 2, 0x10);
  }

  // Wait for erase to complete
  busyCheck_NP(startBank);
}

// Check if an erase succeeded, return 1 if blank and 0 if not
byte blankcheck_NP(int startBank) {

  // Set data pins to input again
  dataIn();
  // Set control pins to input
  controlIn_SNES();

  byte blank = 1;
  if (romType) {
    for (int currBank = startBank; currBank < startBank + numBanks; currBank++) {
      for (unsigned long currByte = 0; currByte < 0x10000; currByte++) {
        if (readBank_SNES(currBank, currByte) != 0xFF) {
          currBank =  startBank + numBanks;
          blank = 0;
        }
      }
    }
  }
  else {
    for (int currBank = 0; currBank < numBanks; currBank++) {
      for (unsigned long currByte = 0x8000; currByte < 0x10000; currByte++) {
        if (readBank_SNES(currBank, currByte) != 0xFF) {
          currBank = numBanks;
          blank = 0;
        }
      }
    }
  }
  return blank;
}

// Check if a write succeeded, returns 0 if all is ok and number of errors if not
unsigned long verifyFlash_NP(int startBank, uint32_t pos) {
  unsigned long  verified = 0;

  // Open file on sd card
  if (myFile.open(filePath, O_READ)) {

    // Set file starting position
    myFile.seekCur(pos);

    // Set data pins to input
    dataIn();
    // Set control pins to input
    controlIn_SNES();

    if (romType) {
      for (int currBank = startBank; currBank < startBank + numBanks; currBank++) {
        for (unsigned long currByte = 0; currByte < 0x10000; currByte += 512) {
          // Fill SDBuffer
          myFile.read(sdBuffer, 512);
          for (int c = 0; c < 512; c++) {
            if (readBank_SNES(currBank, currByte + c) != sdBuffer[c]) {
              verified++;
            }
          }
        }
      }
    }
    else {
      for (int currBank = 0; currBank < numBanks; currBank++) {
        for (unsigned long currByte = 0x8000; currByte < 0x10000; currByte += 512) {
          // Fill SDBuffer
          myFile.read(sdBuffer, 512);
          for (int c = 0; c < 512; c++) {
            if (readBank_SNES(currBank, currByte + c) != sdBuffer[c]) {
              verified++;
            }
          }
        }
      }
    }
    // Close the file:
    myFile.close();
  }
  else {
    // SD Error
    verified = 999999;
    print_Error(F("Can't open file on SD"), false);
  }
  // Return 0 if verified ok, or number of errors
  return verified;
}

// Read flashroms and save them to the SD card
void readFlash_NP() {
  // Set data pins to input
  dataIn();
  // Set control pins to input
  controlIn_SNES();

  print_Msg(F("Saving as "));
  print_Msg(fileName);
  println_Msg(F("..."));
  display_Update();

  // Open file on sd card
  if (!myFile.open(fileName, O_RDWR | O_CREAT)) {
    print_Error(F("Can't create file on SD"), true);
  }
  if (romType) {
    for (int currBank = 0xC0; currBank < 0xC0 + numBanks; currBank++) {
      for (unsigned long currByte = 0; currByte < 0x10000; currByte += 512) {
        for (int c = 0; c < 512; c++) {
          sdBuffer[c] = readBank_SNES(currBank, currByte + c);
        }
        myFile.write(sdBuffer, 512);
      }
    }
  }
  else {
    for (int currBank = 0; currBank < numBanks; currBank++) {
      for (unsigned long currByte = 0x8000; currByte < 0x10000; currByte += 512) {
        for (int c = 0; c < 512; c++) {
          sdBuffer[c] = readBank_SNES(currBank, currByte + c);
        }
        myFile.write(sdBuffer, 512);
      }
    }
  }
  // Close the file:
  myFile.close();
  println_Msg("");
  println_Msg(F("Finished reading"));
  display_Update();
}

// Display protected sectors/banks as 0xc2 and unprotected as 0x00
void readSectorProtection_NP(byte startBank) {

  // Configure control pins
  controlOut_SNES();
  // Set data pins to output
  dataOut();

  // Display Sector Protection Status
  writeBank_SNES(startBank, 0x5555L * 2, 0xaa);
  writeBank_SNES(startBank, 0x2AAAL * 2, 0x55);
  writeBank_SNES(startBank, 0x5555L * 2, 0x90);

  // Configure control pins
  controlIn_SNES();
  // Set data pins to output
  dataIn();
  display_Clear();
  for (int i = 0; i <= 0x1F; i++) {
    print_Msg(F("Sector: 0x"));
    print_Msg(startBank + i, HEX);
    print_Msg(F(" Sector Protect: 0x"));
    println_Msg(readBank_SNES(startBank + i, 0x04), HEX);
  }
  display_Update();
}

// Read the current mapping from the hidden "page buffer" and print it
void printMapping() {
  // Switch to write
  dataOut();
  controlOut_SNES();

  // Reset to defaults
  writeBank_SNES(0xC0, 0x0000, 0x38);
  writeBank_SNES(0xC0, 0x0000, 0xd0);
  // Read Extended Status Register (GSR and PSR)
  writeBank_SNES(0xC0, 0x0000, 0x71);
  // Page Buffer Swap
  writeBank_SNES(0xC0, 0x0000, 0x72);
  // Read Page Buffer
  writeBank_SNES(0xC0, 0x0000, 0x75);

  // Switch to read
  dataIn();
  controlIn_SNES();

  // Read the mapping out of the first chip
  char buffer[3];

  for (int currByte = 0xFF00; currByte < 0xFF50; currByte += 10) {
    for (int c = 0; c < 10; c++) {
      itoa (readBank_SNES(0xC0, currByte + c), buffer, 16);
      for (int i = 0; i < 2 - strlen(buffer); i++) {
        print_Msg("0");
      }
      // Now print the significant bits
      print_Msg(buffer);
    }
    println_Msg("");
  }
  display_Update();

  // Switch to write
  dataOut();
  controlOut_SNES();

  // Reset Flash
  writeBank_SNES(0xC0, 0x5555L * 2, 0xaa);
  writeBank_SNES(0xC0, 0x2AAAL * 2, 0x55);
  writeBank_SNES(0xC0, 0x5555L * 2, 0xf0);

  // Reset Flash
  writeBank_SNES(0xE0, 0x5555L * 2, 0xaa);
  writeBank_SNES(0xE0, 0x2AAAL * 2, 0x55);
  writeBank_SNES(0xE0, 0x5555L * 2, 0xf0);

  // Switch to read
  dataIn();
  controlIn_SNES();
}

// Read the current mapping from the hidden "page buffer"
void readMapping() {

  // Switch to write
  dataOut();
  controlOut_SNES();

  // Reset to defaults
  writeBank_SNES(0xC0, 0x0000, 0x38);
  writeBank_SNES(0xC0, 0x0000, 0xd0);
  // Read Extended Status Register (GSR and PSR)
  writeBank_SNES(0xC0, 0x0000, 0x71);
  // Page Buffer Swap
  writeBank_SNES(0xC0, 0x0000, 0x72);
  // Read Page Buffer
  writeBank_SNES(0xC0, 0x0000, 0x75);

  // Switch to read
  dataIn();
  controlIn_SNES();

  // Get name, add extension and convert to char array for sd lib
  EEPROM_readAnything(0, foldern);
  sprintf(fileName, "NP%d", foldern);
  strcat(fileName, ".MAP");
  sd.mkdir("NP", true);
  sd.chdir("NP");

  // write new folder number back to eeprom
  foldern = foldern + 1;
  EEPROM_writeAnything(0, foldern);

  //open file on sd card
  if (!myFile.open(fileName, O_RDWR | O_CREAT)) {
    print_Error(F("SD Error"), true);
  }

  // Read the mapping info out of the 1st chip
  for (unsigned long currByte = 0xFF00; currByte <= 0xFFFF; currByte++) {
    myFile.write(readBank_SNES(0xC0, currByte));
  }

  // Switch to write
  dataOut();
  controlOut_SNES();

  // Reset to defaults
  writeBank_SNES(0xE0, 0x0000, 0x38);
  writeBank_SNES(0xE0, 0x0000, 0xd0);
  // Read Extended Status Register (GSR and PSR)
  writeBank_SNES(0xE0, 0x0000, 0x71);
  // Page Buffer Swap
  writeBank_SNES(0xE0, 0x0000, 0x72);
  // Read Page Buffer
  writeBank_SNES(0xE0, 0x0000, 0x75);

  // Switch to read
  dataIn();
  controlIn_SNES();

  // Read the mapping info out of the 1st chip
  for (unsigned long currByte = 0xFF00; currByte <= 0xFFFF; currByte++) {
    myFile.write(readBank_SNES(0xE0, currByte));
  }

  // Close the file:
  myFile.close();

  // Switch to write
  dataOut();
  controlOut_SNES();

  // Reset Flash
  writeBank_SNES(0xC0, 0x5555L * 2, 0xaa);
  writeBank_SNES(0xC0, 0x2AAAL * 2, 0x55);
  writeBank_SNES(0xC0, 0x5555L * 2, 0xf0);

  // Reset Flash
  writeBank_SNES(0xE0, 0x5555L * 2, 0xaa);
  writeBank_SNES(0xE0, 0x2AAAL * 2, 0x55);
  writeBank_SNES(0xE0, 0x5555L * 2, 0xf0);

  // Switch to read
  dataIn();
  controlIn_SNES();

  // Signal end of process
  print_Msg(F("Saved to NP/"));
  println_Msg(fileName);
  display_Update();
}

void eraseMapping(byte startBank) {

  if (unlockHirom()) {
    // Get ID
    idFlash_NP(startBank);
    if (strcmp(flashid, "c2f3") == 0) {
      resetFlash_NP(startBank);

      // Switch to write
      dataOut();
      controlOut_SNES();

      // Prepare to erase/write Page Buffer
      writeBank_SNES(startBank, 0x5555L * 2, 0xaa);
      writeBank_SNES(startBank, 0x2AAAL * 2, 0x55);
      writeBank_SNES(startBank, 0x5555L * 2, 0x77);
      // Erase Page Buffer
      writeBank_SNES(startBank, 0x5555L * 2, 0xaa);
      writeBank_SNES(startBank, 0x2AAAL * 2, 0x55);
      writeBank_SNES(startBank, 0x5555L * 2, 0xe0);

      // Wait until complete
      busyCheck_NP(startBank);

      // Switch to read
      dataIn();
      controlIn_SNES();
    }
    else {
      print_Error(F("Error: Wrong Flash ID"), true);
    }
  }
  else {
    print_Error(F("Unlock failed"), true);
  }
}

// Check if the current mapping is all 0xFF
byte blankcheckMapping() {
  byte blank = 1;

  // Switch to write
  dataOut();
  controlOut_SNES();

  // Reset to defaults
  writeBank_SNES(0xC0, 0x0000, 0x38);
  writeBank_SNES(0xC0, 0x0000, 0xd0);
  // Read Extended Status Register (GSR and PSR)
  writeBank_SNES(0xC0, 0x0000, 0x71);
  // Page Buffer Swap
  writeBank_SNES(0xC0, 0x0000, 0x72);
  // Read Page Buffer
  writeBank_SNES(0xC0, 0x0000, 0x75);

  // Switch to read
  dataIn();
  controlIn_SNES();

  // Read the mapping info out of the 1st chip
  for (unsigned long currByte = 0xFF00; currByte <= 0xFFFF; currByte++) {
    if (readBank_SNES(0xC0, currByte) != 0xFF) {
      blank = 0;
    }
  }

  // Switch to write
  dataOut();
  controlOut_SNES();

  // Reset to defaults
  writeBank_SNES(0xE0, 0x0000, 0x38);
  writeBank_SNES(0xE0, 0x0000, 0xd0);
  // Read Extended Status Register (GSR and PSR)
  writeBank_SNES(0xE0, 0x0000, 0x71);
  // Page Buffer Swap
  writeBank_SNES(0xE0, 0x0000, 0x72);
  // Read Page Buffer
  writeBank_SNES(0xE0, 0x0000, 0x75);

  // Switch to read
  dataIn();
  controlIn_SNES();

  // Read the mapping info out of the 1st chip
  for (unsigned long currByte = 0xFF00; currByte <= 0xFFFF; currByte++) {
    if (readBank_SNES(0xE0, currByte) != 0xFF) {
      blank = 0;
    }
  }

  // Switch to write
  dataOut();
  controlOut_SNES();

  // Reset Flash
  writeBank_SNES(0xC0, 0x5555L * 2, 0xaa);
  writeBank_SNES(0xC0, 0x2AAAL * 2, 0x55);
  writeBank_SNES(0xC0, 0x5555L * 2, 0xf0);

  // Reset Flash
  writeBank_SNES(0xE0, 0x5555L * 2, 0xaa);
  writeBank_SNES(0xE0, 0x2AAAL * 2, 0x55);
  writeBank_SNES(0xE0, 0x5555L * 2, 0xf0);

  // Switch to read
  dataIn();
  controlIn_SNES();

  return blank;
}

void writeMapping(byte startBank, uint32_t pos) {

  if (unlockHirom()) {
    // Get ID
    idFlash_NP(startBank);
    if (strcmp(flashid, "c2f3") == 0) {
      resetFlash_NP(startBank);

      // Switch to write
      dataOut();
      controlOut_SNES();

      // Open file on sd card
      if (myFile.open(filePath, O_READ)) {

        // Seek to a new position in the file
        if (pos != 0)
          myFile.seekCur(pos);

        // Write to Page Buffer
        for (unsigned long currByte = 0xFF00; currByte < 0xFFFF; currByte += 128) {
          // Prepare to erase/write Page Buffer
          writeBank_SNES(startBank, 0x5555L * 2, 0xaa);
          writeBank_SNES(startBank, 0x2AAAL * 2, 0x55);
          writeBank_SNES(startBank, 0x5555L * 2, 0x77);
          // Write Page Buffer Command
          writeBank_SNES(startBank, 0x5555L * 2, 0xaa);
          writeBank_SNES(startBank, 0x2AAAL * 2, 0x55);
          writeBank_SNES(startBank, 0x5555L * 2, 0x99);

          myFile.read(sdBuffer, 128);

          for (byte c = 0; c < 128; c++) {
            writeBank_SNES(startBank, currByte + c, sdBuffer[c]);
            // Write last byte twice
            if (c == 127) {
              writeBank_SNES(startBank, currByte + c, sdBuffer[c]);
            }
          }
          busyCheck_NP(startBank);
        }

        // Close the file:
        myFile.close();
        println_Msg("");
      }
      else {
        print_Error(F("Can't open file on SD"), false);
      }

      // Switch to read
      dataIn();
      controlIn_SNES();
    }
    else {
      print_Error(F("Error: Wrong Flash ID"), true);
    }
  }
  else {
    print_Error(F("Unlock failed"), true);
  }
}

/******************************************
  Nintendo Power functions
*****************************************/
// Switch to HiRom All and unlock Write Protection
boolean unlockHirom() {
  romType = 1;
  print_Msg(F("Switch to HiRom..."));
  display_Update();
  if (send_NP(0x04) == 0x2A) {
    println_Msg(F("OK"));
    display_Update();
    // Unlock Write Protection
    print_Msg(F("Enable Write..."));
    display_Update();
    send_NP(0x02);
    if (readBank_Clock(0, 0x2401) == 0x4) {
      println_Msg(F("OK"));
      display_Update();
      return 1;
    }
    else {
      println_Msg(F("failed"));
      display_Update();
      return 0;
    }
  }
  else {
    println_Msg(F("failed"));
    display_Update();
    return 0;
  }
}

// Send a command to the MX15001 chip
byte send_NP(byte command) {
  // Switch to write
  dataOut();
  controlOut_SNES();
  pulse_Clock();

  // Write command
  writeBank_Clock(0, 0x2400, 0x09);
  pulse_Clock();

  // Switch to read
  dataIn();
  controlIn_SNES();
  pulse_Clock();

  // Read status
  NPReady = readBank_Clock(0, 0x2400);

  // Switch to write
  dataOut();
  controlOut_SNES();
  pulse_Clock();

  writeBank_Clock(0, 0x2401, 0x28);
  pulse_Clock();
  writeBank_Clock(0, 0x2401, 0x84);
  pulse_Clock();

  // NP_CMD_06h, send this only if above read has returned 7Dh, not if it's already returning 2Ah
  if (NPReady == 0x7D) {
    writeBank_Clock(0, 0x2400, 0x06);
    pulse_Clock();
    writeBank_Clock(0, 0x2400, 0x39);
    pulse_Clock();
  }

  // Write the command
  writeBank_Clock(0, 0x2400, command);
  pulse_Clock();

  // Switch to read
  dataIn();
  controlIn_SNES();
  pulse_Clock();

  // Read status
  NPReady = readBank_Clock(0, 0x2400);
  return NPReady;
}

// This function will erase and program the NP cart from a 4MB file off the SD card
void write_NP(int startBank, uint32_t pos) {
  // Switch NP cart's mapping
  if (unlockHirom()) {
    // Get ID
    idFlash_NP(startBank);
    if (strcmp(flashid, "c2f3") == 0) {
      print_Msg(F("Flash ID: "));
      println_Msg(flashid);
      display_Update();
      resetFlash_NP(startBank);
      delay_Clock(1000);
      // Erase flash
      print_Msg(F("Blankcheck..."));
      display_Update();
      if (blankcheck_NP(startBank)) {
        println_Msg(F("OK"));
        display_Update();
      }
      else {
        println_Msg(F("Nope"));
        display_Clear();
        print_Msg(F("Erasing..."));
        display_Update();
        eraseFlash_NP(startBank);
        resetFlash_NP(startBank);
        println_Msg(F("Done"));
        print_Msg(F("Blankcheck..."));
        display_Update();
        if (blankcheck_NP(startBank)) {
          println_Msg(F("OK"));
          display_Update();
        }
        else {
          print_Error(F("Could not erase flash"), true);
        }
      }
      // Write flash
      writeFlash_NP(startBank, pos);

      // Reset flash
      resetFlash_NP(startBank);

      // Checking for errors
      print_Msg(F("Verifying..."));
      display_Update();
      writeErrors = verifyFlash_NP(startBank, pos);
      if (writeErrors == 0) {
        println_Msg(F("OK"));
        display_Update();
      }
      else {
        print_Msg(F("Error: "));
        print_Msg(writeErrors);
        println_Msg(F(" bytes "));
        print_Error(F("did not verify."), true);
      }
    }
    else {
      print_Error(F("Error: Wrong Flash ID"), true);
    }
  }
  else {
    print_Error(F("Unlock failed"), true);
  }
}

//******************************************
// End of File
//******************************************