#include <zephyr/logging/log.h>
#include <stddef.h>
#include <stdint.h>
#include <inttypes.h>
#include <math.h>
#include <string.h>
#include <zephyr/kernel.h>

#include <zephyr/sys/printk.h>
#include <zephyr/sys/byteorder.h>
#include <zephyr/sys/util.h>

#include <zephyr/bluetooth/bluetooth.h>
#include <zephyr/bluetooth/gap.h>
#include <zephyr/bluetooth/direction.h>
#include <zephyr/bluetooth/hci.h>
#include <zephyr/bluetooth/hci_vs.h>
#include <zephyr/bluetooth/hci_types.h>

#include "common/message_channel.h"
#include "common/system_state.h"

LOG_MODULE_REGISTER(app, LOG_LEVEL_INF);

#define DEVICE_NAME     CONFIG_BT_DEVICE_NAME
#define DEVICE_NAME_LEN (sizeof(DEVICE_NAME) - 1)
#define NAME_LEN        30
#define PEER_NAME_LEN_MAX 30
/* BT Core 5.3 Vol 6, Part B section 4.4.5.1 Periodic Advertising Trains allows controller to wait
 * 6 periodic advertising events for synchronization establishment, hence timeout must be longer
 * than that.
 */
#define SYNC_CREATE_TIMEOUT_INTERVAL_NUM 7
/* Maximum length of advertising data represented in hexadecimal format */
#define ADV_DATA_HEX_STR_LEN_MAX (BT_GAP_ADV_MAX_EXT_ADV_DATA_LEN * 2 + 1)

#define RSSI_REF_DBM (-59.0f)
#define RSSI_PATH_LOSS_EXPONENT 2.0f
#define RSSI_MAX_DISTANCE_METERS 1000.0f

#define MAX_BEACON_SYNCS 4

struct beacon_entry {
    struct bt_le_per_adv_sync *sync;
    bt_addr_le_t addr;
    uint16_t interval;
    uint16_t timeout;
    uint8_t sid;
    bool in_use;
    bool sync_pending;
    bool cte_enabled;
    char name[NAME_LEN];   /* <-- NEW: keep the parsed name */
};

/* Helper: push one JSON payload to MQTT via zbus */
static void publish_aoa_measurement(const struct beacon_entry *entry,
                                    int16_t rssi_tenths_dbm,
                                    uint32_t distance_mm,
#if defined(CONFIG_BT_DF_CTE_RX_AOA)
                                    bool have_angle,
                                    float angle_deg
#else
                                    bool have_angle,
                                    float angle_deg /* unused */
#endif
                                    )
{
    char addr_str[BT_ADDR_LE_STR_LEN];
    struct payload msg;

    bt_addr_le_to_str(&entry->addr, addr_str, sizeof(addr_str));

    /* Build a compact JSON string. Assumes msg.string is a char[] in your payload. */
    /* rssi_tenths_dbm is 1/10 dBm; convert to float dBm for readability */
    const float rssi_dbm = (float)rssi_tenths_dbm / 10.0f;
    const float distance_m = (float)distance_mm / 1000.0f;

    if (have_angle) {
        /* angle present */
        (void)snprintf(msg.string, sizeof(msg.string),
            "{\"addr\":\"%s\",\"sid\":%u,\"name\":\"%s\",\"rssi_dbm\":%.1f,"
            "\"distance_m\":%.2f,\"angle_deg\":%.3f}",
            addr_str, entry->sid, entry->name, (double)rssi_dbm, (double)distance_m, (double)angle_deg);
    } else {
        /* no angle for this report (still useful) */
        (void)snprintf(msg.string, sizeof(msg.string),
            "{\"addr\":\"%s\",\"sid\":%u,\"name\":\"%s\",\"rssi_dbm\":%.1f,"
            "\"distance_m\":%.2f}",
            addr_str, entry->sid, entry->name, (double)rssi_dbm, (double)distance_m);
    }

    /* Non-blocking publish: if the queue is full we’ll drop this one. */
    int err = zbus_chan_pub(&PAYLOAD_CHAN, &msg, K_NO_WAIT);
    if (err) {
        printk("publish_aoa_measurement: zbus publish failed, err %d\n", err);
    }
}

