// SDK:         nRF Connect SDK v3.2.1
// Toolchain:   nRF Connect SDK Toolchain v3.2.1
// Zephyr: 		4.2.99
// board:       xiao_nrf54l15/nrf54l15/cpuapp
// 2026/02/07

#include <stdio.h>
#include <zephyr/types.h>
#include <zephyr/kernel.h>
#include <zephyr/sys/printk.h>
#include <zephyr/sys/byteorder.h>
#include <zephyr/random/random.h>

#include <zephyr/device.h>
#include <zephyr/drivers/i2c.h>
#include <zephyr/drivers/regulator.h>
#include <zephyr/drivers/gpio.h>
#include <zephyr/drivers/adc.h>
#include <zephyr/drivers/display.h>
#include <zephyr/devicetree.h>

#include <zephyr/bluetooth/bluetooth.h>
#include <zephyr/bluetooth/conn.h>
#include <zephyr/bluetooth/uuid.h>
#include <zephyr/bluetooth/gatt.h>
#include <zephyr/bluetooth/hci.h>
#include <bluetooth/gatt_dm.h>
#include <bluetooth/scan.h>

#include <lvgl.h>   // v9.3.0

#include <zephyr/logging/log.h>

LOG_MODULE_REGISTER(main, CONFIG_LOG_DEFAULT_LEVEL);

// ***********************************************************************************
// Ports and on-board devices
// ***********************************************************************************
static const struct gpio_dt_spec led_builtin = GPIO_DT_SPEC_GET(DT_ALIAS(led0), gpios);
static const struct gpio_dt_spec usrbtn = GPIO_DT_SPEC_GET(DT_ALIAS(sw0), gpios);
static const struct gpio_dt_spec test0 = GPIO_DT_SPEC_GET(DT_ALIAS(p104d0), gpios);
static const struct gpio_dt_spec selsw = GPIO_DT_SPEC_GET(DT_ALIAS(p105d1), gpios);
static const struct gpio_dt_spec rfswctl = GPIO_DT_SPEC_GET(DT_NODELABEL(rfsw_ctl), enable_gpios);

static const struct device *const pdm_imu_reg = DEVICE_DT_GET(DT_NODELABEL(pdm_imu_pwr));
static const struct device *const rfsw_reg = DEVICE_DT_GET(DT_NODELABEL(rfsw_pwr));
static const struct device *const vbat_reg = DEVICE_DT_GET(DT_NODELABEL(vbat_pwr));

// ------------------------------------------------------------------------------
// Initialize function of on-board device
// ---------------------------------------------------------------------------------
static int usrbtn_stat = 0;	// user sw status
int port_init(void) {
	int err;
    bool rdy;
    
    rdy = gpio_is_ready_dt(&led_builtin);	// on board green led
    if (rdy == false) { return -11; }
    rdy = gpio_is_ready_dt(&usrbtn);		// on board push sw
    if (rdy == false) { return -12; }
    rdy = gpio_is_ready_dt(&test0);		    // xiao port D0
    if (rdy == false) { return -13; }
    rdy = gpio_is_ready_dt(&selsw);			// xioa port D1
    if (rdy == false) { return -14; }
	rdy = device_is_ready(rfswctl.port);	// on board resw control
    if (rdy == false) { return -15; }

    err = gpio_pin_configure_dt(&led_builtin, GPIO_OUTPUT_ACTIVE);	// on board green led
	if (err < 0) { return -21; }
    err = gpio_pin_configure_dt(&usrbtn, GPIO_INPUT);				// on board push sw
    if (err < 0) { return -22; }
    err = gpio_pin_configure_dt(&test0, GPIO_OUTPUT_ACTIVE);		// xiao port D0
    if (err < 0) { return 23; }
    err = gpio_pin_configure_dt(&selsw, GPIO_INPUT);				// xioa port D1
    if (err < 0) { return -24; }
	err = gpio_pin_configure_dt(&rfswctl, GPIO_OUTPUT_ACTIVE);		// on board resw control
    if (err < 0) { return -25; }

	// on-board devices power
    regulator_disable(pdm_imu_reg);     // on board pdm and imu power: OFF
	regulator_enable(rfsw_reg);         // on board rfsw power: ON
	regulator_enable(vbat_reg);        	// on board battery voltage read: ON

    // antenna select
    err = gpio_pin_set_dt(&rfswctl, 0); // 0:onboard, 1:external
    if (err < 0) { return -3; }

	// LED off
	err = gpio_pin_set_dt(&led_builtin, 1);		// green led OFF

	return 0;
}

