BlueNRG-2 hot flashes

/***************************************************
  Adafruit MQTT Library ESP8266 Example
  Must use ESP8266 Arduino from:
    https://github.com/esp8266/Arduino
  Works great with Adafruit's Huzzah ESP board & Feather
  ----> https://www.adafruit.com/product/2471
  ----> https://www.adafruit.com/products/2821
  Adafruit invests time and resources providing this open source code,
  please support Adafruit and open-source hardware by purchasing
  products from Adafruit!
  Written by Tony DiCola for Adafruit Industries.
  MIT license, all text above must be included in any redistribution

  Code updated to use MQTT with ESP32 and merged BLE client Arduino example
  thank you Neil Kolban!
  This project creates a BLE client using ESP32 wRover to get services of
  BCN-002 STMicro sensor board, additionally publishes data to Adafruit IO.
 

 
 ****************************************************/
#include <Adafruit_MQTT.h>
#include <Adafruit_MQTT_Client.h>

//#include <EEPROM.h>
#include <WiFi.h>

#include "BLEDevice.h"

bool speed1_State = 0;
bool speed2_State = 0;
bool autoMode_State = 0;

// The remote service we wish to connect to.
static BLEUUID serviceUUID("00000000-0001-11E1-9AB4-0002A5D5C51B");
// The characteristic of the remote service we are interested in.
static BLEUUID    char1UUID("001c0000-0001-11E1-AC36-0002A5D5C51B"); //Env
static BLEUUID    char2UUID("00000100-0001-11E1-AC36-0002A5D5C51B"); //MEMS
static BLEUUID    char3UUID("00020000-0001-11E1-AC36-0002A5D5C51B"); //Gauge (battery)
std::string My_BLE_Address = "ec:4d:a6:01:a1:eb"; //Update as required

static boolean doConnect = false;
static boolean connected = false;
static boolean doScan = false;
static BLERemoteCharacteristic* p1RemoteCharacteristic;
static BLERemoteCharacteristic* p2RemoteCharacteristic;
static BLERemoteCharacteristic* p3RemoteCharacteristic;
static BLEAdvertisedDevice* myDevice;

//Environmental

float DataTemp;
float DataHum;
float DataPres;
//Latest teemperaure, if it increases over 2 degrees then is a hot flash
bool  flashFlag = false;
bool  automodeFlag = false;
int tempEvtCnt = 0;
float DataTempAvg=0; 
float DataTempAvgLast=0; 
  
//MEMS fusion
int16_t DataQi;
int16_t DataQj;
int16_t  DataQk;

float DataQiDec=0;
float DataQjDec=0;
float DataQkDec=0;
float DataQsDec=0; // S factor

//Gauge
float DataVbatt;


/************************* Pin Definition *********************************/
#define RELAY1            5
#define RELAY2            21

/************************* WiFi Access Point *********************************/

#define WLAN_SSID       "Your SSID"
#define WLAN_PASS       "Your SSID password"

/************************* Adafruit.io Setup *********************************/

#define AIO_SERVER      "io.adafruit.com"
#define AIO_SERVERPORT  1883                   // use 8883 for SSL
#define AIO_USERNAME    "your user"//Adafruit.io
#define AIO_KEY         "your Adafruit Key"//Adafruit AIO Key

/************ Global State (you don't need to change this!) ******************/

// Create an ESP8266 WiFiClient class to connect to the MQTT server.
WiFiClient client;
// or... use WiFiFlientSecure for SSL
//WiFiClientSecure client;

// Setup the MQTT client class by passing in the WiFi client and MQTT server and login details.
Adafruit_MQTT_Client mqtt(&client, AIO_SERVER, AIO_SERVERPORT, AIO_USERNAME, AIO_KEY);

/****************************** Feeds ***************************************/

