// SDK:         nRF Connect SDK v3.2.1
// Toolchain:   nRF Connect SDK Toolchain v3.2.1
// board:       xiao_nrf54l15/nrf54l15/cpuapp
// 2026/01/17


//#include <stdio.h>
#include <zephyr/kernel.h>
#include <zephyr/types.h>
//#include <zephyr/sys/printk.h>
#include <zephyr/random/random.h>
#include <zephyr/sys/byteorder.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 <lvgl.h>   // v9.3.0

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

#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_disable(vbat_reg);        // on board battery voltage read: OFF

    // 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, false);		// 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),
};

// --------------------------------------------------------------------------------
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, "  PERIPHERAL");
		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 Peripheral
// ************************************************************************************
#define DEVICE_NAME             CONFIG_BT_DEVICE_NAME
#define DEVICE_NAME_LEN         (sizeof(DEVICE_NAME) - 1)
#define NOTIFY_INTERVAL         1000

#define DATANUM 16 	// timestamp 4, random number 4 * 2, battery voltage 2, tx_power/rssi 2
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 struct bt_conn *current_conn;
static uint16_t current_conn_handle;
bool conn_ready = false;

static void start_advertising_coded(struct k_work *work);
static K_WORK_DEFINE(start_advertising_worker, start_advertising_coded);

static struct bt_le_ext_adv *adv;

// 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 tx_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 rx_uuid = BT_UUID_INIT_128(       // for Write 0020
	0x85, 0xb4, 0x93, 0x81, 0x58, 0x40, 0x0c, 0xbe,
	0xd1, 0x4d, 0x20, 0x00, 0xc4, 0x55, 0x00, 0x55);

static const struct bt_data ad[] = {
    BT_DATA_BYTES(BT_DATA_FLAGS, (BT_LE_AD_GENERAL | BT_LE_AD_NO_BREDR)),
    BT_DATA(BT_DATA_UUID128_ALL, datasvc_uuid.val, sizeof(datasvc_uuid.val)),
    BT_DATA(BT_DATA_NAME_COMPLETE, DEVICE_NAME, DEVICE_NAME_LEN),
};

// ---------------------------------------------------------------------------------
// notify enable
// -----------------------------------------------------------------------------------
static bool notify_enabled;
static void tx_ccc_cfg_changed(const struct bt_gatt_attr *attr, uint16_t value)
{
    notify_enabled = (value == BT_GATT_CCC_NOTIFY);
    LOG_INF("3.1 Notify %s\n", notify_enabled ? "enabled" : "disabled");
}
// ---------------------------------------------------------------------------------
// Callback to receive data written by Central
// --------------------------------------------------------------------------------
static ssize_t rx_write_cb(struct bt_conn *conn, const struct bt_gatt_attr *attr,
			const void *data, uint16_t len, uint16_t offset, uint8_t flags) {
	memcpy(rx_buf, data, len);
	rx_buf_flag = true;
	return len;
}

BT_GATT_SERVICE_DEFINE(data_svc,
	BT_GATT_PRIMARY_SERVICE(&datasvc_uuid),
	BT_GATT_CHARACTERISTIC(&tx_uuid.uuid,
			       BT_GATT_CHRC_NOTIFY,				   
			       BT_GATT_PERM_READ,
			       NULL, NULL, tx_buf),
	BT_GATT_CCC(tx_ccc_cfg_changed, BT_GATT_PERM_READ | BT_GATT_PERM_WRITE),
	BT_GATT_CHARACTERISTIC(&rx_uuid.uuid,
			       BT_GATT_CHRC_WRITE | BT_GATT_CHRC_WRITE_WITHOUT_RESP,
			       BT_GATT_PERM_WRITE,
			       NULL, rx_write_cb, rx_buf),
);

// ------------------------------------------------------------------------------
// connected, mtu updated and disconnected callbacks
// --------------------------------------------------------------------------------
void mtu_updated(struct bt_conn *conn, uint16_t tx, uint16_t rx)
{
	LOG_INF("2.9 Updated MTU: TX: %d RX: %d bytes\n", tx, rx);
}

