/*
 * Based on Zephyr GSM PPP modem driver from Zephyr 2.2.0.
 * To be used with nrf9160 Serial LTE Modem target.
 *
 * SPDX-License-Identifier: Apache-2.0
 */
#include <zephyr/drivers/modem/nrf9160_slm.h>

#define DT_DRV_COMPAT test_nrf9160_slm

#include <zephyr/logging/log.h>
LOG_MODULE_REGISTER(modem_nrf9160_slm, CONFIG_MODEM_LOG_LEVEL);

#include <stdlib.h>
#include <zephyr/kernel.h>
#include <zephyr/device.h>
#include <zephyr/sys/ring_buffer.h>
#include <zephyr/sys/util.h>
#include <zephyr/net/ppp.h>
#include <zephyr/drivers/uart.h>

#include "modem_context.h"
#include "modem_iface_uart.h"
#include "modem_cmd_handler.h"

#include <stdio.h>
#include <stdint.h>

#define NRF9160_SLM_UART_NODE                   DT_INST_BUS(0)
#define NRF9160_SLM_CMD_READ_BUF                128
#define NRF9160_SLM_CMD_AT_TIMEOUT              K_SECONDS(2)
#define NRF9160_SLM_CMD_SETUP_TIMEOUT           K_SECONDS(5)
/* NRF9160_SLM_CMD_LOCK_TIMEOUT should be longer than NRF9160_SLM_CMD_AT_TIMEOUT & NRF9160_SLM_CMD_SETUP_TIMEOUT,
 * otherwise the nrf9160_slm_stop might fail to lock tx.
 */
#define NRF9160_SLM_CMD_LOCK_TIMEOUT            K_SECONDS(12)
#define NRF9160_SLM_RECV_MAX_BUF                24
#define NRF9160_SLM_RECV_BUF_SIZE               512
#define NRF9160_SLM_ATTACH_RETRY_DELAY_MSEC     2000
#define NRF9160_SLM_REGISTER_DELAY_MSEC         2000
#define NRF9160_SLM_RETRY_DELAY                 K_SECONDS(2)

#define NRF9160_SLM_RSSI_RETRY_DELAY_MSEC       2000
#define NRF9160_SLM_RSSI_RETRIES                10
#define NRF9160_SLM_RSSI_INVALID                -1000

#if defined(CONFIG_MODEM_NRF9160_SLM_ENABLE_CESQ_RSSI)
	#define NRF9160_SLM_RSSI_MAXVAL          0
#else
	#define NRF9160_SLM_RSSI_MAXVAL         -51
#endif

/* Modem network registration state */
enum network_state {
	NRF9160_SLM_NET_INIT = -1,
	NRF9160_SLM_NET_NOT_REGISTERED,
	NRF9160_SLM_NET_HOME_NETWORK,
	NRF9160_SLM_NET_SEARCHING,
	NRF9160_SLM_NET_REGISTRATION_DENIED,
	NRF9160_SLM_NET_UNKNOWN,
	NRF9160_SLM_NET_ROAMING,
};

#define NRF9160_SLM_HTTP_RESULT_MAX 256

static struct nrf9160_slm_modem {
	struct k_mutex lock;
	const struct device *dev;
	struct modem_context context;

	struct modem_cmd_handler_data cmd_handler_data;
	uint8_t cmd_match_buf[NRF9160_SLM_CMD_READ_BUF];
	struct k_sem sem_response;
	struct k_sem sem_if_down;

	struct modem_iface_uart_data nrf9160_slm_data;
	struct k_work_delayable nrf9160_slm_configure_work;
	char nrf9160_slm_rx_rb_buf[NRF9160_SLM_RECV_MAX_BUF];

	enum nrf9160_slm_state {
		NRF9160_SLM_START,
		NRF9160_SLM_WAIT_AT,
		NRF9160_SLM_AT_RDY,
		NRF9160_SLM_STATE_INIT,
		NRF9160_SLM_STATE_CONTROL_CHANNEL = NRF9160_SLM_STATE_INIT,
		NRF9160_SLM_STATE_HTTP,
		NRF9160_SLM_STATE_AT_CHANNEL,
		NRF9160_SLM_STATE_DONE,
		NRF9160_SLM_SETUP = NRF9160_SLM_STATE_DONE,
		NRF9160_SLM_REGISTERING,
		NRF9160_SLM_ATTACHING,
		NRF9160_SLM_ATTACHED,
		NRF9160_SLM_SETUP_DONE,
		NRF9160_SLM_STOP,
		NRF9160_SLM_STATE_ERROR,
	} state;

	struct net_if *iface;

	struct k_thread rx_thread;
	struct k_work_q workq;
	struct k_work_delayable rssi_work_handle;
	struct nrf9160_slm_modem_info minfo;

	enum network_state net_state;

	int retries;
	bool modem_info_queried : 1;

	void *user_data;
	void *ready_user_data;

	nrf9160_slm_power_cb modem_on_cb;
	nrf9160_slm_power_cb modem_off_cb;
	nrf9160_slm_ready_cb modem_ready_cb;
	struct net_mgmt_event_callback nrf9160_slm_mgmt_cb;
} slm;

