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

static NimBLEClient* _pClient = nullptr;
static ChronoBLE::ConnectionState currentState;
static const NimBLEAdvertisedDevice* advDevice = nullptr;
static uint32_t scanTimeMs = 5000;
static NimBLEUUID deviceUUID("0000180a-0000-1000-8000-00805f9b34fb");
static NimBLEUUID serviceUUID("00001623-88EC-688C-644B-3FA706C0BB75");
static NimBLEUUID speedCharacteristicUUID("00001624-88EC-688C-644B-3FA706C0BB75");
static NimBLEUUID profileSettingLowUUID("00001626-88EC-688C-644B-3FA706C0BB75");
static NimBLEUUID profileSettingHighUUID("00001628-88EC-688C-644B-3FA706C0BB75");
static const char * charUUIDs[] = {
  "00001624-88EC-688C-644B-3FA706C0BB75",    //notify for speed value?
  "00001625-88EC-688C-644B-3FA706C0BB75",    //00
  "00001626-88EC-688C-644B-3FA706C0BB75",    //power setting low?
  "00001627-88EC-688C-644B-3FA706C0BB75",    //battery level
  "00001628-88EC-688C-644B-3FA706C0BB75",    // power setting high?
  "00001629-88EC-688C-644B-3FA706C0BB75",    //00
  "0000162A-88EC-688C-644B-3FA706C0BB75",
  "00002902-0000-1000-8000-00805f9b34fb"
};

void (*speedCallback)(int speed);

//specific to FX chrono settings
uint8_t profile_bytes[2][5] = {{0x32, 0x32, 0x32, 0x32, 0x64},{0x17, 0x1E, 0x2D, 0x43, 0x5A}};


//connection callback
class ClientCallbacks : public NimBLEClientCallbacks {
    void onConnect(NimBLEClient* pClient) override { Serial.printf("Connected\n"); }

    void onDisconnect(NimBLEClient* pClient, int reason) override {
        Serial.printf("%s Disconnected, reason = %d\n", pClient->getPeerAddress().toString().c_str(), reason);
        //NimBLEDevice::getScan()->start(scanTimeMs, false, true);
        currentState = ChronoBLE::IDLE;
    }
} clientCallbacks;

//device scan callback
class ScanCallbacks : public NimBLEScanCallbacks {
    void onResult(const NimBLEAdvertisedDevice* advertisedDevice) override {
        //Serial.printf("Advertised Device found: %s\n", advertisedDevice->toString().c_str());
        if (advertisedDevice->isAdvertisingService(deviceUUID)) {
            //Serial.printf("Found device!!!\n");
            /** stop scan before connecting */
            NimBLEDevice::getScan()->stop();
            /** Save the device reference in a global for the client to use*/
            advDevice = advertisedDevice;
        }
    }

    /** Callback to process the results of the completed scan or restart it */
    void onScanEnd(const NimBLEScanResults& results, int reason) override {
        //Serial.printf("Scan Ended, reason: %d, device count: %d; Restarting scan\n", reason, results.getCount());
        NimBLEDevice::getScan()->start(scanTimeMs, false, true);
    }
} scanCallbacks;

//notification callback
void notifyCB(NimBLERemoteCharacteristic* pRemoteCharacteristic, uint8_t* pData, size_t length, bool isNotify) {
    uint16_t speed;
    char sbuffer[256];
    std::string str  = (isNotify == true) ? "Notification" : "Indication";
    str             += " from ";
    str             += pRemoteCharacteristic->getClient()->getPeerAddress().toString();
    str             += ": Service = " + pRemoteCharacteristic->getRemoteService()->getUUID().toString();
    str             += ", Characteristic = " + pRemoteCharacteristic->getUUID().toString();
    str             += ", Value = " + std::string((char*)pData, length);
   // Serial.printf("%s\n", str.c_str());
   // Serial.printf("%d\n",pData);

    //if(length>0){
        speed = ((char*)pData)[0];
        speed <<= 8;
        speed |= ((char*)pData)[1];

    if (speed>0){
            float energy;
            float fspeed = speed;
            /* Draw the speed string */
            //if(units == UNITS_IMPERIAL) {
            fspeed *= 0.0475111859;
            //sprintf (sbuffer, "%d FPS", int(fspeed));
            Serial.printf("%d FPS\n", int(fspeed));
            //} else {
            //fspeed *= 0.014481409;
            //sprintf (sbuffer, "%d M/S", int(fspeed));
            //}
        //}
        if (speedCallback!= nullptr){
            speedCallback(fspeed);
        }
    }

}


