/**
  ******************************************************************************
  * @file    main.c
  * @author  Leo_LYF
  * @brief   Multi-Central
  *          
  ******************************************************************************
  */

/* Includes ------------------------------------------------------------------*/

/* Zephyr Begin Includes */

#include <kernel.h>
#include <console/console.h>
#include <sys/printk.h>
#include <string.h>
#include <stdlib.h>
#include <zephyr/types.h>

/* Zephyr END Includes */

/* BLE Begin Includes */
#include <bluetooth/bluetooth.h>
#include <bluetooth/crypto.h>
#include <bluetooth/conn.h>
#include <bluetooth/gatt.h>
#include <bluetooth/hci.h>
#include <bluetooth/uuid.h>
#include <bluetooth/scan.h>
#include <bluetooth/gatt_dm.h>
/* BLE END Includes */

/* Throughput Include */
#include <bluetooth/services/throughput.h>

/* CCL */
#include <bluetooth/conn_ctx.h>

/* Shell Include */
#include <shell/shell_uart.h>

/* Dk_button_and_leds Inculde */
#include <dk_buttons_and_leds.h>

/* Logger */
#include <logging/log.h>
#define LOG_LEVEL LOG_LEVEL_DBG
LOG_MODULE_REGISTER(multi_central);

/*  HID Central Include */
#include <bluetooth/services/hogp.h>

#include <settings/settings.h>

/* Private typedef -----------------------------------------------------------*/
/* Private define ------------------------------------------------------------*/

/* Device Name Begin Defines */

#define DEVICE_NAME	CONFIG_BT_DEVICE_NAME
#define DEVICE_NAME_LEN (sizeof(DEVICE_NAME) - 1)

/* Device Name END Defines */

/* BLE Interval Begin Defines */

#define INTERVAL_MIN	0x08	/* 8 units, 10 ms */
#define INTERVAL_MAX	0x08	/* 8 units, 10 ms */

/* BLE Interval END Defines */

/* Throughput Timeout Define */
#define THROUGHPUT_CONFIG_TIMEOUT K_SECONDS(20)

/* Throughput Write Thread Stack Define */
K_THREAD_STACK_DEFINE(throughput_write_thread_stack, 1024);

/* Write Size Bytes */
#define WRITE_SIZE_BYTES 160


/* HID Central Begin defines */

/**
 * Switch between boot protocol and report protocol mode.
 */
#define KEY_BOOTMODE_MASK DK_BTN2_MSK
/**
 * Switch CAPSLOCK state.
 *
 * @note
 * For simplicity of the code it works only in boot mode.
 */
#define KEY_CAPSLOCK_MASK DK_BTN1_MSK
/**
 * Switch CAPSLOCK state with response
 *
 * Write CAPSLOCK with response.
 * Just for testing purposes.
 * The result should be the same like usine @ref KEY_CAPSLOCK_MASK
 */
#define KEY_CAPSLOCK_RSP_MASK DK_BTN3_MSK

/* Key used to accept or reject passkey value */
#define KEY_PAIRING_ACCEPT DK_BTN1_MSK
#define KEY_PAIRING_REJECT DK_BTN2_MSK

/* HID Central END defines */


/* Private macro -------------------------------------------------------------*/


/* Exported variables --------------------------------------------------------*/
/* Private variables ---------------------------------------------------------*/

/* Throughput Sample Begin variables */

static K_SEM_DEFINE(throughput_sem, 0, 1);

static volatile bool data_length_req;
static volatile bool test_ready;
static struct bt_conn *default_conn;
static struct bt_uuid *uuid128 = BT_UUID_THROUGHPUT;
static struct bt_gatt_exchange_params exchange_params;
static struct bt_le_conn_param *conn_param =
	BT_LE_CONN_PARAM(INTERVAL_MIN, INTERVAL_MAX, 0, 400);

/* Throughput Sample END variables */

/* throughput_write_thread  */
static struct k_thread throughput_write_thread_data;

/* throughput_write_thread id  */
static k_tid_t throughput_write_thread_id;

/* HID Central Begin variables */
static struct bt_conn *auth_conn;

static uint8_t capslock_state;

static void hids_on_ready(struct k_work *work);

static K_WORK_DEFINE(hids_ready_work, hids_on_ready);

/* HID Central END variables */

/* CCL Throughput Instance */
BT_CONN_CTX_DEF(conns, CONFIG_BT_MAX_CONN, sizeof(struct bt_throughput));

