#include "BLE.h"
#include "BLEUtils.h"
#include "BLEServer.h"
#include <esp_log.h>
#include
#include <sys/time.h>
#include
#include "Task.h"
#include "BLE2902.h"

#include "sdkconfig.h"

static char LOG_TAG[] = "SampleHrMon";

/* These are the org.bluetooth.service.heart_rate and org.bluetooth.service.battery_service

assigned UUIDs they are 16 bit, in 128 bit format. This is for ADV only use the
16 bit versions for the services and characteristics
*/
#define HR_SERVICE_UUID128 "0000180D-0000-1000-8000-00805F9B34FB"
#define BATT_SERVICE_UUID128 "0000180F-0000-1000-8000-00805F9B34FB"
#define HR_SERVICE_UUID (uint16_t) 0x180D
#define BATT_SERVICE_UUID (uint16_t) 0x180F
#define BATT_LEVEL_CHAR_UUID (uint16_t) 0x2A19
#define HR_MEASURE_CHAR_UUID (uint16_t) 0x2A37
#define HR_BODY_LOC_CHAR_UUID (uint16_t) 0x2A38

uint8_t serviceUuids[2][16] = {};

BLECharacteristic *pBattCharacteristic;
BLECharacteristic *pHrCharacteristic;
BLECharacteristic *pBdSnLocCharacteristic;

class MyNotifyTask: public Task {
void run(void *data) {
uint8_t value = 100;
uint8_t hrMeas[8] = {}; // for now it is empty
uint8_t hRate = 60;

  while(1) {
     delay(2000);
     ESP_LOGD(LOG_TAG, "*** NOTIFY: %d ***", value);
     pBattCharacteristic->setValue(&value, 1);
     pBattCharacteristic->notify();

     // over time our battery will get to 0%

     if (value > 0)
     {
         value--;
     }

     ESP_LOGD(LOG_TAG, "*** NOTIFY HR MEASURE %d ***", hRate);
     hrMeas[0] = 0x0; // this says we have 1 byte of measure nothing else
     hrMeas[1] = hRate;
     hRate += 5;
     pHrCharacteristic->setValue(hrMeas, 2);
     pHrCharacteristic->notify();
  }
}
};

MyNotifyTask *pMyNotifyTask;

class MyServerCallbacks: public BLEServerCallbacks {
void onConnect(BLEServer *pServer) {
pMyNotifyTask->start();
};

void onDisconnect(BLEServer *pServer) {
pMyNotifyTask->stop();
}
};

static void run() {
esp_ble_adv_data_t *pAdvData;
pMyNotifyTask = new MyNotifyTask();
pMyNotifyTask->setStackSize(8000);

BLE::initServer("MYHRMON");
BLEServer *pServer = new BLEServer();

BLEService *pBattService = pServer->createService(BLEUUID(BATT_SERVICE_UUID));

pServer->setCallbacks(new MyServerCallbacks());

pBattCharacteristic = pBattService->createCharacteristic(
BLEUUID(BATT_LEVEL_CHAR_UUID),
BLECharacteristic::PROPERTY_READ |
BLECharacteristic::PROPERTY_WRITE |
BLECharacteristic::PROPERTY_NOTIFY
);

// https://www.bluetooth.com/specifications/gatt/viewer?attributeXmlFile=org.bluetooth.descriptor.gatt.client_characteristic_configuration.xml
BLE2902 *pBLE2902 = new BLE2902();
pBLE2902->setNotifications(true);
pBattCharacteristic->addDescriptor(pBLE2902);

pBattService->start();

BLEService *pHrService = pServer->createService(BLEUUID(HR_SERVICE_UUID));

pHrCharacteristic = pHrService->createCharacteristic(
BLEUUID(HR_MEASURE_CHAR_UUID),
// BLECharacteristic::PROPERTY_READ |
// BLECharacteristic::PROPERTY_WRITE |
BLECharacteristic::PROPERTY_NOTIFY
);

// https://www.bluetooth.com/specifications/gatt/viewer?attributeXmlFile=org.bluetooth.descriptor.gatt.client_characteristic_configuration.xml
BLE2902 *pHmBLE2902 = new BLE2902();
pHmBLE2902->setNotifications(true);
pHrCharacteristic->addDescriptor(pHmBLE2902);

pBdSnLocCharacteristic = pHrService->createCharacteristic(
BLEUUID(HR_BODY_LOC_CHAR_UUID),
BLECharacteristic::PROPERTY_READ
);

uint8_t newValue = 4; // I think 4 is finger
pBdSnLocCharacteristic->setValue(&newValue, 1);

pHrService->start();

BLEAdvertising *pAdvertising = pServer->getAdvertising();
pAdvertising->setAppearance(833);

/* this is just what we advertise */

/* try to create 2 UUIDS in a row. */
BLEUUID *tempUUID = new BLEUUID(HR_SERVICE_UUID128);
memcpy((void *) serviceUuids[0], (const void *) (tempUUID->getNative()->uuid.uuid128), (size_t) 16);
tempUUID = new BLEUUID(BATT_SERVICE_UUID128);
memcpy((void *) serviceUuids[1], (const void *) (tempUUID->getNative()->uuid.uuid128), (size_t) 16);

pAdvData = pAdvertising->getAdvDataPtr();
pAdvData->p_service_uuid = &serviceUuids[0][0];
pAdvData->service_uuid_len = 32;

// pAdvertising->setServiceUUID(BLEUUID(HR_SERVICE_UUID));
pAdvertising->start();
}

void SampleNotify(void)
{
run();
} // app_main`

and here are the simple methods I added to BLEAdvertising.h

` esp_ble_adv_data_t *getAdvDataPtr(void)
{
return &m_advData;
};

    esp_ble_adv_params_t *getAdvParamsPtr(void)
    {
        return &m_advParams;
    };`