NET_BUF_POOL_DEFINE(nrf9160_slm_recv_pool, NRF9160_SLM_RECV_MAX_BUF, NRF9160_SLM_RECV_BUF_SIZE, 0, NULL);
K_KERNEL_STACK_DEFINE(nrf9160_slm_rx_stack, CONFIG_MODEM_NRF9160_SLM_RX_STACK_SIZE);
K_KERNEL_STACK_DEFINE(nrf9160_slm_workq_stack, CONFIG_MODEM_NRF9160_SLM_WORKQ_STACK_SIZE);

static inline void nrf9160_slm_lock(struct nrf9160_slm_modem *slm)
{
	(void)k_mutex_lock(&slm->lock, K_FOREVER);
}

static inline void nrf9160_slm_unlock(struct nrf9160_slm_modem *slm)
{
	(void)k_mutex_unlock(&slm->lock);
}

static inline int nrf9160_slm_work_reschedule(struct k_work_delayable *dwork, k_timeout_t delay)
{
	return k_work_reschedule_for_queue(&slm.workq, dwork, delay);
}

#if defined(CONFIG_MODEM_NRF9160_SLM_ENABLE_CESQ_RSSI)
	/* helper macro to keep readability */
#define ATOI(s_, value_, desc_) modem_atoi(s_, value_, desc_, __func__)

/**
 * @brief  Convert string to long integer, but handle errors
 *
 * @param  s: string with representation of integer number
 * @param  err_value: on error return this value instead
 * @param  desc: name the string being converted
 * @param  func: function where this is called (typically __func__)
 *
 * @retval return integer conversion on success, or err_value on error
 */
static int modem_atoi(const char *s, const int err_value,
				const char *desc, const char *func)
{
	int ret;
	char *endptr;

	ret = (int)strtol(s, &endptr, 10);
	if ((endptr == NULL) || (*endptr != '\0')) {
		LOG_ERR("bad %s '%s' in %s", s,
			 desc, func);
		return err_value;
	}

	return ret;
}
#endif

static void nrf9160_slm_rx(struct nrf9160_slm_modem *slm)
{
	LOG_DBG("starting");

	while (true) {
		// Wait for data in UART receive buffer.
		(void)k_sem_take(&slm->nrf9160_slm_data.rx_sem, K_FOREVER);

		// The handler will listen on AT channel to get commands.
		slm->context.cmd_handler.process(&slm->context.cmd_handler,
						&slm->context.iface);
	}
}

MODEM_CMD_DEFINE(nrf9160_slm_cmd_ok)
{
	(void)modem_cmd_handler_set_error(data, 0);
	LOG_DBG("ok");
	k_sem_give(&slm.sem_response);
	return 0;
}

MODEM_CMD_DEFINE(nrf9160_slm_cmd_error)
{
	(void)modem_cmd_handler_set_error(data, -EINVAL);
	LOG_DBG("error");
	k_sem_give(&slm.sem_response);
	return 0;
}

/* Handler: +CME Error: <err>[0] */
MODEM_CMD_DEFINE(nrf9160_slm_cmd_exterror)
{
	/* TODO: map extended error codes to values */
	(void)modem_cmd_handler_set_error(data, -EIO);
	k_sem_give(&slm.sem_response);
	return 0;
}

MODEM_CMD_DEFINE(nrf9160_slm_reset_recv)
{
	struct k_work_sync work_sync;

	(void)modem_cmd_handler_set_error(data, -ECOMM);
	k_sem_give(&slm.sem_response);

	(void)k_work_cancel_delayable_sync(&slm.nrf9160_slm_configure_work, &work_sync);

	nrf9160_slm_lock(&slm);
	const struct device* dev = slm.dev;
	nrf9160_slm_power_cb modem_off_cb = slm.modem_off_cb;
	slm.state = NRF9160_SLM_STOP;
	slm.net_state = NRF9160_SLM_NET_INIT;
	nrf9160_slm_unlock(&slm);

	if (modem_off_cb) {
		modem_off_cb(dev/*, user_data*/);
	}
	// TODO: incomplete!

	return 0;	
}

// These commands are always handled.
static const struct modem_cmd response_cmds[] = {
	MODEM_CMD("OK", nrf9160_slm_cmd_ok, 0U, ""),
	MODEM_CMD("ERROR", nrf9160_slm_cmd_error, 0U, ""),
	MODEM_CMD("+CME ERROR: ", nrf9160_slm_cmd_exterror, 1U, ""),
	MODEM_CMD("CONNECT", nrf9160_slm_cmd_ok, 0U, ""),
	// This is not really a command but something the modem sends us after a reset.
	// After this we need to reinitialize the modem.
	MODEM_CMD("\bReady", nrf9160_slm_reset_recv, 0U, ""),
};

static int unquoted_atoi(const char *s, int base)
{
	if (*s == '"') {
		s++;
	}

	return strtol(s, NULL, base);
}

/*
 * Handler: +COPS: <mode>[0],<format>[1],<oper>[2]
 */
MODEM_CMD_DEFINE(on_cmd_atcmdinfo_cops)
{
	if (argc >= 1) {
#if defined(CONFIG_MODEM_CELL_INFO)
		if (argc >= 3) {
			slm.context.data_operator = unquoted_atoi(argv[2], 10);
			LOG_INF("operator: %u",
				slm.context.data_operator);
		}
#endif
		if (unquoted_atoi(argv[0], 10) == 0) {
			slm.context.is_automatic_oper = true;
		} else {
			slm.context.is_automatic_oper = false;
		}
		(void)k_sem_give(&slm.sem_response);
	}

	return 0;
}

/*
 * Provide modem info if modem shell is enabled. This can be shown with
 * "modem list" shell command.
 */

