#include <zephyr/kernel.h>
#include <zephyr/sys/printk.h>

#include <zephyr/bluetooth/bluetooth.h>
#include <mpsl/mpsl_work.h>
#include <sdc_hci_vs.h>

int multithreading_lock_acquire(k_timeout_t timeout);
void multithreading_lock_release(void);

static const sdc_hci_cmd_vs_compat_mode_window_offset_set_t params = {
	.enable = 1,
};
static struct k_work compat_mode_work;
static struct k_sem compat_mode_done;
static int compat_mode_lock_error;
static int compat_mode_result;

static void compat_mode_work_handler(struct k_work *work)
{
	ARG_UNUSED(work);
	printk("MPSL work handler entered\n");

	compat_mode_lock_error = multithreading_lock_acquire(K_NO_WAIT);
	if (!compat_mode_lock_error) {
		printk("About to call SDC compat-mode API (enable=%u)\n",
		       (unsigned int)params.enable);
		compat_mode_result = sdc_hci_cmd_vs_compat_mode_window_offset_set(&params);
		multithreading_lock_release();
	}
	k_sem_give(&compat_mode_done);
}

int main(void)
{
	int err;

	printk("Enabling Bluetooth\n");
	err = bt_enable(NULL);
	if (err) {
		printk("bt_enable failed: %d\n", err);
		return 0;
	}
	printk("Bluetooth enabled\n");

	k_sem_init(&compat_mode_done, 0, 1);
	k_work_init(&compat_mode_work, compat_mode_work_handler);
	printk("Submitting SDC window-offset command to MPSL workqueue (enable=%u)\n",
	       (unsigned int)params.enable);
	mpsl_work_submit(&compat_mode_work);
	err = k_sem_take(&compat_mode_done, K_FOREVER);
	if (err) {
		printk("Waiting for MPSL work item failed: %d\n", err);
		return 0;
	}
	if (compat_mode_lock_error) {
		printk("Controller lock unavailable in MPSL work item: %d\n",
		       compat_mode_lock_error);
	} else {
		printk("MPSL workqueue SDC call returned: %d\n", compat_mode_result);
	}

	return 0;
}