#include "ble.h"
#include <string.h>
#include <stdio.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/event_groups.h"
#include "freertos/queue.h"
#include "nimble/nimble_port.h"
#include "nimble/nimble_port_freertos.h"
#include "host/ble_hs.h"
#include "host/ble_gap.h"
#include "host/util/util.h"
#include "host/ble_store.h"
#include "services/gap/ble_svc_gap.h"
#include "services/gatt/ble_svc_gatt.h"

void ble_store_config_init(void);

#define DEVICE_NAME   "EIS4"
#define CONNECTED_BIT BIT0
#define CMD_QUEUE_LEN 8

/* BLE MIDI Service  03B80E5A-EDE8-4B33-A751-6CE34EC4C700 */
static const ble_uuid128_t midi_svc_uuid = BLE_UUID128_INIT(
    0x00, 0xc7, 0xc4, 0x4e, 0xe3, 0x6c, 0x51, 0xa7,
    0x33, 0x4b, 0xe8, 0xed, 0x5a, 0x0e, 0xb8, 0x03);

/* BLE MIDI Characteristic  7772E5DB-3868-4112-A1A9-F2669D106BF3 */
static const ble_uuid128_t midi_chr_uuid = BLE_UUID128_INIT(
    0xf3, 0x6b, 0x10, 0x9d, 0x66, 0xf2, 0xa9, 0xa1,
    0x12, 0x41, 0x68, 0x38, 0xdb, 0xe5, 0x72, 0x77);

#define MAX_CONNECTIONS 2

static EventGroupHandle_t ble_events;
static QueueHandle_t cmd_queue;
static uint16_t midi_val_hdl;
static uint16_t hid_input_val_hdl;

static struct {
    uint16_t hdl;
    bool midi_notify;
    bool hid_notify;
} conns[MAX_CONNECTIONS];

static int conn_count;

/* ---- HID keyboard report map ---- */

static const uint8_t hid_report_map[] = {
    0x05, 0x01,                         /* Usage Page (Generic Desktop) */
    0x09, 0x06,                         /* Usage (Keyboard) */
    0xA1, 0x01,                         /* Collection (Application) */
    0x85, 0x01,                         /*   Report ID (1) */
    0x05, 0x07,                         /*   Usage Page (Keyboard) */
    0x19, 0xE0, 0x29, 0xE7,            /*   Usage Min/Max (modifiers) */
    0x15, 0x00, 0x25, 0x01,            /*   Logical Min/Max */
    0x75, 0x01, 0x95, 0x08,            /*   8x1-bit modifier keys */
    0x81, 0x02,                         /*   Input (Variable) */
    0x95, 0x01, 0x75, 0x08,            /*   1x8-bit reserved */
    0x81, 0x01,                         /*   Input (Constant) */
    0x05, 0x08,                         /*   Usage Page (LEDs) */
    0x19, 0x01, 0x29, 0x05,            /*   5 LEDs */
    0x95, 0x05, 0x75, 0x01,
    0x91, 0x02,                         /*   Output (Variable) */
    0x95, 0x01, 0x75, 0x03,
    0x91, 0x01,                         /*   Output pad to byte */
    0x05, 0x07,                         /*   Usage Page (Keyboard) */
    0x19, 0x00, 0x29, 0xFF,            /*   Key codes 0-255 */
    0x15, 0x00, 0x26, 0xFF, 0x00,      /*   Logical Min/Max (0-255) */
    0x95, 0x06, 0x75, 0x08,            /*   6x8-bit key array */
    0x81, 0x00,                         /*   Input (Array) */
    0xC0,                               /* End Collection */
};

/* bcdHID=1.11, bCountryCode=0, Flags=NormallyConnectable */
static const uint8_t hid_info[] = { 0x11, 0x01, 0x00, 0x02 };

/* PnP ID: src=USB-IF(2), VID=0x1209(pid.codes), PID=0x0001, ver=0x0100 */
static const uint8_t pnp_id[] = { 0x02, 0x09, 0x12, 0x01, 0x00, 0x00, 0x01 };

/* ---- 7-bit MIDI encoding ---- */

static void encode_float(float val, uint8_t *out)
{
    uint8_t *p = (uint8_t *)&val;
    out[0] = ((p[0] >> 7) & 1) | ((p[1] >> 6) & 2) |
             ((p[2] >> 5) & 4) | ((p[3] >> 4) & 8);
    out[1] = p[0] & 0x7F;
    out[2] = p[1] & 0x7F;
    out[3] = p[2] & 0x7F;
    out[4] = p[3] & 0x7F;
}

