/* mbed Microcontroller Library
 * Copyright (c) 2006-2018 u-blox Limited
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
#include <events/mbed_events.h>
#include <mbed.h>
#include "ble/BLE.h"
#include "ble/DiscoveredCharacteristic.h"
#include "ble/DiscoveredService.h"

/**************************************************************************
 * MANIFEST CONSTANTS
 *************************************************************************/

// The maximum number of BLE addresses we can handle
// Note that this should be big enough to hold as many
// device as we can see around us (so that we know
// which ones are not interesting as well as those
// that are).  And there are loads of these things about.
#define MAX_NUM_BLE_DEVICES 100

// Storage required for a BLE address
#define BLE_ADDRESS_SIZE 6

// Storage required for a BLE address as a string
#define BLE_ADDRESS_STRING_SIZE 19

/**************************************************************************
 * TYPES
 *************************************************************************/

// The states that a BLE connection can be in
typedef enum {
    BLE_CONNECTION_STATE_DISCONNECTED,
    BLE_CONNECTION_STATE_CONNECTING,
    BLE_CONNECTION_STATE_CONNECTED,
    MAX_NUM_BLE_CONNECTION_STATES
} BleConnectionState;

// The states that a BLE device can be in
typedef enum {
    BLE_DEVICE_STATE_UNKNOWN,
    BLE_DEVICE_STATE_NOT_WANTED,
    BLE_DEVICE_STATE_IS_WANTED,
    MAX_NUM_BLE_DEVICE_STATES
} BleDeviceState;

// Structure defining a BLE device
typedef struct {
    char address[BLE_ADDRESS_SIZE];
    BleDeviceState deviceState;
    BleConnectionState connectionState;
    Gap::Handle_t connectionHandle;
} BleDevice;

/**************************************************************************
 * LOCAL VARIABLES
 *************************************************************************/

// Activity LED
static DigitalOut activityLedBar(LED1, 1); // Red (DS9), active low on a NINA B1 board

// Connection LED
static DigitalOut connectedLedBar(LED2, 1); // Green (DS9), active low on a NINA B1 board

// The BLE event queue
static EventQueue bleEventQueue(/* event count */ 16 * EVENTS_EVENT_SIZE);

// The list of BLE devices
static BleDevice bleDeviceList[MAX_NUM_BLE_DEVICES];

// Mutex to protect the list
static Mutex bleListMutex;

// The number of devices in the list
static int numBleDevicesInList = 0;

// TODO are these correct?
static const Gap::ConnectionParams_t connectionParams = {6 /* minConnectionInterval */,
                                                         6 /* maxConnectionInterval */,
                                                         0 /* slaveLatency */,
                                                         100 /* connectionSupervisionTimeout (10 ms units) */};

                                                         // Gap scanning parameters to minimise connection time (from https://os.mbed.com/docs/v5.8/reference/gap.html)
static const GapScanningParams connectionScanParams(GapScanningParams::SCAN_INTERVAL_MAX /* interval */,
                                                    GapScanningParams::SCAN_WINDOW_MAX /* window */,
                                                    3 /* timeout */,
                                                    false /* active scanning */);

static const char *gapAdvertisingDataType[] = {"VALUE_NOT_ALLOWED", "FLAGS", "INCOMPLETE_LIST_16BIT_SERVICE_IDS", "COMPLETE_LIST_16BIT_SERVICE_IDS",
                                               "INCOMPLETE_LIST_32BIT_SERVICE_IDS", "COMPLETE_LIST_32BIT_SERVICE_IDS",
                                               "INCOMPLETE_LIST_128BIT_SERVICE_ID", "COMPLETE_LIST_128BIT_SERVICE_IDS",
                                               "SHORTENED_LOCAL_NAME", "COMPLETE_LOCAL_NAME", "TX_POWER_LEVEL", "DEVICE_ID",
                                               "SLAVE_CONNECTION_INTERVAL_RANGE", "LIST_128BIT_SOLICITATION_IDS", "SERVICE_DATA",
                                               "APPEARANCE", "ADVERTISING_INTERVAL"};

static const char hexTable[] = { '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', 'a', 'b', 'c', 'd', 'e', 'f' };

/**************************************************************************
 * STATIC FUNCTIONS
 *************************************************************************/

