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

#include "streamctrl.h"
#include "bt_connection_manager.h"

#include <ctype.h>
#include <stdint.h>
#include <stdlib.h>
#include <string.h>
#include <sys/errno.h>
#include <zephyr/bluetooth/audio/audio.h>
#include <zephyr/drivers/bluetooth.h>
#include <zephyr/drivers/uart.h>
#include <zephyr/kernel.h>
#include <zephyr/zbus/zbus.h>
#include <zephyr/sys/byteorder.h>

// #include "broadcast_source.h"
#include "zbus_common.h"
#include "peripherals.h"
#include "led_assignments.h"
#include "led.h"
#include "button_assignments.h"
#include "macros_common.h"
#include "audio_system.h"
#include "bt_mgmt.h"
#include "fw_info_app.h"
#include "zephyr/arch/arch_interface.h"
#include "zephyr/bluetooth/bluetooth.h"
#include "zephyr/bluetooth/conn.h"
#include "zephyr/bluetooth/gap.h"
#include "zephyr/bluetooth/hci_types.h"
#include "zephyr/dt-bindings/led/led.h"
#include "zephyr/sys/time_units.h"

#include <zephyr/logging/log.h>
LOG_MODULE_REGISTER(main, CONFIG_MAIN_LOG_LEVEL);

BUILD_ASSERT(CONFIG_BT_AUDIO_CONCURRENT_TX_STREAMS_MAX <= CONFIG_AUDIO_ENCODE_CHANNELS_MAX);

ZBUS_SUBSCRIBER_DEFINE(button_evt_sub, CONFIG_BUTTON_MSG_SUB_QUEUE_SIZE);

ZBUS_MSG_SUBSCRIBER_DEFINE(le_audio_evt_sub);

ZBUS_CHAN_DECLARE(button_chan);
ZBUS_CHAN_DECLARE(le_audio_chan);
ZBUS_CHAN_DECLARE(bt_mgmt_chan);
ZBUS_CHAN_DECLARE(sdu_ref_chan);

ZBUS_OBS_DECLARE(sdu_ref_msg_listen);

#define LED_RGB_2 DT_NODE_CHILD_IDX(DT_NODELABEL(rgb2))

static struct k_thread button_msg_sub_thread_data;
static struct k_thread le_audio_msg_sub_thread_data;

static k_tid_t button_msg_sub_thread_id;
static k_tid_t le_audio_msg_sub_thread_id;

K_THREAD_STACK_DEFINE(button_msg_sub_thread_stack, CONFIG_BUTTON_MSG_SUB_STACK_SIZE);
K_THREAD_STACK_DEFINE(le_audio_msg_sub_thread_stack, CONFIG_LE_AUDIO_MSG_SUB_STACK_SIZE);

static enum stream_state strm_state = STATE_PAUSED;
/* Function for handling all stream state changes */
static void stream_state_set(enum stream_state stream_state_new)
{
	strm_state = stream_state_new;
}

/**
 * @brief	Handle button activity.
 */
static void button_msg_sub_thread(void)
{
	int ret;
	const struct zbus_channel *chan;

	while (1) {
		ret = zbus_sub_wait(&button_evt_sub, &chan, K_FOREVER);
		ERR_CHK(ret);

		struct button_msg msg;

		ret = zbus_chan_read(chan, &msg, ZBUS_READ_TIMEOUT_MS);
		ERR_CHK(ret);

		LOG_DBG("Got btn evt from queue - id = %d, action = %d", msg.button_pin,
			msg.button_action);

		if (msg.button_action != BUTTON_PRESS) {
			LOG_WRN("Unhandled button action");
			return;
		}

		switch (msg.button_pin) {
		case BUTTON_PLAY_PAUSE:
			if (strm_state == STATE_STREAMING) {
				ret = broadcast_stop(0);
				if (ret) {
					LOG_WRN("Failed to stop broadcaster: %d", ret);
				}
			} else if (strm_state == STATE_PAUSED) {
				ret = broadcast_start(0);
				if (ret) {
					LOG_WRN("Failed to start broadcaster: %d", ret);
				}
			} else {
				LOG_WRN("In invalid state: %d", strm_state);
			}

			break;

		case BUTTON_4:
			if (IS_ENABLED(CONFIG_AUDIO_TEST_TONE)) {
				if (strm_state != STATE_STREAMING) {
					LOG_WRN("Not in streaming state");
					break;
				}

				ret = audio_system_encode_test_tone_step();
				if (ret) {
					LOG_WRN("Failed to play test tone, ret: %d", ret);
				}

				break;
			}

			break;

		default:
			LOG_WRN("Unexpected/unhandled button id: %d", msg.button_pin);
		}

		STACK_USAGE_PRINT("button_msg_thread", &button_msg_sub_thread_data);
	}
}

