// SPDX-License-Identifier: Apache-2.0
// Copyright (c) 2012-2024 Scott Penrose <scottp@dd.com.au> and WII5 Buoy contributors
//
// This file is part of WII5 Buoy firmware.
// See LICENSE for full terms.

/**
 * @file WII5Sparton.cpp
 * @brief Sparton AHRS-M1/M2 IMU driver: capture timing, NorthTek configuration.
 */

/*

WII5Sparton

TODO 2024 - Review code, check TODO and add some Command functions for testing
TODO 2024 - Look at getting debug working with serial passthrough in text stage not binary
TODO 2024 - When getting no data - stop capture and report failure for Iridium
TODO 2024 - How to send an error - look at Command return string
TODO 2024 - Debugging text modes?

BINARY File
  https://forum.pjrc.com/threads/55114-SD-Datalogging-Best-Practice-in-2019
    Datalogging is a problem I've been trying to brute-force for half a year now.

    My solution (that I hope to release in a couple more months of testing) runs on top of the SdFs library for the purpose of low-latency high-frequency binary logging. I'm currently able to (somewhat reliably) maintain a logging speed of 925bytes at 500Hz (452KB/s). So in short, it is possible to have a good logging system, although it's highly dependent on how you interact with your data.

    My current best practises:

    1. Initialize the SdFs on SdioConfig(FIFO_SDIO) mode.
    2. Preallocate the maximum length of your log file using the .preAllocate(size_t size) method.
    3. Important: Pre-erase the entire file with zeroes.
    4. Before you start logging (to ensure the file is committed fully to the card), call sync().
    5. Write in chunks of a multiple of 512bytes only. (512, 1024, 2048 byte chucks per write() command)
    6. To minimize blocking, try to write only if .card()->isBusy() returns false. This seems to be related to the internal card data management (I'm guessing things like TRIM and what not). Step 3 helps ensure that the card spends as little time as possible in this busy time.
    7. Use a SLC-type SD card. I cannot stress enough the difference this makes. Before buying this card, I had write() operations take up to 10ms on a SanDisk card, but with this expensive card they 99.9% of the time stayed below 500us.
    8. Call truncate() and then sync() after you've finished writing your entire file. The first function removes any extra pre-allocation and the second makes sure that it's a proper FAT32 file. Note that if you're using an exFAT partition your file will not be saved until you call sync() or close().
    7. On that note, FAT32 is always faster than exFAT, but you do have less capacity.

    I'm still learning more about best practises for logging. My current hang-ups are (I think) related to heap-overflow because of the many sensors I have attached interrupting one another during a write() operation. I'm a very fresh programmer so it's taking me a while but I hope the tips above are helpful enough for you to get started on your 100Hz project.

    Good luck! Let me know if you find any new practises that I can also test.


64Hz Performance Issue:

1 - Version of code that uses standard library loge to disk. No parsing at all.
    This will not allow us to show the 1 Hz update.

      Status: On Hold

2 - Version of the code that uses new library to buffer and then parse int the loops

      Status: WIP

3 - Binary Version: Most likely in interrupt

TODO - Cycle through all the baud rates until we get an OK/Huh so we can
work out it is wokring. Send the stop command as the test. Clear wii5BufferString.
Send twice, that sort of thing. If not our correct baud rate, then
send the program command change to new baud.

PINS:

  * 1 - Ground
  * 2 - TX (actually TX from AVR, so Receive)
  * 3 - RX
  * 4 - Power (+4 volts)
  * 5 - NC
  * 6 - RESET (low is reset, keep high, has weak pullup)

Special Notes:

  * 5 millisecond line delay
  *

baud 4 set drop<cr>

  4 = 9600
  5 = 19200
  6 = 38400
  7 = 57600
  8 = 115200

Text CRC

  TODO - We should at least check we have not lost characters

Binary.

  Try RFS (Remote Function Select) Sparton Binary format with CRC etc

  STATS:Sparton:loops=128008 ERROR:time=512 accel=96
    Standard wii5BufferStrings
    64Hz

# INFO: 2024-03-13T23:49:09 - SPARTON: Records received = 1001
STATS:Sparton:Status=unknown:age=165s
STATS:Sparton:Time:Total=0
STATS:Sparton:SerialManager:TODO
STATS:Sparton:loops=587117 ERROR:time=119 accel=3
  8Hz, SD Card etc

# INFO: 2024-03-13T23:50:32 - SPARTON: Records received = 1001
STATS:Sparton:Status=unknown:age=248s
STATS:Sparton:Time:Total=0
STATS:Sparton:SerialManager:TODO
STATS:Sparton:loops=44851 ERROR:time=199 accel=32
  64Hz, SD Card etc

*/

#include <Arduino.h>
#include <WII5Sparton.h>

// Careful - loss of serial data with this on
// #ifdef SPARTON_SERIAL_DEBUG