// *******************************************************************************
// battery voltage read ADC
// ********************************************************************************
#define DT_SPEC_AND_COMMA(node_id, prop, idx) ADC_DT_SPEC_GET_BY_IDX(node_id, idx),

static const struct adc_dt_spec adc_channels[] = {
	DT_FOREACH_PROP_ELEM(DT_PATH(zephyr_user), io_channels, DT_SPEC_AND_COMMA)
};

static uint16_t adc_buf;
static int32_t val_mv;
static struct adc_sequence sequence = {
	.buffer = &adc_buf,
	.buffer_size = sizeof(adc_buf),
};

// ----------------------------------------------------------------------------
// Initialize function of adc
// ---------------------------------------------------------------------------
int adc_init(void) {
	int err;

	err = adc_is_ready_dt(&adc_channels[7]);
	if (err < 0) { return -1; }
	err = adc_channel_setup_dt(&adc_channels[7]);
	if (err < 0) { return -2; }
	err = adc_sequence_init_dt(&adc_channels[7], &sequence);
	if (err < 0) { return -3; }

	return 0;
}

// ********************************************************************************
// SSD1306 Oled with lvgl
// ********************************************************************************
#define FONTNAME ibmlight
//#define FONTNAME ibmregular
//#define FONTNAME dejavu15
//#define FONTNAME spacemono

static  lv_obj_t *line_1;
static  lv_obj_t *line_2;
static  lv_obj_t *line_3;
static  lv_obj_t *line_4;
static  lv_style_t style;

int ssd_init(void) {
    LV_FONT_DECLARE(FONTNAME);   // 14 characters / LINE
	const struct device *display_dev;

	display_dev = DEVICE_DT_GET(DT_CHOSEN(zephyr_display));
	if (!device_is_ready(display_dev)) {
		LOG_ERR("Device not ready, aborting test");
		return -1;
	}

    // label, font, and style definition
    lv_style_init(&style);
    lv_style_set_text_font(&style, &FONTNAME);
    
	line_1 = lv_label_create(lv_scr_act());
    lv_obj_set_pos(line_1, 0, 0);
    lv_obj_add_style(line_1, &style, 0);
	line_2 = lv_label_create(lv_scr_act());
    lv_obj_set_pos(line_2, 0, 16);
    lv_obj_add_style(line_2, &style, 0);
    line_3 = lv_label_create(lv_scr_act());
    lv_obj_set_pos(line_3, 0, 32);
    lv_obj_add_style(line_3, &style, 0);
	line_4 = lv_label_create(lv_scr_act());
    lv_obj_set_pos(line_4, 0, 48);
    lv_obj_add_style(line_4, &style, 0);

    // title display
	display_blanking_off(display_dev);  // display ON
    	lv_label_set_text(line_1, "extended BLE");
    	lv_label_set_text(line_2, "  CENTRAL");
		if (usrbtn_stat == 1) {
    		lv_label_set_text(line_3, "       1M phy");
		} else {
			lv_label_set_text(line_3, "    coded phy");
		}
    	lv_label_set_text(line_4, "XIAO_nRF54L15");
	lv_task_handler();
	return 0;
}

// ***********************************************************************************
// extended BLE Central 
// ***********************************************************************************
#define CONN_INTERVAL 80	// *0.625 mS
#define DATANUM 16 			// time 4, random number 8, battery voltage 2, rssi 1, txpower 1
union unionData {   		// Union for data type conversion
  uint32_t  dataBuff32[DATANUM/4];
  uint16_t  dataBuff16[DATANUM/2];
  uint8_t   dataBuff8[DATANUM];
};
union unionData ud;
static uint8_t rx_buf[DATANUM];
static uint8_t tx_buf[DATANUM];
static bool rx_buf_flag = false;
static bool tx_buf_flag = false;
static uint32_t tx_rand[4];