static void encode_u16(uint16_t val, uint8_t *out)
{
    uint8_t *p = (uint8_t *)&val;
    out[0] = ((p[0] >> 7) & 1) | ((p[1] >> 6) & 2);
    out[1] = p[0] & 0x7F;
    out[2] = p[1] & 0x7F;
}

static float decode_float(const uint8_t *d)
{
    uint8_t b[4];
    b[0] = d[1] | ((d[0] & 1) << 7);
    b[1] = d[2] | ((d[0] & 2) << 6);
    b[2] = d[3] | ((d[0] & 4) << 5);
    b[3] = d[4] | ((d[0] & 8) << 4);
    float v;
    memcpy(&v, b, 4);
    return v;
}

static uint16_t decode_u16(const uint8_t *d)
{
    uint8_t b[2];
    b[0] = d[1] | ((d[0] & 1) << 7);
    b[1] = d[2] | ((d[0] & 2) << 6);
    uint16_t v;
    memcpy(&v, b, 2);
    return v;
}

/* ---- command parsing from incoming SysEx ---- */

static void parse_one_sysex(const uint8_t *midi, uint16_t mlen)
{
    if (mlen < 3 || midi[0] != 0xF0 || midi[1] != 0x7D) return;

    BleCommand cmd;
    memset(&cmd, 0, sizeof(cmd));
    cmd.type = midi[2];

    switch (cmd.type) {
    case CMD_SET_SWEEP:
        if (mlen < 16) return;
        cmd.sweep.freq_start = decode_float(&midi[3]);
        cmd.sweep.freq_stop  = decode_float(&midi[8]);
        cmd.sweep.ppd        = decode_u16(&midi[13]);
        break;
    case CMD_SET_RTIA:
        if (mlen < 4) return;
        cmd.rtia = midi[3];
        break;
    case CMD_SET_RCAL:
        if (mlen < 4) return;
        cmd.rcal = midi[3];
        break;
    case CMD_SET_ELECTRODE:
        if (mlen < 4) return;
        cmd.electrode = midi[3];
        break;
    case CMD_START_LSV:
        if (mlen < 19) return;
        cmd.lsv.v_start   = decode_float(&midi[3]);
        cmd.lsv.v_stop    = decode_float(&midi[8]);
        cmd.lsv.scan_rate = decode_float(&midi[13]);
        cmd.lsv.lp_rtia   = midi[18];
        break;
    case CMD_START_AMP:
        if (mlen < 19) return;
        cmd.amp.v_hold       = decode_float(&midi[3]);
        cmd.amp.interval_ms  = decode_float(&midi[8]);
        cmd.amp.duration_s   = decode_float(&midi[13]);
        cmd.amp.lp_rtia      = midi[18];
        break;
    case CMD_START_CL:
        if (mlen < 34) return;
        cmd.cl.v_cond    = decode_float(&midi[3]);
        cmd.cl.t_cond_ms = decode_float(&midi[8]);
        cmd.cl.v_free    = decode_float(&midi[13]);
        cmd.cl.v_total   = decode_float(&midi[18]);
        cmd.cl.t_dep_ms  = decode_float(&midi[23]);
        cmd.cl.t_meas_ms = decode_float(&midi[28]);
        cmd.cl.lp_rtia   = midi[33];
        break;

    case CMD_START_PH:
        if (mlen < 8) return;
        cmd.ph.stabilize_s = decode_float(&midi[3]);
        break;
    case CMD_START_CLEAN:
        if (mlen < 13) return;
        cmd.clean.v_mv       = decode_float(&midi[3]);
        cmd.clean.duration_s = decode_float(&midi[8]);
        break;
    case CMD_START_SWEEP:
    case CMD_GET_CONFIG:
    case CMD_STOP_AMP:
    case CMD_GET_TEMP:
    case CMD_START_REFS:
    case CMD_GET_REFS:
    case CMD_CLEAR_REFS:
    case CMD_OPEN_CAL:
    case CMD_CLEAR_OPEN_CAL:
        break;
    default:
        return;
    }

    xQueueSend(cmd_queue, &cmd, 0);
}