/**
 * @brief	Handle Bluetooth LE audio events.
 */
static void le_audio_msg_sub_thread(void)
{
	int ret;
	const struct zbus_channel *chan;

	while (1) {
		struct le_audio_msg msg;

		ret = zbus_sub_wait_msg(&le_audio_evt_sub, &chan, &msg, K_FOREVER);
		ERR_CHK(ret);

		LOG_DBG("Received event = %d, current state = %d", msg.event, strm_state);

		switch (msg.event) {
		case LE_AUDIO_EVT_STREAMING:
			LOG_DBG("LE audio evt streaming");

			audio_system_encoder_start();

			if (strm_state == STATE_STREAMING) {
				LOG_DBG("Got streaming event in streaming state");
				break;
			}

			audio_system_start();
			stream_state_set(STATE_STREAMING);
			ret = led_on(LED_AUDIO_SYNC_STATUS);
			ERR_CHK(ret);
			ret = led_blink(LED_AUDIO_CONN_STATUS);
			ERR_CHK(ret);
			ret = led_on(LED_AUDIO_DEVICE_TYPE, LED_COLOR_GREEN);
			break;

		case LE_AUDIO_EVT_NOT_STREAMING:
			LOG_DBG("LE audio evt not_streaming");

			audio_system_encoder_stop();

			if (strm_state == STATE_PAUSED) {
				LOG_DBG("Got not_streaming event in paused state");
				break;
			}

			stream_state_set(STATE_PAUSED);
			audio_system_stop();
			ret = led_blink(LED_AUDIO_SYNC_STATUS);
			ERR_CHK(ret);
			ret = led_on(LED_AUDIO_CONN_STATUS);
			ERR_CHK(ret);
			ret = led_blink(LED_AUDIO_DEVICE_TYPE, LED_COLOR_GREEN);

			break;

		case LE_AUDIO_EVT_STREAM_SENT:
			/* Nothing to do. */
			break;

		default:
			LOG_WRN("Unexpected/unhandled le_audio event: %d", msg.event);

			break;
		}

		STACK_USAGE_PRINT("le_audio_msg_thread", &le_audio_msg_sub_thread_data);
	}
}

/**
 * @brief	Create zbus subscriber threads.
 *
 * @return	0 for success, error otherwise.
 */
static int zbus_subscribers_create(void)
{
	int ret;

	button_msg_sub_thread_id = k_thread_create(
		&button_msg_sub_thread_data, button_msg_sub_thread_stack,
		CONFIG_BUTTON_MSG_SUB_STACK_SIZE, (k_thread_entry_t)button_msg_sub_thread, NULL,
		NULL, NULL, K_PRIO_PREEMPT(CONFIG_BUTTON_MSG_SUB_THREAD_PRIO), 0, K_NO_WAIT);
	ret = k_thread_name_set(button_msg_sub_thread_id, "BUTTON_MSG_SUB");
	if (ret) {
		LOG_ERR("Failed to create button_msg thread");
		return ret;
	}

	le_audio_msg_sub_thread_id = k_thread_create(
		&le_audio_msg_sub_thread_data, le_audio_msg_sub_thread_stack,
		CONFIG_LE_AUDIO_MSG_SUB_STACK_SIZE, (k_thread_entry_t)le_audio_msg_sub_thread, NULL,
		NULL, NULL, K_PRIO_PREEMPT(CONFIG_LE_AUDIO_MSG_SUB_THREAD_PRIO), 0, K_NO_WAIT);
	ret = k_thread_name_set(le_audio_msg_sub_thread_id, "LE_AUDIO_MSG_SUB");
	if (ret) {
		LOG_ERR("Failed to create le_audio_msg thread");
		return ret;
	}

	ret = zbus_chan_add_obs(&sdu_ref_chan, &sdu_ref_msg_listen, ZBUS_ADD_OBS_TIMEOUT_MS);
	if (ret) {
		LOG_ERR("Failed to add timestamp listener");
		return ret;
	}

	return 0;
}

