/* ===== pfod Command for Menu_1 ====
pfodApp msg {.} --> {,<bg n>~Gyroscope Plot`0~V1|A~Plot Gyroscope}
 */
// Using Arduino Nano 33 BLE Board (Arduino Mbed OS Nano Boards V4.1.5)
// Use ArduinoBLE library V1.3.7
// Use Arduino V1.8.19+ IDE
/* Code generated by pfodDesignerV3 V3.0.4241
 */
/*
 * (c)2014-2024 Forward Computing and Control Pty. Ltd.
 * NSW Australia, www.forward.com.au
 * This code is not warranted to be fit for any purpose. You may only use it at your own risk.
 * This generated code may be freely used for both private and commercial use
 * provided this copyright is maintained.
 */

// include one of the two includes below depending on your Arduino NANO 33 BLE version
// for Arduino NANO 33 BLE (Sense) Ver 2
//#include "Arduino_BMI270_BMM150.h"

// else for Arduino NANO 33 BLE (Sense)   original release
#include <Arduino_LSM9DS1.h>

#include <ArduinoBLE.h>
#include <pfodParser.h>
// install pfodParser from the Arduino Library Manager
// OR download the libraries from http://www.forward.com.au/pfod/pfodParserLibraries/index.html

// slows down max data rate by about half (or more) but ensures each packet is acknowledged
//#define BLE_INDICATE

// un-comment DEBUG_BLE_SERIAL to debug connect/disconnect
//#define DEBUG_BLE_SERIAL
int swap01(int); // method prototype for slider end swaps
float getPlotVarScaling(long varMax, long varMin, float displayMax, float displayMin);

// =========== pfodBLESerial class definition =======
static const char* localName = "Nano 33 BLE"; // <<<<<<  change this string to customize the adverised name of your board (max 31 chars)
class pfodBLESerial : public Stream {
  public:
    static BLEService uartService;    static BLEDescriptor uartNameDescriptor;    static BLECharacteristic rxCharacteristic;    static BLEDescriptor rxNameDescriptor;
    static BLECharacteristic txCharacteristic;    static BLEDescriptor txNameDescriptor;    pfodBLESerial();    bool begin();    void poll();    size_t write(uint8_t); size_t write(const uint8_t*, size_t);
    int read();  int available(); void flush(); int peek(); void close();    bool isConnected();
    static void connectHandler(BLEDevice central);    static void disconnectHandler(BLEDevice central);    static void receiveHandler(BLEDevice central, BLECharacteristic rxCharacteristic);

  private:
    // BLE_MAX_LENGTH for Nano 33 BLE is 247-3
    static const int BLE_MAX_LENGTH = 240;     static const int BLE_RX_MAX_LENGTH = 256;    static volatile size_t rxHead; static volatile size_t rxTail;    static volatile uint8_t rxBuffer[BLE_RX_MAX_LENGTH];
    static size_t txIdx;  static uint8_t txBuffer[BLE_MAX_LENGTH];    static void addReceiveBytes(const uint8_t* bytes, size_t len);    volatile static bool connected;
};
// =========== end pfodBLESerial class definition =======

pfodParser parser("V1"); // create a parser to handle the pfod messages
pfodBLESerial bleSerial; // create a BLE serial connection

unsigned long plot_msOffset = 0; // set by {@} response
bool clearPlot = false; // set by the {@} response code

// ==== not needed for Gyroscope ===
//// plotting data variables
//int plot_1_varMin = 0;
//int plot_1_var = plot_1_varMin;
//float plot_1_scaling;
//float plot_1_varDisplayMin = 0.0;
//int plot_2_varMin = 0;
//int plot_2_var = plot_2_varMin;
//float plot_2_scaling;
//float plot_2_varDisplayMin = 0.0;
//int plot_3_varMin = 0;
//int plot_3_var = plot_3_varMin;
//float plot_3_scaling;
//float plot_3_varDisplayMin = 0.0;

// Gyroscope variables
float x;
float y;
float z;

pfodDelay plotDataTimer; // plot data timer
unsigned long PLOT_DATA_INTERVAL = 1;// 1ms plot data interval, but Indicate BLE packet write takes about 20ms to 30ms and occasionally more
// also in pfodApp connection, set keepAlive to 0 