static void parse_command(const uint8_t *data, uint16_t len)
{
    if (len < 5) return;

    uint16_t i = 1; /* skip BLE MIDI header byte */
    while (i < len) {
        /* skip timestamp bytes (bit 7 set, not F0/F7) */
        if ((data[i] & 0x80) && data[i] != 0xF0 && data[i] != 0xF7) {
            i++;
            continue;
        }
        if (data[i] == 0xF0) {
            uint8_t clean[64];
            uint16_t clen = 0;
            clean[clen++] = 0xF0;
            i++;
            while (i < len && data[i] != 0xF7) {
                if (data[i] & 0x80) { i++; continue; } /* strip timestamps */
                if (clen < sizeof(clean))
                    clean[clen++] = data[i];
                i++;
            }
            if (i < len) i++; /* skip F7 */
            parse_one_sysex(clean, clen);
        } else {
            i++;
        }
    }
}

/* ---- GATT access callbacks ---- */

static int midi_access_cb(uint16_t ch, uint16_t ah,
                          struct ble_gatt_access_ctxt *ctxt, void *arg)
{
    (void)ch; (void)ah; (void)arg;
    if (ctxt->op == BLE_GATT_ACCESS_OP_WRITE_CHR) {
        uint16_t len = OS_MBUF_PKTLEN(ctxt->om);
        uint8_t buf[64];
        if (len > sizeof(buf)) len = sizeof(buf);
        os_mbuf_copydata(ctxt->om, 0, len, buf);
        parse_command(buf, len);
    }
    return 0;
}

static int hid_report_map_cb(uint16_t ch, uint16_t ah,
                             struct ble_gatt_access_ctxt *ctxt, void *arg)
{
    (void)ch; (void)ah; (void)arg;
    if (ctxt->op == BLE_GATT_ACCESS_OP_READ_CHR)
        os_mbuf_append(ctxt->om, hid_report_map, sizeof(hid_report_map));
    return 0;
}

static int hid_info_cb(uint16_t ch, uint16_t ah,
                       struct ble_gatt_access_ctxt *ctxt, void *arg)
{
    (void)ch; (void)ah; (void)arg;
    if (ctxt->op == BLE_GATT_ACCESS_OP_READ_CHR)
        os_mbuf_append(ctxt->om, hid_info, sizeof(hid_info));
    return 0;
}

static int hid_input_report_cb(uint16_t ch, uint16_t ah,
                               struct ble_gatt_access_ctxt *ctxt, void *arg)
{
    (void)ch; (void)ah; (void)arg;
    if (ctxt->op == BLE_GATT_ACCESS_OP_READ_CHR) {
        static const uint8_t empty[8] = {0};
        os_mbuf_append(ctxt->om, empty, sizeof(empty));
    }
    return 0;
}

static int hid_output_report_cb(uint16_t ch, uint16_t ah,
                                struct ble_gatt_access_ctxt *ctxt, void *arg)
{
    (void)ch; (void)ah; (void)arg;
    return 0;
}

static int hid_input_ref_cb(uint16_t ch, uint16_t ah,
                            struct ble_gatt_access_ctxt *ctxt, void *arg)
{
    (void)ch; (void)ah; (void)arg;
    if (ctxt->op == BLE_GATT_ACCESS_OP_READ_DSC) {
        static const uint8_t ref[] = { 0x01, 0x01 }; /* Report ID 1, Input */
        os_mbuf_append(ctxt->om, ref, sizeof(ref));
    }
    return 0;
}

static int hid_output_ref_cb(uint16_t ch, uint16_t ah,
                             struct ble_gatt_access_ctxt *ctxt, void *arg)
{
    (void)ch; (void)ah; (void)arg;
    if (ctxt->op == BLE_GATT_ACCESS_OP_READ_DSC) {
        static const uint8_t ref[] = { 0x01, 0x02 }; /* Report ID 1, Output */
        os_mbuf_append(ctxt->om, ref, sizeof(ref));
    }
    return 0;
}

static int hid_boot_input_cb(uint16_t ch, uint16_t ah,
                            struct ble_gatt_access_ctxt *ctxt, void *arg)
{
    (void)ch; (void)ah; (void)arg;
    if (ctxt->op == BLE_GATT_ACCESS_OP_READ_CHR) {
        static const uint8_t empty[8] = {0};
        os_mbuf_append(ctxt->om, empty, sizeof(empty));
    }
    return 0;
}

static int hid_boot_output_cb(uint16_t ch, uint16_t ah,
                              struct ble_gatt_access_ctxt *ctxt, void *arg)
{
    (void)ch; (void)ah; (void)arg;
    return 0;
}

static int hid_ctrl_cb(uint16_t ch, uint16_t ah,
                       struct ble_gatt_access_ctxt *ctxt, void *arg)
{
    (void)ch; (void)ah; (void)arg;
    return 0;
}