// #define WII5_ZONLY
// #define WII5_STRINGUTIL
// #ifdef WII5_STRINGUTIL
// #include <StringUtil.h>
// using namespace StringUtil;
// #endif

// INTERRUPT - Keep fast and use volatile varialbes.
void WII5Sparton::handleRxChar( uint8_t c ) {
  // Which record we playing with?
  currentRecord = (recordCount % SPARTON_RECORDS);

  // \r and \n for records is an issue...

  // START of Binary
  if (c == 0x1) {
    processMode = 2;
    currentCount = 0;
    startWhen = 0;
    binEscape = false;
    return;
  }
  // END of binary
  else if (c == 0x3) {
    // Record too small !
    if (currentCount < sizeof(WII5_DATA_SpartonBinary)) {
      serialSizeError++;
    }

    // Other validity
    if (
      (binRecords[currentRecord].channel != 0)      // 0 - yep - valid
      || (binRecords[currentRecord].status != 105)  // TODO 105, what is thta?
    ) {
      // TODO Not really time, record data?
      statsTimeError++;
    }

    // Status / Mode
    lastValidRecord = currentRecord;
    recordCount++;
    processMode = 0;

    // TODO consider logging time here for startWhen
    return;
  }

  // Escape character - take off BIN next time
  else if (c == 0x10) {
    binEscape = true;
  }

  // Process a Binary row
  else if (processMode == 2) {
    if (binEscape) {
      // Escape characters need to drop MSB ! (to protect 0x1, 0x3 and 0x10)
      binEscape = false;
      c = c & B01111111;
    }
    if (currentCount < sizeof(WII5_DATA_SpartonBinary)) {
      ((char*)&binRecords[currentRecord])[currentCount] = c;
      currentCount++;
    }

    // Record too large
    else {
      serialSizeError++;
    }
  }
  /* 
  TODO 2024 - passthrough mode?
  else if (processMode == 0) {
    console.log(LOG_DEBUG, F("SPARTON: char=%c"), c);
  }
  */
}

void WII5Sparton::begin() {
  // TODO Use ?
  pinMode(SPARTON_RESET, OUTPUT);
  digitalWrite(SPARTON_RESET, HIGH);

  // Power (V2)
  #if POWER_IMU_SPARTON_PIN>0
  powerSetPin(POWER_IMU_SPARTON_PIN, POWER_IMU_SPARTON_ON);
  powerOff(true); // Forced off at boot
  #endif

  debug = false;
  running = false;
  cancel = false;
  step = WII5SPARTON_OFF;

  consoleDirect = false;
  rawCapture = false;  // TODO NOTE: Debugging - probs should make this false by default

  // Defaults
  HZ = 8;
  binMode = true;

  // TODO - what should these be?
  // recordCalc.setRejectionSigma(7.0);

};

uint16_t WII5Sparton::getHz() {
  return HZ;
}
uint16_t WII5Sparton::getMs() {
  return (1000 / getHz());
}
void WII5Sparton::setHz(uint16_t h) {
  HZ = h;
}
void WII5Sparton::setBinary(bool in) {
  binMode = in;
}

void WII5Sparton::setPassthrough(bool in) {} //  {pt = in;}
bool WII5Sparton::getPassthrough() {} // {return pt;}
void WII5Sparton::setDebug(bool in) {debug = in;}
bool WII5Sparton::getDebug() {return debug;}

void WII5Sparton::setCapture(bool in) {
  // TODO Capture disabled
  capture = false; return;
  // closeFile();
  capture = in;
}

bool WII5Sparton::getCapture() {return capture;}
void WII5Sparton::setRecords(uint32_t in) { recordTotal = in; }
uint32_t WII5Sparton::getRecords() {return recordTotal;}

void WII5Sparton::automatic(uint8_t hz, uint32_t records, bool capture) {
  setHz(hz);
  setRecords(records);
  // (leave as is) setPassthrough(0);
  // Do not change debug
  setCapture(capture);
  start();
}

void WII5Sparton::start() {
  console.log(LOG_DEBUG, F("SPARTON: start current=%d"), step);

  pinMode(SPARTON_RESET, OUTPUT);
  digitalWrite(SPARTON_RESET, HIGH);

  if (step != WII5SPARTON_OFF) {
    console.log(LOG_ERROR, F("SPARTON: ERROR - Start called when Sparton already running. Cleaning up."));
    // closeFile();
    sendLine(0);
    off(true);
    // TODO 2024 - delay? Consider safeDelay - although this is a rare occurecne (aka already running)
    delay(500);
  }

  resetData();
  step = WII5SPARTON_BOOT; stepWait = 0;
  startWhen = 0;  // Clear when binary start
  startCapture = 0; // When we started this capture
  lastWrite = 0; // Last time we wrote to SD Card successfully
  // statsHighClear();

  on(true);
}

