/*
 * Copyright (c) 2018 Nordic Semiconductor ASA
 *
 * SPDX-License-Identifier: LicenseRef-Nordic-5-Clause
 */

/** @file
 *  @brief Nordic UART Bridge Service (NUS) sample
 */
#include "uart_async_adapter.h"

#include <zephyr/types.h>
#include <zephyr/kernel.h>
#include <zephyr/drivers/uart.h>
#include <zephyr/usb/usb_device.h>

#include <zephyr/device.h>
#include <zephyr/devicetree.h>
#include <soc.h>

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

#include <bluetooth/services/nus.h>

#include <dk_buttons_and_leds.h>

#include <zephyr/settings/settings.h>

#include <stdio.h>
#include <zephyr/drivers/gpio.h>

#include <zephyr/logging/log.h>
#include "jfet_files/jfet_common.h"
#include "mainpage.h"

#include <zephyr/drivers/i2c.h>
#include "jfet_files/HDC2080.h"
#include <stdlib.h>

#define LOG_MODULE_NAME peripheral_uart
LOG_MODULE_REGISTER(LOG_MODULE_NAME);
//#define LOG_MODULE_NAME PERIPHERAL_UART_LOG_LEVEL
//LOG_MODULE_REGISTER(MAIN,CONFIG_PERIPHERAL_UART_LOG_LEVEL);

#include <zephyr/drivers/gpio.h>

#define STACKSIZE CONFIG_BT_NUS_THREAD_STACK_SIZE
#define PRIORITY 7

#define DEVICE_NAME CONFIG_BT_DEVICE_NAME
#define DEVICE_NAME_LEN (sizeof(DEVICE_NAME) - 1)

#define RUN_STATUS_LED DK_LED1
#define RUN_LED_BLINK_INTERVAL 1000

#define CON_STATUS_LED DK_LED2

#define KEY_PASSKEY_ACCEPT DK_BTN1_MSK
#define KEY_PASSKEY_REJECT DK_BTN2_MSK

#define UART_BUF_SIZE CONFIG_BT_NUS_UART_BUFFER_SIZE
#define UART_WAIT_FOR_BUF_DELAY K_MSEC(50)
#define UART_WAIT_FOR_RX CONFIG_BT_NUS_UART_RX_WAIT_TIME

/* start I2C test */
#if 1
#if DT_NODE_HAS_STATUS(DT_ALIAS(i2c_0), okay)
#define I2C_DEV_NODE DT_ALIAS(i2c_0)
#elif DT_NODE_HAS_STATUS(DT_ALIAS(i2c_1), okay)
#define I2C_DEV_NODE DT_ALIAS(i2c_1)
#elif DT_NODE_HAS_STATUS(DT_ALIAS(i2c_2), okay)
#define I2C_DEV_NODE DT_ALIAS(i2c_2)
#else
#error "Please set the correct I2C device"
#endif
#endif

const struct device *const i2c_dev = DEVICE_DT_GET(I2C_DEV_NODE);
/* end I2C test */

static K_SEM_DEFINE(ble_init_ok, 0, 1);
K_SEM_DEFINE(hdc_rdy_seen, 0, 1);

static struct bt_conn *current_conn;
static struct bt_conn *auth_conn;

/* start GPIO test */

#if 1
#define BATEN_NODE DT_ALIAS(baten)
#if !DT_NODE_HAS_STATUS(BATEN_NODE, okay)
#error "BATEN alias is not defined"
#endif
const struct gpio_dt_spec baten = GPIO_DT_SPEC_GET_OR(BATEN_NODE, gpios, {0});
#endif

#if 1
#define HDC2080RDY_NODE DT_ALIAS(hdc2080rdy)
#if !DT_NODE_HAS_STATUS(HDC2080RDY_NODE, okay)
#error "HDC2080 alias is not defined"
#endif
static const struct gpio_dt_spec hdc2080 = GPIO_DT_SPEC_GET_OR(HDC2080RDY_NODE, gpios, {0});
static struct gpio_callback hdc2080_cb_data;
#endif

/* end GPIO test */

/* start SPI test */
#include <zephyr/drivers/spi.h>

/* end SPI test */

/* start ADC test */
#include <zephyr/drivers/adc.h>
#include <nrfx_saadc.h>
#include <math.h>
#define ADC_BUFFER_SIZE 1 //!< number of ADC channels to read

#if DT_NODE_HAS_STATUS(DT_ALIAS(adc), okay)
#define ADC_DEV_NODE DT_ALIAS(adc)
#elif
#error "Please set the correct ADC device"
#endif

////#define ADC_DEVICE_NAME DT_ADC_0_NAME
#define ADC_RESOLUTION 10
#define ADC_GAIN ADC_GAIN_1_6
#define ADC_REFERENCE ADC_REF_INTERNAL
#define ADC_ACQUISITION_TIME ADC_ACQ_TIME(ADC_ACQ_TIME_MICROSECONDS, 10)
#define ADC_1ST_CHANNEL_ID 0
#define ADC_1ST_CHANNEL_INPUT NRF_SAADC_INPUT_AIN0
#define ADC_2ND_CHANNEL_ID 1
#define ADC_2ND_CHANNEL_INPUT NRF_SAADC_INPUT_AIN1
#define ADC_3RD_CHANNEL_ID 2
#define ADC_3RD_CHANNEL_INPUT NRF_SAADC_INPUT_AIN2
#define ADC_4TH_CHANNEL_ID 3
#define ADC_4TH_CHANNEL_INPUT NRF_SAADC_INPUT_AIN3
#define ADC_5TH_CHANNEL_ID 4
#define ADC_5TH_CHANNEL_INPUT NRF_SAADC_INPUT_AIN4
#define ADC_6ST_CHANNEL_ID 5
#define ADC_6ST_CHANNEL_INPUT NRF_SAADC_INPUT_AIN5

const struct device *const adc_dev = DEVICE_DT_GET(ADC_DEV_NODE);