static struct beacon_entry beacon_table[MAX_BEACON_SYNCS];
static K_MUTEX_DEFINE(beacon_mutex);
static bool scan_enabled;

static bool data_cb(struct bt_data *data, void *user_data);
static void sync_cb(struct bt_le_per_adv_sync *sync,
		    struct bt_le_per_adv_sync_synced_info *info);
static void term_cb(struct bt_le_per_adv_sync *sync,
		    const struct bt_le_per_adv_sync_term_info *info);
static void recv_cb(struct bt_le_per_adv_sync *sync,
		    const struct bt_le_per_adv_sync_recv_info *info,
		    struct net_buf_simple *buf);
static void cte_recv_cb(struct bt_le_per_adv_sync *sync,
			struct bt_df_per_adv_sync_iq_samples_report const *report);
static void scan_recv(const struct bt_le_scan_recv_info *info,
		      struct net_buf_simple *buf);

#if defined(CONFIG_BT_DF_CTE_RX_AOA)
/* Antenna switch pattern for the AN240205-002 four-element array. VC1 maps to
 * dfegpio0 and VC2 maps to dfegpio1 in the board overlay.
 */
const static uint8_t ant_patterns[] = {
	0x2, /* Antenna 1: VC1=L, VC2=H */
	0x0, /* Antenna 2: VC1=L, VC2=L */
	0x3, /* Antenna 3: VC1=H, VC2=H */
	0x1, /* Antenna 4: VC1=H, VC2=L */
};

#define PI_F 3.14159265358979323846f
#define TWO_PI_F (2.0f * PI_F)
#define DEG_PER_RAD (180.0f / PI_F)
#define AOA_ELEMENT_SPACING_RATIO 0.5f
#define AOA_REFERENCE_SAMPLE_COUNT 8U
#define SWITCH_SAMPLE_COUNT_1US 1U
#define SWITCH_SAMPLE_COUNT_2US 2U

enum aoa_estimation_status {
	AOA_ESTIMATION_SUCCESS,
	AOA_ESTIMATION_NOT_AOA,
	AOA_ESTIMATION_UNSUPPORTED_FORMAT,
	AOA_ESTIMATION_INSUFFICIENT_SAMPLES,
	AOA_ESTIMATION_BAD_PACKET,
};

static inline float wrap_pm_pi(float angle)
{
	while (angle > PI_F) {
		angle -= TWO_PI_F;
	}

	while (angle < -PI_F) {
		angle += TWO_PI_F;
	}

	return angle;
}

