/*******************************************************************************
 * @file    main.c
 * @brief   Medithings VesiScan BASIC
 * @version 1.17
 * @date    2025-12-09
 * @author  Charles KWON
 *
 * [System Architecture Overview]
 * VesiScan BASIC is an nRF52840 (ARM Cortex-M4F, 64MHz) based BLE bladder monitoring patch device.
 * Uses SoftDevice S140 (BLE 5.0) and communicates with the smartphone app via BLE NUS (Nordic UART Service) binary protocol.
 *
 * [Hardware Configuration]
 *   - MCU: nRF52840 (SoftDevice S140 v7.x)
 *   - IMU: ICM42670P 6-axis accel/gyro (I2C, 0x68)
 *   - Temperature: TMP235-Q1 analog sensor (SAADC AIN3)
 *   - Battery: voltage divider (SAADC AIN2, 1/3 prescale)
 *   - Power: POWER_HOLD(P0.8) self-latch circuit, POWER_BUTTON(P1.8) input
 *
 * [Software Boot Sequence - main()]
 *   Phase 1: Basic hardware (power latch, log, GPIO, timers, config, buttons/LEDs)
 *   Phase 2: BLE stack (SoftDevice, power mgmt, DC-DC)
 *   Phase 3: FDS + config load
 *   Phase 4: BLE protocol (GAP, GATT, NUS, Advertising, security)
 *   Phase 5: Application (parser, piezo)
 *   Boot complete -> power button state machine (main_s) -> idle loop
 *
 * [Power Button State Machine - main_s() callback, 5ms interval]
 *   - Short press (<1.5s, cnt_s<150): power OFF
 *   - Medium press (1.5s~10s, cnt_s>=150): start boot sequence (sensor+BLE advertising)
 *   - Long press (>10s, cnt_s>1000): factory reset (passkey reset + power OFF)
 *
 * [Event Flow]
 *   1. Receive command from app via BLE NUS -> nus_data_handler -> dr_cmd_parser
 *   2. Receive command from UART -> uart_event_handle -> dr_cmd_parser
 *   3. Command parser (cmd_parse.c) dispatches by tag (sta?, ssn?, ssp?, etc.)
 *   4. Build binary packets from sensor data via format_data() family
 *   5. Transmit over BLE with CRC16 via dr_binary_tx_safe() (max 100 retries)
 *
 * [Build Modes]
 *   BLE_DEV_MODE = 1       : No security (fast pairing, development)
 *   BLE_DEV_MODE = 0       : Static passkey + MITM protection (production)
 ******************************************************************************/

/*==============================================================================
 * INCLUDES
 *============================================================================*/
#include <stdint.h>
#include <string.h>
#include "nordic_common.h"
#include "nrf.h"
#include "ble_hci.h"           /* BLE HCI error code definitions */
#include "ble_advdata.h"       /* BLE advertising data structures */
#include "ble_advertising.h"   /* BLE advertising module */
#include "ble_srv_common.h"    /* BLE service common utilities */
#include "ble_conn_params.h"   /* BLE connection parameter negotiation */
#include "nrf_sdh.h"           /* SoftDevice handler (BLE stack management) */
#include "nrf_sdh_soc.h"       /* SoftDevice SoC event handler */
#include "nrf_sdh_ble.h"       /* SoftDevice BLE event handler */
#include "nrf_ble_gatt.h"      /* BLE GATT module (MTU negotiation) */
#include "nrf_ble_qwr.h"       /* BLE Queued Write module (long characteristic writes) */
#include "app_timer.h"         /* App timer (RTC1-based soft timer) */
#include "ble_nus.h"           /* Nordic UART Service (BLE bidirectional data) */
#include "app_uart.h"          /* Physical UART driver (debug/factory test) */
#include "app_util_platform.h" /* Interrupt priority macros */
#include "nrf_pwr_mgmt.h"      /* Power management (WFE/sleep when idle) */
#include "nrf_delay.h"         /* Blocking delay (us/ms) */
#include "math.h"
#include "crc16.h"             /* CRC16 computation (BLE packet integrity) */

#if defined (UART_PRESENT)
#include "nrf_uart.h"
#endif
#if defined (UARTE_PRESENT)
#include "nrf_uarte.h"
#endif

#include "nrf_log.h"
#include "nrf_log_ctrl.h"
#include "nrf_log_default_backends.h"

#if BLE_DFU_ENABLED
#include "nrf_pwr_mgmt.h"
#include "ble_dfu.h"
#include "nrf_power.h"
#include "nrf_bootloader_info.h"
#include "nrf_dfu_ble_svci_bond_sharing.h"
#include "nrf_svci_async_function.h"
#include "nrf_svci_async_handler.h"
#endif

/* -- Application module headers -- */
#include "system_interface.h"   /* System interface (IMU sensor I2C communication) */
#include "main.h"               /* Main header (global structs, constants, extern declarations) */
#include "app_raw_main.h"       /* Sensor raw data processing module */
#include "main_timer.h"         /* Main event loop timer (10ms period) */
#include "power_control.h"      /* Power sequence control (ON/OFF/sleep) */
#include "tmp235_q1.h"          /* TMP235-Q1 temperature sensor driver */
#include "fds.h"                /* Flash Data Storage (non-volatile config) */
#include "battery_saadc.h"      /* Battery voltage measurement (SAADC) */

/* -- BLE security headers (when FEATURE_SECURE_CONNECTION enabled) -- */
#if FEATURE_SECURE_CONNECTION
#include "peer_manager.h"           /* Peer Manager (bonding/pairing management) */
#include "peer_manager_handler.h"   /* Peer Manager default event handler */
#include "nrf_ble_lesc.h"           /* BLE LESC (LE Secure Connections) support */
#include "ble_quick_security.h"     /* Quick security setup wrapper */
#include "i2c_manager.h"            /* I2C bus manager (sensor communication) */
#endif

/* -- Crypto / command parser / debug -- */
#include "parser.h"             /* Binary command parser (dr_cmd_parser) */
#include "cmd_table.h"          /* Command table registration (cmd_table_init) */
#include "debug_print.h"        /* Debug output macro (DBG_PRINTF) */
#include "fstorage.h"           /* Flash Storage wrapper (FDS init/save/load) */
#include "dr_piezo.h"           /* Piezo ultrasound driver */
#include "led_control.h"        /* LED direct control driver */



/*==============================================================================
 * Build Configuration
 *============================================================================*/
#define BLE_DEV_MODE        0   /* 1: Dev mode (no security), 0: Production mode (passkey required) */

/*==============================================================================
 * Hardware Pin Definitions
 *============================================================================*/
#define POWER_HOLD          NRF_GPIO_PIN_MAP(0,8)   /* Power self-latch pin (HIGH=power maintained) */
#define POWER_BUTTON        NRF_GPIO_PIN_MAP(1,8)   /* Power button input pin (LOW=pressed) */

/*==============================================================================
 * BLE Configuration
 *============================================================================*/
#define APP_BLE_CONN_CFG_TAG            1                           /* SoftDevice BLE config tag */
#define NUS_SERVICE_UUID_TYPE           BLE_UUID_TYPE_VENDOR_BEGIN  /* NUS UUID type (vendor-specific) */
#define APP_BLE_OBSERVER_PRIO           3                           /* BLE event observer priority */
#define APP_ADV_INTERVAL                64                          /* Advertising interval: 64 x 0.625ms = 40ms */

#define APP_ADV_DURATION_NORMAL         60000                       /* Normal disconnect adv duration: 60000 x 10ms = 10min */
#define APP_ADV_DURATION_UNLIMITED      0                           /* Unintended disconnect: unlimited advertising */
#define APP_ADV_DURATION                APP_ADV_DURATION_NORMAL     /* Default adv duration (changed at runtime by disconnect reason) */

#define MIN_CONN_INTERVAL               MSEC_TO_UNITS(30, UNIT_1_25_MS)   /* Min connection interval: 30ms */
#define MAX_CONN_INTERVAL               MSEC_TO_UNITS(30, UNIT_1_25_MS)   /* Max connection interval: 30ms */
#define SLAVE_LATENCY                   0                                 /* Slave latency: 0 (respond every connection event) */
#define CONN_SUP_TIMEOUT                MSEC_TO_UNITS(4000, UNIT_10_MS)   /* Connection supervision timeout: 4s */
#define FIRST_CONN_PARAMS_UPDATE_DELAY  APP_TIMER_TICKS(5000)             /* Wait 5s before first param update request */
#define NEXT_CONN_PARAMS_UPDATE_DELAY   APP_TIMER_TICKS(30000)            /* Subsequent param update interval: 30s */
#define MAX_CONN_PARAMS_UPDATE_COUNT    3                                 /* Max param update attempts */

/*==============================================================================
 * System Constants
 *============================================================================*/
#define DEAD_BEEF                       0xDEADBEEF  /* SDK error handler magic number */
#define UART_TX_BUF_SIZE                16384       /* UART TX FIFO buffer size (16KB) */
#define UART_RX_BUF_SIZE                512         /* UART RX FIFO buffer size */
#define POWER_ON_DELAY                  5           /* Power button polling interval (5ms) */
#define POWER_OFF_DELAY                 3000        /* Power-off delay: 3s after LED indication */
#define POWER_RESET_DELAY               2000        /* Reset delay: 2s */
#define LED_NUM                         24          /* LED pin number */