static int hid_proto_cb(uint16_t ch, uint16_t ah,
                        struct ble_gatt_access_ctxt *ctxt, void *arg)
{
    (void)ch; (void)ah; (void)arg;
    if (ctxt->op == BLE_GATT_ACCESS_OP_READ_CHR) {
        uint8_t mode = 1; /* Report Protocol */
        os_mbuf_append(ctxt->om, &mode, 1);
    }
    return 0;
}

static int bas_level_cb(uint16_t ch, uint16_t ah,
                        struct ble_gatt_access_ctxt *ctxt, void *arg)
{
    (void)ch; (void)ah; (void)arg;
    if (ctxt->op == BLE_GATT_ACCESS_OP_READ_CHR) {
        uint8_t level = 100;
        os_mbuf_append(ctxt->om, &level, 1);
    }
    return 0;
}

static int dis_pnp_cb(uint16_t ch, uint16_t ah,
                      struct ble_gatt_access_ctxt *ctxt, void *arg)
{
    (void)ch; (void)ah; (void)arg;
    if (ctxt->op == BLE_GATT_ACCESS_OP_READ_CHR)
        os_mbuf_append(ctxt->om, pnp_id, sizeof(pnp_id));
    return 0;
}

/* ---- GATT service table ---- */

static const struct ble_gatt_svc_def gatt_svcs[] = {
    /* Device Information Service */
    {
        .type = BLE_GATT_SVC_TYPE_PRIMARY,
        .uuid = BLE_UUID16_DECLARE(0x180A),
        .characteristics = (struct ble_gatt_chr_def[]) {
            { .uuid = BLE_UUID16_DECLARE(0x2A50), .access_cb = dis_pnp_cb,
              .flags = BLE_GATT_CHR_F_READ },
            { 0 },
        },
    },
    /* Battery Service */
    {
        .type = BLE_GATT_SVC_TYPE_PRIMARY,
        .uuid = BLE_UUID16_DECLARE(0x180F),
        .characteristics = (struct ble_gatt_chr_def[]) {
            { .uuid = BLE_UUID16_DECLARE(0x2A19), .access_cb = bas_level_cb,
              .flags = BLE_GATT_CHR_F_READ | BLE_GATT_CHR_F_NOTIFY },
            { 0 },
        },
    },
    /* HID Service */
    {
        .type = BLE_GATT_SVC_TYPE_PRIMARY,
        .uuid = BLE_UUID16_DECLARE(0x1812),
        .characteristics = (struct ble_gatt_chr_def[]) {
            { .uuid = BLE_UUID16_DECLARE(0x2A4B), .access_cb = hid_report_map_cb,
              .flags = BLE_GATT_CHR_F_READ },
            { .uuid = BLE_UUID16_DECLARE(0x2A4A), .access_cb = hid_info_cb,
              .flags = BLE_GATT_CHR_F_READ },
            /* Input Report */
            { .uuid = BLE_UUID16_DECLARE(0x2A4D), .access_cb = hid_input_report_cb,
              .val_handle = &hid_input_val_hdl,
              .descriptors = (struct ble_gatt_dsc_def[]) {
                  { .uuid = BLE_UUID16_DECLARE(0x2908), .att_flags = BLE_ATT_F_READ,
                    .access_cb = hid_input_ref_cb },
                  { 0 },
              },
              .flags = BLE_GATT_CHR_F_READ | BLE_GATT_CHR_F_NOTIFY | BLE_GATT_CHR_F_READ_ENC },
            /* Output Report (LEDs) */
            { .uuid = BLE_UUID16_DECLARE(0x2A4D), .access_cb = hid_output_report_cb,
              .descriptors = (struct ble_gatt_dsc_def[]) {
                  { .uuid = BLE_UUID16_DECLARE(0x2908), .att_flags = BLE_ATT_F_READ,
                    .access_cb = hid_output_ref_cb },
                  { 0 },
              },
              .flags = BLE_GATT_CHR_F_READ | BLE_GATT_CHR_F_WRITE |
                       BLE_GATT_CHR_F_WRITE_NO_RSP | BLE_GATT_CHR_F_READ_ENC |
                       BLE_GATT_CHR_F_WRITE_ENC },
            /* Boot Keyboard Input Report */
            { .uuid = BLE_UUID16_DECLARE(0x2A22), .access_cb = hid_boot_input_cb,
              .flags = BLE_GATT_CHR_F_READ | BLE_GATT_CHR_F_NOTIFY | BLE_GATT_CHR_F_READ_ENC },
            /* Boot Keyboard Output Report */
            { .uuid = BLE_UUID16_DECLARE(0x2A32), .access_cb = hid_boot_output_cb,
              .flags = BLE_GATT_CHR_F_READ | BLE_GATT_CHR_F_WRITE |
                       BLE_GATT_CHR_F_WRITE_NO_RSP | BLE_GATT_CHR_F_READ_ENC },
            { .uuid = BLE_UUID16_DECLARE(0x2A4C), .access_cb = hid_ctrl_cb,
              .flags = BLE_GATT_CHR_F_WRITE_NO_RSP },
            { .uuid = BLE_UUID16_DECLARE(0x2A4E), .access_cb = hid_proto_cb,
              .flags = BLE_GATT_CHR_F_READ | BLE_GATT_CHR_F_WRITE_NO_RSP },
            { 0 },
        },
    },
    /* MIDI Service */
    {
        .type = BLE_GATT_SVC_TYPE_PRIMARY,
        .uuid = &midi_svc_uuid.u,
        .characteristics = (struct ble_gatt_chr_def[]) {
            { .uuid = &midi_chr_uuid.u, .access_cb = midi_access_cb,
              .val_handle = &midi_val_hdl,
              .flags = BLE_GATT_CHR_F_READ | BLE_GATT_CHR_F_WRITE_NO_RSP | BLE_GATT_CHR_F_NOTIFY },
            { 0 },
        },
    },
    { 0 },
};