static enum aoa_estimation_status
aoa_estimate_deg(const struct bt_df_per_adv_sync_iq_samples_report *report,
		      float *out_angle_deg)
{
	size_t total_samples;
	size_t reference_samples;
	size_t slot_sample_count;
	size_t available_samples;
	size_t max_slots;
	size_t slots_used;
	float phases[ARRAY_SIZE(ant_patterns)];
	float unwrapped[ARRAY_SIZE(ant_patterns)];
	float ref_i = 0.0f;
	float ref_q = 0.0f;
	float ref_phase;
	float sum_x = 0.0f;
	float sum_y = 0.0f;
	float numerator = 0.0f;
	float denominator = 0.0f;

	if (report->cte_type != BT_DF_CTE_TYPE_AOA) {
		return AOA_ESTIMATION_NOT_AOA;
	}

	if (report->packet_status != BT_DF_CTE_CRC_OK) {
		return AOA_ESTIMATION_BAD_PACKET;
	}

	if (report->sample_type != BT_DF_IQ_SAMPLE_8_BITS_INT) {
		return AOA_ESTIMATION_UNSUPPORTED_FORMAT;
	}

	total_samples = report->sample_count;
	if (total_samples <= AOA_REFERENCE_SAMPLE_COUNT) {
		return AOA_ESTIMATION_INSUFFICIENT_SAMPLES;
	}

	slot_sample_count = SWITCH_SAMPLE_COUNT_1US;
	if (report->slot_durations == BT_DF_ANTENNA_SWITCHING_SLOT_2US) {
		slot_sample_count = SWITCH_SAMPLE_COUNT_2US;
	}

	reference_samples = AOA_REFERENCE_SAMPLE_COUNT;
	if (reference_samples > total_samples) {
		reference_samples = total_samples;
	}

	for (size_t idx = 0U; idx < reference_samples; ++idx) {
		ref_i += (float)report->sample[idx].i;
		ref_q += (float)report->sample[idx].q;
	}

	ref_i /= (float)reference_samples;
	ref_q /= (float)reference_samples;
	ref_phase = atan2f(ref_q, ref_i);

	available_samples = total_samples - reference_samples;
	max_slots = available_samples / slot_sample_count;
	if (max_slots > ARRAY_SIZE(ant_patterns)) {
		max_slots = ARRAY_SIZE(ant_patterns);
	}

	if (max_slots < 2U) {
		return AOA_ESTIMATION_INSUFFICIENT_SAMPLES;
	}

	slots_used = 0U;
	for (; slots_used < max_slots; ++slots_used) {
		size_t start = reference_samples + slots_used * slot_sample_count;
		float avg_i = 0.0f;
		float avg_q = 0.0f;

		if ((start + slot_sample_count) > total_samples) {
			break;
		}

		for (size_t s = 0U; s < slot_sample_count; ++s) {
			const struct bt_hci_le_iq_sample *sample =
				&report->sample[start + s];

			avg_i += (float)sample->i;
			avg_q += (float)sample->q;
		}

		avg_i /= (float)slot_sample_count;
		avg_q /= (float)slot_sample_count;
		phases[slots_used] = wrap_pm_pi(atan2f(avg_q, avg_i) - ref_phase);
	}

	if (slots_used < 2U) {
		return AOA_ESTIMATION_INSUFFICIENT_SAMPLES;
	}

	unwrapped[0] = phases[0];
	for (size_t i = 1U; i < slots_used; ++i) {
		float delta = wrap_pm_pi(phases[i] - phases[i - 1U]);

		unwrapped[i] = unwrapped[i - 1U] + delta;
	}

	for (size_t i = 0U; i < slots_used; ++i) {
		sum_x += (float)i;
		sum_y += unwrapped[i];
	}

	float mean_x = sum_x / (float)slots_used;
	float mean_y = sum_y / (float)slots_used;

	for (size_t i = 0U; i < slots_used; ++i) {
		float x = (float)i - mean_x;
		float y = unwrapped[i] - mean_y;

		numerator += x * y;
		denominator += x * x;
	}

	if ((denominator < 1e-6f) && (denominator > -1e-6f)) {
		return AOA_ESTIMATION_INSUFFICIENT_SAMPLES;
	}

	float slope = numerator / denominator;
	float ratio = slope / (TWO_PI_F * AOA_ELEMENT_SPACING_RATIO);

	if (ratio > 1.0f) {
		ratio = 1.0f;
	} else if (ratio < -1.0f) {
		ratio = -1.0f;
	}

	*out_angle_deg = asinf(ratio) * DEG_PER_RAD;

	return AOA_ESTIMATION_SUCCESS;
}
#endif /* CONFIG_BT_DF_CTE_RX_AOA */

static int beacon_index(const struct beacon_entry *entry)
{
	return (int)(entry - beacon_table);
}

static struct beacon_entry *beacon_find_locked(const bt_addr_le_t *addr, uint8_t sid)
{
	for (size_t idx = 0U; idx < ARRAY_SIZE(beacon_table); ++idx) {
		struct beacon_entry *entry = &beacon_table[idx];

		if (!entry->in_use) {
			continue;
		}

		if ((entry->sid == sid) && (bt_addr_le_cmp(&entry->addr, addr) == 0)) {
			return entry;
		}
	}

	return NULL;
}

static struct beacon_entry *beacon_find_by_sync_locked(struct bt_le_per_adv_sync *sync)
{
	for (size_t idx = 0U; idx < ARRAY_SIZE(beacon_table); ++idx) {
		struct beacon_entry *entry = &beacon_table[idx];

		if (entry->in_use && entry->sync == sync) {
			return entry;
		}
	}

	return NULL;
}

