/* INCLUDES */
#include <stdint.h>
#include <stdbool.h>
#include <stddef.h>
#include <string.h>
#include <errno.h>

#include <zephyr/kernel.h>
#include <zephyr/device.h>
#include <zephyr/devicetree.h>
#include <zephyr/drivers/adc.h>
#include <zephyr/drivers/gpio.h>
#include <zephyr/sys/atomic.h>
#include <zephyr/sys/byteorder.h>
#include <zephyr/spinlock.h>

#include <zephyr/bluetooth/bluetooth.h>
#include <zephyr/bluetooth/conn.h>
#include <zephyr/bluetooth/uuid.h>
#include <zephyr/bluetooth/hci.h>
#include <zephyr/bluetooth/gatt.h>

#include "device_data_model.h"

#ifdef CONFIG_TEST_FILL_FLASH
#include <math.h>
#ifndef M_PI
#define M_PI 3.14159265358979323846f
#endif
#endif

#ifdef CONFIG_LOG
#include <zephyr/logging/log.h>
LOG_MODULE_REGISTER(main, CONFIG_LOG_DEFAULT_LEVEL);
#endif

int 	 led_init(void);
void	 led_on(void);
void	 led_off(void);
int      data_storage_init(void);
int      calibration_load(struct calibration_params *out);
int      calibration_save(const struct calibration_params *p);
int      session_start(void);
void     session_stop(void);
void     session_recover(void);
uint16_t session_get_record_count(void);
bool     session_is_active(void);
int      measurement_put(const struct measurement_payload *m);
int      stream_begin(uint8_t session_id);
int      stream_next(struct measurement_payload *out);
bool     stream_has_more(void);
void     erase_all(void);

/* DEFINES */
#define DEVICE_NAME             CONFIG_BT_DEVICE_NAME
#define DEVICE_NAME_LEN         (sizeof(DEVICE_NAME) - 1)

#define SAMPLE_THREAD_STACK     2048
#define SAMPLE_THREAD_PRIO      1

#define SEND_WQ_STACK           2048
#define SEND_WQ_PRIO            5

#define NOTIFY_THREAD_STACK     1024
#define NOTIFY_THREAD_PRIO      6

#define XFER_BUF_SIZE           240
#define NO_DATA_MSG             "No data"

#define BT_UUID_ADC_SRV_VAL \
	BT_UUID_128_ENCODE(0xCDAB0000, 0x1234, 0x5678, 0x1234, 0x56789ABCDEF0)
#define BT_UUID_ADC_MEAS_VAL \
	BT_UUID_128_ENCODE(0xCDAB0001, 0x1234, 0x5678, 0x1234, 0x56789ABCDEF1)
#define BT_UUID_ADC_CFG_VAL \
	BT_UUID_128_ENCODE(0xCDAB0002, 0x1234, 0x5678, 0x1234, 0x56789ABCDEF2)
#define BT_UUID_ADC_CAL_VAL \
	BT_UUID_128_ENCODE(0xCDAB0003, 0x1234, 0x5678, 0x1234, 0x56789ABCDEF3)

#define BT_UUID_ADC_SRV  BT_UUID_DECLARE_128(BT_UUID_ADC_SRV_VAL)
#define BT_UUID_ADC_MEAS BT_UUID_DECLARE_128(BT_UUID_ADC_MEAS_VAL)
#define BT_UUID_ADC_CFG  BT_UUID_DECLARE_128(BT_UUID_ADC_CFG_VAL)
#define BT_UUID_ADC_CAL  BT_UUID_DECLARE_128(BT_UUID_ADC_CAL_VAL)

#define BT_UUID_HIST_SRV_VAL \
	BT_UUID_128_ENCODE(0xc57f2b4f, 0xe71a, 0x4b77, 0x8698, 0x9b652b150abd)
#define BT_UUID_HIST_REQ_VAL \
	BT_UUID_128_ENCODE(0xc57f2b50, 0xe71a, 0x4b77, 0x8698, 0x9b652b150abd)
#define BT_UUID_HIST_STREAM_VAL \
	BT_UUID_128_ENCODE(0xc57f2b51, 0xe71a, 0x4b77, 0x8698, 0x9b652b150abd)

#define BT_UUID_HIST_SRV    BT_UUID_DECLARE_128(BT_UUID_HIST_SRV_VAL)
#define BT_UUID_HIST_REQ    BT_UUID_DECLARE_128(BT_UUID_HIST_REQ_VAL)
#define BT_UUID_HIST_STREAM BT_UUID_DECLARE_128(BT_UUID_HIST_STREAM_VAL)