/**
 * @brief	Zbus listener to receive events from bt_mgmt.
 *
 * @param[in]	chan	Zbus channel.
 *
 * @note	Will in most cases be called from BT_RX context,
 *		so there should not be too much processing done here.
 */
static void bt_mgmt_evt_handler(const struct zbus_channel *chan)
{
	int ret;
	const struct bt_mgmt_msg *msg;

	msg = zbus_chan_const_msg(chan);

	switch (msg->event) {
	case BT_MGMT_EXT_ADV_WITH_PA_READY:
		LOG_INF("Ext adv ready");

		update_ext_adv_data(msg->index, msg->ext_adv);
		LOG_INF("index: %i", msg->index);
		ret = broadcast_start(msg->index);
		if (ret) {
			LOG_ERR("Failed to start broadcaster: %d", ret);
		}

		break;

	default:
		LOG_WRN("Unexpected/unhandled bt_mgmt event: %d", msg->event);
		break;
	}
}

ZBUS_LISTENER_DEFINE(bt_mgmt_evt_listen, bt_mgmt_evt_handler);

/**
 * @brief	Link zbus producers and observers.
 *
 * @return	0 for success, error otherwise.
 */
static int zbus_link_producers_observers(void)
{
	int ret;

	if (!IS_ENABLED(CONFIG_ZBUS)) {
		return -ENOTSUP;
	}

	ret = zbus_chan_add_obs(&button_chan, &button_evt_sub, ZBUS_ADD_OBS_TIMEOUT_MS);
	if (ret) {
		LOG_ERR("Failed to add button sub");
		return ret;
	}

	ret = zbus_chan_add_obs(&le_audio_chan, &le_audio_evt_sub, ZBUS_ADD_OBS_TIMEOUT_MS);
	if (ret) {
		LOG_ERR("Failed to add le_audio sub");
		return ret;
	}

	ret = zbus_chan_add_obs(&bt_mgmt_chan, &bt_mgmt_evt_listen, ZBUS_ADD_OBS_TIMEOUT_MS);
	if (ret) {
		LOG_ERR("Failed to add bt_mgmt listener");
		return ret;
	}

	return 0;
}



uint8_t stream_state_get(void)
{
	return strm_state;
}

void streamctrl_send(struct net_buf const *const audio_frame)
{
	int ret;
	static int prev_ret;

	if (strm_state == STATE_STREAMING) {
		ret = broadcast_send(0, audio_frame);

		if (ret != 0 && ret != prev_ret) {
			if (ret == -ECANCELED) {
				LOG_WRN("Sending operation cancelled");
			} else {
				LOG_WRN("Problem with sending LE audio data, ret: %d", ret);
			}
		}

		prev_ret = ret;
	}
}

#if CONFIG_CUSTOM_BROADCASTER
/* Example of how to create a custom broadcaster */
/**
 * Remember to increase:
 * CONFIG_BT_BAP_BROADCAST_SRC_SUBGROUP_COUNT
 * CONFIG_BT_CTLR_ADV_ISO_STREAM_COUNT (set in hci_ipc.conf)
 * CONFIG_BT_ISO_TX_BUF_COUNT
 * CONFIG_BT_BAP_BROADCAST_SRC_STREAM_COUNT
 * CONFIG_BT_ISO_MAX_CHAN
 */
#error Feature is incomplete and should only be used as a guideline for now
static struct bt_bap_lc3_preset lc3_preset_48 = BT_BAP_LC3_BROADCAST_PRESET_48_4_1(
	BT_AUDIO_LOCATION_FRONT_LEFT | BT_AUDIO_LOCATION_FRONT_RIGHT, BT_AUDIO_CONTEXT_TYPE_MEDIA);

