#include "SDBlock.h"

void SDBlock::begin(uint32_t dId) {
  deviceId = dId;

  // XXX Change to checking each is SECTOR_SIZE or report !
  if (stream && debug) {
    stream->println(F("# SDBlock sizes: "));
    stream->print(F("#  buffer: "));
    stream->println(sizeof(buffer));
    stream->print(F("#  allocation: "));
    stream->println(sizeof(SDBlock_Allocation));
    stream->print(F("#  metadata: "));
    stream->println(sizeof(SDBlock_Metadata));
    stream->print(F("#  loglines: "));
    stream->println(sizeof(SDBlock_LogLines));
    stream->print(F("#  queue: "));
    stream->println(sizeof(SDBlock_Queue));
    stream->print(F("#  block: "));
    stream->println(sizeof(SDBlock_Block));

    stream->print(F("# SDBlock Address Defaults: "));
    stream->print(F("  allocation: "));
    stream->print((uint32_t)SDBLOCK_ACTUAL_ALLOCATION);
    stream->print(F(" - "));
    stream->print((uint32_t)SDBLOCK_ACTUAL_ALLOCATION + SDBLOCK_ALLOCATION_MAX);
    stream->println();

    stream->print(F("#  metadata: "));
    stream->print((uint32_t)SDBLOCK_ACTUAL_METADATA);
    stream->print(F(" - "));
    stream->print((uint32_t)SDBLOCK_ACTUAL_METADATA + SDBLOCK_METADATA_MAX);
    stream->println();

    stream->print(F("#  single: "));
    stream->print((uint32_t)SDBLOCK_ACTUAL_SINGLE);
    stream->print(F(" - "));
    stream->print((uint32_t)SDBLOCK_ACTUAL_SINGLE + SDBLOCK_SINGLE_MAX);
    stream->println();

    stream->print(F("#  loglines: "));
    stream->print((uint32_t)SDBLOCK_ACTUAL_LOGLINES);
    stream->print(F(" - "));
    stream->print((uint32_t)SDBLOCK_ACTUAL_LOGLINES + SDBLOCK_LOGLINES_MAX);
    stream->println();

    stream->print(F("#  queue: "));
    stream->print((uint32_t)SDBLOCK_ACTUAL_QUEUE);
    stream->print(F(" - "));
    stream->print((uint32_t)SDBLOCK_ACTUAL_QUEUE + SDBLOCK_QUEUE_MAX);
    stream->println();

    stream->print(F("#  data: "));
    stream->print((uint32_t)SDBLOCK_ACTUAL_DATA);
    stream->print(F(" - "));
    stream->print((uint32_t)SDBLOCK_ACTUAL_DATA + SDBLOCK_DATA_MAX);
    stream->println();

    stream->print(F("#  max: "));
    stream->print((uint32_t)SDBLOCK_MAX);
    stream->println();

    // Coming up as 3% but shoudl be 72%
    // stream->print(F("  percent: "));
    // stream->print((uint32_t)(((SDBLOCK_ACTUAL_DATA + SDBLOCK_DATA_MAX) * (uint32_t)100) / (uint32_t)SDBLOCK_MAX));
    // stream->println("%");
  }

  autoFormat = false;
}
void SDBlock::end() {
  // SPI.end() /?
}

void SDBlock::setAutoFormat(bool in) {
  autoFormat = in;
}
bool SDBlock::getAutoFormat() {
  return autoFormat;
}

void SDBlock::setDeviceId(uint32_t dId) {
  deviceId = dId;
}

void SDBlock::setRecordId(uint32_t rId) {
  recordId = rId;
}

bool SDBlock::cardIsOpen() {
  return valid;
}

// Close off card, ignoreing any data, ie, it will all be lose
void SDBlock::cardForceClose() {
  valid = false;
  dataBlockCurrent = 0;
  dataBlockStart = 0;
  metadata_dataBlockStart = 0;
  metadata_dataBlocks = 0;
  metadata_resultsBlockStart = 0;
  metadata_resultsBlocks = 0;
}

bool SDBlock::cardErase() {
#ifdef USE_SDFS
  if (!_sdinit()) {
    if (stream)
      stream->println(F("# SDBlock - Init card failed"));
    return false;
  }

  if (stream)
    stream->println(F("# SDBlock: erase started: "));

  uint32_t firstBlock = 0;
  uint32_t lastBlock;
  uint16_t n = 0;
  uint32_t cardSectorCount = sd.card()->sectorCount();

  do {
    lastBlock = firstBlock + ERASE_SIZE - 1;
    if (lastBlock >= cardSectorCount) {
      lastBlock = cardSectorCount - 1;
    }
    if (!sd.card()->erase(firstBlock, lastBlock)) {
      if (stream)
        stream->println(F("# SDBlock: ERROR - erase failed"));
      return false;
    }
    if (stream)
      stream->print('.');
    if ((n++)%64 == 63) {
      if (stream)
        stream->println();
    }
    firstBlock += ERASE_SIZE;
  } while (firstBlock < cardSectorCount);
  stream->println();

  if (!read(0)) {
    if (stream)
      stream->println(F("# SDBlock: ERROR - read block failed"));
    return false;
  }
  if (stream)
    stream->println(F("# SDBlock: erase done"));
  return true;
#else
  if (stream)
    stream->println(F("# SDBlock: driver does not support erase"));
  return false;
#endif
}