/* Handler: <manufacturer> */
MODEM_CMD_DEFINE(on_cmd_atcmdinfo_manufacturer)
{
	size_t out_len;

	out_len = net_buf_linearize(slm.minfo.mdm_manufacturer,
				    sizeof(slm.minfo.mdm_manufacturer) - 1,
				    data->rx_buf, 0, len);
	slm.minfo.mdm_manufacturer[out_len] = '\0';
	LOG_INF("Manufacturer: %s", slm.minfo.mdm_manufacturer);

	return 0;
}

/* Handler: <model> */
MODEM_CMD_DEFINE(on_cmd_atcmdinfo_model)
{
	size_t out_len;

	out_len = net_buf_linearize(slm.minfo.mdm_model,
				    sizeof(slm.minfo.mdm_model) - 1,
				    data->rx_buf, 0, len);
	slm.minfo.mdm_model[out_len] = '\0';
	LOG_INF("Model: %s", slm.minfo.mdm_model);

	return 0;
}

/* Handler: <rev> */
MODEM_CMD_DEFINE(on_cmd_atcmdinfo_revision)
{
	size_t out_len;

	out_len = net_buf_linearize(slm.minfo.mdm_revision,
				    sizeof(slm.minfo.mdm_revision) - 1,
				    data->rx_buf, 0, len);
	slm.minfo.mdm_revision[out_len] = '\0';
	LOG_INF("Revision: %s", slm.minfo.mdm_revision);

	return 0;
}

/* Handler: <IMEI> */
MODEM_CMD_DEFINE(on_cmd_atcmdinfo_imei)
{
	size_t out_len;

	out_len = net_buf_linearize(slm.minfo.mdm_imei, sizeof(slm.minfo.mdm_imei) - 1,
				    data->rx_buf, 0, len);
	slm.minfo.mdm_imei[out_len] = '\0';
	LOG_INF("IMEI: %s", slm.minfo.mdm_imei);

	return 0;
}

#if defined(CONFIG_MODEM_SIM_NUMBERS)
/* Handler: <IMSI> */
MODEM_CMD_DEFINE(on_cmd_atcmdinfo_imsi)
{
	size_t out_len;

	out_len = net_buf_linearize(slm.minfo.mdm_imsi, sizeof(slm.minfo.mdm_imsi) - 1,
				    data->rx_buf, 0, len);
	slm.minfo.mdm_imsi[out_len] = '\0';
	LOG_INF("IMSI: %s", slm.minfo.mdm_imsi);

	return 0;
}

/* Handler: <ICCID> */
MODEM_CMD_DEFINE(on_cmd_atcmdinfo_iccid)
{
	size_t out_len;

	out_len = net_buf_linearize(slm.minfo.mdm_iccid, sizeof(slm.minfo.mdm_iccid) - 1,
				    data->rx_buf, 0, len);
	slm.minfo.mdm_iccid[out_len] = '\0';
	if (slm.minfo.mdm_iccid[0] == '+') {
		/* Seen on U-blox SARA: "+CCID: nnnnnnnnnnnnnnnnnnnn".
		 * Skip over the +CCID bit, which other modems omit.
		 */
		char *p = strchr(slm.minfo.mdm_iccid, ' ');

		if (p) {
			size_t len = strlen(p+1);

			(void)memmove(slm.minfo.mdm_iccid, p+1, len+1);
		}
	}
	LOG_INF("ICCID: %s", slm.minfo.mdm_iccid);

	return 0;
}
#endif /* CONFIG_MODEM_SIM_NUMBERS */

MODEM_CMD_DEFINE(on_cmd_net_reg_sts)
{
	slm.net_state = (enum network_state)atoi(argv[1]);

	switch (slm.net_state) {
	case NRF9160_SLM_NET_NOT_REGISTERED:
		LOG_DBG("Network %s.", "not registered");
		break;
	case NRF9160_SLM_NET_HOME_NETWORK:
		LOG_DBG("Network %s.", "registered, home network");
		break;
	case NRF9160_SLM_NET_SEARCHING:
		LOG_DBG("Searching for network...");
		break;
	case NRF9160_SLM_NET_REGISTRATION_DENIED:
		LOG_DBG("Network %s.", "registration denied");
		break;
	case NRF9160_SLM_NET_UNKNOWN:
		LOG_DBG("Network %s.", "unknown");
		break;
	case NRF9160_SLM_NET_ROAMING:
		LOG_DBG("Network %s.", "registered, roaming");
		break;
	default:
		break;
	}

	return 0;
}

#if defined(CONFIG_MODEM_CELL_INFO)

/*
 * Handler: +CEREG: <n>[0],<stat>[1],<tac>[2],<ci>[3],<AcT>[4]
 */
MODEM_CMD_DEFINE(on_cmd_atcmdinfo_cereg)
{
	if (argc >= 4) {
		slm.context.data_lac = unquoted_atoi(argv[2], 16);
		slm.context.data_cellid = unquoted_atoi(argv[3], 16);
		LOG_INF("lac: %u, cellid: %u",
			slm.context.data_lac,
			slm.context.data_cellid);
	}

	return 0;
}

static const struct setup_cmd query_cellinfo_cmds[] = {
	SETUP_CMD_NOHANDLE("AT+CEREG=2"),
	SETUP_CMD("AT+CEREG?", "", on_cmd_atcmdinfo_cereg, 5U, ","),
	SETUP_CMD_NOHANDLE("AT+COPS=3,2"),
	SETUP_CMD("AT+COPS?", "", on_cmd_atcmdinfo_cops, 3U, ","),
};