/* CCL Central HID Instance  */
BT_CONN_CTX_DEF(conns_hid, CONFIG_BT_MAX_CONN, sizeof(struct bt_hogp));

enum FLAG
{
	THROUGHPUT_FLAG = 0X01,
	HID_FLAG = 0X02,
};

/* Connect flag */
uint8_t connect_flag = 0;

uint8_t connect_cnt = 0;
/* Private function prototypes -----------------------------------------------*/

/* Start the ughput write thread */
void throughput_write_start(void);

static void throughput_received(const struct bt_throughput_metrics *met);

static void throughput_send(const struct bt_throughput_metrics *met);

static uint8_t throughput_read(const struct bt_throughput_metrics *met);

static void scan_start(void);

static void hogp_ready_cb(struct bt_hogp *hogp);

static void hogp_prep_fail_cb(struct bt_hogp *hogp, int err);

static void hogp_pm_update_cb(struct bt_hogp *hogp);

static void gatt_discover_hid(struct bt_conn *conn);

void throughput_write_start(void);

/* Exported functions --------------------------------------------------------*/

/* Private functions ---------------------------------------------------------*/

static const char *phy2str(uint8_t phy)
{
	switch (phy) {
	case 0: return "No packets";
	case BT_GAP_LE_PHY_1M: return "LE 1M";
	case BT_GAP_LE_PHY_2M: return "LE 2M";
	case BT_GAP_LE_PHY_CODED: return "LE Coded";
	default: return "Unknown";
	}
}

static void instruction_print(void)
{
	printk("\nType 'config' to change the configuration parameters.\n");
	printk("You can use the Tab key to autocomplete your input.\n");
	printk("Type 'run' when you are ready to run the test.\n");
}

void scan_filter_match(struct bt_scan_device_info *device_info,
		       struct bt_scan_filter_match *filter_match,
		       bool connectable)
{
	char addr[BT_ADDR_LE_STR_LEN];
	bt_addr_le_to_str(device_info->recv_info->addr, addr, sizeof(addr));
	for (int i = 0; i < filter_match->uuid.count; i++)
	{

		if (memcmp(filter_match->uuid.uuid[i], BT_UUID_THROUGHPUT, sizeof(BT_UUID_THROUGHPUT)) == 0)
		{
			connect_flag = THROUGHPUT_FLAG;
			printk("Filters matched. Throughput Address: %s connectable: %d\n", addr, connectable);
			break;
		}
		else
		{
			connect_flag = HID_FLAG;
			printk("Filters matched. HID Address: %s connectable: %d\n", addr, connectable);
			break;
		}
		
	}

}

void scan_filter_no_match(struct bt_scan_device_info *device_info,
			  bool connectable)
{
	char addr[BT_ADDR_LE_STR_LEN];

	bt_addr_le_to_str(device_info->recv_info->addr, addr, sizeof(addr));

	if (connect_cnt == 2)
	{
		/*Stop scanning during the discovery*/
		bt_scan_stop();
	}
//	printk("Filter not match. Address: %s connectable: %d\n",
//				addr, connectable);
}

void scan_connecting_error(struct bt_scan_device_info *device_info)
{
	printk("Connecting failed\n");
}

static void scan_connecting(struct bt_scan_device_info *device_info,
			    struct bt_conn *conn)
{
	default_conn = bt_conn_ref(conn);
}


BT_SCAN_CB_INIT(scan_cb, scan_filter_match, scan_filter_no_match,
		scan_connecting_error, scan_connecting);

static void exchange_func(struct bt_conn *conn, uint8_t att_err,
			  struct bt_gatt_exchange_params *params)
{
	struct bt_conn_info info = {0};
	int err;

	printk("MTU exchange %s\n", att_err == 0 ? "successful" : "failed");

	err = bt_conn_get_info(conn, &info);
	if (err) {
		printk("Failed to get connection info %d\n", err);
		return;
	}

	if (info.role == BT_CONN_ROLE_MASTER) {
		instruction_print();
		test_ready = true;
	}
}

static void discovery_complete(struct bt_gatt_dm *dm,
			       void *context)
{
	int err;
	struct bt_throughput *throughput = context;

	printk("Service discovery completed\n");

	bt_gatt_dm_data_print(dm);
	bt_throughput_handles_assign(dm, throughput);
	bt_gatt_dm_data_release(dm);

	exchange_params.func = exchange_func;

	err = bt_gatt_exchange_mtu(default_conn, &exchange_params);
	if (err) {
		printk("MTU exchange failed (err %d)\n", err);
	} else {
		printk("MTU exchange pending\n");
	}
	scan_start();
	connect_cnt++;
}

