#include <zephyr/kernel.h>
#include <zephyr/logging/log.h>
#include <zephyr/bluetooth/bluetooth.h>
#include <zephyr/bluetooth/conn.h>
#include <zephyr/bluetooth/gatt.h>
#include <zephyr/bluetooth/uuid.h>
#include <zephyr/bluetooth/gap.h>
#include <zephyr/sys/byteorder.h>
#include <zephyr/sys/printk.h>
#include <string.h>
#include <math.h>
#include <dk_buttons_and_leds.h>

#include "my_custom.h"
#include "compute_distance.h"

LOG_MODULE_REGISTER(central_ble, LOG_LEVEL_INF);

#define TARGET_NAME "MY_BLE_Lector"
#define DEVICE_NAME_MAX_LEN 30

#define TEST_STATUS_LED DK_LED4

static struct bt_conn *default_conn;
static bool connecting = false;
static uint16_t svc_start = 0, svc_end = 0;
static uint16_t write_char_handle = 0;
static bool write_pending = false;

static struct bt_uuid_128 custom_service_uuid = BT_UUID_INIT_128(BT_UUID_CUSTOM_SERVICE_VAL);
static struct bt_uuid_128 write_char_uuid = BT_UUID_INIT_128(BT_UUID_CUSTOM_WRITE_VAL);
static struct bt_uuid_128 confirm_char_uuid = BT_UUID_INIT_128(BT_UUID_CUSTOM_CONFIRM_VAL);

static struct bt_gatt_write_params write_params;
static struct bt_gatt_subscribe_params subscribe_params;

static void write_callback(struct bt_conn *conn, uint8_t err,
			   struct bt_gatt_write_params *params)
{
	write_pending = false;
	if (err) {
		printk("Write failed : 0x%02X\n", err);
	} else {
		printk("Write success !\n");
	}

	if (conn)	{
		bt_conn_disconnect(conn, BT_HCI_ERR_REMOTE_USER_TERM_CONN);
	}
}

static void send_value(uint16_t handle)
{
	if (!default_conn || write_char_handle == 0) {
		printk("Connexion ou handle non disponible\n");
		return;
	}
	static uint32_t value = 5678;

	write_params.handle = handle;
	write_params.offset = 0;
	write_params.data = &value;
	write_params.length = sizeof(value);
	write_params.func = write_callback;

	int err = bt_gatt_write(default_conn, &write_params);
	if (err) {
		printk("bt_gatt_write() failed : %d\n", err);
	}
}

static uint8_t notify_confirm_handler(struct bt_conn *conn,
    struct bt_gatt_subscribe_params *params,
    const void *data, uint16_t length)
{
    if (!data) 	{
        return BT_GATT_ITER_STOP;
	}

    uint32_t received;
    memcpy(&received, data, sizeof(received));

	struct bt_conn *conn_snapshot = default_conn;

	if (received == 0x00000000) {
		// Send value only if not already pending
		if (!write_pending && conn_snapshot) {
			write_pending = true;
			if (is_device_nearby(conn_snapshot))	{
				send_value(write_char_handle);
			}
			else {
				printk("Device not in range\n");
				bt_conn_disconnect(conn_snapshot, BT_HCI_ERR_REMOTE_USER_TERM_CONN);
			}
        }
		return BT_GATT_ITER_CONTINUE;
	}

    printk("Confirmation received : %u\n", received);

    // Allumer LED locale ici (exemple sur DK Nordic avec dk_buttons_and_leds)
    dk_set_led_on(TEST_STATUS_LED);
    return BT_GATT_ITER_CONTINUE;
}

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

    if (!attr) {
        printk("Confirm characteristic not found\n");
        return BT_GATT_ITER_STOP;
    }

    const struct bt_gatt_chrc *gatt_chrc = attr->user_data;

    subscribe_params.notify = notify_confirm_handler;
    subscribe_params.value = BT_GATT_CCC_NOTIFY;
    subscribe_params.value_handle = gatt_chrc->value_handle;
    subscribe_params.ccc_handle = gatt_chrc->value_handle + 1;

    err = bt_gatt_subscribe(conn, &subscribe_params);
    if (err) {
        printk("Subscribe error : %d\n", err);
    } else {
        printk("Subscribed to CONFIRM characteristic\n");
    }

    return BT_GATT_ITER_STOP;
}