bool SDBlock::_sdinit() {
  // Initialize SD Card
  #ifdef USE_SDCORE
  if (!SDCore::begin()) {
    if (!SDCore::begin()) {
      if (stream) {
        stream->println(F("# SDBlock Initialization: FAILED - Make sure than you have your wiring correct and a compatible SD card inserted."));
      }
      return false;
    }
  }
  #endif

  #ifdef USE_SD2CARD
  if (!sd.init()) {
    if (!sd.init()) {
      if (stream)
        stream->println(F("# SDBlock Initialization: FAILED - Make sure than you have your wiring correct and a compatible SD card inserted."));
      return false;
    }
  }
  // TODO Keep
  size = sd.cardSize();
  if (stream) {
    stream->print(F("# SDBlock CardSize (blocks): "));
    stream->print((uint32_t)size);
    stream->println();
  }
  #endif

  #ifdef USE_SDFS
  SdCardFactory cardFactory;
  m_card = cardFactory.newCard(SD_CONFIG);

  /*
  if (!sd.cardBegin(SD_CONFIG)) {
    if (!sd.cardBegin(SD_CONFIG)) {
      if (stream)
        stream->println(F("# SDBlock Initialization: FAILED - Make sure than you have your wiring correct and a compatible SD card inserted."));
      return false;
    }
  }
  */
  #endif

  return true;
}

bool SDBlock::cardOpen(uint32_t cId, bool format) {
  checkInternals();

  valid = false;
  dataBlockCurrent = 0;
  dataBlockStart = 0;

	dataBlockNext = 0;
	metadataBlockNext = 0;
	singleBlockNext = 0;
	loglinesBlockNext = 0;
	queueBlockNext = 0;
  metadata_dataBlockStart = 0;
  metadata_dataBlocks = 0;
  metadata_resultsBlockStart = 0;
  metadata_resultsBlocks = 0;

  // Setup pointers (can be done just once)
  allocation = (SDBlock_Allocation*)&buffer;
  metadata = (SDBlock_Metadata*)&buffer;
  loglines = (SDBlock_LogLines*)&buffer;
  block = (SDBlock_Block*)&buffer;

  if (!_sdinit()) {
    if (stream)
      stream->println(F("# SDBlock - Init card failed"));
    return false;
  }

  cardId = cId;

  if (format) {
    if (stream)
      stream->println(F("# SDBlock - Format Requested"));
    allocationDefault();
  }

  if (!allocationRead()) {
    if (stream) {
      stream->println(F("# SDBlock Initialization: Allocation Read Failed"));
    }
    return false;
  }

	dataBlockNext = allocation->dataBlockNext;	// Next, becasue first record is 0, so initilaly we write there
	metadataBlockNext = allocation->metadataBlockNext;	// And of course where is the Metadata
	singleBlockNext = allocation->singleBlockNext;	// And of course where is the Metadata
	loglinesBlockNext = allocation->loglinesBlockNext;	// And of course where is the Metadata
	queueBlockNext = allocation->queueBlockNext;	// And of course where is the Metadata

  if (stream && debug) {
    stream->print(F("# SDBlock next: "));
    stream->print(F("; cardId: expected="));
    stream->print(cardId);
    stream->print(F(" allocated="));
    stream->println(allocation->cardId);
    stream->print(F("#  metadata: next="));
    stream->print(metadataBlockNext);
    stream->print(F(" records="));
    stream->print(metadataBlockNext - SDBLOCK_ACTUAL_METADATA);

    stream->print(F("; data: next="));
    stream->print(dataBlockNext);
    stream->print(F(" records="));
    stream->println(dataBlockNext - SDBLOCK_ACTUAL_DATA);

    stream->print(F("#  single: next="));
    stream->print(singleBlockNext);
    stream->print(F(" records="));
    stream->print(singleBlockNext - SDBLOCK_ACTUAL_SINGLE);

    stream->print(F("; loglines: next="));
    stream->print(loglinesBlockNext);
    stream->print(F(" records="));
    stream->print(loglinesBlockNext - SDBLOCK_ACTUAL_LOGLINES);

    stream->print(F("; queue: next="));
    stream->print(queueBlockNext);
    stream->print(F(" records="));
    stream->print(queueBlockNext - SDBLOCK_ACTUAL_QUEUE);
    stream->println();

  }

	valid = true;
  return true;
}