static struct beacon_entry *beacon_alloc_locked(void)
{
	for (size_t idx = 0U; idx < ARRAY_SIZE(beacon_table); ++idx) {
		struct beacon_entry *entry = &beacon_table[idx];

		if (entry->in_use) {
			continue;
		}

		(void)memset(entry, 0, sizeof(*entry));
		entry->in_use = true;
		return entry;
	}

	return NULL;
}

static void beacon_release_locked(struct beacon_entry *entry)
{
	(void)memset(entry, 0, sizeof(*entry));
}

static void beacon_sync_create(size_t slot_idx)
{
	struct bt_le_per_adv_sync_param params = { 0 };
	struct bt_le_per_adv_sync *sync = NULL;
	char addr_str[BT_ADDR_LE_STR_LEN];
	int err;

	k_mutex_lock(&beacon_mutex, K_FOREVER);
	if (slot_idx >= ARRAY_SIZE(beacon_table)) {
		k_mutex_unlock(&beacon_mutex);
		return;
	}

	struct beacon_entry *entry = &beacon_table[slot_idx];

	if (!entry->in_use || !entry->sync_pending) {
		k_mutex_unlock(&beacon_mutex);
		return;
	}

	params.options = BT_LE_PER_ADV_SYNC_OPT_SYNC_ONLY_CONST_TONE_EXT;
	params.sid = entry->sid;
	params.skip = 0;
	params.timeout = entry->timeout;
	bt_addr_le_copy(&params.addr, &entry->addr);
	bt_addr_le_to_str(&entry->addr, addr_str, sizeof(addr_str));

	k_mutex_unlock(&beacon_mutex);

	printk("Beacon[%zu]: creating periodic advertising sync with %s (SID %u)...\n",
	       slot_idx, addr_str, params.sid);

	err = bt_le_per_adv_sync_create(&params, &sync);

	k_mutex_lock(&beacon_mutex, K_FOREVER);
	entry = &beacon_table[slot_idx];

	if (!entry->in_use || !entry->sync_pending) {
		k_mutex_unlock(&beacon_mutex);
		if ((err == 0) && (sync != NULL)) {
			(void)bt_le_per_adv_sync_delete(sync);
		}
		return;
	}

	if (err != 0) {
		entry->sync_pending = false;
		k_mutex_unlock(&beacon_mutex);
		printk("Beacon[%zu]: sync create failed (err %d)\n", slot_idx, err);
		return;
	}

	entry->sync = sync;
	k_mutex_unlock(&beacon_mutex);
	printk("Beacon[%zu]: sync create requested.\n", slot_idx);
}

static void beacon_enable_cte(size_t slot_idx)
{
	struct bt_le_per_adv_sync *sync;
	bool already_enabled;
	int err;

	k_mutex_lock(&beacon_mutex, K_FOREVER);
	if (slot_idx >= ARRAY_SIZE(beacon_table)) {
		k_mutex_unlock(&beacon_mutex);
		return;
	}

	struct beacon_entry *entry = &beacon_table[slot_idx];

	if (!entry->in_use || entry->sync == NULL) {
		k_mutex_unlock(&beacon_mutex);
		return;
	}

	already_enabled = entry->cte_enabled;
	if (already_enabled) {
		k_mutex_unlock(&beacon_mutex);
		return;
	}

	entry->cte_enabled = true;
	sync = entry->sync;
	k_mutex_unlock(&beacon_mutex);

	const struct bt_df_per_adv_sync_cte_rx_param cte_rx_params = {
		.max_cte_count = 5,
#if defined(CONFIG_BT_DF_CTE_RX_AOA)
		.cte_types = BT_DF_CTE_TYPE_ALL,
		.slot_durations = 0x2,
		.num_ant_ids = ARRAY_SIZE(ant_patterns),
		.ant_ids = ant_patterns,
#else
		.cte_types = BT_DF_CTE_TYPE_AOD_1US | BT_DF_CTE_TYPE_AOD_2US,
#endif /* CONFIG_BT_DF_CTE_RX_AOA */
	};

	printk("Beacon[%zu]: enabling CTE reception...\n", slot_idx);
	err = bt_df_per_adv_sync_cte_rx_enable(sync, &cte_rx_params);
	if (err != 0) {
		printk("Beacon[%zu]: failed to enable CTE reception (err %d)\n",
		       slot_idx, err);
		k_mutex_lock(&beacon_mutex, K_FOREVER);
		struct beacon_entry *entry_retry = &beacon_table[slot_idx];
		if (entry_retry->in_use && entry_retry->sync == sync) {
			entry_retry->cte_enabled = false;
		}
		k_mutex_unlock(&beacon_mutex);
	} else {
		printk("Beacon[%zu]: CTE reception enabled.\n", slot_idx);
	}
}