static struct bt_conn *current_conn;
static uint16_t current_conn_handle;
bool conn_ready = false;

// 128-bit data service UUID
// 550055c4-0000-4dd1-be0c-40588193b485
static struct bt_uuid_128 datasvc_uuid = BT_UUID_INIT_128(
	0x85, 0xb4, 0x93, 0x81, 0x58, 0x40, 0x0c, 0xbe,
	0xd1, 0x4d, 0x00, 0x00, 0xc4, 0x55, 0x00, 0x55);
static struct bt_uuid_128 rx_uuid = BT_UUID_INIT_128(       // for Notify 0010
	0x85, 0xb4, 0x93, 0x81, 0x58, 0x40, 0x0c, 0xbe,
	0xd1, 0x4d, 0x10, 0x00, 0xc4, 0x55, 0x00, 0x55);
static struct bt_uuid_128 tx_uuid = BT_UUID_INIT_128(       // for Write 0020
	0x85, 0xb4, 0x93, 0x81, 0x58, 0x40, 0x0c, 0xbe,
	0xd1, 0x4d, 0x20, 0x00, 0xc4, 0x55, 0x00, 0x55);

// ----------------------------------------------------------------------
// notify callback
// ----------------------------------------------------------------------
static uint16_t notify_value_handle;
static uint16_t write_value_handle;

static uint8_t notify_cb(struct bt_conn *conn,
						struct bt_gatt_subscribe_params *params,
						const void *data, uint16_t length) {
	if (!data) {
		LOG_ERR("6.1x notify no data\n");
		return BT_GATT_ITER_STOP;
	}

    // reveive data from peripheral
    memcpy(rx_buf, data, DATANUM);
    rx_buf_flag = true;
    
    // send data to peripheral
    int err = bt_gatt_write_without_response(conn, write_value_handle, tx_buf, DATANUM, false);
	if (err) {
        LOG_ERR("8.2x Write failed %d", err);
    }
	tx_buf_flag = false;

	return BT_GATT_ITER_CONTINUE;	
}

//----------------------------
// discover, assign, subscribe
// ---------------------------
static uint16_t notify_ccc_handle;
static struct bt_gatt_subscribe_params sub_params;

int bt_client_handles_assign(struct bt_gatt_dm *dm) {
	const struct bt_gatt_dm_attr *gatt_service_attr = bt_gatt_dm_service_get(dm);
	const struct bt_gatt_service_val *gatt_service = bt_gatt_dm_attr_service_val(gatt_service_attr);
	const struct bt_gatt_dm_attr *gatt_chrc;
	const struct bt_gatt_dm_attr *gatt_desc;

	// data servics
	if (bt_uuid_cmp(gatt_service->uuid, &datasvc_uuid.uuid)) {
		return -ENOTSUP;
	}
	LOG_INF("5.12 Getting handles from service.");

	// tx characteristic
	gatt_chrc = bt_gatt_dm_char_by_uuid(dm, &tx_uuid.uuid);
	if (!gatt_chrc) {
		LOG_ERR("5.13x No tx characteristic found.");
		return -EINVAL;
	}
	write_value_handle = gatt_chrc->handle + 1;
	LOG_INF("5.13 tx characteristic found %04x", write_value_handle);

	// rx characteristic
	gatt_chrc = bt_gatt_dm_char_by_uuid(dm, &rx_uuid.uuid);
	if (!gatt_chrc) {
		LOG_ERR("5.14x No rx characteristic found.");
		return -EINVAL;
	}
	// gatt ccc
	gatt_desc = bt_gatt_dm_desc_by_uuid(dm, gatt_chrc, BT_UUID_GATT_CCC);
	if (!gatt_desc) {
		LOG_ERR("5.14x No CCC descriptor found.");
		return -EINVAL;
	}

	notify_value_handle = gatt_chrc->handle + 1;
	notify_ccc_handle   = gatt_desc->handle;
	LOG_INF("5.15 rx characteristic found %04x %04x", notify_value_handle, notify_ccc_handle);

	// Finally - save connection object
	current_conn = bt_gatt_dm_conn_get(dm);
	return 0;
}
// ------------------------------------------------------------------------------
static bool notify_enabled;
int bt_client_subscribe(void) {
	int err;
	
	if (notify_enabled) {
		notify_enabled = false;	
    	return -EALREADY;
	}
	notify_enabled = true;

	memset(&sub_params, 0, sizeof(sub_params));
	atomic_set_bit(&sub_params.flags[0], BT_GATT_SUBSCRIBE_FLAG_VOLATILE);
	sub_params.value_handle = notify_value_handle;
	sub_params.ccc_handle   = notify_ccc_handle;	
	sub_params.notify       = notify_cb;
	sub_params.value        = BT_GATT_CCC_NOTIFY;	

	err = bt_gatt_subscribe(current_conn, &sub_params);	
	if (err) {
		LOG_ERR("5.32x Subscribe data rx characteristic failed");
		return err;
	} else {
		LOG_INF("5.32 Subscribed to data rx characteristic");
	}
	return 0;
}