#if 0
static const struct adc_channel_cfg m_4th_channel_cfg =   //!< ADC config for ADC input
{
	.gain = ADC_GAIN,
	.reference = ADC_REFERENCE,
	.acquisition_time = ADC_ACQUISITION_TIME,
	.channel_id = ADC_4TH_CHANNEL_ID,
#if defined(CONFIG_ADC_CONFIGURABLE_INPUTS)
	.input_positive = ADC_4TH_CHANNEL_INPUT,
#endif
};
#endif

#if 0
static const struct adc_channel_cfg m_3rd_channel_cfg = 
{
	.gain = ADC_GAIN,
	.reference = ADC_REFERENCE,
	.acquisition_time = ADC_ACQUISITION_TIME,
	.channel_id = ADC_3RD_CHANNEL_ID,
#if defined(CONFIG_ADC_CONFIGURABLE_INPUTS)
	.input_positive = ADC_3RD_CHANNEL_INPUT,
#endif
};
#endif

#if 0
static const struct adc_channel_cfg m_2nd_channel_cfg =   //!< ADC config for Battery input
{
	.gain = ADC_GAIN,
	.reference = ADC_REFERENCE,
	.acquisition_time = ADC_ACQUISITION_TIME,
	.channel_id = ADC_2ND_CHANNEL_ID,
#if defined(CONFIG_ADC_CONFIGURABLE_INPUTS)
	.input_positive = ADC_2ND_CHANNEL_INPUT,
#endif
};
#endif

#if 1
static const struct adc_channel_cfg m_1st_channel_cfg =
	{
		.gain = ADC_GAIN,
		.reference = ADC_REFERENCE,
		.acquisition_time = ADC_ACQUISITION_TIME,
		.channel_id = ADC_1ST_CHANNEL_ID,
#if defined(CONFIG_ADC_CONFIGURABLE_INPUTS)
		.input_positive = ADC_1ST_CHANNEL_INPUT,
#endif
};
#endif

static int16_t m_sample_buffer[ADC_BUFFER_SIZE]; //!< buffer to receive ADC data from configured channels

/*!
 * \fn int adc_sample(void)
 *
 * \brief get a single reading from ADC channel
 *
 * will read ADC_4TH_CHANNEL_ID\n
 * resoultion set to ADC_RESOLUTION in millivolts\n
 * will printout floating point value based on 3.6V max\n
 *
 * \return ADC_RESOLUTION value on ADC_4TH_CHANNEL_ID if pass or -1 if failed\n
 */
int adc_sample(void)
{
	int ret;

	const struct adc_sequence sequence =
		{
			.channels = BIT(ADC_1ST_CHANNEL_ID),
			//		.channels = BIT(ADC_1ST_CHANNEL_ID) | BIT(ADC_2ND_CHANNEL_ID) | BIT(ADC_3RD_CHANNEL_ID) | BIT(ADC_4TH_CHANNEL_ID),
			//		.channels =  BIT(ADC_2ND_CHANNEL_ID) | BIT(ADC_4TH_CHANNEL_ID),
			.buffer = m_sample_buffer,
			.buffer_size = sizeof(m_sample_buffer),
			.resolution = ADC_RESOLUTION,
		};

	if (!adc_dev)
	{
		return -1;
	}

	//    jfet_dk_set_led(BATMONENB,1);
	//    k_sleep(K_MSEC(500));
	// k_sleep(K_MSEC(1000));
	ret = adc_read(adc_dev, &sequence);
	if (ret != 0)
		printk("ADC read err: %d\n", ret);

	/* Print the AIN0 values */
	for (int i = 0; i < ADC_BUFFER_SIZE; i++)
	{
		//		adc_voltage = (float)(((float)m_sample_buffer[i] / 1023.0f) * 3600.0f);
		//		printk("Measured voltage: %3.2f V\n", adc_voltage);
		//        printf("den: %3.2f\r\n",((pow(2,ADC_RESOLUTION) -1 ) * 1.0)  * 3600.0f);
		adc_voltage[i] = (float)(((float)m_sample_buffer[i] / ((pow(2, ADC_RESOLUTION) - 1) * 1.0)) * 3600.0f);
		//		printk("Measured voltage: %3.2f V\n", adc_voltage);
		adc_voltage[i] = adc_voltage[i] / 1000.0;
		//        adc_voltage[i] = adc_voltage[i]/10000.0;
		//		printk("ADC raw value: %d 0x%04X\n", m_sample_buffer[i], m_sample_buffer[i]);
		//		printk("Measured voltage: %3.2f V\n", adc_voltage[i] * 10);
	}
	//    jfet_dk_set_led(BATMONENB,0);

	return ret;
}

/* end ADC test */

char device_name[40];
uint8_t device_name_len;

/* enums */
enum COMMANDS //!< vaild command indexes
{
	CR,
	NONE,
};

char const *validCmds[] = //!< array of valid commands - case insensitive
	{
		"cr",
		"END",
};

static const struct device *uart = DEVICE_DT_GET(DT_CHOSEN(nordic_nus_uart));
static struct k_work_delayable uart_work;

struct uart_data_t
{
	void *fifo_reserved;
	uint8_t data[UART_BUF_SIZE];
	uint16_t len;
};

static K_FIFO_DEFINE(fifo_uart_tx_data);
static K_FIFO_DEFINE(fifo_uart_rx_data);

// 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_NAME_COMPLETE, DEVICE_NAME, DEVICE_NAME_LEN),
// };

static struct bt_data ad[2] =
	{
		BT_DATA_BYTES(BT_DATA_FLAGS, (BT_LE_AD_GENERAL | BT_LE_AD_NO_BREDR)),
		BT_DATA(BT_DATA_NAME_COMPLETE, DEVICE_NAME, DEVICE_NAME_LEN),
};

static const struct bt_data sd[] =
	{
		BT_DATA_BYTES(BT_DATA_UUID128_ALL, BT_UUID_NUS_VAL),
};

#if CONFIG_BT_NUS_UART_ASYNC_ADAPTER
UART_ASYNC_ADAPTER_INST_DEFINE(async_adapter);
#else
static const struct device *const async_adapter;
#endif

