#include <SPI.h>
#include <SPIMemory.h>

#define FLASH_CS_PIN 4
SPIFlash flash(FLASH_CS_PIN);

void setup() {
  pinMode(LED_BUILTIN, OUTPUT);
  digitalWrite(LED_BUILTIN, LOW);
  
  Serial.begin(115200);
  while (!Serial) delay(10);
  
  delay(4000);  // Allow boot garbage to clear
  digitalWrite(LED_BUILTIN, HIGH);

  flash.setClock(5000000);  // Cap SPI clock speed at 5 MHz for stability
  //flash.error(true);        // Enable error reporting for troubleshooting

  delay(500); // Allow time to adjust

  if (!flash.begin()) {
    digitalWrite(LED_BUILTIN, LOW);
    Serial.write("ABORTED_DUMP_FAILURE"); // Mark end of failed dump
    // Stop if flash initialization fails and blink error sequence
    blinkError();
  }

  sendFlashInformation(); // Send flash info before the dump
  delay(4000);  // Wait before starting the dump
  Serial.write("START_OF_DUMP\n"); // Mark start of dump

  dumpFlashContents();

  // If dumpFlashContents exits, something went wrong and we should blink error sequence
  blinkError();
}

void loop() {
  // Do nothing
}

void sendFlashInformation() {
  Serial.write("START_OF_INFORM\n");
  Serial.print("JEDEC ID: 0x");
  Serial.println(flash.getJEDECID(), HEX);
  Serial.print("Capacity: ");
  Serial.println(flash.getCapacity());
  Serial.print("Max Page: ");
  Serial.println(flash.getMaxPage());
  Serial.write("END_OF_INFORM\n");
}

void dumpFlashContents() {
  const size_t blockSize = 256;
  uint8_t bufferA[blockSize]; // Two buffers, for reading each block twice
  uint8_t bufferB[blockSize];
  uint32_t address = 0;

  while (address < flash.getCapacity()) {
    bool success = false;

    // Retry reading the block until both reads match or max retries reached
    for (int retries = 0; retries < 5; retries++) {
      if (flash.readByteArray(address, bufferA, blockSize) && flash.readByteArray(address, bufferB, blockSize)) {
            
        // Compare the two buffers
        if (memcmp(bufferA, bufferB, blockSize) == 0) {
          success = true;
          break; // Matching reads, exit retry loop
        }
      }
    }

    if (success) {
      Serial.write(bufferA, blockSize);
    } else {
      // If unable to get consistent reads, indicate failure and stop
      Serial.write("READ_ERROR_AT_ADDRESS\n");
      Serial.print(address);
      break;
    }
    address += blockSize;
    delay(10);
  }

  Serial.write("END_OF_DUMP"); // Mark end of dump
  
  // Blink builtin led forever (until reset) to indicate finished dumping
  blinkSuccess();
}

void blinkError() {
  // Forever (until reset) blink builtin led in the error sequence
  while (true) {
    digitalWrite(LED_BUILTIN, LOW);
    delay(400);
    digitalWrite(LED_BUILTIN, HIGH);
    delay(400);
  }
}

void blinkSuccess() {
  // Forever (until reset) blink builtin led in the success sequence
  while (true) {
    digitalWrite(LED_BUILTIN, LOW);
    delay(800);
    digitalWrite(LED_BUILTIN, HIGH);
    delay(1500);
  }
}