// Convert a sequence of bytes into a hex string, returning the number
// of characters written. The hex string is NOT null terminated.
static int bytesToHexString(const char *pInBuf, int lenInBuf, char *pOutBuf, int lenOutBuf)
{
    int y = 0;

    for (int x = 0; (x < lenInBuf) && (y < lenOutBuf); x++) {
        pOutBuf[y] = hexTable[(pInBuf[x] >> 4) & 0x0f]; // upper nibble
        y++;
        if (y < lenOutBuf) {
            pOutBuf[y] = hexTable[pInBuf[x] & 0x0f]; // lower nibble
            y++;
        }
    }

    return y;
}

// LED aliveness and status callback
static void periodicCallback(void) {
    Gap::GapState_t gapState;
    
    activityLedBar = !activityLedBar;
    
    gapState = BLE::Instance().gap().getState();
    if (gapState.connected) {
        connectedLedBar = 0;
    } else {
        connectedLedBar = 1;
    }
}

// Print a BLE_ADDRESS_SIZE digit binary BLE address out nicely as a
// string; pBuf must point to storage for a string at least
// BLE_ADDRESS_STRING_LENGTH bytes long
static char *printBleAddress(const char *pAddress, char *pBuf)
{
    int x = 0;
    
    for (int i = (BLE_ADDRESS_SIZE - 1); i >= 0; i--) {
        sprintf(pBuf + x, "%02x:", *(pAddress + i));
        x += 3;
    }
    *(pBuf + x - 1) = 0; // Remove the final ':'

    return pBuf;
}

// Find a BLE device in the list by its address
// Note that this does NOT lock the BLE list
static BleDevice *findBleDeviceInList(const char *pAddress)
{
    BleDevice *pBleDevice = NULL;
    
    for (int x = 0; (x < numBleDevicesInList) && (pBleDevice == NULL); x++) {
        if (memcmp (pAddress, bleDeviceList[x].address, sizeof (bleDeviceList[x].address)) == 0) {
            pBleDevice = &(bleDeviceList[x]);
        }
    }
    
    return pBleDevice;
}

// Find a BLE device in the list by its connection
// Note that this does NOT lock the BLE list
static BleDevice *findBleConnectionInList(Gap::Handle_t connectionHandle)
{
    BleDevice *pBleDevice = NULL;
    
    for (int x = 0; (x < numBleDevicesInList) && (pBleDevice == NULL); x++) {
        if ((bleDeviceList[x].connectionState == BLE_CONNECTION_STATE_CONNECTED) &&
            (bleDeviceList[x].connectionHandle == connectionHandle)) {
            pBleDevice = &(bleDeviceList[x]);
        }
    }
    
    return pBleDevice;
}

// Add a BLE device to the list, returning a pointer
// to the new entry.  If the device is already in the list
// a pointer is returned to the (unmodified) existing entry.
// If the list is already full then NULL is returned.
// Note that this does NOT lock the BLE list.
static BleDevice *addBleDeviceToList(const char *pAddress)
{
    BleDevice *pBleDevice;
    
    pBleDevice = findBleDeviceInList(pAddress);
    if (pBleDevice == NULL) {
        if (numBleDevicesInList < (int) (sizeof(bleDeviceList) / sizeof (bleDeviceList[0]))) {
            pBleDevice = &(bleDeviceList[numBleDevicesInList]);
            memcpy (pBleDevice->address, pAddress, sizeof (pBleDevice->address));
            pBleDevice->deviceState = BLE_DEVICE_STATE_UNKNOWN;
            pBleDevice->connectionState = BLE_CONNECTION_STATE_DISCONNECTED;
            numBleDevicesInList++;
        }
    }
    
    return pBleDevice;
}