static void discovery_service_not_found(struct bt_conn *conn,
					void *context)
{
	printk("Service not found\n");
}

static void discovery_error(struct bt_conn *conn,
			    int err,
			    void *context)
{
	printk("Error while discovering GATT database: (%d)\n", err);
}

struct bt_gatt_dm_cb discovery_cb = {
	.completed         = discovery_complete,
	.service_not_found = discovery_service_not_found,
	.error_found       = discovery_error,
};

static void gatt_discover(struct bt_conn *conn)
{
	int err;

	struct bt_throughput *throughput =
		bt_conn_ctx_get(&conns_ctx_lib, conn);


	if (!throughput) {
		return;
	}

	err = bt_gatt_dm_start(conn,
			       BT_UUID_THROUGHPUT,
			       &discovery_cb,
			       throughput);

	if (err) {
		LOG_ERR("could not start the throughput discovery procedure, error "
			"code: %d", err);
	}

	bt_conn_ctx_release(&conns_ctx_lib, (void *)throughput);

}

static void connected(struct bt_conn *conn, uint8_t hci_err)
{
	char addr[BT_ADDR_LE_STR_LEN];
	struct bt_conn_info info = {0};
	int err;

	static const struct bt_throughput_cb throughput_cb = {
		.data_read = throughput_read,
		.data_received = throughput_received,
		.data_send = throughput_send
	};

	static const struct bt_hogp_init_params hogp_init_params = {
	.ready_cb      = hogp_ready_cb,
	.prep_error_cb = hogp_prep_fail_cb,
	.pm_update_cb  = hogp_pm_update_cb
	};
	
	bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));
	
	if (hci_err) {
		LOG_INF("Failed to connect to %s (%d)", log_strdup(addr),
			hci_err);

		if (default_conn == conn) {
			bt_conn_unref(default_conn);
			default_conn = NULL;

			err = bt_scan_start(BT_SCAN_TYPE_SCAN_ACTIVE);
			if (err) {
				LOG_ERR("Scanning failed to start (err %d)",
					err);
			}
		}

		return;
	}

	LOG_INF("Connected: %s", log_strdup(addr));

	err = bt_conn_get_info(default_conn, &info);
	if (err) {
		printk("Failed to get connection info %d\n", err);
		return;
	}
        
	if (connect_flag == THROUGHPUT_FLAG)
	{
		printk("Throughput Conn. interval is %u units\n", info.le.interval);
		
		struct bt_throughput *throughput = bt_conn_ctx_alloc(&conns_ctx_lib, conn);

  		if (!throughput) 
		{
			LOG_WRN("There is no free memory to "
				"allocate the connection context");
		}
		
		memset(throughput, 0, bt_conn_ctx_block_size_get(&conns_ctx_lib));	

		err = bt_throughput_init(throughput, &throughput_cb);

		bt_conn_ctx_release(&conns_ctx_lib, (void *)throughput);
		
		if (err) 
		{
			LOG_ERR("Throughput Master initialization failed (err %d)", err);
		}
		else
		{
			LOG_INF("Throughput Master module initialized");
		}

		connect_flag = 0;
		
		gatt_discover(conn);
	}
	if (connect_flag == HID_FLAG)
	{
		printk("HID Conn. interval is %u units\n", info.le.interval);
		
		struct bt_hogp *hogp = bt_conn_ctx_alloc(&conns_hid_ctx_lib, conn);
	
		if (!hogp) 
		{
			printk("There is no free memory to hid allocate the hogp connection context");
		}

		memset(hogp, 0, bt_conn_ctx_block_size_get(&conns_hid_ctx_lib));		

		bt_hogp_init(hogp, &hogp_init_params);

		bt_conn_ctx_release(&conns_hid_ctx_lib, (void *)hogp);
		
		connect_flag = 0;	
		
		gatt_discover_hid(conn);
		
	}


	/*Stop scanning during the discovery*/
	err = bt_scan_stop();
	if ((!err) && (err != -EALREADY)) {
		LOG_ERR("Stop LE scan failed (err %d)", err);
	}	

}