#define HIST_CMD_DOWNLOAD       0x0001
#define HIST_CMD_ERASE_ALL      0xFFFF

/* DEVICE TREE BINDINGS */
#if !DT_NODE_EXISTS(DT_PATH(zephyr_user)) || \
    !DT_NODE_HAS_PROP(DT_PATH(zephyr_user), io_channels)
#error "No suitable devicetree overlay specified"
#endif

static const struct adc_dt_spec adc_channel =
	ADC_DT_SPEC_GET(DT_PATH(zephyr_user));
static const struct gpio_dt_spec io_res =
	GPIO_DT_SPEC_GET_OR(DT_NODELABEL(vcc_enable), gpios, {0});
static const struct gpio_dt_spec led =
	GPIO_DT_SPEC_GET(DT_ALIAS(led1), gpios);
/* Driver init runs via Zephyr SYS_INIT and parks the chip in deep-sleep
 * for battery savings; we only ensure init succeeded. */
static const struct device *bma530_dev =
	DEVICE_DT_GET(DT_NODELABEL(bma530));

/* MODULE STATE */
static atomic_t app_mode        = ATOMIC_INIT(APP_MODE_IDLE);
static atomic_t notify_enabled  = ATOMIC_INIT(0);
static atomic_t connected       = ATOMIC_INIT(0);

/* deep_stealth: written and read only on the BT host thread (config
 * write callback writes; disconnected callback reads). Plain bool. */
static bool deep_stealth;

static uint32_t send_interval_ms = DEFAULT_SEND_INTERVAL_MS;
static uint32_t acq_interval_ms  = DEFAULT_ACQUISITION_INTERVAL_MS;

static int64_t  start_time_ms;
static int64_t  last_send_time_ms;

static bool     rec_first_sample    = true;
static uint16_t rec_last_stored_val;
static uint16_t rec_last_stored_ts;

static struct measurement_payload last_payload;

static atomic_t notify_payload_packed;
static K_SEM_DEFINE(notify_sem, 0, 1);


int led_init(void)
{
	int ret;

	if (!gpio_is_ready_dt(&led)) {
		return -ENODEV;
	}

	ret = gpio_pin_configure_dt(&led, GPIO_OUTPUT_ACTIVE);
	return ret;
}

void led_on(void)
{
	(void)gpio_pin_set_dt(&led, 0);
}

void led_off(void)
{
	(void)gpio_pin_set_dt(&led, 1);
}

/* CALIBRATION */
static struct calibration_params calibration;
static K_MUTEX_DEFINE(calib_mtx);

static int calibration_update(uint16_t a, uint16_t c, uint16_t b, uint16_t d)
{
	if (a == b) {
		return -EINVAL;
	}

	float slope  = (float)(d - c) / (float)(b - a);
	float offset = (float)c - ((float)a * slope);

	struct calibration_params snap;

	/* K_FOREVER per project rule — mutex timeouts have caused silent
	 * lock failures in the past. */
	k_mutex_lock(&calib_mtx, K_FOREVER);
	calibration.raw_a    = a;
	calibration.real_c   = c;
	calibration.raw_b    = b;
	calibration.real_d   = d;
	calibration.slope    = slope;
	calibration.offset   = offset;
	calibration.is_valid = true;
	snap = calibration;
	k_mutex_unlock(&calib_mtx);

	return calibration_save(&snap);
}

static uint16_t apply_calibration(uint16_t raw)
{
	uint16_t out = raw;

	k_mutex_lock(&calib_mtx, K_FOREVER);
	if (calibration.is_valid) {
		float val = (float)raw * calibration.slope + calibration.offset;
		if (val < 0.0f)     val = 0.0f;
		if (val > 65535.0f) val = 65535.0f;
		out = (uint16_t)val;
	}
	k_mutex_unlock(&calib_mtx);

	return out;
}

static bool value_changed_enough(uint16_t reference, uint16_t current)
{
	int delta = (int)(reference * STORAGE_CHANGE_PCT / 100);
	int lo    = (int)reference - delta;
	int hi    = (int)reference + delta;
	return ((int)current < lo) || ((int)current > hi);
}

/* ADC SAMPLING */
/* Both must be file-static: the SAADC IRQ may fire after k_poll times
 * out and would write into a dead stack frame otherwise. */
