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

/** @file
 *  @brief Nordic UART Service Client sample
 */

#include <errno.h>
#include <zephyr/device.h>
#include <zephyr/devicetree.h>
#include <zephyr/kernel.h>
#include <zephyr/sys/byteorder.h>
#include <zephyr/sys/printk.h>

#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 <bluetooth/gatt_dm.h>
#include <bluetooth/scan.h>
#include <bluetooth/services/nus.h>
#include <bluetooth/services/nus_client.h>

#include <zephyr/settings/settings.h>

#include <zephyr/drivers/uart.h>

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

/* merge */
#ifdef LTE_MERGE
// #include <zephyr/kernel.h>
// #include <stdio.h>
// #include <string.h>
// #include <stdlib.h>
// #include <app_event_manager.h>
// #include <modem/nrf_modem_lib.h>
// #include <zephyr/sys/reboot.h>
// #include <net/nrf_cloud.h>

/* Module name is used by the Application Event Manager macros in this file */
// #define MODULE main
// #include <caf/events/module_state_event.h>

// #include "modules_common.h"
// #include "events/app_module_event.h"
// #include "events/cloud_module_event.h"
// #include "events/data_module_event.h"
// #include "events/sensor_module_event.h"
// #include "events/util_module_event.h"
// #include "events/modem_module_event.h"

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

/* uncomment out if using thread */
#define USE_THREAD 1

#define LOG_MODULE_NAME central_uart
LOG_MODULE_REGISTER(LOG_MODULE_NAME, 4);

///* UART payload buffer element size. */
// #define UART_BUF_SIZE 20

#define KEY_PASSKEY_ACCEPT DK_BTN1_MSK
#define KEY_PASSKEY_REJECT DK_BTN2_MSK

#define NUS_WRITE_TIMEOUT K_MSEC(150)
#define UART_WAIT_FOR_BUF_DELAY K_MSEC(50)
#define UART_RX_TIMEOUT 50000 /* Wait for RX complete event time in microseconds. */

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

K_SEM_DEFINE(nus_write_sem, 0, 1);

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

static K_FIFO_DEFINE(fifo_uart_tx_data);
K_FIFO_DEFINE(fifo_uart_rx_data);

static struct bt_conn *default_conn;
static struct bt_conn *Gconn[CONFIG_BT_MAX_CONN];
static uint8_t conn_count = 0;
static struct bt_nus_client nus_client;

static void ble_data_sent(struct bt_nus_client *nus, uint8_t err, const uint8_t *const data, uint16_t len) {
  ARG_UNUSED(nus);
  ARG_UNUSED(data);
  ARG_UNUSED(len);

  k_sem_give(&nus_write_sem);

  if (err) {
    LOG_WRN("ATT error code: 0x%02X", err);
  }
}