static void uart_cb(const struct device *dev, struct uart_event *evt, void *user_data)
{
	ARG_UNUSED(dev);

	static size_t aborted_len;
	struct uart_data_t *buf;
	static uint8_t *aborted_buf;
	static bool disable_req;

	switch (evt->type)
	{
	case UART_TX_DONE:
		LOG_DBG("UART_TX_DONE");
		if ((evt->data.tx.len == 0) ||
			(!evt->data.tx.buf))
		{
			return;
		}

		if (aborted_buf)
		{
			buf = CONTAINER_OF(aborted_buf, struct uart_data_t,
							   data);
			aborted_buf = NULL;
			aborted_len = 0;
		}
		else
		{
			buf = CONTAINER_OF(evt->data.tx.buf, struct uart_data_t,
							   data);
		}

		k_free(buf);

		buf = k_fifo_get(&fifo_uart_tx_data, K_NO_WAIT);
		if (!buf)
		{
			return;
		}

		if (uart_tx(uart, buf->data, buf->len, SYS_FOREVER_MS))
		{
			LOG_WRN("Failed to send data over UART");
		}

		break;

	case UART_RX_RDY:
		LOG_DBG("UART_RX_RDY");
		buf = CONTAINER_OF(evt->data.rx.buf, struct uart_data_t, data);
		buf->len += evt->data.rx.len;

		if (disable_req)
		{
			return;
		}

		if ((evt->data.rx.buf[buf->len - 1] == '\n') ||
			(evt->data.rx.buf[buf->len - 1] == '\r'))
		{
			disable_req = true;
			uart_rx_disable(uart);
		}

		break;

	case UART_RX_DISABLED:
		LOG_DBG("UART_RX_DISABLED");
		disable_req = false;

		buf = k_malloc(sizeof(*buf));
		if (buf)
		{
			buf->len = 0;
		}
		else
		{
			LOG_WRN("Not able to allocate UART receive buffer");
			k_work_reschedule(&uart_work, UART_WAIT_FOR_BUF_DELAY);
			return;
		}

		uart_rx_enable(uart, buf->data, sizeof(buf->data),
					   UART_WAIT_FOR_RX);

		break;

	case UART_RX_BUF_REQUEST:
		LOG_DBG("UART_RX_BUF_REQUEST");
		buf = k_malloc(sizeof(*buf));
		if (buf)
		{
			buf->len = 0;
			uart_rx_buf_rsp(uart, buf->data, sizeof(buf->data));
		}
		else
		{
			LOG_WRN("Not able to allocate UART receive buffer");
		}

		break;

	case UART_RX_BUF_RELEASED:
		LOG_DBG("UART_RX_BUF_RELEASED");
		buf = CONTAINER_OF(evt->data.rx_buf.buf, struct uart_data_t,
						   data);

		if (buf->len > 0)
		{
			k_fifo_put(&fifo_uart_rx_data, buf);
		}
		else
		{
			k_free(buf);
		}

		break;

	case UART_TX_ABORTED:
		LOG_DBG("UART_TX_ABORTED");
		if (!aborted_buf)
		{
			aborted_buf = (uint8_t *)evt->data.tx.buf;
		}

		aborted_len += evt->data.tx.len;
		buf = CONTAINER_OF(aborted_buf, struct uart_data_t,
						   data);

		uart_tx(uart, &buf->data[aborted_len],
				buf->len - aborted_len, SYS_FOREVER_MS);

		break;

	default:
		break;
	}
}

static void uart_work_handler(struct k_work *item)
{
	struct uart_data_t *buf;

	buf = k_malloc(sizeof(*buf));
	if (buf)
	{
		buf->len = 0;
	}
	else
	{
		LOG_WRN("Not able to allocate UART receive buffer");
		k_work_reschedule(&uart_work, UART_WAIT_FOR_BUF_DELAY);
		return;
	}

	uart_rx_enable(uart, buf->data, sizeof(buf->data), UART_WAIT_FOR_RX);
}

static bool uart_test_async_api(const struct device *dev)
{
	const struct uart_driver_api *api =
		(const struct uart_driver_api *)dev->api;

	return (api->callback_set != NULL);
}

static int uart_init(void)
{
	int err;
	int pos;
	struct uart_data_t *rx;
	struct uart_data_t *tx;

	if (!device_is_ready(uart))
	{
		return -ENODEV;
	}

	if (IS_ENABLED(CONFIG_USB_DEVICE_STACK))
	{
		err = usb_enable(NULL);
		if (err && (err != -EALREADY))
		{
			LOG_ERR("Failed to enable USB");
			return err;
		}
	}

	rx = k_malloc(sizeof(*rx));
	if (rx)
	{
		rx->len = 0;
	}
	else
	{
		return -ENOMEM;
	}

	k_work_init_delayable(&uart_work, uart_work_handler);

	if (IS_ENABLED(CONFIG_BT_NUS_UART_ASYNC_ADAPTER) && !uart_test_async_api(uart))
	{
		/* Implement API adapter */
		uart_async_adapter_init(async_adapter, uart);
		uart = async_adapter;
	}

	err = uart_callback_set(uart, uart_cb, NULL);
	if (err)
	{
		k_free(rx);
		LOG_ERR("Cannot initialize UART callback");
		return err;
	}

	if (IS_ENABLED(CONFIG_UART_LINE_CTRL))
	{
		LOG_INF("Wait for DTR");
		while (true)
		{
			uint32_t dtr = 0;

			uart_line_ctrl_get(uart, UART_LINE_CTRL_DTR, &dtr);
			if (dtr)
			{
				break;
			}
			/* Give CPU resources to low priority threads. */
			k_sleep(K_MSEC(100));
		}
		LOG_INF("DTR set");
		err = uart_line_ctrl_set(uart, UART_LINE_CTRL_DCD, 1);
		if (err)
		{
			LOG_WRN("Failed to set DCD, ret code %d", err);
		}
		err = uart_line_ctrl_set(uart, UART_LINE_CTRL_DSR, 1);
		if (err)
		{
			LOG_WRN("Failed to set DSR, ret code %d", err);
		}
	}

	tx = k_malloc(sizeof(*tx));

	if (tx)
	{
		pos = snprintf(tx->data, sizeof(tx->data), "Starting BLE Prototype\r\n");

		if ((pos < 0) || (pos >= sizeof(tx->data)))
		{
			k_free(rx);
			k_free(tx);
			LOG_ERR("snprintf returned %d", pos);
			return -ENOMEM;
		}

		tx->len = pos;
	}
	else
	{
		k_free(rx);
		return -ENOMEM;
	}

	err = uart_tx(uart, tx->data, tx->len, SYS_FOREVER_MS);
	if (err)
	{
		k_free(rx);
		k_free(tx);
		LOG_ERR("Cannot display welcome message (err: %d)", err);
		return err;
	}

	err = uart_rx_enable(uart, rx->data, sizeof(rx->data), 50);
	if (err)
	{
		LOG_ERR("Cannot enable uart reception (err: %d)", err);
		/* Free the rx buffer only because the tx buffer will be handled in the callback */
		k_free(rx);
	}

	return err;
}

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

	if (err)
	{
		LOG_ERR("Connection failed (err %u)", err);
		return;
	}

	bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));
	LOG_INF("Connected %s", addr);

	current_conn = bt_conn_ref(conn);

	dk_set_led_on(CON_STATUS_LED);
}

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_INF("Disconnected: %s (reason %u)", addr, reason);

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

	if (current_conn)
	{
		bt_conn_unref(current_conn);
		current_conn = NULL;
		dk_set_led_off(CON_STATUS_LED);
	}
}