static struct k_poll_signal adc_sig = K_POLL_SIGNAL_INITIALIZER(adc_sig);
static int16_t              adc_raw;

static int adc_module_init(void)
{
	if (io_res.port) {
		if (!gpio_is_ready_dt(&io_res)) {
			return -ENODEV;
		}
		int rc = gpio_pin_configure_dt(&io_res, GPIO_OUTPUT_INACTIVE);
		if (rc < 0) {
			return rc;
		}
	}

	if (!device_is_ready(adc_channel.dev)) {
		return -ENODEV;
	}

	return adc_channel_setup_dt(&adc_channel);
}

static int adc_sample_once(uint16_t *result)
{
	struct adc_sequence sequence = {
		.buffer      = &adc_raw,
		.buffer_size = sizeof(adc_raw),
	};
	struct k_poll_event events[] = {
		K_POLL_EVENT_INITIALIZER(K_POLL_TYPE_SIGNAL,
					 K_POLL_MODE_NOTIFY_ONLY,
					 &adc_sig),
	};

	int err = adc_sequence_init_dt(&adc_channel, &sequence);
	if (err < 0) {
		return err;
	}

	k_poll_signal_reset(&adc_sig);

	err = adc_read_async(adc_channel.dev, &sequence, &adc_sig);
	if (err < 0) {
		return err;
	}

	err = k_poll(events, ARRAY_SIZE(events), K_MSEC(ADC_READ_TIMEOUT_MS));
	if (err < 0) {
		return -ETIMEDOUT;
	}

	*result = (adc_raw < 0) ? 0 : (uint16_t)adc_raw;
	return 0;
}

static int adc_sample_average(uint16_t *average)
{
	uint32_t sum = 0;

	if (io_res.port) {
		gpio_pin_set_dt(&io_res, 1);
	}

	for (int i = 0; i < ADC_SAMPLE_COUNT; i++) {
		uint16_t sample = 0;
		int err = adc_sample_once(&sample);
		if (err < 0) {
			if (io_res.port) {
				gpio_pin_set_dt(&io_res, 0);
			}
			return err;
		}
		sum += sample;
	}

	if (io_res.port) {
		gpio_pin_set_dt(&io_res, 0);
	}

	*average = (uint16_t)(sum / ADC_SAMPLE_COUNT);
	return 0;
}

/* MEASUREMENT SNAPSHOT (atomic publish for GATT reads) */
static atomic_t meas_snapshot;

static void publish_measurement(uint16_t timestamp_s, uint16_t value)
{
	uint32_t packed = ((uint32_t)value << 16) | (uint32_t)timestamp_s;
	atomic_set(&meas_snapshot, (atomic_val_t)packed);
}

static void load_measurement(struct measurement_payload *out)
{
	uint32_t packed = (uint32_t)atomic_get(&meas_snapshot);
	out->timestamp_s = (uint16_t)(packed & 0xFFFFU);
	out->value       = (uint16_t)(packed >> 16);
}

/* GATT HANDLERS — ADC SERVICE */
static struct bt_conn *current_conn;
/* K_SPINLOCK chosen over a third mutex: current_conn access is just
 * a pointer load + bt_conn_ref, no blocking ops, and the budget is
 * two mutexes (calib_mtx + nvs_mtx). */
static struct k_spinlock conn_lock;

static struct bt_conn *conn_ref_snapshot(void)
{
	struct bt_conn *snap = NULL;
	k_spinlock_key_t key = k_spin_lock(&conn_lock);
	if (current_conn) {
		snap = bt_conn_ref(current_conn);
	}
	k_spin_unlock(&conn_lock, key);
	return snap;
}

static void start_sampling(uint8_t mode, uint32_t send_ms, uint32_t acq_ms);
static void stop_sampling(void);

static ssize_t read_measurement(struct bt_conn *conn,
				const struct bt_gatt_attr *attr,
				void *buf, uint16_t len, uint16_t offset)
{
	struct measurement_payload payload;
	load_measurement(&payload);
	return bt_gatt_attr_read(conn, attr, buf, len, offset,
				 &payload, sizeof(payload));
}

static ssize_t read_config(struct bt_conn *conn,
			   const struct bt_gatt_attr *attr,
			   void *buf, uint16_t len, uint16_t offset)
{
	uint8_t data[5];
	data[0] = (uint8_t)atomic_get(&app_mode);
	sys_put_le16((uint16_t)(send_interval_ms / 1000U), &data[1]);
	sys_put_le16((uint16_t)(acq_interval_ms  / 1000U), &data[3]);
	return bt_gatt_attr_read(conn, attr, buf, len, offset,
				 data, sizeof(data));
}