static uint8_t ble_data_received(struct bt_nus_client *nus, const uint8_t *data, uint16_t len) {
  ARG_UNUSED(nus);

  char bleData[60];
  //	int64_t time_ms = k_uptime_get();

  memset(bleData, 0, sizeof(bleData));
  strncpy(bleData, data, len);
  bleData[len] = '\0';
  // myPrintkW("ble_data_received() entered... %s\n", bleData);

  //	printk("ble_data_received() entered... len=%d\n", len);
  //	printk("ble_data_received() uptime %3.2f ... len=%d\n", time_ms / 1000.0, len);
  //	printk("ble_data_received() uptime %3.2f len=%d ", time_ms / 1000.0, len);
  //	myPrintkI("\"%s\"\r\n", bleData);
  parseBLEString(bleData);
  //	myPrintkI("BLEID: %s BLEHDCTemp: %s BLEHDCHum: %3.2f\r\n", BLEID, BLEHDCTemp, BLEHDCHum);
  ARG_UNUSED(nus);

#if 0
	int err;

	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 BT_GATT_ITER_CONTINUE;
		}

		/* 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);
		}
		k_free(tx);
	}
#endif

  return BT_GATT_ITER_CONTINUE;
}

#if 0
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[0]);
      aborted_buf = NULL;
      aborted_len = 0;
    } else {
      buf = CONTAINER_OF(evt->data.tx.buf, struct uart_data_t, data[0]);
    }

    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[0]);
    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_RX_TIMEOUT);

    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");
    printf("\r\n\nUART_RX_BUF_RELEASED\r\n");
    buf = CONTAINER_OF(evt->data.rx_buf.buf, struct uart_data_t, data[0]);

    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[0]);

    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_RX_TIMEOUT);
}

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

  if (!device_is_ready(uart)) {
    LOG_ERR("UART device not ready");
    return -ENODEV;
  }

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

  k_work_init_delayable(&uart_work, uart_work_handler);

  err = uart_callback_set(uart, uart_cb, NULL);
  if (err) {
    return err;
  }

  return uart_rx_enable(uart, rx->data, sizeof(rx->data), UART_RX_TIMEOUT);
}
#endif

static void discovery_complete(struct bt_gatt_dm *dm, void *context) {
  struct bt_nus_client *nus = context;
  LOG_INF("Service discovery completed");

  bt_gatt_dm_data_print(dm);

  bt_nus_handles_assign(dm, nus);
  bt_nus_subscribe_receive(nus);

  bt_gatt_dm_data_release(dm);
}

static void discovery_service_not_found(struct bt_conn *conn, void *context) { LOG_INF("Service not found"); }

static void discovery_error(struct bt_conn *conn, int err, void *context) { LOG_WRN("Error while discovering GATT database: (%d)", err); }

struct bt_gatt_dm_cb discovery_cb = {
    .completed = discovery_complete,
    .service_not_found = discovery_service_not_found,
    .error_found = discovery_error,
};

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

  myPrintkS("gatt_discover() entered... count: %d\n", conn_count);
  if (conn != default_conn) {
    return;
  }
  myPrintkS("bt_gatt_dm_start() entered...\n");
  err = bt_gatt_dm_start(conn, BT_UUID_NUS_SERVICE, &discovery_cb, &nus_client);
  myPrintkS("bt_gatt_dm_start() returned...%d \n", err);
  if (err) {
    LOG_ERR("could not start the discovery procedure, error "
            "code: %d",
            err);
  }
}

static void exchange_func(struct bt_conn *conn, uint8_t err, struct bt_gatt_exchange_params *params) {
  if (!err) {
    LOG_INF("MTU exchange done");
  } else {
    LOG_WRN("MTU exchange failed (err %" PRIu8 ")", err);
  }
}

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

  // myPrintkW("connected() entered... %s\n", addr);
  bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));
  //  myPrintkW("bt_addr_le_to_str() returned... %s\n", addr);
  if (conn_err) {
    LOG_INF("Failed to connect to %s, 0x%02x %s", addr, conn_err, bt_hci_err_to_str(conn_err));

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

      (void)k_work_submit(&scan_work);
    }

    return;
  }

  myPrintkI("BT Connected: %s count: %d\r\n", addr, conn_count);
  //  LOG_INF("Connected: %s", addr);

  static struct bt_gatt_exchange_params exchange_params;

  exchange_params.func = exchange_func;
  err = bt_gatt_exchange_mtu(conn, &exchange_params);
  if (err) {
    LOG_WRN("MTU exchange failed (err %d)", err);
  }

  err = bt_conn_set_security(conn, BT_SECURITY_L2);
  if (err) {
    LOG_WRN("Failed to set security: %d", err);

    gatt_discover(conn);
  }

  //  if (conn_count == CONFIG_BT_MAX_CONN) {
  myPrintkI("Maximum connections reached, stopping scan\r\n");
  err = bt_scan_stop();
  if (err) {
    LOG_ERR("Stop LE scan failed (err %d)", err);
  }
  //  }
}

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("BT Disconnected: %s, reason 0x%02x %s", addr, reason, bt_hci_err_to_str(reason));

  if (default_conn != conn) {
    return;
  }

  bt_conn_unref(default_conn);
  default_conn = NULL;

  (void)k_work_submit(&scan_work);
}

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 %s", addr, level, err, bt_security_err_to_str(err));
  }

  gatt_discover(conn);
}

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

static void scan_filter_match(struct bt_scan_device_info *device_info, struct bt_scan_filter_match *filter_match, bool connectable) {
  char addr[BT_ADDR_LE_STR_LEN];

  bt_addr_le_to_str(device_info->recv_info->addr, addr, sizeof(addr));

  LOG_INF("Filters matched. Address: %s connectable: %d", addr, connectable);
}

static void scan_connecting_error(struct bt_scan_device_info *device_info) { LOG_WRN("Connecting failed"); }

static void scan_connecting(struct bt_scan_device_info *device_info, struct bt_conn *conn) { default_conn = bt_conn_ref(conn); }
// static void scan_connecting(struct bt_scan_device_info * device_info, struct bt_conn * conn) { Gconn[conn_count++] = bt_conn_ref(conn); }

static int nus_client_init(void) {
  int err;
  struct bt_nus_client_init_param init = {.cb = {
                                              .received = ble_data_received,
                                              .sent = ble_data_sent,
                                          }};

  err = bt_nus_client_init(&nus_client, &init);
  if (err) {
    LOG_ERR("NUS Client initialization failed (err %d)", err);
    return err;
  }

  LOG_INF("NUS Client module initialized");
  return err;
}

BT_SCAN_CB_INIT(scan_cb, scan_filter_match, NULL, scan_connecting_error, scan_connecting);

static void try_add_address_filter(const struct bt_bond_info *info, void *user_data) {
  int err;
  char addr[BT_ADDR_LE_STR_LEN];
  uint8_t *filter_mode = user_data;

  bt_addr_le_to_str(&info->addr, addr, sizeof(addr));

  struct bt_conn *conn = bt_conn_lookup_addr_le(BT_ID_DEFAULT, &info->addr);

  if (conn) {
    bt_conn_unref(conn);
    return;
  }

  err = bt_scan_filter_add(BT_SCAN_FILTER_TYPE_ADDR, &info->addr);
  if (err) {
    LOG_ERR("Address filter cannot be added (err %d): %s", err, addr);
    return;
  }

  LOG_INF("Address filter added: %s", addr);
  *filter_mode |= BT_SCAN_ADDR_FILTER;
}

static int scan_start(void) {
  int err;
  uint8_t filter_mode = 0;

  err = bt_scan_stop();
  if (err) {
    LOG_ERR("Failed to stop scanning (err %d)", err);
    return err;
  }

  bt_scan_filter_remove_all();

  err = bt_scan_filter_add(BT_SCAN_FILTER_TYPE_UUID, BT_UUID_NUS_SERVICE);
  if (err) {
    LOG_ERR("UUID filter cannot be added (err %d", err);
    return err;
  }
  filter_mode |= BT_SCAN_UUID_FILTER;

  bt_foreach_bond(BT_ID_DEFAULT, try_add_address_filter, &filter_mode);

  err = bt_scan_filter_enable(filter_mode, false);
  if (err) {
    LOG_ERR("Filters cannot be turned on (err %d)", err);
    return err;
  }

  err = bt_scan_start(BT_SCAN_TYPE_SCAN_ACTIVE);
  if (err) {
    LOG_ERR("Scanning failed to start (err %d)", err);
    return err;
  }

  myPrintkI("Scan started\r\n");
  LOG_INF("Scan started");
  return 0;
}

static void scan_work_handler(struct k_work *item) {
  ARG_UNUSED(item);

  (void)scan_start();
}

static void scan_init(void) {
  struct bt_scan_init_param scan_init = {
      .connect_if_match = true,
  };

  bt_scan_init(&scan_init);
  bt_scan_cb_register(&scan_cb);

  k_work_init(&scan_work, scan_work_handler);
  LOG_INF("Scan module initialized");
}

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_WRN("Pairing failed conn: %s, reason %d %s", addr, reason, bt_security_err_to_str(reason));
}

static struct bt_conn_auth_cb conn_auth_callbacks = {
    .cancel = auth_cancel,
};

static struct bt_conn_auth_info_cb conn_auth_info_callbacks = {.pairing_complete = pairing_complete, .pairing_failed = pairing_failed};

#if 1
struct k_timer wdt_timer; //!< timer to reset WDT

/*!
 * \fn void wdt_handler(struct k_work *work)
 *
 * \brief work function WDT timer handler
 *
 *
 * \param work
 *
 */
