#include "events/mbed_events.h"
#include "mbed.h"
#include "ble/BLE.h"
#include "SecurityManager.h"

#include "LEDService.h"

DigitalOut g_led1(LED1, 0);
DigitalOut g_led2(LED2, 0);
DigitalOut g_led3(LED3, 0);
DigitalOut g_led4(LED4, 0);

Serial g_serial(USBTX, USBRX);

const static char DEVICE_NAME[] = "LED";
static const uint16_t g_uuid16_list[] =
{
	LEDService::LED_SERVICE_UUID
};

static EventQueue g_eventQueue(/* event count */10 * /* event size */32);

LEDService *g_ledServicePtr;

void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *params)
{
	BLE::Instance().gap().startAdvertising();
}


void blinkCallback(void)
{
	g_led1 = !g_led1; /* Do blinky on LED1 to indicate system aliveness. */
}

/**
 * This callback allows the LEDService to receive updates to the ledState Characteristic.
 *
 * @param[in] params
 *     Information about the characterisitc being updated.
 */
void onDataWrittenCallback(const GattWriteCallbackParams *params)
{
	if ((params->handle == g_ledServicePtr->getValueHandle()) && (params->len == 1))
	{
		uint8_t val = *(params->data);
	}
}

/**
 * This function is called when the ble initialization process has failled
 */
void onBleInitError(BLE &ble, ble_error_t error)
{
	/* Initialization error handling should go here */
}

/**
 * Callback triggered when the ble initialization process has finished
 */
void bleInitComplete(BLE::InitializationCompleteCallbackContext *params)
{
	BLE& ble = params->ble;
	ble_error_t error = params->error;

	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;
	}

	/* Initialize BLE security */
	uint8_t passkey[] = {'0', '1', '2', '3', '4', '5' };
	bool enableBonding = true;
	bool requireMITM = false;
	ble_error_t status = ble.securityManager().init(enableBonding, requireMITM, SecurityManager::IO_CAPS_DISPLAY_ONLY, passkey);
	if (status != BLE_ERROR_NONE)
	{
		while (true)
		{
			printf("Error! status: %u\r\n", status);
			wait(1);
		}
	}

	ble.gattServer().onDataWritten(onDataWrittenCallback);

	bool initialValueForLEDCharacteristic = false;
	g_ledServicePtr = new LEDService(ble, initialValueForLEDCharacteristic);

	/* setup advertising */
	ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED | GapAdvertisingData::LE_GENERAL_DISCOVERABLE);
	ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS, (uint8_t *)g_uuid16_list, sizeof(g_uuid16_list));
	ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LOCAL_NAME, (uint8_t *)DEVICE_NAME, sizeof(DEVICE_NAME));
	ble.gap().setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
	ble.gap().setAdvertisingInterval(1000); /* 1000ms. */
	ble.gap().startAdvertising();

	/* Trying to retrieve list of bonded peers (should be empty right now) */
	BLEProtocol::Address_t addresses[10];

	Gap::Whitelist_t wl;
	wl.size = 0;
	wl.addresses = addresses;
	wl.capacity = 10;

	ble_error_t ret = ble.securityManager().getAddressesFromBondTable(wl); // This function always returns 6 (BLE_ERROR_INVALID_STATE)

	printf("ble.securityManager().getAddressesFromBondTable(wl): %u\r\n", ret);
}

void scheduleBleEventsProcessing(BLE::OnEventsToProcessCallbackContext* context)
{
	BLE &ble = BLE::Instance();
	g_eventQueue.call(Callback<void()>(&ble, &BLE::processEvents));
}

int main()
{
	printf("BLE Demo\r\n");

	g_eventQueue.call_every(3 * 1000, blinkCallback);

	BLE &ble = BLE::Instance();
	ble.onEventsToProcess(scheduleBleEventsProcessing);
	ble.init(bleInitComplete);

	g_eventQueue.dispatch_forever();

	return 0;
}