// ----------------------------------------------------------------------------
static void discover_completed(struct bt_gatt_dm *dm, void *ctx) {
	int err;

	bt_gatt_dm_data_print(dm);

	err = bt_client_handles_assign(dm);
	if (err) {
		LOG_ERR("5.2x Could not init client object (err %d)\n", err);
		return;
	}
	LOG_INF("5.2 client handles assigned\n");


	err = bt_client_subscribe();	
	if (err && err != -EALREADY) {
		LOG_ERR("5.4x Subscribe failed (err %d)\n", err);
		return;
	} else {
		LOG_INF("5.4 subscribed\n");
	}

	err = bt_gatt_dm_data_release(dm);
	if (err) {
		LOG_ERR("5.6x Could not release the discovery data (err %d)\n", err);
		return;
	}
	LOG_INF("5.6 discover_completed\n");
}

// -----------------------------------------------------------------------------
static void discover_service_not_found(struct bt_conn *conn, void *ctx) {
	LOG_INF("5.9 No more services\n");
}

// -------------------------------------------------------------------------------
static void discover_error_found(struct bt_conn *conn, int err, void *ctx) {
	LOG_ERR("5.9x The discovery procedure failed, err %d\n", err);
}

// ----------------------------------------------------------------------------------
static struct bt_gatt_dm_cb discover_cb = {
	.completed = discover_completed,
	.service_not_found = discover_service_not_found,
	.error_found = discover_error_found,
};

// --------------------------------------------------------------------------------
// filter match
// ----------------------------------------------------------------------------------
static void scan_filter_match(struct bt_scan_device_info *device_info,
			      			struct bt_scan_filter_match *filter_match,
			      			bool connectable) {
	int err;
	char addr[BT_ADDR_LE_STR_LEN];
	struct bt_conn_le_create_param *conn_params_1m;
	struct bt_conn_le_create_param *conn_params_coded;
	bt_addr_le_to_str(device_info->recv_info->addr, addr, sizeof(addr));

	uint8_t prim_phy = device_info->recv_info->primary_phy;
	uint8_t sec_phy = device_info->recv_info->secondary_phy;

	LOG_INF("3.1 Filter matched. Address: %s connectable: %s Pri: %u Sec: %u\n",
		addr, connectable ? "yes" : "no", prim_phy, sec_phy);

	err = bt_scan_stop();
	if (err) {
		LOG_ERR("3.3x Stop LE scan failed (err %d)\n", err);
		return;
	} else {
		LOG_INF("3.3 Stop LE scan\n");
	}

	conn_params_1m = BT_CONN_LE_CREATE_PARAM(
			BT_CONN_LE_OPT_NONE,							// 1M-1/2M
			CONN_INTERVAL,
			CONN_INTERVAL);
	conn_params_coded = BT_CONN_LE_CREATE_PARAM(
			BT_CONN_LE_OPT_CODED | BT_CONN_LE_OPT_NO_1M,	// coded-coded
			CONN_INTERVAL,
			CONN_INTERVAL);
	LOG_INF("3.4 connection create\n");
	// phy select
    if (usrbtn_stat == 1) {
		err = bt_conn_le_create(device_info->recv_info->addr, conn_params_1m,
				BT_LE_CONN_PARAM_DEFAULT,
				&current_conn);
	} else {
		err = bt_conn_le_create(device_info->recv_info->addr, conn_params_coded,
				BT_LE_CONN_PARAM_DEFAULT,
				&current_conn);		
	}
	if (err) {
		LOG_ERR("3.5x Create conn failed (err %d)\n", err);
		return;
	}	
	LOG_INF("3.6 Connection pending\n");
}

