#include "arduino_bma456.h"
#include <bluefruit.h>
#include <Adafruit_LittleFS.h>
#include <InternalFileSystem.h>

// BLE Service
BLEDfu  bledfu;  // OTA DFU service
BLEDis  bledis;  // device information
BLEUart bleuart; // uart over ble
BLEBas  blebas;  // battery

// Analog reading
int adcin = A7;
int adcvalue = 0;
float mv_per_lsb = 3600.0F / 1024.0F; // 10-bit ADC with 3.6V input range
bma4_accel_config accelConfig;
BMA456 bma456accel;

bool initTime = false;
int initialTime = 0;
int timeValue = 0;
void setup()
{
  Serial.begin(115200);
  while ( !Serial ) delay(10);   // for nrf52840 with native usb

  Serial.println("FYP Bridge Program");
  Serial.println("------------------------------------\n");

  // Setup the BLE LED to be enabled on CONNECT
  // Note: This is actually the default behaviour, but provided
  // here in case you want to control this LED manually via PIN 19
  Bluefruit.autoConnLed(true);

  // Config the peripheral connection with maximum bandwidth
  // more SRAM required by SoftDevice
  // Note: All config***() function must be called before begin()
  Bluefruit.configPrphBandwidth(BANDWIDTH_MAX);

  Bluefruit.begin();
  Bluefruit.setTxPower(4);    // Check bluefruit.h for supported values
  Bluefruit.setName("Bridge Sensor Node");
  //Bluefruit.setName(getMcuUniqueID()); // useful testing with multiple central connections
  Bluefruit.Periph.setConnectCallback(connect_callback);
  Bluefruit.Periph.setDisconnectCallback(disconnect_callback);

  // To be consistent OTA DFU should be added first if it exists
  bledfu.begin();

  // Configure and Start Device Information Service
  bledis.setManufacturer("SparkFun");
  bledis.setModel("Pro nRF52840 Mini");
  bledis.begin();

  // Configure and Start BLE Uart Service
  bleuart.begin();

  // Start BLE Battery Service
  blebas.begin();
  blebas.write(100);

  // Set up and start advertising
  startAdv();

  // Set up sensor
  bma456accel.initialize();

  Serial.println("Please use the app to connect");
}

void startAdv(void)
{
  // Advertising packet
  Bluefruit.Advertising.addFlags(BLE_GAP_ADV_FLAGS_LE_ONLY_GENERAL_DISC_MODE);
  Bluefruit.Advertising.addTxPower();

  // Include bleuart 128-bit uuid
  Bluefruit.Advertising.addService(bleuart);

  // Secondary Scan Response packet (optional)
  // Since there is no room for 'Name' in Advertising packet
  Bluefruit.ScanResponse.addName();

  /* Start Advertising
     - Enable auto advertising if disconnected
     - Interval:  fast mode = 20 ms, slow mode = 152.5 ms
     - Timeout for fast mode is 30 seconds
     - Start(timeout) with timeout = 0 will advertise forever (until connected)

     For recommended advertising interval
     https://developer.apple.com/library/content/qa/qa1931/_index.html
  */
  Bluefruit.Advertising.restartOnDisconnect(true);
  Bluefruit.Advertising.setInterval(32, 244);    // in unit of 0.625 ms
  Bluefruit.Advertising.setFastTimeout(30);      // number of seconds in fast mode
  Bluefruit.Advertising.start(0);                // 0 = Don't stop advertising after n seconds
}

void getTime(void) {
  if (!initTime) {
    timeValue = 0;
    initialTime = micros();
    initTime = true;
  }
  timeValue = micros() - initialTime;
  if (timeValue < 0) {
    timeValue += 2 ^ 32 - 1;
  }
}

void loop()
{
  //Serial.print("initialTime: "); Serial.print(initialTime);
  // Get temperature event and print its value.
  //int32_t temp = 0;
  //temp = bma456accel.getTemperature();

  /*char tempVal[5];
    snprintf(tempVal, sizeof(temp), "%f", (temp));

    bleuart.write("Temperature: ");
    bleuart.write(temp);
    bleuart.write("*C");
    bleuart.write("\r\n");
  */

  // Get acceleration events.
  float x[128], y[128], z[128];
  for (int i = 0; i < 128; i++) {
    bma456accel.getAcceleration(&x[i], &y[i], &z[i]);
    getTime();
    Serial.print("t:\t"); Serial.print(timeValue);
    Serial.print("\t,X,\t"); Serial.print(x[i]);
    Serial.print("\tY,\t"); Serial.print(y[i]);
    Serial.print("\tZ,\t"); Serial.print(z[i]);
    Serial.println();
    printOutConfig();
    //delayMicroseconds(625); //delay time between samples
  }

  //Write events to blueuart
  for (int i = 0; i < 128; i++) {
    char xVal[8], yVal[8], zVal[8];
    snprintf(xVal, sizeof(x[i]), "%f", (x[i]));

    snprintf(yVal, sizeof(y[i]), "%f", (y[i]));

    snprintf(zVal, sizeof(z[i]), "%f", (z[i]));

    //Transmit 128 points

    bleuart.write("X,\t"); bleuart.write(xVal);
    bleuart.write("\tY,\t"); bleuart.write(yVal);
    bleuart.write("\tZ,\t"); bleuart.write(zVal);
    //bleuart.write("m/s^2 ");
    bleuart.write("\r\n");
    bleuart.write("------------------------------------");
    //bleuart.write("\r\n");
  }



  // Print to Serial for debugging use only.
  /*
    Serial.print("Temperature: ");
    Serial.print(temp);
    Serial.print("*C");
    Serial.print("\r\n");
  */





  

  /*
    //Adc read
    adcvalue = analogRead(adcin);

    // Display the results
    Serial.print("Adc value: ");
    Serial.print(adcvalue);
    Serial.print(" [");
    Serial.print((float)adcvalue * mv_per_lsb);
    Serial.println(" mV]");
  */
  //Delay.
  //delay(0.625);
}

void printOutConfig(void){
  accelConfig = bma456accel.getAccelConfig();

  Serial.println("--BMA Setup--");
  Serial.print("odr: "); Serial.print(accelConfig.odr); Serial.println();
  Serial.print("bw: "); Serial.print(accelConfig.bandwidth); Serial.println();
  Serial.print("perf_mode: "); Serial.print(accelConfig.perf_mode); Serial.println();
  Serial.print("range: "); Serial.print(accelConfig.range); Serial.println();
  int interruptStatus = bma456accel.getInterruptStatus();
  if (interruptStatus > 0) {
    Serial.print("Interrupt Status: "); Serial.print(interruptStatus); Serial.println();
  }
  Serial.println("--BMA Setup--");
}

// callback invoked when central connects
void connect_callback(uint16_t conn_handle)
{
  // Get the reference to current connection
  BLEConnection* connection = Bluefruit.Connection(conn_handle);

  char central_name[32] = { 0 };
  connection->getPeerName(central_name, sizeof(central_name));

  Serial.print("Connected to ");
  Serial.println(central_name);
}

/**
   Callback invoked when a connection is dropped
   @param conn_handle connection where this event happens
   @param reason is a BLE_HCI_STATUS_CODE which can be found in ble_hci.h
*/
void disconnect_callback(uint16_t conn_handle, uint8_t reason)
{
  (void) conn_handle;
  (void) reason;

  Serial.println();
  Serial.println("Disconnected");
}