void wdt_handler(struct k_work *work) {
  if (stopWDTFeed == 0) /* check if WDT feed disabled*/
  {
    //		myPrintkS("Feeding watchdog... %d\n",stopWDTFeed);
    wdt_feed(wdt, wdt_channel_id);
  }
}

K_WORK_DEFINE(wdt_work, wdt_handler);

/*!
 * \fn void wdt_timer_handler(struct k_timer *dummy)
 *
 * \brief timer CLI command handler
 *
 * timer handler just submits timer work function to the work queue
 *
 * \param dummy pointer to a timer sturcture
 *
 */
void wdt_timer_handler(struct k_timer *dummy) { k_work_submit(&wdt_work); }
#endif

#ifdef USE_THREAD
#define MY_STACK_SIZE 1024
#define MY_PRIORITY 7
const k_tid_t my_uart_thread_id;

void my_uart_thread_logic(void *arg1, void *arg2, void *arg3) {
  int err = 0;

  struct uart_data_t nus_data = {
      .len = 0,
  };
  myPrintkI("starting BLE thread\r\n");
  /* Your logic from main()'s loop goes here */
  while (1) {
    // e.g., k_fifo_get(&fifo_uart_rx_data, K_FOREVER);
    // ... process data ...

    /* IMPORTANT: If your loop doesn't have a blocking call like
       k_fifo_get(..., K_FOREVER), you MUST add a sleep to prevent
       starving other threads of the same priority. */
    //	for (;;)
    {
      /* Wait indefinitely for data to be sent over Bluetooth */
      struct uart_data_t *buf = k_fifo_get(&fifo_uart_rx_data, K_FOREVER);
      printf("\r\n\nUART seen %s\r\n\n", buf->data);

      int plen = MIN(sizeof(nus_data.data) - nus_data.len, buf->len);
      int loc = 0;

      while (plen > 0) {
        memcpy(&nus_data.data[nus_data.len], &buf->data[loc], plen);
        nus_data.len += plen;
        loc += plen;
        if (nus_data.len >= sizeof(nus_data.data) || (nus_data.data[nus_data.len - 1] == '\n') ||
            (nus_data.data[nus_data.len - 1] == '\r')) {
          if (nus_data.data[0] == ':') {
            memset(cliCommand, 0, strlen(cliCommand));
            memcpy(cliCommand, &nus_data.data[1], nus_data.len - 1);
            cliCommand[strlen(cliCommand) - 1] = '\0';
            printf("\r\nCLI Command: %s\r\n", cliCommand);
            executeCmd(cliCommand);
          } else {
            myPrintkI("\r\nBLE Send: %d %s\r\n\n", nus_data.len, nus_data.data);
            err = bt_nus_client_send(&nus_client, nus_data.data, nus_data.len);
            if (err) {
              LOG_WRN("Failed to send data over BLE connection"
                      "(err %d)",
                      err);
            }
            //						err = k_sem_take(&nus_write_sem, NUS_WRITE_TIMEOUT);
            //						if (err)
            //						{
            //							LOG_WRN("NUS send timeout");
            //						}
          }

          nus_data.len = 0;
        }

        plen = MIN(sizeof(nus_data.data), buf->len - loc);
      }

      //			k_free(buf);
    }
    k_msleep(10);
  }
}
#endif