void SDBlock::checkInternals() {
  if (stream) {
		if (sizeof(SDBlock_Allocation) != SDBLOCK_SECTOR_SIZE) {
      stream->print(F("\n\n\n*********\nAllocation Wrong size: "));
      stream->println(sizeof(SDBlock_Allocation));
    }
		if (sizeof(SDBlock_Metadata) != SDBLOCK_SECTOR_SIZE) {
      stream->print(F("\n\n\n*********\nMetadata Wrong size: "));
      stream->println(sizeof(SDBlock_Metadata));
    }
		if (sizeof(SDBlock_LogLines) != SDBLOCK_SECTOR_SIZE) {
      stream->print(F("\n\n\n*********\nLogLines Wrong size: "));
      stream->println(sizeof(SDBlock_LogLines));
    }
		if (sizeof(SDBlock_Queue) != SDBLOCK_SECTOR_SIZE) {
      stream->print(F("\n\n\n*********\nQueue Wrong size: "));
      stream->println(sizeof(SDBlock_Queue));
    }
		if (sizeof(SDBlock_Block) != SDBLOCK_SECTOR_SIZE) {
      stream->print(F("\n\n\n*********\nBlock Wrong size: "));
      stream->println(sizeof(SDBlock_Block));
    }
  }
}

bool SDBlock::cardClose() {
  allocationUpdate();
  allocationRead();
  // XXX Write allocation changes
  #ifdef USE_SDCORE
  SDCore::end();
  #endif

  // SPI.end();
  valid = false;
  dataBlockCurrent = 0;
  dataBlockStart = 0;
  return true;
}

bool SDBlock::read(unsigned long address_in) {
  #ifdef USE_SDCORE
  if (!SDCore::read(address_in, buffer)) {
    if (!SDCore::read(address_in, buffer)) {
      if (stream) {
        stream->print(F("# SDBlock: Failed to read at block: "));
        stream->println(address_in);
      }
      return false;
    }
  }
  #endif

  #ifdef USE_SD2CARD
  if (!sd.readBlock(address_in, (byte*)buffer)) {
    if (!sd.readBlock(address_in, (byte*)buffer)) {
      if (stream) {
        stream->print(F("# SDBlock: Failed to read at block: "));
        stream->println(address_in);
      }
      return false;
    }
  }
  #endif

  #ifdef USE_SDFS
  if (!sd.card()->readSingle(address_in, (uint8_t*)&buffer)) {
    if (!sd.card()->readSingle(address_in, (uint8_t*)&buffer)) {
      if (stream) {
        stream->print(F("# SDBlock: Failed to read at block: "));
        stream->println(address_in);
      }
      return false;
    }
  }
  #endif

  return true;
}

bool SDBlock::write(unsigned long address_in) {
  // XXX Check address_in in range !!!
  #ifdef USE_SDCORE
  if (!SDCore::write(address_in, buffer)) {
    if (!SDCore::write(address_in, buffer)) {
      if (stream) {
        stream->print(F("# SDBlock: Failed to write at block: "));
        stream->println(address_in);
      }
      return false;
    }
  }
  #endif
  #ifdef USE_SD2CARD
  if (!sd.writeBlock(address_in, (byte*)buffer)) {
    if (!sd.writeBlock(address_in, (byte*)buffer)) {
      if (stream) {
        stream->print(F("# SDBlock: Failed to write at block: "));
        stream->println(address_in);
      }
      return false;
    }
  }
  #endif
  #ifdef USE_SDFS
  if (!sd.card()->writeSingle(address_in, (uint8_t*)&buffer)) {
    if (!sd.card()->writeSingle(address_in, (uint8_t*)&buffer)) {
      if (stream) {
        stream->print(F("# SDBlock: Failed to read at block: "));
        stream->println(address_in);
      }
      return false;
    }
  }
  #endif

  return true;
}

// Create a BRAND NEW DEVICE
bool SDBlock::allocationDefault() {
  memset(buffer, 0, sizeof(buffer));
  metadata->blockType = SDBLOCK_TYPE_ALLOCATION_FULL;
	allocation->deviceId = deviceId;
	allocation->recordId = recordId;
	allocation->currentTime = now();

  // First 2 blocks Allocaiton table
  // Next = Metadata
	allocation->metadataBlockNext = SDBLOCK_ACTUAL_METADATA;
  // Next = Single
	allocation->singleBlockNext = SDBLOCK_ACTUAL_SINGLE;
  // Next = Log Lines
	allocation->loglinesBlockNext = SDBLOCK_ACTUAL_LOGLINES;
  // Next = Data
	allocation->dataBlockNext = SDBLOCK_ACTUAL_DATA;
  // Queue
	allocation->queueBlockNext = SDBLOCK_ACTUAL_QUEUE;


  // ESSENTAIL for alloction bloc, or this block will be rejected.
  //  I only keep one other copy
  crcSet();

  return allocationWrite();
}

