/* ===== pfod Command for Menu_1 ====
pfodApp msg {.} --> {,<bg n><+7><r>~Led Control`0~V4|A<bg g><+4>`0~Led is ~~Off\On~|B<+4>~Voltage Plot}
 */
// Using Arduino Nano 33 BLE Board (V2.0.0)
// Use ArduinoBLE library V2.0.0
// Use Arduino V1.8.13+ IDE
/* Code generated by pfodDesignerV3 V3.0.3871
 */
/*
 * (c)2014-2021 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 <ArduinoBLE.h>
#include "SafeString.h"
// install SafeString library V4.0.3+ via Arduino Library Manager, include millisDelay
#include "millisDelay.h"

// download the libraries from http://www.forward.com.au/pfod/pfodParserLibraries/index.html
// pfodParser.zip V3.51+ contains pfodParser, pfodSecurity, pfodDelay, pfodBLEBufferedSerial, pfodSMS and pfodRadio
#include <pfodBLEBufferedSerial.h>


// =========== 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:
    static const int BLE_MAX_LENGTH = 20;     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 =======

pfodBLESerial bleSerial; // create a BLE serial connection
pfodBLEBufferedSerial bleBufferedSerial;

const int Led_pin = 13; // name the output pin for 'Led is'

millisDelay plotDataTimer; // plot data timer
unsigned long PLOT_DATA_INTERVAL = 1000;// mS == 1 sec, edit this to change the plot data interval

void setup() {
  pinMode(Led_pin, OUTPUT); // output for 'Led is' is initially LOW,
  digitalWrite(Led_pin, 0); // set output

  // set advertised local name and service UUID
  // begin initialization
  if (!bleSerial.begin()) {
    //  Serial.println("starting ble failed!");
    while (1);
  }
  bleBufferedSerial.connect(&bleSerial);
  plotDataTimer.start(PLOT_DATA_INTERVAL); // start plot timer

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

const size_t maxCmdLength = 5; // make SafeStrings at least large enough to hold longest cmd
// Use SafeStrings for the commands as comparing two SafeStrings is generally faster as the lengths can be compared first.
createSafeString(onCmdStr, maxCmdLength, "on");
createSafeString(offCmdStr, maxCmdLength, "off");

// input must be large enough to hold longest cmd + 1 delimiter
createSafeString(input, maxCmdLength + 1); //  to read input cmd + 1 delimiter
createSafeString(token, maxCmdLength + 1); // for parsing capacity >= input.capacity()

char delimiters[] = " .,\r\n"; // space dot comma CR NL are cmd delimiters

millisDelay timeout;
unsigned long TIMEOUT_MS = 300; // 0.3sec

// the loop routine runs over and over again forever:
void loop() {
  if (input.read(bleBufferedSerial)) {  // read from Serial, returns true if at least one character was added to SafeString input
    timeout.start(TIMEOUT_MS); // restart a 0.3sec timer every time something is read
  }
  if (input.nextToken(token, delimiters)) { // process at most one token per loop does not return tokens longer than input.capacity()
    if (token == onCmdStr) {
      digitalWrite(Led_pin, 1); // set output
      bleBufferedSerial.print("Led on");
    } else if (token == offCmdStr) {
      digitalWrite(Led_pin, 0); // set output
      bleBufferedSerial.print("Led off");
    } else { // not a valid cmd ignore
      bleBufferedSerial.print("Commands are on,off");
    }
  }
  if (timeout.justFinished()) { // nothing received for 0.3secs, terminated last chars so token will be processed.
    input += delimiters[0]; // any delimiter will do
  }

//  sendData(); // uncomment this to send data every 1sec
  //  <<<<<<<<<<<  Your other loop() code goes here 
  
}

void sendData() {
  if (plotDataTimer.justFinished()) {
    plotDataTimer.repeat(); // restart plot data timer, without drift
    // assign values to plot variables from your loop variables or read ADC inputs
    int plot_1_var = analogRead(A0); // read input to plot 
    // send plot data in CSV format
    bleBufferedSerial.print(millis());// time in milliseconds
    bleBufferedSerial.print(','); bleBufferedSerial.print(plot_1_var);
    bleBufferedSerial.println(); // end of CSV data record
  }
}


// ========== 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)");
BLECharacteristic pfodBLESerial::txCharacteristic("6E400003-B5A3-F393-E0A9-E50E24DCCA9E",  BLEIndicate | BLERead | BLENotify, BLE_MAX_LENGTH);
BLEDescriptor pfodBLESerial::txNameDescriptor("2901", "TX - (Indicate|Notify)");
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);
  if (strnstr((const char*)data, "{!}", len)) {
    central.disconnect();  // need to call this from this method!!
  }
}

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

  BLE.setConnectionInterval(80, 160); // 160 => 200mS max connection interval to match default pfodBLEBufferedSerial setting
  // 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 =========