/* main.c - Application main entry point */

//////////////////////////////////////////////////////////////////////////
//
// zephyr_central tryout based on zephyr samples
// 
// Jens Hofmann, 2023
//
// looks for BLE peripheral with niederfrequent service advertised
// connects and enables notifications on niederfrequent characteristic
//
//////////////////////////////////////////////////////////////////////////

#include <zephyr/console/console.h>
#include <string.h>
#include <stdlib.h>

#include <zephyr/bluetooth/crypto.h>
#include <bluetooth/services/throughput.h>
#include <bluetooth/scan.h>
#include <bluetooth/gatt_dm.h>

#include <zephyr/shell/shell_uart.h>

#include <dk_buttons_and_leds.h>
// ----
#include <zephyr/types.h>
#include <stddef.h>
#include <errno.h>
#include <zephyr/kernel.h>
#include <zephyr/sys/printk.h>
#include <zephyr/device.h>
#include <zephyr/devicetree.h>
#include <zephyr/drivers/gpio.h>

#include <zephyr/bluetooth/bluetooth.h>
#include <zephyr/bluetooth/hci.h>
#include <zephyr/bluetooth/conn.h>
#include <zephyr/bluetooth/uuid.h>
#include <zephyr/bluetooth/gatt.h>
#include <zephyr/sys/byteorder.h>

#define INTERVAL_MIN 0x140 /* 320 units, 400 ms */
#define INTERVAL_MAX 0x140 /* 320 units, 400 ms */
#define CONN_LATENCY 0

#define MIN_CONN_INTERVAL   6
#define MAX_CONN_INTERVAL   3200
#define SUPERVISION_TIMEOUT 1000

#define THROUGHPUT_CONFIG_TIMEOUT K_SECONDS(20)

static struct test_params {
	struct bt_le_conn_param *conn_param;
	struct bt_conn_le_phy_param *phy;
	struct bt_conn_le_data_len_param *data_len;
} test_params = {
	.conn_param = BT_LE_CONN_PARAM(INTERVAL_MIN, INTERVAL_MAX, CONN_LATENCY,
				       SUPERVISION_TIMEOUT),
	.phy = BT_CONN_LE_PHY_PARAM_2M,
	.data_len = BT_LE_DATA_LEN_PARAM_MAX
};

static K_SEM_DEFINE(throughput_sem, 0, 1);
static volatile bool data_length_req = false;

// devicetree node identifier for gpio0
#define GPIO_0 DT_NODELABEL(gpio0)		
// get the device tree binding for gpio0 check, if devicetree node is enabled at buildtime
#if DT_NODE_HAS_STATUS(GPIO_0, okay)
	const struct device *gpio0Dev = DEVICE_DT_GET(GPIO_0);
#else
#error "Node for GPIO is disabled"
#endif

// set GPIO-PIN for notfications
#define GPIO_NOTIFICATION		6
#define GPIO_NOTIFICATION_ALERT	7

// custom service
static const struct bt_uuid_128 niederfrequent_serivce_uuid =
// BT_UUID_INIT_128(0x03, 0x00, 0x13, 0xac,0x42, 0x02, 0x55, 0xBC, 0xEA, 0x11, 0xFB, 0x8D, 0x6C, 0x29, 0xE9, 0x65); // for WAVE
	BT_UUID_INIT_128('n', 'i', 'e', 'd', 'e', 'r', 'f', 'r', 'e', 'q', 'u', 'e', 'n', 't', 0x00, 0x00);

// Characteristic for custom service
static const struct bt_uuid_128 niederfrequent_characteristic_uuid =
// BT_UUID_INIT_128(0x03, 0x00, 0x13, 0xac,0x42, 0x02, 0x55, 0xBC, 0xEA, 0x11, 0xFB, 0x8D, 0xB1, 0x2B, 0xE9, 0x65); // for WAVE
BT_UUID_INIT_128('n', 'i', 'e', 'd', 'e', 'r', 'f', 'r', 'e', 'q', 'u', 'e', 'n', 't', 0x00, 0x01);

// MIDI Service UUID: 03B80E5A-EDE8-4B33-A751-6CE34EC4C700
// static const struct bt_uuid_128 MIDI_service_uuid =
// BT_UUID_INIT_128(0x00, 0xC7, 0xC4, 0x4E,0xE3, 0x6C, 0x51, 0xA7, 0x33, 0x4B, 0xE8, 0xED, 0x5A, 0x0E, 0xB8, 0x03);

static void start_scan(void);

static struct bt_conn *default_conn;

