#include <ArduinoJson.h>

/**");//
 * A BLE client example that is rich in capabilities.
 * There is a lot new capabilities implemented.
 * author unknown
 * updated by chegewara
 */
//-----Setting up Bluetooth
//--Library
#include "BLEDevice.h"

// The remote service we wish to connect to.
static BLEUUID serviceUUID("49535343-FE7D-4AE5-8FA9-9FAFD205E455");
// The characteristic of the remote service we are interested in.
static BLEUUID    charUUID("49535343-1e4d-4bd9-ba61-23c647249616");
//The address of the device (not needed here thanks to the scan
String My_BLE_Address = "04:91:62:9c:09:07";

//--Constants
static BLEAddress *Server_BLE_Address;
String Scaned_BLE_Address;

static boolean doConnect = false;
static boolean connected = false;
static boolean doScan = true;
static BLERemoteCharacteristic* pRemoteCharacteristic;
static BLEAdvertisedDevice* myDevice;

static BLEUUID *Server_BLE_UUID;
String Scaned_BLE_UUID;


//-----Setting up WIFI
//library
#include <WiFi.h> //For ESP32
#include <PubSubClient.h>

// SETUP WIFI CREDENTIALS
//Casa
const char* ssid = "LowiFE9F";
const char* password = "BMTW4WUDXJVS6U";
const char* mqtt_server = "192.168.0.27";
//FabLab
//const char* ssid = "Iaac-Wifi";
//const char* password = "enteriaac2013";
//const char* mqtt_server = "172.16.22.235";

const char* mqtt_user = "admin";
const char* mqtt_pass = "AL300188BCN";
bool bSend = 0;  



WiFiClient espClient;
PubSubClient client(espClient);


//Setting up LED pins
#define LEDBLEIN 18 //IO18
#define LEDWIFIOUT 13 //IO13
        
static void notifyCallback(
  BLERemoteCharacteristic* pBLERemoteCharacteristic,
  uint8_t* pData,
  size_t length,
  bool isNotify) {
    Serial.print("Notify callback for characteristic ");
    Serial.print(pBLERemoteCharacteristic->getUUID().toString().c_str());
    Serial.print(" of data length ");
    Serial.println(length);
    Serial.print("data: ");
    DynamicJsonDocument doc(1024);
    doc["sensor"] = "TH_1";
    uint8_t delim = ';';
    String temp;
    for (int i = 0 ; (uint8_t)pData[i] != delim ; i++){

       temp += (char)pData[i];
      int j = i;
    }
    doc["temperature"] = temp.toFloat();// temp; //atof(temp);
    bool hum = 0;
    //size_t hum_start=0;
    temp = "";
    for(size_t i = 0; i < length ; i++){//Strictly inferior to lenght because last element is an empty string
      if (hum){
        temp += (char)pData[i];
      }
      if ((uint8_t)pData[i] == delim){
        hum = 1;
        //hum_start = i+1;
      }
    }
    doc["humidity"] = temp.toFloat();// temp; //atof(temp);
    if (doc["humidity"] != -1){// Check if data is valid to be sent to MQTT
      bSend = 1;
    }
    serializeJson(doc, Serial);
    Serial.println("");
    char json_string[1024];
    serializeJson(doc, json_string);
    //json_string.toCharArray();
    
//    serializeJsonPretty(doc, Serial);
      digitalWrite(LEDBLEIN,HIGH);    
    if (bSend == 1){ //Message is complete we send it.
      //We are now connected to the BLE and send info to wifi
        // Send value as characters
        Serial.print("Publish message: ESP of Adrien : ");
        // SET THE TOPIC TO PUBLISH HERE
        //client.publish("autonomousboard",pData,length);
        client.publish("autonomousboard",json_string);
        Serial.println("");
        digitalWrite(LEDWIFIOUT,HIGH);  
        float LEDtime = millis();
        while(millis()-LEDtime < 1000){//Light LED to confirm data sent
        }
        digitalWrite(LEDBLEIN,LOW);
        digitalWrite(LEDWIFIOUT,LOW);
        bSend = 0;

    } else {
        float LEDtime = millis();
        while(millis()-LEDtime < 200){//Light LED to confirm data sent
        }
       digitalWrite(LEDBLEIN,LOW);     
    }
}

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

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

