/*
 * Copyright (c) 2023 Nordic Semiconductor ASA
 *
 * SPDX-License-Identifier: Apache-2.0
 */

#include <stdint.h>
#include <string.h>

#include <zephyr/kernel.h>

#include <zephyr/net_buf.h>
#include <zephyr/sys/printk.h>
#include <zephyr/logging/log.h>

#include <zephyr/bluetooth/ead.h>
#include <zephyr/bluetooth/att.h>
#include <zephyr/bluetooth/gatt.h>
#include <zephyr/bluetooth/addr.h>
#include <zephyr/bluetooth/conn.h>
#include <zephyr/bluetooth/uuid.h>
#include <zephyr/bluetooth/hci.h>
#include <zephyr/bluetooth/bluetooth.h>

#include "common/bt_str.h"

#include "common.h"

LOG_MODULE_REGISTER(ead_central_sample, CONFIG_BT_EAD_LOG_LEVEL);

static uint8_t *received_data;
static size_t received_data_size;
static struct key_material keymat;

static bt_addr_le_t peer_addr;
static struct bt_conn *default_conn;

static struct bt_conn_cb central_cb;
static struct bt_conn_auth_cb central_auth_cb;

static struct k_poll_signal conn_signal;
static struct k_poll_signal passkey_enter_signal;
static struct k_poll_signal device_found_cb_completed;

/* GATT Discover data */
static uint8_t gatt_disc_err;
static uint16_t gatt_disc_end_handle;
static uint16_t gatt_disc_start_handle;
static struct k_poll_signal gatt_disc_signal;

/* GATT Read data */
static uint8_t gatt_read_err;
static uint8_t *gatt_read_res;
static uint16_t gatt_read_len;
static uint16_t gatt_read_handle;
static struct k_poll_signal gatt_read_signal;

static bool data_parse_cb(struct bt_data *data, void *user_data)
{
	size_t *parsed_data_size = (size_t *)user_data;

	if (data->type == BT_DATA_ENCRYPTED_AD_DATA) {
		int err;
		struct net_buf_simple decrypted_buf;
		size_t decrypted_data_size = BT_EAD_DECRYPTED_PAYLOAD_SIZE(data->data_len);
		uint8_t decrypted_data[decrypted_data_size];

		err = bt_ead_decrypt(keymat.session_key, keymat.iv, data->data, data->data_len,
				     decrypted_data);
		if (err < 0) {
			LOG_ERR("Error during decryption (err %d)", err);
		}

		net_buf_simple_init_with_data(&decrypted_buf, &decrypted_data[0],
					      decrypted_data_size);

		bt_data_parse(&decrypted_buf, &data_parse_cb, user_data);
	} else {
		LOG_INF("len : %u", data->data_len);
		LOG_INF("type: 0x%02x", data->type);
		LOG_HEXDUMP_INF(data->data, data->data_len, "data:");

		/* Copy the data out if we are running in a test context */
		if (received_data != NULL) {
			if (bt_data_get_len(data, 1) <=
			    (received_data_size - (*parsed_data_size))) {
				*parsed_data_size +=
					bt_data_serialize(data, &received_data[*parsed_data_size]);
			}
		} else {
			*parsed_data_size += bt_data_get_len(data, 1);
		}
	}

	return true;
}

static void device_found(const bt_addr_le_t *addr, int8_t rssi, uint8_t type,
			 struct net_buf_simple *ad)
{
	int err;
	size_t parsed_data_size;
	char addr_str[BT_ADDR_LE_STR_LEN];

	if (default_conn) {
		return;
	}

	bt_addr_le_to_str(addr, addr_str, sizeof(addr_str));

	/* We are only interested in the previously connected device. */
	if (!bt_addr_le_eq(addr, &peer_addr)) {
		return;
	}

	LOG_DBG("Peer found.");

	parsed_data_size = 0;
	LOG_INF("Received data size: %zu", ad->len);
	bt_data_parse(ad, data_parse_cb, &parsed_data_size);

	LOG_DBG("All data parsed. (total size: %zu)", parsed_data_size);

	err = bt_le_scan_stop();
	if (err) {
		LOG_DBG("Failed to stop scanner (err %d)", err);
		return;
	}

	k_poll_signal_raise(&device_found_cb_completed, 0);
}

static void connect_device_found(const bt_addr_le_t *addr, int8_t rssi, uint8_t type,
				 struct net_buf_simple *ad)
{
	int err;
	char addr_str[BT_ADDR_LE_STR_LEN];

	if (default_conn) {
		return;
	}

	/* Connect only to devices in close range */
	if (rssi < -70) {
		return;
	}

	bt_addr_le_to_str(addr, addr_str, sizeof(addr_str));

	LOG_DBG("Device found: %s (RSSI %d)", addr_str, rssi);

	err = bt_le_scan_stop();
	if (err) {
		LOG_DBG("Failed to stop scanner (err %d)", err);
		return;
	}