/*==============================================================================
 * BLE Instances (statically allocated via SoftDevice macros)
 *============================================================================*/
BLE_NUS_DEF(m_nus, NRF_SDH_BLE_TOTAL_LINK_COUNT);  /* Nordic UART Service instance */
NRF_BLE_GATT_DEF(m_gatt);                          /* GATT module instance (MTU negotiation) */
NRF_BLE_QWR_DEF(m_qwr);                            /* Queued Write instance */
BLE_ADVERTISING_DEF(m_advertising);                /* Advertising module instance */

/*==============================================================================
 * Timer Instances
 *============================================================================*/
APP_TIMER_DEF(m_power_on_delay_timer_id);          /* Power button polling timer (5ms single-shot, main_s callback) */
APP_TIMER_DEF(m_power_off_delay_timer_id);         /* Power-off delay timer (physical power cut after 3s) */
APP_TIMER_DEF(m_PM_timer_id);                      /* Peer Manager timer (forced disconnect) */

/*==============================================================================
 * Static Variables
 *============================================================================*/
#if FEATURE_SECURE_CONNECTION
static pm_peer_id_t m_peer_to_be_deleted = PM_PEER_ID_INVALID;
#endif

static uint16_t   m_conn_handle          = BLE_CONN_HANDLE_INVALID;         /* Current BLE connection handle */
static uint16_t   m_ble_nus_max_data_len = BLE_GATT_ATT_MTU_DEFAULT - 3;    /* NUS max data length (MTU - overhead) */
static ble_uuid_t m_adv_uuids[]          = {{BLE_UUID_NUS_SERVICE, NUS_SERVICE_UUID_TYPE}};  /* UUIDs included in advertising */

static uint8_t  m_tx_buffer[BLE_NUS_MAX_DATA_LEN] = {0}; /* ASCII text transmission buffer */
static uint16_t m_tx_len = 0;                            /* Data length to transmit */
static volatile bool m_tx_in_progress = false;           /* TX in progress flag */
static volatile bool m_tx_complete_pending = false;      /* TX completion pending flag */
static uint8_t  c_addr[6] = {0};                         /* Connected peer BLE address */

static char * roles_str[] = {"INVALID_ROLE", "CENTRAL", "PERIPHERAL"};

/*==============================================================================
 * Global Variables
 * IEC 62304 §5.5.3: all global buffers zero-initialized for deterministic startup
 *============================================================================*/
volatile uint8_t Sj_type;                     /* Command type identifier */
bool power_off_duble_prohibit = false;        /* Power-off double prevention flag */
volatile bool power_state = false;            /* Power state tracking */

/* -- External module flags (defined in main_timer/power_control) -- */
extern bool go_device_power_off;       /* Power-off request flag */
extern bool go_sleep_mode_enter;       /* Sleep mode entry request flag */
extern bool go_NVIC_SystemReset;       /* System reset request flag */
extern bool ble_got_new_data;          /* BLE new data received flag */
extern bool motion_data_once;          /* Motion data one-shot transmit flag */
extern bool con_single;                /* Single connection mode flag */
extern bool info4;                     /* Device info transmission complete flag */
extern uint8_t add_cycle;              /* Additional measurement cycle counter */
extern bool motion_raw_data_enabled;   /* Motion raw data streaming enabled */

uint16_t cnt_s;                                         /* Power button polling counter (5ms units, 150=0.75s) */
char ble_tx_buffer[BLE_NUS_MAX_DATA_LEN] = {0};         /* BLE text transmission buffer */
uint8_t ble_bin_buffer[BLE_NUS_MAX_DATA_LEN] = {0};     /* BLE binary response buffer */
uint16_t ble_bin_buff[BLE_NUS_MAX_DATA_LEN/2] = {0};    /* BLE binary transmission buffer (word units) */
which_cmd_t cmd_type_t;                                 /* Current command source (CMD_BLE or CMD_UART) */

bool device_status = false;    /* Device active state (true=running) */
bool device_reset = true;      /* Booting flag (true=not yet booted) */

#if FEATURE_SECURE_CONNECTION
bool erase_bonds;              /* Bond erase flag (set by button state at boot) */
#endif

volatile bool ble_connection_st;                   /* BLE connection state (1=connected, 0=disconnected) */
volatile bool data_tx_in_progress = false;         /* Binary TX in progress flag */

/* -- BLE TX async retry state -- */
static volatile bool    s_tx_pending = false;                           /* TX retry pending */
static uint8_t          s_tx_pending_buf[BLE_NUS_MAX_DATA_LEN] = {0};   /* Pending packet (with CRC) */
static uint16_t         s_tx_pending_len = 0;                           /* Pending packet length */

char m_static_passkey[PASSKEY_LENGTH] = DEFAULT_PASSKEY;    /* Static passkey (6 digits, loaded from FDS) */
char SERIAL_NO[SERIAL_NO_LENGTH] = {0};                     /* Serial number (used as BLE device name) */
char HW_NO[HW_NO_LENGTH] = {0};                             /* Hardware number (FDS stored/read) */
bool bond_data_delete;                                      /* Bond data delete request flag */
uint32_t m_life_cycle;                                      /* Device life cycle counter */
uint8_t resetCount = 0;                                     /* Communication timeout counter (reset detection) */
bool info4;                                                 /* Measurement with sensor info (battery/IMU/temp) flag */
uint8_t m_reset_status;                                     /* Reset status code (1=normal, 2=SW reset, 5=security reset, 10=bond complete) */

/*==============================================================================
 * Suppress Unused Variable Warnings
 *============================================================================*/
#if FEATURE_SECURE_CONNECTION
static void __attribute__((unused)) suppress_unused_warnings(void)
{
    (void)m_peer_to_be_deleted;
    (void)roles_str;
    (void)c_addr;
}
#endif

/*==============================================================================
 * Power Management
 *============================================================================*/

/**
 * @brief Initialize power self-latch pin
 *
 * Configure P0.8 (POWER_HOLD) as output and drive HIGH to maintain the external power latch.
 * When P0.8 goes LOW, physical power is cut off.
 * Must be called first upon main() entry (Phase 0).
 */
static void power_hold_init(void)
{
    NRF_P0->DIRSET = (1 << 8);   /* Set P0.8 as output */
    NRF_P0->OUTSET = (1 << 8);   /* Set P0.8 HIGH -> maintain power */
}

/**
 * @brief Physical power ON/OFF control
 *
 * Directly controls the POWER_HOLD pin for physical device power ON/OFF.
 * When OFF, the power latch releases and the entire system shuts down (non-recoverable).
 */
static void power_control_handler(on_off_cont_t device_power_st)
{
    if (device_power_st == OFF)
    {
        nrf_gpio_pin_clear(POWER_HOLD);   /* P0.8 LOW -> release power latch -> power cut */
        DBG_PRINTF("[PWR] OFF\r\n");
    }
    else if (device_power_st == ON)
    {
        nrf_gpio_pin_set(POWER_HOLD);     /* P0.8 HIGH -> maintain power */
        DBG_PRINTF("[PWR] ON\r\n");
    }
}

/*==============================================================================
 * GPIO Initialization
 *============================================================================*/

/**
 * @brief GPIO initialization
 *
 * Configure power button input and power hold pin only.
 */
static void gpio_init(void)
{
    nrf_gpio_cfg_input(POWER_BUTTON, NRF_GPIO_PIN_NOPULL);

    DBG_PRINTF("[GPIO] OK (BTN=%d)\r\n", nrf_gpio_pin_read(POWER_BUTTON));
}

/*==============================================================================
 * Configuration Loading
 * Load defaults before reading from FDS (Flash Data Storage).
 * After FDS init, load_flash_config() overwrites with flash-stored values.
 *============================================================================*/

/**
 * @brief Load default configuration values
 *
 * Set safe defaults in global variables before reading from FDS.
 * Initializes serial number, passkey, reset status, etc. to defaults.
 */
static void load_default_config(void)
{
    memset(SERIAL_NO, 0, SERIAL_NO_LENGTH);
    memcpy(SERIAL_NO, SERIAL_NUMBER, strlen(SERIAL_NUMBER));

    memset(m_static_passkey, 0, PASSKEY_LENGTH);
    memcpy(m_static_passkey, DEFAULT_PASSKEY, PASSKEY_LENGTH);

    m_reset_status   = 1;
    bond_data_delete = 0;

    DBG_PRINTF("[CFG] Default (S/N=%s)\r\n", SERIAL_NO);
}

/**
 * @brief Load configuration from internal flash (FDS) into global variables
 *
 * Must be called after ble_stack_init() -> fs_storage_init() -> config_load().
 * Copies the m_config struct read from FDS into runtime global variables.
 * Empty fields (0x00 or 0xFF) are filled with defaults and m_need_save_defaults is set.
 */
static bool m_need_save_defaults = false;