static int nrf9160_slm_query_cellinfo(struct nrf9160_slm_modem *slm)
{
	int ret;

	ret = modem_cmd_handler_setup_cmds_nolock(&slm->context.iface,
						  &slm->context.cmd_handler,
						  query_cellinfo_cmds,
						  ARRAY_SIZE(query_cellinfo_cmds),
						  &slm->sem_response,
						  NRF9160_SLM_CMD_SETUP_TIMEOUT);
	if (ret < 0) {
		LOG_WRN("modem query for cell info returned %d", ret);
	}

	return ret;
}
#endif /* CONFIG_MODEM_CELL_INFO */

#if defined(CONFIG_MODEM_NRF9160_SLM_ENABLE_CESQ_RSSI)
/*
 * Handler: +CESQ: <rxlev>[0],<ber>[1],<rscp>[2],<ecn0>[3],<rsrq>[4],<rsrp>[5]
 */
MODEM_CMD_DEFINE(on_cmd_atcmdinfo_rssi_cesq)
{
	int rsrp, rscp, rxlev;

	rsrp = ATOI(argv[5], 0, "rsrp");
	rscp = ATOI(argv[2], 0, "rscp");
	rxlev = ATOI(argv[0], 0, "rxlev");

	if ((rsrp >= 0) && (rsrp <= 97)) {
		slm.minfo.mdm_rssi = -140 + (rsrp - 1);
		LOG_DBG("RSRP: %d", slm.minfo.mdm_rssi);
	} else if ((rscp >= 0) && (rscp <= 96)) {
		slm.minfo.mdm_rssi = -120 + (rscp - 1);
		LOG_DBG("RSCP: %d", slm.minfo.mdm_rssi);
	} else if ((rxlev >= 0) && (rxlev <= 63)) {
		slm.minfo.mdm_rssi = -110 + (rxlev - 1);
		LOG_DBG("RSSI: %d", slm.minfo.mdm_rssi);
	} else {
		slm.minfo.mdm_rssi = NRF9160_SLM_RSSI_INVALID;
		LOG_DBG("RSRP/RSCP/RSSI not known");
	}

	return 0;
}
#else
/* Handler: +CSQ: <signal_power>[0],<qual>[1] */
MODEM_CMD_DEFINE(on_cmd_atcmdinfo_rssi_csq)
{
	/* Expected response is "+CSQ: <signal_power>,<qual>" */
	if (argc > 0) {
		int rssi = atoi(argv[0]);

		if ((rssi >= 0) && (rssi <= 31)) {
			rssi = -113 + (rssi * 2);
		} else {
			rssi = NRF9160_SLM_RSSI_INVALID;
		}

		slm.minfo.mdm_rssi = rssi;
		LOG_DBG("RSSI: %d", rssi);
	}

	return 0;
}
#endif

#if defined(CONFIG_MODEM_NRF9160_SLM_ENABLE_CESQ_RSSI)
static const struct modem_cmd read_rssi_cmd =
	MODEM_CMD("+CESQ:", on_cmd_atcmdinfo_rssi_cesq, 6U, ",");
#else
static const struct modem_cmd read_rssi_cmd =
	MODEM_CMD("+CSQ:", on_cmd_atcmdinfo_rssi_csq, 2U, ",");
#endif

static const struct setup_cmd setup_modem_info_cmds[] = {
	/* query modem info */
	SETUP_CMD("AT+CGMI", "", on_cmd_atcmdinfo_manufacturer, 0U, ""),
	SETUP_CMD("AT+CGMM", "", on_cmd_atcmdinfo_model, 0U, ""),
	SETUP_CMD("AT+CGMR", "", on_cmd_atcmdinfo_revision, 0U, ""),
	SETUP_CMD("AT+CGSN", "", on_cmd_atcmdinfo_imei, 0U, ""),
#if defined(CONFIG_MODEM_SIM_NUMBERS)
	SETUP_CMD("AT+CIMI", "", on_cmd_atcmdinfo_imsi, 0U, ""),
	// No phone number on NB-IoT
	//SETUP_CMD("AT+CCID", "", on_cmd_atcmdinfo_iccid, 0U, ""),
#endif
};

static const struct setup_cmd setup_cmds[] = {
	/* extended errors in numeric form */
	SETUP_CMD_NOHANDLE("AT+CMEE=1"),
	/* create PDP context */
	//SETUP_CMD_NOHANDLE("AT+CGDCONT=1,\"IP\",\"" CONFIG_MODEM_NRF9160_SLM_APN "\""),
};

MODEM_CMD_DEFINE(on_cmd_atcmdinfo_attached)
{
	/* Expected response is "+CGATT: 0|1" so simply look for '1' */
	if ((argc > 0) && (atoi(argv[0]) == 1)) {
		LOG_INF("Attached to packet service!");
	}

	return 0;
}


static const struct modem_cmd read_cops_cmd =
	MODEM_CMD_ARGS_MAX("+COPS:", on_cmd_atcmdinfo_cops, 1U, 4U, ",");

static const struct modem_cmd check_net_reg_cmd =
	MODEM_CMD("+CEREG: ", on_cmd_net_reg_sts, 2U, ",");

static const struct modem_cmd check_attached_cmd =
	MODEM_CMD("+CGATT:", on_cmd_atcmdinfo_attached, 1U, ",");