// the setup routine runs once on reset:
void setup() {
#ifdef DEBUG_BLE_SERIAL
  Serial.begin(9600);
  for (int i = 10; i > 0; i--) {
    Serial.print(' '); Serial.print(i);
    delay(1000);
  }
  Serial.println();
#endif

//=== not needed for Gyroscope ===
// calculate the plot vars scaling here once to reduce computation
//  plot_1_scaling = getPlotVarScaling(1023,plot_1_varMin,1023.0,plot_1_varDisplayMin);
//  plot_2_scaling = getPlotVarScaling(1023,plot_2_varMin,1023.0,plot_2_varDisplayMin);
//  plot_3_scaling = getPlotVarScaling(1023,plot_3_varMin,1023.0,plot_3_varDisplayMin);

  // set advertised local name and service UUID
  // begin initialization
  if (!bleSerial.begin()) {
    while (1) {
#ifdef DEBUG_BLE_SERIAL
      Serial.println("Starting BLE failed!");
#endif
      delay(3000); // repeat every 3 sec
    }
  }

  if (!IMU.begin()) {
    Serial.println("Failed to initialize IMU!");
    while (1);
  }

  parser.connect(&bleSerial);
#ifdef DEBUG_BLE_SERIAL
  Serial.println("BLE Started");
#endif

  plotDataTimer.start(PLOT_DATA_INTERVAL); // start plot timer

  // <<<<<<<<< Your extra setup code goes here
}

bool plotData = false;  // only send data when chart open
bool haveNewData = false; // set to true when new data read and set to false when data sent

// the loop routine runs over and over again forever:
void loop() {
  if (IMU.gyroscopeAvailable()) {
    IMU.readGyroscope(x, y, z);
    haveNewData = true;
  }
  
  uint8_t cmd = parser.parse(); // parse incoming data from connection
  // parser returns non-zero when a pfod command is fully parsed
  if (cmd != 0) { // have parsed a complete msg { to }
    plotData = false;
    uint8_t* pfodFirstArg = parser.getFirstArg(); // may point to \0 if no arguments in this msg.
    pfod_MAYBE_UNUSED(pfodFirstArg); // may not be used, just suppress warning
    long pfodLongRtn; // used for parsing long return arguments, if any
    pfod_MAYBE_UNUSED(pfodLongRtn); // may not be used, just suppress warning
    if ('.' == cmd) {
      // pfodApp has connected and sent {.} , it is asking for the main menu
      if (!parser.isRefresh()) {
        sendMainMenu(); // send back the menu designed
      } else {
        sendMainMenuUpdate(); // menu is cached just send update
      }

      // handle {@} request
    } else if('@'==cmd) { // pfodApp requested 'current' time
      plot_msOffset = millis(); // capture current millis as offset rawdata timestamps
      clearPlot = true; // clear plot on reconnect as have new plot_msOffset
      parser.print(F("{@`0}")); // return `0 as 'current' raw data milliseconds
    

    // now handle commands returned from button/sliders
    } else if('A'==cmd) { // user pressed -- 'Plot Gyroscope'
      // in the main Menu of Menu_1 
      // return plotting msg.
      parser.print(F("{=Gyroscope`1000~ss.S")); // the `1000 display 1000 points on the chart before rolling (default 500)
      // ss.S the .S adds one decimal point to the display of secs
      if (clearPlot) {
        clearPlot = false;
        parser.print(F("~C"));
      }
      parser.print(F("|time (ss.S)|X~~~X degs/sec|Y~~~Y degs/sec|Z~~~Z degs/sec}"));
      plotData = true;

    } else if ('!' == cmd) {
      // CloseConnection command
      closeConnection(parser.getPfodAppStream());
    } else {
      // unknown command
      parser.print(F("{}")); // always send back a pfod msg otherwise pfodApp will disconnect.
    }
  }
  sendData();
  //  <<<<<<<<<<<  Your other loop() code goes here 
  
}

void closeConnection(Stream *io) {
  // add any special code here to force connection to be dropped
  ((pfodBLESerial*)io)->close();
}