// Notice MQTT paths for AIO follow the form: <username>/feeds/<feedname>
Adafruit_MQTT_Publish Humidity = Adafruit_MQTT_Publish(&mqtt, AIO_USERNAME "/feeds/humidity");
Adafruit_MQTT_Publish Temperature = Adafruit_MQTT_Publish(&mqtt, AIO_USERNAME "/feeds/temperature");
Adafruit_MQTT_Publish Qi = Adafruit_MQTT_Publish(&mqtt, AIO_USERNAME "/feeds/qi");
Adafruit_MQTT_Publish Qj = Adafruit_MQTT_Publish(&mqtt, AIO_USERNAME "/feeds/qj");
Adafruit_MQTT_Publish Qk = Adafruit_MQTT_Publish(&mqtt, AIO_USERNAME "/feeds/qk");
Adafruit_MQTT_Publish Qs = Adafruit_MQTT_Publish(&mqtt, AIO_USERNAME "/feeds/qs");
Adafruit_MQTT_Publish Pressure = Adafruit_MQTT_Publish(&mqtt, AIO_USERNAME "/feeds/pressure");
Adafruit_MQTT_Publish Battery = Adafruit_MQTT_Publish(&mqtt, AIO_USERNAME "/feeds/battery");
Adafruit_MQTT_Publish HotFlash = Adafruit_MQTT_Publish(&mqtt, AIO_USERNAME "/feeds/hot-flash");

Adafruit_MQTT_Subscribe Speed2 = Adafruit_MQTT_Subscribe(&mqtt, AIO_USERNAME"/feeds/max-speed"); 
Adafruit_MQTT_Subscribe Speed1 = Adafruit_MQTT_Subscribe(&mqtt, AIO_USERNAME "/feeds/min-speed");
Adafruit_MQTT_Subscribe TurnOff = Adafruit_MQTT_Subscribe(&mqtt, AIO_USERNAME "/feeds/turn-off");
Adafruit_MQTT_Subscribe AutoMode = Adafruit_MQTT_Subscribe(&mqtt, AIO_USERNAME "/feeds/auto-mode");


/*************************** Sketch Code ************************************/

// Bug workaround for Arduino 1.6.6, it seems to need a function declaration
// for some reason (only affects ESP8266, likely an arduino-builder bug).
//void MQTT_connect();

static void NotifyCallback(
  BLERemoteCharacteristic* pBLERemoteCharacteristic,
  uint8_t* pData,
  size_t length,
  bool isNotify) {
  if(pBLERemoteCharacteristic->getUUID().toString() == char1UUID.toString())
     dataEnvironmental(pData);
  else if (pBLERemoteCharacteristic->getUUID().toString() == char2UUID.toString())
     dataMotion(pData);
  else if (pBLERemoteCharacteristic->getUUID().toString() == char3UUID.toString())
     dataGauge(pData);

}

void dataEnvironmental(uint8_t* pData) {
  float firstTempData = 0;
  //Presure
  Serial.print("Pressure: ");
  DataPres = pData[5] << 24|pData[4] << 16|pData[3] << 8 |pData[2];
  Serial.print( DataPres/100);
  Serial.println(" mBar");
  
  //Temperature
  Serial.print("New temperature: ");
  DataTemp = pData[9] << 8 |pData[8];
  DataTemp = DataTemp/10; 
  Serial.print(DataTemp);
  Serial.println(" C"); 
  tempEvtCnt += 1;  
  Serial.print("Count temp=");
  Serial.println(tempEvtCnt);
  DataTempAvg = DataTempAvg+DataTemp;
  if (tempEvtCnt==1)
     firstTempData = DataTemp;
  if (tempEvtCnt==10){
    DataTempAvg = DataTempAvg/10;
  }
  Serial.print("Avg temperature ");
  Serial.println(DataTempAvg);
  
  //Humidity
  Serial.print("Humidity: ");
  DataHum = pData[7] << 8 |pData[6];
  Serial.print( DataHum/10);
  Serial.println(" %");   
  }

void dataMotion(uint8_t* pData) {
  //MEMS fusion data
  DataQi = pData[3] << 8 |pData[2];
  DataQj = pData[5] << 8 |pData[4];
  DataQk = pData[7] << 8 |pData[6];      

  DataQiDec = float(DataQi)/10000;
  DataQjDec = float(DataQj)/10000;
  DataQkDec = float(DataQk)/10000;

  //DataQsDec =  sqrt(1 - (DataQiDec*DataQiDec)+(DataQjDec*DataQjDec)+(DataQkDec*DataQkDec));
  DataQsDec =  sqrt(1 - (sq(DataQiDec)+ sq(DataQjDec)+ sq(DataQkDec)));
  
  Serial.print("Qi: ");
  Serial.println(DataQiDec,4);
  Serial.print("Qj: ");
  Serial.println(DataQjDec, 4);
  Serial.print("Qk: ");
  Serial.println(DataQkDec, 4);
  Serial.print("Qs: ");
  Serial.println(DataQsDec, 4);
}