static void scan_init(void)
{
	int err;
	struct bt_le_scan_param scan_param = {
		.type = BT_LE_SCAN_TYPE_PASSIVE,
		.options = BT_LE_SCAN_OPT_FILTER_DUPLICATE,
		.interval = 0x0010,
		.window = 0x0004,
	};

	struct bt_scan_init_param scan_init = {
		.connect_if_match = 1,
		.scan_param = &scan_param,
		.conn_param = conn_param
	};

	bt_scan_init(&scan_init);
	bt_scan_cb_register(&scan_cb);

	err = bt_scan_filter_add(BT_SCAN_FILTER_TYPE_UUID, uuid128);
	if (err) {
		printk("Scanning filters cannot be set\n");

		return;
	}
	
	err = bt_scan_filter_add(BT_SCAN_FILTER_TYPE_UUID, BT_UUID_HIDS);
	if (err) 
	{
		printk("Scanning filters cannot be set (err %d)\n", err);

		return;
	}


	err = bt_scan_filter_enable(BT_SCAN_UUID_FILTER, false);
	if (err) {
		printk("Filters cannot be turned on\n");
	}
}

static void scan_start(void)
{
	int err;

	err = bt_scan_start(BT_SCAN_TYPE_SCAN_PASSIVE);
	if (err) {
		printk("Starting scanning failed (err %d)\n", err);
		return;
	}
	else
	{
		printk("Starting scanning successed\n");
	}
}


static void disconnected(struct bt_conn *conn, uint8_t reason)
{
	char addr[BT_ADDR_LE_STR_LEN];
	int err;

	bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));

	LOG_INF("Disconnected: %s (reason %u)", log_strdup(addr),
		reason);

	err = bt_conn_ctx_free(&conns_ctx_lib, conn);

	if (err) {
		LOG_WRN("The memory was not allocated for the throughput context of this "
			"connection.");
	}

	bt_conn_unref(conn);
	default_conn = NULL;

	/* Re-connect using same roles */
	scan_start();
}

static void discovery_completed_cb(struct bt_gatt_dm *dm,
				   void *context)
{
	int err;

	struct bt_hogp *hogp = context;
	printk("The discovery procedure succeeded\n");

	bt_gatt_dm_data_print(dm);

	err = bt_hogp_handles_assign(dm, hogp);
	
	if (err) 
	{
		printk("Could not init HIDS client object, error: %d\n", err);
	}

	err = bt_gatt_dm_data_release(dm);
	
	if (err) 
	{
		printk("Could not release the discovery data, error code: %d\n", err);
	}
	
	scan_start();
	connect_cnt++;
}

static void discovery_service_not_found_cb(struct bt_conn *conn,
					   void *context)
{
	printk("The service could not be found during the discovery\n");
}

static void discovery_error_found_cb(struct bt_conn *conn,
				     int err,
				     void *context)
{
	printk("The discovery procedure failed with %d\n", err);
}

static const struct bt_gatt_dm_cb discovery_hid = {
	.completed = discovery_completed_cb,
	.service_not_found = discovery_service_not_found_cb,
	.error_found = discovery_error_found_cb,
};

static void gatt_discover_hid(struct bt_conn *conn)
{
	int err;

	struct bt_hogp *hogp  = bt_conn_ctx_get(&conns_hid_ctx_lib, conn);
	
	if (!hogp) 
	{
		return;
	}


	err = bt_gatt_dm_start(conn, BT_UUID_HIDS, &discovery_hid, hogp);
	if (err) 
	{
		printk("could not start the discovery procedure, error "
			"code: %d\n", err);
	}
	bt_conn_ctx_release(&conns_hid_ctx_lib, (void *) hogp);
}

static void security_changed(struct bt_conn *conn, bt_security_t level,
			     enum bt_security_err err)
{
	char addr[BT_ADDR_LE_STR_LEN];

	bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));

	if (!err) {
		printk("Security changed: %s level %u\n", addr, level);
	} else {
		printk("Security failed: %s level %u err %d\n", addr, level,
			err);
	}

	gatt_discover_hid(conn);
}

static uint8_t hogp_notify_cb(struct bt_hogp *hogp,
			     struct bt_hogp_rep_info *rep,
			     uint8_t err,
			     const uint8_t *data)
{
	uint8_t size = bt_hogp_rep_size(rep);
	uint8_t i;

	if (!data) {
		return BT_GATT_ITER_STOP;
	}
	printk("Notification, id: %u, size: %u, data:",
	       bt_hogp_rep_id(rep),
	       size);
	for (i = 0; i < size; ++i) {
		printk(" 0x%x", data[i]);
	}
	printk("\n");
	return BT_GATT_ITER_CONTINUE;
}