void sendData() {
//  if (plotDataTimer.justFinished()) {
//    plotDataTimer.restart(); // restart plot data timer

  if (plotData && haveNewData) { // only send data if chart is showing and have new data
    parser.print(millis()-plot_msOffset);// time in milliseconds
    parser.print(','); parser.print(x); // plot the Gyroscope values
    parser.print(','); parser.print(y);
    parser.print(','); parser.print(z);
    parser.println(); // end of CSV data record
    haveNewData = false; // clear flag
  }
}


float getPlotVarScaling(long varMax, long varMin, float displayMax, float displayMin) {
  long varRange = varMax - varMin;
  if (varRange == 0) { varRange = 1; } // prevent divide by zero
  return (displayMax - displayMin)/((float)varRange);
}


void sendMainMenu() {
  // !! Remember to change the parser version string
  //    every time you edit this method
  parser.print(F("{,"));  // start a Menu screen pfod message
  // send menu background, format, prompt, refresh and version
  parser.print(F("<bg n>~Gyroscope Plot`0"));
  parser.sendVersion(); // send the menu version 
  // send menu items
  parser.print(F("|A"));
  parser.print(F("~Plot Gyroscope"));
  parser.print(F("}"));  // close pfod message
}

void sendMainMenuUpdate() {
  parser.print(F("{;"));  // start an Update Menu pfod message
  // send menu items
  parser.print(F("|A"));
  parser.print(F("}"));  // close pfod message
  // ============ end of menu ===========
}
// ========== pfodBLESerial ===========
BLEService pfodBLESerial::uartService("6E400001-B5A3-F393-E0A9-E50E24DCCA9E");
BLEDescriptor pfodBLESerial::uartNameDescriptor("2901", localName);
BLECharacteristic pfodBLESerial::rxCharacteristic("6E400002-B5A3-F393-E0A9-E50E24DCCA9E", BLEWrite, BLE_MAX_LENGTH); 
BLEDescriptor pfodBLESerial::rxNameDescriptor("2901", "RX - (Write)");
#ifdef BLE_INDICATE
BLECharacteristic pfodBLESerial::txCharacteristic("6E400003-B5A3-F393-E0A9-E50E24DCCA9E",  BLEIndicate | BLERead | BLENotify, BLE_MAX_LENGTH);
BLEDescriptor pfodBLESerial::txNameDescriptor("2901", "TX - (Indicate | Notify)");
#else
BLECharacteristic pfodBLESerial::txCharacteristic("6E400003-B5A3-F393-E0A9-E50E24DCCA9E",  BLERead | BLENotify, BLE_MAX_LENGTH);
BLEDescriptor pfodBLESerial::txNameDescriptor("2901", "TX - (Notify)");
#endif
volatile size_t pfodBLESerial::rxHead;
volatile size_t pfodBLESerial::rxTail;
volatile uint8_t pfodBLESerial::rxBuffer[BLE_RX_MAX_LENGTH];
size_t pfodBLESerial::txIdx;
uint8_t pfodBLESerial::txBuffer[BLE_MAX_LENGTH];
volatile bool pfodBLESerial::connected;

pfodBLESerial::pfodBLESerial() {
  connected = false;
}

void pfodBLESerial::connectHandler(BLEDevice central) {
  (void)(central); // may not be used, just suppress warning
#ifdef DEBUG_BLE_SERIAL
  // print the central's MAC address:
  Serial.print("Connected to central: ");   Serial.println(central.address());
#endif
  pfodBLESerial::connected = true;
}

void pfodBLESerial::disconnectHandler(BLEDevice central) {
  (void)(central); // may not be used, just suppress warning
#ifdef DEBUG_BLE_SERIAL
  // print the central's MAC address:
  Serial.print("Disconnected from central: "); Serial.println(central.address());
#endif
  pfodBLESerial::connected = false;
}