static uint8_t discover_write_char_cb(struct bt_conn *conn, const struct bt_gatt_attr *attr,
				      struct bt_gatt_discover_params *params)
{
	static struct bt_gatt_discover_params discover_confirm_params;

	if (!attr) {
		printk("Write characteristic not found\n");
		return BT_GATT_ITER_STOP;
	}

	const struct bt_gatt_chrc *chrc = attr->user_data;
	printk("Write characteristic found, handle = 0x%04x\n", chrc->value_handle);

	//Discover confirm characteristic
	discover_confirm_params.uuid = &confirm_char_uuid.uuid;
    discover_confirm_params.start_handle = svc_start;
    discover_confirm_params.end_handle = svc_end;
    discover_confirm_params.type = BT_GATT_DISCOVER_CHARACTERISTIC;
    discover_confirm_params.func = discover_confirm_char_func;	

	write_char_handle = chrc->value_handle;

    return bt_gatt_discover(conn, &discover_confirm_params);
}

static uint8_t discover_service_cb(struct bt_conn *conn, const struct bt_gatt_attr *attr,
				   struct bt_gatt_discover_params *params)
{
	static struct bt_gatt_discover_params discover_write_params;

	if (!attr) {
		printk("Service not found\n");
		return BT_GATT_ITER_STOP;
	}

	const struct bt_gatt_service_val *service = attr->user_data;
	svc_start = attr->handle + 1;
    svc_end = service->end_handle;
	printk("Service found from 0x%04x to 0x%04x\n", svc_start, svc_end);

	//Discover write characteristic
	discover_write_params.uuid = &write_char_uuid.uuid;
	discover_write_params.start_handle = svc_start;
	discover_write_params.end_handle = svc_end;
	discover_write_params.type = BT_GATT_DISCOVER_CHARACTERISTIC;
	discover_write_params.func = discover_write_char_cb;

	return bt_gatt_discover(conn, &discover_write_params);
}

static void connected(struct bt_conn *conn, uint8_t err)
{
	if (err) {
		printk("Connection failed : %u\n", err);
		connecting = false;
		return;
	}

	printk("Connected !\n");
	default_conn = bt_conn_ref(conn);

	static struct bt_gatt_discover_params discover_params = {
		.uuid = &custom_service_uuid.uuid,
		.start_handle = 0x0001,
		.end_handle = 0xffff,
		.type = BT_GATT_DISCOVER_PRIMARY,
		.func = discover_service_cb,
	};
	
	bt_gatt_discover(conn, &discover_params);
}

static void disconnected(struct bt_conn *conn, uint8_t reason)
{
	printk("Disconnected (reason %u)\n", reason);

	// Cancel any pending operations
    if (write_pending) {
        bt_gatt_cancel(conn, &write_params);
        write_pending = false;
    }

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

BT_CONN_CB_DEFINE(conn_callbacks) = {
	.connected = connected,
	.disconnected = disconnected,
};

static bool parse_name(struct bt_data *data, void *user_data)
{
	char *name = user_data;

	if (data->type == BT_DATA_NAME_COMPLETE || data->type == BT_DATA_NAME_SHORTENED) {
		size_t len = MIN(data->data_len, DEVICE_NAME_MAX_LEN);
		memcpy(name, data->data, len);
		name[len] = '\0';
		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 name[DEVICE_NAME_MAX_LEN + 1] = {0};

	bt_data_parse(ad, parse_name, name);

	if (connecting || strcmp(name, TARGET_NAME) != 0) {
		return;
	}

	printk("Peripheral %s found, connection...\n", name);
	connecting = true;
	bt_le_scan_stop();

	struct bt_conn_le_create_param create_param = BT_CONN_LE_CREATE_PARAM_INIT(
		BT_CONN_LE_OPT_NONE, BT_GAP_SCAN_FAST_INTERVAL, BT_GAP_SCAN_FAST_INTERVAL);

	bt_conn_le_create(addr, &create_param, BT_LE_CONN_PARAM_DEFAULT, &default_conn);
}

int main(void)
{
	int err;

	err = dk_leds_init();
	if (err) {
		LOG_ERR("LEDs init failed (err %d)\n", err);
		return -1;
	}

	printk("BLE central starting...\n");

	err = bt_enable(NULL);
	if (err) {
		printk("Bluetooth error : %d\n", err);
		return 0;
	}
	printk("Scanning in progress...\n");
	bt_le_scan_start(BT_LE_SCAN_PASSIVE, device_found);


    return 0;
}