static const struct setup_cmd connect_cmds[] = {
	/* connect to network, but do not enable GNSS */
	SETUP_CMD_NOHANDLE("AT+CFUN=21"),
};

static int nrf9160_slm_query_modem_info(struct nrf9160_slm_modem *slm)
{
	int ret;

	if (slm->modem_info_queried) {
		return 0;
	}

	ret = modem_cmd_handler_setup_cmds_nolock(&slm->context.iface,
						  &slm->context.cmd_handler,
						  setup_modem_info_cmds,
						  ARRAY_SIZE(setup_modem_info_cmds),
						  &slm->sem_response,
						  NRF9160_SLM_CMD_SETUP_TIMEOUT);

	if (ret < 0) {
		return ret;
	}

	slm->modem_info_queried = true;

	return 0;
}

static int nrf9160_slm_setup_mccmno(struct nrf9160_slm_modem *slm)
{
	int ret = 0;

	if (CONFIG_MODEM_NRF9160_SLM_MANUAL_MCCMNO[0] != '\0') {
		/* use manual MCC/MNO entry */
		ret = modem_cmd_send_nolock(&slm->context.iface,
					    &slm->context.cmd_handler,
					    NULL, 0,
					    "AT+COPS=1,2,\""
					    CONFIG_MODEM_NRF9160_SLM_MANUAL_MCCMNO
					    "\"",
					    &slm->sem_response,
					    NRF9160_SLM_CMD_AT_TIMEOUT);
	} else {

/* First AT+COPS? is sent to check if automatic selection for operator
 * is already enabled, if yes we do not send the command AT+COPS= 0,0.
 */

		ret = modem_cmd_send_nolock(&slm->context.iface,
					    &slm->context.cmd_handler,
					    &read_cops_cmd,
					    1, "AT+COPS?",
					    &slm->sem_response,
					    NRF9160_SLM_CMD_SETUP_TIMEOUT);

		if (ret < 0) {
			return ret;
		}

		if (!slm->context.is_automatic_oper) {
			/* register operator automatically */
			ret = modem_cmd_send_nolock(&slm->context.iface,
						    &slm->context.cmd_handler,
						    NULL, 0, "AT+COPS=0,0",
						    &slm->sem_response,
						    NRF9160_SLM_CMD_AT_TIMEOUT);
		}
	}

	if (ret < 0) {
		LOG_ERR("AT+COPS ret:%d", ret);
	}

	return ret;
}

static void query_rssi(struct nrf9160_slm_modem *slm, bool lock)
{
	int ret;

#if defined(CONFIG_MODEM_NRF9160_SLM_ENABLE_CESQ_RSSI)
	ret = modem_cmd_send_ext(&slm->context.iface, &slm->context.cmd_handler, &read_rssi_cmd, 1,
				 "AT+CESQ", &slm->sem_response, NRF9160_SLM_CMD_SETUP_TIMEOUT,
				 lock ? 0 : MODEM_NO_TX_LOCK);
#else
	ret = modem_cmd_send_ext(&slm->context.iface, &slm->context.cmd_handler, &read_rssi_cmd, 1,
				 "AT+CSQ", &slm->sem_response, NRF9160_SLM_CMD_SETUP_TIMEOUT,
				 lock ? 0 : MODEM_NO_TX_LOCK);
#endif

	if (ret < 0) {
		LOG_DBG("No answer to RSSI readout, %s", "ignoring...");
	}
}

static inline void query_rssi_lock(struct nrf9160_slm_modem *slm)
{
	query_rssi(slm, true);
}

static inline void query_rssi_nolock(struct nrf9160_slm_modem *slm)
{
	query_rssi(slm, false);
}

static void rssi_handler(struct k_work *work)
{
	struct k_work_delayable *dwork = k_work_delayable_from_work(work);
	struct nrf9160_slm_modem *slm = CONTAINER_OF(dwork, struct nrf9160_slm_modem, rssi_work_handle);

	nrf9160_slm_lock(slm);
	query_rssi_lock(slm);

#if defined(CONFIG_MODEM_CELL_INFO)
	(void)nrf9160_slm_query_cellinfo(slm);
#endif
	(void)nrf9160_slm_work_reschedule(&slm->rssi_work_handle,
				  K_SECONDS(CONFIG_MODEM_NRF9160_SLM_RSSI_POLLING_PERIOD));
	nrf9160_slm_unlock(slm);
}