// Special update in place
bool SDBlock::allocationUpdate() {
  if (!allocationRead()) {
    if (stream)
      stream->println(F("# SDBlock: Failed to update allocation table"));
    return false;
  }

  if (stream) {
    stream->println(F("# SDBlock: Allocation changes: "));

    stream->print(F("#  metadata: "));
    stream->print(allocation->metadataBlockNext);
    stream->print(F(" -> "));
    stream->println(metadataBlockNext);

    stream->print(F("#  single: "));
    stream->print(allocation->singleBlockNext);
    stream->print(F(" -> "));
    stream->println(singleBlockNext);

    stream->print(F("#  loglines: "));
    stream->print(allocation->loglinesBlockNext);
    stream->print(F(" -> "));
    stream->println(loglinesBlockNext);

    stream->print(F("#  queue: "));
    stream->print(allocation->queueBlockNext);
    stream->print(F(" -> "));
    stream->println(queueBlockNext);

    stream->print(F("#  data: "));
    stream->print(allocation->dataBlockNext);
    stream->print(F(" -> "));
    stream->println(dataBlockNext);
  }

  allocation->dataBlockNext = dataBlockNext;
  allocation->metadataBlockNext = metadataBlockNext;
  allocation->singleBlockNext = singleBlockNext;
  allocation->loglinesBlockNext = loglinesBlockNext;
  allocation->queueBlockNext = queueBlockNext;
  allocation->currentTime = now();
  allocation->deviceId = deviceId;
  allocation->recordId = recordId;

  allocation->cardId = cardId;

  // Update and set the CRC
  crcSet();

  return allocationWrite();
}

bool SDBlock::allocationWrite() {
  response = write(SDBLOCK_OFFSET);
  if (!response) {
    response = write(SDBLOCK_OFFSET);
    if (!response) {
      if (stream) {
        stream->println("# SDBlock: DISASTER: Failed to write allocation block 1");
      }
    }
  }

  response = write(SDBLOCK_OFFSET + 1);
  if (!response) {
    response = write(SDBLOCK_OFFSET + 1);
    if (!response) {
      if (stream) {
        stream->println("# SDBlock: DISASTER: Failed to write allocation block 2");
      }
    }
  }

  return true;
}

/*
bool SDBlock::metadataStatusBLK(uint32_t block, uint32_t status) {
  response = read(block);
  if (!response) {
    response = read(block);
    if (!response) {
      stream->println(F("# SDBlock - metadataStatusBLK - Catastrophic Failure"));
      return false;
    }
  }
  metadata->status = status;
  crcSet();
  response = write(block);
  if (!response) {
    response = write(block);
    if (!response) {
      if (stream)
        stream->println(F("# SDBlock: DISASTER: Can't write update BLK"));
      return false;
    }
  }
  return true;
}

bool SDBlock::dataStatusBLK(uint32_t block, uint32_t status) {
  response = read(block);
  if (!response) {
    response = read(block);
    if (!response) {
      stream->println(F("# SDBlock - dataStatusBLK - Catastrophic Failure"));
      return false;
    }
  }
  data->status = status;
  response = write(block);
  if (!response) {
    response = write(block);
    if (!response) {
      if (stream)
        stream->println(F("# SDBlock: DISASTER: Can't write update BLK"));
      return false;
    }
  }
  return true;
}
*/

uint32_t SDBlock::getDataNext() {
  return dataBlockNext;
}

uint32_t SDBlock::getMetadataNext() {
  return metadataBlockNext;
}
uint32_t SDBlock::getMetadataFirst() {
  return 	SDBLOCK_ACTUAL_METADATA;
}
uint32_t SDBlock::getDataFirst() {
  return 	SDBLOCK_ACTUAL_DATA;
}
uint8_t SDBlock::getCardId() {
  return cardId;
}
bool SDBlock::validRange(char *str, uint32_t test, uint32_t start, uint32_t max) {
  if (
    (test < start)
    || ((test - start) > max)
  ) {
    if (str && stream) {
      stream->print(F("# SDBlock: ERROR "));
      stream->print(str);
      stream->print(F(" out of range: current="));
      stream->print(test);
      stream->print(F(", start="));
      stream->print(start);
      stream->print(F(", max="));
      stream->print(max);
      stream->println();
    }
    return false;
  }
  return true;
}

// XXX Generalisation of Valid - e.g. CRC and Block type
bool SDBlock::allocationValid() {
  if (!crcCheck()) {
    if (stream)
      stream->println(F("# SDBlock: Allocation: CRC Failed"));
    return false;
  }

  if (allocation->blockType != SDBLOCK_TYPE_ALLOCATION_FULL) {
    if (stream && debug) {
      stream->print(F("# SDBlock: BlockType: "));
      stream->println(allocation->blockType);
      stream->print(F("Expected: "));
      stream->println(SDBLOCK_TYPE_ALLOCATION_FULL);
    }
    return false;
  }

  // Example Special case - we can write some areas update ourselives
  if (!validRange(NULL, allocation->queueBlockNext, SDBLOCK_ACTUAL_QUEUE, SDBLOCK_QUEUE_MAX)) {
    if (stream)
      stream->println("# SDBlock: NOTE: queueBlock automatically updated");
  	allocation->queueBlockNext = SDBLOCK_ACTUAL_QUEUE;
  }

  // XXX These could be improved ! Fully check range etc
  if (
    !validRange("metadataBlock", allocation->metadataBlockNext, SDBLOCK_ACTUAL_METADATA, SDBLOCK_METADATA_MAX)
    || !validRange("singleBlock", allocation->singleBlockNext, SDBLOCK_ACTUAL_SINGLE, SDBLOCK_SINGLE_MAX)
    || !validRange("loglinesBlock", allocation->loglinesBlockNext, SDBLOCK_ACTUAL_LOGLINES, SDBLOCK_LOGLINES_MAX)
    || !validRange("queueBlock", allocation->queueBlockNext, SDBLOCK_ACTUAL_QUEUE, SDBLOCK_QUEUE_MAX)
    || !validRange("dataBlock", allocation->dataBlockNext, SDBLOCK_ACTUAL_DATA, SDBLOCK_DATA_MAX)
  ) {
    if (stream)
      stream->println(F("# SDBlock: Failed one or more dataBlock tests"));
    return false;
  }

  return true;
}

