#include "env.h"
#include "display.h"

Display display;
Adafruit_NAU7802 nau;
unsigned long _millis = 0;
int32_t val = 0;
#if BLE
  BLEServer *pServer = NULL;
  BLECharacteristic *millisCharacteristic = NULL;
  BLECharacteristic *weightCharacteristic = NULL;
  BLEAdvertising *pAdvertising = NULL;
  bool deviceConnected = false;
  bool oldDeviceConnected = false;

  // Callback class to handle connection events
  class MyServerCallbacks : public BLEServerCallbacks {
    void onConnect(BLEServer* pServer) {
      deviceConnected = true;
      BLEDevice::startAdvertising();
    }

    void onDisconnect(BLEServer* pServer) {
      deviceConnected = false;
    }
  };
#endif

void setup() {
  Serial.begin(115200);
  display.setup();

  if (! nau.begin()) {
    Serial.println("Failed to find NAU7802");
    while (1) delay(10);  // Don't proceed.
  }

  nau.setLDO(LDO);
  nau.setGain(GAIN);
  nau.setRate(SPS);

  // Take 10 readings to flush out readings
  for (uint8_t i=0; i<10; i++) {
    while (! nau.available()) delay(1);
    nau.read();
  }

  while (! nau.calibrate(NAU7802_CALMOD_INTERNAL)) {
    Serial.println("Failed to calibrate internal offset, retrying!");
    delay(1000);
  }

  while (! nau.calibrate(NAU7802_CALMOD_OFFSET)) {
    Serial.println("Failed to calibrate system offset, retrying!");
    delay(1000);
  }

  #if BLE
    // initialize the Bluetooth® Low Energy hardware
    BLEDevice::init("Smaage");
    pServer = BLEDevice::createServer();
    pServer->setCallbacks(new MyServerCallbacks());
    
    BLEService *pService = pServer->createService(SERVICE_UUID);
    
    millisCharacteristic = pService->createCharacteristic(
        MILLIS_UUID,
        BLECharacteristic::PROPERTY_READ | BLECharacteristic::PROPERTY_NOTIFY
    );
    weightCharacteristic = pService->createCharacteristic(
        WEIGHT_UUID,
        BLECharacteristic::PROPERTY_READ | BLECharacteristic::PROPERTY_NOTIFY
    );
    millisCharacteristic->setValue((uint8_t *)&_millis, sizeof _millis);
    weightCharacteristic->setValue((uint8_t *)&val, sizeof val);
    pService->start();
    
    pAdvertising = BLEDevice::getAdvertising();
    pAdvertising->addServiceUUID(SERVICE_UUID);
    pAdvertising->setScanResponse(true);
    BLEDevice::startAdvertising();
    
    Serial.println("BLE device is now advertising...");
    Serial.print("BLE Address: ");
    Serial.println(BLEDevice::getAddress().toString().c_str());
  #endif

  display.print("Smaage is ready!");
}

void loop() {
  while (!nau.available()) {
    delay(1);
  }
  val = nau.read();

  #if BLE
    if (deviceConnected) {
      // Send the sensor reading
      _millis = millis();
      millisCharacteristic->setValue((uint8_t *)&_millis, sizeof _millis);
      millisCharacteristic->notify();
      weightCharacteristic->setValue((uint8_t *)&val, sizeof val);
      weightCharacteristic->notify();
    }
    // disconnecting
    if (!deviceConnected && oldDeviceConnected) {
      delay(500);                   // give the bluetooth stack the chance to get things ready
      pServer->startAdvertising();  // restart advertising
      Serial.println("start advertising");
      oldDeviceConnected = deviceConnected;
    }
    // connecting
    if (deviceConnected && !oldDeviceConnected) {
      // do stuff here on connecting
      oldDeviceConnected = deviceConnected;
    }
  #else
    Serial.print(millis()); Serial.print(","); Serial.println(val);
  #endif
}