#ifdef CONFIG_BT_NUS_SECURITY_ENABLED
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_INF("Security changed: %s level %u", addr, level);
	}
	else
	{
		LOG_WRN("Security failed: %s level %u err %d", addr,
				level, err);
	}
}
#endif

BT_CONN_CB_DEFINE(conn_callbacks) = {
	.connected = connected,
	.disconnected = disconnected,
#ifdef CONFIG_BT_NUS_SECURITY_ENABLED
	.security_changed = security_changed,
#endif
};

#if defined(CONFIG_BT_NUS_SECURITY_ENABLED)
static void auth_passkey_display(struct bt_conn *conn, unsigned int passkey)
{
	char addr[BT_ADDR_LE_STR_LEN];

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

	LOG_INF("Passkey for %s: %06u", addr, passkey);
}

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

	auth_conn = bt_conn_ref(conn);

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

	LOG_INF("Passkey for %s: %06u", addr, passkey);
	LOG_INF("Press Button 1 to confirm, Button 2 to reject.");
}

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_INF("Pairing cancelled: %s", addr);
}

static void pairing_complete(struct bt_conn *conn, bool bonded)
{
	char addr[BT_ADDR_LE_STR_LEN];

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

	LOG_INF("Pairing completed: %s, bonded: %d", addr, bonded);
}

static void pairing_failed(struct bt_conn *conn, enum bt_security_err reason)
{
	char addr[BT_ADDR_LE_STR_LEN];

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

	LOG_INF("Pairing failed conn: %s, reason %d", addr, reason);
}

static struct bt_conn_auth_cb conn_auth_callbacks = {
	.passkey_display = auth_passkey_display,
	.passkey_confirm = auth_passkey_confirm,
	.cancel = auth_cancel,
};

static struct bt_conn_auth_info_cb conn_auth_info_callbacks = {
	.pairing_complete = pairing_complete,
	.pairing_failed = pairing_failed};
#else
static struct bt_conn_auth_cb conn_auth_callbacks;
static struct bt_conn_auth_info_cb conn_auth_info_callbacks;
#endif

static void bt_receive_cb(struct bt_conn *conn, const uint8_t *const data,
						  uint16_t len)
{
	int err;
	char addr[BT_ADDR_LE_STR_LEN] = {0};

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

	LOG_INF("Received data from: %s", addr);

	for (uint16_t pos = 0; pos != len;)
	{
		struct uart_data_t *tx = k_malloc(sizeof(*tx));

		if (!tx)
		{
			LOG_WRN("Not able to allocate UART send data buffer");
			return;
		}

		/* Keep the last byte of TX buffer for potential LF char. */
		size_t tx_data_size = sizeof(tx->data) - 1;

		if ((len - pos) > tx_data_size)
		{
			tx->len = tx_data_size;
		}
		else
		{
			tx->len = (len - pos);
		}

		memcpy(tx->data, &data[pos], tx->len);

		pos += tx->len;

		/* Append the LF character when the CR character triggered
		 * transmission from the peer.
		 */
		if ((pos == len) && (data[len - 1] == '\r'))
		{
			tx->data[tx->len] = '\n';
			tx->len++;
		}

		err = uart_tx(uart, tx->data, tx->len, SYS_FOREVER_MS);
		if (err)
		{
			k_fifo_put(&fifo_uart_tx_data, tx);
		}

		/* show TX sent */
		tx->data[tx->len] = '\0';
		err = bleCommand(tx->data);
	}
}

static struct bt_nus_cb nus_cb = {
	.received = bt_receive_cb,
};

void error(void)
{
	dk_set_leds_state(DK_ALL_LEDS_MSK, DK_NO_LEDS_MSK);

	//myprintkE("Fatal Error!!\r\n");
	while (true)
	{
		/* Spin for ever */
		k_sleep(K_MSEC(1000));
	}
}

#ifdef CONFIG_BT_NUS_SECURITY_ENABLED
static void num_comp_reply(bool accept)
{
	if (accept)
	{
		bt_conn_auth_passkey_confirm(auth_conn);
		LOG_INF("Numeric Match, conn %p", (void *)auth_conn);
	}
	else
	{
		bt_conn_auth_cancel(auth_conn);
		LOG_INF("Numeric Reject, conn %p", (void *)auth_conn);
	}

	bt_conn_unref(auth_conn);
	auth_conn = NULL;
}

void button_changed(uint32_t button_state, uint32_t has_changed)
{
	uint32_t buttons = button_state & has_changed;

	if (auth_conn)
	{
		if (buttons & KEY_PASSKEY_ACCEPT)
		{
			num_comp_reply(true);
		}

		if (buttons & KEY_PASSKEY_REJECT)
		{
			num_comp_reply(false);
		}
	}
}
#endif /* CONFIG_BT_NUS_SECURITY_ENABLED */

