#ifndef BLUETOOTH_h
#define BLUETOOTH_h

#include <NimBLEDevice.h>
#include "logger.h"



static bool doConnect = false;
static uint32_t scanTime = 0;

static NimBLEAdvertisedDevice* advDevice;

 static void scanEndedCB(NimBLEScanResults results){
    logger.log(5, "Scan Ended" );    
  } // END SCANENDEDCB




class ClientCallbacks : public NimBLEClientCallbacks {
    void onConnect(NimBLEClient* pClient) {
      logger.log(5, "Connected");
    };

    void onDisconnect(NimBLEClient* pClient) {
        logger.log(5, "Disconnected - Starting Scan" );
        NimBLEDevice::getScan()->start(scanTime, scanEndedCB);
    };
    bool onConnParamsUpdateRequest(NimBLEClient* pClient, const ble_gap_upd_params* params) {
        if(params->itvl_min < 24) { /** 1.25ms units */
            return false;
        } else if(params->itvl_max > 40) { /** 1.25ms units */
            return false;
        } else if(params->latency > 2) { /** Number of intervals allowed to skip */
            return false;
        } else if(params->supervision_timeout > 100) { /** 10ms units */
            return false;
        }

        return true;
    };

    uint32_t onPassKeyRequest(){
        return 123456;
    };

    bool onConfirmPIN(uint32_t pass_key){
        return true;
    };
    void onAuthenticationComplete(ble_gap_conn_desc* desc){

        if(!desc->sec_state.encrypted) {
            logger.log(5, "Encrypt connection failed.");
            NimBLEDevice::getClientByID(desc->conn_handle)->disconnect();
            return;
        }
    };
};// END CLIENTCALLBACK

class AdvertisedDeviceCallbacks: public NimBLEAdvertisedDeviceCallbacks {
  private:
     LOGGER logger;


    void onResult(NimBLEAdvertisedDevice* advertisedDevice) {

        logger.log(5, advertisedDevice->toString().c_str() );    
        if(advertisedDevice->isAdvertisingService(NimBLEUUID::fromString("0x180F") ))
        {
            logger.log(5, "Service Found");
            NimBLEDevice::getScan()->stop();
            advDevice = advertisedDevice;
            doConnect = true;
        }
    };
}; // END ADVERTISED CALLBACK







class BLUETOOTH
{
  private:

   ClientCallbacks clientCB;