static struct bt_le_per_adv_sync_cb sync_callbacks = {
	.synced = sync_cb,
	.term = term_cb,
	.recv = recv_cb,
	.cte_report_cb = cte_recv_cb,
};

static struct bt_le_scan_cb scan_callbacks = {
	.recv = scan_recv,
};

static uint16_t sync_create_timeout_get(uint16_t interval)
{
	uint32_t interval_us;
	uint32_t timeout;

	/* Add retries and convert to unit in 10's of ms */
	interval_us = BT_GAP_PER_ADV_INTERVAL_TO_US(interval);
	timeout = BT_GAP_US_TO_PER_ADV_SYNC_TIMEOUT(interval_us) * SYNC_CREATE_TIMEOUT_INTERVAL_NUM;

	/* Enforce restraints */
	timeout = CLAMP(timeout, BT_GAP_PER_ADV_MIN_TIMEOUT, BT_GAP_PER_ADV_MAX_TIMEOUT);

	return (uint16_t)timeout;
}

static const char *phy2str(uint8_t phy)
{
	switch (phy) {
	case 0: return "No packets";
	case BT_GAP_LE_PHY_1M: return "LE 1M";
	case BT_GAP_LE_PHY_2M: return "LE 2M";
	case BT_GAP_LE_PHY_CODED: return "LE Coded";
	default: return "Unknown";
	}
}

static const char *cte_type2str(uint8_t type)
{
	switch (type) {
	case BT_DF_CTE_TYPE_AOA: return "AOA";
	case BT_DF_CTE_TYPE_AOD_1US: return "AOD 1 [us]";
	case BT_DF_CTE_TYPE_AOD_2US: return "AOD 2 [us]";
	case BT_DF_CTE_TYPE_NONE: return "";
	default: return "Unknown";
	}
}

static const char *sample_type2str(enum bt_df_iq_sample type)
{
	switch (type) {
	case BT_DF_IQ_SAMPLE_8_BITS_INT:
		return "8 bits int";
	case BT_DF_IQ_SAMPLE_16_BITS_INT:
		return "16 bits int";
	default:
		return "Unknown";
	}
}

static const char *pocket_status2str(uint8_t status)
{
	switch (status) {
	case BT_DF_CTE_CRC_OK: return "CRC OK";
	case BT_DF_CTE_CRC_ERR_CTE_BASED_TIME: return "CRC not OK, CTE Info OK";
	case BT_DF_CTE_CRC_ERR_CTE_BASED_OTHER: return "CRC not OK, Sampled other way";
	case BT_DF_CTE_INSUFFICIENT_RESOURCES: return "No resources";
	default: return "Unknown";
	}
}

static uint32_t estimate_distance_mm(int16_t rssi_tenths_dbm)
{
	float rssi_dbm = (float)rssi_tenths_dbm / 10.0f;
	float ratio = (RSSI_REF_DBM - rssi_dbm) / (10.0f * RSSI_PATH_LOSS_EXPONENT);
	float meters = powf(10.0f, ratio);

	if (meters < 0.0f) {
		meters = 0.0f;
	} else if (meters > RSSI_MAX_DISTANCE_METERS) {
		meters = RSSI_MAX_DISTANCE_METERS;
	}

	return (uint32_t)(meters * 1000.0f + 0.5f);
}

static inline uint32_t adv_interval_to_ms(uint16_t interval)
{
	return ((uint32_t)interval * 5U) / 4U;
}