static struct bt_gatt_cb gatt_callbacks = {
	.att_mtu_updated = mtu_updated,
};

// ----------------------------------------------------------------------------
static void connected(struct bt_conn *conn, uint8_t conn_err) {
	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("2.0x Connection failed, err 0x%02x %s\n", conn_err, bt_hci_err_to_str(conn_err));
		return;
	}
	LOG_INF("2.0 Connection get\n");
	
	current_conn = bt_conn_ref(conn);
	err = bt_hci_get_conn_handle(current_conn, &current_conn_handle);
	if (err) {
		LOG_ERR("2.0xx No connection handle (err %d)\n", err);
		return;
	}

	err = bt_conn_get_info(conn, &info);
	if (err) {
		LOG_ERR("2.1x 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("2.2 Connected: %s, tx %u, rx %u\n", addr, txphy, rxphy);			   
	}
	conn_ready = true;
}

// ------------------------------------------------------------------------------------
static void disconnected(struct bt_conn *conn, uint8_t reason) {
	LOG_ERR("2.2x Disconnected, reason 0x%02x %s\n", reason, bt_hci_err_to_str(reason));
    if (current_conn) {
        bt_conn_unref(current_conn);
        current_conn = NULL;
    }
	k_work_submit(&start_advertising_worker);
	conn_ready = false;
}

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

// -------------------------------------------------------------------------------------
// Advertising
// -------------------------------------------------------------------------------------
static int create_advertising_coded(void) {
	int err;
	struct bt_le_adv_param param_1m =
		BT_LE_ADV_PARAM_INIT(BT_LE_ADV_OPT_CONN |
				     BT_LE_ADV_OPT_EXT_ADV | BT_LE_ADV_OPT_NO_2M,	// 1M-1M
				     BT_GAP_ADV_FAST_INT_MIN_2,		// 100 mS
				     BT_GAP_ADV_FAST_INT_MAX_2,		// 150 mS
				     NULL);
	struct bt_le_adv_param param_coded =
		BT_LE_ADV_PARAM_INIT(BT_LE_ADV_OPT_CONN |
				     BT_LE_ADV_OPT_EXT_ADV |						// coded-coded				 
				     BT_LE_ADV_OPT_CODED |
				     BT_LE_ADV_OPT_REQUIRE_S8_CODING,
				     BT_GAP_ADV_FAST_INT_MIN_2,
				     BT_GAP_ADV_FAST_INT_MAX_2,
				     NULL);				 

	// phy select
    if (usrbtn_stat == 1) {
        err = bt_le_ext_adv_create(&param_1m, NULL, &adv);
    } else {
        err = bt_le_ext_adv_create(&param_coded, NULL, &adv);
    };
	if (err) {
		LOG_ERR("1.0x Failed to create advertiser set (err %d)\n", err);
		return err;
	}
	LOG_INF("1.0 Created adv: %p\n", adv);

	err = bt_le_ext_adv_set_data(adv, ad, ARRAY_SIZE(ad), NULL, 0);
	if (err) {
		LOG_ERR("1.1x Failed to set advertising data (err %d)\n", err);
		return err;
	}
	LOG_INF("1.1 Set adv data\n");
	return 0;
}

// ------------------------------------------------------------------------------------
static void start_advertising_coded(struct k_work *work) {
	int err;

	err = bt_le_ext_adv_start(adv, BT_LE_EXT_ADV_START_DEFAULT);
	if (err) {
		LOG_ERR("2.0x Failed to start advertising set (err %d)\n", err);
		return;
	}
	LOG_INF("2.0 Advertiser %p set started\n", adv);
}