static struct bt_uuid_16 uuid = BT_UUID_INIT_16(0);
static struct bt_gatt_discover_params discover_params;
static struct bt_gatt_subscribe_params subscribe_params;

static volatile bool connection_active = false;

int setup_gpio_output(void)
{
	int ret = 0;
	if(!device_is_ready(gpio0Dev))
		ret = -ENODEV;
	else
	{
		ret = gpio_pin_configure(gpio0Dev, GPIO_NOTIFICATION, GPIO_OUTPUT_LOW | GPIO_ACTIVE_HIGH);
		if(!ret)
		{
			ret = gpio_pin_configure(gpio0Dev, GPIO_NOTIFICATION_ALERT, GPIO_OUTPUT_LOW | GPIO_ACTIVE_HIGH);
		}
	}

	return ret;
}

static uint8_t notify_func(struct bt_conn *conn,
			   struct bt_gatt_subscribe_params *params,
			   const void *data, uint16_t length)
{
	if (!data) {
		printk("[UNSUBSCRIBED]\n");
		params->value_handle = 0U;
		return BT_GATT_ITER_STOP;
	}

	gpio_pin_toggle(gpio0Dev, GPIO_NOTIFICATION);

//	printk("[NOTIFICATION] data %p length %u\n", data, length);
	
	return BT_GATT_ITER_CONTINUE;
}

static uint8_t discover_func(struct bt_conn *conn,
			     const struct bt_gatt_attr *attr,
			     struct bt_gatt_discover_params *params)
{
	int err;

	if (!attr) {
		printk("Discover complete\n");
		(void)memset(params, 0, sizeof(*params));
		return BT_GATT_ITER_STOP;
	}

	printk("[ATTRIBUTE] handle %u\n", attr->handle);

	if (!bt_uuid_cmp(discover_params.uuid, (const struct bt_uuid *)&niederfrequent_serivce_uuid)) 
	{
		printk("ooo discover_func service discovered, start searching for characteristics\n");
		discover_params.uuid = (const struct bt_uuid *)&niederfrequent_characteristic_uuid;
		discover_params.start_handle = attr->handle + 1;
		discover_params.type = BT_GATT_DISCOVER_CHARACTERISTIC;

		err = bt_gatt_discover(conn, &discover_params);
		if (err) 
		{
			printk("Discover failed (err %d)\n", err);
		}
	} 
	else if (!bt_uuid_cmp(discover_params.uuid,
				 (const struct bt_uuid *)&niederfrequent_characteristic_uuid)) 
		{
			printk("ooo discover_func characteristic discovered, start searching for CCC-services\n");
			memcpy(&uuid, BT_UUID_GATT_CCC, sizeof(uuid));
			discover_params.uuid = &uuid.uuid;
			discover_params.start_handle = attr->handle + 2;
			discover_params.type = BT_GATT_DISCOVER_DESCRIPTOR;
			// subscribe_params.value_handle = attr->handle + 1;
			subscribe_params.value_handle = bt_gatt_attr_value_handle(attr);

			err = bt_gatt_discover(conn, &discover_params);
			if (err)
			{
				printk("Discovery of CCC-service failed (err %d)\n", err);
			}
		} 
		else
		{
			printk("ooo subscribe to notifications\n");
			subscribe_params.notify = notify_func;
			subscribe_params.value = BT_GATT_CCC_NOTIFY;
			subscribe_params.ccc_handle = attr->handle;

			err = bt_gatt_subscribe(conn, &subscribe_params);
			if (err && err != -EALREADY) 
			{
				printk("Subscribe failed (err %d)\n", err);
			} 
			else 
			{
				printk("[SUBSCRIBED]\n");
			}

			return BT_GATT_ITER_STOP;
		}

	return BT_GATT_ITER_STOP;
}

#define BUF_LEN 64