bool SDBlock::allocationRead() {
  // Read first record
  response = read(SDBLOCK_OFFSET);
  if (!response)
    response = read(SDBLOCK_OFFSET);

  // Read second record
  if (!response || !crcCheck()) {
    if (stream) {
      stream->println(F("# SDBlock: Failed to read allocation block 1, trying block 2"));
    }
    response = read(SDBLOCK_OFFSET + 1);
    if (!response)
      response = read(SDBLOCK_OFFSET + 1);
  }

  // Check valid Allocation table, and CRC Valid.
  if (!allocationValid()) {
    // Time to assume this is a new disk (VERY DANGEROUS)

    if (autoFormat) {
      if (!allocationDefault()) {
        // console.log
        if (stream)
          stream->println(F("# SDBlock: Failed... like real bad - unable to make new allocation table!"));
        return false;
      }
      return true;
    }
    else {
      if (stream)
        stream->println(F("# SDBlock: Failed... like real bad - no allocation valid, and no autoFormat"));
      return false;
    }
  }

  return true;
}

void SDBlock::crcUpdate() {
  // 512 byte buffer - whole thing, except the CRC (last 4 bytes);
  crc = CRC32::calculate(buffer, 508);
}

bool SDBlock::crcCheck() {
  // Update the CRC from Buffer
  crcUpdate();
  // Compare to what is stored in buffer
  return (crc == block->crc);
}

void SDBlock::crcSet() {
  crcUpdate();
  block->crc = crc;
}

bool SDBlock::getMBR() {
  // NOTE: Assumes openCard()

  // XXX Keep the esentils.

  // Read MBR
  response = read(0);
  if (!response) {
    // console.log(LOG_INFO, F("MBR Reading: FAILED"));
    stream->println(F("# SDBlock: Failed read MBR"));
    return false;
  }

  // Check if MBR is valid (MBR Signature: 0x55AA at end of the sector)
  if (buffer[0x1FE] != 0x55 || buffer[0x1FF] != 0xAA) {
    stream->println(F("# SDBlock: This SD Card doesnt use MBR"));
    return false;
  }

  // Show all information from MBR
  // Reference for MBR Sector: https://en.wikipedia.org/wiki/Master_boot_record
  stream->println(F("# SD Card Information:"));

  // Original Physical Drive
  stream->print(F("# Physical Drive: "));
  stream->println(buffer[0x0DC], HEX);

  // Timestamp
  stream->print(F("# Timestamp: "));
  stream->print(buffer[0x0DD]);
  stream->print(F(":"));
  stream->print(buffer[0x0DE]);
  stream->print(F(":"));
  stream->println(buffer[0x0DF]);

  // Disk Signature
  stream->print(F("# Disk Signature: "));
  stream->print(buffer[0x1B8], HEX);
  stream->print(buffer[0x1B9], HEX);
  stream->print(buffer[0x1BA], HEX);
  stream->print(buffer[0x1BB], HEX);
  stream->print(buffer[0x1BC], HEX);
  stream->println(buffer[0x1BD], HEX);

  // Partition Entries
  for (byte i = 0; i < 4; i++) {

      // Calculate blocks count
      unsigned long blocks = (unsigned long)buffer[0x1CA + (i * 16)]
                           | (unsigned long)buffer[0x1CB + (i * 16)] << 8
                           | (unsigned long)buffer[0x1CC + (i * 16)] << 16
                           | (unsigned long)buffer[0x1CD + (i * 16)] << 24;

      // If its a partition with 0 blocks, its not a partition ;)
      if (blocks == 0) {
        continue;
      }

      // XXX Urgent - convert to console !

      // Partition Number
      stream->print(F("# Partition "));
      stream->println(i + 1);

      // Is bootable
      stream->print(F("#\tBootable: "));
      if (buffer[0x1BE + (i * 16)]) {
          stream->println(F("YES"));
      } else {
          stream->println(F("NO"));
      }

      // Partition Type
      stream->print(F("#\tPartition Type: "));
      stream->println(buffer[0x1C2], HEX);

      // LBA
      stream->print(F("#\tLBA: "));
      stream->print(buffer[0x1C9 + (i * 16)], HEX);
      stream->print(buffer[0x1C8 + (i * 16)], HEX);
      stream->print(buffer[0x1C7 + (i * 16)], HEX);
      stream->println(buffer[0x1C6 + (i * 16)], HEX);

      // Number of blocks
      stream->print(F("#\tBlocks: "));
      stream->println(blocks);

      // Size
      stream->print(F("#\tSize: "));
      stream->print(blocks >> 11);
      stream->println(F(" MB"));
  }
}