static ssize_t write_config(struct bt_conn *conn,
			    const struct bt_gatt_attr *attr,
			    const void *buf, uint16_t len,
			    uint16_t offset, uint8_t flags)
{
	if (offset != 0) {
		return BT_GATT_ERR(BT_ATT_ERR_INVALID_OFFSET);
	}
	if (len != 5) {
		return BT_GATT_ERR(BT_ATT_ERR_INVALID_ATTRIBUTE_LEN);
	}

	const uint8_t *data = buf;
	uint8_t  command = data[0];
	uint16_t s_s     = sys_get_le16(&data[1]);
	uint16_t a_s     = sys_get_le16(&data[3]);

	uint32_t s_ms = (uint32_t)s_s * 1000U;
	uint32_t a_ms = (uint32_t)a_s * 1000U;

	if (s_ms < MIN_SEND_INTERVAL_MS) s_ms = MIN_SEND_INTERVAL_MS;
	if (a_ms < MIN_ACQ_INTERVAL_MS)  a_ms = MIN_ACQ_INTERVAL_MS;

	switch (command) {
	case APP_MODE_IDLE:
		deep_stealth = false;
		stop_sampling();
		break;
	case APP_MODE_NOTIFY:
		deep_stealth = false;
		start_sampling(APP_MODE_NOTIFY, s_ms, a_ms);
		break;
	case APP_MODE_RECORD:
		/* Deep stealth: start RECORD, disconnect peer, then the
		 * disconnect callback defers bt_disable() to the system
		 * workqueue (cannot run from BT host thread). Stack stays
		 * off until power-cycle. */
		deep_stealth = true;
		start_sampling(APP_MODE_RECORD, s_ms, a_ms);
		{
			struct bt_conn *c = conn_ref_snapshot();
			if (c) {
				bt_conn_disconnect(c, BT_HCI_ERR_REMOTE_USER_TERM_CONN);
				bt_conn_unref(c);
			}
		}
		break;
	default:
		return BT_GATT_ERR(BT_ATT_ERR_VALUE_NOT_ALLOWED);
	}

	return len;
}

static ssize_t read_calibration(struct bt_conn *conn,
				const struct bt_gatt_attr *attr,
				void *buf, uint16_t len, uint16_t offset)
{
	uint16_t data[4];

	k_mutex_lock(&calib_mtx, K_FOREVER);
	data[0] = sys_cpu_to_le16(calibration.raw_a);
	data[1] = sys_cpu_to_le16(calibration.real_c);
	data[2] = sys_cpu_to_le16(calibration.raw_b);
	data[3] = sys_cpu_to_le16(calibration.real_d);
	k_mutex_unlock(&calib_mtx);

	return bt_gatt_attr_read(conn, attr, buf, len, offset,
				 data, sizeof(data));
}

static ssize_t write_calibration(struct bt_conn *conn,
				 const struct bt_gatt_attr *attr,
				 const void *buf, uint16_t len,
				 uint16_t offset, uint8_t flags)
{
	if (offset != 0) {
		return BT_GATT_ERR(BT_ATT_ERR_INVALID_OFFSET);
	}
	if (len != 8) {
		return BT_GATT_ERR(BT_ATT_ERR_INVALID_ATTRIBUTE_LEN);
	}

	const uint8_t *data = buf;
	uint16_t a = sys_get_le16(&data[0]);
	uint16_t c = sys_get_le16(&data[2]);
	uint16_t b = sys_get_le16(&data[4]);
	uint16_t d = sys_get_le16(&data[6]);

	if (calibration_update(a, c, b, d) < 0) {
		return BT_GATT_ERR(BT_ATT_ERR_VALUE_NOT_ALLOWED);
	}

	return len;
}

static void adc_ccc_cfg_changed(const struct bt_gatt_attr *attr, uint16_t value)
{
	atomic_set(&notify_enabled, (value == BT_GATT_CCC_NOTIFY) ? 1 : 0);
}

/* GATT HANDLERS — HISTORY SERVICE */
static struct k_work_q send_wq;
static K_THREAD_STACK_DEFINE(send_wq_stack, SEND_WQ_STACK);
static struct k_work send_work;
/* xfer_buf is single-writer: only send_work_handler fills it, and a
 * second submission of send_work is a no-op while one is pending. */