/* ---- send empty keyboard report ---- */

static int conn_find(uint16_t hdl)
{
    for (int i = 0; i < conn_count; i++)
        if (conns[i].hdl == hdl) return i;
    return -1;
}

static void send_empty_hid_report(uint16_t hdl)
{
    static const uint8_t empty[8] = {0};
    struct os_mbuf *om = ble_hs_mbuf_from_flat(empty, sizeof(empty));
    if (om)
        ble_gatts_notify_custom(hdl, hid_input_val_hdl, om);
}

/* ---- GAP / advertising ---- */

static void start_adv(void);

static int gap_event_cb(struct ble_gap_event *event, void *arg)
{
    (void)arg;
    switch (event->type) {
    case BLE_GAP_EVENT_CONNECT:
        if (event->connect.status == 0) {
            uint16_t hdl = event->connect.conn_handle;
            if (conn_count < MAX_CONNECTIONS) {
                conns[conn_count].hdl = hdl;
                conns[conn_count].midi_notify = false;
                conns[conn_count].hid_notify = false;
                conn_count++;
            }
            xEventGroupSetBits(ble_events, CONNECTED_BIT);
            ble_att_set_preferred_mtu(128);
            ble_gattc_exchange_mtu(hdl, NULL, NULL);
            ble_gap_security_initiate(hdl);
            printf("BLE: connected (%d/%d)\n", conn_count, MAX_CONNECTIONS);
            if (conn_count < MAX_CONNECTIONS)
                start_adv();
        } else {
            start_adv();
        }
        break;
    case BLE_GAP_EVENT_DISCONNECT: {
        uint16_t hdl = event->disconnect.conn.conn_handle;
        int idx = conn_find(hdl);
        if (idx >= 0) {
            conns[idx] = conns[--conn_count];
        }
        if (conn_count == 0)
            xEventGroupClearBits(ble_events, CONNECTED_BIT);
        printf("BLE: disconnected (%d/%d)\n", conn_count, MAX_CONNECTIONS);
        start_adv();
        break;
    }
    case BLE_GAP_EVENT_SUBSCRIBE: {
        int idx = conn_find(event->subscribe.conn_handle);
        if (idx >= 0) {
            if (event->subscribe.attr_handle == midi_val_hdl)
                conns[idx].midi_notify = event->subscribe.cur_notify;
            if (event->subscribe.attr_handle == hid_input_val_hdl) {
                conns[idx].hid_notify = event->subscribe.cur_notify;
                if (conns[idx].hid_notify)
                    send_empty_hid_report(event->subscribe.conn_handle);
            }
        }
        break;
    }
    case BLE_GAP_EVENT_REPEAT_PAIRING: {
        struct ble_gap_conn_desc desc;
        ble_gap_conn_find(event->repeat_pairing.conn_handle, &desc);
        ble_store_util_delete_peer(&desc.peer_id_addr);
        return BLE_GAP_REPEAT_PAIRING_RETRY;
    }
    default:
        break;
    }
    return 0;
}