static void nrf9160_slm_finalize_connection(struct k_work *work)
{
	int ret = 0;
	struct k_work_delayable *dwork = k_work_delayable_from_work(work);
	struct nrf9160_slm_modem *slm = CONTAINER_OF(dwork, struct nrf9160_slm_modem, nrf9160_slm_configure_work);

	nrf9160_slm_lock(slm);
	nrf9160_slm_ready_cb modem_ready_cb = NULL;
	const struct device* dev = slm->dev;

	/* If already attached, jump right to RSSI readout */
	if (slm->state == NRF9160_SLM_ATTACHED) {
		goto attached;
	}

	/* If attach check failed, we should not redo every setup step */
	if (slm->state == NRF9160_SLM_ATTACHING) {
		goto attaching;
	}

	/* If modem is searching for network, we should skip the setup step */
	if (slm->state == NRF9160_SLM_REGISTERING) {
		goto registering;
	}

	slm->state = NRF9160_SLM_SETUP;

	/*if (IS_ENABLED(CONFIG_MODEM_NRF9160_SLM_FACTORY_RESET_AT_BOOT)) {
		(void)modem_cmd_send_nolock(&slm->context.iface,
					    &slm->context.cmd_handler,
					    &response_cmds[0],
					    ARRAY_SIZE(response_cmds),
					    "AT", &slm->sem_response,
					    NRF9160_SLM_CMD_AT_TIMEOUT);
		(void)k_sleep(K_SECONDS(5));
	}*/

	ret = modem_cmd_handler_setup_cmds_nolock(&slm->context.iface,
						  &slm->context.cmd_handler,
						  setup_cmds,
						  ARRAY_SIZE(setup_cmds),
						  &slm->sem_response,
						  NRF9160_SLM_CMD_SETUP_TIMEOUT);
	if (ret < 0) {
		LOG_DBG("%s returned %d, %s", "setup_cmds", ret, "retrying...");
		(void)nrf9160_slm_work_reschedule(&slm->nrf9160_slm_configure_work, NRF9160_SLM_RETRY_DELAY);
		goto unlock;
	}

	// TODO: setup SLM data mode timeout

	ret = nrf9160_slm_setup_mccmno(slm);
	if (ret < 0) {
		LOG_ERR("%s returned %d, %s", "gsm_setup_mccmno", ret, "retrying...");

		(void)nrf9160_slm_work_reschedule(&slm->nrf9160_slm_configure_work, NRF9160_SLM_RETRY_DELAY);
		goto unlock;
	}

	ret = nrf9160_slm_query_modem_info(slm);
	if (ret < 0) {
		LOG_DBG("Unable to query modem information %d", ret);
		(void)nrf9160_slm_work_reschedule(&slm->nrf9160_slm_configure_work, NRF9160_SLM_RETRY_DELAY);
		goto unlock;
	}

	ret = modem_cmd_handler_setup_cmds_nolock(&slm->context.iface,
						  &slm->context.cmd_handler,
						  connect_cmds,
						  ARRAY_SIZE(connect_cmds),
						  &slm->sem_response,
						  NRF9160_SLM_CMD_SETUP_TIMEOUT);
	if (ret < 0) {
		LOG_DBG("%s returned %d, %s", "connect_cmds", ret, "retrying...");
		(void)nrf9160_slm_work_reschedule(&slm->nrf9160_slm_configure_work, NRF9160_SLM_RETRY_DELAY);
		goto unlock;
	}

	slm->state = NRF9160_SLM_REGISTERING;
registering:

	/* Wait for cell tower registration */
	ret = modem_cmd_send_nolock(&slm->context.iface,
				    &slm->context.cmd_handler,
				    &check_net_reg_cmd, 1,
				    "AT+CEREG?",
				    &slm->sem_response,
				    NRF9160_SLM_CMD_SETUP_TIMEOUT);
	if ((ret < 0) || ((slm->net_state != NRF9160_SLM_NET_ROAMING) &&
			 (slm->net_state != NRF9160_SLM_NET_HOME_NETWORK))) {
		if (slm->retries == 0) {
			slm->retries = CONFIG_MODEM_NRF9160_SLM_REGISTER_TIMEOUT *
				(MSEC_PER_SEC / NRF9160_SLM_REGISTER_DELAY_MSEC);
		} else {
			slm->retries--;
		}

		(void)nrf9160_slm_work_reschedule(&slm->nrf9160_slm_configure_work,
					  K_MSEC(NRF9160_SLM_REGISTER_DELAY_MSEC));
		goto unlock;
	}

	slm->retries = 0;
	slm->state = NRF9160_SLM_ATTACHING;
attaching:
	LOG_DBG("modem RSSI: %d, %s", slm->minfo.mdm_rssi, "enable connection");

	/* Don't initialize connection until we're attached to packet service */
	ret = modem_cmd_send_nolock(&slm->context.iface,
				    &slm->context.cmd_handler,
				    &check_attached_cmd, 1,
				    "AT+CGATT?",
				    &slm->sem_response,
				    NRF9160_SLM_CMD_SETUP_TIMEOUT);
	if (ret < 0) {
		/*
		 * retries not set        -> trigger N attach retries
		 * retries set            -> decrement and retry
		 * retries set, becomes 0 -> trigger full retry
		 */
		if (slm->retries == 0) {
			slm->retries = CONFIG_MODEM_NRF9160_SLM_ATTACH_TIMEOUT *
				(MSEC_PER_SEC / NRF9160_SLM_ATTACH_RETRY_DELAY_MSEC);
		} else {
			slm->retries--;
		}

		LOG_DBG("Not attached, %s", "retrying...");

		(void)nrf9160_slm_work_reschedule(&slm->nrf9160_slm_configure_work,
					K_MSEC(NRF9160_SLM_ATTACH_RETRY_DELAY_MSEC));
		goto unlock;
	}

	/* Attached, clear retry counter */
	LOG_DBG("modem attach returned %d, %s", ret, "read RSSI");
	slm->state = NRF9160_SLM_ATTACHED;
	slm->retries = NRF9160_SLM_RSSI_RETRIES;

 attached:
	query_rssi_nolock(slm);

	slm->state = NRF9160_SLM_SETUP_DONE;
	modem_ready_cb = slm->modem_ready_cb;

	// We never get to PPP.
	// Instead we need a callback here to notify networking is up a bit more directly.

unlock:
	nrf9160_slm_unlock(slm);
	if (modem_ready_cb) {
		modem_ready_cb(dev, 1/*, slm->ready_user_data*/);
	}
}