bool SDBlock::dataIsOpen() {
  return (valid && (dataBlockStart || dataBlockCurrent));
}

bool SDBlock::dataWrite(void* in, uint16_t size) {
  if (!dataIsOpen()) {
    if (stream)
      stream->println(F("# SDBlock: Data was never open. Nothing to do"));
    return false;
  }

  // Helpful all in one write
  if (size > 0) {
    if (!dataPrepare(in, size)) {
      if (stream)
        stream->println(F("# SDBlock: Failed inline prepare"));
      return false;
    }
  }

  // Leave off for data blocs - just too slow (About 80% of the time in SDBlock)
    // crcSet();
  response = write(dataBlockCurrent);
  if (!response) {
    response = write(dataBlockCurrent);
    if (!response) {
      if (stream)
        stream->println(F("# SDBlock: DISASTER: Can't write"));
      return false;
    }
  }

  dataLast = dataBlockNext;
  dataBlockCurrent++;
  return true;
}

bool SDBlock::dataOpen(uint32_t recId) {
  // Woops.... already started... Record to serial port then disreguard
  if (dataIsOpen()) {
    if (stream)
      stream->println(F("# SDBlock: Data was already started, that run will be lost (TODO record numbers)"));
  }

  dataBlockStart = dataBlockNext;
  dataBlockCurrent = dataBlockNext;
  dataBlockRecordId = recId;
  recordId = recId; // XXX Both?

  if (stream) {
    stream->print(F("# SDBlock: dataOpen at "));
    stream->println(dataBlockNext);
  }
  return true;
}

// dataClose - assumed to be input, but could also be output, only decide on close
bool SDBlock::dataClose(bool output) {
  if (!dataIsOpen()) {
    if (stream)
      stream->println(F("# SDBlock: Data was never open. Nothing to do"));
    return false;
  }

  // Update our allocation table ready to save
  dataBlockNext = dataBlockCurrent;

  if (output) {
    metadata_resultsBlockStart = dataBlockStart;
    metadata_resultsBlocks = dataBlockCurrent - dataBlockStart;
    if (stream) {
      stream->print(F("# SDBlock: dataClose added "));
      stream->println(metadata_resultsBlocks);
    }
  }
  else {
    metadata_dataBlockStart = dataBlockStart;
    metadata_dataBlocks = dataBlockCurrent - dataBlockStart;
    if (stream) {
      stream->print(F("# SDBlock: dataClose added "));
      stream->println(metadata_dataBlocks);
    }
  }

  // Clear data
  dataBlockStart = 0;
  dataBlockCurrent = 0;

  return true;
}

bool SDBlock::dataPrepare(void* in, uint16_t size) {
  memset(buffer, 0, sizeof(buffer));
  block->blockType = SDBLOCK_TYPE_BLOCK_FULL;
  block->recordId = dataBlockRecordId;

  if (size > 0) {
    if (size > sizeof(block->data)) {
      if (stream)
        stream->println(F("# SDBlock: ERROR: Trying to set data beyond size"));
      return false;
    }
    memcpy(block->data, in, size);
  }
  return true;
}

//bool SDBlock::dataRead(uint32_t fromEnd) {
//}
/*
void SDBlock::dataList(unsigned long address, Print* altstream = NULL, CallbackFunction metadata_cb = NULL, CallbackFunction data_cb = NULL) {
}
*/

bool SDBlock::metadataPrepare(void* in, uint16_t size) {
  memset(buffer, 0, sizeof(buffer));
  metadata->blockType = SDBLOCK_TYPE_METADATA_FULL;
  metadata->status = 0;
  metadata->deviceId = deviceId;
  metadata->recordId = recordId;
  metadata->dataBlockStart = metadata_dataBlockStart;
  metadata->dataBlocks = metadata_dataBlocks;
  metadata->resultsBlockStart = metadata_resultsBlockStart;
  metadata->resultsBlocks = metadata_resultsBlocks;

  // Clear the data blocks
  metadata_dataBlockStart = 0;
  metadata_dataBlocks = 0;
  metadata_resultsBlockStart = 0;
  metadata_resultsBlocks = 0;

  if (size > 0) {
    if (size > sizeof(metadata->data)) {
      if (stream)
        stream->println(F("# SDBlock: ERROR: Trying to set data beyond size"));
      return false;
    }
    memcpy(metadata->data, in, size);
  }
  return true;
}

bool SDBlock::metadataWrite(void* in, uint16_t size) {
  if (!cardIsOpen()) {
    if (stream)
      stream->println(F("# SDBlock: Data was never open. Nothing to do"));
    return false;
  }
  // Helpful all in one write
  if (size > 0) {
    if (!metadataPrepare(in, size)) {
      if (stream)
        stream->println(F("# SDBlock: Failed inline prepare"));
      return false;
    }
  }
  crcSet();
  response = write(metadataBlockNext);
  if (!response) {
    response = write(metadataBlockNext);
    if (!response) {
      if (stream)
        stream->println(F("# SDBlock: DISASTER: Can't write metadata"));
      return false;
    }
  }

  metadataLast = metadataBlockNext;
  metadataBlockNext++;
  return true;
}