static void configure_gpio(void)
{
	int err;

#ifdef CONFIG_BT_NUS_SECURITY_ENABLED
	err = dk_buttons_init(button_changed);
	if (err)
	{
		LOG_ERR("Cannot init buttons (err: %d)", err);
	}
#endif /* CONFIG_BT_NUS_SECURITY_ENABLED */

	err = dk_leds_init();
	if (err)
	{
		LOG_ERR("Cannot init LEDs (err: %d)", err);
	}
}

/*
 * \fn int main(void)
 *
 * \brief main() starting point
 *
 *	sets up all interfaces, peripherals, and BLE operations
 *
 * \return TRUE if passed, FALSE if failed<p>
 *
 */
int main(void)
{
	int err = 0;
	//char str[120];
	uint8_t status;
	uint8_t byte[2];
	uint8_t tempStatus;
	uint8_t humidityStatus;
	uint8_t intEnables;

	/* find the reason that we started the program */
	ret = NRF_POWER->RESETREAS;
	if (ret == 0)
		//sprintfresetCause, "Power applied reset, ");
	if (ret & 0x1)
		strcat(resetCause, "Pin reset, ");
	if (ret & 0x2)
		strcat(resetCause, "Global Watchdog reset, ");
	if (ret & 0x4)
		strcat(resetCause, "Wakeup from system off detect from GPIO reset, ");
	if (ret & 0x10)
		strcat(resetCause, "Wakeup from system off debug interface mode reset, ");
	if (ret & 0x10000)
		strcat(resetCause, "AIRCR.SYSRESETREQ reset, ");
	if (ret & 0x20000)
		strcat(resetCause, "CPU lockup reset, ");
	if (ret & 0x40000)
		strcat(resetCause, "CTRL_AP reset, ");
	if (strlen(resetCause) > 90)
		resetCause[90] = '\0';
	resetCause[strlen(resetCause) - 2] = '\0';
	NRF_POWER->RESETREAS = ret;
	//sprintfstr, "resetCause: %s\r\n", resetCause);
	//myprintkS(str);

	configure_gpio(); /* configure buttons and LEDS */

	/* start GPIO test */
	printk("*** GPIO Init ***\r\n");

	/* check if hdc2080 (GPIO Inoput) ready */
	if (!gpio_is_ready_dt(&hdc2080))
	{
		//sprintfstr, "Error: HDC2080 GPIO %s is not ready\n", hdc2080.port->name);
		//myprintkE(str);
		error();
	}
	//sprintfstr, "HDC2080 GPIO %s is ready\n", hdc2080.port->name);
	//myprintkI(str);

	/* configure hdc2080 GPIO as input */
	err = gpio_pin_configure_dt(&hdc2080, GPIO_INPUT);
	if (err)
	{
		//sprintfstr, "Error %d: failed to configure %s pin %d\n", err, hdc2080.port->name, hdc2080.pin);
		//myprintkE(str);
		error();
	}
	//sprintfstr, "HDC2080 passed configure %s pin %d\n", hdc2080.port->name, hdc2080.pin);
	//myprintkI(str);

	/* configure HDC2080 GPIO interrupt */
	//	ret = gpio_pin_interrupt_configure_dt(&hdc2080   GPIO_INT_EDGE_TO_ACTIVE);
	err = gpio_pin_interrupt_configure_dt(&hdc2080, GPIO_INT_EDGE_FALLING);
	//	ret = gpio_pin_interrupt_configure_dt(&hdc2080,   GPIO_INT_EDGE_RISING);
	//	ret = gpio_pin_interrupt_configure_dt(&hdc2080,   GPIO_INT_EDGE_BOTH);
	if (err != 0)
	{
		//sprintfstr, "Error %d: failed to configure interrupt on %s pin %d\n", err, hdc2080.port->name, hdc2080.pin);
		//myprintkE(str);
		error();
	}
	//sprintfstr, "HDC2080 passed configure interrupt on %s pin %d\n", hdc2080.port->name, hdc2080.pin);
	//myprintkI(str);

	/* set up HDC2080 GPIO interrupt callback */
	gpio_init_callback(&hdc2080_cb_data, button_pressed, BIT(hdc2080.pin));
	gpio_add_callback(hdc2080.port, &hdc2080_cb_data);
	//sprintfstr, "Set up HDC2080 at %s pin %d\n", hdc2080.port->name, hdc2080.pin);
	//myprintkI(str);

	/* set up SPI CS1N as output/pullup */
	if (!gpio_is_ready_dt(&spics1n))
	{
		//myprintkE("SPI CS1N device is not ready\r\n");
	}
	else
		//myprintkI("SPI CS1N device is ready\r\n");
	gpio_pin_configure_dt(&spics1n, GPIO_OUTPUT | GPIO_PULL_UP | GPIO_ACTIVE_LOW);
	gpio_pin_set(spics1n.port, spics1n.pin, 0);

	/* set up SPI CS2N as output/pullup */
	if (!gpio_is_ready_dt(&spics2n))
	{
		//myprintkE("SPI CS2N device is not ready\r\n");
	}
	else
		//myprintkI("SPI CS2N device is ready\r\n");
	gpio_pin_configure_dt(&spics2n, GPIO_OUTPUT | GPIO_PULL_UP | GPIO_ACTIVE_LOW);
	gpio_pin_set(spics2n.port, spics2n.pin, 0);

	/* set up BATEN as output/pullup */
	if (!gpio_is_ready_dt(&baten))
	{
		//myprintkE("BATEN device is not ready\r\n");
	}
	else
		//myprintkI("BATEN device is ready\r\n");
	gpio_pin_configure_dt(&baten, GPIO_OUTPUT | GPIO_PULL_UP);

#if 0
	int test = 0;
	while(1)
	{
		/* toggle test */
//		gpio_pin_toggle(spics1n.port, spics1n.pin);
//		k_sleep(K_MSEC(10));

		/* set/clear test */
		if(test++ & 0x01)
		{
//			printk("set\r\n");
			gpio_pin_set(spics1n.port, spics1n.pin, 1);
			k_sleep(K_MSEC(10));
		}
		else
		{
//			printk("clear\r\n");
			gpio_pin_set(spics1n.port, spics1n.pin, 0);
			k_sleep(K_MSEC(20));
		}
	}
#endif

#if 0
	int test = 0;
	while(1)
	{
		/* toggle test */
//		gpio_pin_toggle(spics2n.port, spics2n.pin);
//		k_sleep(K_MSEC(10));

		/* set/clear test */
		if(test++ & 0x01)
		{
//			printk("set\r\n");
			gpio_pin_set(spics2n.port, spics2n.pin, 1);
		}
		else
		{
//			printk("clear\r\n");
			gpio_pin_set(spics2n.port, spics2n.pin, 0);
		}
		k_sleep(K_MSEC(20));
	}
#endif

#if 0
	int test = 0;
	while(1)
	{
		/* toggle test */
//		gpio_pin_toggle(baten.port, baten.pin);
//		k_sleep(K_MSEC(10));

		/* set/clear test */
		if(test++ & 0x01)
		{
//			printk("set\r\n");
			gpio_pin_set(baten.port, baten.pin, 1);
		}
		else
		{
//			printk("clear\r\n");
			gpio_pin_set(baten.port, baten.pin, 0);
		}
		k_sleep(K_MSEC(20));
	}
#endif

	/* end GPIO test */

	err = uart_init(); /* initialize UART */
	if (err)
	{
		//myprintkE("UART device is not ready\r\n");
		error();
	}
	//myprintkI("UART device is ready\r\n");

	err = init_i2c(); /* initialize I2C peripheral returns TRUE if passed */
	if (err == FALSE)
	{
		//myprintkE("I2C interface is not ready\r\n");
		error();
	}
	//myprintkI("I2C interface is ready\r\n");

/* start I2C test */
#if 0
	unsigned char datas[6];
	while(1)
	{
		/* 4. verify i2c_read() */
		datas[0] = 0xFC;
		i2c_write(i2c_dev, datas, 1, 0x40);

		i2c_read(i2c_dev, datas, 2, 0x40);
		printk("I2C 0x%02X 0x%02X\r\n",datas[0],datas[1]);
		k_sleep(K_MSEC(1000));
	}
#endif

	/* end I2C test */

	/* start SPI test */

	printk("*** SPI Init ***\r\n");

	if (!device_is_ready(spi_dev))
	{
		//myprintkE("SPI device is not ready\r\n");
	}
	else
		//myprintkI("SPI device is ready\r\n");

#if 0

	printk("SPI Test Running\r\n");
	while(1)
	{
		/* zero buffer and load the command */
		for(int i=0;i<4;i++)
			buffer_tx[i] = 0;
		buffer_tx[0] = 0x9F;
		tx_bufs[0].len = 4;
		rx_bufs[0].len = 4;
 
    /* enable CS1 and send command  */
     	ret = spi_transceive(spi_dev, &spi_cfg1, &tx, &rx);
		k_sleep(K_MSEC(10));

    /* enable CS2 and send command  */
     	ret = spi_transceive(spi_dev, &spi_cfg2, &tx, &rx);
		k_sleep(K_MSEC(10));
	}
#endif

/* end SPi test */

/* start Flash test */
#if 0
	uint8_t readBuffer[10];
//uint8_t flashRead[256];
//uint8_t data[3];

	printk("Flash Test Running\r\n");
	while(1)
	{
//		ret = initW25Q64JV();
		readNVFlashxStatus(readBuffer);
//		flashxErasePage(1);
//		flashxEraseBlock1(1);
//		flashxEraseBlock2(1);
//		flashxEraseSector(1);
//		flashxErase();
//		flashxReadPage(1, flashRead);
//		data[0] = 0xAB;
//		flashxWritePage(1, 3, data, 1);
//		flashxFillPage(1, 0xCD);
//		flashxClearParameters();
//		setFlashxParameters(TRUE);


		k_sleep(K_MSEC(100));
	}
#endif

	/* end Flash test */

	/* start ADC test */

	printk("*** ADC Init ***\r\n");

	if (!device_is_ready(adc_dev)) /* check if ADC peripheral ready */
	{
		//myprintkE("ADC device is not ready\n");
		return FALSE;
	}
	//myprintkI("ADC device is ready\n");

	err = adc_channel_setup(adc_dev, &m_1st_channel_cfg);
	if (err)
	{
		//sprintfstr, "Error in adc AIN0 setup: %d\n", err);
		//myprintkE(str);
	}
	//myprintkI("ADC AIN0 is ready\n");

#if 0

	printk("ADC Test Running\r\n");
	while(1)
	{
		err = adc_sample();
		printf("ADC: %3.2f\r\n",adc_voltage[0]);
		k_sleep(K_MSEC(5000));
	}
#endif

	/* end ADC test */

	/* get Nordic unique device ID */
	//sprintfdevIDStr, "%08X_%08X", NRF_FICR->DEVICEID[0], NRF_FICR->DEVICEID[1]);
	//	sprintf(devIDStr,"%08X",NRF_FICR->DEVICEID[0]);
	//sprintfstr, "Device ID: %s\r\n", devIDStr);
	//myprintkS(str);
	//sprintfstr, "Board: %s%s\r\nFW VERSION: V %d.%02d %s%s\r\n", BOARD_NAME, BOARD_NAME_EXT, VERSION_MAJOR, VERSION_MINOR, VERSION_DATE, VERSION_DATE_EXT);
	//myprintkS(str);
	//sprintfstr, "Built:%s %s\r\n", __DATE__, __TIME__);
	//myprintkS(str);

	/* is BLE security UART enabled */
	if (IS_ENABLED(CONFIG_BT_NUS_SECURITY_ENABLED))
	{
		err = bt_conn_auth_cb_register(&conn_auth_callbacks); /* register authorization callbacks */
		if (err)
		{
			//myprintkE("Failed to register BLE authorization callbacks.\n");
			error();
		}
		//myprintkI("Passed register BLE authorization callbacks.\n");

		err = bt_conn_auth_info_cb_register(&conn_auth_info_callbacks); /* register authorization info callbacks */
		if (err)
		{
			//myprintkE("Failed to register BLE authorization info callbacks.\n");
			error();
		}
		//myprintkI("passed register BLE authorization info callbacks.\n");
	}

	err = bt_enable(NULL); /* enable Bluetooth */
	if (err)
	{
		//sprintfstr, "Failed to enable BLE. %d\n", err);
		//myprintkE(str);
		error();
	}
	//myprintkI("Bluetooth Enabled\r\n");

	k_sem_give(&ble_init_ok); /* set semaphore to indicate BLE OK */

	if (IS_ENABLED(CONFIG_SETTINGS))
	{
		err = settings_load();
	}

	err = bt_nus_init(&nus_cb); /* initialize Nordic UART Service */
	if (err)
	{
		//sprintfstr, "Failed to initialize UART service (err: %d)", err);
		//myprintkE(str);
		error();
	}
	//myprintkI("initialized BLE UART service\r\n");

	/* create unique BLE advertizer name */
	strcpy(device_name, DEVICE_NAME);
//	strcat(device_name, devIDStr);
	device_name_len = strlen(device_name);
	//sprintfstr, "BLE Advertizing name: %s\r\n", device_name);
	//myprintkS(str);
	// ad[1].type = BT_DATA_NAME_COMPLETE;
	// ad[1].data_len = device_name_len;
	// ad[1].data = device_name;

	err = bt_le_adv_start(BT_LE_ADV_CONN, ad, ARRAY_SIZE(ad), sd, ARRAY_SIZE(sd)); /* start BLE advertising */
	if (err)
	{
		//sprintfstr, "Advertising failed to start (err %d)", err);
		//myprintkE(str);
		error();
	}
	//sprintfstr, "Advertising started (name %s)\r\n", device_name);
	//myprintkI(str);

	rescan(); /* scasn for all I2C and SPI devices */

	for (;;)
	{

		/* Don't go any further until HDC2080 RDY seen */
		k_sem_take(&hdc_rdy_seen, K_FOREVER);
		{
			readReg(MEASUREMENT_CONFIG, byte, 1);
			//sprintfstr, "meas config: 0x%02X\r\n", byte[0]);
			//myprintkI(str);
			//sprintfstr, "Low T: %3.2f High T: %3.2f \r\n", readLowTempThreshold(), readHighTempThreshold());
			//myprintkI(str);
			//sprintfstr, "Low H: %3.2f High H: %3.2f \r\n", readLowHumidityThreshold(), readHighHumidityThreshold());
			//myprintkI(str);

			status = readInterruptStatus();			   /* get current interupt status */
			readReg(INTERRUPT_CONFIG, &intEnables, 1); /* get current interup enables */

			hdcTemp = readTemp();
			hdcHumidity = readHumidity();
			//sprintfstr, "Temp: %3.2f Humidity: %3.2f 0x%02X\r\n", hdcTemp, hdcHumidity, status);
			//myprintkI(str);

			/* get interupt state for temp and humidity */
			tempStatus = 0;
			humidityStatus = 0;
			/* set up temp status */
			if (status & TL_STATUS)
			{
				tempStatus |= LOWTEMPACTIVE;
			}
			if (status & TH_STATUS)
			{
				tempStatus |= HIGHTEMPACTIVE;
			}
			if (intEnables & TL_ENABLE)
			{
				tempStatus |= LOWTEMPENABLE;
			}
			if (intEnables & TH_ENABLE)
			{
				tempStatus |= HIGHTEMPENABLE;
			}

			/* set up humidity status */
			if (status & HL_STATUS)
			{
				humidityStatus |= LOWHUMIDITYACTIVE;
			}
			if (status & HH_STATUS)
			{
				humidityStatus |= HIGHHUMIDITYACTIVE;
			}
			if (intEnables & HL_ENABLE)
			{
				humidityStatus |= LOWHUMIDITYENABLE;
			}
			if (intEnables & HH_ENABLE)
			{
				humidityStatus |= HIGHHUMIDITYENABLE;
			}

			if ((byte[0] & 0x6) == 0x0)
			{
				//sprintfstr, "ID: %s t_hdc %3.2f %02X h_hdc %3.2f %02X\r\n", devIDStr, hdcTemp, tempStatus, hdcHumidity, humidityStatus);
			}
			else if ((byte[0] & 0x6) == 0x2)
			{
				//sprintfstr, "ID: %s t_hdc %3.2f %02X\r\n", devIDStr, hdcTemp, tempStatus);
			}
			else if ((byte[0] & 0x6) == 0x4)
			{
				//sprintfstr, "ID: %s h_hdc %3.2f %02X\r\n", devIDStr, hdcHumidity, humidityStatus);
			}
			if (status & DRDY_STATUS)
				//myprintkW("DRDY Set\r\n");
			if (status & TH_STATUS)
				//myprintkW("Temp High Set\r\n");
			if (status & TL_STATUS)
				//myprintkW("Temp Low Set\r\n");
			if (status & HH_STATUS)
				//myprintkW("Humidity High Set\r\n");
			if (status & HL_STATUS);
				//myprintkW("Humidity Low Set\r\n");
			// if (bt_nus_send(NULL, str, strlen(str)))
			// {
			// 	LOG_WRN("Failed to send data over BLE connection");
			// }
		}
	}
}