static void start_adv(void)
{
    struct ble_hs_adv_fields fields = {0};
    fields.flags = BLE_HS_ADV_F_DISC_GEN | BLE_HS_ADV_F_BREDR_UNSUP;
    fields.name = (uint8_t *)DEVICE_NAME;
    fields.name_len = strlen(DEVICE_NAME);
    fields.name_is_complete = 1;
    fields.appearance = 0x03C1; /* Keyboard */
    fields.appearance_is_present = 1;
    ble_uuid16_t adv_uuids[] = {
        BLE_UUID16_INIT(0x1812), /* HID */
        BLE_UUID16_INIT(0x180F), /* Battery */
    };
    fields.uuids16 = adv_uuids;
    fields.num_uuids16 = 2;
    fields.uuids16_is_complete = 0;
    ble_gap_adv_set_fields(&fields);

    struct ble_hs_adv_fields rsp = {0};
    rsp.uuids128 = (ble_uuid128_t *)&midi_svc_uuid;
    rsp.num_uuids128 = 1;
    rsp.uuids128_is_complete = 1;
    ble_gap_adv_rsp_set_fields(&rsp);

    struct ble_gap_adv_params params = {0};
    params.conn_mode = BLE_GAP_CONN_MODE_UND;
    params.disc_mode = BLE_GAP_DISC_MODE_GEN;
    ble_gap_adv_start(BLE_OWN_ADDR_PUBLIC, NULL, BLE_HS_FOREVER,
                      &params, gap_event_cb, NULL);
}

static void on_sync(void)
{
    uint8_t addr_type;
    ble_hs_id_infer_auto(0, &addr_type);
    start_adv();
    printf("BLE: advertising as \"%s\"\n", DEVICE_NAME);
}

static void on_reset(int reason) { (void)reason; }

static void host_task(void *param)
{
    (void)param;
    nimble_port_run();
    nimble_port_freertos_deinit();
}

/* ---- SysEx send ---- */

static int send_sysex(const uint8_t *sysex, uint16_t len)
{
    if (conn_count == 0)
        return -1;

    uint16_t pkt_len = len + 3;
    uint8_t pkt[80];
    if (pkt_len > sizeof(pkt))
        return -1;

    pkt[0] = 0x80;
    pkt[1] = 0x80;
    memcpy(&pkt[2], sysex, len - 1);
    pkt[len + 1] = 0x80;
    pkt[len + 2] = 0xF7;

    int sent = 0;
    for (int i = 0; i < conn_count; i++) {
        if (!conns[i].midi_notify) continue;
        struct os_mbuf *om = ble_hs_mbuf_from_flat(pkt, pkt_len);
        if (!om) continue;
        if (ble_gatts_notify_custom(conns[i].hdl, midi_val_hdl, om) == 0)
            sent++;
    }
    return sent > 0 ? 0 : -1;
}

/* ---- public API ---- */

int ble_init(void)
{
    ble_events = xEventGroupCreate();
    cmd_queue = xQueueCreate(CMD_QUEUE_LEN, sizeof(BleCommand));

    int rc = nimble_port_init();
    if (rc != ESP_OK) return rc;

    ble_hs_cfg.sync_cb  = on_sync;
    ble_hs_cfg.reset_cb = on_reset;
    ble_hs_cfg.sm_bonding  = 1;
    ble_hs_cfg.sm_mitm     = 0;
    ble_hs_cfg.sm_sc       = 1;
    ble_hs_cfg.sm_io_cap   = BLE_SM_IO_CAP_NO_IO;
    ble_hs_cfg.sm_our_key_dist   = BLE_SM_PAIR_KEY_DIST_ENC | BLE_SM_PAIR_KEY_DIST_ID;
    ble_hs_cfg.sm_their_key_dist = BLE_SM_PAIR_KEY_DIST_ENC | BLE_SM_PAIR_KEY_DIST_ID;

    ble_svc_gap_init();
    ble_svc_gatt_init();
    ble_svc_gap_device_name_set(DEVICE_NAME);
    ble_svc_gap_device_appearance_set(0x03C1);

    rc = ble_gatts_count_cfg(gatt_svcs);
    if (rc) return rc;
    rc = ble_gatts_add_svcs(gatt_svcs);
    if (rc) return rc;

    ble_store_config_init();
    nimble_port_freertos_init(host_task);
    return 0;
}

int ble_is_connected(void)
{
    return conn_count > 0;
}

void ble_wait_for_connection(void)
{
    xEventGroupWaitBits(ble_events, CONNECTED_BIT, pdFALSE, pdTRUE, portMAX_DELAY);
    vTaskDelay(pdMS_TO_TICKS(1000));
}