	err = bt_conn_le_create(addr, BT_CONN_LE_CREATE_CONN, BT_LE_CONN_PARAM_DEFAULT,
				&default_conn);
	if (err) {
		LOG_DBG("Failed to connect to %s (err %d)", addr_str, err);
		return;
	}

	k_poll_signal_raise(&device_found_cb_completed, 0);
}

static int start_scan(bool connect)
{
	int err;

	k_poll_signal_reset(&conn_signal);
	k_poll_signal_reset(&device_found_cb_completed);

	err = bt_le_scan_start(BT_LE_SCAN_PASSIVE, connect ? connect_device_found : device_found);
	if (err) {
		LOG_DBG("Scanning failed to start (err %d)", err);
		return -1;
	}

	LOG_DBG("Scanning started.");

	if (connect) {
		LOG_DBG("Waiting for connection");
		await_signal(&conn_signal);
	}

	await_signal(&device_found_cb_completed);

	return 0;
}

static uint8_t gatt_read_cb(struct bt_conn *conn, uint8_t att_err,
			    struct bt_gatt_read_params *params, const void *data, uint16_t read_len)
{
	gatt_read_err = att_err;
	gatt_read_len = read_len;
	gatt_read_handle = params->by_uuid.start_handle;

	if (!att_err) {
		memcpy(gatt_read_res, data, read_len);
		k_poll_signal_raise(&gatt_read_signal, 0);
	} else {
		LOG_ERR("ATT error (err %d)", att_err);
	}

	return BT_GATT_ITER_STOP;
}

static int gatt_read(struct bt_conn *conn, const struct bt_uuid *uuid, size_t read_size,
		     uint16_t start_handle, uint16_t end_handle, uint8_t *buf)
{
	int err;
	size_t offset;
	uint16_t handle;
	struct bt_gatt_read_params params;

	gatt_read_res = &buf[0];

	params.handle_count = 0;
	params.by_uuid.start_handle = start_handle;
	params.by_uuid.end_handle = end_handle;
	params.by_uuid.uuid = uuid;
	params.func = gatt_read_cb;

	k_poll_signal_reset(&gatt_read_signal);

	err = bt_gatt_read(conn, &params);
	if (err) {
		LOG_DBG("GATT read failed (err %d)", err);
		return -1;
	}

	await_signal(&gatt_read_signal);

	offset = gatt_read_len;
	handle = gatt_read_handle;

	while (offset < read_size) {
		gatt_read_res = &buf[offset];

		params.handle_count = 1;
		params.single.handle = handle;
		params.single.offset = offset;

		k_poll_signal_reset(&gatt_read_signal);

		err = bt_gatt_read(conn, &params);
		if (err) {
			LOG_DBG("GATT read failed (err %d)", err);
			return -1;
		}

		await_signal(&gatt_read_signal);

		offset += gatt_read_len;
	}

	return 0;
}

static uint8_t gatt_discover_cb(struct bt_conn *conn, const struct bt_gatt_attr *attr,
				struct bt_gatt_discover_params *params)
{
	gatt_disc_err = attr ? 0 : BT_ATT_ERR_ATTRIBUTE_NOT_FOUND;

	if (attr) {
		gatt_disc_start_handle = attr->handle;
		gatt_disc_end_handle = ((struct bt_gatt_service_val *)attr->user_data)->end_handle;
	}

	k_poll_signal_raise(&gatt_disc_signal, 0);

	return BT_GATT_ITER_STOP;
}

static int gatt_discover_primary_service(struct bt_conn *conn, const struct bt_uuid *service_type,
					 uint16_t *start_handle, uint16_t *end_handle)
{
	int err;
	struct bt_gatt_discover_params params;

	params.type = BT_GATT_DISCOVER_PRIMARY;
	params.start_handle = BT_ATT_FIRST_ATTRIBUTE_HANDLE;
	params.end_handle = BT_ATT_LAST_ATTRIBUTE_HANDLE;
	params.uuid = service_type;
	params.func = gatt_discover_cb;

	k_poll_signal_reset(&gatt_disc_signal);

	err = bt_gatt_discover(conn, &params);
	if (err) {
		LOG_DBG("Primary service discover failed (err %d)", err);
		return -1;
	}

	await_signal(&gatt_disc_signal);

	*start_handle = gatt_disc_start_handle;
	*end_handle = gatt_disc_end_handle;

	return gatt_disc_err;
}

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

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

	if (conn_err) {
		LOG_DBG("Failed to connect to %s (err %u)", addr, conn_err);

		bt_conn_unref(default_conn);
		default_conn = NULL;

		(void)start_scan(true);

		return;
	}

	LOG_DBG("Connected to: %s", addr);

	k_poll_signal_raise(&conn_signal, 0);
}

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

	LOG_DBG("Disconnected: %s, reason 0x%02x %s", addr, reason, bt_hci_err_to_str(reason));

	if (default_conn != conn) {
		return;
	}

	bt_conn_unref(default_conn);
	default_conn = NULL;
}

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) {
		LOG_DBG("Security changed: %s level %u", addr, level);
	} else {
		LOG_DBG("Security failed: %s level %u err %d %s", addr, level,
			err, bt_security_err_to_str(err));
	}
}