// Constructor implementation
ChronoBLE::ChronoBLE() {
    _setState(IDLE);
}
void ChronoBLE::init() {
    NimBLEDevice::init("FX-Chrono-Client");
    NimBLEDevice::setPower(3); /** 3dbm */
    NimBLEScan* pScan = NimBLEDevice::getScan();
    pScan->setScanCallbacks(&scanCallbacks, false);
    pScan->setInterval(100);
    pScan->setWindow(100);
    pScan->setActiveScan(true);
}
void ChronoBLE::_setState(ConnectionState state){
    currentState=state;
}
void ChronoBLE::setSpeedCallback(void (*pFunc)(int speed)){speedCallback=pFunc;}
bool ChronoBLE::isIdle(){if (currentState==IDLE){return true;}return false;}
bool ChronoBLE::isConnected(){
    if (currentState==CONNECTED){
        if (_pClient!= nullptr){
            if (!_pClient->isConnected()){
                _disconnectDevice();
                return false;
            }
            return true;
        }
        _disconnectDevice();
    }
    return false;
}
bool ChronoBLE::foundDevice(){if (advDevice!=nullptr){return true;}return false;}
void ChronoBLE::startScan(){
    Serial.println("starting scan!!!");
    _setState(SCANNING);
    NimBLEDevice::getScan()->start(scanTimeMs, false, true);
}
void ChronoBLE::_disconnectDevice(){
    _setState(IDLE);
    advDevice = nullptr;
}
bool ChronoBLE::connectToDevice() {
    _setState(CONNECTING);

    /** Check if we have a client we should reuse first **/
    if (NimBLEDevice::getCreatedClientCount()) {
        /**
         *  Special case when we already know this device, we send false as the
         *  second argument in connect() to prevent refreshing the service database.
         *  This saves considerable time and power.
         */
        _pClient = NimBLEDevice::getClientByPeerAddress(advDevice->getAddress());
        if (_pClient) {
            if (!_pClient->connect(advDevice, false)) {
                _disconnectDevice();
                return false;
            }
        } else {
            /**
             *  We don't already have a client that knows this device,
             *  check for a client that is disconnected that we can use.
             */
            _pClient = NimBLEDevice::getDisconnectedClient();
        }
    }

    /** No client to reuse? Create a new one. */
    if (!_pClient) {
        if (NimBLEDevice::getCreatedClientCount() >= NIMBLE_MAX_CONNECTIONS) {
            Serial.printf("Max clients reached - no more connections available\n");
            _disconnectDevice();
            return false;
        }

        _pClient = NimBLEDevice::createClient();

        _pClient->setClientCallbacks(&clientCallbacks, false);
        /**
         *  Set initial connection parameters:
         *  These settings are safe for 3 clients to connect reliably, can go faster if you have less
         *  connections. Timeout should be a multiple of the interval, minimum is 100ms.
         *  Min interval: 12 * 1.25ms = 15, Max interval: 12 * 1.25ms = 15, 0 latency, 150 * 10ms = 1500ms timeout
         */
        _pClient->setConnectionParams(12, 12, 0, 150);

        /** Set how long we are willing to wait for the connection to complete (milliseconds), default is 30000. */
        _pClient->setConnectTimeout(5 * 1000);

        if (!_pClient->connect(advDevice)) {
            /** Created a client but failed to connect, don't need to keep it as it has no data */
            NimBLEDevice::deleteClient(_pClient);
            _disconnectDevice();
            return false;
        }
    }

    if (!_pClient->isConnected()) {
        if (!_pClient->connect(advDevice)) {
            _disconnectDevice();
            return false;
        }
    }

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

    //set speed profile to default: FAC
   /* if (!setSpeedProfile(0x64, 0x5A)){
        Serial.println("unable to set speed profile!!!!");
        return false;
    }*/

    pSvc = _pClient->getService(serviceUUID);
    if (pSvc) {
        pChr = pSvc->getCharacteristic(profileSettingLowUUID);
        if (pChr) {
            //if (pChr->canRead()) {
                //Serial.printf("%s Value: %s\n", pChr->getUUID().toString().c_str(), pChr->readValue().c_str());
            //}
            if (pChr->canWrite()) {
                if (pChr->writeValue(profile_bytes[0][1])) {
                    Serial.printf("Wrote new value to: %s\n", pChr->getUUID().toString().c_str());
                } else {
                    _pClient->disconnect();
                    _disconnectDevice();
                    return false;
                }
                //if (pChr->canRead()) {
                    //Serial.printf("The value of: %s is now: %s\n", pChr->getUUID().toString().c_str(), pChr->readValue().c_str());
                //}
            }
        }
    } else {
        Serial.printf("unable to set profile low speed setting\n");
    }

    //then, set the high speed profile on the device
    if (pSvc) {
        pChr = pSvc->getCharacteristic(profileSettingHighUUID);
        if (pChr) {
            //if (pChr->canRead()) {
                //Serial.printf("%s Value: %s\n", pChr->getUUID().toString().c_str(), pChr->readValue().c_str());
            //}
            if (pChr->canWrite()) {
                if (pChr->writeValue(profile_bytes[1][1])) {
                    Serial.printf("Wrote new value to: %s\n", pChr->getUUID().toString().c_str());
                } else {
                    _pClient->disconnect();
                    _disconnectDevice();
                    return false;
                }
                //if (pChr->canRead()) {
                    //Serial.printf("The value of: %s is now: %s\n", pChr->getUUID().toString().c_str(), pChr->readValue().c_str());
                //}
            }
        }
    } else {
        Serial.printf("unable to set profile high speed setting\n");
    }

    //finally, connect to speed characteristic and register for notifications
    if (pSvc) {
        pChr = pSvc->getCharacteristic(speedCharacteristicUUID);
        if (pChr) {
            if (pChr->canNotify()) {
                if (!pChr->subscribe(true, notifyCB)) {
                    _pClient->disconnect();
                    _disconnectDevice();
                    return false;
                }
            }
            else{
                _disconnectDevice();
                return false;
            }
        } else {
            _disconnectDevice();
            return false;
        }
    }
    else {
        _disconnectDevice();
        return false;
    }

    //device is fully setup and connected
    _setState(CONNECTED);

    return true;
}
bool ChronoBLE::changeGunType(int type) {

    if (_pClient == nullptr){return false;}
    
    if (!_pClient->isConnected()) {
        return false;
    }

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

    pSvc = _pClient->getService(serviceUUID);
    if (pSvc) {
        pChr = pSvc->getCharacteristic(profileSettingLowUUID);
        if (pChr) {
            if (pChr->canWrite()) {
                if (pChr->writeValue(profile_bytes[0][type])) {
                    Serial.printf("Wrote new value to: %s\n", pChr->getUUID().toString().c_str());
                } else {
                    return false;
                }
                //if (pChr->canRead()) {
                    //Serial.printf("The value of: %s is now: %s\n", pChr->getUUID().toString().c_str(), pChr->readValue().c_str());
                //}
            }
        }
    } else {
        Serial.printf("unable to set profile low speed setting\n");
    }

    //then, set the high speed profile on the device
    if (pSvc) {
        pChr = pSvc->getCharacteristic(profileSettingHighUUID);
        if (pChr) {
            //if (pChr->canRead()) {
                //Serial.printf("%s Value: %s\n", pChr->getUUID().toString().c_str(), pChr->readValue().c_str());
            //}
            if (pChr->canWrite()) {
                if (pChr->writeValue(profile_bytes[1][type])) {
                    Serial.printf("Wrote new value to: %s\n", pChr->getUUID().toString().c_str());
                } else {
                    return false;
                }
                //if (pChr->canRead()) {
                    //Serial.printf("The value of: %s is now: %s\n", pChr->getUUID().toString().c_str(), pChr->readValue().c_str());
                //}
            }
        }
    } else {
        Serial.printf("unable to set profile high speed setting\n");
    }

    return true;
}
bool ChronoBLE::setSpeedProfile(uint8_t low, uint8_t high){
    //first set the low speed profile on the device
    /*NimBLERemoteService*        pSvc = nullptr;
    NimBLERemoteCharacteristic* pChr = nullptr;
    pSvc = pClient->getService(serviceUUID);
    if (pSvc) {
        pChr = pSvc->getCharacteristic(profileSettingLowUUID);
        if (pChr) {
            //if (pChr->canRead()) {
                //Serial.printf("%s Value: %s\n", pChr->getUUID().toString().c_str(), pChr->readValue().c_str());
            //}
            if (pChr->canWrite()) {
                if (pChr->writeValue(0x32)) {
                    Serial.printf("Wrote new value to: %s\n", pChr->getUUID().toString().c_str());
                } else {
                    pClient->disconnect();
                    return false;
                }
                //if (pChr->canRead()) {
                    //Serial.printf("The value of: %s is now: %s\n", pChr->getUUID().toString().c_str(), pChr->readValue().c_str());
                //}
            }
        }
    } else {
        Serial.printf("unable to set profile low speed setting\n");
    }

    //then, set the high speed profile on the device
    if (pSvc) {
        pChr = pSvc->getCharacteristic(profileSettingHighUUID);
        if (pChr) {
            //if (pChr->canRead()) {
                //Serial.printf("%s Value: %s\n", pChr->getUUID().toString().c_str(), pChr->readValue().c_str());
            //}
            if (pChr->canWrite()) {
                if (pChr->writeValue(0x17)) {
                    Serial.printf("Wrote new value to: %s\n", pChr->getUUID().toString().c_str());
                } else {
                    pClient->disconnect();
                    return false;
                }
                //if (pChr->canRead()) {
                    //Serial.printf("The value of: %s is now: %s\n", pChr->getUUID().toString().c_str(), pChr->readValue().c_str());
                //}
            }
        }
    } else {
        Serial.printf("unable to set profile high speed setting\n");
    }*/
}