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

#include <zephyr/types.h>
#include <stddef.h>
#include <errno.h>
#include <zephyr/kernel.h>
#include <zephyr/sys/printk.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>

/* UUID Definitions */
#define BT_UUID_FTS_VAL      BT_UUID_128_ENCODE(0x316aacf0, 0xa926, 0x48e7, 0x8d7b, 0xa678a1b0feff)
#define BT_UUID_FTS_RX_VAL   BT_UUID_128_ENCODE(0x316aacf1, 0xa926, 0x48e7, 0x8d7b, 0xa678a1b0feff)

static struct bt_uuid_128 fts_service_uuid = BT_UUID_INIT_128(BT_UUID_FTS_VAL);
static struct bt_uuid_128 fts_rx_uuid      = BT_UUID_INIT_128(BT_UUID_FTS_RX_VAL);

struct bt_conn *btconn;
static uint16_t fts_rx_handle;
static struct bt_gatt_discover_params discover_params;

static struct k_timer mtimer;
static struct bt_conn *default_conn;

static void start_scan(void);

/* Function to send data to the discovered FTS RX handle */
int bt_fts_client_send(const uint8_t *data, uint16_t len)
{
	if (!btconn || !fts_rx_handle) {
		printk("FTS RX handle not discovered or not connected\n");
		return -EIO;
	}

	int err = bt_gatt_write_without_response(btconn, fts_rx_handle, data, len, false);
	printk("bt_gatt_write_without_response len:%d 0x%04x: %d\n", len, fts_rx_handle, err);

	return err;
}

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 we found the Service, now look for the Characteristic */
	if (!bt_uuid_cmp(params->uuid, &fts_service_uuid.uuid)) {
		printk("Found FTS Service, discovering RX Characteristic...\n");
		discover_params.uuid = &fts_rx_uuid.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("Characteristic Discover failed (err %d)\n", err);
		}
		return BT_GATT_ITER_STOP;
	} 
	
	/* If we found the Characteristic, store its value handle */
	if (!bt_uuid_cmp(params->uuid, &fts_rx_uuid.uuid)) {
		fts_rx_handle = bt_gatt_attr_value_handle(attr);
		printk("Found FTS RX handle (UUID: ...f1): 0x%04x\n", fts_rx_handle);
		return BT_GATT_ITER_STOP;
	}

	return BT_GATT_ITER_CONTINUE;
}

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

	fts_rx_handle = 0;
	discover_params.uuid = &fts_service_uuid.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(conn, &discover_params);
	if (err) {
		printk("Service Discover failed (err %d)\n", err);
		return;
	}

	printk("Discovery started for Service ...f0\n");
}

static void timeout_handler( struct k_timer *timer )
{
    static uint8_t data[155] ={0};
    static uint16_t count=0;
	
    if (fts_rx_handle) {
        printk("send sequence %d\n", count);
        data[0] = count++;
        bt_fts_client_send(data, sizeof(data));
        bt_fts_client_send(data, sizeof(data));
        bt_fts_client_send(data, sizeof(data));
        bt_fts_client_send(data, sizeof(data));
        bt_fts_client_send(data, sizeof(data));
        bt_fts_client_send(data, sizeof(data));
        bt_fts_client_send(data, sizeof(data));
        bt_fts_client_send(data, sizeof(data));
    } else {
        printk("Waiting for handle discovery...\n");
    }
}

/* Scan and Connection Logic */
static bool data_cb(struct bt_data *data, void *user_data)
{
    char *name = user_data;
    if (data->type == BT_DATA_NAME_SHORTENED || data->type == BT_DATA_NAME_COMPLETE) {
        memcpy(name, data->data, MIN(data->data_len, 31));
        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 addr_str[BT_ADDR_LE_STR_LEN];
	int err;

	if (default_conn || rssi < -50) {
		return;
	}

	if (type != BT_GAP_ADV_TYPE_ADV_IND && type != BT_GAP_ADV_TYPE_SCAN_RSP) {
		return;
	}

    char name[32] = {0};
    bt_data_parse(ad, data_cb, name);

    if (name[0] != 0 && !strcmp("NordicTEST001", name)) {
        bt_addr_le_to_str(addr, addr_str, sizeof(addr_str));
        printk("Found target: %s (RSSI %d)\n", name, rssi);
        
        bt_le_scan_stop();
        err = bt_conn_le_create(addr, BT_CONN_LE_CREATE_CONN,
                                BT_LE_CONN_PARAM_DEFAULT, &default_conn);
        if (err) {
            printk("Connection failed (err %d)\n", err);
            start_scan();
        }
    }
}

static void start_scan(void)
{
	int err = bt_le_scan_start(BT_LE_SCAN_ACTIVE, device_found);
	if (err) {
		printk("Scanning failed (err %d)\n", err);
	} else {
		printk("Scanning started\n");
	}
}

static void exchange_func(struct bt_conn *conn, uint8_t err,
			  struct bt_gatt_exchange_params *params)
{
	if (err) {
		printk("MTU exchange failed (err %u)\n", err);
	} else {
		printk("MTU exchange complete: %u bytes\n", bt_gatt_get_mtu(conn));
	}
    
    k_timer_start(&mtimer, K_MSEC(2000), K_MSEC(1000));
}

static struct bt_gatt_exchange_params exchange_params;

static void connected(struct bt_conn *conn, uint8_t err)
{
	if (err) {
		printk("Connect failed (err %u)\n", err);
		bt_conn_unref(default_conn);
		default_conn = NULL;
		start_scan();
		return;
	}

	printk("Connected\n");
    btconn = conn;

    /* Request Data Length Update (DLE) */
    struct bt_conn_le_data_len_param data_len_params = {
        .tx_max_len = 162,
        .tx_max_time = 7500,
    };
    err = bt_conn_le_data_len_update(conn, &data_len_params);
    if (err) {
        printk("Data length update request failed (err %d)\n", err);
    } else {
        printk("Data length update requested\n");
    }

    /* Exchange MTU */
    exchange_params.func = exchange_func;
    err = bt_gatt_exchange_mtu(conn, &exchange_params);
    if (err) {
        printk("MTU exchange failed (err %d)\n", err);
        /* Fallback: start timer anyway */
        k_timer_start(&mtimer, K_MSEC(2000), K_MSEC(1000));
    }

    gatt_discover(conn);
}

static void disconnected(struct bt_conn *conn, uint8_t reason)
{
	printk("Disconnected (reason 0x%02x)\n", reason);
	bt_conn_unref(default_conn);
	default_conn = NULL;
    btconn = NULL;
    fts_rx_handle = 0;
	start_scan();
}

static void data_len_updated(struct bt_conn *conn,
				 struct bt_conn_le_data_len_info *info)
{
	printk("Data length updated: TX %u/%u bytes, RX %u/%u bytes\n",
	       info->tx_max_len, info->tx_max_time,
	       info->rx_max_len, info->rx_max_time);
}

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

int main(void)
{
	int err;
    k_timer_init(&mtimer, timeout_handler, NULL);

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

	printk("Bluetooth initialized\n");
	start_scan();
	return 0;
}