static uint8_t hogp_boot_mouse_report(struct bt_hogp *hogp,
				     struct bt_hogp_rep_info *rep,
				     uint8_t err,
				     const uint8_t *data)
{
	uint8_t size = bt_hogp_rep_size(rep);
	uint8_t i;

	if (!data) {
		return BT_GATT_ITER_STOP;
	}
	printk("Notification, mouse boot, size: %u, data:", size);
	for (i = 0; i < size; ++i) {
		printk(" 0x%x", data[i]);
	}
	printk("\n");
	return BT_GATT_ITER_CONTINUE;
}

static uint8_t hogp_boot_kbd_report(struct bt_hogp *hogp,
				   struct bt_hogp_rep_info *rep,
				   uint8_t err,
				   const uint8_t *data)
{
	uint8_t size = bt_hogp_rep_size(rep);
	uint8_t i;

	if (!data) {
		return BT_GATT_ITER_STOP;
	}
	printk("Notification, keyboard boot, size: %u, data:", size);
	for (i = 0; i < size; ++i) {
		printk(" 0x%x", data[i]);
	}
	printk("\n");
	return BT_GATT_ITER_CONTINUE;
}

static void hogp_ready_cb(struct bt_hogp *hogp)
{
	k_work_submit(&hids_ready_work);
}

static void hids_on_ready(struct k_work *work)
{
int err;
	struct bt_hogp_rep_info *rep = NULL;
	
	const struct bt_conn_ctx *ctx = bt_conn_ctx_get_by_id(&conns_hid_ctx_lib, 0);
	if (!ctx)
	{
		printk("HIDS CCL get error\n");
	}
	struct bt_hogp * hogp = ctx->data;
	
	printk("HIDS is ready to work\n");

	while (NULL != (rep = bt_hogp_rep_next(hogp, rep))) 
	{
		if (bt_hogp_rep_type(rep) == BT_HIDS_REPORT_TYPE_INPUT) 
		{
			printk("Subscribe to report id: %u\n", bt_hogp_rep_id(rep));

			err = bt_hogp_rep_subscribe(hogp, rep,
							   hogp_notify_cb);
			if (err) 
			{
				printk("Subscribe error (%d)\n", err);
			}
		}
	}
	
	if (hogp->rep_boot.kbd_inp) 
	{
		printk("Subscribe to boot keyboard report\n");
		
		err = bt_hogp_rep_subscribe(hogp, hogp->rep_boot.kbd_inp, hogp_boot_kbd_report);
		
		if (err) 
		{
			printk("Subscribe error (%d)\n", err);
		}
	}
	
	if (hogp->rep_boot.mouse_inp) 
	{
		printk("Subscribe to boot mouse report\n");
		
		err = bt_hogp_rep_subscribe(hogp, hogp->rep_boot.mouse_inp, hogp_boot_mouse_report);
		
		if (err) 
		{
			printk("Subscribe error (%d)\n", err);
		}
	}
	
	bt_conn_ctx_release(&conns_hid_ctx_lib, (void *) hogp);

}

static void hogp_prep_fail_cb(struct bt_hogp *hogp, int err)
{
	printk("ERROR: HIDS client preparation failed!\n");
}

static void hogp_pm_update_cb(struct bt_hogp *hogp)
{
	printk("Protocol mode updated: %s\n",
	      bt_hogp_pm_get(hogp) == BT_HIDS_PM_BOOT ?
	      "BOOT" : "REPORT");
}

static void button_bootmode(void)
{
	const struct bt_conn_ctx *ctx = bt_conn_ctx_get_by_id(&conns_hid_ctx_lib, 0);
	
	if (!ctx)
	{
		printk("HIDS CCL get error\n");
	}

	struct bt_hogp * hogp = ctx->data;

	if (!bt_hogp_ready_check(hogp)) 
	{
		printk("HID device not ready\n");
		return;
	}

	
	int err;
	
	enum bt_hids_pm pm = bt_hogp_pm_get(hogp);

	printk("Setting protocol mode: %s\n",
	       (pm == BT_HIDS_PM_BOOT) ?
	       "BOOT" : "REPORT");
	       
	err = bt_hogp_pm_write(hogp,
				      (pm == BT_HIDS_PM_BOOT) ?
				      BT_HIDS_PM_REPORT :
				      BT_HIDS_PM_BOOT);

	bt_conn_ctx_release(&conns_hid_ctx_lib, (void *) hogp);

	if (err) 
	{
		printk("Cannot change protocol mode (err %d)\n", err);
	}
}

static void hidc_write_cb(struct bt_hogp *hidc,
			  struct bt_hogp_rep_info *rep,
			  uint8_t err)
{
	printk("Caps lock sent\n");
}