void WII5Sparton::resetData() {
  // Values used in collecting
  serialSizeError = 0;
  statsTimeError = 0;

  blockNext = 0;
  captureWriteMax = 0;
  captureWriteMin = 1000;
  captureWriteOver = 0;

  // Current record.
  #ifdef SPARTON_SIZES
  for (uint8_t t = 0; t < SIZES_LENGTH; t++) {
    sizes[t] = 0;
  }
  #endif
  recordCount = 0; sendCount = 0;

  // Averages
  // avgAccelX.reset();
  // avgAccelY.reset();
  // avgAccelZ.reset();
  // avgPoseX.reset();
  // avgPoseY.reset();
  // avgPoseZ.reset();

  // Internal storage
  #ifdef SPRTON_RECORD
  recordReady = -1;
  record[0].rec = 0; record[0].stamp = 0; record[0].stampActual = 0; record[0].stampErr = 0;
  record[0].pose_x = NAN; record[0].pose_y = NAN; record[0].pose_z = NAN;
  record[0].mag_x = NAN; record[0].mag_y = NAN; record[0].mag_z = NAN;
  record[0].gyro_x = NAN; record[0].gyro_y = NAN; record[0].gyro_z = NAN;
  record[0].accel_x = NAN; record[0].accel_y = NAN; record[0].accel_z = NAN;
  record[1].rec = 0; record[0].stamp = 0; record[0].stampActual = 0; record[0].stampErr = 0;
  record[1].pose_x = NAN; record[1].pose_y = NAN; record[1].pose_z = NAN;
  record[1].mag_x = NAN; record[1].mag_y = NAN; record[1].mag_z = NAN;
  record[1].gyro_x = NAN; record[1].gyro_y = NAN; record[1].gyro_z = NAN;
  record[1].accel_x = NAN; record[1].accel_y = NAN; record[1].accel_z = NAN;
  recordPrev.rec = 0; recordPrev.stamp = 0; recordPrev.stampActual = 0; recordPrev.stampErr = 0;
  recordPrev.pose_x = NAN; recordPrev.pose_y = NAN; recordPrev.pose_z = NAN;
  recordPrev.mag_x = NAN; recordPrev.mag_y = NAN; recordPrev.mag_z = NAN;
  recordPrev.gyro_x = NAN; recordPrev.gyro_y = NAN; recordPrev.gyro_z = NAN;
  recordPrev.accel_x = NAN; recordPrev.accel_y = NAN; recordPrev.accel_z = NAN;
  #endif

  console.log(LOG_INFO, F("SPARTON: Reset Sparton Data 0 - %d"), sizeof(binRecords));
  memset(&binRecords, 0, sizeof(binRecords));
}

void WII5Sparton::info() {
  if (debug)
    console.log(LOG_DEBUG, F("SPARTON: info current=%d"), step);
  recordCount = 0; sendCount = 0;
  step = WII5SPARTON_INFO; stepWait = 0;
  on();
}

void WII5Sparton::captureRun() {
  // processMode = WII5SERIALPARSER_RECORD;  // Capture fields
  // last = WII5SERIALLAST_NONE;
  step = WII5SPARTON_CAPTURE; stepWait = 0;
}

bool WII5Sparton::captureRunning() {
  return (step == WII5SPARTON_CAPTURE);
}

bool WII5Sparton::captureFinished() {
  return (
    (step == WII5SPARTON_OFF)
  );
}

// WAITING - It will wait here, basically forever, until you call captureRun()
// this is to allow preparation followed by remote trigger at a certain time.
// However - if it is past this - e.g. nowait - you could deadlock
bool WII5Sparton::captureWaiting() {
  return (step == WII5SPARTON_PROGRAM_START);
}

void WII5Sparton::stop(bool force) {
  if (debug)
    console.log(LOG_DEBUG, F("SPARTON: cancel current=%d"), step);
  if (step != WII5SPARTON_OFF) {
    step = WII5SPARTON_CANCEL; stepWait = 0;
  }

  // Force to off state - done by sleep
  if (force) {
    powerOff(true);
    step = WII5SPARTON_OFF;
  }
}

void WII5Sparton::setRawCapture(bool in) {
  rawCapture = in;
}
bool WII5Sparton::getRawCapture() {
  return rawCapture;
}

void WII5Sparton::setConsoleDirect(bool in) {
  consoleDirect = in;
}
bool WII5Sparton::getConsoleDirect() {
  return consoleDirect;
}

bool WII5Sparton::isRunning() {
  return running;
}

// on
void WII5Sparton::on(bool force) {
  if (!running || force) {
    if (debug)
      console.log(LOG_DEBUG, F("SPARTON: power ON, baud=%lu"), (uint32_t)SerialIMU_Baud);
    running = true;
    cancel = false;
    wii5Controller.shared5On();
    #if POWER_IMU_SPARTON_PIN>0
    powerOn();
    #endif
    // TODO: investigate whether pinMode(15, INPUT_PULLUP) was needed here
    SerialIMU.begin(SerialIMU_Baud);
    // SerialIMU.attachInterrupt(handleRxChar);

    // stream = &SerialIMU;
    // beginSerialManager();
  }
}