static uint8_t xfer_buf[XFER_BUF_SIZE];

static ssize_t hist_read_req(struct bt_conn *conn,
			     const struct bt_gatt_attr *attr,
			     void *buf, uint16_t len, uint16_t offset)
{
	uint8_t resp[5];
	resp[0] = FIXED_SESSION_ID;
	sys_put_le32((uint32_t)session_get_record_count(), &resp[1]);
	return bt_gatt_attr_read(conn, attr, buf, len, offset,
				 resp, sizeof(resp));
}

static ssize_t hist_write_req(struct bt_conn *conn,
			      const struct bt_gatt_attr *attr,
			      const void *buf, uint16_t len,
			      uint16_t offset, uint8_t flags)
{
	if (offset != 0) {
		return BT_GATT_ERR(BT_ATT_ERR_INVALID_OFFSET);
	}
	if (len != sizeof(uint16_t)) {
		return BT_GATT_ERR(BT_ATT_ERR_INVALID_ATTRIBUTE_LEN);
	}

	uint16_t cmd = sys_get_le16(buf);

	if (cmd == HIST_CMD_ERASE_ALL) {
		erase_all();
		return len;
	}

	if (cmd != HIST_CMD_DOWNLOAD) {
		return BT_GATT_ERR(BT_ATT_ERR_VALUE_NOT_ALLOWED);
	}

	if (stream_begin(FIXED_SESSION_ID) < 0) {
		return BT_GATT_ERR(BT_ATT_ERR_VALUE_NOT_ALLOWED);
	}

	k_work_submit_to_queue(&send_wq, &send_work);
	return len;
}

static void hist_ccc_cfg_changed(const struct bt_gatt_attr *attr, uint16_t value)
{
	ARG_UNUSED(attr);
	ARG_UNUSED(value);
}

/* GATT SERVICE DEFINITIONS (BT_GATT_SERVICE_DEFINE for both services) */
BT_GATT_SERVICE_DEFINE(adc_svc,
	BT_GATT_PRIMARY_SERVICE(BT_UUID_ADC_SRV),

	BT_GATT_CHARACTERISTIC(BT_UUID_ADC_MEAS,
			       BT_GATT_CHRC_READ | BT_GATT_CHRC_NOTIFY,
			       BT_GATT_PERM_READ,
			       read_measurement, NULL, NULL),
	BT_GATT_CCC(adc_ccc_cfg_changed,
		    BT_GATT_PERM_READ | BT_GATT_PERM_WRITE),

	BT_GATT_CHARACTERISTIC(BT_UUID_ADC_CFG,
			       BT_GATT_CHRC_READ | BT_GATT_CHRC_WRITE,
			       BT_GATT_PERM_READ | BT_GATT_PERM_WRITE,
			       read_config, write_config, NULL),

	BT_GATT_CHARACTERISTIC(BT_UUID_ADC_CAL,
			       BT_GATT_CHRC_READ | BT_GATT_CHRC_WRITE,
			       BT_GATT_PERM_READ | BT_GATT_PERM_WRITE,
			       read_calibration, write_calibration, NULL),
);

BT_GATT_SERVICE_DEFINE(hist_svc,
	BT_GATT_PRIMARY_SERVICE(BT_UUID_HIST_SRV),

	BT_GATT_CHARACTERISTIC(BT_UUID_HIST_REQ,
			       BT_GATT_CHRC_READ | BT_GATT_CHRC_WRITE,
			       BT_GATT_PERM_READ | BT_GATT_PERM_WRITE,
			       hist_read_req, hist_write_req, NULL),

	BT_GATT_CHARACTERISTIC(BT_UUID_HIST_STREAM, BT_GATT_CHRC_NOTIFY,
			       BT_GATT_PERM_READ, NULL, NULL, NULL),
	BT_GATT_CCC(hist_ccc_cfg_changed,
		    BT_GATT_PERM_READ | BT_GATT_PERM_WRITE),
);

static const struct bt_gatt_attr *meas_attr   = &adc_svc.attrs[2];
static const struct bt_gatt_attr *stream_attr = &hist_svc.attrs[4];

/* ADVERTISING + CONN CALLBACKS */
static const struct bt_data ad[] = {
	BT_DATA_BYTES(BT_DATA_FLAGS, (BT_LE_AD_GENERAL | BT_LE_AD_NO_BREDR)),
	BT_DATA(BT_DATA_NAME_COMPLETE, DEVICE_NAME, DEVICE_NAME_LEN),
};