static void button_capslock(void)
{
	const struct bt_conn_ctx *ctx = bt_conn_ctx_get_by_id(&conns_hid_ctx_lib, 0);
	if (!ctx)
	{
		printk("HIDS CCL get error\n");
	}
	struct bt_hogp * hogp = ctx->data;

	int err;
	uint8_t data;

	if (!bt_hogp_ready_check(hogp)) 
	{
		printk("HID device not ready\n");
		return;
	}
	
	if (!hogp->rep_boot.kbd_out) 
	{
		printk("HID device does not have Keyboard OUT report\n");
		return;
	}
	
	if (bt_hogp_pm_get(hogp) != BT_HIDS_PM_BOOT) 
	{
		printk("This function works only in BOOT Report mode\n");
		return;
	}
	
	capslock_state = capslock_state ? 0 : 1;
	
	data = capslock_state ? 0x02 : 0;
	
	err = bt_hogp_rep_write_wo_rsp(hogp, hogp->rep_boot.kbd_out,
				       &data, sizeof(data),
				       hidc_write_cb);
				       
	bt_conn_ctx_release(&conns_hid_ctx_lib, (void *) hogp);

	if (err) 
	{
		printk("Keyboard data write error (err: %d)\n", err);
		return;
	}
	
	printk("Caps lock send (val: 0x%x)\n", data);
}


static uint8_t capslock_read_cb(struct bt_hogp *hogp,
			     struct bt_hogp_rep_info *rep,
			     uint8_t err,
			     const uint8_t *data)
{
	if (err) {
		printk("Capslock read error (err: %u)\n", err);
		return BT_GATT_ITER_STOP;
	}
	if (!data) {
		printk("Capslock read - no data\n");
		return BT_GATT_ITER_STOP;
	}
	printk("Received data (size: %u, data[0]: 0x%x)\n",
	       bt_hogp_rep_size(rep), data[0]);

	return BT_GATT_ITER_STOP;
}


static void capslock_write_cb(struct bt_hogp *hogp,
			      struct bt_hogp_rep_info *rep,
			      uint8_t err)
{
	int ret;

	printk("Capslock write result: %u\n", err);

	ret = bt_hogp_rep_read(hogp, rep, capslock_read_cb);
	if (ret) {
		printk("Cannot read capslock value (err: %d)\n", ret);
	}
}


static void button_capslock_rsp(void)
{
	const struct bt_conn_ctx *ctx = bt_conn_ctx_get_by_id(&conns_hid_ctx_lib, 0);

	if (!ctx)
	{
		printk("HIDS CCL get error\n");
	}
	struct bt_hogp * hogp = ctx->data;

	if (!bt_hogp_ready_check(hogp)) 
	{
		printk("HID device not ready\n");
		return;
	}
	
	if (!hogp->rep_boot.kbd_out) 
	{
		printk("HID device does not have Keyboard OUT report\n");
		return;
	}
	
	int err;
	uint8_t data;

	capslock_state = capslock_state ? 0 : 1;
	data = capslock_state ? 0x02 : 0;
	err = bt_hogp_rep_write(hogp, hogp->rep_boot.kbd_out, capslock_write_cb,
				&data, sizeof(data));

	bt_conn_ctx_release(&conns_hid_ctx_lib, (void *) hogp);
	
	if (err) 
	{
		printk("Keyboard data write error (err: %d)\n", err);
		return;
	}
	
	printk("Caps lock send using write with response (val: 0x%x)\n", data);
}


static void num_comp_reply(bool accept)
{
	if (accept) {
		bt_conn_auth_passkey_confirm(auth_conn);
		printk("Numeric Match, conn %p\n", auth_conn);
	} else {
		bt_conn_auth_cancel(auth_conn);
		printk("Numeric Reject, conn %p\n", auth_conn);
	}

	bt_conn_unref(auth_conn);
	auth_conn = NULL;
}


static void button_handler(uint32_t button_state, uint32_t has_changed)
{
	uint32_t button = button_state & has_changed;

	if (auth_conn) {
		if (button & KEY_PAIRING_ACCEPT) {
			num_comp_reply(true);
		}

		if (button & KEY_PAIRING_REJECT) {
			num_comp_reply(false);
		}

		return;
	}

	if (button & KEY_BOOTMODE_MASK) {
		button_bootmode();
	}
	if (button & KEY_CAPSLOCK_MASK) {
		button_capslock();
	}
	if (button & KEY_CAPSLOCK_RSP_MASK) {
		button_capslock_rsp();
	}
}