static void load_flash_config(void)
{
    m_need_save_defaults = false;

    /* Hardware number -- fill with default if empty */
    if (m_config.hw_no[0] == 0 || m_config.hw_no[0] == (char)0xFF)
    {
        memset(m_config.hw_no, 0, HW_NO_LENGTH);
        memcpy(m_config.hw_no, HARDWARE_VERSION, strlen(HARDWARE_VERSION));
        DBG_PRINTF("[CFG] HW empty, set default: %s\r\n", HARDWARE_VERSION);
        m_need_save_defaults = true;
    }

    /* Serial number -- fill with default if empty */
    if (m_config.serial_no[0] == 0 || m_config.serial_no[0] == (char)0xFF)
    {
        memset(m_config.serial_no, 0, SERIAL_NO_LENGTH);
        memcpy(m_config.serial_no, SERIAL_NUMBER, strlen(SERIAL_NUMBER));
        DBG_PRINTF("[CFG] S/N empty, set default: %s\r\n", SERIAL_NUMBER);
        m_need_save_defaults = true;
    }

    /* Copy serial number -> BLE device name */
    memset(SERIAL_NO, 0, SERIAL_NO_LENGTH);
    memcpy(SERIAL_NO, m_config.serial_no, SERIAL_NO_LENGTH);

    /* Copy passkey */
    memset(m_static_passkey, 0, PASSKEY_LENGTH);
    memcpy(m_static_passkey, m_config.static_passkey, PASSKEY_LENGTH);

    /* Bond data delete flag */
    bond_data_delete = m_config.bond_data_delete;

    /* Reset status */
    m_reset_status = m_config.reset_status;

    DBG_PRINTF("[CFG] HW=%.12s S/N=%s passkey=%.6s bond=%d rst=%d\r\n",
               m_config.hw_no, SERIAL_NO, m_static_passkey, bond_data_delete, m_reset_status);
    DBG_PRINTF("[CFG] piezo: freq=%u cyc=%u avg=%u delay=%u samples=%u\r\n",
               m_config.piezo_freq_option, m_config.piezo_cycles,
               m_config.piezo_averaging, m_config.piezo_delay_us,
               m_config.piezo_num_samples);
}

/*==============================================================================
 * Timer Callback Functions
 *============================================================================*/
static void main_s(void * p_context);                       /* Power button state machine (forward declaration) */
static void t_power_off_timeout_handler(void * p_context);  /* Power-off timeout */
static void PM_s(void * p_context);                         /* Peer Manager timer */

/**
 * @brief Power-off timeout callback
 *
 * Called after POWER_OFF_DELAY (3s) elapses.
 * Turns off LED and sets POWER_HOLD pin LOW for physical power cut.
 * After this function, the device is completely off (non-recoverable).
 */
static void t_power_off_timeout_handler(void * p_context)
{
    UNUSED_PARAMETER(p_context);
    APP_ERROR_CHECK(app_timer_stop(m_power_off_delay_timer_id));
    DBG_PRINTF("[PWR] Off timeout\r\n");
    led_set_state(LED_STATE_OFF);                       /* LED OFF */
    power_control_handler(OFF);                        /* P0.8 LOW -> power cut */
}

/**
 * @brief Peer Manager timer callback
 *
 * Force BLE disconnect when in security-related state (m_reset_status==5).
 * Used to drop the existing connection for reconnection after bond deletion.
 */
static void PM_s(void * p_context)
{
    UNUSED_PARAMETER(p_context);
    APP_ERROR_CHECK(app_timer_stop(m_PM_timer_id));

    if (m_reset_status == 5)
    {
        DBG_PRINTF("[PM] Kill\r\n");
        sd_ble_gap_disconnect(m_conn_handle, BLE_HCI_REMOTE_USER_TERMINATED_CONNECTION);
    }
}

/*==============================================================================
 * Timer Initialization / Start Functions
 *============================================================================*/

/**
 * @brief Initialize all app timers (boot Phase 3)
 *
 * Initialize app_timer module then create each timer:
 *   - m_power_on_delay_timer: power button polling (5ms single-shot, main_s callback)
 *   - m_power_off_delay_timer: power-off delay (3s single-shot)
 *   - m_PM_timer: Peer Manager disconnect (single-shot)
 *   - main_timer: main event loop (10ms single-shot, main_loop callback)
 *   - battery_timer: battery monitoring (5s repeating)
 *   - power_timer: power sequence (20ms single-shot, power_loop callback)
 */
static void timers_init(void)
{
    ret_code_t err_code = app_timer_init();
    APP_ERROR_CHECK(err_code);

    APP_ERROR_CHECK(app_timer_create(&m_power_on_delay_timer_id, APP_TIMER_MODE_SINGLE_SHOT, main_s));
    APP_ERROR_CHECK(app_timer_create(&m_power_off_delay_timer_id, APP_TIMER_MODE_SINGLE_SHOT, t_power_off_timeout_handler));
    APP_ERROR_CHECK(app_timer_create(&m_PM_timer_id, APP_TIMER_MODE_SINGLE_SHOT, PM_s));
    
    main_timer_init();
    battery_timer_init();
    power_timer_init();
    
#if FEATURE_PRINTF
    full_timer_init();
#endif
}

/**
 * @brief Start the power button polling timer
 *
 * Fires main_s() callback after 5ms (POWER_ON_DELAY).
 * main_s() checks button state; if still pressed, re-arms the timer to drive the state machine.
 */
static void timers_start(void)
{
    ret_code_t err_code;
    err_code = app_timer_start(m_power_on_delay_timer_id, APP_TIMER_TICKS(POWER_ON_DELAY), NULL);
    APP_ERROR_CHECK(err_code);
}

/*==============================================================================
 * BLE DFU Handler
 * Handles DFU (Device Firmware Update) events for firmware update over BLE.
 * Logs bootloader entry preparation, success/failure events.
 *============================================================================*/
#if BLE_DFU_ENABLED
static void ble_dfu_evt_handler(ble_dfu_buttonless_evt_type_t event)
{
    switch (event)
    {
        case BLE_DFU_EVT_BOOTLOADER_ENTER_PREPARE:
            DBG_PRINTF("[DFU] Prepare\r\n");
            break;
        case BLE_DFU_EVT_BOOTLOADER_ENTER:
            DBG_PRINTF("[DFU] Enter\r\n");
            break;
        case BLE_DFU_EVT_BOOTLOADER_ENTER_FAILED:
            DBG_PRINTF("[DFU] Failed\r\n");
            break;
        case BLE_DFU_EVT_RESPONSE_SEND_ERROR:
            DBG_PRINTF("[DFU] Error\r\n");
            APP_ERROR_CHECK(false);
            break;
        default:
            break;
    }
}
#endif

/*==============================================================================
 * BLE GAP Initialization
 *============================================================================*/

/**
 * @brief Initialize GAP (Generic Access Profile) parameters
 *
 * 1) Set device name to SERIAL_NO -> shown during BLE scan
 * 2) Configure connection parameters (20~75ms interval, slave latency 0, supervision timeout 4s)
 * 3) Set static passkey (when FEATURE_STATIC_PASSKEY enabled)
 */
static void gap_params_init(void)
{
    uint32_t                err_code;
    ble_gap_conn_params_t   gap_conn_params;
    ble_gap_conn_sec_mode_t sec_mode;
#if FEATURE_STATIC_PASSKEY
    ble_opt_t               ble_opt;
#endif

    BLE_GAP_CONN_SEC_MODE_SET_OPEN(&sec_mode);

    err_code = sd_ble_gap_device_name_set(&sec_mode,
                                          (const uint8_t *) SERIAL_NO,
                                          strlen(SERIAL_NO));
    APP_ERROR_CHECK(err_code);

    memset(&gap_conn_params, 0, sizeof(gap_conn_params));
    gap_conn_params.min_conn_interval = MIN_CONN_INTERVAL;
    gap_conn_params.max_conn_interval = MAX_CONN_INTERVAL;
    gap_conn_params.slave_latency     = SLAVE_LATENCY;
    gap_conn_params.conn_sup_timeout  = CONN_SUP_TIMEOUT;

    err_code = sd_ble_gap_ppcp_set(&gap_conn_params);
    APP_ERROR_CHECK(err_code);

#if FEATURE_STATIC_PASSKEY
    ble_opt.gap_opt.passkey.p_passkey = (const uint8_t *)m_static_passkey;
    err_code = sd_ble_opt_set(BLE_GAP_OPT_PASSKEY, &ble_opt);
    APP_ERROR_CHECK(err_code);
#endif
}

/*==============================================================================
 * BLE Service Initialization
 *============================================================================*/
static void nrf_qwr_error_handler(uint32_t nrf_error)
{
    APP_ERROR_HANDLER(nrf_error);
}

/* Async MAA TX complete handler (forward declarations) */
extern bool maa_async_on_tx_ready(void);
extern bool maa_async_is_busy(void);
extern void maa_async_abort(void);

/* 2026-03-17: Prevent blocking in BLE callback -- buffer command for main loop processing */
static volatile uint8_t  pending_cmd_buf[BLE_NUS_MAX_DATA_LEN] = {0};
static volatile uint8_t  pending_cmd_len = 0;

/**
 * @brief NUS (Nordic UART Service) data receive handler
 *
 * Called when data is received from the smartphone app over BLE.
 *   - RX_DATA event: copy received data to buffer (dr_cmd_parser called in main loop)
 *   - TX_RDY event: BLE TX buffer has space, continue async MAA transmission
 */