// ---------------------------------------------------------------------------
// scan init
// ----------------------------------------------------------------------------
BT_SCAN_CB_INIT(scan_cb, scan_filter_match, NULL, NULL, NULL);

static void scan_init(void) {
	int err;

	struct bt_le_scan_param scan_param_1m = {
		.type     = BT_LE_SCAN_TYPE_ACTIVE,
		.interval = BT_GAP_SCAN_FAST_INTERVAL,
		.window   = BT_GAP_SCAN_FAST_WINDOW,
		.options = BT_LE_SCAN_OPT_NONE,      // primary=1M,secondary=1M/2M
	};
	struct bt_scan_init_param scan_init_param_1m = {
		.connect_if_match = 0,
		.scan_param = &scan_param_1m,
		.conn_param = NULL
	};

	struct bt_le_scan_param scan_param_coded = {
		.type     = BT_LE_SCAN_TYPE_ACTIVE,
		.interval = BT_GAP_SCAN_FAST_INTERVAL,
		.window   = BT_GAP_SCAN_FAST_WINDOW,
		.options = BT_LE_SCAN_OPT_CODED | BT_LE_SCAN_OPT_NO_1M
	};
	struct bt_scan_init_param scan_init_param_coded = {
		.connect_if_match = 0,
		.scan_param = &scan_param_coded,
		.conn_param = NULL
	};

	// phy select
    if (usrbtn_stat == 1) {
		bt_scan_init(&scan_init_param_1m);
	} else {
		bt_scan_init(&scan_init_param_coded);
	}	
	bt_scan_cb_register(&scan_cb);

	LOG_INF("1.1 scan param initialize\n");

	// service UUID filter
	err = bt_scan_filter_add(BT_SCAN_FILTER_TYPE_UUID, &datasvc_uuid.uuid);
	if (err) {
    	LOG_ERR("1.2x Scanning filters cannot be set (err %d)\n", err);
    	return;
	}
	LOG_INF("1.2 Scanning filters set\n");
	
	err = bt_scan_filter_enable(BT_SCAN_UUID_FILTER, false);
	if (err) {
    	LOG_ERR("1.4x Filters cannot be turned on (err %d)\n", err);
		return;
	}
	LOG_INF("1.4 scan_filter_enabled\n");
}

// ----------------------------------------------------------------------
// mtu exchane
// ----------------------------------------------------------------------
static void mtu_exchange_cb(struct bt_conn *conn, uint8_t err,
			    struct bt_gatt_exchange_params *params) {
	printk("%s: MTU exchange %s (%u)\n", __func__,
	       err == 0U ? "successful" : "failed",
	       bt_gatt_get_mtu(conn));
}

static struct bt_gatt_exchange_params mtu_exchange_params = {
	.func = mtu_exchange_cb
};

static int mtu_exchange(struct bt_conn *conn) {
	int err;

	printk("%s: Current MTU = %u\n", __func__, bt_gatt_get_mtu(conn));

	printk("%s: Exchange MTU...\n", __func__);
	err = bt_gatt_exchange_mtu(conn, &mtu_exchange_params);
	if (err) {
		printk("%s: MTU exchange failed (err %d)", __func__, err);
		return err;
	}
	return 0;
}


// --------------------------------------------
// Get the current RSSI value
// --------------------------------------------
static void read_conn_rssi(uint16_t handle, int8_t *rssi) {
	struct net_buf *buf, *rsp = NULL;
	struct bt_hci_cp_read_rssi *cp;
	struct bt_hci_rp_read_rssi *rp;
	int err;

	buf = bt_hci_cmd_alloc(K_FOREVER);
	if (!buf) {
		LOG_ERR("Unable to allocate command buffer\n");
		return;
	}

	cp = net_buf_add(buf, sizeof(*cp));
	cp->handle = sys_cpu_to_le16(handle);

	err = bt_hci_cmd_send_sync(BT_HCI_OP_READ_RSSI, buf, &rsp);
	if (err) {
		LOG_ERR("Read RSSI err: %d\n", err);
		return;
	}

	rp = (void *)rsp->data;
	*rssi = rp->rssi;
	net_buf_unref(rsp);
}