void WII5Sparton::setBaudrate(uint32_t baud) {
  console.log(LOG_DEBUG, F("SPARTON: Debug set baud=%lu"), baud);
  SerialIMU.begin(baud);
}

// off
void WII5Sparton::off(bool force) {
  if (running || force) {
    if (debug)
      console.log(LOG_DEBUG, F("SPARTON: power off"));
    running = false;
    SerialIMU.end();
    // stream = NULL;
    // endSerialManager();
    displaySTATS();
    #if POWER_IMU_SPARTON_PIN>0
    powerOff();
    #endif
    wii5Controller.shared5Off();
  }
}

// Sparton Things:
//  - Turn Off
//  - Start now
//  - Start at X

bool firstBoot = true;

void WII5Sparton::sendLine(uint16_t l) {
  programLine(l);
  SerialIMU.println(wii5BufferString);
  // TODO Configurable?
  console.log(LOG_DEBUG, F("SPARTON: > %s"), wii5BufferString);
}

char c;
bool first;
uint8_t startReady;
void WII5Sparton::loop() {
  first = (step != stepLast);
  stepLast = step;

  while (SerialIMU.available())
    handleRxChar(SerialIMU.read());

  // WII5SerialManager::loop();
  switch (step) {
    case WII5SPARTON_OFF:
      off();
      break;

    case WII5SPARTON_BOOT:
      on();
      // Wait half a second then spin on
      if (stepWait > 500) {
        console.log(LOG_INFO, F("SPARTON: Startup"));
        if (firstBoot) {
          step = WII5SPARTON_BOOT_BAUD; stepWait = 0;
          if (debug)
            console.log(LOG_DEBUG, F("step boot->boot_baud"));
        }
        else {
          step = WII5SPARTON_BOOT_STOP; stepWait = 0;
          if (debug)
            console.log(LOG_DEBUG, F("step boot->boot_stop"));
        }
      }
      break;

    // TODO Reset !!!
    //  - first - high, 500ms later, low, or reversed !

    // First time boot - so lets check the baud rate is valid
    case WII5SPARTON_BOOT_BAUD:
      step = WII5SPARTON_BOOT_STOP; stepWait = 0;
      if (debug)
        console.log(LOG_DEBUG, F("step boot_baud->boot_stop"));
      firstBoot = false;
      break;

    case WII5SPARTON_BOOT_STOP:
      // First line is STOP
      // processMode = WII5SERIALPARSER_NONE;
      sendLine(0);
      sendLine(0);
      step = WII5SPARTON_BOOT_WAIT; stepWait = 0;
      if (debug)
        console.log(LOG_DEBUG, F("step boot_stop->boot_wait"));
      break;

    case WII5SPARTON_BOOT_WAIT:
      // First time - clear the captture file
      // Give it a full second to absorb all the junk
      if (stepWait > 1000) {
        SerialIMU.flush();
        // processMode = WII5SERIALPARSER_NONE;
        step = WII5SPARTON_READY; stepWait = 0;
        if (debug)
          console.log(LOG_DEBUG, F("step boot_wait->ready"));
      }
      break;

    case WII5SPARTON_READY:
      // Short Params (no gyro or mag)
      programStart = 20; programEnd = 30; sendCount = 0;
      // Experiment
      // programStart = 50; programEnd = 60; sendCount = 0;
      step = WII5SPARTON_PROGRAM_BASE; stepWait = 0;
      if (debug)
        console.log(LOG_DEBUG, F("step ready->program_base"));
      break;

    case WII5SPARTON_PROGRAM_BASE:
      if (stepWait > 250) {
        if (sendCount <= (programEnd - programStart)) {
          // Send next line, inremement counter, and wait another 10 ms
          sendLine(programStart + sendCount);
          sendCount++;
          stepWait = 0;
        }
        else {
          step = WII5SPARTON_PROGRAM_START; stepWait = 0;
        }
      }
      break;

    case WII5SPARTON_PROGRAM_START:
      // // processMode = WII5SERIALPARSER_RECORD;  // Capture fields
      // // last = WII5SERIALLAST_NONE;
      // step = WII5SPARTON_CAPTURE; stepWait = 0;
      // Sits here, waiting forever, until we move it on manually when ready to run.
      // But we stillshould add a timeout
      /// SKIPT to capture
      if (true) {
        // processMode = WII5SERIALPARSER_RECORD;  // Capture fields
        // last = WII5SERIALLAST_NONE;
        step = WII5SPARTON_CAPTURE; stepWait = 0;
      }
      break;

    case WII5SPARTON_CAPTURE:
      // Reset counters, so as not to loose first lot
      if (first) {
        console.log(LOG_INFO, F("SPARTON: Capturing"));
        resetData();
        timeStart = now();
      }

      // We do everything else in the gap between the records (TBC)
      // TODO This has not been tested
      else if (startWhen > 5) {

        // TODO Fix hardcoded time 5 above

        // TODO Fix hard coded 15 recods per block

        // Do we need to wrtie a block of data
        if ( (recordCount / 15) > blockNext) {

          // TODO test if block too far
          if ( (recordCount / 15) > (blockNext + 1) ) {
            console.log(LOG_FATAL, F("Sparton: Block too far, all data probably lost. block=%lu, expected=%lu, records=%lu"),
              blockNext,
              recordCount / 15,
              recordCount
            );
          }

          captureWriteTime = 0;
          if (sdBlock.dataIsOpen()) {
            if (sdBlock.dataWrite(
              &binRecords[(blockNext % 2) * 15],
              15 * 33
            )) {
              lastWrite = 0;
              // TODO No, slow
              /*
              console.log(LOG_INFO, F("Sparton: Block record write OK: block=%lu, records=%lu, binRecords=%d, size=%d"),
                blockNext, recordCount,
                int(&binRecords[(blockNext % 2) * 15]),
                int(15 * 33)
              );
              */
            }
            else {
              // TODO No, slow
              console.log(LOG_ERROR, F("Sparton: Block record write FAILED: %lu"), blockNext);
              // At this point - we stop processing ! But need to log that.
              console.log(LOG_ERROR, F("Sparton: CANCEL CAPTURE - Can't write to SD Card"));
              stop();
            }
          }
          else {
            console.log(LOG_ERROR, F("Sparton: Block Data not open"));
            // At this point - we stop processing ! But need to log that.
            console.log(LOG_ERROR, F("Sparton: CANCEL CAPTURE - Can't write to SD Card"));
            stop();
          }

          if (captureWriteTime > captureWriteMax)
            captureWriteMax = captureWriteTime;
          if (captureWriteTime < captureWriteMin)
            captureWriteMin = captureWriteTime;
          if (captureWriteTime > 5)
            captureWriteOver++;

          // TODO Catch errors?

          blockNext++;
        }

        else if ((stepWait > 700) && ((recordCount % HZ) == 0) && (lastValidRecord < SPARTON_RECORDS) ) {
          // console.printBinary((char*)&binBuffer[binBufferReady], sizeof(binBuffer[0]));
          wii5Display.atDataSend(
            // NOTE: Must not add the "@" - it isn't used in CRC calculation
            // line,stamp,err,8000,0,0,accel:x,y,z,gyro:x,y,z,mag:x,y,z,pos:x,y,z
            F("CaptureSummary"),
            F("%lu/%lu,%d,%d,%d,%d,%d,%d,block=%lu/%lu"),
            recordCount, recordTotal,
            int(binRecords[lastValidRecord].accel_x),
            int(binRecords[lastValidRecord].accel_y),
            int(binRecords[lastValidRecord].accel_z),
            int(binRecords[lastValidRecord].pose_x),
            int(binRecords[lastValidRecord].pose_y),
            int(binRecords[lastValidRecord].pose_z),
            blockNext, (recordTotal / 15)
          );
          /*
          Not recommended - 1 per second at 9600
          wii5Display.statusSend(
            F("Sparton"),
            F("record=%lu/%lu,z=%d"),
            recordCount, recordTotal,
            int(binRecords[lastValidRecord].accel_z)
          );
          */
          stepWait = 0;

        }

        // New 2024 - Check if recorsds never increasing and report errors !
        //  No records but startTime > 2 minutes - FAILURE !
        // TODO 2024 - This does NOT capture if the sparton fails half way through.... 
        //  What we want is no records for more than 2 minutes.
        // Change this code so startCapture is something like lastWriteTime or lastRecordTime and check that isn't 2 minutes old
        else if ( lastWrite > 120000) {
          console.log(LOG_INFO, F("Sparton: Failure ! No writes for 2 minutes"));
          snprintf_P(
            wii5Commands.lastCommandMessage, sizeof(wii5Commands.lastCommandMessage),
            PSTR("@Err,sparton,norecords")
          );
          wii5Commands.lastCommandCmd = 0;
          wii5Communications.sendError();
          step = WII5SPARTON_PROGRAM_STOP; stepWait = 0;
        }

        // Do we have enough?
        else if (recordCount >= recordTotal) {
          // If we are new block. OR half way through a block...
          if (
            sdBlock.dataIsOpen()
            && ((recordCount / 15) >= blockNext)
            && ((recordCount % 15) > 0)
          ) {

            // If half way, blank the rest
            if ((recordCount % 15) > 0) {
              // Blank the other records - 0 or 15 start + left over to end
              memset(&binRecords[((blockNext % 2) * 15) + (recordCount % 15)], 0, 33 * (15 - (recordCount % 15)));
            }

            if (!sdBlock.dataWrite(
              &binRecords[(blockNext % 2) * 15],
              15 * 33
            )) {
              console.log(LOG_ERROR, F("Sparton: Block record write FAILED: %lu"), blockNext);
            }

            blockNext++;
          }

          timeEnd = now();
          console.log(LOG_INFO, F("SPARTON: Finished Capture. Records=%lu of %lu, block written=%lu"), recordCount, recordTotal, blockNext);
          // processMode = WII5SERIALPARSER_NONE;
          // last = WII5SERIALLAST_NONE;
          // Display high speed stats
          // statsHighDisplay();
          if (debug)
            console.log(LOG_DEBUG, F("step capture->program_stop"));
          step = WII5SPARTON_PROGRAM_STOP; stepWait = 0;
        }
      }
      break;

    case WII5SPARTON_PROGRAM_STOP:
      // TODO send the stop command
      // programStart = 0; programEnd = 1;
      // step = WII5SPARTON_PROGRAM_BASE; stepWait = 0;
      // processMode = WII5SERIALPARSER_NONE;
      sendLine(0);
      step = WII5SPARTON_FINISH; stepWait = 0;
      // last = WII5SERIALLAST_NONE;
      if (debug)
        console.log(LOG_DEBUG, F("step program_stop->finish"));
      break;

    case WII5SPARTON_CANCEL:
      // Shut it down., power will go down at OFF
      // closeFile();
      sendLine(0);
      step = WII5SPARTON_FINISH; stepWait = 0;
      cancel = true;
      if (debug)
        console.log(LOG_DEBUG, F("step cancel->finish"));
      break;

    case WII5SPARTON_FINISH:
      step = WII5SPARTON_OFF; stepWait = 0;
      if (debug)
        console.log(LOG_DEBUG, F("step finish->off"));
      break;

    // TODO DEBUG THIS !
    case WII5SPARTON_INFO:
      if (first)  {
        sendCount = 0;
      }
      else if (stepWait > 500) {
        // Standard setup commands
        if (sendCount <= 12) {
          // Send next line, inremement counter, and wait another 10 ms
          sendLine(sendCount);
          sendCount++;
          stepWait = 0;
        }

        // 13 = 30 = Version
        else if (sendCount == 13) {
          sendLine(80);
          sendCount++;
          stepWait = 0;
        }

        // 14 = 31 = Config XML
        else if (sendCount == 14) {
          if (stepWait > 3000) {
            sendLine(81);
            sendCount++;
            stepWait = 0;
          }
        }

        // Wait 5 seconds to end
        else if (stepWait > 60000) {
          step = WII5SPARTON_OFF; stepWait = 0;
        }
      }
      break;

    default:
      console.log(LOG_FATAL, F("SPARTON: Default step... step=%d"), step);
      step = WII5SPARTON_OFF;
      // Power off
      break;
  }

  while (SerialIMU.available())
    handleRxChar(SerialIMU.read());

  return;
}