#ifdef USE_THREAD
K_THREAD_DEFINE(my_uart_thread_id, MY_STACK_SIZE, my_uart_thread_logic, NULL, NULL, NULL, MY_PRIORITY, 0, 0);
#endif

int central_uart_main(void) {
  int err;
  k_msgq_init(&my_msgq1, my_msgq1_buffer, sizeof(struct data_item_type), 1);
  k_msgq_init(&my_msgq2, my_msgq2_buffer, sizeof(struct data_item_type), 1);
  k_msgq_init(&my_msgq3, my_msgq3_buffer, sizeof(struct data_battery_type), 1);

#if 1
  struct wdt_timeout_cfg wdt_config = {
      /* Reset SoC when watchdog timer expires. */
      .flags = WDT_FLAG_RESET_SOC,

      /* Expire watchdog after max window */
      .window.min = WDT_MIN_WINDOW,
      .window.max = WDT_MAX_WINDOW,
  };
#endif

  err = bt_conn_auth_cb_register(&conn_auth_callbacks);
  if (err) {
    //		LOG_ERR("Failed to register authorization callbacks.");
    myPrintkE("Failed to register authorization callbacks.\r\n");
    return 0;
  }
  myPrintkI("Passed register authorization callbacks.\r\n");

  err = bt_conn_auth_info_cb_register(&conn_auth_info_callbacks);
  if (err) {
    myPrintkE("Failed to register authorization info callbacks.\r\n");
    return 0;
  }
  myPrintkI("Passed register authorization info callbacks.\r\n");

  err = bt_enable(NULL);
  if (err) {
    myPrintkE("Bluetooth init failed (err %d)\r\n", err);
    //		LOG_ERR("Bluetooth init failed (err %d)", err);
    return 0;
  }
  myPrintkI("Bluetooth initialized\r\n");
  //	LOG_INF("Bluetooth initialized");

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

  //	err = uart_init();
  //	if (err != 0)
  //	{
  //		printk("\r\nuart_init failed (err %d)\r\n", err);
  //		return 0;
  //	}
  //		printk("\r\nuart_init passed (err %d)\r\n", err);

  err = nus_client_init();
  if (err != 0) {
    myPrintkE("nus_client_init failed (err %d)\r\n", err);
    return 0;
  }
  myPrintkI("nus_client_init passed (err %d)\r\n", err);

  scan_init();
  err = scan_start();
  if (err) {
    myPrintkE("scan_start failed (err %d)\r\n", err);
    return 0;
  }
  myPrintkI("scan_start passed (err %d)\r\n", err);

  //	printk("Starting Bluetooth Central UART (central_uart_tjm) sample\n");
  myPrintkI("Starting Bluetooth Central UART (central_uart_tjm)\r\n");

  /* set up watchdog timer */
#if 1
  if (!device_is_ready(wdt)) {
    myPrintkE("%s: Watchdog device not ready.\n", wdt->name);
    return 0;
  }
  myPrintkI("%s: Watchdog device ready.\n", wdt->name);

  wdt_channel_id = wdt_install_timeout(wdt, &wdt_config);

  switch (wdt_channel_id) {
  case -EBUSY:
    myPrintkE("Watchdog timeout can not be installed while watchdog has already been setup.\n");
    return 0;
  case -ENOTSUP:
    myPrintkE("Watchdog Callback support rejected, continuing anyway\n");
    return 0;
  case -EINVAL:
    myPrintkE("Watchdog install error: invalid configuration\n");
    return 0;
  default:
    if (wdt_channel_id < 0) {
      myPrintkE("Watchdog install error: %d\n", wdt_channel_id);
      return 0;
    }
    myPrintkI("Watchdog install passed\n");
  }

#if 0
	if (wdt_channel_id < 0)
	{
		myPrintkE("Watchdog install error\n");
		return 0;
	}
	myPrintkI("Watchdog install passed\n");

	if (wdt_channel_id == -ENOTSUP)
	{
		/* IWDG driver for STM32 doesn't support callback */
		myPrintkE("Watchdog Callback support rejected, continuing anyway\n");
		wdt_config.callback = NULL;
		wdt_channel_id = wdt_install_timeout(wdt, &wdt_config);
	}
	myPrintkI("Watchdog Callback support passed\n");
#endif

  err = wdt_setup(wdt, WDT_OPT);
  if (err < 0) {
    myPrintkE("Watchdog setup error\n");
    return 0;
  }
  myPrintkI("Watchdog setup passed. Start WDT feed timer\n");

#if 1
  k_timer_init(&wdt_timer, wdt_timer_handler, NULL);
  k_timer_start(&wdt_timer, K_MSEC(WDG_FEED_INTERVAL), K_MSEC(WDG_FEED_INTERVAL));
#else
  /* Feeding watchdog. */
  myPrintkS("Feeding watchdog %d times %d ms\n", WDT_FEED_TRIES, WDG_FEED_INTERVAL);
  for (int i = 0; i < WDT_FEED_TRIES; ++i) {
    myPrintkS("Feeding watchdog...\n");
    wdt_feed(wdt, wdt_channel_id);
    k_sleep(K_MSEC(WDG_FEED_INTERVAL));
  }

  /* Waiting for the SoC reset. */
  myPrintkS("Waiting for reset...\n");
  while (1) {
    k_yield();
  }
  return 0;
#endif

#endif /* end WDT timer */

#ifdef LTE_MERGE
  /* merge from dl_agri_new_nrf9160 */

  if (app_event_manager_init()) {
    /* Without the Application Event Manager, the application will not work
     * as intended. A reboot is required in an attempt to recover.
     */
    //		LOG_ERR("Application Event Manager could not be initialized, rebooting...");
    //		strcpy(flashParameters.sysRebootReason, "Event manager init failed");
    //		setFlashxParameters(runFlag ? false : true);
    //		k_sleep(K_SECONDS(5));
    //		sys_reboot(SYS_REBOOT_COLD);
  } else {
    //		module_set_state(MODULE_STATE_READY);
    //		SEND_EVENT(app, APP_EVT_START);

#if defined(CONFIG_NRF_MODEM_LIB)
//		modem_init();
#endif
  }

#endif /* end dl_agri)new_nrf9160 merge */

#ifndef USE_THREAD
  struct uart_data_t nus_data = {
      .len = 0,
  };

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

    int plen = MIN(sizeof(nus_data.data) - nus_data.len, buf->len);
    int loc = 0;

    while (plen > 0) {
      memcpy(&nus_data.data[nus_data.len], &buf->data[loc], plen);
      nus_data.len += plen;
      loc += plen;
      if (nus_data.len >= sizeof(nus_data.data) || (nus_data.data[nus_data.len - 1] == '\n') || (nus_data.data[nus_data.len - 1] == '\r')) {
        if (nus_data.data[0] == ':') {
          memset(cliCommand, 0, strlen(cliCommand));
          memcpy(cliCommand, &nus_data.data[1], nus_data.len - 1);
          cliCommand[strlen(cliCommand) - 1] = '\0';
          //					printf("\r\nCLI Command: %s\r\n", cliCommand);
          executeCmd(cliCommand);
        } else {
          myPrintkI("\r\nBLE Send: %d %s\r\n\n", nus_data.len, nus_data.data);
          err = bt_nus_client_send(&nus_client, nus_data.data, nus_data.len);
        }
        if (err) {
          LOG_WRN("Failed to send data over BLE connection"
                  "(err %d)",
                  err);
        }

        err = k_sem_take(&nus_write_sem, NUS_WRITE_TIMEOUT);
        if (err) {
          LOG_WRN("NUS send timeout");
        }

        nus_data.len = 0;
      }

      plen = MIN(sizeof(nus_data.data), buf->len - loc);
    }

    k_free(buf);
  }