static const struct bt_le_adv_param *adv_params = BT_LE_ADV_PARAM(
	(BT_LE_ADV_OPT_CONN | BT_LE_ADV_OPT_USE_IDENTITY),
	BT_GAP_ADV_FAST_INT_MIN_2,
	BT_GAP_ADV_FAST_INT_MAX_2,
	NULL);

static struct k_work adv_work;
static struct k_work bt_disable_work;

static void adv_work_handler(struct k_work *work)
{
	ARG_UNUSED(work);
	int err = bt_le_adv_start(adv_params, ad, ARRAY_SIZE(ad), NULL, 0);
	if (err < 0) {
#ifdef CONFIG_LOG
		LOG_WRN("Adv start failed (%d)", err);
#endif
	}
}

/* bt_disable() must not run on the BT host thread; defer to sysworkq. */
static void bt_disable_work_handler(struct k_work *work)
{
	ARG_UNUSED(work);
	bt_disable();
}

static void connected_cb(struct bt_conn *conn, uint8_t err)
{
	if (err) {
		return;
	}
	current_conn = bt_conn_ref(conn);
	atomic_set(&connected, 1);
}

static void disconnected_cb(struct bt_conn *conn, uint8_t reason)
{
	ARG_UNUSED(conn);
	ARG_UNUSED(reason);

	atomic_set(&connected, 0);

	struct bt_conn *to_unref = current_conn;
	current_conn = NULL;

	if (to_unref) {
		bt_conn_unref(to_unref);
	}

	if (deep_stealth) {
		k_work_submit(&bt_disable_work);
	} else {
		k_work_submit(&adv_work);
	}
}

static void recycled_cb(void)
{
	if (!deep_stealth) {
		k_work_submit(&adv_work);
	}
}

BT_CONN_CB_DEFINE(conn_callbacks) = {
	.connected    = connected_cb,
	.disconnected = disconnected_cb,
	.recycled     = recycled_cb,
};

static void send_work_handler(struct k_work *work)
{
	ARG_UNUSED(work);

	struct bt_conn *conn = conn_ref_snapshot();
	if (!conn) {
		return;
	}

	uint16_t mtu     = bt_gatt_get_mtu(conn);
	uint16_t max_len = (mtu > 3) ? (mtu - 3) : 20;
	if (max_len > XFER_BUF_SIZE) {
		max_len = XFER_BUF_SIZE;
	}

	bool wrote_any = false;

	while (stream_has_more()) {
		int pos = 0;
		struct measurement_payload item;

		while ((pos + (int)sizeof(item)) <= max_len &&
		       stream_has_more() &&
		       stream_next(&item) == 0) {
			memcpy(xfer_buf + pos, &item, sizeof(item));
			pos += (int)sizeof(item);
		}

		if (pos == 0) {
			break;
		}

		int rc;
		do {
			rc = bt_gatt_notify(conn, stream_attr, xfer_buf, pos);
			if (rc == -ENOMEM) {
				led_on();
				k_sleep(K_MSEC(2));
			}
		} while (rc == -ENOMEM);

		if (rc < 0) {
			goto out;
		}
		wrote_any = true;
	}

	if (!wrote_any) {
		(void)bt_gatt_notify(conn, stream_attr,
				     NO_DATA_MSG, sizeof(NO_DATA_MSG) - 1);
	}

out:
	bt_conn_unref(conn);
}

/* SAMPLER THREAD */
static struct k_timer sample_timer;
static K_SEM_DEFINE(sample_sem, 0, 1);
static K_THREAD_STACK_DEFINE(sample_stack, SAMPLE_THREAD_STACK);
static struct k_thread sample_thread_data;

static void sample_timer_cb(struct k_timer *t)
{
	ARG_UNUSED(t);
	k_sem_give(&sample_sem);
	led_on();
}

/* Publishes the latest measurement for the notify thread to send.
 * Never calls bt_gatt_notify itself: that call can block K_FOREVER on
 * ATT TX buffer exhaustion if the central stops acking, and we must
 * not let it wedge the sampler. Same path is used in NOTIFY (0x01)
 * and RECORD (0x02) — in 0x02 the BLE stack is disabled, so the notify
 * thread early-returns on conn_ref_snapshot()==NULL. */
static void ble_notify_measurement(uint16_t timestamp_s, uint16_t value)
{
	if (!atomic_get(&notify_enabled)) {
		return;
	}

	uint32_t packed = ((uint32_t)value << 16) | (uint32_t)timestamp_s;
	atomic_set(&notify_payload_packed, (atomic_val_t)packed);
	k_sem_give(&notify_sem);
}