uint32_t SDBlock::metadataCount() {
  return metadataBlockNext - SDBLOCK_ACTUAL_METADATA;
}

// fromEnd is first, then num back from there
void SDBlock::metadataList(uint32_t fromEnd, uint32_t num, Print *altstream) {
  if (!altstream)
    altstream = stream;

  if (debug) {
    altstream->print(F("# SDBlock: metadataList - fromEnd/num= metadtaaBlockNext="));
    altstream->print(fromEnd);
    altstream->print("/");
    altstream->print(num);
    altstream->print(" ");
  	altstream->print(metadataBlockNext);
    altstream->println();
  }

  altstream->println(F("# SDBlock: Start of metadata requested"));
  for (count = 0; count < num; count++) {
    if (metadataRead(fromEnd + count)) {
      altstream->print(F("@SDBlock,metadata"));
      altstream->print(F(",count="));
      altstream->print(count);
      altstream->print(F(",add="));
      altstream->print(metadataBlockNext - (fromEnd + count) - 1);  // NOTE: -1 because Next is next to use not this
      altstream->print(F(",dev="));
      altstream->print(metadata->deviceId);
      altstream->print(F(",rec="));
      altstream->print(metadata->recordId);
      altstream->print(F(",bSt="));
      altstream->print(metadata->dataBlockStart);
      altstream->print(F(",bs="));
      altstream->print(metadata->dataBlocks);
      altstream->print(F(",rSt="));
      altstream->print(metadata->resultsBlockStart);
      altstream->print(F(",rs="));
      altstream->print(metadata->resultsBlocks);
      altstream->print(F(",status="));
      altstream->print(metadata->status);
      altstream->println();
    }
    else {
      altstream->println(F("# SDBlock: End of metadata reached (before number requested)"));
      break;
    }
  }
  altstream->println(F("# SDBlock: End of metadata requested"));
}

bool SDBlock::metadataUpdateStatus(uint32_t address_in, uint32_t record_in, uint32_t status_in) {
  if (!metadataReadBLK(address_in)) {
    if (stream)
      stream->println(F("# SDBlock: Failed to update metadata status"));
    return false;
  }

  // Check record is valid
  if (metadata->recordId != record_in) {
    if (stream) {
      stream->print(F("# SDBlock: Failed to match record Id: Request="));
      stream->print(record_in);
      stream->print(F(", rec="));
      stream->println(metadata->recordId);
    }
    return false;
  }

  if (metadata->status == status_in) {
    if (stream && debug)
      stream->println(F("SDBlock: status not changed, not writing"));
    return true;
  }
  metadata->status = status_in;
  crcSet();
  response = write(address_in);
  if (!response) {
    response = write(address_in);
    if (!response) {
      if (stream)
        stream->println(F("# SDBlock: DISASTER: Can't write metadata"));
      return false;
    }
  }
  return true;
}

bool SDBlock::metadataRead(uint32_t fromEnd) {
  if (stream && debug) {
    stream->print(F("# SDBlock: metadataRead - fromEnd="));
    stream->print(fromEnd);
    stream->println();
    stream->print(F("# SDBlock: metadataRead - metadataBlockNext="));
    stream->print(metadataBlockNext);
    stream->println();
    stream->print(F("# SDBlock: metadataBlockNext - fromEnd - 1="));
    stream->print(metadataBlockNext - fromEnd - 1);
    stream->println();
  }
  return metadataReadBLK(metadataBlockNext - fromEnd - 1);
}

bool SDBlock::metadataReadBLK(uint32_t address_in) {
  if (stream && debug) {
    stream->print(F("# SDBlock: metadataReadBLK - block/start "));
    stream->print(address_in);
    stream->print("/");
    stream->print(SDBLOCK_ACTUAL_METADATA);
    stream->println();
  }
  if (!cardIsOpen()) {
    if (stream)
      stream->println(F("# SDBlock: Data was never open. Nothing to do"));
    return false;
  }

  if (address_in >= SDBLOCK_ACTUAL_METADATA) {
    return read(address_in);
    // XXX crc check, record type check
  }
  return false;
}

bool SDBlock::resultsRead(uint32_t fromEnd) {
  return resultsReadBLK(dataBlockNext - fromEnd - 1);
}

bool SDBlock::resultsReadBLK(uint32_t address_in) {
  if (!cardIsOpen()) {
    if (stream)
      stream->println(F("# SDBlock: Data was never open. Nothing to do"));
    return false;
  }

  if (address_in >= SDBLOCK_ACTUAL_DATA) {
    return read(address_in);
    // XXX crc check, record type check
  }
  return false;
}