static void auth_passkey_display(struct bt_conn *conn, unsigned int passkey)
{
	char addr[BT_ADDR_LE_STR_LEN];

	bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));

	printk("Passkey for %s: %06u\n", addr, passkey);
}


static void auth_passkey_confirm(struct bt_conn *conn, unsigned int passkey)
{
	char addr[BT_ADDR_LE_STR_LEN];

	auth_conn = bt_conn_ref(conn);

	bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));

	printk("Passkey for %s: %06u\n", addr, passkey);
	printk("Press Button 1 to confirm, Button 2 to reject.\n");
}


static void auth_cancel(struct bt_conn *conn)
{
	char addr[BT_ADDR_LE_STR_LEN];

	bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));

	printk("Pairing cancelled: %s\n", addr);
}


static void pairing_confirm(struct bt_conn *conn)
{
	char addr[BT_ADDR_LE_STR_LEN];

	bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));

	bt_conn_auth_pairing_confirm(conn);

	printk("Pairing confirmed: %s\n", addr);
}


static void pairing_complete(struct bt_conn *conn, bool bonded)
{
	char addr[BT_ADDR_LE_STR_LEN];

	bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));

	printk("Pairing completed: %s, bonded: %d\n", addr, bonded);
}


static void pairing_failed(struct bt_conn *conn, enum bt_security_err reason)
{
	char addr[BT_ADDR_LE_STR_LEN];

	bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));

	printk("Pairing failed conn: %s, reason %d\n", addr, reason);
}

static struct bt_conn_auth_cb conn_auth_callbacks = {
	.passkey_display = auth_passkey_display,
	.passkey_confirm = auth_passkey_confirm,
	.cancel = auth_cancel,
	.pairing_confirm = pairing_confirm,
	.pairing_complete = pairing_complete,
	.pairing_failed = pairing_failed
};

static bool le_param_req(struct bt_conn *conn, struct bt_le_conn_param *param)
{
	printk("Connection parameters update request received.\n");
	printk("Minimum interval: %d, Maximum interval: %d\n",
	       param->interval_min, param->interval_max);
	printk("Latency: %d, Timeout: %d\n", param->latency, param->timeout);

	return true;
}

static void le_param_updated(struct bt_conn *conn, uint16_t interval,
			     uint16_t latency, uint16_t timeout)
{
	printk("Connection parameters updated.\n"
	       " interval: %d, latency: %d, timeout: %d\n",
	       interval, latency, timeout);

	k_sem_give(&throughput_sem);
}

static void le_phy_updated(struct bt_conn *conn,
			   struct bt_conn_le_phy_info *param)
{
	printk("LE PHY updated: TX PHY %s, RX PHY %s\n",
	       phy2str(param->tx_phy), phy2str(param->rx_phy));

	k_sem_give(&throughput_sem);
}

static void le_data_length_updated(struct bt_conn *conn,
				   struct bt_conn_le_data_len_info *info)
{
	if (!data_length_req) {
		return;
	}

	printk("LE data len updated: TX (len: %d time: %d)"
	       " RX (len: %d time: %d)\n", info->tx_max_len,
	       info->tx_max_time, info->rx_max_len, info->rx_max_time);

	data_length_req = false;
	k_sem_give(&throughput_sem);
}

static uint8_t throughput_read(const struct bt_throughput_metrics *met)
{
	printk("[peer] received %u bytes (%u KB)"
	       " in %u GATT writes at %u bps\n",
	       met->write_len, met->write_len / 1024, met->write_count,
	       met->write_rate);

	k_sem_give(&throughput_sem);

	return BT_GATT_ITER_STOP;
}

static void throughput_received(const struct bt_throughput_metrics *met)
{
	static uint32_t kb;

	if (met->write_len == 0) {
		kb = 0;
		printk("\n");

		return;
	}

	if ((met->write_len / 1024) != kb) {
		kb = (met->write_len / 1024);
		printk("=");
	}
}

static void throughput_send(const struct bt_throughput_metrics *met)
{
	printk("\n[local] received %u bytes (%u KB)"
		" in %u GATT writes at %u bps\n",
		met->write_len, met->write_len / 1024,
		met->write_count, met->write_rate);
}