static void sample_thread_fn(void *p1, void *p2, void *p3)
{
	ARG_UNUSED(p1); ARG_UNUSED(p2); ARG_UNUSED(p3);

	while (true) {
		k_sem_take(&sample_sem, K_FOREVER);
		led_off();

		enum app_mode mode = (enum app_mode)atomic_get(&app_mode);
		if (mode == APP_MODE_IDLE) {
			continue;
		}

		uint16_t value = 0;
		if (adc_sample_average(&value) < 0) {
			continue;
		}

		value = apply_calibration(value);

		/* post-calibration values above SENSOR_OFF_VALUE
		 * indicate the sensor is physically disconnected. */
		if (value > SENSOR_OFF_VALUE) {
			value = 0;
		}

		int64_t  now         = k_uptime_get();
		uint32_t elapsed_ms  = (uint32_t)(now - start_time_ms);
		uint16_t timestamp_s = (uint16_t)((elapsed_ms / 1000U) + 1U);

		/* Auto-stop guard: timestamp_s is uint16_t (~18.2h). */
		if (elapsed_ms >= (AUTO_STOP_LIMIT_S * 1000U)) {
			stop_sampling();
			continue;
		}

		publish_measurement(timestamp_s, value);
		last_payload.timestamp_s = timestamp_s;
		last_payload.value       = value;

		uint32_t s_ms = send_interval_ms;
		if ((now - last_send_time_ms) >= (int64_t)s_ms) {
			ble_notify_measurement(timestamp_s, value);
			last_send_time_ms = now;
		}

		if (mode == APP_MODE_RECORD) {
			bool should_store = false;

			if (rec_first_sample) {
				should_store = true;
				rec_first_sample = false;
			} else if (value_changed_enough(rec_last_stored_val, value)) {
				should_store = true;
			} else if ((uint16_t)(timestamp_s - rec_last_stored_ts)
				   >= STORAGE_HEARTBEAT_S) {
				should_store = true;
			}

			if (should_store) {
				struct measurement_payload rec = {
					.timestamp_s = timestamp_s,
					.value       = value,
				};
				if (measurement_put(&rec) == 0) {
					rec_last_stored_val = value;
					rec_last_stored_ts  = timestamp_s;
				}
			}
		}
	}
}

/* NOTIFY THREAD — owns bt_gatt_notify so the sampler never blocks on it.
 * If ATT TX buffers are exhausted, bt_gatt_notify can wait K_FOREVER
 * inside the host. Isolating it here means the worst case is "live
 * notifications pause"; sampling, recording and the timer keep going. */
static K_THREAD_STACK_DEFINE(notify_stack, NOTIFY_THREAD_STACK);
static struct k_thread notify_thread_data;

static void notify_thread_fn(void *p1, void *p2, void *p3)
{
	ARG_UNUSED(p1); ARG_UNUSED(p2); ARG_UNUSED(p3);

	while (true) {
		k_sem_take(&notify_sem, K_FOREVER);

		if (!atomic_get(&notify_enabled)) {
			continue;
		}

		struct bt_conn *conn = conn_ref_snapshot();
		if (!conn) {
			continue;
		}

		uint32_t packed = (uint32_t)atomic_get(&notify_payload_packed);
		struct measurement_payload payload = {
			.timestamp_s = (uint16_t)(packed & 0xFFFFU),
			.value       = (uint16_t)(packed >> 16),
		};

		(void)bt_gatt_notify(conn, meas_attr, &payload, sizeof(payload));
		bt_conn_unref(conn);
	}
}

/* MODE TRANSITIONS */
#ifdef CONFIG_TEST_FILL_FLASH
static void test_fill_thread_fn(void *p1, void *p2, void *p3);
static K_SEM_DEFINE(test_fill_sem, 0, 1);
static K_THREAD_STACK_DEFINE(test_fill_stack, 2048);
static struct k_thread test_fill_thread_data;
#endif

static void start_sampling(uint8_t mode, uint32_t send_ms, uint32_t acq_ms)
{
	send_interval_ms  = send_ms;
	acq_interval_ms   = acq_ms;
	start_time_ms     = k_uptime_get();
	last_send_time_ms = 0;

	rec_first_sample    = true;
	rec_last_stored_val = 0;
	rec_last_stored_ts  = 0;

	if (mode == APP_MODE_RECORD) {
		(void)session_start();
	}

	atomic_set(&app_mode, mode);

#ifdef CONFIG_TEST_FILL_FLASH
	if (mode == APP_MODE_RECORD) {
		k_sem_give(&test_fill_sem);
		return;
	}
#endif

	k_timer_start(&sample_timer, K_MSEC(acq_ms), K_MSEC(acq_ms));
}