void ble_write_thread(void)
{
	uint8_t myBuff[80];
	//	uint8_t str[40];
	//	uint8_t status;
	int i;

	/* Don't go any further until BLE is initialized */
	k_sem_take(&ble_init_ok, K_FOREVER);

	for (;;)
	{
		/* Wait indefinitely for data to be sent over bluetooth */
		struct uart_data_t *buf = k_fifo_get(&fifo_uart_rx_data, K_FOREVER); /* get uart data */

		/* show RX received */
		//		hdcTemp = readTemp();
		//		hdcHumidity = readHumidity();
		//		status = readInterruptStatus();
		//		sprintf(str,"T: %3.2f H: %3.2f \r\n",hdcTemp, hdcHumidity);
		//		printk("Tempx: %3.2f Humidity: %3.2f 0x%02X\r\n",hdcTemp, hdcHumidity,status);
		strncpy(myBuff, buf->data, buf->len);
		myBuff[buf->len] = '\0';
		for (i = 0; i < buf->len + 1; i++)
		{
			if ((myBuff[i] == '\r') || (myBuff[i] == '\n'))
				myBuff[i] = '\0';
			//			printk("i: %d 0x%02X %c\r\n",i,myBuff[i],myBuff[i]);
		}
		//		printk("\r\nRX Line: %s Len %d\r\n",myBuff,strlen(myBuff));
		executeCmd(myBuff);
//		printk("myBuff1 %s\r\n",myBuff);
#if 0
		strcat(myBuff,str);
//		strcat(myBuff,"xyz");
		printk("myBuff2 %s\r\n",myBuff);
		printk("str %s\r\n",str);
		printk("\r\nRX Line: %s Len %d\r\n",myBuff,strlen(myBuff));
//		if (bt_nus_send(NULL, buf->data, buf->len)) 
		if (bt_nus_send(NULL, myBuff, strlen(myBuff)) )
		{
			LOG_WRN("Failed to send data over BLE connection");
		}
#endif

		k_free(buf);
	}
}