static void nus_data_handler(ble_nus_evt_t * p_evt)
{
    if (p_evt->type == BLE_NUS_EVT_RX_DATA)
    {
        cmd_type_t = CMD_BLE;
        ble_got_new_data = true;

        /* Central may have increased connection interval; re-request fast interval (once per 30s) */
        {
            static uint32_t last_update_tick = 0;
            uint32_t now_tick = app_timer_cnt_get();
            if (last_update_tick == 0 || app_timer_cnt_diff_compute(now_tick, last_update_tick) >= APP_TIMER_TICKS(30000))
            {
                ble_gap_conn_params_t conn_params;
                conn_params.min_conn_interval = MIN_CONN_INTERVAL;
                conn_params.max_conn_interval = MAX_CONN_INTERVAL;
                conn_params.slave_latency     = SLAVE_LATENCY;
                conn_params.conn_sup_timeout  = CONN_SUP_TIMEOUT;
                sd_ble_gap_conn_param_update(m_conn_handle, &conn_params);
                last_update_tick = now_tick;
            }
        }

        /* Only copy in callback; process in main loop */
        if (p_evt->params.rx_data.length <= BLE_NUS_MAX_DATA_LEN)
        {
            memcpy((void *)pending_cmd_buf, p_evt->params.rx_data.p_data, p_evt->params.rx_data.length);
            pending_cmd_len = p_evt->params.rx_data.length;
        }
    }
    else if (p_evt->type == BLE_NUS_EVT_TX_RDY)
    {
        /* BLE TX buffer has space - continue async MAA transmission */
        if (maa_async_is_busy())
        {
            maa_async_on_tx_ready();
        }
        /* Retry pending packet (dr_binary_tx_safe async) */
        else if (s_tx_pending)
        {
            uint16_t send_len = s_tx_pending_len;
            uint32_t err = ble_nus_data_send(&m_nus, s_tx_pending_buf, &send_len, m_conn_handle);
            if (err == NRF_SUCCESS)
            {
                s_tx_pending = false;
            }
            /* NRF_ERROR_RESOURCES -> retry on next TX_RDY */
        }
    }
}

/**
 * @brief Initialize BLE services
 *
 * Initialize QWR (Queued Write) + NUS (Nordic UART Service) + DFU (optional).
 * Register nus_data_handler as the NUS data_handler for BLE receive processing.
 */
static void services_init(void)
{
    uint32_t           err_code;
    ble_nus_init_t     nus_init;
    nrf_ble_qwr_init_t qwr_init = {0};

    qwr_init.error_handler = nrf_qwr_error_handler;
    err_code = nrf_ble_qwr_init(&m_qwr, &qwr_init);
    APP_ERROR_CHECK(err_code);

    memset(&nus_init, 0, sizeof(nus_init));
    nus_init.data_handler = nus_data_handler;
    err_code = ble_nus_init(&m_nus, &nus_init);
    APP_ERROR_CHECK(err_code);

#if BLE_DFU_ENABLED
    ble_dfu_buttonless_init_t dfus_init = {0};
    dfus_init.evt_handler = ble_dfu_evt_handler;
    err_code = ble_dfu_buttonless_init(&dfus_init);
    APP_ERROR_CHECK(err_code);
#endif
}

/*==============================================================================
 * BLE Connection Parameter Negotiation
 *============================================================================*/
/** @brief Disconnect on connection parameter negotiation failure */
static void on_conn_params_evt(ble_conn_params_evt_t * p_evt)
{
    if (p_evt->evt_type == BLE_CONN_PARAMS_EVT_FAILED)
    {
        DBG_PRINTF("[CONN] Param negotiation FAILED -> disconnect\r\n");
        uint32_t err_code = sd_ble_gap_disconnect(m_conn_handle, BLE_HCI_CONN_INTERVAL_UNACCEPTABLE);
        APP_ERROR_CHECK(err_code);
    }
}

static void conn_params_error_handler(uint32_t nrf_error)
{
    APP_ERROR_HANDLER(nrf_error);
}

/**
 * @brief Initialize BLE connection parameter module
 *
 * Wait 5s after connection -> request param update -> retry every 30s up to 3 times.
 * On failure, on_conn_params_evt disconnects.
 */
static void conn_params_init(void)
{
    uint32_t               err_code;
    ble_conn_params_init_t cp_init;

    memset(&cp_init, 0, sizeof(cp_init));
    cp_init.p_conn_params                  = NULL;
    cp_init.first_conn_params_update_delay = FIRST_CONN_PARAMS_UPDATE_DELAY;
    cp_init.next_conn_params_update_delay  = NEXT_CONN_PARAMS_UPDATE_DELAY;
    cp_init.max_conn_params_update_count   = MAX_CONN_PARAMS_UPDATE_COUNT;
    cp_init.start_on_notify_cccd_handle    = BLE_GATT_HANDLE_INVALID;
    cp_init.disconnect_on_fail             = false;
    cp_init.evt_handler                    = on_conn_params_evt;
    cp_init.error_handler                  = conn_params_error_handler;

    err_code = ble_conn_params_init(&cp_init);
    APP_ERROR_CHECK(err_code);
}

/*==============================================================================
 * BLE Stack Initialization
 *============================================================================*/
static void ble_evt_handler(ble_evt_t const * p_ble_evt, void * p_context);

/**
 * @brief Initialize SoftDevice BLE stack
 *
 * 1) Request SoftDevice enable
 * 2) Set BLE default config (auto-compute RAM start address)
 * 3) Enable BLE
 * 4) Register BLE event observer (ble_evt_handler)
 */
static void ble_stack_init(void)
{
    ret_code_t err_code;

    err_code = nrf_sdh_enable_request();
    APP_ERROR_CHECK(err_code);

    uint32_t ram_start = 0;
    err_code = nrf_sdh_ble_default_cfg_set(APP_BLE_CONN_CFG_TAG, &ram_start);
    APP_ERROR_CHECK(err_code);

    err_code = nrf_sdh_ble_enable(&ram_start);
    APP_ERROR_CHECK(err_code);

    NRF_SDH_BLE_OBSERVER(m_ble_observer, APP_BLE_OBSERVER_PRIO, ble_evt_handler, NULL);
}

/*==============================================================================
 * BLE GATT (Generic Attribute Profile)
 * Determines max data size per transfer via MTU negotiation.
 *============================================================================*/

/** @brief GATT MTU update event handler -- updates NUS max data length */
void gatt_evt_handler(nrf_ble_gatt_t * p_gatt, nrf_ble_gatt_evt_t const * p_evt)
{
    if ((m_conn_handle == p_evt->conn_handle) && (p_evt->evt_id == NRF_BLE_GATT_EVT_ATT_MTU_UPDATED))
    {
        m_ble_nus_max_data_len = p_evt->params.att_mtu_effective - OPCODE_LENGTH - HANDLE_LENGTH;
    }
}

/** @brief Initialize GATT module -- set MTU to max size */
static void gatt_init(void)
{
    ret_code_t err_code;

    err_code = nrf_ble_gatt_init(&m_gatt, gatt_evt_handler);
    APP_ERROR_CHECK(err_code);

    err_code = nrf_ble_gatt_att_mtu_periph_set(&m_gatt, NRF_SDH_BLE_GATT_MAX_MTU_SIZE);
    APP_ERROR_CHECK(err_code);
}

/*==============================================================================
 * BLE Advertising
 *============================================================================*/

/**
 * @brief BLE advertising event handler
 *
 * - FAST: advertising started (LED indication)
 * - IDLE: advertising timeout -> set sleep mode entry flag
 */
static void on_adv_evt(ble_adv_evt_t ble_adv_evt)
{
    switch (ble_adv_evt)
    {
        case BLE_ADV_EVT_FAST:
            led_set_state(LED_STATE_ADVERTISING);
            DBG_PRINTF("[ADV] Fast\r\n");
            break;
        case BLE_ADV_EVT_IDLE:
            DBG_PRINTF("[ADV] Idle\r\n");
            go_sleep_mode_enter = true;
            main_timer_start();
            break;
        default:
            break;
    }
}

/**
 * @brief Initialize BLE advertising
 *
 * Advertising data: full device name, NUS UUID.
 * Advertising mode: Fast (40ms interval), default duration 10 min.
 * May switch to unlimited advertising at runtime based on disconnect reason.
 */
static void advertising_init(void)
{
    uint32_t               err_code;
    ble_advertising_init_t init;

    memset(&init, 0, sizeof(init));

    init.advdata.name_type          = BLE_ADVDATA_FULL_NAME;
    init.advdata.include_appearance = true;
    init.advdata.flags              = BLE_GAP_ADV_FLAGS_LE_ONLY_GENERAL_DISC_MODE;

    init.srdata.uuids_complete.uuid_cnt = sizeof(m_adv_uuids) / sizeof(m_adv_uuids[0]);
    init.srdata.uuids_complete.p_uuids  = m_adv_uuids;

    init.config.ble_adv_fast_enabled  = true;
    init.config.ble_adv_fast_interval = APP_ADV_INTERVAL;
    init.config.ble_adv_fast_timeout  = APP_ADV_DURATION;
    init.evt_handler = on_adv_evt;

    err_code = ble_advertising_init(&m_advertising, &init);
    APP_ERROR_CHECK(err_code);

    ble_advertising_conn_cfg_tag_set(&m_advertising, APP_BLE_CONN_CFG_TAG);
}

#if FEATURE_SECURE_CONNECTION
/** @brief Delete all bonding (pairing) info from flash */
static void delete_bonds(void)
{
    ret_code_t err_code;
    DBG_PRINTF("[PM] Erase bonds\r\n");
    err_code = pm_peers_delete();
    APP_ERROR_CHECK(err_code);
}