void dataGauge(uint8_t* pData) {
  //Gauge data for battery
  DataVbatt = pData[5] << 8 |pData[4]; 
  Serial.print("Battery voltage: ");
  Serial.println(DataVbatt/1000,4);
}

class MyClientCallback : public BLEClientCallbacks {
  void onConnect(BLEClient* pclient) {
  }

  void onDisconnect(BLEClient* pclient) {
    connected = false;
    Serial.println("onDisconnect");
  }
};

bool connectToServer() {
    Serial.print("Forming a connection to ");
    Serial.println(myDevice->getAddress().toString().c_str());
    
    BLEClient*  pClient  = BLEDevice::createClient();
    Serial.println(" - Created client");

    pClient->setClientCallbacks(new MyClientCallback());

    // Connect to the remove BLE Server.
    pClient->connect(myDevice);  // if you pass BLEAdvertisedDevice instead of address, it will be recognized type of peer device address (public or private)
    Serial.println(" - Connected to server");

    // Obtain a reference to the service we are after in the remote BLE server.
    BLERemoteService* pRemoteService = pClient->getService(serviceUUID);
    if (pRemoteService == nullptr) {
      Serial.print("Failed to find our service UUID: ");
      //Serial.println(serviceUUID.toString().c_str());
      pClient->disconnect();
      return false;
    }
    Serial.println(" - Found our service");

    Serial.print("Searching first char UUID Environmental");
    //Serial.println(char1UUID.toString().c_str());
    // Obtain a reference to the characteristic in the service of the remote BLE server.
    p1RemoteCharacteristic = pRemoteService->getCharacteristic(char1UUID);
    if (p1RemoteCharacteristic == nullptr) {
      Serial.print("Failed to find our characteristic UUID: ");
      //Serial.println(char1UUID.toString().c_str());
      pClient->disconnect();
      return false;
    }
    Serial.println(" - Found our first characteristic");
    //Serial.println(char1UUID.toString().c_str());
    
    // Read the value of the characteristic.
//    if(p1RemoteCharacteristic->canRead()) {
//      std::string value = p1RemoteCharacteristic->readValue();
//      Serial.print("The characteristic value was: ");
//      Serial.println(value.c_str());
//    }

    Serial.print("Searching second char UUID Motion");
    //Serial.println(char2UUID.toString().c_str());
    // Obtain a reference to the characteristic in the service of the remote BLE server.
    p2RemoteCharacteristic = pRemoteService->getCharacteristic(char2UUID);
    if (p2RemoteCharacteristic == nullptr) {
      Serial.print("Failed to find our characteristic UUID: ");
      //Serial.println(char2UUID.toString().c_str());
      pClient->disconnect();
      return false;
    }
    Serial.println(" - Found our second characteristic");
    //Serial.println(char2UUID.toString().c_str());
    
    Serial.print("Searching third char UUID Gauge");
    //Serial.println(char2UUID.toString().c_str());
    // Obtain a reference to the characteristic in the service of the remote BLE server.
    p3RemoteCharacteristic = pRemoteService->getCharacteristic(char3UUID);
    if (p3RemoteCharacteristic == nullptr) {
      Serial.print("Failed to find our characteristic UUID: ");
      //Serial.println(char3UUID.toString().c_str());
      pClient->disconnect();
      return false;
    }
    Serial.println(" - Found our third characteristic");
    //Serial.println(char3UUID.toString().c_str());    
    connected = true;
    return true;
}

void getCharacteristics() {
     if(p1RemoteCharacteristic->canNotify())
      p1RemoteCharacteristic->registerForNotify(NotifyCallback);

    //Serial.println("Unregister for notifying!");
    delay(1000);
    p1RemoteCharacteristic->registerForNotify(NULL);
         
    if(p2RemoteCharacteristic->canNotify())
      p2RemoteCharacteristic->registerForNotify(NotifyCallback);

    //Serial.println("Unregister for notifying!");
    delay(100);
    p2RemoteCharacteristic->registerForNotify(NULL);

    if(p3RemoteCharacteristic->canNotify())
      p3RemoteCharacteristic->registerForNotify(NotifyCallback);

    //Serial.println("Unregister for notifying!");
    delay(100);
    p3RemoteCharacteristic->registerForNotify(NULL);
  }
  
/**
 * Scan for BLE servers and find the first one that advertises the service we are looking for.
 */
