// SD Card 2: Benchmark SD card raw block reads and writes
#include "SFBSDRaw.h"           // Load the SDRaw library

SDRaw mySD;                     // Allocate space for a raw sdcard controller
u8 buffer[512];                 // And a disk-block-sized buffer

bool wantBench = false;
void startBench(u8*) { wantBench = true; }  // Just set a flag and let loop() do it

void setup() {
  mySD.begin(NORTH);            // Set up for 'IXM SD Cell' connectivity on North
  Body.reflex('b',startBench);  // Create reflex
}

void loop() { 
  if (!wantBench) { ledToggle(BODY_RGB_BLUE_PIN);  delay(1000); return; } 
  wantBench = false;
  if (!mySD.init()) { logNormal("Can't init\n"); return; }

  u32 startBlock, blockNumber, start, totalBlocks;
  startBlock = 0;               // Just read from disk start
  blockNumber = startBlock;
  for (start = millis(); IS_EARLIER(millis(),start+1000); ++blockNumber) {
    if (!mySD.readBlock(blockNumber,buffer)) {
      logNormal("Read failed, block %d\n",blockNumber);
      return;
    }
  }
  totalBlocks = blockNumber-startBlock; 
  // 512*totalBlocks == totalBytes; totalBytes/totalMs === KBytes/second
  logNormal("Read %d blocks at %fKB/sec\n", 
            totalBlocks, 512.0*totalBlocks/(millis()-start));

  delay(1000);                  // The pause that refreeshes

  startBlock = blockNumber;     // Start writing where we stopped reading
  for (start = millis(); IS_EARLIER(millis(),start+1000); ++blockNumber) {
    if (!mySD.writeBlock(blockNumber,buffer)) { // Always writing the same data!
      logNormal("Write failed, block %d\n",blockNumber);
      return;
    }
  }
  totalBlocks = blockNumber-startBlock;
  logNormal("Wrote %d blocks at %fKB/sec\n", 
            totalBlocks, 512.0*totalBlocks/(millis()-start));
}