/* TJM added functions */
int init_i2c()
{
	uint32_t i2c_cfg = I2C_SPEED_SET(I2C_SPEED_FAST) | I2C_MODE_CONTROLLER;
	char str[40];

	printk("*** I2C Init ***\r\n");
	//	myPrintkS("Initializing I2C device\r\n");

	if (!device_is_ready(i2c_dev)) /* check if I2C peripheral ready */
	{
		//myprintkE("I2C device is not ready\n");
		return FALSE;
	}
	//myprintkI("I2C device is ready\n");

	/* 1. verify i2c_configure() */
	if (i2c_configure(i2c_dev, i2c_cfg)) /* configure I2C peripheral */
	{
		//myprintkE("I2C config failed\n");
		return FALSE;
	}
	//sprintfstr, "I2C config passed %08X\n", i2c_cfg);
	//myprintkI(str);
	return TRUE;
}

void button_pressed(const struct device *dev, struct gpio_callback *cb, uint32_t pins)
{
	//	int pinx,piny;

	if (BIT(hdc2080.pin) & pins)
	{
		//		pinx = gpio_pin_get_raw(hdc2080.port,hdc2080.pin);
		//		piny = gpio_pin_get(hdc2080.port,hdc2080.pin);
		//		printk("Button %d %d %08X pressed at %" PRIu32 "\n",pinx, piny, pins, k_cycle_get_32());

		/* show RX received */
		k_sem_give(&hdc_rdy_seen); /* indicate HDC2080 RDY seen */
	}
	else
		printk("unknown button!!\r\n");
}