static void stop_sampling(void)
{
	k_timer_stop(&sample_timer);

	enum app_mode prev = (enum app_mode)atomic_set(&app_mode, APP_MODE_IDLE);

	if (prev == APP_MODE_RECORD && !rec_first_sample) {
		(void)measurement_put(&last_payload);
		session_stop();
	} else if (prev == APP_MODE_RECORD) {
		session_stop();
	}
}

/* TEST FILL FLASH (ifdef CONFIG_TEST_FILL_FLASH) — simplified */
#ifdef CONFIG_TEST_FILL_FLASH
static void test_fill_thread_fn(void *p1, void *p2, void *p3)
{
	ARG_UNUSED(p1); ARG_UNUSED(p2); ARG_UNUSED(p3);

	while (true) {
		k_sem_take(&test_fill_sem, K_FOREVER);

		int64_t  t0    = k_uptime_get();
		uint32_t count = 0;
		uint16_t ts    = 1;

		while (atomic_get(&app_mode) == APP_MODE_RECORD) {
			float angle = (float)ts * 2.0f * M_PI / 100.0f;
			float v     = 70.0f + 10.0f * sinf(angle);

			struct measurement_payload rec = {
				.timestamp_s = ts,
				.value       = (uint16_t)v,
			};

			int rc = measurement_put(&rec);
			if (rc == -ENOSPC || rc < 0) {
				break;
			}

			count++;
			ts++;
			if (ts == 0) {
				ts = 1;
			}
		}

		int64_t elapsed_ms = k_uptime_get() - t0;

		atomic_set(&app_mode, APP_MODE_IDLE);
		session_stop();

#ifdef CONFIG_LOG
		LOG_INF("TEST: fill-flash done — %u records in %lld ms",
			count, elapsed_ms);
#endif
	}
}
#endif /* CONFIG_TEST_FILL_FLASH */

/* MAIN */
int main(void)
{
	int err;

	if (gpio_is_ready_dt(&led)) {
		gpio_pin_configure_dt(&led, GPIO_OUTPUT_ACTIVE);
	}
	err = led_init();
	if (err){
		return err;
	}

	err = adc_module_init();
	if (err) {
		return err;
	}

	(void)device_is_ready(bma530_dev);

	err = data_storage_init();
	if (err) {
		return err;
	}

	(void)calibration_load(&calibration);
	session_recover();

	k_timer_init(&sample_timer, sample_timer_cb, NULL);

	k_thread_create(&sample_thread_data, sample_stack,
			K_THREAD_STACK_SIZEOF(sample_stack),
			sample_thread_fn, NULL, NULL, NULL,
			SAMPLE_THREAD_PRIO, 0, K_NO_WAIT);
	k_thread_name_set(&sample_thread_data, "sampler");

	k_thread_create(&notify_thread_data, notify_stack,
			K_THREAD_STACK_SIZEOF(notify_stack),
			notify_thread_fn, NULL, NULL, NULL,
			NOTIFY_THREAD_PRIO, 0, K_NO_WAIT);
	k_thread_name_set(&notify_thread_data, "notify");

	k_work_init(&adv_work, adv_work_handler);
	k_work_init(&bt_disable_work, bt_disable_work_handler);

	k_work_queue_start(&send_wq, send_wq_stack,
			   K_THREAD_STACK_SIZEOF(send_wq_stack),
			   SEND_WQ_PRIO, NULL);
	k_thread_name_set(&send_wq.thread, "ble_send_wq");
	k_work_init(&send_work, send_work_handler);

	err = bt_enable(NULL);
	if (err) {
		return err;
	}

	k_work_submit(&adv_work);

#ifdef CONFIG_TEST_FILL_FLASH
	k_thread_create(&test_fill_thread_data, test_fill_stack,
			K_THREAD_STACK_SIZEOF(test_fill_stack),
			test_fill_thread_fn, NULL, NULL, NULL,
			SAMPLE_THREAD_PRIO, 0, K_NO_WAIT);
	k_thread_name_set(&test_fill_thread_data, "test_fill");
#endif

	k_yield();
	return 0;
}