class MyAdvertisedDeviceCallbacks: public BLEAdvertisedDeviceCallbacks {
 /**
   * Called for each advertising BLE server.
   */
  void onResult(BLEAdvertisedDevice advertisedDevice) {
    Serial.print("BLE Advertised Device found: ");
    Serial.println(advertisedDevice.toString().c_str());

    // We have found a device, let us now see if it contains the service we are looking for.
    if (advertisedDevice.getAddress().toString()==  My_BLE_Address ) {
      //Serial.println("Found DEVICE!");
      BLEDevice::getScan()->stop();
      myDevice = new BLEAdvertisedDevice(advertisedDevice);
      doConnect = true;
      doScan = true;

    } // Found our server
  } // onResult
 // }
}; // MyAdvertisedDeviceCallbacks


void fanSpeed(byte speedLevel) {
    switch (speedLevel){
      case 1:
        Serial.println("Turning ON low speed and OFF high speed");
        digitalWrite(RELAY1,HIGH);
        digitalWrite(RELAY2,LOW);
        break;
      case 3:
        Serial.println("Turning OFF low speed and ON high speed");
        digitalWrite(RELAY1,LOW);
        digitalWrite(RELAY2,HIGH);
        break;
      default:
        Serial.println("Just turn OFF everything");
        digitalWrite(RELAY2,LOW);
        digitalWrite(RELAY1,LOW);        
    } 
  }


void hotFlashEvent(bool enable){
  if(enable) {
    digitalWrite(RELAY1,LOW);
    digitalWrite(RELAY2, HIGH);
    delay(5000); //Turn on air a little bit
    digitalWrite(RELAY1, LOW);
    digitalWrite(RELAY2, LOW);
    }
   //automodeFlag = false;
  }

    
void setup() {
  pinMode(RELAY1, OUTPUT);
  pinMode(RELAY2, OUTPUT);
  digitalWrite(RELAY1, LOW);
  digitalWrite(RELAY2, LOW);
  // Debug console
  Serial.begin(115200);
  
  Serial.println();
  Serial.print("Connecting to ");
  Serial.println(WLAN_SSID);

  WiFi.begin(WLAN_SSID, WLAN_PASS);
  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
    Serial.print(".");
  }
  Serial.println();

  Serial.println("WiFi connected");
  Serial.println("IP address: "); 
  Serial.println(WiFi.localIP());
 

  // Setup MQTT subscription for onoff feed.
  mqtt.subscribe(&Speed1);
  mqtt.subscribe(&Speed2);
  mqtt.subscribe(&TurnOff);
  mqtt.subscribe(&AutoMode);
  
  BLEDevice::init("");
  // Retrieve a Scanner and set the callback we want to use to be informed when we
  // have detected a new device.  Specify that we want active scanning and start the
  // scan to run for 5 seconds.
  BLEScan* pBLEScan = BLEDevice::getScan();
  pBLEScan->setAdvertisedDeviceCallbacks(new MyAdvertisedDeviceCallbacks());
  pBLEScan->setInterval(1349);
  pBLEScan->setWindow(449);
  pBLEScan->setActiveScan(true);
  pBLEScan->start(5, false);
}