static void broadcast_create(struct broadcast_source_big *broadcast_param)
{
	static enum bt_audio_location location[2] = {BT_AUDIO_LOCATION_FRONT_LEFT,
						     BT_AUDIO_LOCATION_FRONT_RIGHT};
	static struct subgroup_config subgroups[2];

	subgroups[0].group_lc3_preset = lc3_preset_48;
	subgroups[0].num_bises = 2;
	subgroups[0].context = BT_AUDIO_CONTEXT_TYPE_MEDIA;
	subgroups[0].location = location;

	subgroups[1].group_lc3_preset = lc3_preset_48;
	subgroups[1].num_bises = 2;
	subgroups[1].context = BT_AUDIO_CONTEXT_TYPE_MEDIA;
	subgroups[1].location = location;

	broadcast_param->packing = BT_ISO_PACKING_INTERLEAVED;

	broadcast_param->encryption = false;

	bt_audio_codec_cfg_meta_set_bcast_audio_immediate_rend_flag(
		&subgroups[0].group_lc3_preset.codec_cfg);
	bt_audio_codec_cfg_meta_set_bcast_audio_immediate_rend_flag(
		&subgroups[1].group_lc3_preset.codec_cfg);

	uint8_t spanish_src[3] = "spa";
	uint8_t english_src[3] = "eng";

	bt_audio_codec_cfg_meta_set_stream_lang(&subgroups[0].group_lc3_preset.codec_cfg,
						(uint32_t)sys_get_le24(english_src));
	bt_audio_codec_cfg_meta_set_stream_lang(&subgroups[1].group_lc3_preset.codec_cfg,
						(uint32_t)sys_get_le24(spanish_src));

	broadcast_param->subgroups = subgroups;
	broadcast_param->num_subgroups = 2;
}
#endif /* CONFIG_CUSTOM_BROADCASTER */


/* UART device */
#define UART_RX_BUF_SIZE 32 
#define UART_RX_TIMEOUT 200
static const struct device *uart = DEVICE_DT_GET(DT_NODELABEL(uart0));
static uint8_t rx_buf[UART_RX_BUF_SIZE] = {0};
struct tx_msg {
	void *fifo_reserved;
	uint8_t data[128];
	size_t len;
};
K_FIFO_DEFINE(uart_tx_fifo);
#define UART_TX_ERR_CHK(err_code, err_msg, msg, msg_length)                                                        \
  do {                                                                         \
    if (err_code) {                                                            \
      if (err_code == -EBUSY) {                                                \
		struct tx_msg *q_msg = k_malloc(sizeof(struct tx_msg)); \
		memcpy(q_msg->data, msg, msg_length); \
		q_msg->len = msg_length; \
		k_fifo_put(&uart_tx_fifo, q_msg); \
	  } \
	  else { \
        LOG_ERR("%s", err_msg);                                                    \
        PRINT_AND_OOPS(err_code);                                              \
      }                                                                        \
    }                                                                          \
  } while (0)


/**
 * @brief parses the command requesting a new broadcast name
 * 		  and saves it into new_name
 * 
 * @param buf pointer to the char array containing the command received
			  including the entire command, e.g. change name to 'new name'
			  buffer contains: 'N new name'
 * @return 0 if successful
 *		   -EINVAL if request invalid (e.g. no name specified, name too long) 
				   or not a name change request
 */
static int parse_and_set_new_broadcast_name(uint8_t big_index, uint8_t *buf, size_t len)
{
	while(len && isspace(*buf)){
		buf++;
		len--;
	}

	if(len==0){
		LOG_ERR("no new name provided");
		return -EINVAL;
	}

	if(len>=BROADCAST_NAME_MAX_LENGTH){
		LOG_ERR("Invalid name: too long (%i chars)", len);
		return -EINVAL;
	}

	uint8_t new_broadcast_name[BROADCAST_NAME_MAX_LENGTH];
	strncpy(new_broadcast_name, buf, len);
	new_broadcast_name[len] = '\0';

	LOG_INF("parsed new broadcast name: %s (%i chars)", new_broadcast_name, len);

	int ret = change_broadcast_name(big_index, new_broadcast_name, strlen(new_broadcast_name));
	ERR_CHK_MSG(ret, "Failure changing broadcast name");

	return 0;
}


/**
 * @brief receive commands from UART
 */