/**
 * @brief Start BLE advertising (secure mode)
 *
 * If erase_bonds_flag is true, delete all bond info first, then
 * start advertising from PM_EVT_PEERS_DELETE_SUCCEEDED event.
 * If false, start Fast mode advertising immediately.
 */
static void advertising_start(bool erase_bonds_flag)
{
    if (erase_bonds_flag == true)
    {
        DBG_PRINTF("[BOND] Delete requested (count_before=%lu)\r\n", (unsigned long)pm_peer_count());

        /* Clear bond delete request flag in flash -> prevent re-deletion on next boot */
        bond_data_delete = false;
        m_config.bond_data_delete = 0;
        config_save();

        delete_bonds();    /* After deletion, advertising starts from pm_evt_handler */
    }
    else
    {
        ret_code_t err_code = ble_advertising_start(&m_advertising, BLE_ADV_MODE_FAST);
        APP_ERROR_CHECK(err_code);
    }
}
#else
/** @brief Start BLE advertising (non-secure mode, immediate Fast advertising) */
static void advertising_start(void)
{
    uint32_t err_code = ble_advertising_start(&m_advertising, BLE_ADV_MODE_FAST);
    APP_ERROR_CHECK(err_code);
}
#endif

/*==============================================================================
 * BLE Security (Peer Manager)
 * LESC/static passkey MITM protection, bonding management
 *============================================================================*/
#if FEATURE_SECURE_CONNECTION
/**
 * @brief Peer Manager event handler
 *
 * Handles BLE security events:
 *   - CONN_SEC_SUCCEEDED: security established -> verify MITM then activate connection
 *   - CONN_SEC_FAILED: security failed -> retry on PIN/key missing
 *   - PEERS_DELETE_SUCCEEDED: bond deletion complete -> restart advertising
 *   - CONN_SEC_CONFIG_REQ: allow re-pairing config
 *   - PEER_DATA_UPDATE_SUCCEEDED: peer data updated -> save address, update reset status
 */
static void pm_evt_handler(pm_evt_t const * p_evt)
{
    pm_peer_data_bonding_t peer_bonding_data;
    uint32_t return_code;
    ret_code_t err_code;

    /* SDK default handler + security processing (SDK handler called inside ble_quick_security) */
    ble_security_quick_pm_handler(p_evt);

    switch (p_evt->evt_id)
    {
        /* Security connection succeeded */
        case PM_EVT_CONN_SEC_SUCCEEDED:
        {
            pm_conn_sec_status_t conn_sec_status;
            err_code = pm_conn_sec_status_get(p_evt->conn_handle, &conn_sec_status);
            APP_ERROR_CHECK(err_code);

            /* Verify MITM protection (always pass in dev mode) */
            if (conn_sec_status.mitm_protected || BLE_DEV_MODE)
            {
                DBG_PRINTF("[PM] Secured\r\n");
                ble_connection_st = 1;       /* Activate connection state */
                battery_timer_start();       /* Start battery monitoring */
            }
            else
            {
                /* MITM not protected -> delete peer and disconnect */
                DBG_PRINTF("[PM] Sec FAIL\r\n");
                err_code = pm_peer_id_get(m_conn_handle, &m_peer_to_be_deleted);
                APP_ERROR_CHECK(err_code);
                err_code = sd_ble_gap_disconnect(m_conn_handle, BLE_HCI_REMOTE_USER_TERMINATED_CONNECTION);
                APP_ERROR_CHECK(err_code);
            }
        }
        break;

        /* Security connection failed (retry/disconnect handled by ble_quick_security) */
        case PM_EVT_CONN_SEC_FAILED:
            DBG_PRINTF("[PM] Sec failed\r\n");
            break;

        /* All bonds deleted -> restart advertising only if no active connection */
        case PM_EVT_PEERS_DELETE_SUCCEEDED:
            DBG_PRINTF("[BOND] Deleted all (count_after=%lu)\r\n", (unsigned long)pm_peer_count());
            if (m_conn_handle == BLE_CONN_HANDLE_INVALID)
            {
                advertising_start(false);
            }
            break;

        /* PM_EVT_CONN_SEC_CONFIG_REQ -> handled by ble_quick_security */

        /* Peer data (bonding info) update succeeded -> save peer BLE address */
        case PM_EVT_PEER_DATA_UPDATE_SUCCEEDED:
        {
            return_code = pm_peer_data_bonding_load(p_evt->peer_id, &peer_bonding_data);
            if (return_code == NRF_SUCCESS)
            {
                memcpy(c_addr, peer_bonding_data.peer_ble_id.id_addr_info.addr, sizeof(c_addr));

                /* Log peer ID and address when new bond is created */
                if (p_evt->params.peer_data_update_succeeded.data_id == PM_PEER_DATA_ID_BONDING)
                {
                    DBG_PRINTF("[BOND] Created peer_id=%u addr=%02X:%02X:%02X:%02X:%02X:%02X (count=%lu)\r\n",
                               p_evt->peer_id, c_addr[5], c_addr[4], c_addr[3], c_addr[2], c_addr[1], c_addr[0],
                               (unsigned long)pm_peer_count());
                }
            }
            m_reset_status = 10;   /* Bond complete status */
        }
        break;

        default:
            break;
    }
}

/**
 * @brief Initialize Peer Manager
 *
 * Initialize BLE security module and register event handler.
 * Security level determined by BLE_DEV_MODE (0=passkey required, 1=no security).
 */
static void peer_manager_init(void)
{
    ret_code_t err_code;
    
    ble_security_quick_init(BLE_DEV_MODE);
    
    err_code = pm_register(pm_evt_handler);
    APP_ERROR_CHECK(err_code);
    
    DBG_PRINTF("[PM] Init (mode=%d)\r\n", BLE_DEV_MODE);
}
#endif

/*==============================================================================
 * BLE Event Handler
 * Handles all BLE events from the SoftDevice.
 *============================================================================*/

/**
 * @brief BLE event handler (SoftDevice observer)
 *
 * Key events handled:
 *   - DISCONNECTED: connection lost -> device sleep, state reset
 *   - CONNECTED: connection established -> assign QWR handle, set TX power +8dBm
 *   - PHY_UPDATE_REQUEST: auto-accept PHY update
 *   - TIMEOUT: connection/GATT timeout -> force disconnect
 *   - SEC_PARAMS_REQUEST: security parameter request (reject if security unused)
 *   - PASSKEY_DISPLAY: display passkey (debug log)
 *   - AUTH_KEY_REQUEST: respond with static passkey
 *   - HVN_TX_COMPLETE: TX complete -> clear transmission flag
 */