static void nrf9160_slm_configure(struct k_work *work)
{
	struct k_work_delayable *dwork = k_work_delayable_from_work(work);
	struct nrf9160_slm_modem *slm = CONTAINER_OF(dwork, struct nrf9160_slm_modem,
					     nrf9160_slm_configure_work);
	int ret = -1;

	nrf9160_slm_lock(slm);

	if (slm->state == NRF9160_SLM_WAIT_AT) {
		goto wait_at;
	}

	if (slm->state == NRF9160_SLM_START) {
		LOG_DBG("Starting modem %p configuration", slm);

		// FIXME
		if (slm->modem_on_cb != NULL) {
			slm->modem_on_cb(slm->dev/*, slm->user_data*/);
		}

		slm->state = NRF9160_SLM_WAIT_AT;
	}

wait_at:
	ret = modem_cmd_send_nolock(&slm->context.iface,
				    &slm->context.cmd_handler,
				    response_cmds,
				    ARRAY_SIZE(response_cmds),
				    "AT", &slm->sem_response,
				    NRF9160_SLM_CMD_AT_TIMEOUT);
	if (ret < 0) {
		LOG_DBG("modem not ready %d", ret);
		goto retry;
	}

	slm->state = NRF9160_SLM_AT_RDY;

	k_work_init_delayable(&slm->nrf9160_slm_configure_work, nrf9160_slm_finalize_connection);

retry:
	(void)nrf9160_slm_work_reschedule(&slm->nrf9160_slm_configure_work, K_NO_WAIT);
	nrf9160_slm_unlock(slm);
}

void nrf9160_slm_start(const struct device *dev)
{
	int ret;
	struct nrf9160_slm_modem *slm = dev->data;

	nrf9160_slm_lock(slm);

	if (slm->state != NRF9160_SLM_STOP) {
		LOG_ERR("nrf9160_slm is already %s", "started");
		goto unlock;
	}

	slm->state = NRF9160_SLM_START;

	/* Re-init underlying UART comms */
	ret = modem_iface_uart_init_dev(&slm->context.iface, DEVICE_DT_GET(NRF9160_SLM_UART_NODE));
	if (ret < 0) {
		LOG_ERR("modem_iface_uart_init returned %d", ret);
		slm->state = NRF9160_SLM_STATE_ERROR;
		goto unlock;
	}

	k_work_init_delayable(&slm->nrf9160_slm_configure_work, nrf9160_slm_configure);
	(void)nrf9160_slm_work_reschedule(&slm->nrf9160_slm_configure_work, K_NO_WAIT);

unlock:
	nrf9160_slm_unlock(slm);
}

void nrf9160_slm_stop(const struct device *dev)
{
	struct nrf9160_slm_modem *slm = dev->data;
	struct k_work_sync work_sync;

	if (slm->state == NRF9160_SLM_STOP) {
		LOG_ERR("nrf9160_slm is already %s", "stopped");
		return;
	}

	(void)k_work_cancel_delayable_sync(&slm->nrf9160_slm_configure_work, &work_sync);

	nrf9160_slm_lock(slm);

	if (slm->modem_off_cb != NULL) {
		slm->modem_off_cb(slm->dev/*, slm->user_data*/);
	}

	slm->state = NRF9160_SLM_STOP;
	slm->net_state = NRF9160_SLM_NET_INIT;
	nrf9160_slm_unlock(slm);
}

void nrf9160_slm_register_modem_power_callback(const struct device *dev,
					   nrf9160_slm_power_cb modem_on,
					   nrf9160_slm_power_cb modem_off/*,
					   void *user_data*/)
{
	struct nrf9160_slm_modem *slm = dev->data;

	nrf9160_slm_lock(slm);

	slm->modem_on_cb = modem_on;
	slm->modem_off_cb = modem_off;

	//slm->user_data = user_data;
	nrf9160_slm_unlock(slm);
}

void nrf9160_slm_register_modem_ready_callback(const struct device *dev,
					   nrf9160_slm_ready_cb modem_ready/*,
					   void *user_data*/)
{
	struct nrf9160_slm_modem *slm = dev->data;

	nrf9160_slm_lock(slm);

	slm->modem_ready_cb = modem_ready;
	//slm->ready_user_data = user_data;
	nrf9160_slm_unlock(slm);
}

const struct nrf9160_slm_modem_info *nrf9160_slm_modem_info(const struct device *dev)
{
	struct nrf9160_slm_modem *slm = dev->data;

	return &slm->minfo;
}

static void nrf9160_slm_mgmt_event_handler(struct net_mgmt_event_callback *cb,
			  uint32_t mgmt_event, struct net_if *iface)
{
	if ((mgmt_event & NET_EVENT_IF_DOWN) != mgmt_event) {
		return;
	}

	/* Right now we only support 1 GSM instance */
	if (iface != slm.iface) {
		return;
	}

	if (mgmt_event == NET_EVENT_IF_DOWN) {
		LOG_INF("GSM network interface down");
		/* raise semaphore to indicate the interface is down */
		k_sem_give(&slm.sem_if_down);
		return;
	}
}