int ble_recv_command(BleCommand *cmd, uint32_t timeout_ms)
{
    TickType_t ticks = (timeout_ms == UINT32_MAX) ? portMAX_DELAY : pdMS_TO_TICKS(timeout_ms);
    return xQueueReceive(cmd_queue, cmd, ticks) == pdTRUE ? 0 : -1;
}

int ble_send_sweep_start(uint32_t num_points, float freq_start, float freq_stop)
{
    uint8_t sx[20];
    uint16_t p = 0;
    sx[p++] = 0xF0; sx[p++] = 0x7D; sx[p++] = RSP_SWEEP_START;
    encode_u16((uint16_t)num_points, &sx[p]); p += 3;
    encode_float(freq_start, &sx[p]); p += 5;
    encode_float(freq_stop, &sx[p]); p += 5;
    sx[p++] = 0xF7;
    return send_sysex(sx, p);
}

int ble_send_eis_point(uint16_t index, const EISPoint *pt)
{
    uint8_t sx[64];
    uint16_t p = 0;
    sx[p++] = 0xF0; sx[p++] = 0x7D; sx[p++] = RSP_DATA_POINT;
    encode_u16(index, &sx[p]); p += 3;
    encode_float(pt->freq_hz,        &sx[p]); p += 5;
    encode_float(pt->mag_ohms,       &sx[p]); p += 5;
    encode_float(pt->phase_deg,      &sx[p]); p += 5;
    encode_float(pt->z_real,         &sx[p]); p += 5;
    encode_float(pt->z_imag,         &sx[p]); p += 5;
    encode_float(pt->rtia_mag_before, &sx[p]); p += 5;
    encode_float(pt->rtia_mag_after,  &sx[p]); p += 5;
    encode_float(pt->rev_mag,        &sx[p]); p += 5;
    encode_float(pt->rev_phase,      &sx[p]); p += 5;
    encode_float(pt->pct_err,        &sx[p]); p += 5;
    sx[p++] = 0xF7;
    return send_sysex(sx, p);
}

int ble_send_sweep_end(void)
{
    uint8_t sx[] = { 0xF0, 0x7D, RSP_SWEEP_END, 0xF7 };
    return send_sysex(sx, sizeof(sx));
}

int ble_send_config(const EISConfig *cfg)
{
    uint8_t sx[32];
    uint16_t p = 0;
    sx[p++] = 0xF0; sx[p++] = 0x7D; sx[p++] = RSP_CONFIG;
    encode_float(cfg->freq_start_hz, &sx[p]); p += 5;
    encode_float(cfg->freq_stop_hz, &sx[p]); p += 5;
    encode_u16(cfg->points_per_decade, &sx[p]); p += 3;
    sx[p++] = (uint8_t)cfg->rtia;
    sx[p++] = (uint8_t)cfg->rcal;
    sx[p++] = (uint8_t)cfg->electrode;
    sx[p++] = 0xF7;
    return send_sysex(sx, p);
}

int ble_send_lsv_start(uint32_t num_points, float v_start, float v_stop)
{
    uint8_t sx[20];
    uint16_t p = 0;
    sx[p++] = 0xF0; sx[p++] = 0x7D; sx[p++] = RSP_LSV_START;
    encode_u16((uint16_t)num_points, &sx[p]); p += 3;
    encode_float(v_start, &sx[p]); p += 5;
    encode_float(v_stop, &sx[p]); p += 5;
    sx[p++] = 0xF7;
    return send_sysex(sx, p);
}

int ble_send_lsv_point(uint16_t index, float v_mv, float i_ua)
{
    uint8_t sx[20];
    uint16_t p = 0;
    sx[p++] = 0xF0; sx[p++] = 0x7D; sx[p++] = RSP_LSV_POINT;
    encode_u16(index, &sx[p]); p += 3;
    encode_float(v_mv, &sx[p]); p += 5;
    encode_float(i_ua, &sx[p]); p += 5;
    sx[p++] = 0xF7;
    return send_sysex(sx, p);
}

int ble_send_lsv_end(void)
{
    uint8_t sx[] = { 0xF0, 0x7D, RSP_LSV_END, 0xF7 };
    return send_sysex(sx, sizeof(sx));
}

int ble_send_amp_start(float v_hold)
{
    uint8_t sx[12];
    uint16_t p = 0;
    sx[p++] = 0xF0; sx[p++] = 0x7D; sx[p++] = RSP_AMP_START;
    encode_float(v_hold, &sx[p]); p += 5;
    sx[p++] = 0xF7;
    return send_sysex(sx, p);
}