void loop() {
  // If the flag "doConnect" is true then we have scanned for and found the desired
  // BLE Server with which we wish to connect.  Now we connect to it.  Once we are 
  // connected we set the connected flag to be true.
  if (doConnect == true) {
    if (connectToServer()) {
      Serial.println("We are now connected to the BLE Server.");
    } else {
      Serial.println("We have failed to connect to the server; there is nothin more we will do.");
    }
    doConnect = false;
  }

  // If we are connected to a peer BLE Server, update the characteristic each time we are reached
  // with the current time since boot.
  if (connected) {
    //Get callbacks
    getCharacteristics();
  }else if(doScan){
    BLEDevice::getScan()->start(0);  // this is just eample to start scan after disconnect, most likely there is better way to do it in arduino
  }

  // Ensure the connection to the MQTT server is alive (this will make the first
  // connection and automatically reconnect when disconnected).  See the MQTT_connect
  // function definition further below.
  MQTT_connect(); 
  // this is our 'wait for incoming subscription packets' busy subloop
  // try to spend your time here

  Adafruit_MQTT_Subscribe *subscription;
  while ((subscription = mqtt.readSubscription(20000))) {
    if (subscription == &Speed1) {
      //automodeFlag = false; //Disable auto mode
      Serial.print(F("Low speed got: "));
      Serial.println((char *)Speed1.lastread);
      speed1_State = atoi((char *)Speed1.lastread);
      if (speed1_State == 1 && (speed2_State == 1 || speed2_State == 0) ){
         fanSpeed(1);
        }
      else if (speed1_State == 0) 
         fanSpeed(2);
     }
     if (subscription == &Speed2) {
      //automodeFlag = false;//Disable auto mode
      Serial.print(F("High speed got: "));
      Serial.println((char *)Speed2.lastread);
      speed2_State = atoi((char *)Speed2.lastread);
      if (speed2_State == 1 && (speed1_State == 1 || speed1_State == 0) ){
         fanSpeed(3);
        }
      else if (speed2_State == 0) 
         fanSpeed(4);
     }
     if (subscription == &AutoMode) {
      Serial.print(F("Auto mode got: "));
      Serial.println((char *)AutoMode.lastread);
      autoMode_State = atoi((char *)AutoMode.lastread);
      if (autoMode_State == 1){
         //fanSpeed(3);
         automodeFlag = true;
         if (DataTempAvg>=32)
          fanSpeed(1); //turn on air.
         else{
            fanSpeed(0);//Turn off air.
         }
        }
      else if (autoMode_State == 0) 
         automodeFlag = false;
     }
     if (subscription == &TurnOff) {
      Serial.print(F("Turn off got: "));
      Serial.println((char *)TurnOff.lastread);
      int turnOff_State = atoi((char *)TurnOff.lastread);
      if (turnOff_State == 1){
         fanSpeed(5);
         automodeFlag = false;
        }
      else if (turnOff_State == 0) 
         fanSpeed(6);
     } 
  }

  // Now we can publish stuff!
 if (connected) {
  if (! Temperature.publish(DataTempAvg)) {
    Serial.println(F("Failed"));
  } else {
    Serial.println(F("Posted temperature!"));
    if (DataTempAvg > (DataTempAvgLast+2)) {
      flashFlag = true;
      Serial.println("Hot flash detected");
      hotFlashEvent( automodeFlag);
    }
    else
      flashFlag= false;
    DataTempAvgLast = DataTemp;
    tempEvtCnt = 0;
    DataTempAvg = 0;
  }
  if (! HotFlash.publish(flashFlag)) {
    Serial.println(F("Failed"));
    flashFlag = false;
  } else {
    Serial.println(F("Posted hot flash flag!"));
    flashFlag = false;
  }
  if (! Humidity.publish(DataHum/10)) {
    Serial.println(F("Failed"));
  } else {
    Serial.println(F("Posted humidity!"));
  } 
  if (! Qi.publish(DataQiDec)) {
    Serial.println(F("Failed"));
  } else {
    Serial.println(F("Posted qi motion sensor!"));
  } 
  if (! Qj.publish(DataQjDec)) {
    Serial.println(F("Failed"));
  } else {
    Serial.println(F("Posted qj motion sensor!"));
  } 
  if (! Qk.publish(DataQkDec)) {
    Serial.println(F("Failed"));
  } else {
    Serial.println(F("Posted qk motion sensor!"));
  } 
  if (! Qs.publish(DataQsDec)) {
    Serial.println(F("Failed"));
  } else {
    Serial.println(F("Posted qs motion sensor!"));
  }
  if (! Pressure.publish(DataPres/100)) {
    Serial.println(F("Failed"));
  } else {
    Serial.println(F("Posted pressure!"));
  }
  if (! Battery.publish(DataVbatt/1000)) {
    Serial.println(F("Failed"));
  } else {
    Serial.println(F("Posted battery mV!"));
  }    
  delay(500);
  // ping the server to keep the mqtt connection alive
  // NOT required if you are publishing once every KEEPALIVE seconds
  /*
    if(! mqtt.ping()) {
    mqtt.disconnect();
    }
  */
 }
}

// Function to connect and reconnect as necessary to the MQTT server.
// Should be called in the loop function and it will take care if connecting.
void MQTT_connect() {
  int8_t ret;

  // Stop if already connected.
  if (mqtt.connected()) {
    return;
  }

  Serial.print("Connecting to MQTT... ");

  uint8_t retries = 3;
   
  while ((ret = mqtt.connect()) != 0) { // connect will return 0 for connected
    Serial.println(mqtt.connectErrorString(ret));
    Serial.println("Retrying MQTT connection in 5 seconds...");
    mqtt.disconnect();
    delay(5000);  // wait 5 seconds
    retries--;
    if (retries == 0) {
      // basically die and wait for WDT to reset me
      while (1);
    }
  }
  Serial.println("MQTT Connected!");
}