uint8_t bleCommand(uint8_t *data)
{
	char str[80];
	int cmd;

	//sprintfstr, "\r\n BLE Command: %s Len %d\r\n", data, strlen(data));
	//myprintkI(str);
	cmd = getCmd(data);
	printk("cmd: %d numParameters: %d ", cmd, numParameters);
	//	for(int i=0;i< numParameters;i++)
	//	{
	//		printk("i: %d %s\r\n",i,cmdLineParameter[i]);
	//	}

	/* execute BLE command */
	switch (cmd)
	{
	case CR:
		printk("Conv Rate %d\r\n", atoi(cmdLineParameter[1]));
		setRate(atoi(cmdLineParameter[1])); /* set auto conversion rate */
		break;

	default:
		//sprintfstr, "unknown BLE command %s\r\n", data);
		//myprintkW(str);
	}
	return TRUE;
}

int getCmd(uint8_t *line)
{
	int i;
	int len = strlen(line);

	/* parse all parameters on the command line into cmdLineparameter array */
	numParameters = 0; /* no parameters seen yet */

	cmdLineParameter[numParameters++] = &line[0];
	for (i = 0; i < len; i++)
	{
		//    	printk("%d %c\r\n",i,line[i]);
		if (line[i] == ' ')
		{
			line[i] = '\0';
			cmdLineParameter[numParameters++] = &line[i + 1];
		}
	}
	line[i] = '\0';
	//  	for(i=0;i<numParameters;i++)
	//		printk("i: %d %s\r\n",i,cmdLineParameter[i]);

	for (i = 0; i < sizeof(validCmds) / sizeof(validCmds[0]); i++)
	{
		if (strcmp(validCmds[i], cmdLineParameter[0]) == 0)
		{
			return i;
		}
	}
	return -1;
}

/**
 * \fn void rescan(void)
 *
 * \brief rescan all I2C and SPI devices
 *
 * \return none
 *
 */
void rescan(void)
{
	int err;

	err = initHDC2080(); /* check if HDC2080 available */
						 //	if (err == FALSE)
						 //	{
						 //		error();
						 //	}

	err = initW25Q64JV(); /* check if HDC2080 available */
						  //	if (err == FALSE)
						  //	{
						  //		error();
						  //	}
	return;
}

/**
 * \fn void myPrintf(char *str)
 *
 * \brief prints a string to the UART
 *
 * @param str ASCII string to print
 *
 * \return HAL_OK if passed<p>
 *
 */
void myPrintf(char *str)
{
	printk("%s", str);
	return;
}

/*
 * Black \033[0;30m.
 * Red \033[0;31m.
 * Green \033[0;32m.
 * Yellow \033[0;33m.
 * Blue \033[0;34m.
 * Purple \033[0;35m.
 * Cyan \033[0;36m.
 * White \033[0;37m.
 */

/* yellow (Warning)*/
void myPrintkW(char *str)
{
	// printk("%s", "\33[1;33m");
	// printk("%s", str);
	// printk("%s", "\33[0m");
	return;
}

/* Green (Information)*/
void myPrintkI(char *str)
{
	// printk("%s", "\33[1;32m");
	// printk("%s", str);
	// printk("%s", "\33[0m");
	return;
}

/* CYAN (Status)*/
void myPrintkS(char *str)
{
	// printk("%s", "\33[1;36m");
	// printk("%s", str);
	// printk("%s", "\33[0m");
	return;
}

/* Red (Error)*/
void myPrintkE(char *str)
{
	// printk("%s", "\33[1;31m");
	// printk("%s", str);
	// printk("%s", "\33[0m");
	return;
}
K_THREAD_DEFINE(ble_write_thread_id, STACKSIZE, ble_write_thread, NULL, NULL,
				NULL, PRIORITY, 0, 0);