// Process an advertisement and connect to the device if
// it is one that we want
static void advertisementCallback(const Gap::AdvertisementCallbackParams_t *pParams)
{
    char buf[BLE_ADDRESS_STRING_SIZE + 32];
    BleDevice *pBleDevice;
    ble_error_t bleError;
    int recordLength;
    int x = 0;
    bool discoverable = false;

    pBleDevice = addBleDeviceToList((const char *) pParams->peerAddr);
    if (pBleDevice != NULL) {
        // Only bother checking on the device if it isn't marked as "not wanted"
        // and if we're not already [attempting to] connect[ed] to it
        if ((pBleDevice->deviceState == BLE_DEVICE_STATE_UNKNOWN) &&
            ((pBleDevice->connectionState != BLE_CONNECTION_STATE_CONNECTED) ||
             (pBleDevice->connectionState != BLE_CONNECTION_STATE_CONNECTING))) {
            printf("BLE device %s is visible.\n", printBleAddress((char *) pParams->peerAddr, buf));
            // Check if the device is discoverable
            while ((x < pParams->advertisingDataLen) && !discoverable) {
                /* The advertising payload is a collection of key/value records where
                 * byte 0: length of the record excluding this byte but including the "type" byte
                 * byte 1: The key, it is the type of the data
                 * byte [2..N] The value. N is equal to byte0 - 1 */
                recordLength = pParams->advertisingData[x];
                if ((recordLength > 0) && (pParams->advertisingData[x + 1] != 0)) {  // Type of 0 is not allowed
                    const int type = pParams->advertisingData[x + 1];
                    const char *pValue = (const char *) pParams->advertisingData + x + 2;
                    printf("  Advertising payload type 0x%02x", type);
                    if (type < (int) (sizeof(gapAdvertisingDataType) / sizeof(gapAdvertisingDataType[0]))) {
                        printf(" (%s)", gapAdvertisingDataType[type]);
                    }
                    printf(" (%d byte(s)): 0x%.*s.\n", recordLength - 1,
                           bytesToHexString(pValue, recordLength - 1, buf, sizeof(buf)), buf);
                    if ((type == GapAdvertisingData::FLAGS) &&
                        (*pValue & (GapAdvertisingData::LE_GENERAL_DISCOVERABLE | GapAdvertisingData::LE_LIMITED_DISCOVERABLE))) {
                        discoverable = true;
                    }
                    x += recordLength + 1;
                } else {
                    x++;
                }
            }
            
            if (discoverable) {
                printf("  ...and discoverable, attempting to connect to it");
                bleError = BLE::Instance().gap().connect(pParams->peerAddr, BLEProtocol::AddressType::RANDOM_STATIC, NULL, NULL);
                //bleError = BLE::Instance().gap().connect(pParams->peerAddr, BLEProtocol::AddressType::RANDOM_STATIC, &connectionParams, &connectionScanParams);
                if (bleError == BLE_ERROR_NONE) {
                    pBleDevice->connectionState = BLE_CONNECTION_STATE_CONNECTING;
                    printf(", connect() successfully issued.\n");
                } else if (bleError == BLE_ERROR_INVALID_STATE) {
                    printf(", but CAN'T as BLE is in an invalid state (may already be connecting?).\n");
                } else {
                    printf(", but unable to issue connect (error %d).\n", bleError);
                }
            } else {
                printf("  ...but not discoverable.\n");
                pBleDevice->deviceState = BLE_DEVICE_STATE_NOT_WANTED;
            }
        }
    } else {
        printf("BLE device list is full (%d device(s))!\n", numBleDevicesInList);
    }
}

// Act on the discovery of a service
static void serviceDiscoveryCallback(const DiscoveredService *pService) {
    if (pService->getUUID().shortOrLong() == UUID::UUID_TYPE_SHORT) {
        printf("Service 0x%x attrs[%u %u].\n", pService->getUUID().getShortUUID(),
               pService->getStartHandle(), pService->getEndHandle());
    } else {
        printf("Service 0x");
        const char *pLongUUIDBytes = (char *) pService->getUUID().getBaseUUID();
        for (unsigned int i = 0; i < UUID::LENGTH_OF_LONG_UUID; i++) {
            printf("%02x", *(pLongUUIDBytes + i));
        }
        printf(" attrs[%u %u].\n", pService->getStartHandle(), pService->getEndHandle());
    }
}

// Act on the discovery of a characteristic
static void characteristicDiscoveryCallback(const DiscoveredCharacteristic *pCharacteristic) {
    printf("  Characteristic 0x%x valueAttr[%u] props[0x%x].\n", pCharacteristic->getUUID().getShortUUID(),
           pCharacteristic->getValueHandle(), (uint8_t) pCharacteristic->getProperties().broadcast());
}

// Handle end of service discovery
static void discoveryTerminationCallback(Gap::Handle_t connectionHandle) {
    char addressString[BLE_ADDRESS_STRING_SIZE];
    BleDevice *pBleDevice = findBleConnectionInList(connectionHandle);

    printf("Terminated service discovery for handle %u", connectionHandle);
    if (pBleDevice != NULL) {
        printf(", BLE device %s", printBleAddress(pBleDevice->address, addressString));
    }
    printf(".\n");
}