static void ble_evt_handler(ble_evt_t const * p_ble_evt, void * p_context)
{
    uint32_t err_code;
#if FEATURE_STATIC_PASSKEY
    ble_gap_evt_t const * p_gap_evt = &p_ble_evt->evt.gap_evt;
#endif

#if FEATURE_SECURE_CONNECTION
    if (!ble_security_is_dev_mode())
    {
        pm_handler_secure_on_connection(p_ble_evt);
    }
#endif

    switch (p_ble_evt->header.evt_id)
    {
        /* BLE connection lost */
        case BLE_GAP_EVT_DISCONNECTED:
        {
            uint8_t disc_reason = p_ble_evt->evt.gap_evt.params.disconnected.reason;

            /* Determine unintended disconnect:
             *   0x08 supervision timeout, 0x22 LMP response timeout,
             *   0x3D MIC failure,         0x3E connection failed to be established
             * -> maintain unlimited advertising + block sleep entry
             */
            bool unintended_disc = (disc_reason == BLE_HCI_CONNECTION_TIMEOUT)                ||
                                   (disc_reason == BLE_HCI_STATUS_CODE_LMP_RESPONSE_TIMEOUT)  ||
                                   (disc_reason == BLE_HCI_CONN_TERMINATED_DUE_TO_MIC_FAILURE)||
                                   (disc_reason == BLE_HCI_CONN_FAILED_TO_BE_ESTABLISHED);

            DBG_PRINTF("[BLE] Disconnected (reason 0x%02X)%s\r\n",
                       disc_reason, unintended_disc ? " [UNINTENDED]" : "");

            ble_connection_st = 0;
            m_conn_handle = BLE_CONN_HANDLE_INVALID;
            m_tx_in_progress = false;

            maa_async_abort();  // Prevent hang caused by async measurement state
            s_tx_pending = false;  // Clear pending TX

            /* Adjust advertising duration at runtime based on disconnect reason */
            (void)sd_ble_gap_adv_stop(m_advertising.adv_handle);
            m_advertising.adv_modes_config.ble_adv_fast_timeout =
                unintended_disc ? APP_ADV_DURATION_UNLIMITED : APP_ADV_DURATION_NORMAL;

            ret_code_t adv_err = ble_advertising_start(&m_advertising, BLE_ADV_MODE_FAST);
            if (adv_err != NRF_SUCCESS && adv_err != NRF_ERROR_INVALID_STATE)
            {
                APP_ERROR_CHECK(adv_err);
            }

            /* Unintended disconnect: block sleep entry to ensure continuous advertising */
            if (!unintended_disc && device_status == true)
            {
                if (device_sleep_mode() == 0)
                {
                    device_status = false;
                }
            }
        }
        break;

        case BLE_GAP_EVT_CONNECTED:
            DBG_PRINTF("[BLE] Connected\r\n");
            
#if FEATURE_SECURE_CONNECTION
            ble_connection_st = 1;
            battery_timer_start();
#endif
            
            m_conn_handle = p_ble_evt->evt.gap_evt.conn_handle;
            err_code = nrf_ble_qwr_conn_handle_assign(&m_qwr, m_conn_handle);
            APP_ERROR_CHECK(err_code);
            
            sd_ble_gap_tx_power_set(BLE_GAP_TX_POWER_ROLE_CONN, m_conn_handle, 4);

            // Request 2M PHY (falls back to 1M automatically if unsupported)
            {
                ble_gap_phys_t const phys = {
                    .rx_phys = BLE_GAP_PHY_2MBPS,
                    .tx_phys = BLE_GAP_PHY_2MBPS,
                };
                err_code = sd_ble_gap_phy_update(m_conn_handle, &phys);
                APP_ERROR_CHECK(err_code);
            }

            led_set_state(LED_STATE_OFF);  /* Connection complete -> LED OFF */

            break;

        case BLE_GAP_EVT_PHY_UPDATE_REQUEST:
        {
            ble_gap_phys_t const phys = {
                .rx_phys = BLE_GAP_PHY_AUTO,
                .tx_phys = BLE_GAP_PHY_AUTO,
            };
            err_code = sd_ble_gap_phy_update(p_ble_evt->evt.gap_evt.conn_handle, &phys);
            APP_ERROR_CHECK(err_code);
        }
        break;

        case BLE_GAP_EVT_PHY_UPDATE:
        {
            ble_gap_evt_phy_update_t const * p_phy = &p_ble_evt->evt.gap_evt.params.phy_update;
            DBG_PRINTF("[BLE] PHY updated: TX=%s, RX=%s\r\n",
                p_phy->tx_phy == BLE_GAP_PHY_2MBPS ? "2M" : "1M",
                p_phy->rx_phy == BLE_GAP_PHY_2MBPS ? "2M" : "1M");
        }
        break;

        case BLE_GATTC_EVT_TIMEOUT:
        case BLE_GATTS_EVT_TIMEOUT:
            DBG_PRINTF("[BLE] GATT Timeout -> disconnect\r\n");
            err_code = sd_ble_gap_disconnect(p_ble_evt->evt.gap_evt.conn_handle,
                                             BLE_HCI_REMOTE_USER_TERMINATED_CONNECTION);
            APP_ERROR_CHECK(err_code);
            break;

        case BLE_GAP_EVT_SEC_PARAMS_REQUEST:
#if !FEATURE_SECURE_CONNECTION
            err_code = sd_ble_gap_sec_params_reply(m_conn_handle,
                                                   BLE_GAP_SEC_STATUS_PAIRING_NOT_SUPP,
                                                   NULL, NULL);
            APP_ERROR_CHECK(err_code);
#else
            if (ble_security_is_dev_mode()) {
                err_code = sd_ble_gap_sec_params_reply(m_conn_handle,
                                                       BLE_GAP_SEC_STATUS_PAIRING_NOT_SUPP,
                                                       NULL, NULL);
                APP_ERROR_CHECK(err_code);
                DBG_PRINTF("DEV: Rejected security request\r\n");
            }
#endif
            break;

#if FEATURE_SECURE_CONNECTION
        case BLE_GAP_EVT_PASSKEY_DISPLAY:
        {
            char passkey[PASSKEY_LENGTH + 1];
            memcpy(passkey, p_ble_evt->evt.gap_evt.params.passkey_display.passkey, PASSKEY_LENGTH);
            passkey[PASSKEY_LENGTH] = 0;
            DBG_PRINTF("[BLE] Passkey: %s\r\n", passkey);
        }
        break;

        case BLE_GAP_EVT_AUTH_KEY_REQUEST:
#if FEATURE_STATIC_PASSKEY
            if (p_gap_evt->params.auth_key_request.key_type == BLE_GAP_AUTH_KEY_TYPE_PASSKEY)
            {
                err_code = sd_ble_gap_auth_key_reply(p_gap_evt->conn_handle,
                                                     BLE_GAP_AUTH_KEY_TYPE_PASSKEY,
                                                     (const uint8_t *)m_static_passkey);
                APP_ERROR_CHECK(err_code);
            }
#endif
            break;

        case BLE_GAP_EVT_LESC_DHKEY_REQUEST:
            break;
#endif

        case BLE_GATTS_EVT_HVN_TX_COMPLETE:
            m_tx_in_progress = false;
            m_tx_complete_pending = false;  /* Notify waiting functions of TX completion */
            break;

        default:
            break;
    }
}


/*==============================================================================
 * Utility Functions
 *============================================================================*/

/** @brief LED init + erase_bonds default (boot Phase 5) */
static void buttons_leds_init(bool * p_erase_bonds)
{
    led_init();
    *p_erase_bonds = false;
}

static void log_init(void)
{
    ret_code_t err_code = NRF_LOG_INIT(NULL);
    APP_ERROR_CHECK(err_code);
    NRF_LOG_DEFAULT_BACKENDS_INIT();
}

/**
 * @brief UART receive event handler
 *
 * Accumulates bytes received from physical UART (1Mbps) into a buffer.
 * On receiving '\n' or '\r', passes data to received_command_process().
 * Used for debug and factory testing.
 */
void uart_event_handle(app_uart_evt_t * p_event)
{
    static uint8_t data_array[BLE_NUS_MAX_DATA_LEN] = {0};
    static uint8_t index = 0;

    switch (p_event->evt_type)
    {
        case APP_UART_DATA_READY:
            UNUSED_VARIABLE(app_uart_get(&data_array[index]));
            index++;

            if ((data_array[index - 1] == '\n') || (data_array[index - 1] == '\r') || (index >= m_ble_nus_max_data_len))
            {
                if (index > 1)
                {
                    cmd_type_t = CMD_UART;
                    ble_got_new_data = true;
                    dr_cmd_parser(data_array, index);
                }
                index = 0;
            }
            break;

        case APP_UART_COMMUNICATION_ERROR:
            break;

        case APP_UART_FIFO_ERROR:
            APP_ERROR_HANDLER(p_event->data.error_code);
            break;

        default:
            break;
    }
}

/** @brief UART init (1Mbps, no flow control, no parity) -- currently unused (power saving) */
#if 0
static void uart_init(void)
{
    uint32_t err_code;
    app_uart_comm_params_t const comm_params =
    {
        .rx_pin_no    = RX_PIN_NUMBER,
        .tx_pin_no    = TX_PIN_NUMBER,
        .rts_pin_no   = RTS_PIN_NUMBER,
        .cts_pin_no   = CTS_PIN_NUMBER,
        .flow_control = APP_UART_FLOW_CONTROL_DISABLED,
        .use_parity   = false,
#if defined (UART_PRESENT)
        .baud_rate    = NRF_UART_BAUDRATE_1000000
#else
        .baud_rate    = NRF_UARTE_BAUDRATE_1000000
#endif
    };

    APP_UART_FIFO_INIT(&comm_params,
                       UART_RX_BUF_SIZE,
                       UART_TX_BUF_SIZE,
                       uart_event_handle,
                       APP_IRQ_PRIORITY_LOWEST,
                       err_code);
    APP_ERROR_CHECK(err_code);
}
#endif

static void power_management_init(void)
{
    ret_code_t err_code;
    err_code = nrf_pwr_mgmt_init();
    APP_ERROR_CHECK(err_code);
}

/**
 * @brief Idle state processing
 *
 * Process BLE security (LESC) requests, flush logs, then
 * put the CPU into low-power state via nrf_pwr_mgmt_run().
 * Automatically wakes on BLE events.
 */
static void idle_state_handle(void)
{
#if FEATURE_SECURE_CONNECTION
    ret_code_t err_code;
    err_code = nrf_ble_lesc_request_handler();
    APP_ERROR_CHECK(err_code);
#endif

    if (NRF_LOG_PROCESS() == false)
    {
        nrf_pwr_mgmt_run();
    }
}

void assert_nrf_callback(uint16_t line_num, const uint8_t * p_file_name)
{
    app_error_handler(DEAD_BEEF, line_num, p_file_name);
}

/**
 * @brief Enter sleep mode
 *
 * Indicate to user via LED, then start 3s (POWER_OFF_DELAY) wait timer.
 * On timer expiry, t_power_off_timeout_handler() cuts physical power.
 */
void sleep_mode_enter(void)
{
    led_set_state(LED_STATE_POWER_OFF);
    DBG_PRINTF("[SYS] Sleep\r\n");
    APP_ERROR_CHECK(app_timer_start(m_power_off_delay_timer_id, APP_TIMER_TICKS(POWER_OFF_DELAY), NULL));
}

/**
 * @brief Device power off
 *
 * Indicate to user via LED, then start 3s wait timer.
 * Cuts physical power using the same mechanism as sleep_mode_enter().
 */
void device_power_off(void)
{
    led_set_state(LED_STATE_POWER_OFF);
    APP_ERROR_CHECK(app_timer_start(m_power_off_delay_timer_id, APP_TIMER_TICKS(POWER_OFF_DELAY), NULL));
}