#endif
  return 0;
}

void parseBLEString(char *bleData) {
  myPrintkS("parseBLEString BLE data seen %s\r\n", bleData);
  uint8_t i;
  int len = strlen(bleData);
  struct data_item_type tempBLEData;
  struct data_item_type temp2BLEData;
  struct data_battery_type tempBatteryData;

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

  BLEParameter[numBLEParameters++] = &bleData[0];
  for (i = 0; i < len; i++) {
    //            printf("%d 0x%02X %c\r\n",i,bleData[i],bleData[i]);
    if ((bleData[i] == ' ') || (bleData[i] == '\t')) {
      bleData[i] = '\0';
      BLEParameter[numBLEParameters++] = &bleData[i + 1];
    }
  }
  bleData[i] = '\0';

  /* wait for HSC temperature/humidity data */
  if (strcmp(BLEParameter[2], "t1") == 0) {
    strcpy(tempBLEData.BLEId, BLEParameter[1]);
    tempBLEData.BLETemp = atof(BLEParameter[3]);
    tempBLEData.BLEHum = atof(BLEParameter[5]);
    memcpy(my_msgq1_buffer, &tempBLEData, sizeof(struct data_item_type));
    myPrintkS("BLEId: %s BLEHDC2080Temp: %3.2f BLEHDC2080Hum: %3.2f\r\n", tempBLEData.BLEId, tempBLEData.BLETemp, tempBLEData.BLEHum);
    /* send data to consumers */
    if (k_msgq_put(&my_msgq1, my_msgq1_buffer, K_NO_WAIT) != 0) {
      myPrintkW("Message queue 1 is full, purging old data\r\n");
      /* message queue is full: purge old data & try again */
      k_msgq_purge(&my_msgq1);
      k_msgq_put(&my_msgq1, my_msgq1_buffer, K_NO_WAIT);
    }
  }

  /* wait for BME688 temperature/humidity data */
  if (strcmp(BLEParameter[2], "t2") == 0) {
    strcpy(temp2BLEData.BLEId, BLEParameter[1]);
    temp2BLEData.BLETemp = atof(BLEParameter[3]);
    temp2BLEData.BLEHum = atof(BLEParameter[5]);
    memcpy(my_msgq2_buffer, &temp2BLEData, sizeof(struct data_item_type));
    myPrintkS("BLEId: %s BLEBME688Temp2: %3.2f BLEBME688Hum2: %3.2f\r\n", temp2BLEData.BLEId, temp2BLEData.BLETemp, temp2BLEData.BLEHum);
    /* send data to consumers */
    if (k_msgq_put(&my_msgq2, my_msgq2_buffer, K_NO_WAIT) != 0) {
      myPrintkW("Message queue 2 is full, purging old data\r\n");
      /* message queue is full: purge old data & try again */
      k_msgq_purge(&my_msgq2);
      k_msgq_put(&my_msgq2, my_msgq2_buffer, K_NO_WAIT);
    }
  }

  /* wait for Battery data */
  if (strcmp(BLEParameter[2], "bat") == 0) {
    strcpy(tempBatteryData.BLEId, BLEParameter[1]);
    tempBatteryData.BLEBattery = atoi(BLEParameter[3]);
    memcpy(my_msgq3_buffer, &tempBatteryData, sizeof(struct data_battery_type));
    myPrintkS("BLEId: %s Battery: %d \r\n", tempBatteryData.BLEId, tempBatteryData.BLEBattery);
    /* send data to consumers */
    if (k_msgq_put(&my_msgq3, my_msgq3_buffer, K_NO_WAIT) != 0) {
      myPrintkW("Message queue 3 is full, purging old data\r\n");
      /* message queue is full: purge old data & try again */
      k_msgq_purge(&my_msgq3);
      k_msgq_put(&my_msgq3, my_msgq3_buffer, K_NO_WAIT);
    }
  }
}