void pfodBLESerial::receiveHandler(BLEDevice central, BLECharacteristic rxCharacteristic) {
  (void)(central); // may not be used, just suppress warning
  size_t len = rxCharacteristic.valueLength();
  const unsigned char *data = rxCharacteristic.value();
#ifdef DEBUG_BLE_SERIAL
  Serial.print("'"); Serial.write(data, len);  Serial.print("'"); Serial.println();
#endif
  pfodBLESerial::addReceiveBytes((const uint8_t*)data, len);
  // check for {!} in data
  if (len < 3) {
    return; // not there
  }
  size_t idx = len-1;
  bool disconnect = false;
  if ((data[idx-2] == '{') && (data[idx-1] == '!') && (data[idx] == '}')) {
    disconnect = true;
  } else {
    // not {!} at end of data so can just make data[idx] = '0' and use strstr
    uint8_t* nonConstData = (uint8_t*)data;
    nonConstData[idx] = 0;
    if (strstr((const char*)data, "{!}")) {
      disconnect = true;
    }
  }
  if (disconnect) {
    central.disconnect();  // need to call this from this method!!
  }
}

bool pfodBLESerial::begin() { // begin initialization
  if (!BLE.begin()) {
    return false;
  }

  BLE.setConnectionInterval(6, 6);  // min BLE Version 4 connection interval is 7.5ms => 6 * 1.25m counts, so try to force that.

  // set advertised local name and service UUID:
  BLE.setLocalName(localName);
  BLE.setAdvertisedService(uartService);
  rxCharacteristic.addDescriptor(rxNameDescriptor);
  rxCharacteristic.addDescriptor(uartNameDescriptor);
  txCharacteristic.addDescriptor(txNameDescriptor);
  txCharacteristic.addDescriptor(uartNameDescriptor);
  // add the characteristic to the service
  uartService.addCharacteristic(rxCharacteristic);
  uartService.addCharacteristic(txCharacteristic);
  // add service
  BLE.addService(uartService);
  // assign event handlers for connected, disconnected to peripheral
  BLE.setEventHandler(BLEConnected, connectHandler);
  BLE.setEventHandler(BLEDisconnected, disconnectHandler);
  // assign event handlers for characteristic
  rxCharacteristic.setEventHandler(BLEWritten, receiveHandler);
  // start advertising
  BLE.advertise();
  return true;
}

bool pfodBLESerial::isConnected() {
  return (connected && txCharacteristic.subscribed());
}

void pfodBLESerial::close() {
#ifdef DEBUG_BLE_SERIAL
  Serial.println(F("close() called"));
#endif
}

int pfodBLESerial::read() {
  if (rxTail == rxHead) {
    return -1;
  }
  // note increment rxHead befor writing so need to increment rxTail befor reading
  rxTail = (rxTail + 1) % sizeof(rxBuffer);
  uint8_t b = rxBuffer[rxTail];
  return b;
}

void pfodBLESerial::addReceiveBytes(const uint8_t* bytes, size_t len) {
  // note increment rxHead befor writing so need to increment rxTail befor reading
  for (size_t i = 0; i < len; i++) {
    rxHead = (rxHead + 1) % sizeof(rxBuffer);
    rxBuffer[rxHead] = bytes[i];
  }
}

void pfodBLESerial::poll() {
  BLE.poll();
}

// called as part of parser.parse() so will poll() each loop()
int pfodBLESerial::available() {
  poll();
  flush(); // send any pending data now. This happens at the top of each loop()
  int rtn = ((rxHead + sizeof(rxBuffer)) - rxTail ) % sizeof(rxBuffer);
  return rtn;
}

void pfodBLESerial::flush() {
  if (txIdx == 0) {
    return;
  }
  txCharacteristic.setValue(txBuffer, txIdx);
  txIdx = 0;
  poll();
}

int pfodBLESerial::peek() {
  poll();
  if (rxTail == rxHead) {
    return -1;
  }
  size_t nextIdx = (rxTail + 1) % sizeof(rxBuffer);
  uint8_t byte = rxBuffer[nextIdx];
  return byte;
}

size_t pfodBLESerial::write(const uint8_t* bytes, size_t len) {
  for (size_t i = 0; i < len; i++) {
    write(bytes[i]);
  }
  return len; // just assume it is all written
}

size_t pfodBLESerial::write(uint8_t b) {
  poll();
  if (!isConnected()) {
    return 1;
  }
  txBuffer[txIdx++] = b;
  if ((txIdx == sizeof(txBuffer)) || (b == ((uint8_t)'\n')) || (b == ((uint8_t)'}')) ) {
    flush(); // send this buffer if full or end of msg or rawdata newline
  }
  return 1;
}

int swap01(int in) {
  return (in==0)?1:0;
}
// ============= end generated code =========