// --------------------------------------------
// RSSI Monitor
// -------------------------------------------- 
#define RSSI_ALPHA_SHIFT 4

static struct bt_conn *conn_ref;
static struct k_work_delayable rssi_work;
static atomic_t avg_rssi = ATOMIC_INIT(-127 << 8); // Shift to increase resolution

// get rssi
static int8_t read_rssi_once(struct bt_conn *conn)
{
    int8_t rssi = -127;
    uint16_t handle;

    bt_hci_get_conn_handle(conn, &handle);
    read_conn_rssi(handle, &rssi);
    return rssi;
}

// moving filter
static void rssi_work_handler(struct k_work *work)
{
    if (conn_ref) {
        int8_t rssi = read_rssi_once(conn_ref);	// get rssi
        int32_t rssi8 = rssi << 8;				// increase resolution
        int32_t old = atomic_get(&avg_rssi);
        int32_t new = old + ((rssi8 - old) >> RSSI_ALPHA_SHIFT);	
        atomic_set(&avg_rssi, new);		
    }
	// sampling time 50mS
    k_work_schedule(&rssi_work, K_MSEC(CONN_INTERVAL * 0.625));
}

// monitoring start
void rssi_monitor_start(struct bt_conn *conn)
{
    conn_ref = bt_conn_ref(conn);

    k_work_init_delayable(&rssi_work, rssi_work_handler);
    k_work_schedule(&rssi_work, K_NO_WAIT);
}

// monitoring stop
void rssi_monitor_stop(void)
{
    k_work_cancel_delayable(&rssi_work);

    if (conn_ref) {
        bt_conn_unref(conn_ref);
        conn_ref = NULL;
    }
}

// Get the moving average RSSI
int8_t rssi_monitor_get(void)
{
    return (int8_t)(atomic_get(&avg_rssi) >> 8);
}

// --------------------------------------------
// txpower control PID
// --------------------------------------------
#define TXPWRMAX 8		// Maximum txpower dBm
#define TXPWRMIN -22	// Minimum txpower dBm
#define RSSI0 -80		// RSSI Target Value dBm
#define Kp 0.50			// Proportional gain
#define Ki 0.35			// Integral gain
#define Kd 0.00			// Differential gain
#define T 1.00			// Sampling time sec
static double error1 = 0;
static double error_i = 0;
static int8_t txpower_control(int8_t rssi) {
	// PID
	double error = RSSI0 - rssi;	
	error_i = error_i + (error + error1) * T / 2;
	double error_d = (error - error1) / T;
	double u_p = error * Kp;
	double u_i = error_i * Ki;
	double u_d = error_d * Kd;
	double txpwr = u_p + u_i + u_d;
	// Saturation processing
	if ( txpwr < TXPWRMIN) {
		txpwr = TXPWRMIN; 
		error_i = (TXPWRMIN - u_p - u_d) / Ki;
		error_i = error_i - (error + error1) * T / 2;
	} else if ( txpwr > TXPWRMAX) {
		txpwr = TXPWRMAX;
		error_i = (TXPWRMAX - u_p - u_d) / Ki;
		error_i = error_i - (error + error1) * T / 2;
	}
	error1 = error;

	// for serial plotter(Arduino IDE 1.8.19)
	char str[64];
	int a0, a1, a2, a3;
	int b0, b1, b2, b3;
	    a0 = (int)u_p; b0 = (int)((u_p - a0) * 100); if(b0 < 0) {b0 = -b0;}
        a1 = (int)u_i; b1 = (int)((u_i - a1) * 100); if(b1 < 0) {b1 = -b1;}
        a2 = (int)u_d; b2 = (int)((u_d - a2) * 100); if(b2 < 0) {b2 = -b2;}
		a3 = (int)txpwr; b3 = (int)((txpwr - a3) * 100); if(b3 < 0) {b3 = -b3;}
//		snprintf(str, sizeof(str), "%2d.%02d %2d.%02d %2d.%02d %2d.%02d", 
//									a0, b0, a1, b1, a2, b2, a3, b3);
		snprintf(str, sizeof(str), "%2d.%02d", a3, b3);
		printk("%d %d %d %d %d %s\n", TXPWRMIN, 0, TXPWRMAX, RSSI0, rssi, str);
	return (int8_t)txpwr;
}