int test_run(const struct shell *shell,
	     const struct bt_le_conn_param *conn_param,
	     const struct bt_conn_le_phy_param *phy,
	     const struct bt_conn_le_data_len_param *data_len)
{
	//int err;
	//uint64_t stamp;
	//int64_t delta;
	//uint32_t data = 0;
	//uint32_t prog = 0;

	///* a dummy data buffer */
	//static char dummy[256];

	//if (!default_conn) {
	//	shell_error(shell, "Device is disconnected %s",
	//		    "Connect to the peer device before running test");
	//	return -EFAULT;
	//}

	//if (!test_ready) {
	//	shell_error(shell, "Device is not ready."
	//		"Please wait for the service discovery and MTU exchange end");
	//	return 0;
	//}

	//shell_print(shell, "\n==== Starting throughput test ====");

	//err = connection_configuration_set(shell, conn_param, phy, data_len);
	//if (err) {
	//	return err;
	//}

	///* reset peer metrics */
	//err = bt_throughput_write(&throughput, dummy, 1);
	//if (err) {
	//	shell_error(shell, "Reset peer metrics failed.");
	//	return err;
	//}

	///* Make sure that all BLE procedures are finished. */
	//k_sleep(K_MSEC(500));

	///* get cycle stamp */
	//stamp = k_uptime_get_32();

	//while (prog < IMG_SIZE) {
	//	err = bt_throughput_write(&throughput, dummy, 244);
	//	if (err) {
	//		shell_error(shell, "GATT write failed (err %d)", err);
	//		break;
	//	}

	//	/* print graphics */
	//	printk("%c", img[prog / IMG_X][prog % IMG_X]);
	//	data += 244;
	//	prog++;
	//}

	//delta = k_uptime_delta(&stamp);

	//printk("\nDone\n");
	//printk("[local] sent %u bytes (%u KB) in %lld ms at %llu kbps\n",
	//       data, data / 1024, delta, ((uint64_t)data * 8 / delta));

	///* read back char from peer */
	//err = bt_throughput_read(&throughput);
	//if (err) {
	//	shell_error(shell, "GATT read failed (err %d)", err);
	//	return err;
	//}

	//k_sem_take(&throughput_sem, THROUGHPUT_CONFIG_TIMEOUT);

	instruction_print();
	throughput_write_start();
	return 0;
}

void throughput_write_thread(void)
{
	int err;
	uint8_t write_data[WRITE_SIZE_BYTES];
	memset(write_data, 0xAB, sizeof(write_data));

	for (;;) 
	{	
		const struct bt_conn_ctx *ctx = bt_conn_ctx_get_by_id(&conns_ctx_lib, 0);
		struct bt_throughput *throughput = ctx->data;
		err = bt_throughput_write(throughput, write_data, WRITE_SIZE_BYTES);
		bt_conn_ctx_release(&conns_ctx_lib,(void *)ctx->data);
		if (err) 
		{
			printk("GATT write failed (err %d)", err);
		}
		k_msleep(10);
	}
}

void throughput_write_start(void)
{
	int ret;
	if (true) {
		throughput_write_thread_id = k_thread_create(&throughput_write_thread_data,
						    throughput_write_thread_stack,
						    1024,
						    (k_thread_entry_t)throughput_write_thread,
						    NULL, NULL, NULL,
						    3,
						    0,
						    K_NO_WAIT);
		ret = k_thread_name_set(throughput_write_thread_id, "THROUGHPUT_WRITE");
	}
}


void main(void)
{
	int err;

	static struct bt_conn_cb conn_callbacks = {
	    .connected = connected,
	    .disconnected = disconnected,
	    .le_param_req = le_param_req,
	    .le_param_updated = le_param_updated,
	    .le_phy_updated = le_phy_updated,
	    .le_data_len_updated = le_data_length_updated
	};

	printk("Starting Bluetooth Throughput example\n");

	bt_conn_cb_register(&conn_callbacks);

	err = bt_conn_auth_cb_register(&conn_auth_callbacks);
	if (err) {
		printk("failed to register authorization callbacks.\n");
		return;
	}

	err = bt_enable(NULL);
	if (err) {
		printk("Bluetooth init failed (err %d)\n", err);
		return;
	}

	printk("Bluetooth initialized\n");

	if (IS_ENABLED(CONFIG_SETTINGS)) 
	{
		settings_load();
	}

	scan_init();

	err = dk_buttons_init(button_handler);
	
	if (err) 
	{
		printk("Failed to initialize buttons (err %d)\n", err);
		return;
	}


	printk("\n");
	printk("Start master on the board.\n");
	scan_start();
}


/************************ (C) COPYRIGHT DAVID XU *****END OF FILE**************/