void WII5Sparton::repl() {
  uint8_t lastDot = 0;
  console.log(LOG_FATAL, F("SPARTON: REPL. '...' to exit."));

  start();
  // SerialIMU.detachInterrupt();
  while (1) {
    if (SerialConsole.available()) {
      char c = SerialConsole.read();
      if (c == '.')
        lastDot++;
      else
        lastDot = 0;
      if (lastDot >= 3) {
        SerialConsole.flush();
        SerialIMU.flush();
        stop();
        console.log(LOG_FATAL, F("SPARTON: Completed REPL. User exit."));
        return;
      }
      SerialIMU.write(c);
    }

    if (SerialIMU.available()) {
      char c = SerialIMU.read() ;
      SerialConsole.write(c);
    }
  }
}

void WII5Sparton::displaySTATS() {
  wii5Display.atStatsSend(
    F("Sparton"),
    F("errorTime=%lu,errorSize=%lu,writeMax=%lu,writeMin=%lu,writeOver=%lu"),
    statsTimeError, serialSizeError,
    captureWriteMax, captureWriteMin, captureWriteOver
  );
  #ifdef SPARTON_SIZES
  for (uint8_t t = 0; t < SIZES_LENGTH; t++) {
    if (sizes[t] > 0) {
      if (t == 0)
        console.printf(F("# SPARTON Record Size: others = %d\r\n"), sizes[t]);
      else
        console.printf(F("# SPARTON Record Size: %d = %d\r\n"), t + SIZES_START, sizes[t]);
    }
  }
  #endif
}