// ----------------------------------------------------------------------
// connected callback
// ------------------------------------------------------------------------
static void connected(struct bt_conn *conn, uint8_t conn_err) {
	LOG_INF("4.0 Connection procedure\n");
	int err;
	struct bt_conn_info info;
	char addr[BT_ADDR_LE_STR_LEN];

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

	if (conn_err) {
		LOG_ERR("4.1x Failed to connect to %s, 0x%02x %s\n", addr, conn_err,
		       bt_hci_err_to_str(conn_err));
		return;
	}
	// MTU update
	(void)mtu_exchange(conn);

	current_conn = conn;
	err = bt_hci_get_conn_handle(current_conn, &current_conn_handle);
	if (err) {
		LOG_ERR("4.1x No connection handle (err %d)\n", err);
		return;
	} else {
		rssi_monitor_start(conn);	// rssi monitoring start
	}

	err = bt_conn_get_info(conn, &info);
	if (err) {
		LOG_ERR("4.3x Failed to get connection info (err %d)\n", err);
		return;
	} else {
		const struct bt_conn_le_phy_info *phy_info;
		phy_info = info.le.phy;
		uint8_t txphy = phy_info->tx_phy;
		uint8_t rxphy = phy_info->rx_phy;
		LOG_INF("4.3 Connected: %s, tx %u, rx %u\n", addr, txphy, rxphy);
	}

	if (conn == current_conn) {		
		err = bt_gatt_dm_start(conn, &datasvc_uuid.uuid, &discover_cb, NULL);
		if (err) {
			LOG_ERR("4.4x Failed to start discovery (err %d)\n", err);
			return;
		}
		LOG_INF("4.4 start discovery\n");
	}
	conn_ready = true;
}

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

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

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

	if (current_conn != conn) {
		return;
	}
	rssi_monitor_stop();	// rssi monitoring stop
	
	// re-start scan
	conn_ready = false;
	notify_enabled = false;
	memset(&sub_params, 0, sizeof(sub_params));
	notify_value_handle = 0;
	notify_ccc_handle   = 0;
	bt_conn_unref(current_conn);
	current_conn = NULL;
	err = bt_scan_start(BT_SCAN_TYPE_SCAN_ACTIVE);
	if (err) {
		LOG_ERR("4.7x Scanning failed to start (err %d)\n", err);
		return;
	}
	LOG_INF("4.7 Disconnected Scanning re-start\n");
//	conn_ready = false;
}
// ----------------------------------------------------------------------------
BT_CONN_CB_DEFINE(conn_callbacks) = {
	.connected = connected,
	.disconnected = disconnected,
};