bool connectToServer() {
    Serial.println(sizeof(myDevice->getAddress().toString().c_str()));
    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");


    // Obtain a reference to the characteristic in the service of the remote BLE server.
    pRemoteCharacteristic = pRemoteService->getCharacteristic(charUUID);
    if (pRemoteCharacteristic == nullptr) {
      Serial.print("Failed to find our characteristic UUID: ");
      Serial.println(charUUID.toString().c_str());
      pClient->disconnect();
      return false;
    }
    Serial.println(" - Found our characteristic");

    // Read the value of the characteristic.
    if(pRemoteCharacteristic->canRead()) {
      std::string value = pRemoteCharacteristic->readValue();
      Serial.print("The characteristic value was: ");
      Serial.println(value.c_str());
    }

    if(pRemoteCharacteristic->canNotify())
      pRemoteCharacteristic->registerForNotify(notifyCallback);

    connected = true;
    return true;
}
/**
 * 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) {
//            int imax = advertisedDevice.getServiceUUIDCount();
    Serial.print("BLE Advertised Device found: ");
    Serial.println(advertisedDevice.toString().c_str());
       Server_BLE_Address = new BLEAddress(advertisedDevice.getAddress());
       Scaned_BLE_Address = Server_BLE_Address->toString().c_str();
       Serial.println(Scaned_BLE_Address);
    if(Scaned_BLE_Address == My_BLE_Address){
 
    doConnect = true;
    BLEDevice::getScan()->stop();
    myDevice = new BLEAdvertisedDevice(advertisedDevice);
    }

    // We have found a device, let us now see if it contains the service we are looking for.
    if (advertisedDevice.haveServiceUUID() && advertisedDevice.isAdvertisingService(serviceUUID)) {
      Serial.println("MATCHING SERVICE !!!!!");
      BLEDevice::getScan()->stop();
      myDevice = new BLEAdvertisedDevice(advertisedDevice);
      doConnect = true;
      doScan = true;

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


void setup() {
  Serial.begin(115200);
  //LED Setup
  pinMode(LEDBLEIN,OUTPUT);
  pinMode(LEDWIFIOUT,OUTPUT);
  float LEDtime = millis();
  while(millis()-LEDtime < 1000){//Light LED to confirm data reception
    digitalWrite(LEDBLEIN,HIGH);  
    digitalWrite(LEDWIFIOUT,HIGH);          
  }
    digitalWrite(LEDBLEIN,LOW);  
    digitalWrite(LEDWIFIOUT,LOW);
  //---WIFI Setup
    Serial.println("Starting Arduino WIFI Client application");
    setup_wifi();
    client.setServer(mqtt_server, 1883);
    client.setCallback(callback);
  
  //---BLE Set up
    Serial.println("Starting Arduino BLE Client application...");
    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(2, false);
} // End of setup.


// This is the Arduino main loop function.
void loop() {
  Serial.println("Start 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) {
     Serial.println("Doconnect");
    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;
  }

  //BLE connection check
  // 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) {


  }else if(doScan){
    BLEDevice::getScan()->start(0);  // this is just example to start scan after disconnect, most likely there is better way to do it in arduino
  }

  //Wifi connection check
   if (!client.connected()) reconnect();
    client.loop();
  
  //delay(1000); // Delay a second between loops.
  float LOOPtime = millis();
  while(millis()-LOOPtime < 1000){//Wait a bit before repeating loop
  }
} // End of loop


//WIFI function 
void callback(char* topic, byte* payload, unsigned int length) {
  
  Serial.print("Message arrived [");
  Serial.print(topic);
  Serial.print("] ");
  for (int i = 0; i < length; i++) {
    Serial.print((char)payload[i]);
  }
  Serial.println();

  String strPayload = String((char*)payload);

  // Serial.println(strPayload.toFloat());
  // Serial.println(strPayload.toInt());

  // USE RECEIVED DATA HERE
  //if (strPayload.toInt() > 5) digitalWrite(LED_BUILTIN, LOW);
  //else digitalWrite(LED_BUILTIN, HIGH);

}

void reconnect() {
  
  // Loop until we're reconnected
  while (!client.connected()) {
    
    Serial.print("Attempting MQTT connection...");
    // Create a random client ID
    String clientId = "ESP8266Client-";
    clientId += String(random(0xffff), HEX);
   
    // Attempt to connect
    if (client.connect(clientId.c_str(), mqtt_user, mqtt_pass)) {
      Serial.println("connected");
      
      // SET THE TOPIC TO SUBSCRIBE HERE
      client.subscribe("testing");
    
    } else {

      Serial.print("failed, rc=");
      Serial.print(client.state());
      Serial.println(" try again in 5 seconds");
      // Wait 5 seconds before retrying
      delay(5000);

    }
  }
}

void setup_wifi() {
  
  delay(10);
  // We start by connecting to a WiFi network
  Serial.println();
  Serial.print("Connecting to ");
  Serial.println(ssid);

  WiFi.begin(ssid, password);

  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
    Serial.print(".");
  }

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