static bool data_cb(struct bt_data *data, void *user_data)
{
    char *name = user_data;
    uint8_t len;

    switch (data->type) {
    case BT_DATA_NAME_SHORTENED:
    case BT_DATA_NAME_COMPLETE:
        len = MIN(data->data_len, NAME_LEN - 1);
        memcpy(name, data->data, len);
        name[len] = '\0';
        return false;
    default:
        return true;
    }
}

static void sync_cb(struct bt_le_per_adv_sync *sync,
		    struct bt_le_per_adv_sync_synced_info *info)
{
	char le_addr[BT_ADDR_LE_STR_LEN];
	int slot_idx = -1;

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

	k_mutex_lock(&beacon_mutex, K_FOREVER);
	struct beacon_entry *entry = beacon_find_by_sync_locked(sync);
	if (entry != NULL) {
		entry->sync_pending = false;
		entry->interval = info->interval;
		slot_idx = beacon_index(entry);
	}
	k_mutex_unlock(&beacon_mutex);

	printk("PER_ADV_SYNC[%u] (slot %d): [DEVICE]: %s synced, Interval 0x%04x (%u ms), PHY %s\n",
	       bt_le_per_adv_sync_get_index(sync), slot_idx, le_addr, info->interval,
	       adv_interval_to_ms(info->interval), phy2str(info->phy));

	if (slot_idx >= 0) {
		beacon_enable_cte((size_t)slot_idx);
	}
}

static void term_cb(struct bt_le_per_adv_sync *sync,
		    const struct bt_le_per_adv_sync_term_info *info)
{
	char le_addr[BT_ADDR_LE_STR_LEN];
	int slot_idx = -1;

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

	k_mutex_lock(&beacon_mutex, K_FOREVER);
	struct beacon_entry *entry = beacon_find_by_sync_locked(sync);
	if (entry != NULL) {
		slot_idx = beacon_index(entry);
		beacon_release_locked(entry);
	}
	k_mutex_unlock(&beacon_mutex);

	printk("PER_ADV_SYNC[%u] (slot %d): [DEVICE]: %s sync terminated\n",
	       bt_le_per_adv_sync_get_index(sync), slot_idx, le_addr);
}

static void recv_cb(struct bt_le_per_adv_sync *sync,
		    const struct bt_le_per_adv_sync_recv_info *info,
		    struct net_buf_simple *buf)
{
	static char data_str[ADV_DATA_HEX_STR_LEN_MAX];
	char le_addr[BT_ADDR_LE_STR_LEN];

	bt_addr_le_to_str(info->addr, le_addr, sizeof(le_addr));
	bin2hex(buf->data, buf->len, data_str, sizeof(data_str));

	printk("PER_ADV_SYNC[%u]: [DEVICE]: %s, tx_power %i, "
	       "RSSI %i, CTE %s, data length %u, data: %s\n",
	       bt_le_per_adv_sync_get_index(sync), le_addr, info->tx_power,
	       info->rssi, cte_type2str(info->cte_type), buf->len, data_str);
}