// TODO Totaly - this can be common wii5BufferString for all Serial Devices or even the Console
void WII5Sparton::programLine(uint16_t l) {
  memset(wii5BufferString, 0, WII5_BUFFER_STRING);

  // Main Programming
  switch (l) {

    // CODE = Wave Tank and original code using all things, including magnatometer
    // PROGRAM=0..12, START=13..13, PROGRAM+START=0..13, STOP=0..0
  	case 0:
      strcpy_P(wii5BufferString ,(PGM_P) F("chan0TriggerDivisor 0 set drop"));
      break;
  	case 1:
// TODO: confirm whether the reset+wait sequence here is actually required
      strcpy_P(wii5BufferString ,(PGM_P) F("accelrange 2 set drop"));
      break;
  	case 2:
      strcpy_P(wii5BufferString ,(PGM_P) F("chan0Enables array[ 0 15 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 ]array set drop"));
      break;
  	case 3:
      if (binMode)
        strcpy_P(wii5BufferString ,(PGM_P) F("chan0Format 2 set drop"));
      else
        strcpy_P(wii5BufferString ,(PGM_P) F("chan0Format 3 set drop"));
      break;
  	case 4:
      strcpy_P(wii5BufferString ,(PGM_P) F("chan0EnableBit cputime dvid@ set drop"));
      break;
  	case 5:
      strcpy_P(wii5BufferString ,(PGM_P) F("chan0EnableBit magp dvid@ set drop"));
      break;
  	case 6:
      strcpy_P(wii5BufferString ,(PGM_P) F("chan0EnableBit accelp dvid@ set drop"));
      break;
  	case 7:
      strcpy_P(wii5BufferString ,(PGM_P) F("chan0EnableBit roll dvid@ set drop"));
      break;
  	case 8:
      strcpy_P(wii5BufferString ,(PGM_P) F("chan0EnableBit pitch dvid@ set drop"));
      break;
  	case 9:
      strcpy_P(wii5BufferString ,(PGM_P) F("chan0EnableBit yaw dvid@ set drop"));
      break;
  	case 10:
      strcpy_P(wii5BufferString ,(PGM_P) F("chan0EnableBit gyrop dvid@ set drop"));
      break;
  	case 11:
      strcpy_P(wii5BufferString ,(PGM_P) F("chan0Trigger 6 set drop"));
      break;
  	case 12:
      if (HZ == 64) {
        // 64Hz
        strcpy_P(wii5BufferString ,(PGM_P) F("chanTimerX100us 156 set drop"));
        console.log(LOG_DEBUG, F("HZ 64, setting 100us timer to 156"));
      }
      else if (HZ == 8) {
        // 8Hz
        // TODO
        // strcpy_P(wii5BufferString ,(PGM_P) F("chanTimerX100us 10000 set drop"));
        strcpy_P(wii5BufferString ,(PGM_P) F("chanTimerX100us 1250 set drop"));
        console.log(LOG_DEBUG, F("HZ 8, setting 100us timer to 1250"));
      }
      else if (HZ == 1) {
        strcpy_P(wii5BufferString ,(PGM_P) F("chanTimerX100us 10000 set drop"));
        console.log(LOG_DEBUG, F("HZ 8, setting 100us timer to 1250"));
      }
      else {
        console.log(LOG_FATAL, F("Invalid HZ for Sprton = %d"), HZ);
      }
      break;
  	case 13:
      strcpy_P(wii5BufferString ,(PGM_P) F("chan0TriggerDivisor 1 set drop"));
      break;

    // CODE = Cut down code for WII5.1 Tas Delivery - no Gyro or Mag
    // PROGRAM=20..29
  	case 20:
      strcpy_P(wii5BufferString ,(PGM_P) F("chan0TriggerDivisor 0 set drop"));
      break;
  	case 21:
      strcpy_P(wii5BufferString ,(PGM_P) F("chan0Enables array[ 0 15 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 ]array set drop"));
      break;
  	case 22:
      if (binMode)
        strcpy_P(wii5BufferString ,(PGM_P) F("chan0Format 2 set drop"));
      else
        strcpy_P(wii5BufferString ,(PGM_P) F("chan0Format 3 set drop"));
      break;
  	case 23:
      strcpy_P(wii5BufferString ,(PGM_P) F("chan0EnableBit cputime dvid@ set drop"));
      break;
  	case 24:
      strcpy_P(wii5BufferString ,(PGM_P) F("chan0EnableBit accelp dvid@ set drop"));
      break;
  	case 25:
      strcpy_P(wii5BufferString ,(PGM_P) F("chan0EnableBit roll dvid@ set drop"));
      break;
  	case 26:
      strcpy_P(wii5BufferString ,(PGM_P) F("chan0EnableBit pitch dvid@ set drop"));
      break;
  	case 27:
      strcpy_P(wii5BufferString ,(PGM_P) F("chan0EnableBit yaw dvid@ set drop"));
      break;
  	case 28:
      strcpy_P(wii5BufferString ,(PGM_P) F("chan0Trigger 6 set drop"));
      break;
  	case 29:
      if (HZ == 64) {
        // 64Hz
        strcpy_P(wii5BufferString ,(PGM_P) F("chanTimerX100us 156 set drop"));
        console.log(LOG_DEBUG, F("HZ 64, setting 100us timer to 156"));
      }
      else if (HZ == 8) {
        // 8Hz
        // TODO
        // strcpy_P(wii5BufferString ,(PGM_P) F("chanTimerX100us 10000 set drop"));
        strcpy_P(wii5BufferString ,(PGM_P) F("chanTimerX100us 1250 set drop"));
        console.log(LOG_DEBUG, F("HZ 8, setting 100us timer to 1250"));
      }
      else if (HZ == 1) {
        strcpy_P(wii5BufferString ,(PGM_P) F("chanTimerX100us 10000 set drop"));
        console.log(LOG_DEBUG, F("HZ 8, setting 100us timer to 1250"));
      }
      else {
        console.log(LOG_FATAL, F("Invalid HZ for Sprton = %d"), HZ);
      }
      break;
  	case 30:
      strcpy_P(wii5BufferString ,(PGM_P) F("chan0TriggerDivisor 1 set drop"));
      break;

    // PROGRAM=0..12, START=13..13, PROGRAM+START=0..13, STOP=0..0
  	case 50:
      strcpy_P(wii5BufferString ,(PGM_P) F("chan0TriggerDivisor 0 set drop"));
      break;
  	case 51:
      strcpy_P(wii5BufferString ,(PGM_P) F("accelrange 2 set drop"));
      break;
  	case 52:
      strcpy_P(wii5BufferString ,(PGM_P) F("chan0Enables array[ 0 15 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 ]array set drop"));
      break;
  	case 53:
      if (binMode)
        strcpy_P(wii5BufferString ,(PGM_P) F("chan0Format 2 set drop"));
      else
        strcpy_P(wii5BufferString ,(PGM_P) F("chan0Format 3 set drop"));
      break;
  	case 54:
      strcpy_P(wii5BufferString ,(PGM_P) F("chan0EnableBit cputime dvid@ set drop"));
      break;
  	case 55:
      strcpy_P(wii5BufferString ,(PGM_P) F("chan0EnableBit roll dvid@ set drop"));
      break;
  	case 56:
      strcpy_P(wii5BufferString ,(PGM_P) F("chan0EnableBit pitch dvid@ set drop"));
      break;
  	case 57:
      strcpy_P(wii5BufferString ,(PGM_P) F("chan0EnableBit yaw dvid@ set drop"));
      break;
  	case 58:
      strcpy_P(wii5BufferString ,(PGM_P) F("chan0Trigger 6 set drop"));
      break;
  	case 59:
      if (HZ == 64) {
        // 64Hz
        strcpy_P(wii5BufferString ,(PGM_P) F("chanTimerX100us 156 set drop"));
        console.log(LOG_DEBUG, F("HZ 64, setting 100us timer to 156"));
      }
      else if (HZ == 8) {
        // 8Hz
        // TODO
        // strcpy_P(wii5BufferString ,(PGM_P) F("chanTimerX100us 10000 set drop"));
        strcpy_P(wii5BufferString ,(PGM_P) F("chanTimerX100us 1250 set drop"));
        console.log(LOG_DEBUG, F("HZ 8, setting 100us timer to 1250"));
      }
      else if (HZ == 1) {
        strcpy_P(wii5BufferString ,(PGM_P) F("chanTimerX100us 10000 set drop"));
        console.log(LOG_DEBUG, F("HZ 8, setting 100us timer to 1250"));
      }
      else {
        console.log(LOG_FATAL, F("Invalid HZ for Sprton = %d"), HZ);
      }
      break;
  	case 60:
      strcpy_P(wii5BufferString ,(PGM_P) F("chan0TriggerDivisor 1 set drop"));
      break;


    // Debugging / Logging
    //  14-20
  	case 80:
      strcpy_PF(wii5BufferString ,F("chan0TriggerDivisor 0 set drop"));
      break;
    case 81:
      strcpy_P(wii5BufferString, (PGM_P) F("name di."));
      break;
    case 82:
      strcpy_P(wii5BufferString, (PGM_P) F("serialnumber di."));
      break;
    case 83:
      strcpy_P(wii5BufferString, (PGM_P) F("VERSION di."));
      break;
    case 84:
      strcpy_P(wii5BufferString, (PGM_P) F("VERSION_M4 di."));
      break;
    case 85:
      strcpy_P(wii5BufferString, (PGM_P) F("gyroSampleRate di."));
      break;
    case 86:
      strcpy_P(wii5BufferString, (PGM_P) F("save_mask d.on"));
      break;
    case 87:
      strcpy_P(wii5BufferString, (PGM_P) F("db.print"));
      break;

    case 90:
      strcpy_P(wii5BufferString, (PGM_P) F("VERSION di."));
      break;

    case 91:
      strcpy_P(wii5BufferString, (PGM_P) F("chan0Enable."));
      break;

    case 100:
      strcpy_P(wii5BufferString, (PGM_P) F("baud 8 set drop"));
      break;

    default:
      console.log(LOG_FATAL, F("Sparton: request invalid programming string"));
      wii5BufferString[0] = '\0';
      break;
  }
}

WII5Sparton wii5Sparton;