/*
 * 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)*/
#if 0
void myPrintkW(char *str) {
  printk("%s", "\33[1;33m");
  printk("%s", str);
  // printk("%s", myStr);
  printk("%s", "\33[0m");
  return;
}
#endif

int myPrintkW(char *restrict fmt, ...) {
  int n = 0;
  va_list ap;

  printk("%s", "\33[1;33m");
  va_start(ap, fmt);
  //  n = vprintf(fmt, ap);
  vprintk(fmt, ap);
  va_end(ap);
  printk("%s", "\33[0m");
  // printf("n=%d\n", n);
  return n;
}

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

int myPrintkI(char *restrict fmt, ...) {
  int n = 0;
  va_list ap;

  printk("%s", "\33[1;32m");
  va_start(ap, fmt);
  //  n = vprintf(fmt, ap);
  vprintk(fmt, ap);
  va_end(ap);
  printk("%s", "\33[0m");
  // printf("n=%d\n", n);
  return n;
}

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

int myPrintkS(char *restrict fmt, ...) {
  int n = 0;
  va_list ap;

  printk("%s", "\33[1;36m");
  va_start(ap, fmt);
  //  n = vprintf(fmt, ap);
  vprintk(fmt, ap);
  va_end(ap);
  printk("%s", "\33[0m");
  // printf("n=%d\n", n);
  return n;
}

/* Red (Error)*/
#if 0
void myPrintkE(char *str) {
  printk("%s", "\33[1;31m");
  printk("%s", str);
  // printk("%s", myStr);
  printk("%s", "\33[0m");
  return;
}
#endif

int myPrintkE(char *restrict fmt, ...) {
  int n = 0;
  va_list ap;

  printk("%s", "\33[1;31m");
  va_start(ap, fmt);
  //  n = vprintf(fmt, ap);
  vprintk(fmt, ap);
  va_end(ap);
  printk("%s", "\33[0m");
  // printf("n=%d\n", n);
  return n;
}