/*==============================================================================
 * Data Transmission Functions
 * Transmit sensor data/responses to the smartphone app via BLE NUS.
 * Packet structure: [data][CRC16 2 bytes]
 *============================================================================*/

/**
 * @brief ASCII text BLE transmission
 *
 * Copy string content up to '\r', append CRC16, and send via BLE.
 * Returns immediately if a transmission is already in progress (m_tx_in_progress).
 */
void data_tx_handler(char const *p_data_to_send)
{
    if (m_tx_in_progress)
    {
        return;
    }

    char const *p_end_char = strchr(p_data_to_send, '\r');
    if (p_end_char == NULL)
    {
        return;
    }

    uint16_t data_len = p_end_char - p_data_to_send;
    if (data_len > (BLE_NUS_MAX_DATA_LEN - 2))
    {
        return;
    }

    memcpy(m_tx_buffer, p_data_to_send, data_len);
    uint16_t crc = crc16_compute(m_tx_buffer, data_len, NULL);
    m_tx_buffer[data_len]     = (uint8_t)(crc & 0xFF);
    m_tx_buffer[data_len + 1] = (uint8_t)((crc >> 8) & 0xFF);
    m_tx_len = data_len + 2;

    if (ble_connection_st == BLE_CONNECTED_ST)
    {
        m_tx_in_progress = true;
        uint32_t err_code = ble_nus_data_send(&m_nus, m_tx_buffer, &m_tx_len, m_conn_handle);

        if (err_code == NRF_SUCCESS)
        {
            // OK
        }
        else if (err_code == NRF_ERROR_RESOURCES)
        {
            // Retry later
        }
        else if (err_code == NRF_ERROR_INVALID_STATE || err_code == NRF_ERROR_NOT_FOUND)
        {
            ble_connection_st = BLE_DISCONNECTED_ST;
            m_conn_handle = BLE_CONN_HANDLE_INVALID;
            m_tx_in_progress = false;
        }
        else
        {
            DBG_PRINTF("[BLE TX] Err:0x%X\r\n", err_code);  // Log instead of APP_ERROR_CHECK - jhChun 26.03.16
            m_tx_in_progress = false;
            //APP_ERROR_CHECK(err_code);
        }
    }
}

/**
 * @brief Format single uint16_t value into binary packet
 *
 * Packet structure: [tag 4 bytes][value 2 bytes] = 6 bytes total (3 words).
 * Tag is 4 ASCII chars (e.g. "rta:", "rsn:"), value stored big-endian.
 */
void single_format_data(uint8_t *buffer, const char *tag, const uint16_t value)
{
    uint16_t tag1 = ((uint16_t)tag[1] << 8) | (uint16_t)tag[0];
    uint16_t tag2 = ((uint16_t)tag[3] << 8) | (uint16_t)tag[2];

    buffer[0] = (uint8_t)(tag1 & 0xFF);
    buffer[1] = (uint8_t)((tag1 >> 8) & 0xFF);
    buffer[2] = (uint8_t)(tag2 & 0xFF);
    buffer[3] = (uint8_t)((tag2 >> 8) & 0xFF);
    buffer[5] = (uint8_t)(value & 0xFF);
    buffer[4] = (uint8_t)((value >> 8) & 0xFF);
}

/**
 * @brief Format uint16_t array into binary packet
 *
 * Packet structure: [tag 4 bytes][data0 2 bytes][data1 2 bytes]...
 * Each value stored big-endian; length is array element count, not byte count.
 */
void format_data(uint8_t *buffer, const char *tag, const uint16_t *data_array, size_t length)
{
    uint16_t tag1 = ((uint16_t)tag[1] << 8) | (uint16_t)tag[0];
    uint16_t tag2 = ((uint16_t)tag[3] << 8) | (uint16_t)tag[2];

    buffer[0] = (uint8_t)(tag1 & 0xFF);
    buffer[1] = (uint8_t)((tag1 >> 8) & 0xFF);
    buffer[2] = (uint8_t)(tag2 & 0xFF);
    buffer[3] = (uint8_t)((tag2 >> 8) & 0xFF);

    for (size_t i = 0; i < length; i++)
    {
        buffer[4 + i * 2 + 1] = (uint8_t)(data_array[i] & 0xFF);
        buffer[4 + i * 2]     = (uint8_t)((data_array[i] >> 8) & 0xFF);
    }
}

/**
 * @brief Format uint8_t byte array into binary packet
 *
 * Packet structure: [tag 4 bytes][byte0][byte1]...
 */
void format_data_byte(uint8_t *buffer, const char *tag, const uint8_t *data_array, size_t length)
{
    uint16_t tag1 = ((uint16_t)tag[1] << 8) | (uint16_t)tag[0];
    uint16_t tag2 = ((uint16_t)tag[3] << 8) | (uint16_t)tag[2];

    buffer[0] = (uint8_t)(tag1 & 0xFF);
    buffer[1] = (uint8_t)((tag1 >> 8) & 0xFF);
    buffer[2] = (uint8_t)(tag2 & 0xFF);
    buffer[3] = (uint8_t)((tag2 >> 8) & 0xFF);

    for (size_t i = 0; i < length; i++)
    {
        buffer[4 + i] = (uint8_t)(data_array[i] & 0xFF);
    }
}

/**
 * @brief Format ASCII string into binary packet
 *
 * Packet structure: [tag 4 bytes][char0][char1]...
 * Used for transmitting serial number, passkey, firmware version, etc.
 */
void ascii_format_data(uint8_t *buffer, const char *tag, const char *data_ascii, size_t length)
{
    uint16_t tag1 = ((uint16_t)tag[1] << 8) | (uint16_t)tag[0];
    uint16_t tag2 = ((uint16_t)tag[3] << 8) | (uint16_t)tag[2];

    buffer[0] = (uint8_t)(tag1 & 0xFF);
    buffer[1] = (uint8_t)((tag1 >> 8) & 0xFF);
    buffer[2] = (uint8_t)(tag2 & 0xFF);
    buffer[3] = (uint8_t)((tag2 >> 8) & 0xFF);

    for (size_t i = 0; i < length; i++)
    {
        buffer[4 + i] = (uint8_t)(data_ascii[i] & 0xFF);
    }
}

/**
 * @brief BLE binary safe transmission (preamble for dr_binary_tx_safe)
 *
 * On NRF_ERROR_RESOURCES (TX queue full), retries at 5ms intervals.
 * Returns gracefully on connection errors.
 *
 * @param ble_bin_buff Data buffer to transmit
 * @param length uint16_t word count (actual bytes = length * 2)
 */

/**
 * @brief Safe delay for BLE operations
 *
 * Uses interrupt-safe nrf_delay_ms().
 * BLE stack runs on interrupts, so TX complete events are processed during delay.
 *
 * Note: Do NOT use __WFE() or sd_app_evt_wait() here!
 * It conflicts with SoftDevice power management.
 *
 * @param ms Delay time in milliseconds
 */
void dr_sd_delay_ms(uint32_t ms)
{
    if (ms == 0) return;
    nrf_delay_ms(ms);
}

extern int SEGGER_RTT_vprintf(unsigned, const char *, va_list *);
static void log_printf(const char *fmt, ...)
{
    va_list args;
    va_start(args, fmt);
    SEGGER_RTT_vprintf(0, fmt, &args);
    va_end(args);
}

void param_error(const char *cmd)
{
    char resp_error[4];
    resp_error[0] = 'r';
    resp_error[1] = cmd[1];
    resp_error[2] = cmd[2];
    resp_error[3] = '!';

    single_format_data(ble_bin_buffer, resp_error, 65533);  /* err_code3 */
    dr_binary_tx_safe(ble_bin_buffer, 3);
}

void dr_binary_tx_safe(uint8_t const *ble_bin_buff, uint16_t length)
{
    uint32_t err_code;
    static uint8_t tx_buffer[BLE_NUS_MAX_DATA_LEN] = {0};
    uint16_t retry_count = 0;
    const uint16_t MAX_RETRIES = 20;  /* Max retries: 2ms x 20 = 40ms (was 5ms x 100 = 500ms) */

    if (ble_connection_st == 0) return;

    if (length * sizeof(uint16_t) > (BLE_NUS_MAX_DATA_LEN - 2)) return;

    if (ble_connection_st == BLE_CONNECTED_ST)
    {
        memcpy(tx_buffer, ble_bin_buff, length * sizeof(uint16_t));
        uint16_t crc = crc16_compute(tx_buffer, length * sizeof(uint16_t), NULL);
        tx_buffer[length * sizeof(uint16_t)]     = (uint8_t)(crc & 0xFF);
        tx_buffer[length * sizeof(uint16_t) + 1] = (uint8_t)((crc >> 8) & 0xFF);

        uint16_t total_len = length * sizeof(uint16_t) + 2;

        do
        {
            uint16_t send_len = total_len;
            err_code = ble_nus_data_send(&m_nus, tx_buffer, &send_len, m_conn_handle);

            if (err_code == NRF_SUCCESS)
            {
                return;
            }
            else if (err_code == NRF_ERROR_RESOURCES)
            {
                nrf_delay_ms(2);  /* Was 5ms -> 2ms */
                retry_count++;
            }
            else if (err_code == NRF_ERROR_INVALID_STATE || err_code == NRF_ERROR_NOT_FOUND)
            {
                DBG_PRINTF("[BLE TX] Disconnected\r\n");
                return;
            }
            else
            {
                DBG_PRINTF("[BLE TX] Err:0x%X\r\n", err_code);
                return;
            }
        } while (retry_count < MAX_RETRIES);

        if (retry_count >= MAX_RETRIES)
        {
            DBG_PRINTF("[BLE TX] FAIL %u retries\r\n", retry_count);
        }
    }
}