  static void notifyCB(NimBLERemoteCharacteristic* pRemoteCharacteristic, uint8_t* pData, size_t length, bool isNotify){
    std::string str = (isNotify == true) ? "Notification" : "Indication";
    str += " from ";
    /** NimBLEAddress and NimBLEUUID have std::string operators */
    str += std::string(pRemoteCharacteristic->getRemoteService()->getClient()->getPeerAddress());
    str += ": Service = " + std::string(pRemoteCharacteristic->getRemoteService()->getUUID());
    str += ", Characteristic = " + std::string(pRemoteCharacteristic->getUUID());
    str += ", Value = " + std::string((char*)pData, length);
    logger.log(5,"NOTIFY CALLBACK ");
    logger.log(5,str.c_str());
   } // END NOTIFYCB





bool connectToServer() {
    logger.log(5,"Connected To Server");
    NimBLEClient* pClient = nullptr;
    if(NimBLEDevice::getClientListSize()) {
        pClient = NimBLEDevice::getClientByPeerAddress(advDevice->getAddress());

        if(pClient){

            if(!pClient->connect(advDevice, false)) {
                logger.log(5,"Reconnect Failed");
                return false;
            }
            logger.log(5,"Reconnected Client");
        }
        else {

            pClient = NimBLEDevice::getDisconnectedClient();
        }
    }

    if(!pClient) {
        if(NimBLEDevice::getClientListSize() >= NIMBLE_MAX_CONNECTIONS) {
           logger.log(5,"Max clients reached");
            return false;
        }

        pClient = NimBLEDevice::createClient();
        pClient->setClientCallbacks(&clientCB, false);

        pClient->setConnectionParams(12,12,0,51);

        /** Set how long we are willing to wait for the connection to complete (seconds), default is 30. */
        pClient->setConnectTimeout(5);
        logger.log(5,advDevice->toString().c_str());
        if (!pClient->connect(advDevice)) {

            NimBLEDevice::deleteClient(pClient);
            logger.log(5,"Failed to connect, deleted client");
            Serial.println("Failed to connect, deleted client");
            return false;
        }

    }

    if(!pClient->isConnected()) {
        if (!pClient->connect(advDevice)) {
        logger.log(5,"Failed to connect");

            return false;
        }
    }

    logger.log(5,"Connected to");
    logger.log(5,pClient->getPeerAddress().toString().c_str());

    /** Now we can read/write/subscribe the charateristics of the services we are interested in */
    NimBLERemoteService* pSvc = nullptr;
    NimBLERemoteCharacteristic* pChr = nullptr;
    NimBLERemoteDescriptor* pDsc = nullptr;

    pSvc = pClient->getService(NimBLEUUID::fromString("0x181D"));
    if(pSvc) {     /** make sure it's not null */
              logger.log(5,"Service found");


        pChr = pSvc->getCharacteristic(NimBLEUUID::fromString("0x2A9D"));

        if(pChr) {     /** make sure it's not null */
            logger.log(5,"Charactheristic found");
          
            if(pChr->canRead()) {
                           logger.log(5,"Can read");


                Serial.print(pChr->getUUID().toString().c_str());
                Serial.print(" Value: ");
                Serial.println(pChr->readValue().c_str());
            }


            /** registerForNotify() has been deprecated and replaced with subscribe() / unsubscribe().
             *  Subscribe parameter defaults are: notifications=true, notifyCallback=nullptr, response=false.
             *  Unsubscribe parameter defaults are: response=false.
             */
            if(pChr->canNotify()) {
                  logger.log(5,"Can Notify");

                //if(!pChr->registerForNotify(notifyCB)) {
                if(!pChr->subscribe(true, notifyCB)) {
                    /** Disconnect if subscribe failed */
                    pClient->disconnect();
                    return false;
                }
            }
            else if(pChr->canIndicate()) {
                     logger.log(5,"Can indicate");


                /** Send false as first argument to subscribe to indications instead of notifications */
                //if(!pChr->registerForNotify(notifyCB, false)) {
                if(!pChr->subscribe(false, notifyCB)) {
                    /** Disconnect if subscribe failed */
                    pClient->disconnect();
                    return false;
                }
            }
        }

    } else {
    logger.log(5,"Service not found");

    }
    return true;
}



  

  public:
    BLUETOOTH () {} // CONSTRUCTOR

    void setup (){
      
      NimBLEDevice::init("");
      NimBLEDevice::setSecurityAuth(/*BLE_SM_PAIR_AUTHREQ_BOND | BLE_SM_PAIR_AUTHREQ_MITM |*/ BLE_SM_PAIR_AUTHREQ_SC);
      NimBLEDevice::setPower(ESP_PWR_LVL_P9); /** +9db */
      NimBLEScan* pScan = NimBLEDevice::getScan();
      pScan->setAdvertisedDeviceCallbacks(new AdvertisedDeviceCallbacks());
      pScan->setInterval(45);
      pScan->setWindow(15);
      pScan->setActiveScan(true);
      pScan->start(scanTime, scanEndedCB);
    } // setup()
    
    void loop (){
  if(doConnect){
      Serial.println("doCOnnect=true");
      if(connectToServer()) {
          logger.log(1, "Connected to BLE");

          Serial.println("Success! we should now be getting notifications, scanning for more!");
      } else {
         Serial.println("Failed to connect, starting scan");
      }
            NimBLEDevice::getScan()->start(scanTime,scanEndedCB);

  }
            doConnect = false;

    
    } // loop() 
};
#endif