static bool eir_found(struct bt_data *data, void *user_data)
{
	bt_addr_le_t *addr = user_data;
	char buf[BUF_LEN];
	int i;

	printk("[AD]: %u data_len %u\n", data->type, data->data_len);

	switch (data->type) {
	case BT_DATA_UUID128_SOME:
	case BT_DATA_UUID128_ALL:
		printk("\n=== found UUID128  ===\n");
		if (data->data_len % BT_UUID_SIZE_128 != 0U)
		{
			printk("AD malformed\n");
			return true;
		}

		int err;
		struct bt_le_conn_param *param;
		struct bt_uuid_128 uuid_temp;
		uuid_temp.uuid.type = BT_UUID_TYPE_128;
		memcpy(uuid_temp.val,data->data,BT_UUID_SIZE_128);
		bt_uuid_to_str((const struct bt_uuid *)&uuid_temp, buf, BUF_LEN - 1);
		printk("----------> UUID_128: %s\n", buf);
		bt_uuid_to_str((const struct bt_uuid *)&niederfrequent_serivce_uuid, buf, BUF_LEN - 1);
		printk("----------> niederfrequent uuid: %s\n", buf);


		k_sleep(K_MSEC(3000));

		if (!bt_uuid_cmp((const struct bt_uuid *)&uuid_temp, (const struct bt_uuid *)&niederfrequent_serivce_uuid))	// should be niederfrequent service uuid!
		{
			printk("***  niederfrequent service found! ***\n");

			err = bt_le_scan_stop();
			if (err) {
				printk("Stop LE scan failed (err %d)\n", err);
				return true;
			}

			param = BT_LE_CONN_PARAM_DEFAULT;
			param->interval_min = 6;
			param->interval_max = 6;
			err = bt_conn_le_create(addr, BT_CONN_LE_CREATE_CONN,
						param, &default_conn);
			if (err) {
				printk("Create conn failed (err %d)\n", err);
				start_scan();
			}

			return false;

		}

		return true;

	case BT_DATA_NAME_SHORTENED:
	case BT_DATA_NAME_COMPLETE:
		memcpy(buf, data->data, MIN(data->data_len, BUF_LEN - 1));
		buf[MIN(data->data_len, BUF_LEN - 1)] = 0;
		printk("\n=== found Name: %s ===\n", buf);
	/*
		if(!strcmp(buf,"Wave"))
		{
			int err;
			struct bt_le_conn_param *param;
			printk("Wave found - trying to connect...\n");
			err = bt_le_scan_stop();
			if (err) {
				printk("Stop LE scan failed (err %d)\n", err);
				return true;
			}

			param = BT_LE_CONN_PARAM_DEFAULT;
			param->interval_min = 6;
			param->interval_max = 6;
			err = bt_conn_le_create(addr, BT_CONN_LE_CREATE_CONN,
						param, &default_conn);
			if (err) {
				printk("Create conn failed (err %d)\n", err);
				start_scan();
			}

			return false;
		}
	*/	
		return true;

	case BT_DATA_UUID16_SOME:
	case BT_DATA_UUID16_ALL:
		printk("\n=== found UUID16 ===\n");
		if (data->data_len % sizeof(uint16_t) != 0U) {
			printk("AD malformed\n");
			return true;
		}

		for (i = 0; i < data->data_len; i += sizeof(uint16_t)) {
			struct bt_le_conn_param *param;
			struct bt_uuid *uuid;
			uint16_t u16;
			int err;

			memcpy(&u16, &data->data[i], sizeof(u16));
			printk("### Service 16 found %x\n", u16);

			uuid = BT_UUID_DECLARE_16(sys_le16_to_cpu(u16));
			if (bt_uuid_cmp(uuid,BT_UUID_HRS))  // HRS-Service
			{
				continue;
			}

			printk("### HRS Service found\n");

			err = bt_le_scan_stop();
			if (err) {
				printk("Stop LE scan failed (err %d)\n", err);
				continue;
			}

			param = BT_LE_CONN_PARAM_DEFAULT;
			err = bt_conn_le_create(addr, BT_CONN_LE_CREATE_CONN,
						param, &default_conn);
			if (err) {
				printk("Create conn failed (err %d)\n", err);
				start_scan();
			}

			return false;
		}
	}

	return true;
}

static void device_found(const bt_addr_le_t *addr, int8_t rssi, uint8_t type,
			 struct net_buf_simple *ad)
{
	char dev[BT_ADDR_LE_STR_LEN];
	
	bt_addr_le_to_str(addr, dev, sizeof(dev));
	printk("[DEVICE]: %s, AD evt type %u, AD data len %u, RSSI %i\n",
	       dev, type, ad->len, rssi);

	/* We're only interested in connectable events */
	if (type == BT_GAP_ADV_TYPE_ADV_IND ||
	    type == BT_GAP_ADV_TYPE_ADV_DIRECT_IND)
	{
		bt_data_parse(ad, eir_found, (void *)addr);
	}
}

static void start_scan(void)
{
	int err;

	/* Use active scanning and disable duplicate filtering to handle any
	 * devices that might update their advertising data at runtime. */
	struct bt_le_scan_param scan_param = {
		.type       = BT_LE_SCAN_TYPE_ACTIVE,
		.options    = BT_LE_SCAN_OPT_NONE,
		.interval   = BT_GAP_SCAN_FAST_INTERVAL,
		.window     = BT_GAP_SCAN_FAST_WINDOW,
	};

	err = bt_le_scan_start(&scan_param, device_found);
	if (err) {
		printk("Scanning failed to start (err %d)\n", err);
		return;
	}

	printk("Scanning successfully started\n");
}

