// SD Card 3: Streaming buffered SD card I/O via packet commands
#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 initted = false;           // So we can blink different if initted
u64 lastAddress;                // Remember where we where

void doInit(u8 * packet) {      // E.g., 'iw' tries to init west
  u8 face;
  if (packetScanf(packet,"i%F\n",&face) != 3) return; // Ignore bad formats
  mySD.begin(face);             // Set up for 'IXM SD Cell' connectivity 
  if ((initted = mySD.init())) {     // Try to init, remember if it worked
    mySD.setBlockBuffer(buffer,512); // Success!  Provide a block buffer
    logNormal("Card has %d blocks\n", mySD.getBlockCount());
  }
  lastAddress = 0;              // Reset position
}
void doBlock(u8 * packet) {     // Sets lastAddress to some block number
  if (!initted) return;         // Not ready
  u32 block;
  if (packetScanf(packet,"b%d\n",&block) != 3) return; // Ignore bad formats
  lastAddress = ((u64) block)*512;  // Convert to byte address
}
void doWrite(u8 * packet) {     // Writes contents of packet starting at lastAddress
  if (!initted) return;         // Not ready
  u32 len = packetLength(packet)-1; // All but the leading 'w'
  if (mySD.write(lastAddress,&packet[1],len)) lastAddress += len;
  else logNormal("Write failed\n");
}
void doRead(u8 *) {             // Read 32 bytes starting at lastAddress
  if (!initted) return;         // Not ready
  pprintf("L:");                // Start the response
  for (u32 i = 0; i < 32; ++i) {
    u8 ch;
    if (!mySD.read(lastAddress++,&ch,1)) break; // Read a byte if possible
    pprintf("%#c",ch);          // Send it out, escaping 'non-printable' bytes
  }
  pprintf("\n");                // End the response
}
void doSync(u8 *) {
  if (!initted) return;         // Not ready
  mySD.sync();                  // Do it
}
void setup() {
  Body.reflex('i',doInit);      // Try to init the card
  Body.reflex('b',doBlock);     // Specify block number to start at
  Body.reflex('r',doRead);      // Read some bytes from the card
  Body.reflex('w',doWrite);     // Write some bytes to the card
  Body.reflex('s',doSync);      // Ensure everything's been written
}

void loop() { 
  u8 pin = initted?BODY_RGB_GREEN_PIN:BODY_RGB_RED_PIN;
  ledOn(pin); 
  delay(1000);
  ledOff(pin); 
  delay(1000);
}