static void cte_recv_cb(struct bt_le_per_adv_sync *sync,
                        const struct bt_df_per_adv_sync_iq_samples_report *report)
{
    int slot_idx = -1;
    int16_t rssi_tenths_dbm = report->rssi;
    int16_t rssi_whole_dbm = rssi_tenths_dbm / 10;
    int16_t rssi_remainder = rssi_tenths_dbm % 10;
    uint8_t rssi_frac_dbm = (rssi_remainder < 0) ? (uint8_t)(-rssi_remainder) :
                                               (uint8_t)rssi_remainder;

    uint32_t distance_mm = estimate_distance_mm(rssi_tenths_dbm);
    uint32_t distance_m_whole = distance_mm / 1000U;
    uint32_t distance_mm_remainder = distance_mm % 1000U;
    uint32_t distance_m_frac = (distance_mm_remainder + 5U) / 10U;

    if (distance_m_frac >= 100U) {
        distance_m_frac -= 100U;
        ++distance_m_whole;
    }

    k_mutex_lock(&beacon_mutex, K_FOREVER);
    struct beacon_entry *entry = beacon_find_by_sync_locked(sync);
    if (entry != NULL) {
        slot_idx = beacon_index(entry);
    }
    k_mutex_unlock(&beacon_mutex);

    /* --- Keep your detailed printk output --- */
    printk("CTE[%u] (slot %d): samples type: %s, samples count %u, "
           "cte type %s, slot durations: %u [us], packet status %s, "
           "RSSI %d.%01u dBm (~%u.%02u m)\n",
           bt_le_per_adv_sync_get_index(sync),
           slot_idx,
           sample_type2str(report->sample_type),
           report->sample_count,
           cte_type2str(report->cte_type),
           report->slot_durations,
           pocket_status2str(report->packet_status),
           rssi_whole_dbm,
           rssi_frac_dbm,
           distance_m_whole,
           distance_m_frac);

    /* --- Angle estimation + publish --- */
    bool have_angle = false;
    float angle_deg = 0.0f;

#if defined(CONFIG_BT_DF_CTE_RX_AOA)
    enum aoa_estimation_status status = aoa_estimate_deg(report, &angle_deg);

    switch (status) {
    case AOA_ESTIMATION_SUCCESS:
        have_angle = true;
        printk("  -> AoA estimate: %.3f deg\n", (double)angle_deg);
        break;

    case AOA_ESTIMATION_NOT_AOA:
        /* no AoA samples in this report */
        break;

    case AOA_ESTIMATION_UNSUPPORTED_FORMAT:
        printk("  -> AoA: unsupported IQ sample type %u\n", report->sample_type);
        break;

    case AOA_ESTIMATION_INSUFFICIENT_SAMPLES:
        printk("  -> AoA: insufficient IQ samples for estimation (count %u)\n",
               report->sample_count);
        break;

    case AOA_ESTIMATION_BAD_PACKET:
        printk("  -> AoA: skipped due to packet status %u\n", report->packet_status);
        break;
    }
#endif /* CONFIG_BT_DF_CTE_RX_AOA */

    /* --- NEW: publish to MQTT via zbus --- */
    if (entry != NULL) {
        publish_aoa_measurement(entry, rssi_tenths_dbm, distance_mm, have_angle, angle_deg);
    }

    /* --- Optional: still print IQ samples if desired --- */
    if (IS_ENABLED(CONFIG_DF_LOCATOR_APP_IQ_REPORT_PRINT_IQ_SAMPLES)) {
        for (uint8_t idx = 0; idx < report->sample_count; idx++) {
            if (report->sample_type == BT_DF_IQ_SAMPLE_8_BITS_INT) {
                printk(" IQ[%d]: %d, %d\n",
                       idx, report->sample[idx].i, report->sample[idx].q);
            } else if (IS_ENABLED(CONFIG_BT_DF_VS_CL_IQ_REPORT_16_BITS_IQ_SAMPLES)) {
                printk(" IQ[%" PRIu8 "]: %d, %d\n",
                       idx, report->sample16[idx].i, report->sample16[idx].q);
            } else {
                printk("Unhandled vendor specific IQ samples type\n");
                break;
            }
        }
    }
}


static void scan_recv(const struct bt_le_scan_recv_info *info,
		      struct net_buf_simple *buf)
{
	char le_addr[BT_ADDR_LE_STR_LEN];
    char name[NAME_LEN] = {0};
    bool start_sync = false;
    int slot_idx = -1;

    bt_data_parse(buf, data_cb, name);

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

	printk("[DEVICE]: %s, AD evt type %u, Tx Pwr: %i, RSSI %i %s C:%u S:%u "
	       "D:%u SR:%u E:%u Prim: %s, Secn: %s, Interval: 0x%04x (%u ms), "
	       "SID: %u\n",
	       le_addr, info->adv_type, info->tx_power, info->rssi, name,
	       (info->adv_props & BT_GAP_ADV_PROP_CONNECTABLE) != 0,
	       (info->adv_props & BT_GAP_ADV_PROP_SCANNABLE) != 0,
	       (info->adv_props & BT_GAP_ADV_PROP_DIRECTED) != 0,
	       (info->adv_props & BT_GAP_ADV_PROP_SCAN_RESPONSE) != 0,
	       (info->adv_props & BT_GAP_ADV_PROP_EXT_ADV) != 0,
	       phy2str(info->primary_phy), phy2str(info->secondary_phy),
	       info->interval, adv_interval_to_ms(info->interval), info->sid);