static int nrf9160_slm_init(const struct device *dev)
{
	struct nrf9160_slm_modem *slm = dev->data;
	int ret;

	LOG_DBG("Nordic NRF9160 SLM (%p)", slm);

	(void)k_mutex_init(&slm->lock);
	slm->dev = dev;

	slm->cmd_handler_data.cmds[CMD_RESP] = response_cmds;
	slm->cmd_handler_data.cmds_len[CMD_RESP] = ARRAY_SIZE(response_cmds);
	slm->cmd_handler_data.match_buf = &slm->cmd_match_buf[0];
	slm->cmd_handler_data.match_buf_len = sizeof(slm->cmd_match_buf);
	slm->cmd_handler_data.buf_pool = &nrf9160_slm_recv_pool;
	slm->cmd_handler_data.alloc_timeout = K_NO_WAIT;
	slm->cmd_handler_data.eol = "\r\n";

	(void)k_sem_init(&slm->sem_response, 0, 1);
	(void)k_sem_init(&slm->sem_if_down, 0, 1);

	ret = modem_cmd_handler_init(&slm->context.cmd_handler,
				   &slm->cmd_handler_data);
	if (ret < 0) {
		LOG_DBG("cmd handler error %d", ret);
		return ret;
	}

#if defined(CONFIG_MODEM_SHELL)
	/* modem information storage */
	slm->context.data_manufacturer = slm->minfo.mdm_manufacturer;
	slm->context.data_model = slm->minfo.mdm_model;
	slm->context.data_revision = slm->minfo.mdm_revision;
	slm->context.data_imei = slm->minfo.mdm_imei;
#if defined(CONFIG_MODEM_SIM_NUMBERS)
	slm->context.data_imsi = slm->minfo.mdm_imsi;
	slm->context.data_iccid = slm->minfo.mdm_iccid;
#endif	/* CONFIG_MODEM_SIM_NUMBERS */
	slm->context.data_rssi = &slm->minfo.mdm_rssi;
#endif	/* CONFIG_MODEM_SHELL */

	slm->context.is_automatic_oper = true;
	slm->nrf9160_slm_data.rx_rb_buf = &slm->nrf9160_slm_rx_rb_buf[0];
	slm->nrf9160_slm_data.rx_rb_buf_len = sizeof(slm->nrf9160_slm_rx_rb_buf);
	slm->nrf9160_slm_data.hw_flow_control = DT_PROP(NRF9160_SLM_UART_NODE,
						hw_flow_control);

	ret = modem_iface_uart_init(&slm->context.iface, &slm->nrf9160_slm_data,
				DEVICE_DT_GET(NRF9160_SLM_UART_NODE));
	if (ret < 0) {
		LOG_DBG("iface uart error %d", ret);
		return ret;
	}

	struct uart_config uart_cfg;
	int err = uart_config_get(DEVICE_DT_GET(NRF9160_SLM_UART_NODE), &uart_cfg);
	if (err) {
		LOG_ERR("Failed to get uart config for nrf9160 uart: %d\n", err);
	}
	LOG_DBG("UART config: baud: %d stop: %d parity: %d flow: %d data: %d", uart_cfg.baudrate, uart_cfg.stop_bits, uart_cfg.parity, uart_cfg.flow_ctrl, uart_cfg.data_bits);
	//err = uart_configure(uart0_dev, &uart_cfg);
	//if (err) {
	//    printk("Failed to configure uart for nrf9160: %d\n", err);
	//}

	/* This is what SLM does but it's not quite using Zephyr APIs
	int RTS_PIN = nrf_uarte_rts_pin_get(NRF_UARTE0);
	int CTS_PIN = nrf_uarte_cts_pin_get(NRF_UARTE0);
	nrf_uarte_hwfc_pins_set(NRF_UARTE0,
				RTS_PIN,
				CTS_PIN);*/

	ret = modem_context_register(&slm->context);
	if (ret < 0) {
		LOG_DBG("context error %d", ret);
		return ret;
	}

	/* Initialize to stop state so that it can be started later */
	slm->state = NRF9160_SLM_STOP;

	slm->net_state = NRF9160_SLM_NET_INIT;

	LOG_DBG("iface->read %p iface->write %p",
		slm->context.iface.read, slm->context.iface.write);

	(void)k_thread_create(&slm->rx_thread, nrf9160_slm_rx_stack,
			      K_KERNEL_STACK_SIZEOF(nrf9160_slm_rx_stack),
			      (k_thread_entry_t) nrf9160_slm_rx,
			      slm, NULL, NULL, K_PRIO_COOP(7), 0, K_NO_WAIT);
	(void)k_thread_name_set(&slm->rx_thread, "nrf9160_slm_rx");

	/* initialize the work queue */
	k_work_queue_init(&slm->workq);
	k_work_queue_start(&slm->workq, nrf9160_slm_workq_stack, K_KERNEL_STACK_SIZEOF(nrf9160_slm_workq_stack),
			   K_PRIO_COOP(7), NULL);
	(void)k_thread_name_set(&slm->workq.thread, "nrf9160_slm_workq");

	net_mgmt_init_event_callback(&slm->nrf9160_slm_mgmt_cb, nrf9160_slm_mgmt_event_handler,
				     NET_EVENT_IF_DOWN);
	net_mgmt_add_event_callback(&slm->nrf9160_slm_mgmt_cb);

	return 0;
}

DEVICE_DT_DEFINE(DT_INST(0, test_nrf9160_slm), nrf9160_slm_init, NULL, &slm, NULL,
		 POST_KERNEL, CONFIG_MODEM_NRF9160_SLM_INIT_PRIORITY, NULL);