// When a connection has been made, find out what services are available and their characteristics
static void connectionCallback(const Gap::ConnectionCallbackParams_t *pParams) {
    char addressString[BLE_ADDRESS_STRING_SIZE];
    BleDevice *pBleDevice = findBleDeviceInList((char *) pParams->peerAddr);

    printf("BLE device %s is connected.\n", printBleAddress((char *) pParams->peerAddr, addressString));
    if (pBleDevice != NULL) {
        pBleDevice->connectionHandle = pParams->handle;
        pBleDevice->connectionState = BLE_CONNECTION_STATE_CONNECTED;
        if (pParams->role == Gap::CENTRAL) {
            printf("  Attempting to discover its services and characteristics...\n");
            BLE &ble = BLE::Instance();
            ble.gattClient().onServiceDiscoveryTermination(discoveryTerminationCallback);
            ble.gattClient().launchServiceDiscovery(pParams->handle, serviceDiscoveryCallback,
                                                    characteristicDiscoveryCallback,
                                                    BLE_UUID_UNKNOWN, BLE_UUID_UNKNOWN);
        }
    }
}

// Handle BLE peer disconnection
static void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *pParams) {
    char addressString[BLE_ADDRESS_STRING_SIZE];
    BleDevice *pBleDevice = findBleConnectionInList(pParams->handle);

    printf("Disconnected");
    if (pBleDevice != NULL) {
        printf(" from device %s", printBleAddress(pBleDevice->address, addressString));
        if (pBleDevice->connectionState == BLE_CONNECTION_STATE_CONNECTING) {
            // If we were connecting to this device and it's rudely bounced us then
            // it probably doesn't want to know about us so cross it off our Christmas list
            pBleDevice->deviceState = BLE_DEVICE_STATE_NOT_WANTED;
            printf(" immediately on connection attempt, so dropping it");
        } else {
            pBleDevice->connectionState = BLE_CONNECTION_STATE_DISCONNECTED;
        }
    }
    printf(".\n");

    /* Start scanning again */
    BLE::Instance().gap().startScan(advertisementCallback);
}

// Handle BLE initialisation error
static void onBleInitError(BLE &ble, ble_error_t error)
{
   printf("!!! BLE Error %d !!!\n", error);
}

// Handle BLE being initialised, finish configuration here
static void bleInitComplete(BLE::InitializationCompleteCallbackContext *pParams)
{
    BLE& ble = pParams->ble;
    ble_error_t error = pParams->error;
    Gap::AddressType_t addr_type;
    Gap::Address_t address;
    BLE::Instance().gap().getAddress(&addr_type, address);
    char addressString[BLE_ADDRESS_STRING_SIZE];

    if (error != BLE_ERROR_NONE) {
        /* In case of error, forward the error handling to onBleInitError */
        onBleInitError(ble, error);
        return;
    }

    /* Ensure that it is the default instance of BLE */
    if (ble.getInstanceID() != BLE::DEFAULT_INSTANCE) {
        return;
    }

    printf("This device's BLE address is %s.\n", printBleAddress((char *) address, addressString));

    ble.gap().onDisconnection(disconnectionCallback);
    ble.gap().onConnection(connectionCallback);

    // scan interval: 400 ms and scan window: 400 ms.
    // Every 400 ms the device will scan for 400 ms
    // This means that the device will scan continuously.
    ble.gap().setScanParams(400, 400);
    ble.gap().startScan(advertisementCallback);
}

// Throw a BLE event onto the BLE event queue
static void scheduleBleEventsProcessing(BLE::OnEventsToProcessCallbackContext* pContext) {
    BLE &ble = BLE::Instance();
    bleEventQueue.call(Callback<void()>(&ble, &BLE::processEvents));
}

// Run BLE for a given time; use -1 for infinity, in which
// case this function will never return
static void runBle(int durationMs)
{
    BLE &ble = BLE::Instance();
    
    ble.onEventsToProcess(scheduleBleEventsProcessing);
    ble.init(bleInitComplete);
    bleEventQueue.dispatch(durationMs);
}

/**************************************************************************
 * PUBLIC FUNCTIONS
 *************************************************************************/

// Main
int main()
{
    printf("\n\nStarting up.\n");
    
    bleEventQueue.call_every(500, periodicCallback);
    runBle(-1);
}