int ble_send_amp_point(uint16_t index, float t_ms, float i_ua)
{
    uint8_t sx[20];
    uint16_t p = 0;
    sx[p++] = 0xF0; sx[p++] = 0x7D; sx[p++] = RSP_AMP_POINT;
    encode_u16(index, &sx[p]); p += 3;
    encode_float(t_ms, &sx[p]); p += 5;
    encode_float(i_ua, &sx[p]); p += 5;
    sx[p++] = 0xF7;
    return send_sysex(sx, p);
}

int ble_send_amp_end(void)
{
    uint8_t sx[] = { 0xF0, 0x7D, RSP_AMP_END, 0xF7 };
    return send_sysex(sx, sizeof(sx));
}

int ble_send_cl_start(uint32_t num_points)
{
    uint8_t sx[10];
    uint16_t p = 0;
    sx[p++] = 0xF0; sx[p++] = 0x7D; sx[p++] = RSP_CL_START;
    encode_u16((uint16_t)num_points, &sx[p]); p += 3;
    sx[p++] = 0xF7;
    return send_sysex(sx, p);
}

int ble_send_cl_point(uint16_t index, float t_ms, float i_ua, uint8_t phase)
{
    uint8_t sx[20];
    uint16_t p = 0;
    sx[p++] = 0xF0; sx[p++] = 0x7D; sx[p++] = RSP_CL_POINT;
    encode_u16(index, &sx[p]); p += 3;
    encode_float(t_ms, &sx[p]); p += 5;
    encode_float(i_ua, &sx[p]); p += 5;
    sx[p++] = phase & 0x7F;
    sx[p++] = 0xF7;
    return send_sysex(sx, p);
}

int ble_send_cl_result(float i_free_ua, float i_total_ua)
{
    uint8_t sx[16];
    uint16_t p = 0;
    sx[p++] = 0xF0; sx[p++] = 0x7D; sx[p++] = RSP_CL_RESULT;
    encode_float(i_free_ua, &sx[p]); p += 5;
    encode_float(i_total_ua, &sx[p]); p += 5;
    sx[p++] = 0xF7;
    return send_sysex(sx, p);
}

int ble_send_cl_end(void)
{
    uint8_t sx[] = { 0xF0, 0x7D, RSP_CL_END, 0xF7 };
    return send_sysex(sx, sizeof(sx));
}

int ble_send_ph_result(float v_ocp_mv, float ph, float temp_c)
{
    uint8_t sx[20];
    uint16_t p = 0;
    sx[p++] = 0xF0; sx[p++] = 0x7D; sx[p++] = RSP_PH_RESULT;
    encode_float(v_ocp_mv, &sx[p]); p += 5;
    encode_float(ph, &sx[p]); p += 5;
    encode_float(temp_c, &sx[p]); p += 5;
    sx[p++] = 0xF7;
    return send_sysex(sx, p);
}

int ble_send_temp(float temp_c)
{
    uint8_t sx[12];
    uint16_t p = 0;
    sx[p++] = 0xF0; sx[p++] = 0x7D; sx[p++] = RSP_TEMP;
    encode_float(temp_c, &sx[p]); p += 5;
    sx[p++] = 0xF7;
    return send_sysex(sx, p);
}

int ble_send_ref_frame(uint8_t mode, uint8_t rtia_idx)
{
    uint8_t sx[] = { 0xF0, 0x7D, RSP_REF_FRAME, mode & 0x7F, rtia_idx & 0x7F, 0xF7 };
    return send_sysex(sx, sizeof(sx));
}

int ble_send_ref_lp_range(uint8_t mode, uint8_t low_idx, uint8_t high_idx)
{
    uint8_t sx[] = { 0xF0, 0x7D, RSP_REF_LP_RANGE, mode & 0x7F, low_idx & 0x7F, high_idx & 0x7F, 0xF7 };
    return send_sysex(sx, sizeof(sx));
}

int ble_send_refs_done(void)
{
    uint8_t sx[] = { 0xF0, 0x7D, RSP_REFS_DONE, 0xF7 };
    return send_sysex(sx, sizeof(sx));
}

int ble_send_ref_status(uint8_t has_refs)
{
    uint8_t sx[] = { 0xF0, 0x7D, RSP_REF_STATUS, has_refs & 0x7F, 0xF7 };
    return send_sysex(sx, sizeof(sx));
}