static void uart_command_handler(uint8_t *buf, size_t offset, size_t len)
{

	char cmd = buf[offset];

	int8_t channel = -EINVAL;
	uint8_t additional_offset = 1;
	for(; additional_offset<len; additional_offset++)
	{
		if(isspace(buf[offset+additional_offset])){
			continue;
		}

		if(!isdigit(buf[offset+additional_offset]))
		{
			channel = -EINVAL;
			LOG_WRN("received command with invalid channel specifier %c", buf[offset+additional_offset]);
			break;
		}

		channel = atoi(&buf[offset+additional_offset]);
		if(channel > NUM_CHANNELS)
		{
			LOG_WRN("received command for invalid channel (%i)", channel);
			channel = -EINVAL;
		}
		break;
	}

	LOG_INF("cmd: %c, channel: %u", cmd, channel);
	if(channel >= 0)
	{
		if(cmd == 'E')
		{
			enable_bt((uint8_t)channel);
		}
		else if(cmd == 'D'){ 
			disable_bt((uint8_t)channel);
		}
		else if(cmd == 'N'){
			// after loop buf[offset+additional_offset] is pointing to the
			// channel index. increment again so we can use it to get first
			// char after channel index to start parsing new name
			additional_offset++;
			uint8_t new_offset = offset + additional_offset;
			size_t remaining_len = len - additional_offset;
			parse_and_set_new_broadcast_name((uint8_t)channel, buf + new_offset,
                                                   remaining_len);
		}
		else if(cmd == 'I'){
			// not an actual requirement, just introduced for testing
			// not set up to accept name, only uses cofigured BROADCAST_NAME_ALT
			initialise_bc((uint8_t)channel, CONFIG_BT_AUDIO_BROADCAST_NAME_ALT, sizeof(CONFIG_BT_AUDIO_BROADCAST_NAME_ALT));
		}
	}
}

void try_send_next(){
	static struct tx_msg *current_msg = NULL;

	current_msg = k_fifo_get(&uart_tx_fifo, K_NO_WAIT);
	if(current_msg) {
		int ret = uart_tx(uart, current_msg->data, current_msg->len, SYS_FOREVER_MS);
		UART_TX_ERR_CHK(ret, "FAILURE trying to send queued tx", current_msg->data, current_msg->len);
	}
	k_free(current_msg);
}

static void uart_cb(const struct device *dev, struct uart_event *evt, void *user_data)
{
	int ret;
	led_on(LED_RGB_2, LED_COLOR_GREEN);
	switch(evt->type){
		case UART_RX_RDY:
			uart_command_handler(evt->data.rx.buf, evt->data.rx.offset, evt->data.rx.len);
			break;
		case UART_RX_DISABLED:
			ret = uart_rx_enable(uart, rx_buf, sizeof(rx_buf), UART_RX_TIMEOUT);
			ERR_CHK_MSG(ret, "FAILURE reenabling rx");
			break;
		case UART_TX_DONE:
			try_send_next();
			break;
		default:
			break;
	}
}


int main(void)
{
	int ret;

	LOG_DBG("Main started");


	ret = peripherals_init();
	ERR_CHK(ret);

	ret = fw_info_app_print();
	ERR_CHK(ret);

	ret = bt_mgmt_init();
	ERR_CHK(ret);

	ret = audio_system_init();
	ERR_CHK(ret);

	ret = zbus_subscribers_create();
	ERR_CHK_MSG(ret, "Failed to create zbus subscriber threads");

	ret = zbus_link_producers_observers();
	ERR_CHK_MSG(ret, "Failed to link zbus producers and observers");


	ret = uart_callback_set(uart, uart_cb, NULL);
	ERR_CHK(ret);

	ret = uart_rx_enable(uart, rx_buf, sizeof(rx_buf), UART_RX_TIMEOUT);
	ERR_CHK(ret);

	// add tx buffer to check serial connection is built properly
	static uint8_t tx_buf[] = {"*****\n ***\n  *\nHello from TXBUF\n  *\n ***\n****"};
	ret = uart_tx(uart, tx_buf, sizeof(tx_buf), SYS_FOREVER_MS);
	ERR_CHK(ret);

	ret = initialise_broadcast(0, CONFIG_BT_AUDIO_BROADCAST_NAME, sizeof(CONFIG_BT_AUDIO_BROADCAST_NAME));
	ERR_CHK_MSG(ret, "Failed to initialise bluetooth broadcast 0");

	ret = audio_system_config_set(
		bt_audio_codec_cfg_freq_to_freq_hz(CONFIG_BT_AUDIO_PREF_SAMPLE_RATE_VALUE),
		CONFIG_BT_AUDIO_BITRATE_BROADCAST_SRC, VALUE_NOT_SET);
	ERR_CHK_MSG(ret, "Failed to set sample- and bitrate");

	ret = initialise_broadcast(1, CONFIG_BT_AUDIO_BROADCAST_NAME_ALT, sizeof(CONFIG_BT_AUDIO_BROADCAST_NAME_ALT));
	ERR_CHK_MSG(ret, "Failed to initialise bluetooth broadcast 1");

	return 0;
}