static void connected(struct bt_conn *conn, uint8_t conn_err)
{
	char addr[BT_ADDR_LE_STR_LEN];
	// int err;

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

	if (conn_err) {
		printk("Failed to connect to %s (%u)\n", addr, conn_err);

		bt_conn_unref(default_conn);
		default_conn = NULL;
		connection_active = false;

		start_scan();
		return;
	}

	printk("Connected: %s\n", addr);

	connection_active = true;

/*
	if (conn == default_conn)
	{
		discover_params.uuid = (const struct bt_uuid *)&niederfrequent_serivce_uuid;
		discover_params.func = discover_func;
		discover_params.start_handle = BT_ATT_FIRST_ATTRIBUTE_HANDLE;
		discover_params.end_handle = BT_ATT_LAST_ATTRIBUTE_HANDLE;
		discover_params.type = BT_GATT_DISCOVER_PRIMARY;

		err = bt_gatt_discover(default_conn, &discover_params);
		if (err) {
			printk("Discover failed(err %d)\n", err);
			return;
		}
	}
*/
}

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

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

	printk("Disconnected: %s (reason 0x%02x)\n", addr, reason);

	if (default_conn != conn) {
		return;
	}

	bt_conn_unref(default_conn);
	default_conn = NULL;
	connection_active = false;

//	start_scan();
}

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\n");

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

BT_CONN_CB_DEFINE(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

};

void main(void)
{
	struct bt_conn_info info = {0};
//	struct bt_conn_le_data_len_param data_len;
	int err;
	err = bt_enable(NULL);

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

	printk("Bluetooth initialized\n");

	// Setup aux test points as outputs
	if(setup_gpio_output() < 0)
	{
		printk("Device %s is not ready for setting notification pins as output\n", gpio0Dev->name);
		return;
	}

	default_conn = NULL;
	connection_active = false;

	while(1)
	{
		start_scan();

		while(!connection_active)	// wait for connection to be established
			k_sleep(K_MSEC(1));		// let other threads work, too

		if (default_conn)
		{
			err = bt_conn_get_info(default_conn, &info);
			if (err) 
			{
				printk("ERROR: Failed to get connection info %d\n", err);
			}
			else
			{
				if (info.role != BT_CONN_ROLE_CENTRAL)
				{
					printk("this is not configured as BLE central\n");
				}
				else
				{
					printk("the current tx data length setting is %d\n",info.le.data_len->tx_max_len);
					printk("the current tx time setting is %d\n",info.le.data_len->tx_max_time);
				}
			}

			printk("=> switch to DLE mode, if possible...\n");

			data_length_req = true;

			test_params.data_len->tx_max_len = 80;
			test_params.data_len->tx_max_time = 2120;

			printk("test_params.data_len->tx_max_len: %d\n", test_params.data_len->tx_max_len);
			printk("test_params.data_len->tx_max_time: %d\n", test_params.data_len->tx_max_time);
			err = bt_conn_le_data_len_update(default_conn, test_params.data_len);
			if (err)
			{
				printk("ERROR: LE data length update failed: %d", err);
			}
			else
			{
				printk("LE Data length update pending...\n");
				err = k_sem_take(&throughput_sem, THROUGHPUT_CONFIG_TIMEOUT);
				if (err)
				{
					printk("ERROR: LE Data Length update timeout");
				}
				else
				{
					printk("*** data length has been updated! ***\n");

					discover_params.uuid = (const struct bt_uuid *)&niederfrequent_serivce_uuid;
					discover_params.func = discover_func;
					discover_params.start_handle = BT_ATT_FIRST_ATTRIBUTE_HANDLE;
					discover_params.end_handle = BT_ATT_LAST_ATTRIBUTE_HANDLE;
					discover_params.type = BT_GATT_DISCOVER_PRIMARY;

					err = bt_gatt_discover(default_conn, &discover_params);
					if (err)
					{
						printk("Discover failed(err %d)\n", err);
					}
				}
			}
			
			err = bt_conn_get_info(default_conn, &info);
			if (err) 
			{
				printk("ERROR: Failed to get connection info after length update %d\n", err);
			}
			else
			{
					printk("the current tx data length setting after update is %d\n",info.le.data_len->tx_max_len);
					printk("the current tx time setting is after update %d\n",info.le.data_len->tx_max_time);
			}

		}

		while(connection_active)	// stay connected and do not scan again, until connection is finished
			k_sleep(K_MSEC(1));		// let other threads work, too
	}

}