#include "BLEDevice.h"
//#include "BLEScan.h"
// Servicios y caracteristicas del sensor XSENS DOT, con UUID en formato
// hexadecimal
// 1517xxxx-4947-11E9-8646-D663BD873D93
//-----------------------------------------------------------------------
//-----------------------------------------------------------------------
// The remote service we wish to connect to.
// 0x2000
// Servicio de medición, permite iniciar y detener la medición del sensor, 
// configurar modos de carga y recibir datos de la medición.
static BLEUUID serviceUUID("15172000-4947-11E9-8646-D663BD873D93");
//-----------------------------------------------------------------------
//-----------------------------------------------------------------------
// The characteristic of the remote service we are interested in.
// Devuelve los datos de los modos de carga útil, con longitud 
// entre 0 y 20 bits. 0x2004
static BLEUUID short_payload_charUUID("15172004-4947-11E9-8646-D663BD873D93");
// Controla el modo de start/stop y carga de la medición. 0x2001
static BLEUUID control_charUUID("15172001-4947-11E9-8646-D663BD873D93");
//-----------------------------------------------------------------------
static boolean doConnect = false;
static boolean connected = false;
static boolean doScan = false;
static BLERemoteCharacteristic* control_pRemoteCharacteristic;
static BLERemoteCharacteristic* short_payload_pRemoteCharacteristic;
static BLEAdvertisedDevice* myDevice;
const uint8_t startmeasure[] = {0x01, 0x01, 0x05};
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: ");
 Serial.println((char*)pData);
}
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");
 // Obtain a reference to the characteristic in the service of the remote BLE server.
 //-----------------------------------------------------------------------
 //-----------------------------------------------------------------------
 // Controla el modo de start/stop y carga de la medición. 0x2001
 // 15172001-4947-11E9-8646-D663BD873D93
 control_pRemoteCharacteristic = pRemoteService->getCharacteristic(control_charUUID);
 if (control_pRemoteCharacteristic == nullptr) {
 Serial.print("Failed to find our characteristic UUID: ");
 Serial.println(control_charUUID.toString().c_str());
 pClient->disconnect();
 return false;
 }
 Serial.println(" - Found our characteristic Control");
 
 //-----------------------------------------------------------------------
 //-----------------------------------------------------------------------
 // Devuelve los datos de los modos de carga útil, con longitud 
 // entre 0 y 20 bits. 0x2004
 short_payload_pRemoteCharacteristic = pRemoteService->getCharacteristic(short_payload_charUUID);
 if (short_payload_pRemoteCharacteristic == nullptr) {
 Serial.print("Failed to find our characteristic UUID: ");
 Serial.println(short_payload_charUUID.toString().c_str());
 pClient->disconnect();
 return false;
 }
 Serial.println(" - Found our characteristic Short payload length");
 //Read the notify of the characteristic.
 if(short_payload_pRemoteCharacteristic->canNotify()) {
 short_payload_pRemoteCharacteristic->registerForNotify(notifyCallback);
 Serial.print("Registered notification was: ");
 }
 
 // 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());
 Parse(value.c_str());
 }
 
 if(pRemoteCharacteristic->canNotify())
 pRemoteCharacteristic->registerForNotify(notifyCallback);
 */
 connected = true;
 return true;
}
//Convert String to Decimal
void Parse(const char * Valor) //Decimal
{
 //char input = Valor;
 int x;
 char *endptr;
 x = strtol(Valor, &endptr, 16);
 Serial.println("NULL");
 Serial.println(x, HEX);
 Serial.println(x, DEC);
}
/**
 * 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) {
 if(advertisedDevice.getAddress().toString() == "d4:22:cd:00:45:8b") {
 Serial.print("BLE Advertised Device found: ");
 Serial.println(advertisedDevice.toString().c_str());
 Serial.println(advertisedDevice.haveServiceUUID());
 // We have found a device, let us now see if it contains the service we are looking for.
 //if (advertisedDevice.haveServiceUUID() && advertisedDevice.isAdvertisingService(serviceUUID)) {
 BLEDevice::getScan()->stop();
 myDevice = new BLEAdvertisedDevice(advertisedDevice);
 doConnect = true;
 doScan = true;
 //} // Found our server
 } //Pregunta de dirección MAC
 } // onResult
}; // MyAdvertisedDeviceCallbacks
void setup() {
 Serial.begin(115200);
 Serial.println("Starting Arduino BLE Client application...");
 BLEDevice::init("");
 //pServerAddress = new BLEAddress("d4:22:cd:00:45:8b");
 //BLEAddress mac = new BLEAddress("");
 // 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);
} // End of setup.
// This is the Arduino main loop function.
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) {
 Serial.println("Connected");
 /*
 String newValue = "Time since boot: " + String(millis()/1000);
 Serial.println("Setting new characteristic value to \"" + newValue + "\"");
 
 // Set the characteristic's value to be the array of bytes that is actually a string.
 control_pRemoteCharacteristic->writeValue(newValue.c_str(), newValue.length());
 */
 }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
 }
 
 delay(1000); // Delay a second between loops.
} // End of loop