/*==============================================================================
 * Power Button State Machine (main_s)
 *
 * Called repeatedly by a 5ms single-shot timer (m_power_on_delay_timer).
 *
 * [Boot phase] (booted == false)
 *   - cnt_s < 400 (< 2s): short press -> power OFF
 *   - cnt_s >= 400 (~2s): normal boot sequence start
 *   - cnt_s > 1000 (~5s): factory reset (passkey reset + power OFF)
 *   - m_reset_status == 2: reboot after software reset
 *
 * [Running phase] (booted == true)
 *   - Button pressed 2s (cnt_s >= 400): power OFF
 *   - Button released: reset counter, keep polling
 *============================================================================*/
static bool booted = false;

static void main_s(void * p_context)
{
    UNUSED_PARAMETER(p_context);
    APP_ERROR_CHECK(app_timer_stop(m_power_on_delay_timer_id));

    bool button_released = nrf_gpio_pin_read(POWER_BUTTON);

    /* ---- Running phase: post-boot button polling ---- */
    if (booted)
    {
        if (!button_released)
        {
            cnt_s++;
            if (cnt_s == 400)   /* 400 x 5ms = 2s */
            {
                DBG_PRINTF("[BTN] Power OFF\r\n");
                led_set_state(LED_STATE_POWER_OFF);
                go_device_power_off = true;
                main_timer_start();
                return;
            }
        }
        else
        {
            cnt_s = 0;
        }
        timers_start();
        return;
    }

    /* ---- Boot phase ---- */
    if (button_released)
    {
        if ((cnt_s < 250) && (m_reset_status != 2))
        {
            led_set_state(LED_STATE_OFF);
            power_control_handler(OFF);
            cnt_s = 0;
        }
        else if (cnt_s >= 3000)  /* 3000 x 5ms = 15s */
        {
            DBG_PRINTF("[BTN] Bond Delete\r\n");
            power_control_handler(ON);
            nrf_delay_ms(100);
            bond_data_delete = true;
            m_config.bond_data_delete = (uint8_t)bond_data_delete;
            config_save();
            nrf_delay_ms(1000);
            go_device_power_off = true;
            main_timer_start();
        }
        else if (cnt_s > 250 || (m_reset_status == 2))  /* 250 x 5ms = 1.25s + α */
        {
            DBG_PRINTF("[BTN] Boot (cnt=%d)\r\n", cnt_s);
            device_reset = false;

            power_control_handler(ON);
            battery_timer_start();

#if FEATURE_SECURE_CONNECTION
            /* DEV mode: bonds already deleted in ble_security_quick_init() -> prevent duplicate */
            advertising_start(!ble_security_is_dev_mode() && (erase_bonds || bond_data_delete));
#else
            advertising_start();
#endif
            DBG_PRINTF("[BOOT] ADV started\r\n");
            m_reset_status = 1;
            DBG_PRINTF("[BOOT] Ready\r\n");

            /* Boot complete -> switch to running phase */
            booted = true;
            cnt_s = 0;
            timers_start();
        }
    }
    else
    {
        cnt_s++;
        device_reset = false;

        if (cnt_s == 250)   /* 250 x 5ms = 1.25s + α */
        {
            led_set_state(LED_STATE_POWER_ON);
            DBG_PRINTF("[BTN] 2.0s\r\n");
        }

        timers_start();
    }
}

/*==============================================================================
 * Main Function
 *
 * Performs system initialization in phases (Phase 0~9), then enters infinite loop.
 * In the loop, idle_state_handle() keeps the CPU in low-power state,
 * waking on BLE/timer interrupts to process events.
 *============================================================================*/
int main(void)
{
    bool erase_bonds_local = false;

    /*──────────────────────────────────────────────────────────────
     *  Phase 1: Basic hardware init (BLE-independent)
     *
     *  - Power self-latch (P0.8 HIGH)
     *  - RTT log output
     *  - GPIO (power button input)
     *  - App timers (power polling, battery, main loop)
     *  - Default config (serial number, passkey)
     *  - Buttons/LEDs
     *────────────────────────────────────────────────────────────*/
    power_hold_init();

    if (power_off_duble_prohibit)
    {
        return 0;
    }
    cnt_s = 0;

    log_init();

    DBG_PRINTF("\r\n========================================\r\n");
    DBG_PRINTF(" Medithings v1.17\r\n");
    DBG_PRINTF("========================================\r\n");

    DBG_PRINTF("[1] HW Init\r\n");
    gpio_init();
    info4 = false;
    timers_init();
    load_default_config();
    buttons_leds_init(&erase_bonds_local);
#if FEATURE_SECURE_CONNECTION
    erase_bonds = erase_bonds_local;
#endif
    DBG_PRINTF("  gpio/timer/config/btn OK\r\n");

    /*──────────────────────────────────────────────────────────────
     *  Phase 2: BLE stack init
     *
     *  - Power management module (WFE/sleep when idle)
     *  - SoftDevice S140 (BLE 5.0 protocol stack)
     *  - DC-DC converter enable (required after SoftDevice)
     *────────────────────────────────────────────────────────────*/
    DBG_PRINTF("[2] BLE Stack\r\n");
    power_management_init();
    ble_stack_init();
    APP_ERROR_CHECK(sd_power_dcdc_mode_set(NRF_POWER_DCDC_ENABLE));
    DBG_PRINTF("  pwr/stack/dcdc OK\r\n");

    /*──────────────────────────────────────────────────────────────
     *  Phase 3: Internal flash config (must init after BLE stack)
     *
     *  - FDS (Flash Data Storage) init
     *  - Read stored config from flash (serial, passkey, piezo, etc.)
     *  - Overwrite defaults with flash values
     *────────────────────────────────────────────────────────────*/
    DBG_PRINTF("[3] FDS\r\n");
    fs_storage_init();
    config_load();
    load_flash_config();
    DBG_PRINTF("  fds OK\r\n");

    /*──────────────────────────────────────────────────────────────
     *  Phase 4: BLE protocol setup
     *
     *  - GAP: device name (serial number), connection params, passkey
     *  - GATT: MTU size negotiation
     *  - Services: NUS (Nordic UART Service), QWR, DFU
     *  - Advertising: advertising data (name, UUID)
     *  - Connection parameter negotiation module
     *  - Security: Peer Manager (bonding/passkey)
     *────────────────────────────────────────────────────────────*/
    DBG_PRINTF("[4] BLE Protocol\r\n");
    gap_params_init();
    gatt_init();
    services_init();
    advertising_init();
    conn_params_init();
#if FEATURE_SECURE_CONNECTION
    peer_manager_init();

    /* Log bond state at boot time */
    {
        uint32_t bond_count = pm_peer_count();
        DBG_PRINTF("[BOOT] Bond state: count=%lu flag_delete=%d erase_bonds=%d\r\n",
                   (unsigned long)bond_count, bond_data_delete, erase_bonds);
        if (bond_count > 0)
        {
            pm_peer_id_t pid = pm_next_peer_id_get(PM_PEER_ID_INVALID);
            while (pid != PM_PEER_ID_INVALID)
            {
                DBG_PRINTF("[BOOT] Bonded peer id=%u\r\n", pid);
                pid = pm_next_peer_id_get(pid);
            }
        }
    }
#endif
    DBG_PRINTF("  gap/gatt/svc/adv/conn/sec OK\r\n");

    if (m_need_save_defaults)
    {
        config_save();
        m_need_save_defaults = false;
        DBG_PRINTF("  fds defaults saved\r\n");
    }

    /*──────────────────────────────────────────────────────────────
     *  Phase 5: Application init
     *
     *  - Command parser: process BLE received commands (log, BLE TX, CRC)
     *  - Piezo driver: GPIO/Timer/PPI setup for ultrasound measurement
     *  - IMU (ICM42670P) is NOT initialized here
     *    -> imu_read_direct() self-configures/reads/sleeps on each msp? command
     *────────────────────────────────────────────────────────────*/
    DBG_PRINTF("[5] App\r\n");
    g_plat.log       = log_printf;
    g_plat.tx_bin    = dr_binary_tx_safe;
    g_plat.crc_check = true;
    g_log_enable     = true;
    cmd_table_init();   /* Inject command table into parser */
    dr_piezo_init();
    DBG_PRINTF("  parser/piezo OK\r\n");

    /*──────────────────────────────────────────────────────────────
     *  Boot complete -> start power button state machine
     *────────────────────────────────────────────────────────────*/
    DBG_PRINTF("\r\n========================================\r\n");
    DBG_PRINTF(" READY [%s]\r\n", SERIAL_NO);
    DBG_PRINTF("========================================\r\n\r\n");

    timers_start();

    // MAIN LOOP
    for (;;)
    {
        /* Process command received in BLE callback here (prevents blocking) */
        if (pending_cmd_len > 0)
        {
            uint8_t len = pending_cmd_len;
            pending_cmd_len = 0;
            dr_cmd_parser((const uint8_t *)pending_cmd_buf, len);
        }

        idle_state_handle();
    }
}