	 if (info->interval == 0U) {
        return;
    }

    k_mutex_lock(&beacon_mutex, K_FOREVER);
    struct beacon_entry *entry = beacon_find_locked(info->addr, info->sid);
    if (entry == NULL) {
        entry = beacon_alloc_locked();
        if (entry != NULL) {
            bt_addr_le_copy(&entry->addr, info->addr);
            entry->sid = info->sid;
        }
    }

    if (entry != NULL) {
        entry->interval = info->interval;
        entry->timeout = sync_create_timeout_get(info->interval);

        /* Store the last seen name, if present */
        if (name[0] != '\0') {
            strncpy(entry->name, name, sizeof(entry->name));
            entry->name[sizeof(entry->name) - 1] = '\0';
        }

        if (!entry->sync && !entry->sync_pending) {
            entry->sync_pending = true;
            entry->cte_enabled = false;
            slot_idx = beacon_index(entry);
            start_sync = true;
        }
    }
    k_mutex_unlock(&beacon_mutex);

	if (entry == NULL) {
		printk("No available slots for periodic advertiser %s (SID %u)\n",
		       le_addr, info->sid);
	} else if (start_sync && slot_idx >= 0) {
		beacon_sync_create((size_t)slot_idx);
	}
}

static int scan_init(void)
{
	printk("Scan callbacks register...");
	bt_le_scan_cb_register(&scan_callbacks);
	printk("success.\n");

	printk("Periodic Advertising callbacks register...");
	bt_le_per_adv_sync_cb_register(&sync_callbacks);
	printk("success.\n");

	return 0;
}

static int scan_enable(void)
{
	struct bt_le_scan_param param = {
			.type       = BT_LE_SCAN_TYPE_ACTIVE,
			.options    = BT_LE_SCAN_OPT_FILTER_DUPLICATE,
			.interval   = BT_GAP_SCAN_FAST_INTERVAL,
			.window     = BT_GAP_SCAN_FAST_WINDOW,
			.timeout    = 0U, };
	int err;

	if (!scan_enabled) {
		printk("Start scanning...");
		err = bt_le_scan_start(&param, NULL);
		if (err != 0) {
			printk("failed (err %d)\n", err);
			return err;
		}
		printk("success\n");
		scan_enabled = true;
	}

	return 0;
}

int main(void)
{
	LOG_INF("app: boot");

	int err;

	printk("Starting Connectionless Locator Demo\n");

	printk("Waiting for network interface (W5500)...");
	err = system_state_wait_for_network(K_FOREVER);
	if (err != 0) {
		printk("failed (err %d)\n", err);
		return err;
	}
	printk("ready\n");

	printk("Waiting for MQTT connection...");
	err = system_state_wait_for_mqtt(K_FOREVER);
	if (err != 0) {
		printk("failed (err %d)\n", err);
		return err;
	}
	printk("ready\n");

	printk("Bluetooth initialization...");
	err = bt_enable(NULL);
	if (err != 0) {
		printk("failed (err %d)\n", err);
		return err;
	}
	printk("success\n");

	err = scan_init();
	if (err != 0) {
		printk("Scan init failed (err %d)\n", err);
		return err;
	}

	err = scan_enable();
	if (err != 0) {
		return err;
	}

	while (true) {
		k_sleep(K_SECONDS(60));
	}

	return 0;
}



/* Old Code for publishing test messages periodically
static void publish_loop(void)
{
	struct payload msg;
	int err;

	while (true) {
		strcpy(msg.string, "hello from nRF5340");
		err = zbus_chan_pub(&PAYLOAD_CHAN, &msg, K_MSEC(100));
		if (err) {
			LOG_WRN("Failed to queue payload on PAYLOAD_CHAN, err %d", err);
		} else {
			LOG_INF("Queued payload for publish: %s", msg.string);
		}

		k_sleep(K_SECONDS(10));
	}
}

void main(void)
{
	LOG_INF("app: boot");

	//Give the transport threads time to bring up MQTT before publishing
	k_sleep(K_SECONDS(5));
	publish_loop();
}
*/