// *******************************************************************************
// *******************************************************************************
// *******************************************************************************
int main(void)
{
    LOG_INF("XIAO_nRF54L15 Central\n");
    int err;
	int ret;
	current_conn = NULL;

	// ports and on board devices initialization
	err = port_init();
	if (err < 0) {
		LOG_ERR("0.1x port_init err:%d\n", err);
		return err;
	}
	LOG_INF("0.1 port_init done\n");

	// Read phy select sw
	ret = gpio_pin_set_dt(&led_builtin, 0);		// green led ON
	k_msleep(1000);
	if (gpio_pin_get_dt(&usrbtn) == 1) {			// push user button sw
		usrbtn_stat = 1;
	} else {
		usrbtn_stat = 0;
	}	
	ret = gpio_pin_set_dt(&led_builtin, 1);		// green led OFF

    // vbat read adc initialization
	err = adc_init();
	if (err < 0) {
		LOG_ERR("0.2x adc_init err:%d\n", err);
		return err;		
	}
	LOG_INF("0.2 adc_init done\n");

	// SSD1306 with u8g2 initialization
	err = ssd_init();
	if (err < 0) {
		LOG_ERR("0.3x ssd_init err:%d\n", err);
		return err;		
	}
	k_msleep(2000);
	LOG_INF("0.3 ssd_init done\n");

	// bluetooth initialization
	LOG_INF("0.0 Starting Bluetooth Central coded\n");

	err = bt_enable(NULL);
	if (err) {
		LOG_ERR("0.1x Bluetooth init failed (err %d)\n", err);
		return err;
	}
	LOG_INF("0.1 Bluetooth initialized\n");

	// scan initialize, uuid match filter set
	scan_init();

	// scan start
	err = bt_scan_start(BT_SCAN_TYPE_SCAN_ACTIVE);
	if (err) {
		LOG_ERR("2.1x Scanning failed to start (err %d)\n", err);
		return err;
	}
	LOG_INF("2.1 Scanning successfully started\n");

	// loop
    while(1) {
        if (conn_ready == true && rx_buf_flag == true && tx_buf_flag == false) {
            uint32_t timestamp = k_uptime_get_32();
			ret = gpio_pin_set_dt(&led_builtin, 0);

        	// Vbat read
			regulator_enable(vbat_reg);
			k_msleep(5);
			err = adc_read_dt(&adc_channels[7], &sequence);
			if (err < 0) {
				LOG_ERR("Could not read adc (%d)\n", err);
				return err;
			}
			val_mv = (int32_t)adc_buf;
			regulator_disable(vbat_reg);
			// conversion to mV
			err = adc_raw_to_millivolts_dt(&adc_channels[7], &val_mv);
			if (err < 0) {
				LOG_ERR("value in mV not available\n");
			}
			uint16_t vbat = (uint16_t)(val_mv * 2);
        
            // receive data buffering
			
            memcpy(ud.dataBuff8, rx_buf, DATANUM);		
            rx_buf_flag = false;
			
            uint32_t rx_time = ud.dataBuff32[0];
            uint32_t rx_rand = ud.dataBuff32[2];
            uint16_t rx_vbat = ud.dataBuff16[6];
            int8_t rx_rssi = ud.dataBuff8[14];
			

            LOG_INF("Rcvd %8u %08X %4u %3d", rx_time, rx_rand, rx_vbat, rx_rssi);

            // send data buffering
            tx_rand[0] = sys_rand32_get();	// data sample
            int8_t rssi = -127;
			int8_t txpower = 8;
			rssi = rssi_monitor_get();
			txpower = txpower_control(rssi);

            ud.dataBuff32[0] = timestamp;
            ud.dataBuff32[2] = tx_rand[0];
            ud.dataBuff16[6] = vbat;
            ud.dataBuff8[14] = rssi;
			ud.dataBuff8[15] = txpower; // txpower value to peripheral
			// sample data
			for (int i = 16; i < DATANUM; i++) {
				ud.dataBuff8[i] = 0xAA;
			}

            memcpy(tx_buf, ud.dataBuff8, DATANUM);
			tx_buf_flag = true;

            LOG_INF("Sent %8u %08X %4u %3d %3d\n", timestamp, tx_rand[2], vbat, rssi, txpower);

            // data display
            char line_1_str[32];
            char line_2_str[32];
            char line_3_str[32];
            char line_4_str[32];

            snprintf(line_1_str, sizeof(line_1_str), "Rcvd %08X", rx_rand);
            snprintf(line_2_str, sizeof(line_2_str), "%4dmV %3ddBm", rx_vbat, rx_rssi);
            snprintf(line_3_str, sizeof(line_3_str), "Sent %08X", tx_rand[2]);
            snprintf(line_4_str, sizeof(line_4_str), "%4dmV %3ddBm", vbat, rssi);

            lv_label_set_text(line_1, line_1_str);
            lv_label_set_text(line_2, line_2_str);
            lv_label_set_text(line_3, line_3_str);
            lv_label_set_text(line_4, line_4_str);
	        lv_task_handler();
            // 36 mS

			tx_rand[3] = tx_rand[2]; tx_rand[2] = tx_rand[1]; tx_rand[1] = tx_rand[0];

			ret = gpio_pin_set_dt(&led_builtin, 1);
        }
        k_msleep(50);
    }    
	return 0;
}