// ---------------------------------------------------------------------------------
// Get the current RSSI value during connection
// -----------------------------------------------------------------------------------
#define AVE 4
int16_t rssi_buf[AVE];

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;
/*
	// moving average
	int16_t rssi_sum = 0;
	for (int i = 0; i < (AVE - 1); i++) {
		rssi_buf[i] = rssi_buf[i + 1];
	}
	rssi_buf[AVE - 1] = rp->rssi;
	for (int i = 0; i < AVE; i++) {
		rssi_sum = rssi_sum + rssi_buf[i];
	}
	*rssi = rssi_sum / AVE;
*/
	*rssi = rp->rssi;

	net_buf_unref(rsp);
}

// ******************************************************************************
// ******************************************************************************
// ******************************************************************************
int main(void)
{
//	LOG_INF("********\n");	// If FAULT occurs, add or remove this line
	LOG_INF("0.0 XIAO_nRF54L15 extended BLE 1M/coded phy Peripheral\n");
	int err;
	int ret;
		
	// 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, true);		// 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, false);		// 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 Peripheral coded\n");

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

	bt_gatt_cb_register(&gatt_callbacks);	// for MTU update

	err = create_advertising_coded();
	if (err) {
		LOG_ERR("0.3x Advertising failed to create (err %d)\n", err);
		return err;
	}
	LOG_INF("0.3 Advertising created\n");

	k_work_submit(&start_advertising_worker);

	while(1) {
		if (conn_ready == true) {		
//    		ret = gpio_pin_set_dt(&led_builtin, true);		// green led ON
			uint32_t timestamp = k_uptime_get_32();

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

			// send data buffering
			uint32_t rand = sys_rand32_get();	// data sample
			int8_t rssi = -127;
        	read_conn_rssi(current_conn_handle, &rssi);	// central rssi

			ud.dataBuff32[0] = timestamp;
			ud.dataBuff32[1] = rand;
			ud.dataBuff16[6] = vbat;
        	ud.dataBuff16[7] = (uint16_t)rssi;		
			
			// sample data
			for (int i = 16; i < DATANUM; i++) {
				ud.dataBuff8[i] = 0x55;
			}

			memcpy(tx_buf, ud.dataBuff8, DATANUM);
			tx_buf_flag = true;
ret = gpio_pin_set_dt(&led_builtin, true);		// green led ON
			bt_gatt_notify(current_conn, &data_svc.attrs[1], tx_buf, DATANUM);
ret = gpio_pin_set_dt(&led_builtin, false);		// green led OFF		
			tx_buf_flag = false;			
			LOG_INF("Sent %8u %08X %4u %3d", timestamp, rand, vbat, rssi);

			// wait for rcvd_flaag
			uint32_t start_time = k_uptime_get_32();
			uint32_t while_time = 0;
			while (rx_buf_flag != 1) {
				while_time = k_uptime_get_32() - start_time;
				if (while_time > NOTIFY_INTERVAL + 200) {
					break;
				}
				k_msleep(10);	// chack every 10 mS
			}
			LOG_INF("wait    %d", while_time);

			// received data buffering
ret = gpio_pin_set_dt(&led_builtin, true);		// green led ON			
			memcpy(ud.dataBuff8, rx_buf, DATANUM);
ret = gpio_pin_set_dt(&led_builtin, false);		// green led OFF		
			rx_buf_flag = false;
        	uint32_t rx_time = ud.dataBuff32[0];
        	uint32_t rx_rand = ud.dataBuff32[1];
        	uint16_t rx_vbat = ud.dataBuff16[6];
        	int8_t rx_rssi = ud.dataBuff16[7];

			LOG_INF("Rcvd %8u %08X %4u %3d\n", rx_time, rx_rand, rx_vbat, rx_rssi);
/*
       		 // 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", rand);
        	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
*/
			ret = gpio_pin_set_dt(&led_builtin, false);		// green led OFF

        	uint32_t elapsed = k_uptime_get_32() - timestamp;
        	if(elapsed < NOTIFY_INTERVAL) {
            	k_msleep(NOTIFY_INTERVAL - elapsed);
			}
		}
		ret = gpio_pin_set_dt(&led_builtin, false);
	}
	return 0;
}