static void identity_resolved(struct bt_conn *conn, const bt_addr_le_t *rpa,
			      const bt_addr_le_t *identity)
{
	char addr_identity[BT_ADDR_LE_STR_LEN];
	char addr_rpa[BT_ADDR_LE_STR_LEN];

	bt_addr_le_to_str(identity, addr_identity, sizeof(addr_identity));
	bt_addr_le_to_str(rpa, addr_rpa, sizeof(addr_rpa));

	LOG_DBG("Identity resolved %s -> %s", addr_rpa, addr_identity);

	bt_addr_le_copy(&peer_addr, identity);
}

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

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

	snprintk(passkey_str, ARRAY_SIZE(passkey_str), "%06u", passkey);

	printk("Passkey for %s: %s\n", addr, passkey_str);

	k_poll_signal_raise(&passkey_enter_signal, 0);
}

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

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

	snprintk(passkey_str, ARRAY_SIZE(passkey_str), "%06u", passkey);

	LOG_DBG("Passkey for %s: %s", addr, passkey_str);
}

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

	LOG_DBG("Pairing cancelled: %s", addr);
}

static int init_bt(void)
{
	int err;

	default_conn = NULL;

	k_poll_signal_init(&conn_signal);
	k_poll_signal_init(&passkey_enter_signal);
	k_poll_signal_init(&gatt_disc_signal);
	k_poll_signal_init(&gatt_read_signal);
	k_poll_signal_init(&device_found_cb_completed);

	err = bt_enable(NULL);
	if (err) {
		LOG_ERR("Bluetooth init failed (err %d)", err);
		return -1;
	}

	LOG_DBG("Bluetooth initialized");

	err = bt_unpair(BT_ID_DEFAULT, BT_ADDR_LE_ANY);
	if (err) {
		LOG_ERR("Unpairing failed (err %d)", err);
	}

	central_cb.connected = connected;
	central_cb.disconnected = disconnected;
	central_cb.security_changed = security_changed;
	central_cb.identity_resolved = identity_resolved;

	bt_conn_cb_register(&central_cb);

	central_auth_cb.pairing_confirm = NULL;
	central_auth_cb.passkey_confirm = auth_passkey_confirm;
	central_auth_cb.passkey_display = auth_passkey_display;
	central_auth_cb.passkey_entry = NULL;
	central_auth_cb.oob_data_request = NULL;
	central_auth_cb.cancel = auth_cancel;

	err = bt_conn_auth_cb_register(&central_auth_cb);
	if (err) {
		return -1;
	}

	return 0;
}

int run_central_sample(int get_passkey_confirmation(struct bt_conn *conn),
		       uint8_t *test_received_data, size_t test_received_data_size,
		       struct key_material *test_received_keymat)
{
	int err;
	bool connect;
	uint16_t end_handle;
	uint16_t start_handle;

	if (test_received_data != NULL) {
		received_data = test_received_data;
		received_data_size = test_received_data_size;
	}

	/* Initialize Bluetooth and callbacks */
	err = init_bt();
	if (err) {
		return -1;
	}

	/* Start scan and connect to our peripheral */
	connect = true;
	err = start_scan(connect);
	if (err) {
		return -2;
	}

	/* Update connection security level */
	err = bt_conn_set_security(default_conn, BT_SECURITY_L4);
	if (err) {
		LOG_ERR("Failed to set security (err %d)", err);
		return -3;
	}

	await_signal(&passkey_enter_signal);

	err = get_passkey_confirmation(default_conn);
	if (err) {
		LOG_ERR("Security update failed");
		return -4;
	}

	/* Locate the primary service */
	err = gatt_discover_primary_service(default_conn, BT_UUID_CUSTOM_SERVICE, &start_handle,
					    &end_handle);
	if (err) {
		LOG_ERR("Service not found (err %d)", err);
		return -5;
	}

	/* Read the Key Material characteristic */
	err = gatt_read(default_conn, BT_UUID_GATT_EDKM, sizeof(keymat), start_handle, end_handle,
			(uint8_t *)&keymat);
	if (err) {
		LOG_ERR("GATT read failed (err %d)", err);
		return -6;
	}

	LOG_HEXDUMP_DBG(keymat.session_key, BT_EAD_KEY_SIZE, "Session Key");
	LOG_HEXDUMP_DBG(keymat.iv, BT_EAD_IV_SIZE, "IV");

	if (test_received_keymat != NULL) {
		memcpy(test_received_keymat, &keymat, sizeof(keymat));
	}

	/* Start a new scan to get and decrypt the Advertising Data */
	err = bt_conn_disconnect(default_conn, BT_HCI_ERR_REMOTE_USER_TERM_CONN);
	if (err) {
		LOG_ERR("Failed to disconnect.");
		return -7;
	}

	connect = false;
	err = start_scan(connect);
	if (err) {
		return -2;
	}

	return 0;
}