/*

bool SDBlock::metadataUpdate(uint32_t fromEnd) {
  if (!metadataRead(fromEnd)) {
    if (stream)
      stream->println(F("# SDBlock: Failed to read metadata block during update"));
    return false;
  }

  // Allocation blocsk

  // Update and set the CRC
  crcSet();

  response = write(metadataBlockNext);
  if (!response) {
    response = write(metadataBlockNext);
    if (!response) {
      if (stream)
        stream->println(F("# SDBlock: DISASTER: Can't write metadata"));
      return false;
    }
  }

*/


bool SDBlock::singlePrepare(void* in, uint16_t size) {
  memset(buffer, 0, sizeof(buffer));
  block->blockType = SDBLOCK_TYPE_SINGLE_FULL;
  block->recordId = recordId;
  if (size > 0) {
    if (size > sizeof(block->data)) {
      if (stream)
        stream->println(F("# SDBlock: ERROR: Trying to set data beyond size"));
      return false;
    }
    memcpy(block->data, in, size);
  }
  return true;
}

uint32_t SDBlock::singleCount() {
  return singleBlockNext - SDBLOCK_ACTUAL_SINGLE;
}

bool SDBlock::singleRead(uint32_t fromEnd) {
  if (!cardIsOpen()) {
    if (stream)
      stream->println(F("# SDBlock: Data was never open. Nothing to do"));
    return false;
  }

  address_local = singleBlockNext - fromEnd - 1;
  if (address_local >= SDBLOCK_ACTUAL_SINGLE) {
    if (stream) {
      stream->print("# SingleRead Address=");
      stream->println(address_local);
    }
    return read(address_local);
    // optional crc check, record type check
  }
  return false;
}

bool SDBlock::singleWrite(void* in, uint16_t size) {
  if (!cardIsOpen()) {
    if (stream)
      stream->println(F("# SDBlock: Data was never open. Nothing to do"));
    return false;
  }

  if (size > 0) {
    if (!singlePrepare(in, size)) {
      if (stream)
        stream->println(F("# SDBlock: Failed inline prepare"));
      return false;
    }
  }

  // crcSet();
  response = write(singleBlockNext);
  if (!response) {
    response = write(singleBlockNext);
    if (!response) {
      if (stream)
        stream->println(F("# SDBlock: DISASTER: Can't write single"));
      return false;
    }
  }

  singleLast = singleBlockNext;
  singleBlockNext++;
  if (stream && debug) {
    stream->print(F("# SDBlock: Single Write complete, nextblock="));
    stream->println(singleBlockNext);
  }
  return true;
}

bool SDBlock::queuePrepare(uint32_t status, void* in, uint16_t in_size, void* out, uint16_t out_size) {
  memset(buffer, 0, sizeof(buffer));
  block->blockType = SDBLOCK_TYPE_QUEUE_FULL;
  block->recordId = recordId;
  if (in_size > 0) {
    if (in_size > sizeof(queue->dataIn)) {
      if (stream)
        stream->println(F("# SDBlock: ERROR: Trying to set data beyond size"));
    }
    else {
      memcpy(queue->dataIn, in, in_size);
    }
  }
  if (out_size > 0) {
    if (out_size > sizeof(queue->dataOut)) {
      if (stream)
        stream->println(F("# SDBlock: ERROR: Trying to set data beyond size"));
    }
    else {
      memcpy(queue->dataOut, out, out_size);
    }
  }
  return true;
}

uint32_t SDBlock::queueCount() {
  return queueBlockNext - SDBLOCK_ACTUAL_SINGLE;
}

		bool queueUpdate(uint32_t fromEnd, uint32_t status, void* out = NULL, uint16_t out_size = 0);
		bool queueRead(uint32_t fromEnd);
		uint32_t queueCount();

bool SDBlock::queueRead(uint32_t fromEnd) {
  if (!cardIsOpen()) {
    if (stream)
      stream->println(F("# SDBlock: Data was never open. Nothing to do"));
    return false;
  }

  address_local = queueBlockNext - fromEnd - 1;
  if (address_local >= SDBLOCK_ACTUAL_SINGLE) {
    if (stream) {
      stream->print("SingleRead Address=");
      stream->println(address_local);
    }
    return read(address_local);
    // optional crc check, record type check
  }
  return false;
}

bool SDBlock::queueWrite(uint32_t status, void* in, uint16_t in_size, void* out, uint16_t out_size) {
  if (!cardIsOpen()) {
    if (stream)
      stream->println(F("# SDBlock: Data was never open. Nothing to do"));
    return false;
  }

  if (size > 0) {
    if (!queuePrepare(in, size)) {
      if (stream)
        stream->println(F("# SDBlock: Failed inline prepare"));
      return false;
    }
  }

  if (status > 0)
    queue->status = status;

  // (slow) crcSet();
  response = write(queueBlockNext);
  if (!response) {
    response = write(queueBlockNext);
    if (!response) {
      if (stream)
        stream->println(F("# SDBlock: DISASTER: Can't write queue"));
      return false;
    }
  }

  queueLast = queueBlockNext;
  queueBlockNext++;
  if (stream && debug) {
    stream->print(F("# SDBlock: Single Write complete, nextblock="));
    stream->println(queueBlockNext);
  }
  return true;
}

SDBlock sdBlock;