BLE DEVICE I AM USING IS NOT GOING TO SLEEP WHY IT IS NOT GOING TO SLEEP .THE COMPONENTS THT ARE CONECTED TO THE DEVICE ARE INA226AIDGST,WS2812B-MINI,MDBT50Q-1MV2, THESE ARE THE COMPONENTS I AM USING AND ALSO THE DEVICE IS NOT GOING TO SLEEP WHY

int main(void) {
bool erase_bonds;
// Initialize.
log_init();
timers_init();
buttons_leds_init(&erase_bonds);
power_management_init();
// NRF_LOG_INFO("before the blestack function.");
ble_stack_init();
ret_code_t err_code = sd_power_dcdc_mode_set(NRF_POWER_DCDC_ENABLE);
APP_ERROR_CHECK(err_code);
gap_params_init();
gatt_init();
services_init();
advertising_init();
err_code = sd_ble_gap_tx_power_set(BLE_GAP_TX_POWER_ROLE_ADV, m_advertising.adv_handle, -8);
APP_ERROR_CHECK(err_code);
conn_params_init();
peer_manager_init();
nrf_delay_ms(200);
//twi_init();
ws2812_init();
//ina226_init();
application_timers_start();
advertising_start(erase_bonds);
NRF_LOG_INFO("After all initialization");
//sht41_read();
//process_data();
int cnt = 0;
bool interval_changed = false;
for (;;) {
if (updtmrexp) {
//check_why_i_woke_up();
updtmrexp = false;
g_do_sensor_read = false;
if (tx_enable == 1) {
NRF_LOG_INFO("TX enabled -> reading sensor");
//sht41_read();
//battery_level_update(70);
//process_data();
} else {
NRF_LOG_WARNING("TX disabled -> skipping sensor read");
}
if (g_interval_changed) {
g_interval_changed = false;
// Prevent invalid interval (safety guard)
if (new_tx_interval == 0)
new_tx_interval = 8; // fallback to 8 seconds
tx_interval = new_tx_interval;
NRF_LOG_INFO("Applying new tx_interval: %d sec", tx_interval);
uint32_t ticks = APP_TIMER_TICKS(tx_interval * 1000UL);
ret_code_t err;
// === SAFE STOP ===
err = app_timer_stop(m_timer_id);
if (err != NRF_SUCCESS && err != NRF_ERROR_INVALID_STATE) {
NRF_LOG_WARNING("app_timer_stop() failed: 0x%08X", err);
// Do NOT reset device in production
}
// === SAFE RESTART ===
err = app_timer_start(m_timer_id, ticks, NULL);
if (err != NRF_SUCCESS) {
NRF_LOG_ERROR("app_timer_start() failed: 0x%08X", err);
// Optional: try again once
nrf_delay_ms(10);
err = app_timer_start(m_timer_id, ticks, NULL);
}
if (err == NRF_SUCCESS) {
NRF_LOG_INFO("Timer successfully restarted with new interval");
} else
NRF_LOG_ERROR("Failed to restart timer!");
}
}
ws2812_set_color(100,0,0); // Cyan
idle_state_handle();
}
}

void check_why_i_woke_up(void) {
for (uint8_t i = 0; i < 48; i++) {
if (NVIC_GetPendingIRQ((IRQn_Type)i)) {
NRF_LOG_INFO("Woke up due to IRQ ID: %d", i);
}
}
}

void nrf_pwr_mgmt_run(void)
{
//__set_FPSCR(__get_FPSCR() & ~(0x0000001F));
// (void) __get_FPSCR();
// NVIC_ClearPendingIRQ(FPU_IRQn);
//__disable_irq();

//ws2812_set_color(0,0,0);
// nrf_delay_ms(300);
PWR_MGMT_FPU_SLEEP_PREPARE();
// ws2812_set_color(0,0,100);
//nrf_delay_ms(300);
PWR_MGMT_SLEEP_LOCK_ACQUIRE();
//ws2812_set_color(100,0,0);
//nrf_delay_ms(300);
PWR_MGMT_CPU_USAGE_MONITOR_SECTION_ENTER();
//ws2812_set_color(100,100,100);
//nrf_delay_ms(300);
// ws2812_set_color(150,150,150);
// cnt=10000;
//while(cnt--);
PWR_MGMT_DEBUG_PIN_SET();
//ws2812_set_color(150,150,0);
// nrf_delay_ms(300);
// ws2812_set_color(150,150,0);
// cnt=10000;
//while(cnt--);

// Wait for an event.
#ifdef SOFTDEVICE_PRESENT
if (nrf_sdh_is_enabled())
{
ret_code_t ret_code = sd_app_evt_wait();
// ws2812_set_color(0,100,100);
//nrf_delay_ms(300);
ASSERT((ret_code == NRF_SUCCESS) || (ret_code == NRF_ERROR_SOFTDEVICE_NOT_ENABLED));
UNUSED_VARIABLE(ret_code);
check_why_i_woke_up();
// NRF_LOG_INFO("AFTER NRF SD APP EVENT WAIT");
ws2812_set_color(100,0,0);
//nrf_delay_ms(600);
}
else
#endif // SOFTDEVICE_PRESENT
{
// Wait for an event.
__WFE();
// Clear the internal event register.
__SEV();
__WFE();
}
NRF_LOG_INFO("after if else of the nrf_sdh_is_enabled");
NRF_LOG_FLUSH();
PWR_MGMT_DEBUG_PIN_CLEAR();
PWR_MGMT_CPU_USAGE_MONITOR_SECTION_EXIT();
PWR_MGMT_SLEEP_LOCK_RELEASE();
}

THIS IS THE CODE PLEASE SOME BODY HELP WITH THIS

Parents
  • Hello,

    The code could use some cleanup. Next time, please use “insert code” to improve formatting, and remove any commented code that you are not using. It would also be helpful if you could explain how you are determining that the device isn't entering sleep. Are you receiving any debug logs from the device?

    Best regards,

    Vidar

  • The current consumption of the board is same like the 1.6Milli amphs and it is not changing so thats how i am knowing that the board did not went into the sleep what do you say about this any improvements please tell m

    #include "app_error.h"
    #include "app_timer.h"
    #include "ble.h"
    #include "ble_advdata.h"
    #include "ble_advertising.h"
    #include "ble_bas.h"
    #include "ble_conn_params.h"
    #include "ble_conn_state.h"
    #include "ble_cus.h"
    #include "ble_dis.h"
    #include "ble_err.h"
    #include "ble_hci.h"
    #include "ble_srv_common.h"
    #include "bsp_btn_ble.h"
    #include "fds.h"
    #include "nordic_common.h"
    #include "nrf.h"
    #include "nrf_ble_gatt.h"
    #include "nrf_ble_gq.h"
    #include "nrf_ble_qwr.h"
    #include "nrf_delay.h"
    #include "nrf_drv_twi.h"
    #include "nrf_gpio.h"
    #include "nrf_log.h"
    #include "nrf_log_ctrl.h"
    #include "nrf_log_default_backends.h"
    #include "nrf_pwr_mgmt.h"
    #include "nrf_sdh.h"
    #include "nrf_sdh_ble.h"
    #include "nrf_sdh_soc.h"
    #include "nrfx_pwm.h"
    #include "pca10056.h"
    #include "peer_manager.h"
    #include "peer_manager_handler.h"
    #include "sensorsim.h"
    #include <math.h>
    #include <stdbool.h>
    #include <stdint.h>
    #include <string.h>
    /*ADDED FOR CUSTOM TEMPERATURE READ CHARACTERSTIC*/
    #include "ble_srv_common.h"
    /*ADDED FOR CUSTOM TEMPERATURE READ CHARACTERSTIC*/
    
    #define TWI_INSTANCE_ID 0
    
    #define I2C_INSTANCE 0
    
    #define INA226_ADDR 0x40
    #define SHT4X_ADDR 0x44
    
    #define TWIM_TIMEOUT 5
    
    #define EMA_ALPHA 0.25f
    #define TREND_WINDOW 10
    
    #define SPIKE_CLAMP 8.0f
    #define TREND_DEADBAND 0.3f
    
    #define RATE_UP 1.0f
    #define RATE_DOWN 2.0f
    
    #define BAT_LOW_LIMIT 3.30f
    #define BAT_FULL 4.20f
    
    volatile uint8_t ack          = 0;
    volatile uint8_t tx_enable    = 1;
    volatile uint32_t tx_interval = 60;
    
    volatile uint8_t new_tx_enable    = 0;
    volatile uint32_t new_tx_interval = 0;
    
    /*
    uint8_t ack = 1;
    uint8_t config_ack = 1;
    
    ble_cus_ack_update(&m_cus, ack);
    ble_cus_config_ack_update(&m_cus, config_ack);
    */
    #define SHT41_ADDR 0x44
    #define SHT41_MEASURE_HIGH_PRECISION 0xFD
    
    static const nrf_drv_twi_t m_twi = NRF_DRV_TWI_INSTANCE(TWI_INSTANCE_ID);
    
    static volatile bool m_xfer_done = false;
    volatile bool g_do_sensor_read   = false;
    volatile bool g_config_received  = false;
    volatile bool g_ack_received     = false;
    volatile bool g_session_done     = false;
    
    volatile bool g_interval_changed = false;
    
    static float ema_soc     = 0;
    static float display_soc = 0;
    static float hist[TREND_WINDOW];
    static uint8_t hist_index = 0;
    static bool initialized   = false;
    
    uint8_t m_buffer[6];
    
    // static bool hts_indication_enabled        = false;
    // static bool humidity_notification_enabled = false;
    
    #define DEVICE_NAME "NORD_003"                   /**< Name of device. Will be included in the advertising data. */
    #define MANUFACTURER_NAME "Nordic Semiconductor" /**< Manufacturer. Will be passed to Device Information Service. */
    #define MODEL_NUM "NRF52832"                     /**< Model number. Will be passed to Device Information Service. */
    #define MANUFACTURER_ID \
      0x1122334455 /**< Manufacturer ID, part of System ID. Will be passed to Device Information Service. */
    #define ORG_UNIQUE_ID \
      0x667788     /**< Organizational Unique ID, part of System ID. Will be passed to Device Information Service. */
    
    #define APP_BLE_OBSERVER_PRIO 3 /**< Application's BLE observer priority. You shouldn't need to modify this value. */
    #define APP_BLE_CONN_CFG_TAG 1  /**< A tag identifying the SoftDevice BLE configuration. */
    #define APP_ADV_INTERVAL 100 /**< The advertising interval (in units of 0.625 ms. This value corresponds to 25 ms). */
    
    #define APP_ADV_DURATION 0   /**< The advertising duration (180 seconds) in units of 10 milliseconds. */
    
    #define BATTERY_LEVEL_MEAS_INTERVAL APP_TIMER_TICKS(2000) /**< Battery level measurement interval (ticks). */
    #define MIN_BATTERY_LEVEL 81  /**< Minimum battery level as returned by the simulated measurement function. */
    #define MAX_BATTERY_LEVEL 100 /**< Maximum battery level as returned by the simulated measurement function. */
    #define BATTERY_LEVEL_INCREMENT                                                                                    \
      1    /**< Value by which the battery level is incremented/decremented for each call to the simulated measurement \
              function. */
    
    #define TEMP_TYPE_AS_CHARACTERISTIC \
      0    /**< Determines if temperature type is given as characteristic (1) or as a field of measurement (0). */
    
    #define MIN_CELCIUS_DEGREES                                                                                          \
      3688 /**< Minimum temperature in celcius for use in the simulated measurement function (multiplied by 100 to avoid \
              floating point arithmetic). */
    #define MAX_CELCIUS_DEGRESS                                                                                          \
      3972 /**< Maximum temperature in celcius for use in the simulated measurement function (multiplied by 100 to avoid \
              floating point arithmetic). */
    #define CELCIUS_DEGREES_INCREMENT                                                                                     \
      36   /**< Value by which temperature is incremented/decremented for each call to the simulated measurement function \
              (multiplied by 100 to avoid floating point arithmetic). */
    
    #define MIN_CONN_INTERVAL                                                                                            \
      MSEC_TO_UNITS(10, UNIT_1_25_MS)                          /**< Minimum acceptable connection interval (0.5 seconds) \
                                                                */
    #define MAX_CONN_INTERVAL MSEC_TO_UNITS(100, UNIT_1_25_MS) /**< Maximum acceptable connection interval (1 second). */
    #define SLAVE_LATENCY 0                                    /**< Slave latency. */
    #define CONN_SUP_TIMEOUT MSEC_TO_UNITS(4000, UNIT_10_MS)   /**< Connection supervisory timeout (4 seconds). */
    
    #define FIRST_CONN_PARAMS_UPDATE_DELAY                                                                 \
      APP_TIMER_TICKS(5000) /**< Time from initiating event (connect or start of indication) to first time \
                               sd_ble_gap_conn_param_update is called (5 seconds). */
    #define NEXT_CONN_PARAMS_UPDATE_DELAY \
      APP_TIMER_TICKS(                    \
          30000) /**< Time between each call to sd_ble_gap_conn_param_update after the first call (30 seconds). */
    #define MAX_CONN_PARAMS_UPDATE_COUNT                                                        \
      3          /**< Number of attempts before giving up the connection parameter negotiation. \
                  */
    
    #define SEC_PARAM_BOND 1                               /**< Perform bonding. */
    #define SEC_PARAM_MITM 0                               /**< Man In The Middle protection not required. */
    #define SEC_PARAM_LESC 0                               /**< LE Secure Connections not enabled. */
    #define SEC_PARAM_KEYPRESS 0                           /**< Keypress notifications not enabled. */
    #define SEC_PARAM_IO_CAPABILITIES BLE_GAP_IO_CAPS_NONE /**< No I/O capabilities. */
    #define SEC_PARAM_OOB 0                                /**< Out Of Band data not available. */
    #define SEC_PARAM_MIN_KEY_SIZE 7                       /**< Minimum encryption key size. */
    #define SEC_PARAM_MAX_KEY_SIZE 16                      /**< Maximum encryption key size. */
    
    #define DEAD_BEEF \
      0xDEADBEEF /**< Value used as error code on stack dump, can be used to identify stack location on stack unwind. */
    /* This code belongs in main.c*/
    #define NOTIFICATION_INTERVAL APP_TIMER_TICKS(1000)
    
    #define DHT11_PIN 17
    
    APP_TIMER_DEF(m_timer_id);
    
    /* This code belongs in main.c*/
    APP_TIMER_DEF(m_notification_timer_id);
    /* This code belongs in main.c*/
    
    /* This code belongs in main.c*/
    
    APP_TIMER_DEF(m_battery_timer_id);  /**< Battery timer. */
    BLE_BAS_DEF(m_bas);                 /**< Structure used to identify the battery service. */
                                        /**< Structure used to identify the health thermometer service. */
    NRF_BLE_GATT_DEF(m_gatt);           /**< GATT module instance. */
    NRF_BLE_QWR_DEF(m_qwr);             /**< Context for the Queued Write module.*/
    BLE_ADVERTISING_DEF(m_advertising); /**< Advertising module instance. */
    
    NRF_BLE_GQ_DEF(m_ble_gatt_queue,    /**< BLE GATT Queue instance. */
                   NRF_SDH_BLE_PERIPHERAL_LINK_COUNT,
                   NRF_BLE_GQ_QUEUE_SIZE);
    
    BLE_CUS_DEF(m_cus);
    
    static uint16_t m_conn_handle = BLE_CONN_HANDLE_INVALID; /**< Handle of the current connection. */
    // static bool m_hts_meas_ind_conf_pending =
    //     false;                                     /**< Flag to keep track of when an indication confirmation is pending.
    //     */
    static sensorsim_cfg_t m_battery_sim_cfg;          /**< Battery Level sensor simulator configuration. */
    static sensorsim_state_t m_battery_sim_state;      /**< Battery Level sensor simulator state. */
    static sensorsim_cfg_t m_temp_celcius_sim_cfg;     /**< Temperature simulator configuration. */
    static sensorsim_state_t m_temp_celcius_sim_state; /**< Temperature simulator state. */
    
    /*ADDED FOR CUSTOM TEMPERATURE READ CHARACTERSTIC*/
    #define CUSTOM_CHAR_UUID_BASE \
      {0x23, 0xD1, 0xBC, 0xEA, 0x5F, 0x78, 0x23, 0x15, 0xDE, 0xEF, 0x12, 0x12, 0x00, 0x00, 0x00, 0x00}
    #define CUSTOM_CHAR_UUID 0x1523
    
    float temperature, humidity;
    
    static ble_uuid_t m_adv_uuids[] = {
        /*  {BLE_UUID_HEALTH_THERMOMETER_SERVICE, BLE_UUID_TYPE_BLE},*/
        {BLE_MYDEVICE_SERVICE_UUID, BLE_UUID_TYPE_VENDOR_BEGIN},    // ✅ ADD THIS
    };
    
    static volatile bool updtmrexp = false;
    
    // YOUR_JOB: Use UUIDs for service(s) used in your application.
    static void advertising_start(bool erase_bonds);
    static void temperature_measurement_send(void);
    
    /**@brief Callback function for asserts in the SoftDevice.
     *
     * @details This function will be called in case of an assert in the SoftDevice.
     *
     * @warning This handler is an example only and does not fit a final product. You need to analyze
     *          how your product is supposed to react in case of Assert.
     * @warning On assert from the SoftDevice, the system can only recover on reset.
     *
     * @param[in] line_num   Line number of the failing ASSERT call.
     * @param[in] file_name  File name of the failing ASSERT call.
     */
    void assert_nrf_callback(uint16_t line_num, const uint8_t* p_file_name) {
      app_error_handler(DEAD_BEEF, line_num, p_file_name);
    }
    void check_why_i_woke_up(void);
    /**@brief Function for handling Service errors.
     *
     * @details A pointer to this function will be passed to each service which may need to inform the
     *          application about an error.
     *
     * @param[in] nrf_error  Error code containing information about what went wrong.
    // */
    // static void service_error_handler(uint32_t nrf_error) {
    //   APP_ERROR_HANDLER(nrf_error);
    // }
    
    static void service_error_handler(uint32_t nrf_error) {
      if (nrf_error == BLE_ERROR_GATTS_SYS_ATTR_MISSING)
        return;
    
      APP_ERROR_HANDLER(nrf_error);
    }
    
    /**@brief Function for handling Peer Manager events.
     *
     * @param[in] p_evt  Peer Manager event.
     */
    static void pm_evt_handler(pm_evt_t const* p_evt) {
      ret_code_t err_code;
      uint8_t temp_value;
      ble_gatts_value_t gatts_value;    // Declare a variable of type ble_gatts_value_t
    
      pm_handler_on_pm_evt(p_evt);
      pm_handler_flash_clean(p_evt);
    
      switch (p_evt->evt_id) {
      case PM_EVT_CONN_SEC_SUCCEEDED:
        break;
        // Rest of your code...
      case PM_EVT_PEERS_DELETE_SUCCEEDED:
        //  advertising_start(false);    // Start advertising when peers are deleted
        break;
    
      default:
        break;
      }
    }
    
    /* This code belongs in main.c*/
    
    /**@brief Function for handling the Custom Service Service events.
     *
     * @details This function will be called for all Custom Service events which are passed to
     *          the application.
     *
     * @param[in]   p_cus_service  Custom Service structure.
     * @param[in]   p_evt          Event received from the Custom Service.
     *
     */
    /* This code belongs in main.c*/
    
    static void on_cus_evt(ble_cus_t* p_cus_service, ble_cus_evt_t* p_evt) {
      switch (p_evt->evt_type) {
      case BLE_CUS_EVT_NOTIFICATION_ENABLED:
    
        break;
    
      case BLE_CUS_EVT_NOTIFICATION_DISABLED:
        break;
    
      case BLE_CUS_EVT_CONNECTED:
        break;
    
      case BLE_CUS_EVT_DISCONNECTED:
        break;
    
      default:
        // No implementation needed.
        break;
      }
    }
    
    /**@brief Function for performing a battery measurement, and update the Battery Level characteristic in the Battery
     * Service.
     */
    static void battery_level_update(uint8_t battery_level) {
      ret_code_t err_code;
      // uint8_t battery_level;
    
      // battery_level = /*(uint8_t)sensorsim_measure(&m_battery_sim_state, &m_battery_sim_cfg)*/88;
    
      err_code = ble_bas_battery_level_update(&m_bas, battery_level, BLE_CONN_HANDLE_ALL);
      if ((err_code != NRF_SUCCESS) && (err_code != NRF_ERROR_INVALID_STATE) && (err_code != NRF_ERROR_RESOURCES) &&
          (err_code != NRF_ERROR_BUSY) && (err_code != BLE_ERROR_GATTS_SYS_ATTR_MISSING)) {
        APP_ERROR_HANDLER(err_code);
      }
    }
    
    /**@brief Function for handling the Battery measurement timer timeout.
     *
     * @details This function will be called each time the battery level measurement timer expires.
     *
     * @param[in] p_context   Pointer used for passing some arbitrary information (context) from the
     *                        app_start_timer() call to the timeout handler.
     */
    static void battery_level_meas_timeout_handler(void* p_context) {
      UNUSED_PARAMETER(p_context);
      battery_level_update(78);
    }
    
    /**@brief Function for populating simulated health thermometer measurement.
     */
    
    /**@brief Function for the Timer initialization.
     *
     * @details Initializes the timer module. This creates and starts application timers.
     */
    
    static void advertising_stop(void) {
      ret_code_t err_code;
    
      err_code = sd_ble_gap_adv_stop(m_advertising.adv_handle);
      NRF_LOG_INFO("The error code after stopped the advertizing is %d\n", err_code);
      // if (err_code != NRF_SUCCESS &&
      //     err_code != NRF_ERROR_INVALID_STATE)
      //{
      APP_ERROR_CHECK(err_code);
      // }
    }
    
    void timer_handler(void* p_context) {
      updtmrexp        = true;
      g_do_sensor_read = true;
      NRF_LOG_INFO("Timer interrupt fired");
      if (m_conn_handle == BLE_CONN_HANDLE_INVALID) {
      }
    }
    
    static void timers_init(void) {
      ret_code_t err_code;
    
      // Initialize timer module.
      err_code = app_timer_init();
      APP_ERROR_CHECK(err_code);
      err_code = app_timer_create(&m_timer_id, APP_TIMER_MODE_REPEATED, timer_handler);
      APP_ERROR_CHECK(err_code);
    }
    
    /**@brief Function for the GAP initialization.
     *
     * @details This function sets up all the necessary GAP (Generic Access Profile) parameters of the
     *          device including the device name, appearance, and the preferred connection parameters.
     */
    static void gap_params_init(void) {
      ret_code_t err_code;
      ble_gap_conn_params_t gap_conn_params;
      ble_gap_conn_sec_mode_t sec_mode;
      BLE_GAP_CONN_SEC_MODE_SET_OPEN(&sec_mode);
    
      err_code = sd_ble_gap_device_name_set(&sec_mode, (const uint8_t*)DEVICE_NAME, strlen(DEVICE_NAME));
      APP_ERROR_CHECK(err_code);
    
      err_code = sd_ble_gap_appearance_set(BLE_APPEARANCE_GENERIC_THERMOMETER);
      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);
    }
    
    /**@brief Function for  initializing the GATT module.
     */
    static void gatt_init(void) {
      ret_code_t err_code = nrf_ble_gatt_init(&m_gatt, NULL);
      APP_ERROR_CHECK(err_code);
    }
    
    /**@brief Function for simulating and sending one Temperature Measurement.
     */
    
    /**@brief Function for handling Queued Write Module errors.
     *
     * @details A pointer to this function will be passed to each service which may need to inform the
     *          application about an error.
     *
     * @param[in]   nrf_error   Error code containing information about what went wrong.
     */
    static void nrf_qwr_error_handler(uint32_t nrf_error) {
      APP_ERROR_HANDLER(nrf_error);
    }
    
    /**@brief Function for initializing services that will be used by the application.
     *
     * @details Initialize the Health Thermometer, Battery and Device Information services.
     */
    
    /*ADDED FOR CUSTOM TEMPERATURE READ CHARACTERSTIC*/
    /*ADDED FOR CUSTOM TEMPERATURE READ CHARACTERSTIC*/
    static void services_init(void) {
      ret_code_t err_code;
      ble_bas_init_t bas_init;
      ble_dis_init_t dis_init;
      nrf_ble_qwr_init_t qwr_init = {0};
      ble_dis_sys_id_t sys_id;
    
      // Initialize Queued Write Module.
      qwr_init.error_handler = nrf_qwr_error_handler;
    
      err_code = nrf_ble_qwr_init(&m_qwr, &qwr_init);
      APP_ERROR_CHECK(err_code);
    
      // Initialize Health Thermometer Service
    
      // Initialize Battery Service.
      memset(&bas_init, 0, sizeof(bas_init));
    
      // Here the sec level for the Battery Service can be changed/increased.
      bas_init.bl_rd_sec        = SEC_OPEN;
      bas_init.bl_cccd_wr_sec   = SEC_OPEN;
      bas_init.bl_report_rd_sec = SEC_OPEN;
    
      bas_init.evt_handler          = NULL;
      bas_init.support_notification = true;
      bas_init.p_report_ref         = NULL;
      bas_init.initial_batt_level   = 100;
    
      err_code = ble_bas_init(&m_bas, &bas_init);
      APP_ERROR_CHECK(err_code);
    
      // Initialize Device Information Service.
      memset(&dis_init, 0, sizeof(dis_init));
    
      ble_srv_ascii_to_utf8(&dis_init.manufact_name_str, MANUFACTURER_NAME);
      ble_srv_ascii_to_utf8(&dis_init.model_num_str, MODEL_NUM);
    
      sys_id.manufacturer_id            = MANUFACTURER_ID;
      sys_id.organizationally_unique_id = ORG_UNIQUE_ID;
      dis_init.p_sys_id                 = &sys_id;
    
      dis_init.dis_char_rd_sec = SEC_OPEN;
    
      err_code = ble_dis_init(&dis_init);
      APP_ERROR_CHECK(err_code);
      ble_cus_init_t cus_init;
      memset(&cus_init, 0, sizeof(cus_init));
      /* This code belongs in services_init() in main.c*/
      /* This code belongs in services_init in main.c*/
    
      // Set the cus event handler
      cus_init.evt_handler = on_cus_evt;
    
      BLE_GAP_CONN_SEC_MODE_SET_OPEN(&cus_init.temperature_char_attr_md.read_perm);
      BLE_GAP_CONN_SEC_MODE_SET_OPEN(&cus_init.temperature_char_attr_md.write_perm);
    
      // Set permissions for second characteristic (read only open, write no access)
      BLE_GAP_CONN_SEC_MODE_SET_OPEN(&cus_init.humidity_char_attr_md.read_perm);
      BLE_GAP_CONN_SEC_MODE_SET_NO_ACCESS(&cus_init.humidity_char_attr_md.write_perm);
    
      BLE_GAP_CONN_SEC_MODE_SET_OPEN(&cus_init.tx_enable_char_attr_md.read_perm);
      BLE_GAP_CONN_SEC_MODE_SET_OPEN(&cus_init.tx_enable_char_attr_md.write_perm);
    
      BLE_GAP_CONN_SEC_MODE_SET_OPEN(&cus_init.tx_interval_char_attr_md.read_perm);
      BLE_GAP_CONN_SEC_MODE_SET_OPEN(&cus_init.tx_interval_char_attr_md.write_perm);
    
      BLE_GAP_CONN_SEC_MODE_SET_OPEN(&cus_init.ack_char_attr_md.read_perm);
      // BLE_GAP_CONN_SEC_MODE_SET_NO_ACCESS(&cus_init.ack_char_attr_md.write_perm);
      BLE_GAP_CONN_SEC_MODE_SET_OPEN(&cus_init.ack_char_attr_md.write_perm);
    
      BLE_GAP_CONN_SEC_MODE_SET_OPEN(&cus_init.config_ack_char_attr_md.read_perm);
      BLE_GAP_CONN_SEC_MODE_SET_NO_ACCESS(&cus_init.config_ack_char_attr_md.write_perm);
    
      err_code = ble_cus_init(&m_cus, &cus_init);
      APP_ERROR_CHECK(err_code);
    }
    
    /**@brief Function for initializing the sensor simulators.
     */
    static void sensor_simulator_init(void) {
      m_battery_sim_cfg.min          = MIN_BATTERY_LEVEL;
      m_battery_sim_cfg.max          = MAX_BATTERY_LEVEL;
      m_battery_sim_cfg.incr         = BATTERY_LEVEL_INCREMENT;
      m_battery_sim_cfg.start_at_max = true;
    
      sensorsim_init(&m_battery_sim_state, &m_battery_sim_cfg);
    
      // Temperature is in celcius (it is multiplied by 100 to avoid floating point arithmetic).
      m_temp_celcius_sim_cfg.min          = MIN_CELCIUS_DEGREES;
      m_temp_celcius_sim_cfg.max          = MAX_CELCIUS_DEGRESS;
      m_temp_celcius_sim_cfg.incr         = CELCIUS_DEGREES_INCREMENT;
      m_temp_celcius_sim_cfg.start_at_max = false;
    
      sensorsim_init(&m_temp_celcius_sim_state, &m_temp_celcius_sim_cfg);
    }
    
    /**@brief Function for starting application timers.
     */
    static void application_timers_start(void) {
      ret_code_t err_code;
      uint32_t ticks = APP_TIMER_TICKS(tx_interval * 1000);
      err_code       = app_timer_start(m_timer_id, ticks, NULL);
      APP_ERROR_CHECK(err_code);
    }
    
    /**@brief Function for handling the Connection Parameters Module.
     *
     * @details This function will be called for all events in the Connection Parameters Module which
     *          are passed to the application.
     *          @note All this function does is to disconnect. This could have been done by simply
     *                setting the disconnect_on_fail config parameter, but instead we use the event
     *                handler mechanism to demonstrate its use.
     *
     * @param[in] p_evt  Event received from the Connection Parameters Module.
     */
    static void on_conn_params_evt(ble_conn_params_evt_t* p_evt) {
      ret_code_t err_code;
    
      if (p_evt->evt_type == BLE_CONN_PARAMS_EVT_FAILED) {
        err_code = sd_ble_gap_disconnect(m_conn_handle, BLE_HCI_CONN_INTERVAL_UNACCEPTABLE);
        APP_ERROR_CHECK(err_code);
      }
    }
    
    /**@brief Function for handling a Connection Parameters error.
     *
     * @param[in] nrf_error  Error code containing information about what went wrong.
     */
    static void conn_params_error_handler(uint32_t nrf_error) {
      APP_ERROR_HANDLER(nrf_error);
    }
    
    /**@brief Function for initializing the Connection Parameters module.
     */
    static void conn_params_init(void) {
      ret_code_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);
    }
    
    /**@brief Function for putting the chip into sleep mode.
     *
     * @note This function will not return.
     */
    static void sleep_mode_enter(void) {
      ret_code_t err_code;
    
      err_code = bsp_indication_set(BSP_INDICATE_IDLE);
      APP_ERROR_CHECK(err_code);
    
      // Prepare wakeup buttons.
      err_code = bsp_btn_ble_sleep_mode_prepare();
      APP_ERROR_CHECK(err_code);
    
      // Go to system-off mode (this function will not return; wakeup will cause a reset).
      err_code = sd_power_system_off();
      APP_ERROR_CHECK(err_code);
    }
    
    /**@brief Function for handling advertising events.
     *
     * @details This function will be called for advertising events which are passed to the application.
     *
     * @param[in] ble_adv_evt  Advertising event.
     */
    static void on_adv_evt(ble_adv_evt_t ble_adv_evt) {
      ret_code_t err_code;
    
      switch (ble_adv_evt) {
      case BLE_ADV_EVT_FAST:
        NRF_LOG_INFO("Fast advertising.");
        err_code = bsp_indication_set(BSP_INDICATE_ADVERTISING);
        APP_ERROR_CHECK(err_code);
        break;
    
      case BLE_ADV_EVT_IDLE:
        NRF_LOG_INFO("Advertising stopped -> entering low power wait");
        break;
      default:
        break;
      }
    }
    
    /**@brief Function for handling BLE events.
     *
     * @param[in]   p_ble_evt   Bluetooth stack event.
     * @param[in]   p_context   Unused.
     */
    static void ble_evt_handler(ble_evt_t const* p_ble_evt, void* p_context) {
      uint32_t err_code = NRF_SUCCESS;
    
      switch (p_ble_evt->header.evt_id) {
      case BLE_GAP_EVT_CONNECTED:
        NRF_LOG_INFO("Connected.");
        err_code = bsp_indication_set(BSP_INDICATE_CONNECTED);
        APP_ERROR_CHECK(err_code);
        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);
        uint8_t clear_ack = 0;
        uint8_t val       = 0;
        ble_cus_config_ack_update(&m_cus, val);
        break;
    
      case BLE_GAP_EVT_DISCONNECTED:
        NRF_LOG_INFO("Disconnected.");
        ack           = 0;
        m_conn_handle = BLE_CONN_HANDLE_INVALID;
        //  advertising_start(false);
        break;
      case BLE_GAP_EVT_PHY_UPDATE_REQUEST: {
        ble_gap_phys_t const phys = {
            //.rx_phys = BLE_GAP_PHY_1MBPS,
            //.tx_phys = BLE_GAP_PHY_1MBPS,
            .rx_phys = BLE_GAP_PHY_2MBPS,
            .tx_phys = BLE_GAP_PHY_2MBPS,
        };
        err_code = sd_ble_gap_phy_update(p_ble_evt->evt.gap_evt.conn_handle, &phys);
        APP_ERROR_CHECK(err_code);
      } break;
      case BLE_GATTC_EVT_TIMEOUT:
        // Disconnect on GATT Client timeout event.
        NRF_LOG_DEBUG("GATT Client Timeout.");
        err_code = sd_ble_gap_disconnect(p_ble_evt->evt.gattc_evt.conn_handle, BLE_HCI_REMOTE_USER_TERMINATED_CONNECTION);
        APP_ERROR_CHECK(err_code);
        break;
    
      case BLE_GATTS_EVT_TIMEOUT:
        // Disconnect on GATT Server timeout event.
        NRF_LOG_DEBUG("GATT Server Timeout.");
        err_code = sd_ble_gap_disconnect(p_ble_evt->evt.gatts_evt.conn_handle, BLE_HCI_REMOTE_USER_TERMINATED_CONNECTION);
        APP_ERROR_CHECK(err_code);
        break;
      default:
        // No implementation needed.
        break;
      }
    }
    
    /**@brief Function for initializing the BLE stack.
     *
     * @details Initializes the SoftDevice and the BLE event interrupt.
     */
    static void ble_stack_init(void) {
      ret_code_t err_code;
      uint32_t ram_start = 0;
    
      if (!nrf_sdh_is_enabled()) {
        err_code = nrf_sdh_enable_request();
        APP_ERROR_CHECK(err_code);
      }
    
      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_LOG_INFO("SoftDevice enabled state: %d", nrf_sdh_is_enabled());
      NRF_SDH_BLE_OBSERVER(m_ble_observer, APP_BLE_OBSERVER_PRIO, ble_evt_handler, NULL);
    }
    
    /**@brief Function for handling events from the BSP module.
     *
     * @param[in]   event   Event generated by button press.
     */
    static void bsp_event_handler(bsp_event_t event) {
      ret_code_t err_code;
    
      switch (event) {
      case BSP_EVENT_SLEEP:
        sleep_mode_enter();
        break;
    
      case BSP_EVENT_DISCONNECT:
        err_code = sd_ble_gap_disconnect(m_conn_handle, BLE_HCI_REMOTE_USER_TERMINATED_CONNECTION);
        if (err_code != NRF_ERROR_INVALID_STATE) {
          APP_ERROR_CHECK(err_code);
        }
        break;
    
      case BSP_EVENT_WHITELIST_OFF:
        if (m_conn_handle == BLE_CONN_HANDLE_INVALID) {
          err_code = ble_advertising_restart_without_whitelist(&m_advertising);
          if (err_code != NRF_ERROR_INVALID_STATE) {
            APP_ERROR_CHECK(err_code);
          }
        }
        break;
    
      case BSP_EVENT_KEY_0:
        if (m_conn_handle != BLE_CONN_HANDLE_INVALID) {
          // temperature_measurement_send();
        }
        break;
    
      default:
        break;
      }
    }
    
    /**@brief Function for the Peer Manager initialization.
     */
    static void peer_manager_init(void) {
      ble_gap_sec_params_t sec_param;
      ret_code_t err_code;
    
      err_code = pm_init();
      APP_ERROR_CHECK(err_code);
    
      memset(&sec_param, 0, sizeof(ble_gap_sec_params_t));
    
      // Security parameters to be used for all security procedures.
      // sec_param.bond           = SEC_PARAM_BOND;
      // sec_param.mitm           = SEC_PARAM_MITM;   for custom
      // sec_param.lesc           = SEC_PARAM_LESC;
      // sec_param.keypress       = SEC_PARAM_KEYPRESS;
      // sec_param.io_caps        = SEC_PARAM_IO_CAPABILITIES;
      sec_param.bond           = 1;
      sec_param.mitm           = 0;
      sec_param.lesc           = 0;
      sec_param.keypress       = 0;
      sec_param.io_caps        = BLE_GAP_IO_CAPS_NONE;
      sec_param.oob            = SEC_PARAM_OOB;
      sec_param.min_key_size   = SEC_PARAM_MIN_KEY_SIZE;
      sec_param.max_key_size   = SEC_PARAM_MAX_KEY_SIZE;
      sec_param.kdist_own.enc  = 1;
      sec_param.kdist_own.id   = 1;
      sec_param.kdist_peer.enc = 1;
      sec_param.kdist_peer.id  = 1;
    
      err_code = pm_sec_params_set(&sec_param);
      APP_ERROR_CHECK(err_code);
    
      err_code = pm_register(pm_evt_handler);
      APP_ERROR_CHECK(err_code);
    }
    
    /**@brief Clear bond information from persistent storage.
     */
    static void delete_bonds(void) {
      ret_code_t err_code;
    
      NRF_LOG_INFO("Erase bonds!");
    
      err_code = pm_peers_delete();
      APP_ERROR_CHECK(err_code);
    }
    
    /**@brief Function for initializing the Advertising functionality.
     *
     * @details Encodes the required advertising data and passes it to the stack.
     *          Also builds a structure to be passed to the stack when starting advertising.
     */
    static void advertising_init(void) {
      ret_code_t err_code;
      ble_advertising_init_t init;
    
      memset(&init, 0, sizeof(init));
    
      // 🔥 SMALLEST POSSIBLE CONFIG
    
      init.advdata.name_type          = BLE_ADVDATA_FULL_NAME;    // reduce further
      init.advdata.include_appearance = false;                    // 🔥 VERY IMPORTANT
      init.advdata.flags              = BLE_GAP_ADV_FLAGS_LE_ONLY_GENERAL_DISC_MODE;
    
      // 🔥 ONLY ONE UUID (TEMP FIX)
      init.advdata.uuids_complete.uuid_cnt = 1;
      init.advdata.uuids_complete.p_uuids  = &m_adv_uuids[0];
    
      // 🔥 NO scan response
      memset(&init.srdata, 0, sizeof(init.srdata));
    
      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);
    }
    
    /**@brief Function for initializing buttons and leds.
     *
     * @param[out] p_erase_bonds  Will be true if the clear bonding button was pressed to wake the application up.
     */
    static void buttons_leds_init(bool* p_erase_bonds) {
      ret_code_t err_code;
      bsp_event_t startup_event;
    
      err_code = bsp_init(BSP_INIT_LEDS | BSP_INIT_BUTTONS, bsp_event_handler);
      APP_ERROR_CHECK(err_code);
    
      err_code = bsp_btn_ble_init(NULL, &startup_event);
      APP_ERROR_CHECK(err_code);
    
      *p_erase_bonds = (startup_event == BSP_EVENT_CLEAR_BONDING_DATA);
    }
    
    /**@brief Function for initializing the nrf log module.
     */
    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 Function for initializing power management.
     */
    static void power_management_init(void) {
      ret_code_t err_code;
      err_code = nrf_pwr_mgmt_init();
      APP_ERROR_CHECK(err_code);
    }
    
    /**@brief Function for handling the idle state (main loop).
     *
     * @details If there is no pending log operation, then sleep until next the next event occurs.
     */
    
    /**@brief Function for starting advertising.
     */
    static void advertising_start(bool erase_bonds) {
      if (erase_bonds == true) {
        delete_bonds();
        // Advertising is started by PM_EVT_PEERS_DELETE_SUCCEEDED event.
      } else {
        uint32_t err_code = ble_advertising_start(&m_advertising, BLE_ADV_MODE_FAST);
        APP_ERROR_CHECK(err_code);
      }
    }
    
    // void twi_handler(nrf_drv_twi_evt_t const* p_event, void* p_context) {
    //   if (p_event->type == NRF_DRV_TWI_EVT_DONE) {
    //     m_xfer_done = true;
    //   }
    // }
    
    static volatile bool m_xfer_success = false;    // Track actual success
    
    void twi_handler(nrf_drv_twi_evt_t const* p_event, void* p_context) {
      if (p_event->type == NRF_DRV_TWI_EVT_DONE) {
        m_xfer_success = true;
        m_xfer_done    = true;
      } else if (p_event->type == NRF_DRV_TWI_EVT_ADDRESS_NACK || p_event->type == NRF_DRV_TWI_EVT_DATA_NACK) {
        m_xfer_success = false;    // Transaction failed (Sensor not ready/found)
        m_xfer_done    = true;     // Unblock the while loop immediately!
      }
    }
    
    // void sht41_read(void) {
    //   ret_code_t err_code;
    //   uint8_t cmd = SHT41_MEASURE_HIGH_PRECISION;
    
    //  m_xfer_done = false;
    
    //  err_code = nrf_drv_twi_tx(&m_twi, SHT41_ADDR, &cmd, 1, false);
    //  APP_ERROR_CHECK(err_code);
    //  uint32_t timeout = 1000000;
    
    //  while (!m_xfer_done && timeout--) {
    //  }
    
    //  if (timeout == 0) {
    //    NRF_LOG_ERROR("TWI timeout");
    //    return;
    //  }
    //  nrf_delay_ms(10);
    
    //  m_xfer_done = false;
    
    //  err_code = nrf_drv_twi_rx(&m_twi, SHT41_ADDR, m_buffer, 6);
    //  APP_ERROR_CHECK(err_code);
    //  timeout = 1000000;
    //  while (!m_xfer_done && timeout--) {
    //  }
    
    //  if (timeout == 0) {
    //    NRF_LOG_ERROR("TWI timeout");
    //    return;
    //  }
    //}
    
    // void sht41_read(void) {
    //     ret_code_t err_code;
    //     uint8_t cmd = SHT41_MEASURE_HIGH_PRECISION;
    
    //    m_xfer_done = false;
    //    err_code = nrf_drv_twi_tx(&m_twi, SHT41_ADDR, &cmd, 1, false);
    //    APP_ERROR_CHECK(err_code);
    
    //    uint32_t timeout = 10000;
    //    while (!m_xfer_done && timeout > 0) {
    //        nrf_delay_us(10);
    //        timeout--;
    //    }
    
    //    if (timeout == 0) {
    //        // TWI TX Timeout
    //        return;
    //    }
    
    //    nrf_delay_ms(10); // Wait for the SHT41 to perform the physical measurement
    
    //    m_xfer_done = false;
    //    err_code = nrf_drv_twi_rx(&m_twi, SHT41_ADDR, m_buffer, 6);
    //    APP_ERROR_CHECK(err_code);
    
    //    timeout = 10000;
    //    while (!m_xfer_done && timeout > 0) {
    //        nrf_delay_us(10);
    //        timeout--;
    //    }
    
    //    if (timeout == 0) {
    //        // TWI RX Timeout
    //        return;
    //    }
    //}
    
    void sht41_read(void) {
      ret_code_t err_code;
      uint8_t cmd = SHT41_MEASURE_HIGH_PRECISION;
    
      m_xfer_done    = false;
      m_xfer_success = false;
      err_code       = nrf_drv_twi_tx(&m_twi, SHT41_ADDR, &cmd, 1, false);
      if (err_code != NRF_SUCCESS)
        return;    // <-- REMOVED APP_ERROR_CHECK
    
      uint32_t timeout = 10000;
      while (!m_xfer_done && timeout > 0) {
        nrf_delay_us(10);
        timeout--;
      }
    
      if (timeout == 0 || !m_xfer_success)
        return;            // Exit on NACK or timeout
    
      nrf_delay_ms(10);    // Wait for the SHT41 measurement
    
      m_xfer_done    = false;
      m_xfer_success = false;
      err_code       = nrf_drv_twi_rx(&m_twi, SHT41_ADDR, m_buffer, 6);
      if (err_code != NRF_SUCCESS)
        return;    // <-- REMOVED APP_ERROR_CHECK
    
      timeout = 10000;
      while (!m_xfer_done && timeout > 0) {
        nrf_delay_us(10);
        timeout--;
      }
    
      if (timeout == 0 || !m_xfer_success)
        return;
    }
    
    // void sht41_read(void) {
    //     ret_code_t err_code;
    //     uint8_t cmd = SHT41_MEASURE_HIGH_PRECISION;
    
    //    m_xfer_done = false;
    //    m_xfer_success = false;
    //    err_code = nrf_drv_twi_tx(&m_twi, SHT41_ADDR, &cmd, 1, false);
    //    APP_ERROR_CHECK(err_code);
    
    //    uint32_t timeout = 10000;
    //    while (!m_xfer_done && timeout > 0) {
    //        nrf_delay_us(10);
    //        timeout--;
    //    }
    
    //    if (timeout == 0 || !m_xfer_success) return; // Exit on NACK or timeout
    
    //    nrf_delay_ms(10); // Wait for the SHT41 to perform the physical measurement
    
    //    m_xfer_done = false;
    //    m_xfer_success = false;
    //    err_code = nrf_drv_twi_rx(&m_twi, SHT41_ADDR, m_buffer, 6);
    //    APP_ERROR_CHECK(err_code);
    
    //    timeout = 10000;
    //    while (!m_xfer_done && timeout > 0) {
    //        nrf_delay_us(10);
    //        timeout--;
    //    }
    
    //    if (timeout == 0 || !m_xfer_success) return;
    //}
    
    uint8_t battery_percentage(void);
    uint8_t battery_soc_update();
    
    uint8_t get_filtered_battery_soc(void);
    
    // void process_data() {
    //   uint16_t   rawTemp = (m_buffer[0] << 8) | m_buffer[1];
    //   uint16_t   rawHum  = (m_buffer[3] << 8) | m_buffer[4];
    
    //  temperature = -45 + 175 * ((float)rawTemp / 65535.0);
    //  humidity    = -6 + 125 * ((float)rawHum / 65535.0);
    
    //  //NRF_LOG_INFO("Temperature: %d C", (int)temperature);
    //  //NRF_LOG_INFO("Humidity: %d %%", (int)humidity);
    ////  uint8_t batt = battery_percentage();
    
    ////battery_level_update(batt);
    // uint8_t batt = get_filtered_battery_soc();
    //   battery_level_update(batt);
    
    //  nrf_delay_ms(600);
    // //NRF_LOG_INFO("Sending SOC: %d %%", soc);
    //  nrf_delay_ms(600);
    // // if (m_conn_handle != BLE_CONN_HANDLE_INVALID) {
    //  //  battery_level_update(soc);
    //    ble_cus_temperature_update(&m_cus, (int16_t )temperature);
    //    ble_cus_humidity_update(&m_cus, (int16_t )humidity);
    //  //}
    //}
    
    void twi_init(void) {
      ret_code_t err_code;
    
      const nrf_drv_twi_config_t config = {.scl                = ARDUINO_SCL_PIN,
                                           .sda                = ARDUINO_SDA_PIN,
                                           .frequency          = NRF_DRV_TWI_FREQ_100K,
                                           .interrupt_priority = APP_IRQ_PRIORITY_LOW,
                                           .clear_bus_init     = true};
    
      err_code = nrf_drv_twi_init(&m_twi, &config, twi_handler, NULL);
      APP_ERROR_CHECK(err_code);
    
      nrf_drv_twi_enable(&m_twi);
    }
    
    // void twi_init(void) {
    //     ret_code_t err_code;
    
    //    const nrf_drv_twi_config_t config = {
    //        .scl                = ARDUINO_SCL_PIN,
    //        .sda                = ARDUINO_SDA_PIN,
    //        .frequency          = NRF_DRV_TWI_FREQ_100K,
    //        .interrupt_priority = APP_IRQ_PRIORITY_HIGH,
    //        .clear_bus_init     = false
    //    };
    
    //    err_code = nrf_drv_twi_init(&m_twi, &config, twi_handler, NULL);
    //    APP_ERROR_CHECK(err_code);
    // nrf_drv_twi_enable(&m_twi);
    //    // Keep it disabled until actually needed
    //   // nrf_drv_twi_disable(&m_twi);
    //}
    
    #define WS2812_PIN 16
    #define LED_COUNT 1
    
    /* PWM instance */
    static nrfx_pwm_t pwm = NRFX_PWM_INSTANCE(0);
    
    /* WS2812 timing values (PWM duty cycles) */
    #define PWM_PERIOD 20
    
    static uint16_t pwm_buffer[24 * LED_COUNT + 50];
    
    /* Initialize PWM */
    void ws2812_init(void) {
      nrfx_pwm_config_t config = {
          .output_pins  = {WS2812_PIN, NRFX_PWM_PIN_NOT_USED, NRFX_PWM_PIN_NOT_USED, NRFX_PWM_PIN_NOT_USED},
          .irq_priority = APP_IRQ_PRIORITY_LOWEST,
          .base_clock   = NRF_PWM_CLK_16MHz,
          .count_mode   = NRF_PWM_MODE_UP,
          .top_value    = PWM_PERIOD,
          .load_mode    = NRF_PWM_LOAD_COMMON,
          .step_mode    = NRF_PWM_STEP_AUTO};
      nrfx_pwm_init(&pwm, &config, NULL);
    }
    
    #define WS2812_1 (14 | 0x8000)
    #define WS2812_0 (6 | 0x8000)
    #define WS2812_RESET (0 | 0x8000)
    
    void ws2812_set_color(uint8_t r, uint8_t g, uint8_t b) {
      // WS2812 expects GRB
      uint32_t color = ((uint32_t)g << 16) | ((uint32_t)r << 8) | b;
    
      for (int i = 0; i < 24; i++) {
        if (color & (1UL << (23 - i)))
          pwm_buffer[i] = WS2812_1;
        else
          pwm_buffer[i] = WS2812_0;
      }
    
      for (int i = 24; i < 24 + 50; i++)
        pwm_buffer[i] = WS2812_RESET;
    
      nrf_pwm_sequence_t seq = {.values.p_common = pwm_buffer, .length = 24 + 50, .repeats = 0, .end_delay = 0};
    
      nrfx_pwm_simple_playback(&pwm, &seq, 1, NRFX_PWM_FLAG_STOP);
    }
    
    uint16_t ina226_read_register(uint8_t reg) {
      ret_code_t err;
      uint8_t data[2];
    
      m_xfer_done = false;
    
      err = nrf_drv_twi_tx(&m_twi, INA226_ADDR, &reg, 1, true);
      if (err != NRF_SUCCESS) {
        NRF_LOG_ERROR("INA226 TX FAILED");
        return 0;
      }
    
      nrf_delay_ms(2);
    
      m_xfer_done = false;
    
      err = nrf_drv_twi_rx(&m_twi, INA226_ADDR, data, 2);
      if (err != NRF_SUCCESS) {
        NRF_LOG_ERROR("INA226 RX FAILED");
        return 0;
      }
    
      uint16_t raw = (data[0] << 8) | data[1];
    
      return raw;
    }
    
    float ina226_get_bus_voltage(void) {
      NRF_LOG_INFO("INA226 BUS VOLTAGE READ START");
    
      uint16_t raw = ina226_read_register(0x02);
    
      NRF_LOG_INFO("RAW BUS VOLTAGE REGISTER: %d", raw);
    
      float voltage = raw * 1.25f / 1000.0f;
    
      NRF_LOG_INFO("CALCULATED BUS VOLTAGE: %.2f V", voltage);
      //  if(voltage==0) ws2812_set_color(255,0,0);
      return voltage;
    }
    
    uint8_t battery_percentage(void) {
      float v = ina226_get_bus_voltage();
      if (v < 2.5 || v > 5.5) {
        NRF_LOG_ERROR("INVALID VOLTAGE RANGE -> SENSOR FAULT");
        return 0;
      }
      float percent = ((v - 3.3) / (4.2 - 3.3)) * 100;
      if (percent < 0)
        percent = 0;
      if (percent > 100)
        percent = 100;
    
      return (uint8_t)percent;
    }
    
    // bool i2c_write(uint8_t addr, uint8_t *data, uint8_t len)
    //{
    //     ret_code_t err;
    //     m_xfer_done = false; // Reset the flag
    
    //    // Start the transfer
    //    err = nrf_drv_twi_tx(&m_twi, addr, data, len, false);
    //    if(err != NRF_SUCCESS) return false;
    
    //    // CRITICAL: Wait for the hardware to finish sending bits
    //    uint32_t timeout = 100000;
    //    while(!m_xfer_done && timeout--)
    //    {
    //      //  __WFE(); // Wait for event to save power while hardware works
    //    }
    
    //    return m_xfer_done; // Only return true if transfer actually completed
    //}
    
    // bool i2c_write(uint8_t addr, uint8_t *data, uint8_t len)
    //{
    //     ret_code_t err;
    //     m_xfer_done = false;
    
    //    err = nrf_drv_twi_tx(&m_twi, addr, data, len, false);
    //    if(err != NRF_SUCCESS) return false;
    
    //    // Timeout: 10,000 iterations * 10 microseconds = 100 ms max wait
    //    uint32_t timeout = 10000;
    //    while(!m_xfer_done && timeout > 0)
    //    {
    //        nrf_delay_us(10); // Forces the loop to respect real-world time
    //        timeout--;
    //    }
    
    //    return m_xfer_done;
    //}
    
    bool i2c_write(uint8_t addr, uint8_t* data, uint8_t len) {
      ret_code_t err;
      m_xfer_done    = false;
      m_xfer_success = false;
    
      err = nrf_drv_twi_tx(&m_twi, addr, data, len, false);
      if (err != NRF_SUCCESS)
        return false;
    
      uint32_t timeout = 10000;
      while (!m_xfer_done && timeout > 0) {
        nrf_delay_us(10);
        timeout--;
      }
    
      return (m_xfer_done && m_xfer_success);    // Must actually succeed, not just timeout
    }
    
    // bool i2c_write(uint8_t addr, uint8_t *data, uint8_t len)
    //{
    //     ret_code_t err;
    //     for(int i = 0; i < 3; i++)
    //     {
    //         m_xfer_done = false;
    //         err = nrf_drv_twi_tx(&m_twi, addr, data, len, false);
    
    //        if(err != NRF_SUCCESS) return false;
    
    //        // CRITICAL: You MUST wait for the interrupt to fire
    //        uint32_t timeout = 100000;
    //        while(!m_xfer_done && timeout--) {
    //            __WFE(); // Wait for event to save power
    //        }
    
    //        if(m_xfer_done) return true;
    //    }
    //    NRF_LOG_ERROR("I2C WRITE TIMEOUT");
    //    return false;
    //}
    
    // bool i2c_read(uint8_t addr, uint8_t reg, uint8_t *rx, uint8_t len)
    //{
    //     ret_code_t err;
    
    //    NRF_LOG_INFO("---------------------------------");
    //    NRF_LOG_INFO("I2C READ START");
    //    NRF_LOG_INFO("Device Addr: 0x%02X", addr);
    //    NRF_LOG_INFO("Register: 0x%02X", reg);
    // nrf_delay_ms(600);
    //    for(int i = 0; i < 3; i++)
    //    {
    //        NRF_LOG_INFO("I2C TX Attempt %d", i+1);
    // nrf_delay_ms(200);
    //        m_xfer_done = false;
    
    //        err = nrf_drv_twi_tx(&m_twi, addr, &reg, 1, true);
    
    //        if(err != NRF_SUCCESS)
    //        {
    //            NRF_LOG_ERROR("TX START FAILED: %d", err);
    //            continue;
    //        }
    
    //        uint32_t timeout = 1000000;
    
    //        while(!m_xfer_done && timeout--)
    //        {
    //        }
    
    //        if(timeout == 0)
    //        {
    //            NRF_LOG_ERROR("TX TIMEOUT");
    //            continue;
    //        }
    
    //        NRF_LOG_INFO("TX COMPLETE");
    // nrf_delay_ms(300);
    //        m_xfer_done = false;
    
    //        err = nrf_drv_twi_rx(&m_twi, addr, rx, len);
    
    //        if(err != NRF_SUCCESS)
    //        {
    //            NRF_LOG_ERROR("RX START FAILED: %d", err);
    //            continue;
    //        }
    
    //        timeout = 1000000;
    
    //        while(!m_xfer_done && timeout--)
    //        {
    //        }
    
    //        if(timeout == 0)
    //        {
    //            NRF_LOG_ERROR("RX TIMEOUT");
    //            continue;
    //        }
    
    //        NRF_LOG_INFO("RX COMPLETE");
    // nrf_delay_ms(600);
    //        for(int j=0;j<len;j++)
    //        {
    //            NRF_LOG_INFO("RX[%d] = 0x%02X", j, rx[j]);
    //        }
    
    //        return true;
    //    }
    // nrf_delay_ms(600);
    //    NRF_LOG_ERROR("I2C READ FAILED AFTER RETRIES");
    
    //    return false;
    //}
    
    // bool i2c_read(uint8_t addr, uint8_t reg, uint8_t *rx, uint8_t len)
    //{
    //     ret_code_t err;
    
    //    for(int i = 0; i < 3; i++) // Retries
    //    {
    //        m_xfer_done = false;
    
    //        err = nrf_drv_twi_tx(&m_twi, addr, &reg, 1, true);
    //        if(err != NRF_SUCCESS) continue;
    
    //        uint32_t timeout = 10000;
    //        while(!m_xfer_done && timeout > 0)
    //        {
    //            nrf_delay_us(10);
    //            timeout--;
    //        }
    
    //        if(timeout == 0) continue; // TX Timeout
    
    //        m_xfer_done = false;
    //        err = nrf_drv_twi_rx(&m_twi, addr, rx, len);
    //        if(err != NRF_SUCCESS) continue;
    
    //        timeout = 10000;
    //        while(!m_xfer_done && timeout > 0)
    //        {
    //            nrf_delay_us(10);
    //            timeout--;
    //        }
    
    //        if(timeout == 0) continue; // RX Timeout
    
    //        return true; // Success
    //    }
    
    //    return false; // Failed after retries
    //}
    
    bool i2c_read(uint8_t addr, uint8_t reg, uint8_t* rx, uint8_t len) {
      ret_code_t err;
    
      for (int i = 0; i < 3; i++)    // Retries
      {
        m_xfer_done    = false;
        m_xfer_success = false;
    
        err = nrf_drv_twi_tx(&m_twi, addr, &reg, 1, true);
        if (err != NRF_SUCCESS)
          continue;
    
        uint32_t timeout = 10000;
        while (!m_xfer_done && timeout > 0) {
          nrf_delay_us(10);
          timeout--;
        }
    
        if (timeout == 0 || !m_xfer_success)
          continue;    // Retry if timed out or NACKed
    
        m_xfer_done    = false;
        m_xfer_success = false;
    
        err = nrf_drv_twi_rx(&m_twi, addr, rx, len);
        if (err != NRF_SUCCESS)
          continue;
    
        timeout = 10000;
        while (!m_xfer_done && timeout > 0) {
          nrf_delay_us(10);
          timeout--;
        }
    
        if (timeout == 0 || !m_xfer_success)
          continue;     // Retry if timed out or NACKed
    
        return true;    // Success!
      }
    
      return false;     // Failed after 3 retries
    }
    
    uint16_t ina_read(uint8_t reg) {
      uint8_t buf[2];
    
      NRF_LOG_INFO("=================================");
      NRF_LOG_INFO("INA226 REGISTER READ");
      NRF_LOG_INFO("Register Address: 0x%02X", reg);
      if (!i2c_read(INA226_ADDR, reg, buf, 2)) {
        NRF_LOG_ERROR("INA226 REGISTER READ FAILED");
        return 0;
      }
    
      NRF_LOG_INFO("INA226 RAW DATA HIGH BYTE: 0x%02X", buf[0]);
      NRF_LOG_INFO("INA226 RAW DATA LOW BYTE : 0x%02X", buf[1]);
      uint16_t value = (buf[0] << 8) | buf[1];
    
      NRF_LOG_INFO("INA226 REGISTER VALUE: %d", value);
    
      NRF_LOG_INFO("=================================");
      return value;
    }
    
    float ina_current() {
      uint16_t raw = ina_read(0x04);
    
      float current_lsb = 0.001f;
      return raw * current_lsb;
    }
    
    float ina_bus_voltage() {
      NRF_LOG_INFO("INA226 BUS VOLTAGE READ START");
      uint16_t raw = ina_read(0x02);
    
      NRF_LOG_INFO("RAW BUS VOLTAGE REGISTER: %d", raw);
      float voltage = raw * 1.25f / 1000.0f;
      NRF_LOG_INFO("CALCULATED BUS VOLTAGE: " NRF_LOG_FLOAT_MARKER " V", NRF_LOG_FLOAT(voltage));
      voltage *= 4.0f;
      NRF_LOG_INFO("CALCULATED BUS VOLTAGE: " NRF_LOG_FLOAT_MARKER " V", NRF_LOG_FLOAT(voltage));
      NRF_LOG_INFO("INA226 BUS VOLTAGE READ END");
      return voltage;
    }
    
    float soc_from_voltage(float v) {
      if (v >= 4.20)
        return 100;
    
      if (v >= 4.00)
        return 85 + (v - 4.00) * 75;
    
      if (v >= 3.90)
        return 75 + (v - 3.90) * 100;
    
      if (v >= 3.80)
        return 60 + (v - 3.80) * 150;
    
      if (v >= 3.70)
        return 50 + (v - 3.70) * 100;
    
      if (v >= 3.60)
        return 35 + (v - 3.60) * 150;
    
      if (v >= 3.50)
        return 25 + (v - 3.50) * 100;
    
      if (v >= 3.40)
        return 10 + (v - 3.40) * 150;
    
      if (v >= 3.30)
        return (v - 3.30) * 100;
    
      return 0;
    }
    
    // --- INA226 Registers & Config ---
    #define INA226_REG_CONFIG 0x00
    #define INA226_REG_SHUNTVOLTAGE 0x01
    #define INA226_REG_BUSVOLTAGE 0x02
    #define INA226_REG_POWER 0x03
    #define INA226_REG_CURRENT 0x04
    #define INA226_REG_CALIBRATION 0x05
    
    #define INA226_CONFIG_4_SAMPLES 0x4327
    #define INA226_CALIBRATION_VAL 1004
    
    // --- Battery Filter Constants ---
    #define SPIKE_CLAMP_PCT 8.0f
    #define TREND_DEADBAND_PCT 0.3f
    #define RATE_UP_PCT_SAMPLE 1.0f
    #define RATE_DOWN_PCT_SAMPLE 2.0f
    
    static float s_ema_soc     = 0.0f;
    static float s_display_soc = 0.0f;
    static float s_hist[TREND_WINDOW];
    static uint8_t s_hist_idx = 0;
    static bool s_initialised = false;
    
    float soc_filter(float raw) {
      if (!initialized) {
        ema_soc     = raw;
        display_soc = raw;
    
        for (int i = 0; i < TREND_WINDOW; i++)
          hist[i] = raw;
    
        initialized = true;
      }
    
      float clean = (fabsf(raw - ema_soc) > SPIKE_CLAMP) ? ema_soc : raw;
    
      ema_soc = EMA_ALPHA * clean + (1 - EMA_ALPHA) * ema_soc;
    
      hist[hist_index] = ema_soc;
    
      hist_index = (hist_index + 1) % TREND_WINDOW;
    
      float oldest = hist[hist_index];
    
      float trend_delta = ema_soc - oldest;
    
      int8_t trend = 0;
    
      if (trend_delta > TREND_DEADBAND)
        trend = 1;
      if (trend_delta < -TREND_DEADBAND)
        trend = -1;
    
      float error = ema_soc - display_soc;
    
      if (trend >= 0 && error > 0)
        display_soc += fminf(error, RATE_UP);
    
      else if (trend <= 0 && error < 0)
        display_soc += fmaxf(error, -RATE_DOWN);
    
      if (display_soc > 100)
        display_soc = 100;
      if (display_soc < 0)
        display_soc = 0;
    
      return display_soc;
    }
    
    bool ina226_write_register(uint8_t reg, uint16_t value) {
      uint8_t data[3];
      data[0] = reg;
      data[1] = (value >> 8) & 0xFF;
      data[2] = value & 0xFF;
      return i2c_write(INA226_ADDR, data, 3);
    }
    
    void ina226_init(void) {
      // Write Configuration (Averaging: 4 samples)
      if (ina226_write_register(INA226_REG_CONFIG, INA226_CONFIG_4_SAMPLES)) {
        NRF_LOG_INFO("INA226 Configured for 4 Samples.");
      } else {
        NRF_LOG_ERROR("INA226 Config FAILED.");
      }
      nrf_delay_ms(5);
    
      // Write Calibration (Max Current: 2A, Shunt: 0.051 Ohm)
      if (ina226_write_register(INA226_REG_CALIBRATION, INA226_CALIBRATION_VAL)) {
        NRF_LOG_INFO("INA226 Calibration Set.");
      } else {
        NRF_LOG_ERROR("INA226 Calibration FAILED.");
      }
      nrf_delay_ms(5);
    }
    
    float CAL_SoC(float v) {
      if (v >= 4.20f)
        return 100.0f;
      if (v >= 4.00f)
        return 85.0f + (v - 4.00f) * 75.0f;
      if (v >= 3.90f)
        return 75.0f + (v - 3.90f) * 100.0f;
      if (v >= 3.80f)
        return 60.0f + (v - 3.80f) * 150.0f;
      if (v >= 3.70f)
        return 50.0f + (v - 3.70f) * 100.0f;
      if (v >= 3.60f)
        return 35.0f + (v - 3.60f) * 150.0f;
      if (v >= 3.50f)
        return 25.0f + (v - 3.50f) * 100.0f;
      if (v >= 3.40f)
        return 10.0f + (v - 3.40f) * 150.0f;
      if (v >= 3.30f)
        return (v - 3.30f) * 100.0f;
      return 0.0f;
    }
    
    uint8_t get_filtered_battery_soc(void) {
      float battVoltage = ina226_get_bus_voltage();
      battVoltage *= 1.3;
      NRF_LOG_INFO("battVoltage     = " NRF_LOG_FLOAT_MARKER " %%", NRF_LOG_FLOAT(battVoltage));
    
      if (battVoltage < 2.5f || battVoltage > 5.5f) {
        NRF_LOG_ERROR("INVALID VOLTAGE RANGE -> SENSOR FAULT");
        //   ws2812_set_color(0,0,255);
        return battVoltage;
        return (uint8_t)s_display_soc;    // Return last known good state
      }
      float raw_soc = CAL_SoC(battVoltage);
    
      if (!s_initialised) {
        s_ema_soc     = raw_soc;
        s_display_soc = raw_soc;
        for (uint8_t i = 0; i < TREND_WINDOW; i++)
          s_hist[i] = raw_soc;
        s_initialised = true;
      }
    
      float clean_soc = (fabsf(raw_soc - s_ema_soc) > SPIKE_CLAMP_PCT) ? s_ema_soc : raw_soc;
      s_ema_soc       = EMA_ALPHA * clean_soc + (1.0f - EMA_ALPHA) * s_ema_soc;
    
      s_hist[s_hist_idx] = s_ema_soc;
      s_hist_idx         = (s_hist_idx + 1) % TREND_WINDOW;
    
      float oldest_ema  = s_hist[s_hist_idx];
      float trend_delta = s_ema_soc - oldest_ema;
    
      int8_t trend = 0;
      if (trend_delta > TREND_DEADBAND_PCT)
        trend = +1;
      if (trend_delta < -TREND_DEADBAND_PCT)
        trend = -1;
    
      float error = s_ema_soc - s_display_soc;
      if (trend >= 0 && error > 0.0f)
        s_display_soc += fminf(error, RATE_UP_PCT_SAMPLE);
      else if (trend <= 0 && error < 0.0f)
        s_display_soc += fmaxf(error, -RATE_DOWN_PCT_SAMPLE);
    
      s_display_soc = fmaxf(0.0f, fminf(100.0f, s_display_soc));
    
      NRF_LOG_INFO("Raw SoC     = " NRF_LOG_FLOAT_MARKER " %%", NRF_LOG_FLOAT(raw_soc));
      NRF_LOG_INFO("EMA SoC     = " NRF_LOG_FLOAT_MARKER " %%", NRF_LOG_FLOAT(s_ema_soc));
      NRF_LOG_INFO("Display SoC = " NRF_LOG_FLOAT_MARKER " %%", NRF_LOG_FLOAT(s_display_soc));
      NRF_LOG_INFO("Trend       = %s", trend > 0 ? "CHARGING" : trend < 0 ? "DISCHARGING" : "STABLE");
    
      return (uint8_t)s_display_soc;
    }
    uint8_t battery_soc_update() {
      // ws2812_set_color(100,0,100);
      NRF_LOG_INFO("=================================");
      NRF_LOG_INFO("BATTERY CHECK START");
      float v = ina_bus_voltage();
      NRF_LOG_INFO("CALCULATED BUS VOLTAGE: " NRF_LOG_FLOAT_MARKER " V", NRF_LOG_FLOAT(v));
      if (v < 2.5 || v > 5.5) {
        NRF_LOG_ERROR("INVALID VOLTAGE RANGE -> SENSOR FAULT");
        return 0;
      }
      float raw_soc = soc_from_voltage(v);
      NRF_LOG_INFO("RAW SOC CALCULATED: " NRF_LOG_FLOAT_MARKER " %%", NRF_LOG_FLOAT(raw_soc));
      ;
      float soc = soc_filter(raw_soc);
      NRF_LOG_INFO("FILTERED SOC: " NRF_LOG_FLOAT_MARKER " %%", NRF_LOG_FLOAT(soc));
      ;
      //  NRF_LOG_INFO("FILTERED SOC: %.2f %%", soc);
      if (v < BAT_LOW_LIMIT) {
        NRF_LOG_WARNING("BATTERY LOW CONDITION");
        return 0;
      }
      NRF_LOG_INFO("BATTERY STATUS NORMAL");
      NRF_LOG_INFO("BATTERY CHECK END");
      NRF_LOG_INFO("=================================");
      return (uint8_t)soc;
    }
    
    // void ina226_init()
    //{
    ////nrf_drv_twi_enable(&m_twi);
    //    uint8_t config[3];
    
    //    config[0] = 0x00;
    //    config[1] = 0x45;
    //    config[2] = 0x27;
    
    //  //  NRF_LOG_INFO("Initializing INA226...");
    // nrf_delay_ms(100);
    //    if(i2c_write(INA226_ADDR, config, 3))
    //    {
    //       // NRF_LOG_INFO("INA226 config write SUCCESS");
    //        nrf_delay_ms(100);
    //        }
    //    else
    //    {
    //        NRF_LOG_ERROR("INA226 config write FAILED");
    //         nrf_delay_ms(100);
    //        }
    //    //    nrf_drv_twi_disable(&m_twi); // <--- Add this
    //}
    
    /**
     * @brief Industrial-safe I2C Scan with Bus Recovery
     */
    
    // void perform_system_update(void) {
    //    // 1. Power up the bus
    //   // nrf_drv_twi_enable(&m_twi);
    
    //    // 2. Physical reads (Keep these fast)
    //    sht41_read();            // Climate
    //    float v_bus = ina_bus_voltage(); // Battery raw
    
    //    // 3. Power down the bus immediately
    //    nrf_drv_twi_disable(&m_twi);
    
    //    // 4. Data Processing (CPU only, no I2C needed here)
    //    uint16_t rawTemp = (m_buffer[0] << 8) | m_buffer[1];
    //    uint16_t rawHum  = (m_buffer[3] << 8) | m_buffer[4];
    //    temperature = -45 + 175 * ((float)rawTemp / 65535.0);
    //    humidity    = -6 + 125 * ((float)rawHum / 65535.0);
    
    //    float raw_soc = soc_from_voltage(v_bus);
    //    uint8_t soc = (uint8_t)soc_filter(raw_soc);
    
    //    // 5. Update BLE Services
    //    ble_cus_temperature_update(&m_cus, (int16_t)temperature);
    //    ble_cus_humidity_update(&m_cus, (int16_t)humidity);
    //    battery_level_update(soc);
    
    //    NRF_LOG_INFO("Update: %d C, %d%% Hum, %d%% Batt", (int)temperature, (int)humidity, soc);
    //}
    
    void perform_system_update(void) {
      // REMOVE: nrf_drv_twi_enable(&m_twi);
    
      sht41_read();
      float v_bus = ina_bus_voltage();
    
      // REMOVE: nrf_drv_twi_disable(&m_twi); <--- THIS WAS THE KILLER
    
      uint16_t rawTemp = (m_buffer[0] << 8) | m_buffer[1];
      uint16_t rawHum  = (m_buffer[3] << 8) | m_buffer[4];
      temperature      = -45 + 175 * ((float)rawTemp / 65535.0);
      humidity         = -6 + 125 * ((float)rawHum / 65535.0);
    
      float raw_soc = soc_from_voltage(v_bus);
      uint8_t soc   = (uint8_t)soc_filter(raw_soc);
    
      ble_cus_temperature_update(&m_cus, (int16_t)temperature);
      ble_cus_humidity_update(&m_cus, (int16_t)humidity);
      battery_level_update(soc);
    
      NRF_LOG_INFO("Update: %d C, %d%% Hum, %d%% Batt", (int)temperature, (int)humidity, soc);
    }
    
    // void idle_state_handle(void) {
    //   if (NRF_LOG_PROCESS() == false) {
    //   }
    //    //  __WFE();
    //  //  ws2812_set_color(0,0,0);     // OFF
    //  //ws2812_set_color(255,0,0);
    //  //nrf_delay_ms(300);
    //  ws2812_set_color(255,0,0);
    //  nrf_delay_ms(300);
    //     nrf_pwr_mgmt_run();
    
    // //    ws2812_set_color(0,0,255);
    // //nrf_delay_ms(300);
    //  //NRF_LOG_INFO("Sleep check: session_done=%d sensor=%d ack=%d",
    //    //         g_session_done, g_do_sensor_read, g_ack_received);
    //  //if (!g_do_sensor_read &&
    //  //    !g_ack_received &&
    //  //    !g_session_done &&
    //  //    NRF_LOG_PROCESS() == false) {
    //  //  nrf_pwr_mgmt_run();
    //  //}
    // // }
    //}
    
    //void idle_state_handle(void) {
    //  //  if (!NRF_LOG_PROCESS()) {
    //  nrf_pwr_mgmt_run();
    //  // }
    //}
    
    static void idle_state_handle(void)
    {
        if (NRF_LOG_PROCESS() == false)
        {
            nrf_pwr_mgmt_run();
        }
    }
    
    #define HTU21D_ADDR 0x40
    #define HTU21D_TEMP_CMD 0xE3
    #define HTU21D_HUM_CMD 0xE5
    #define INA226_ADDR 0x40
    #define SHT41_ADDR 0x44
    
    bool htu21d_read(float* temp, float* hum) {
      ret_code_t err;
      uint8_t cmd;
      uint8_t data[3];
    
      /* ---------- TEMPERATURE ---------- */
    
      cmd = HTU21D_TEMP_CMD;
    
      err = nrf_drv_twi_tx(&m_twi, HTU21D_ADDR, &cmd, 1, false);
      if (err != NRF_SUCCESS)
        return false;
      err = nrf_drv_twi_rx(&m_twi, HTU21D_ADDR, data, 3);
      if (err != NRF_SUCCESS)
        return false;
    
      uint16_t rawTemp = (data[0] << 8) | data[1];
      rawTemp &= 0xFFFC;
    
      *temp = -46.85 + 175.72 * ((float)rawTemp / 65536.0);
    
      /* ---------- HUMIDITY ---------- */
    
      cmd = HTU21D_HUM_CMD;
    
      err = nrf_drv_twi_tx(&m_twi, HTU21D_ADDR, &cmd, 1, false);
      if (err != NRF_SUCCESS)
        return false;
    
      //nrf_delay_ms(60);
    
      err = nrf_drv_twi_rx(&m_twi, HTU21D_ADDR, data, 3);
      if (err != NRF_SUCCESS)
        return false;
    
      uint16_t rawHum = (data[0] << 8) | data[1];
      rawHum &= 0xFFFC;
    
      *hum = -6 + 125 * ((float)rawHum / 65536.0);
    
      return true;
    }
    
    void process_data() {
      float temp, hum;
    
      if (!htu21d_read(&temp, &hum)) {
        NRF_LOG_ERROR("HTU21D read failed");
        return;
      }
      temperature = temp;
      humidity    = hum;
      NRF_LOG_INFO("Temperature: %d C", (int)temperature);
      NRF_LOG_INFO("Humidity: %d %%", (int)humidity);
    
      uint8_t batt = get_filtered_battery_soc();
    
      battery_level_update(batt);
      ble_cus_temperature_update(&m_cus, (int16_t)(temperature * 100));
      ble_cus_humidity_update(&m_cus, (int16_t)(humidity * 100));
      // ble_cus_temperature_update(&m_cus,(int16_t)temperature);
      // ble_cus_humidity_update(&m_cus,(int16_t)humidity);
    }
    
    /**@brief Function for application main entry.
     */
    int main(void) {
      bool erase_bonds;
      // Initialize.
       log_init();
      timers_init();
       buttons_leds_init(&erase_bonds);
       power_management_init();
      // NRF_LOG_INFO("before the blestack function.");
       ble_stack_init();
       ret_code_t err_code = sd_power_dcdc_mode_set(NRF_POWER_DCDC_ENABLE);
       APP_ERROR_CHECK(err_code);
       gap_params_init();
       gatt_init();
       services_init();
       advertising_init();
       err_code = sd_ble_gap_tx_power_set(BLE_GAP_TX_POWER_ROLE_ADV, m_advertising.adv_handle, -8);
       APP_ERROR_CHECK(err_code);
       conn_params_init();
       peer_manager_init();
       //nrf_delay_ms(200);
        //twi_init();
        ws2812_init();
       //ina226_init();
      application_timers_start();
       advertising_start(erase_bonds);
       NRF_LOG_INFO("After all initialization");
       //sht41_read();
       //process_data();
      int cnt               = 0;
      bool interval_changed = false; 
      for (;;) {
        if (updtmrexp) {
          //check_why_i_woke_up();
          updtmrexp        = false;
          g_do_sensor_read = false;
           if (tx_enable == 1) {
             NRF_LOG_INFO("TX enabled -> reading sensor");
             //sht41_read();
             //battery_level_update(70);
             //process_data();
           } else {
             NRF_LOG_WARNING("TX disabled -> skipping sensor read");
           }
            if (g_interval_changed) {
              g_interval_changed = false;
              // Prevent invalid interval (safety guard)
              if (new_tx_interval == 0)
                new_tx_interval = 8;    // fallback to 8 seconds
              tx_interval = new_tx_interval;
              NRF_LOG_INFO("Applying new tx_interval: %d sec", tx_interval);
              uint32_t ticks = APP_TIMER_TICKS(tx_interval * 1000UL);
              ret_code_t err;
              // === SAFE STOP ===
              err = app_timer_stop(m_timer_id);
              if (err != NRF_SUCCESS && err != NRF_ERROR_INVALID_STATE) {
                NRF_LOG_WARNING("app_timer_stop() failed: 0x%08X", err);
                // Do NOT reset device in production
              }
              // === SAFE RESTART ===
              err = app_timer_start(m_timer_id, ticks, NULL);
              if (err != NRF_SUCCESS) {
                NRF_LOG_ERROR("app_timer_start() failed: 0x%08X", err);
                // Optional: try again once
                //nrf_delay_ms(10);
                err = app_timer_start(m_timer_id, ticks, NULL);
              }
              if (err == NRF_SUCCESS) {
                NRF_LOG_INFO("Timer successfully restarted with new interval");
              } else
                NRF_LOG_ERROR("Failed to restart timer!");
            }
          }
        //  ws2812_set_color(100,0,0); // Cyan
        idle_state_handle();
        }
    }
    /**
     * Copyright (c) 2016 - 2019, Nordic Semiconductor ASA
     *
     * All rights reserved.
     *
     * Redistribution and use in source and binary forms, with or without modification,
     * are permitted provided that the following conditions are met:
     *
     * 1. Redistributions of source code must retain the above copyright notice, this
     *    list of conditions and the following disclaimer.
     *
     * 2. Redistributions in binary form, except as embedded into a Nordic
     *    Semiconductor ASA integrated circuit in a product or a software update for
     *    such product, must reproduce the above copyright notice, this list of
     *    conditions and the following disclaimer in the documentation and/or other
     *    materials provided with the distribution.
     *
     * 3. Neither the name of Nordic Semiconductor ASA nor the names of its
     *    contributors may be used to endorse or promote products derived from this
     *    software without specific prior written permission.
     *
     * 4. This software, with or without modification, must only be used with a
     *    Nordic Semiconductor ASA integrated circuit.
     *
     * 5. Any software provided in binary form under this license must not be reverse
     *    engineered, decompiled, modified and/or disassembled.
     *
     * THIS SOFTWARE IS PROVIDED BY NORDIC SEMICONDUCTOR ASA "AS IS" AND ANY EXPRESS
     * OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
     * OF MERCHANTABILITY, NONINFRINGEMENT, AND FITNESS FOR A PARTICULAR PURPOSE ARE
     * DISCLAIMED. IN NO EVENT SHALL NORDIC SEMICONDUCTOR ASA OR CONTRIBUTORS BE
     * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
     * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
     * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
     * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
     * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
     * OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
     *
     */
    #include "sdk_common.h"
    #if NRF_MODULE_ENABLED(NRF_PWR_MGMT)
    
    #include "nrf_pwr_mgmt.h"
    #include "nrf.h"
    #include "nrf_mtx.h"
    #include "nrf_power.h"
    #include "app_error.h"
    #include "nrf_assert.h"
    #include "nrf_log_ctrl.h"
    #include "app_util_platform.h"
    
    #define NRF_LOG_MODULE_NAME pwr_mgmt
    #if NRF_PWR_MGMT_CONFIG_LOG_ENABLED
        #define NRF_LOG_LEVEL       NRF_PWR_MGMT_CONFIG_LOG_LEVEL
        #define NRF_LOG_INFO_COLOR  NRF_PWR_MGMT_CONFIG_INFO_COLOR
        #define NRF_LOG_DEBUG_COLOR NRF_PWR_MGMT_CONFIG_DEBUG_COLOR
    #else
        #define NRF_LOG_LEVEL       0
    #endif // NRF_PWR_MGMT_CONFIG_LOG_ENABLED
    #include "nrf_log.h"
    NRF_LOG_MODULE_REGISTER();
    
    #ifdef SOFTDEVICE_PRESENT
        #include "nrf_soc.h"
        #include "nrf_sdh.h"
    #endif // SOFTDEVICE_PRESENT
    
    
    #if NRF_PWR_MGMT_CONFIG_USE_SCHEDULER
        #if (APP_SCHEDULER_ENABLED != 1)
            #error "APP_SCHEDULER is required."
        #endif
        #include "app_scheduler.h"
    #endif // NRF_PWR_MGMT_CONFIG_USE_SCHEDULER
    
    
    // Create section "pwr_mgmt_data".
    NRF_SECTION_SET_DEF(pwr_mgmt_data,
                        nrf_pwr_mgmt_shutdown_handler_t,
                        NRF_PWR_MGMT_CONFIG_HANDLER_PRIORITY_COUNT);
    
    static nrf_pwr_mgmt_evt_t   m_pwr_mgmt_evt;     /**< Event type which will be passed to the shutdown
                                                         handlers.*/
    static nrf_mtx_t            m_sysoff_mtx;       /**< Module API lock.*/
    static bool                 m_shutdown_started; /**< True if application started the shutdown preparation. */
    static nrf_section_iter_t   m_handlers_iter;    /**< Shutdown handlers iterator. */
    
    #if (NRF_PWR_MGMT_CONFIG_FPU_SUPPORT_ENABLED && __FPU_PRESENT)
        #define PWR_MGMT_FPU_SLEEP_PREPARE()     pwr_mgmt_fpu_sleep_prepare()
    
         __STATIC_INLINE void pwr_mgmt_fpu_sleep_prepare(void)
         {
            uint32_t original_fpscr;
    
            CRITICAL_REGION_ENTER();
            original_fpscr = __get_FPSCR();
            /*
             * Clear FPU exceptions.
             * Without this step, the FPU interrupt is marked as pending,
             * preventing system from sleeping. Exceptions cleared:
             * - IOC - Invalid Operation cumulative exception bit.
             * - DZC - Division by Zero cumulative exception bit.
             * - OFC - Overflow cumulative exception bit.
             * - UFC - Underflow cumulative exception bit.
             * - IXC - Inexact cumulative exception bit.
             * - IDC - Input Denormal cumulative exception bit.
             */
            __set_FPSCR(original_fpscr & ~0x9Fu);
            __DMB();
            NVIC_ClearPendingIRQ(FPU_IRQn);
            CRITICAL_REGION_EXIT();
    
            /*
             * The last chance to indicate an error in FPU to the user 
             * as the FPSCR is now cleared
             *
             * This assert is related to previous FPU operations 
             * and not power management.
             *
             * Critical FPU exceptions signaled:
             * - IOC - Invalid Operation cumulative exception bit.
             * - DZC - Division by Zero cumulative exception bit.
             * - OFC - Overflow cumulative exception bit.
             */
            ASSERT((original_fpscr & 0x7) == 0);
         }
    #else
        #define PWR_MGMT_FPU_SLEEP_PREPARE()
    #endif // NRF_PWR_MGMT_CONFIG_FPU_SUPPORT_ENABLED
    
    
    #if NRF_PWR_MGMT_CONFIG_DEBUG_PIN_ENABLED
        #undef  PWR_MGMT_SLEEP_IN_CRITICAL_SECTION_REQUIRED
        #define PWR_MGMT_SLEEP_IN_CRITICAL_SECTION_REQUIRED
    
        #include "nrf_gpio.h"
        #define PWR_MGMT_DEBUG_PINS_INIT()   pwr_mgmt_debug_pins_init()
        #define PWR_MGMT_DEBUG_PIN_CLEAR()   nrf_gpio_pin_clear(NRF_PWR_MGMT_SLEEP_DEBUG_PIN)
        #define PWR_MGMT_DEBUG_PIN_SET()     nrf_gpio_pin_set(NRF_PWR_MGMT_SLEEP_DEBUG_PIN)
    
        __STATIC_INLINE void pwr_mgmt_debug_pins_init(void)
        {
            nrf_gpio_pin_clear(NRF_PWR_MGMT_SLEEP_DEBUG_PIN);
            nrf_gpio_cfg_output(NRF_PWR_MGMT_SLEEP_DEBUG_PIN);
        }
    
    #else
        #define PWR_MGMT_DEBUG_PIN_CLEAR()
        #define PWR_MGMT_DEBUG_PIN_SET()
        #define PWR_MGMT_DEBUG_PINS_INIT()
    #endif
    
    
    #if NRF_PWR_MGMT_CONFIG_CPU_USAGE_MONITOR_ENABLED
        #undef  PWR_MGMT_SLEEP_IN_CRITICAL_SECTION_REQUIRED
        #define PWR_MGMT_SLEEP_IN_CRITICAL_SECTION_REQUIRED
    
        #undef  PWR_MGMT_TIMER_REQUIRED
        #define PWR_MGMT_TIMER_REQUIRED
        #include "app_timer.h"
    
        #define PWR_MGMT_CPU_USAGE_MONITOR_INIT()    pwr_mgmt_cpu_usage_monitor_init()
        #define PWR_MGMT_CPU_USAGE_MONITOR_UPDATE()  pwr_mgmt_cpu_usage_monitor_update()
        #define PWR_MGMT_CPU_USAGE_MONITOR_SUMMARY() NRF_LOG_INFO("Maximum CPU usage: %u%%", \
                                                                  m_max_cpu_usage)
        #define PWR_MGMT_CPU_USAGE_MONITOR_SECTION_ENTER()  \
            {                                               \
                uint32_t sleep_start = app_timer_cnt_get()
    
        #define PWR_MGMT_CPU_USAGE_MONITOR_SECTION_EXIT()                       \
                uint32_t sleep_end = app_timer_cnt_get();                       \
                uint32_t sleep_duration;                                        \
                sleep_duration = app_timer_cnt_diff_compute(sleep_end,          \
                                                           sleep_start);        \
                m_ticks_sleeping += sleep_duration;                             \
            }
    
        static uint32_t m_ticks_sleeping;    /**< Number of ticks spent in sleep mode (__WFE()). */
        static uint32_t m_ticks_last;        /**< Number of ticks from the last CPU usage computation. */
        static uint8_t  m_max_cpu_usage;     /**< Maximum observed CPU usage (0 - 100%). */
    
        __STATIC_INLINE void pwr_mgmt_cpu_usage_monitor_init(void)
        {
            m_ticks_sleeping    = 0;
            m_ticks_last        = 0;
            m_max_cpu_usage     = 0;
        }
    
        __STATIC_INLINE void pwr_mgmt_cpu_usage_monitor_update(void)
        {
            uint32_t delta;
            uint32_t ticks;
            uint8_t  cpu_usage;
    
            ticks = app_timer_cnt_get();
            delta = app_timer_cnt_diff_compute(ticks, m_ticks_last);
            cpu_usage = 100 * (delta - m_ticks_sleeping) / delta;
    
            NRF_LOG_INFO("CPU Usage: %u%%", cpu_usage);
            if (m_max_cpu_usage < cpu_usage)
            {
                m_max_cpu_usage = cpu_usage;
            }
    
            m_ticks_last        = ticks;
            m_ticks_sleeping    = 0;
        }
    
    #else
        #define PWR_MGMT_CPU_USAGE_MONITOR_INIT()
        #define PWR_MGMT_CPU_USAGE_MONITOR_UPDATE()
        #define PWR_MGMT_CPU_USAGE_MONITOR_SUMMARY()
        #define PWR_MGMT_CPU_USAGE_MONITOR_SECTION_ENTER()
        #define PWR_MGMT_CPU_USAGE_MONITOR_SECTION_EXIT()
    #endif // NRF_PWR_MGMT_CONFIG_CPU_USAGE_MONITOR_ENABLED
    
    
    #if NRF_PWR_MGMT_CONFIG_STANDBY_TIMEOUT_ENABLED
        #undef  PWR_MGMT_TIMER_REQUIRED
        #define PWR_MGMT_TIMER_REQUIRED
    
        #define PWR_MGMT_STANDBY_TIMEOUT_INIT()  pwr_mgmt_standby_timeout_clear()
        #define PWR_MGMT_STANDBY_TIMEOUT_CLEAR() pwr_mgmt_standby_timeout_clear()
        #define PWR_MGMT_STANDBY_TIMEOUT_CHECK() pwr_mgmt_standby_timeout_check()
    
        static uint16_t m_standby_counter;     /**< Number of seconds from the last activity
                                                    (@ref pwr_mgmt_feed). */
    
        __STATIC_INLINE void pwr_mgmt_standby_timeout_clear(void)
        {
            m_standby_counter = 0;
        }
    
        __STATIC_INLINE void pwr_mgmt_standby_timeout_check(void)
        {
            if (m_standby_counter < NRF_PWR_MGMT_CONFIG_STANDBY_TIMEOUT_S)
            {
                m_standby_counter++;
            }
            else if (m_shutdown_started == false)
            {
                nrf_pwr_mgmt_shutdown(NRF_PWR_MGMT_SHUTDOWN_GOTO_SYSOFF);
            }
        }
    
    #else
        #define PWR_MGMT_STANDBY_TIMEOUT_INIT()
        #define PWR_MGMT_STANDBY_TIMEOUT_CLEAR()
        #define PWR_MGMT_STANDBY_TIMEOUT_CHECK()
    #endif // NRF_PWR_MGMT_CONFIG_STANDBY_TIMEOUT_ENABLED
    
    
    #if NRF_PWR_MGMT_CONFIG_AUTO_SHUTDOWN_RETRY
        #undef  PWR_MGMT_TIMER_REQUIRED
        #define PWR_MGMT_TIMER_REQUIRED
    
        #define PWR_MGMT_AUTO_SHUTDOWN_RETRY() pwr_mgmt_auto_shutdown_retry()
    
        __STATIC_INLINE void pwr_mgmt_auto_shutdown_retry(void)
        {
            if (m_shutdown_started)
            {
                // Try to continue the shutdown procedure.
                nrf_pwr_mgmt_shutdown(NRF_PWR_MGMT_SHUTDOWN_CONTINUE);
            }
        }
    
    #else
        #define PWR_MGMT_AUTO_SHUTDOWN_RETRY()
    #endif // NRF_PWR_MGMT_CONFIG_AUTO_SHUTDOWN_RETRY
    
    
    #ifdef PWR_MGMT_SLEEP_IN_CRITICAL_SECTION_REQUIRED
        #define PWR_MGMT_SLEEP_INIT()           pwr_mgmt_sleep_init()
        #define PWR_MGMT_SLEEP_LOCK_ACQUIRE()   CRITICAL_REGION_ENTER()
        #define PWR_MGMT_SLEEP_LOCK_RELEASE()   CRITICAL_REGION_EXIT()
    
        __STATIC_INLINE void pwr_mgmt_sleep_init(void)
        {
        #ifdef SOFTDEVICE_PRESENT
            ASSERT(current_int_priority_get() >= APP_IRQ_PRIORITY_LOW);
        #endif
            SCB->SCR |= SCB_SCR_SEVONPEND_Msk;
        }
    
    #else
        #define PWR_MGMT_SLEEP_INIT()
        #define PWR_MGMT_SLEEP_LOCK_ACQUIRE()
        #define PWR_MGMT_SLEEP_LOCK_RELEASE()
    #endif // PWR_MGMT_SLEEP_IN_CRITICAL_SECTION_REQUIRED
    
    
    #ifdef PWR_MGMT_TIMER_REQUIRED
        #include "app_timer.h"
        #define PWR_MGMT_TIMER_CREATE()     pwr_mgmt_timer_create()
    
        APP_TIMER_DEF(m_pwr_mgmt_timer);    /**< Timer used by this module. */
    
        /**@brief Handle events from m_pwr_mgmt_timer.
         */
        static void nrf_pwr_mgmt_timeout_handler(void * p_context)
        {
            PWR_MGMT_CPU_USAGE_MONITOR_UPDATE();
            PWR_MGMT_AUTO_SHUTDOWN_RETRY();
            PWR_MGMT_STANDBY_TIMEOUT_CHECK();
        }
    
        __STATIC_INLINE ret_code_t pwr_mgmt_timer_create(void)
        {
            ret_code_t ret_code = app_timer_create(&m_pwr_mgmt_timer,
                                                   APP_TIMER_MODE_REPEATED,
                                                   nrf_pwr_mgmt_timeout_handler);
            if (ret_code != NRF_SUCCESS)
            {
                return ret_code;
            }
    
            return app_timer_start(m_pwr_mgmt_timer, APP_TIMER_TICKS(1000), NULL);
        }
    #else
        #define PWR_MGMT_TIMER_CREATE() NRF_SUCCESS
    #endif // PWR_MGMT_TIMER_REQUIRED
    
    ret_code_t nrf_pwr_mgmt_init(void)
    {
        NRF_LOG_INFO("Init");
    
        m_shutdown_started = false;
        nrf_mtx_init(&m_sysoff_mtx);
        nrf_section_iter_init(&m_handlers_iter, &pwr_mgmt_data);
    
        PWR_MGMT_SLEEP_INIT();
        PWR_MGMT_DEBUG_PINS_INIT();
        PWR_MGMT_STANDBY_TIMEOUT_INIT();
        PWR_MGMT_CPU_USAGE_MONITOR_INIT();
    
        return PWR_MGMT_TIMER_CREATE();
    }
    void ws2812_set_color(uint8_t r, uint8_t g, uint8_t b);
    #include "nrf_delay.h"
    
    void check_why_i_woke_up(void) {
        for (uint8_t i = 0; i < 48; i++) {
            if (NVIC_GetPendingIRQ((IRQn_Type)i)) {
                NRF_LOG_INFO("Woke up due to IRQ ID: %d", i);
            }
        }
    }
    
    void nrf_pwr_mgmt_run(void)
    {
    //__set_FPSCR(__get_FPSCR() & ~(0x0000001F));
    //    (void) __get_FPSCR();
    //    NVIC_ClearPendingIRQ(FPU_IRQn);
    //__disable_irq();
    
    //ws2812_set_color(0,0,0);
    // nrf_delay_ms(300);
        PWR_MGMT_FPU_SLEEP_PREPARE();
      //  ws2812_set_color(0,0,100);
       //nrf_delay_ms(300);
        PWR_MGMT_SLEEP_LOCK_ACQUIRE();
         //ws2812_set_color(100,0,0);
         //nrf_delay_ms(300);
        PWR_MGMT_CPU_USAGE_MONITOR_SECTION_ENTER();
         //ws2812_set_color(100,100,100);
         //nrf_delay_ms(300);
      //   ws2812_set_color(150,150,150);
    // cnt=10000;
    //while(cnt--);
        PWR_MGMT_DEBUG_PIN_SET();
        //ws2812_set_color(150,150,0);
        // nrf_delay_ms(300);
    //     ws2812_set_color(150,150,0);
    // cnt=10000;
    //while(cnt--);
    
        // Wait for an event.
    #ifdef SOFTDEVICE_PRESENT
        if (nrf_sdh_is_enabled())
        {
        sd_nvic_ClearPendingIRQ(SD_EVT_IRQn);
            ret_code_t ret_code = sd_app_evt_wait();
         //    ws2812_set_color(0,100,100);
         //nrf_delay_ms(300);
            ASSERT((ret_code == NRF_SUCCESS) || (ret_code == NRF_ERROR_SOFTDEVICE_NOT_ENABLED));
            UNUSED_VARIABLE(ret_code);
            check_why_i_woke_up();
            // NRF_LOG_INFO("AFTER NRF SD APP EVENT WAIT");
           // ws2812_set_color(100,0,0);
            //nrf_delay_ms(600);
        }
        else
    #endif // SOFTDEVICE_PRESENT
        {
            // Wait for an event.
            __WFE();
            // Clear the internal event register.
            __SEV();
            __WFE();
        }
        PWR_MGMT_DEBUG_PIN_CLEAR();
        PWR_MGMT_CPU_USAGE_MONITOR_SECTION_EXIT();
        PWR_MGMT_SLEEP_LOCK_RELEASE();
    }
    
    void nrf_pwr_mgmt_feed(void)
    {
        NRF_LOG_DEBUG("Feed");
        // It does not stop started shutdown process.
        PWR_MGMT_STANDBY_TIMEOUT_CLEAR();
    }
    
    /**@brief Function runs the shutdown procedure.
     */
    static void shutdown_process(void)
    {
        NRF_LOG_INFO("Shutdown started. Type %d", m_pwr_mgmt_evt);
        // Executing all callbacks.
        for (/* m_handlers_iter is initialized in nrf_pwr_mgmt_init(). Thanks to that each handler is
                called only once.*/;
             nrf_section_iter_get(&m_handlers_iter) != NULL;
             nrf_section_iter_next(&m_handlers_iter))
        {
            nrf_pwr_mgmt_shutdown_handler_t * p_handler =
                (nrf_pwr_mgmt_shutdown_handler_t *) nrf_section_iter_get(&m_handlers_iter);
            if ((*p_handler)(m_pwr_mgmt_evt))
            {
                NRF_LOG_INFO("SysOff handler 0x%08X => ready", (unsigned int)*p_handler);
            }
            else
            {
                // One of the modules is not ready.
                NRF_LOG_INFO("SysOff handler 0x%08X => blocking", (unsigned int)*p_handler);
                return;
            }
        }
    
        PWR_MGMT_CPU_USAGE_MONITOR_SUMMARY();
        NRF_LOG_INFO("Shutdown complete.");
        NRF_LOG_FINAL_FLUSH();
    
        if ((m_pwr_mgmt_evt == NRF_PWR_MGMT_EVT_PREPARE_RESET)
         || (m_pwr_mgmt_evt == NRF_PWR_MGMT_EVT_PREPARE_DFU))
        {
            NVIC_SystemReset();
        }
        else
        {
            // Enter System OFF.
    #ifdef SOFTDEVICE_PRESENT
            if (nrf_sdh_is_enabled())
            {
                ret_code_t ret_code = sd_power_system_off();
                ASSERT((ret_code == NRF_SUCCESS) || (ret_code == NRF_ERROR_SOFTDEVICE_NOT_ENABLED));
                UNUSED_VARIABLE(ret_code);
    #ifdef DEBUG
                while (true)
                {
                    /* Since the CPU is kept on in an emulated System OFF mode, it is recommended
                     * to add an infinite loop directly after entering System OFF, to prevent
                     * the CPU from executing code that normally should not be executed. */
                    __WFE();
    
                }
    #endif
            }
    #endif // SOFTDEVICE_PRESENT
            nrf_power_system_off();
        }
    }
    
    #if NRF_PWR_MGMT_CONFIG_USE_SCHEDULER
    /**@brief Handle events from app_scheduler.
     */
    static void scheduler_shutdown_handler(void * p_event_data, uint16_t event_size)
    {
        UNUSED_PARAMETER(p_event_data);
        UNUSED_PARAMETER(event_size);
        shutdown_process();
    }
    #endif // NRF_PWR_MGMT_CONFIG_USE_SCHEDULER
    
    void nrf_pwr_mgmt_shutdown(nrf_pwr_mgmt_shutdown_t shutdown_type)
    {
        // Check if shutdown procedure is not started.
        if (!nrf_mtx_trylock(&m_sysoff_mtx))
        {
            return;
        }
    
        if (shutdown_type != NRF_PWR_MGMT_SHUTDOWN_CONTINUE)
        {
            if (m_shutdown_started)
            {
                nrf_mtx_unlock(&m_sysoff_mtx);
                return;
            }
            else
            {
                m_pwr_mgmt_evt      = (nrf_pwr_mgmt_evt_t)shutdown_type;
                m_shutdown_started  = true;
            }
        }
    
        ASSERT(m_shutdown_started);
        NRF_LOG_INFO("Shutdown request %d", shutdown_type);
    
    #if NRF_PWR_MGMT_CONFIG_USE_SCHEDULER
        ret_code_t ret_code = app_sched_event_put(NULL, 0, scheduler_shutdown_handler);
        APP_ERROR_CHECK(ret_code);
    #else
        shutdown_process();
    #endif // NRF_PWR_MGMT_CONFIG_USE_SCHEDULER
    
        nrf_mtx_unlock(&m_sysoff_mtx);
    }
    
    #endif // NRF_MODULE_ENABLED(NRF_PWR_MGMT)
    

Reply
  • The current consumption of the board is same like the 1.6Milli amphs and it is not changing so thats how i am knowing that the board did not went into the sleep what do you say about this any improvements please tell m

    #include "app_error.h"
    #include "app_timer.h"
    #include "ble.h"
    #include "ble_advdata.h"
    #include "ble_advertising.h"
    #include "ble_bas.h"
    #include "ble_conn_params.h"
    #include "ble_conn_state.h"
    #include "ble_cus.h"
    #include "ble_dis.h"
    #include "ble_err.h"
    #include "ble_hci.h"
    #include "ble_srv_common.h"
    #include "bsp_btn_ble.h"
    #include "fds.h"
    #include "nordic_common.h"
    #include "nrf.h"
    #include "nrf_ble_gatt.h"
    #include "nrf_ble_gq.h"
    #include "nrf_ble_qwr.h"
    #include "nrf_delay.h"
    #include "nrf_drv_twi.h"
    #include "nrf_gpio.h"
    #include "nrf_log.h"
    #include "nrf_log_ctrl.h"
    #include "nrf_log_default_backends.h"
    #include "nrf_pwr_mgmt.h"
    #include "nrf_sdh.h"
    #include "nrf_sdh_ble.h"
    #include "nrf_sdh_soc.h"
    #include "nrfx_pwm.h"
    #include "pca10056.h"
    #include "peer_manager.h"
    #include "peer_manager_handler.h"
    #include "sensorsim.h"
    #include <math.h>
    #include <stdbool.h>
    #include <stdint.h>
    #include <string.h>
    /*ADDED FOR CUSTOM TEMPERATURE READ CHARACTERSTIC*/
    #include "ble_srv_common.h"
    /*ADDED FOR CUSTOM TEMPERATURE READ CHARACTERSTIC*/
    
    #define TWI_INSTANCE_ID 0
    
    #define I2C_INSTANCE 0
    
    #define INA226_ADDR 0x40
    #define SHT4X_ADDR 0x44
    
    #define TWIM_TIMEOUT 5
    
    #define EMA_ALPHA 0.25f
    #define TREND_WINDOW 10
    
    #define SPIKE_CLAMP 8.0f
    #define TREND_DEADBAND 0.3f
    
    #define RATE_UP 1.0f
    #define RATE_DOWN 2.0f
    
    #define BAT_LOW_LIMIT 3.30f
    #define BAT_FULL 4.20f
    
    volatile uint8_t ack          = 0;
    volatile uint8_t tx_enable    = 1;
    volatile uint32_t tx_interval = 60;
    
    volatile uint8_t new_tx_enable    = 0;
    volatile uint32_t new_tx_interval = 0;
    
    /*
    uint8_t ack = 1;
    uint8_t config_ack = 1;
    
    ble_cus_ack_update(&m_cus, ack);
    ble_cus_config_ack_update(&m_cus, config_ack);
    */
    #define SHT41_ADDR 0x44
    #define SHT41_MEASURE_HIGH_PRECISION 0xFD
    
    static const nrf_drv_twi_t m_twi = NRF_DRV_TWI_INSTANCE(TWI_INSTANCE_ID);
    
    static volatile bool m_xfer_done = false;
    volatile bool g_do_sensor_read   = false;
    volatile bool g_config_received  = false;
    volatile bool g_ack_received     = false;
    volatile bool g_session_done     = false;
    
    volatile bool g_interval_changed = false;
    
    static float ema_soc     = 0;
    static float display_soc = 0;
    static float hist[TREND_WINDOW];
    static uint8_t hist_index = 0;
    static bool initialized   = false;
    
    uint8_t m_buffer[6];
    
    // static bool hts_indication_enabled        = false;
    // static bool humidity_notification_enabled = false;
    
    #define DEVICE_NAME "NORD_003"                   /**< Name of device. Will be included in the advertising data. */
    #define MANUFACTURER_NAME "Nordic Semiconductor" /**< Manufacturer. Will be passed to Device Information Service. */
    #define MODEL_NUM "NRF52832"                     /**< Model number. Will be passed to Device Information Service. */
    #define MANUFACTURER_ID \
      0x1122334455 /**< Manufacturer ID, part of System ID. Will be passed to Device Information Service. */
    #define ORG_UNIQUE_ID \
      0x667788     /**< Organizational Unique ID, part of System ID. Will be passed to Device Information Service. */
    
    #define APP_BLE_OBSERVER_PRIO 3 /**< Application's BLE observer priority. You shouldn't need to modify this value. */
    #define APP_BLE_CONN_CFG_TAG 1  /**< A tag identifying the SoftDevice BLE configuration. */
    #define APP_ADV_INTERVAL 100 /**< The advertising interval (in units of 0.625 ms. This value corresponds to 25 ms). */
    
    #define APP_ADV_DURATION 0   /**< The advertising duration (180 seconds) in units of 10 milliseconds. */
    
    #define BATTERY_LEVEL_MEAS_INTERVAL APP_TIMER_TICKS(2000) /**< Battery level measurement interval (ticks). */
    #define MIN_BATTERY_LEVEL 81  /**< Minimum battery level as returned by the simulated measurement function. */
    #define MAX_BATTERY_LEVEL 100 /**< Maximum battery level as returned by the simulated measurement function. */
    #define BATTERY_LEVEL_INCREMENT                                                                                    \
      1    /**< Value by which the battery level is incremented/decremented for each call to the simulated measurement \
              function. */
    
    #define TEMP_TYPE_AS_CHARACTERISTIC \
      0    /**< Determines if temperature type is given as characteristic (1) or as a field of measurement (0). */
    
    #define MIN_CELCIUS_DEGREES                                                                                          \
      3688 /**< Minimum temperature in celcius for use in the simulated measurement function (multiplied by 100 to avoid \
              floating point arithmetic). */
    #define MAX_CELCIUS_DEGRESS                                                                                          \
      3972 /**< Maximum temperature in celcius for use in the simulated measurement function (multiplied by 100 to avoid \
              floating point arithmetic). */
    #define CELCIUS_DEGREES_INCREMENT                                                                                     \
      36   /**< Value by which temperature is incremented/decremented for each call to the simulated measurement function \
              (multiplied by 100 to avoid floating point arithmetic). */
    
    #define MIN_CONN_INTERVAL                                                                                            \
      MSEC_TO_UNITS(10, UNIT_1_25_MS)                          /**< Minimum acceptable connection interval (0.5 seconds) \
                                                                */
    #define MAX_CONN_INTERVAL MSEC_TO_UNITS(100, UNIT_1_25_MS) /**< Maximum acceptable connection interval (1 second). */
    #define SLAVE_LATENCY 0                                    /**< Slave latency. */
    #define CONN_SUP_TIMEOUT MSEC_TO_UNITS(4000, UNIT_10_MS)   /**< Connection supervisory timeout (4 seconds). */
    
    #define FIRST_CONN_PARAMS_UPDATE_DELAY                                                                 \
      APP_TIMER_TICKS(5000) /**< Time from initiating event (connect or start of indication) to first time \
                               sd_ble_gap_conn_param_update is called (5 seconds). */
    #define NEXT_CONN_PARAMS_UPDATE_DELAY \
      APP_TIMER_TICKS(                    \
          30000) /**< Time between each call to sd_ble_gap_conn_param_update after the first call (30 seconds). */
    #define MAX_CONN_PARAMS_UPDATE_COUNT                                                        \
      3          /**< Number of attempts before giving up the connection parameter negotiation. \
                  */
    
    #define SEC_PARAM_BOND 1                               /**< Perform bonding. */
    #define SEC_PARAM_MITM 0                               /**< Man In The Middle protection not required. */
    #define SEC_PARAM_LESC 0                               /**< LE Secure Connections not enabled. */
    #define SEC_PARAM_KEYPRESS 0                           /**< Keypress notifications not enabled. */
    #define SEC_PARAM_IO_CAPABILITIES BLE_GAP_IO_CAPS_NONE /**< No I/O capabilities. */
    #define SEC_PARAM_OOB 0                                /**< Out Of Band data not available. */
    #define SEC_PARAM_MIN_KEY_SIZE 7                       /**< Minimum encryption key size. */
    #define SEC_PARAM_MAX_KEY_SIZE 16                      /**< Maximum encryption key size. */
    
    #define DEAD_BEEF \
      0xDEADBEEF /**< Value used as error code on stack dump, can be used to identify stack location on stack unwind. */
    /* This code belongs in main.c*/
    #define NOTIFICATION_INTERVAL APP_TIMER_TICKS(1000)
    
    #define DHT11_PIN 17
    
    APP_TIMER_DEF(m_timer_id);
    
    /* This code belongs in main.c*/
    APP_TIMER_DEF(m_notification_timer_id);
    /* This code belongs in main.c*/
    
    /* This code belongs in main.c*/
    
    APP_TIMER_DEF(m_battery_timer_id);  /**< Battery timer. */
    BLE_BAS_DEF(m_bas);                 /**< Structure used to identify the battery service. */
                                        /**< Structure used to identify the health thermometer service. */
    NRF_BLE_GATT_DEF(m_gatt);           /**< GATT module instance. */
    NRF_BLE_QWR_DEF(m_qwr);             /**< Context for the Queued Write module.*/
    BLE_ADVERTISING_DEF(m_advertising); /**< Advertising module instance. */
    
    NRF_BLE_GQ_DEF(m_ble_gatt_queue,    /**< BLE GATT Queue instance. */
                   NRF_SDH_BLE_PERIPHERAL_LINK_COUNT,
                   NRF_BLE_GQ_QUEUE_SIZE);
    
    BLE_CUS_DEF(m_cus);
    
    static uint16_t m_conn_handle = BLE_CONN_HANDLE_INVALID; /**< Handle of the current connection. */
    // static bool m_hts_meas_ind_conf_pending =
    //     false;                                     /**< Flag to keep track of when an indication confirmation is pending.
    //     */
    static sensorsim_cfg_t m_battery_sim_cfg;          /**< Battery Level sensor simulator configuration. */
    static sensorsim_state_t m_battery_sim_state;      /**< Battery Level sensor simulator state. */
    static sensorsim_cfg_t m_temp_celcius_sim_cfg;     /**< Temperature simulator configuration. */
    static sensorsim_state_t m_temp_celcius_sim_state; /**< Temperature simulator state. */
    
    /*ADDED FOR CUSTOM TEMPERATURE READ CHARACTERSTIC*/
    #define CUSTOM_CHAR_UUID_BASE \
      {0x23, 0xD1, 0xBC, 0xEA, 0x5F, 0x78, 0x23, 0x15, 0xDE, 0xEF, 0x12, 0x12, 0x00, 0x00, 0x00, 0x00}
    #define CUSTOM_CHAR_UUID 0x1523
    
    float temperature, humidity;
    
    static ble_uuid_t m_adv_uuids[] = {
        /*  {BLE_UUID_HEALTH_THERMOMETER_SERVICE, BLE_UUID_TYPE_BLE},*/
        {BLE_MYDEVICE_SERVICE_UUID, BLE_UUID_TYPE_VENDOR_BEGIN},    // ✅ ADD THIS
    };
    
    static volatile bool updtmrexp = false;
    
    // YOUR_JOB: Use UUIDs for service(s) used in your application.
    static void advertising_start(bool erase_bonds);
    static void temperature_measurement_send(void);
    
    /**@brief Callback function for asserts in the SoftDevice.
     *
     * @details This function will be called in case of an assert in the SoftDevice.
     *
     * @warning This handler is an example only and does not fit a final product. You need to analyze
     *          how your product is supposed to react in case of Assert.
     * @warning On assert from the SoftDevice, the system can only recover on reset.
     *
     * @param[in] line_num   Line number of the failing ASSERT call.
     * @param[in] file_name  File name of the failing ASSERT call.
     */
    void assert_nrf_callback(uint16_t line_num, const uint8_t* p_file_name) {
      app_error_handler(DEAD_BEEF, line_num, p_file_name);
    }
    void check_why_i_woke_up(void);
    /**@brief Function for handling Service errors.
     *
     * @details A pointer to this function will be passed to each service which may need to inform the
     *          application about an error.
     *
     * @param[in] nrf_error  Error code containing information about what went wrong.
    // */
    // static void service_error_handler(uint32_t nrf_error) {
    //   APP_ERROR_HANDLER(nrf_error);
    // }
    
    static void service_error_handler(uint32_t nrf_error) {
      if (nrf_error == BLE_ERROR_GATTS_SYS_ATTR_MISSING)
        return;
    
      APP_ERROR_HANDLER(nrf_error);
    }
    
    /**@brief Function for handling Peer Manager events.
     *
     * @param[in] p_evt  Peer Manager event.
     */
    static void pm_evt_handler(pm_evt_t const* p_evt) {
      ret_code_t err_code;
      uint8_t temp_value;
      ble_gatts_value_t gatts_value;    // Declare a variable of type ble_gatts_value_t
    
      pm_handler_on_pm_evt(p_evt);
      pm_handler_flash_clean(p_evt);
    
      switch (p_evt->evt_id) {
      case PM_EVT_CONN_SEC_SUCCEEDED:
        break;
        // Rest of your code...
      case PM_EVT_PEERS_DELETE_SUCCEEDED:
        //  advertising_start(false);    // Start advertising when peers are deleted
        break;
    
      default:
        break;
      }
    }
    
    /* This code belongs in main.c*/
    
    /**@brief Function for handling the Custom Service Service events.
     *
     * @details This function will be called for all Custom Service events which are passed to
     *          the application.
     *
     * @param[in]   p_cus_service  Custom Service structure.
     * @param[in]   p_evt          Event received from the Custom Service.
     *
     */
    /* This code belongs in main.c*/
    
    static void on_cus_evt(ble_cus_t* p_cus_service, ble_cus_evt_t* p_evt) {
      switch (p_evt->evt_type) {
      case BLE_CUS_EVT_NOTIFICATION_ENABLED:
    
        break;
    
      case BLE_CUS_EVT_NOTIFICATION_DISABLED:
        break;
    
      case BLE_CUS_EVT_CONNECTED:
        break;
    
      case BLE_CUS_EVT_DISCONNECTED:
        break;
    
      default:
        // No implementation needed.
        break;
      }
    }
    
    /**@brief Function for performing a battery measurement, and update the Battery Level characteristic in the Battery
     * Service.
     */
    static void battery_level_update(uint8_t battery_level) {
      ret_code_t err_code;
      // uint8_t battery_level;
    
      // battery_level = /*(uint8_t)sensorsim_measure(&m_battery_sim_state, &m_battery_sim_cfg)*/88;
    
      err_code = ble_bas_battery_level_update(&m_bas, battery_level, BLE_CONN_HANDLE_ALL);
      if ((err_code != NRF_SUCCESS) && (err_code != NRF_ERROR_INVALID_STATE) && (err_code != NRF_ERROR_RESOURCES) &&
          (err_code != NRF_ERROR_BUSY) && (err_code != BLE_ERROR_GATTS_SYS_ATTR_MISSING)) {
        APP_ERROR_HANDLER(err_code);
      }
    }
    
    /**@brief Function for handling the Battery measurement timer timeout.
     *
     * @details This function will be called each time the battery level measurement timer expires.
     *
     * @param[in] p_context   Pointer used for passing some arbitrary information (context) from the
     *                        app_start_timer() call to the timeout handler.
     */
    static void battery_level_meas_timeout_handler(void* p_context) {
      UNUSED_PARAMETER(p_context);
      battery_level_update(78);
    }
    
    /**@brief Function for populating simulated health thermometer measurement.
     */
    
    /**@brief Function for the Timer initialization.
     *
     * @details Initializes the timer module. This creates and starts application timers.
     */
    
    static void advertising_stop(void) {
      ret_code_t err_code;
    
      err_code = sd_ble_gap_adv_stop(m_advertising.adv_handle);
      NRF_LOG_INFO("The error code after stopped the advertizing is %d\n", err_code);
      // if (err_code != NRF_SUCCESS &&
      //     err_code != NRF_ERROR_INVALID_STATE)
      //{
      APP_ERROR_CHECK(err_code);
      // }
    }
    
    void timer_handler(void* p_context) {
      updtmrexp        = true;
      g_do_sensor_read = true;
      NRF_LOG_INFO("Timer interrupt fired");
      if (m_conn_handle == BLE_CONN_HANDLE_INVALID) {
      }
    }
    
    static void timers_init(void) {
      ret_code_t err_code;
    
      // Initialize timer module.
      err_code = app_timer_init();
      APP_ERROR_CHECK(err_code);
      err_code = app_timer_create(&m_timer_id, APP_TIMER_MODE_REPEATED, timer_handler);
      APP_ERROR_CHECK(err_code);
    }
    
    /**@brief Function for the GAP initialization.
     *
     * @details This function sets up all the necessary GAP (Generic Access Profile) parameters of the
     *          device including the device name, appearance, and the preferred connection parameters.
     */
    static void gap_params_init(void) {
      ret_code_t err_code;
      ble_gap_conn_params_t gap_conn_params;
      ble_gap_conn_sec_mode_t sec_mode;
      BLE_GAP_CONN_SEC_MODE_SET_OPEN(&sec_mode);
    
      err_code = sd_ble_gap_device_name_set(&sec_mode, (const uint8_t*)DEVICE_NAME, strlen(DEVICE_NAME));
      APP_ERROR_CHECK(err_code);
    
      err_code = sd_ble_gap_appearance_set(BLE_APPEARANCE_GENERIC_THERMOMETER);
      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);
    }
    
    /**@brief Function for  initializing the GATT module.
     */
    static void gatt_init(void) {
      ret_code_t err_code = nrf_ble_gatt_init(&m_gatt, NULL);
      APP_ERROR_CHECK(err_code);
    }
    
    /**@brief Function for simulating and sending one Temperature Measurement.
     */
    
    /**@brief Function for handling Queued Write Module errors.
     *
     * @details A pointer to this function will be passed to each service which may need to inform the
     *          application about an error.
     *
     * @param[in]   nrf_error   Error code containing information about what went wrong.
     */
    static void nrf_qwr_error_handler(uint32_t nrf_error) {
      APP_ERROR_HANDLER(nrf_error);
    }
    
    /**@brief Function for initializing services that will be used by the application.
     *
     * @details Initialize the Health Thermometer, Battery and Device Information services.
     */
    
    /*ADDED FOR CUSTOM TEMPERATURE READ CHARACTERSTIC*/
    /*ADDED FOR CUSTOM TEMPERATURE READ CHARACTERSTIC*/
    static void services_init(void) {
      ret_code_t err_code;
      ble_bas_init_t bas_init;
      ble_dis_init_t dis_init;
      nrf_ble_qwr_init_t qwr_init = {0};
      ble_dis_sys_id_t sys_id;
    
      // Initialize Queued Write Module.
      qwr_init.error_handler = nrf_qwr_error_handler;
    
      err_code = nrf_ble_qwr_init(&m_qwr, &qwr_init);
      APP_ERROR_CHECK(err_code);
    
      // Initialize Health Thermometer Service
    
      // Initialize Battery Service.
      memset(&bas_init, 0, sizeof(bas_init));
    
      // Here the sec level for the Battery Service can be changed/increased.
      bas_init.bl_rd_sec        = SEC_OPEN;
      bas_init.bl_cccd_wr_sec   = SEC_OPEN;
      bas_init.bl_report_rd_sec = SEC_OPEN;
    
      bas_init.evt_handler          = NULL;
      bas_init.support_notification = true;
      bas_init.p_report_ref         = NULL;
      bas_init.initial_batt_level   = 100;
    
      err_code = ble_bas_init(&m_bas, &bas_init);
      APP_ERROR_CHECK(err_code);
    
      // Initialize Device Information Service.
      memset(&dis_init, 0, sizeof(dis_init));
    
      ble_srv_ascii_to_utf8(&dis_init.manufact_name_str, MANUFACTURER_NAME);
      ble_srv_ascii_to_utf8(&dis_init.model_num_str, MODEL_NUM);
    
      sys_id.manufacturer_id            = MANUFACTURER_ID;
      sys_id.organizationally_unique_id = ORG_UNIQUE_ID;
      dis_init.p_sys_id                 = &sys_id;
    
      dis_init.dis_char_rd_sec = SEC_OPEN;
    
      err_code = ble_dis_init(&dis_init);
      APP_ERROR_CHECK(err_code);
      ble_cus_init_t cus_init;
      memset(&cus_init, 0, sizeof(cus_init));
      /* This code belongs in services_init() in main.c*/
      /* This code belongs in services_init in main.c*/
    
      // Set the cus event handler
      cus_init.evt_handler = on_cus_evt;
    
      BLE_GAP_CONN_SEC_MODE_SET_OPEN(&cus_init.temperature_char_attr_md.read_perm);
      BLE_GAP_CONN_SEC_MODE_SET_OPEN(&cus_init.temperature_char_attr_md.write_perm);
    
      // Set permissions for second characteristic (read only open, write no access)
      BLE_GAP_CONN_SEC_MODE_SET_OPEN(&cus_init.humidity_char_attr_md.read_perm);
      BLE_GAP_CONN_SEC_MODE_SET_NO_ACCESS(&cus_init.humidity_char_attr_md.write_perm);
    
      BLE_GAP_CONN_SEC_MODE_SET_OPEN(&cus_init.tx_enable_char_attr_md.read_perm);
      BLE_GAP_CONN_SEC_MODE_SET_OPEN(&cus_init.tx_enable_char_attr_md.write_perm);
    
      BLE_GAP_CONN_SEC_MODE_SET_OPEN(&cus_init.tx_interval_char_attr_md.read_perm);
      BLE_GAP_CONN_SEC_MODE_SET_OPEN(&cus_init.tx_interval_char_attr_md.write_perm);
    
      BLE_GAP_CONN_SEC_MODE_SET_OPEN(&cus_init.ack_char_attr_md.read_perm);
      // BLE_GAP_CONN_SEC_MODE_SET_NO_ACCESS(&cus_init.ack_char_attr_md.write_perm);
      BLE_GAP_CONN_SEC_MODE_SET_OPEN(&cus_init.ack_char_attr_md.write_perm);
    
      BLE_GAP_CONN_SEC_MODE_SET_OPEN(&cus_init.config_ack_char_attr_md.read_perm);
      BLE_GAP_CONN_SEC_MODE_SET_NO_ACCESS(&cus_init.config_ack_char_attr_md.write_perm);
    
      err_code = ble_cus_init(&m_cus, &cus_init);
      APP_ERROR_CHECK(err_code);
    }
    
    /**@brief Function for initializing the sensor simulators.
     */
    static void sensor_simulator_init(void) {
      m_battery_sim_cfg.min          = MIN_BATTERY_LEVEL;
      m_battery_sim_cfg.max          = MAX_BATTERY_LEVEL;
      m_battery_sim_cfg.incr         = BATTERY_LEVEL_INCREMENT;
      m_battery_sim_cfg.start_at_max = true;
    
      sensorsim_init(&m_battery_sim_state, &m_battery_sim_cfg);
    
      // Temperature is in celcius (it is multiplied by 100 to avoid floating point arithmetic).
      m_temp_celcius_sim_cfg.min          = MIN_CELCIUS_DEGREES;
      m_temp_celcius_sim_cfg.max          = MAX_CELCIUS_DEGRESS;
      m_temp_celcius_sim_cfg.incr         = CELCIUS_DEGREES_INCREMENT;
      m_temp_celcius_sim_cfg.start_at_max = false;
    
      sensorsim_init(&m_temp_celcius_sim_state, &m_temp_celcius_sim_cfg);
    }
    
    /**@brief Function for starting application timers.
     */
    static void application_timers_start(void) {
      ret_code_t err_code;
      uint32_t ticks = APP_TIMER_TICKS(tx_interval * 1000);
      err_code       = app_timer_start(m_timer_id, ticks, NULL);
      APP_ERROR_CHECK(err_code);
    }
    
    /**@brief Function for handling the Connection Parameters Module.
     *
     * @details This function will be called for all events in the Connection Parameters Module which
     *          are passed to the application.
     *          @note All this function does is to disconnect. This could have been done by simply
     *                setting the disconnect_on_fail config parameter, but instead we use the event
     *                handler mechanism to demonstrate its use.
     *
     * @param[in] p_evt  Event received from the Connection Parameters Module.
     */
    static void on_conn_params_evt(ble_conn_params_evt_t* p_evt) {
      ret_code_t err_code;
    
      if (p_evt->evt_type == BLE_CONN_PARAMS_EVT_FAILED) {
        err_code = sd_ble_gap_disconnect(m_conn_handle, BLE_HCI_CONN_INTERVAL_UNACCEPTABLE);
        APP_ERROR_CHECK(err_code);
      }
    }
    
    /**@brief Function for handling a Connection Parameters error.
     *
     * @param[in] nrf_error  Error code containing information about what went wrong.
     */
    static void conn_params_error_handler(uint32_t nrf_error) {
      APP_ERROR_HANDLER(nrf_error);
    }
    
    /**@brief Function for initializing the Connection Parameters module.
     */
    static void conn_params_init(void) {
      ret_code_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);
    }
    
    /**@brief Function for putting the chip into sleep mode.
     *
     * @note This function will not return.
     */
    static void sleep_mode_enter(void) {
      ret_code_t err_code;
    
      err_code = bsp_indication_set(BSP_INDICATE_IDLE);
      APP_ERROR_CHECK(err_code);
    
      // Prepare wakeup buttons.
      err_code = bsp_btn_ble_sleep_mode_prepare();
      APP_ERROR_CHECK(err_code);
    
      // Go to system-off mode (this function will not return; wakeup will cause a reset).
      err_code = sd_power_system_off();
      APP_ERROR_CHECK(err_code);
    }
    
    /**@brief Function for handling advertising events.
     *
     * @details This function will be called for advertising events which are passed to the application.
     *
     * @param[in] ble_adv_evt  Advertising event.
     */
    static void on_adv_evt(ble_adv_evt_t ble_adv_evt) {
      ret_code_t err_code;
    
      switch (ble_adv_evt) {
      case BLE_ADV_EVT_FAST:
        NRF_LOG_INFO("Fast advertising.");
        err_code = bsp_indication_set(BSP_INDICATE_ADVERTISING);
        APP_ERROR_CHECK(err_code);
        break;
    
      case BLE_ADV_EVT_IDLE:
        NRF_LOG_INFO("Advertising stopped -> entering low power wait");
        break;
      default:
        break;
      }
    }
    
    /**@brief Function for handling BLE events.
     *
     * @param[in]   p_ble_evt   Bluetooth stack event.
     * @param[in]   p_context   Unused.
     */
    static void ble_evt_handler(ble_evt_t const* p_ble_evt, void* p_context) {
      uint32_t err_code = NRF_SUCCESS;
    
      switch (p_ble_evt->header.evt_id) {
      case BLE_GAP_EVT_CONNECTED:
        NRF_LOG_INFO("Connected.");
        err_code = bsp_indication_set(BSP_INDICATE_CONNECTED);
        APP_ERROR_CHECK(err_code);
        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);
        uint8_t clear_ack = 0;
        uint8_t val       = 0;
        ble_cus_config_ack_update(&m_cus, val);
        break;
    
      case BLE_GAP_EVT_DISCONNECTED:
        NRF_LOG_INFO("Disconnected.");
        ack           = 0;
        m_conn_handle = BLE_CONN_HANDLE_INVALID;
        //  advertising_start(false);
        break;
      case BLE_GAP_EVT_PHY_UPDATE_REQUEST: {
        ble_gap_phys_t const phys = {
            //.rx_phys = BLE_GAP_PHY_1MBPS,
            //.tx_phys = BLE_GAP_PHY_1MBPS,
            .rx_phys = BLE_GAP_PHY_2MBPS,
            .tx_phys = BLE_GAP_PHY_2MBPS,
        };
        err_code = sd_ble_gap_phy_update(p_ble_evt->evt.gap_evt.conn_handle, &phys);
        APP_ERROR_CHECK(err_code);
      } break;
      case BLE_GATTC_EVT_TIMEOUT:
        // Disconnect on GATT Client timeout event.
        NRF_LOG_DEBUG("GATT Client Timeout.");
        err_code = sd_ble_gap_disconnect(p_ble_evt->evt.gattc_evt.conn_handle, BLE_HCI_REMOTE_USER_TERMINATED_CONNECTION);
        APP_ERROR_CHECK(err_code);
        break;
    
      case BLE_GATTS_EVT_TIMEOUT:
        // Disconnect on GATT Server timeout event.
        NRF_LOG_DEBUG("GATT Server Timeout.");
        err_code = sd_ble_gap_disconnect(p_ble_evt->evt.gatts_evt.conn_handle, BLE_HCI_REMOTE_USER_TERMINATED_CONNECTION);
        APP_ERROR_CHECK(err_code);
        break;
      default:
        // No implementation needed.
        break;
      }
    }
    
    /**@brief Function for initializing the BLE stack.
     *
     * @details Initializes the SoftDevice and the BLE event interrupt.
     */
    static void ble_stack_init(void) {
      ret_code_t err_code;
      uint32_t ram_start = 0;
    
      if (!nrf_sdh_is_enabled()) {
        err_code = nrf_sdh_enable_request();
        APP_ERROR_CHECK(err_code);
      }
    
      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_LOG_INFO("SoftDevice enabled state: %d", nrf_sdh_is_enabled());
      NRF_SDH_BLE_OBSERVER(m_ble_observer, APP_BLE_OBSERVER_PRIO, ble_evt_handler, NULL);
    }
    
    /**@brief Function for handling events from the BSP module.
     *
     * @param[in]   event   Event generated by button press.
     */
    static void bsp_event_handler(bsp_event_t event) {
      ret_code_t err_code;
    
      switch (event) {
      case BSP_EVENT_SLEEP:
        sleep_mode_enter();
        break;
    
      case BSP_EVENT_DISCONNECT:
        err_code = sd_ble_gap_disconnect(m_conn_handle, BLE_HCI_REMOTE_USER_TERMINATED_CONNECTION);
        if (err_code != NRF_ERROR_INVALID_STATE) {
          APP_ERROR_CHECK(err_code);
        }
        break;
    
      case BSP_EVENT_WHITELIST_OFF:
        if (m_conn_handle == BLE_CONN_HANDLE_INVALID) {
          err_code = ble_advertising_restart_without_whitelist(&m_advertising);
          if (err_code != NRF_ERROR_INVALID_STATE) {
            APP_ERROR_CHECK(err_code);
          }
        }
        break;
    
      case BSP_EVENT_KEY_0:
        if (m_conn_handle != BLE_CONN_HANDLE_INVALID) {
          // temperature_measurement_send();
        }
        break;
    
      default:
        break;
      }
    }
    
    /**@brief Function for the Peer Manager initialization.
     */
    static void peer_manager_init(void) {
      ble_gap_sec_params_t sec_param;
      ret_code_t err_code;
    
      err_code = pm_init();
      APP_ERROR_CHECK(err_code);
    
      memset(&sec_param, 0, sizeof(ble_gap_sec_params_t));
    
      // Security parameters to be used for all security procedures.
      // sec_param.bond           = SEC_PARAM_BOND;
      // sec_param.mitm           = SEC_PARAM_MITM;   for custom
      // sec_param.lesc           = SEC_PARAM_LESC;
      // sec_param.keypress       = SEC_PARAM_KEYPRESS;
      // sec_param.io_caps        = SEC_PARAM_IO_CAPABILITIES;
      sec_param.bond           = 1;
      sec_param.mitm           = 0;
      sec_param.lesc           = 0;
      sec_param.keypress       = 0;
      sec_param.io_caps        = BLE_GAP_IO_CAPS_NONE;
      sec_param.oob            = SEC_PARAM_OOB;
      sec_param.min_key_size   = SEC_PARAM_MIN_KEY_SIZE;
      sec_param.max_key_size   = SEC_PARAM_MAX_KEY_SIZE;
      sec_param.kdist_own.enc  = 1;
      sec_param.kdist_own.id   = 1;
      sec_param.kdist_peer.enc = 1;
      sec_param.kdist_peer.id  = 1;
    
      err_code = pm_sec_params_set(&sec_param);
      APP_ERROR_CHECK(err_code);
    
      err_code = pm_register(pm_evt_handler);
      APP_ERROR_CHECK(err_code);
    }
    
    /**@brief Clear bond information from persistent storage.
     */
    static void delete_bonds(void) {
      ret_code_t err_code;
    
      NRF_LOG_INFO("Erase bonds!");
    
      err_code = pm_peers_delete();
      APP_ERROR_CHECK(err_code);
    }
    
    /**@brief Function for initializing the Advertising functionality.
     *
     * @details Encodes the required advertising data and passes it to the stack.
     *          Also builds a structure to be passed to the stack when starting advertising.
     */
    static void advertising_init(void) {
      ret_code_t err_code;
      ble_advertising_init_t init;
    
      memset(&init, 0, sizeof(init));
    
      // 🔥 SMALLEST POSSIBLE CONFIG
    
      init.advdata.name_type          = BLE_ADVDATA_FULL_NAME;    // reduce further
      init.advdata.include_appearance = false;                    // 🔥 VERY IMPORTANT
      init.advdata.flags              = BLE_GAP_ADV_FLAGS_LE_ONLY_GENERAL_DISC_MODE;
    
      // 🔥 ONLY ONE UUID (TEMP FIX)
      init.advdata.uuids_complete.uuid_cnt = 1;
      init.advdata.uuids_complete.p_uuids  = &m_adv_uuids[0];
    
      // 🔥 NO scan response
      memset(&init.srdata, 0, sizeof(init.srdata));
    
      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);
    }
    
    /**@brief Function for initializing buttons and leds.
     *
     * @param[out] p_erase_bonds  Will be true if the clear bonding button was pressed to wake the application up.
     */
    static void buttons_leds_init(bool* p_erase_bonds) {
      ret_code_t err_code;
      bsp_event_t startup_event;
    
      err_code = bsp_init(BSP_INIT_LEDS | BSP_INIT_BUTTONS, bsp_event_handler);
      APP_ERROR_CHECK(err_code);
    
      err_code = bsp_btn_ble_init(NULL, &startup_event);
      APP_ERROR_CHECK(err_code);
    
      *p_erase_bonds = (startup_event == BSP_EVENT_CLEAR_BONDING_DATA);
    }
    
    /**@brief Function for initializing the nrf log module.
     */
    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 Function for initializing power management.
     */
    static void power_management_init(void) {
      ret_code_t err_code;
      err_code = nrf_pwr_mgmt_init();
      APP_ERROR_CHECK(err_code);
    }
    
    /**@brief Function for handling the idle state (main loop).
     *
     * @details If there is no pending log operation, then sleep until next the next event occurs.
     */
    
    /**@brief Function for starting advertising.
     */
    static void advertising_start(bool erase_bonds) {
      if (erase_bonds == true) {
        delete_bonds();
        // Advertising is started by PM_EVT_PEERS_DELETE_SUCCEEDED event.
      } else {
        uint32_t err_code = ble_advertising_start(&m_advertising, BLE_ADV_MODE_FAST);
        APP_ERROR_CHECK(err_code);
      }
    }
    
    // void twi_handler(nrf_drv_twi_evt_t const* p_event, void* p_context) {
    //   if (p_event->type == NRF_DRV_TWI_EVT_DONE) {
    //     m_xfer_done = true;
    //   }
    // }
    
    static volatile bool m_xfer_success = false;    // Track actual success
    
    void twi_handler(nrf_drv_twi_evt_t const* p_event, void* p_context) {
      if (p_event->type == NRF_DRV_TWI_EVT_DONE) {
        m_xfer_success = true;
        m_xfer_done    = true;
      } else if (p_event->type == NRF_DRV_TWI_EVT_ADDRESS_NACK || p_event->type == NRF_DRV_TWI_EVT_DATA_NACK) {
        m_xfer_success = false;    // Transaction failed (Sensor not ready/found)
        m_xfer_done    = true;     // Unblock the while loop immediately!
      }
    }
    
    // void sht41_read(void) {
    //   ret_code_t err_code;
    //   uint8_t cmd = SHT41_MEASURE_HIGH_PRECISION;
    
    //  m_xfer_done = false;
    
    //  err_code = nrf_drv_twi_tx(&m_twi, SHT41_ADDR, &cmd, 1, false);
    //  APP_ERROR_CHECK(err_code);
    //  uint32_t timeout = 1000000;
    
    //  while (!m_xfer_done && timeout--) {
    //  }
    
    //  if (timeout == 0) {
    //    NRF_LOG_ERROR("TWI timeout");
    //    return;
    //  }
    //  nrf_delay_ms(10);
    
    //  m_xfer_done = false;
    
    //  err_code = nrf_drv_twi_rx(&m_twi, SHT41_ADDR, m_buffer, 6);
    //  APP_ERROR_CHECK(err_code);
    //  timeout = 1000000;
    //  while (!m_xfer_done && timeout--) {
    //  }
    
    //  if (timeout == 0) {
    //    NRF_LOG_ERROR("TWI timeout");
    //    return;
    //  }
    //}
    
    // void sht41_read(void) {
    //     ret_code_t err_code;
    //     uint8_t cmd = SHT41_MEASURE_HIGH_PRECISION;
    
    //    m_xfer_done = false;
    //    err_code = nrf_drv_twi_tx(&m_twi, SHT41_ADDR, &cmd, 1, false);
    //    APP_ERROR_CHECK(err_code);
    
    //    uint32_t timeout = 10000;
    //    while (!m_xfer_done && timeout > 0) {
    //        nrf_delay_us(10);
    //        timeout--;
    //    }
    
    //    if (timeout == 0) {
    //        // TWI TX Timeout
    //        return;
    //    }
    
    //    nrf_delay_ms(10); // Wait for the SHT41 to perform the physical measurement
    
    //    m_xfer_done = false;
    //    err_code = nrf_drv_twi_rx(&m_twi, SHT41_ADDR, m_buffer, 6);
    //    APP_ERROR_CHECK(err_code);
    
    //    timeout = 10000;
    //    while (!m_xfer_done && timeout > 0) {
    //        nrf_delay_us(10);
    //        timeout--;
    //    }
    
    //    if (timeout == 0) {
    //        // TWI RX Timeout
    //        return;
    //    }
    //}
    
    void sht41_read(void) {
      ret_code_t err_code;
      uint8_t cmd = SHT41_MEASURE_HIGH_PRECISION;
    
      m_xfer_done    = false;
      m_xfer_success = false;
      err_code       = nrf_drv_twi_tx(&m_twi, SHT41_ADDR, &cmd, 1, false);
      if (err_code != NRF_SUCCESS)
        return;    // <-- REMOVED APP_ERROR_CHECK
    
      uint32_t timeout = 10000;
      while (!m_xfer_done && timeout > 0) {
        nrf_delay_us(10);
        timeout--;
      }
    
      if (timeout == 0 || !m_xfer_success)
        return;            // Exit on NACK or timeout
    
      nrf_delay_ms(10);    // Wait for the SHT41 measurement
    
      m_xfer_done    = false;
      m_xfer_success = false;
      err_code       = nrf_drv_twi_rx(&m_twi, SHT41_ADDR, m_buffer, 6);
      if (err_code != NRF_SUCCESS)
        return;    // <-- REMOVED APP_ERROR_CHECK
    
      timeout = 10000;
      while (!m_xfer_done && timeout > 0) {
        nrf_delay_us(10);
        timeout--;
      }
    
      if (timeout == 0 || !m_xfer_success)
        return;
    }
    
    // void sht41_read(void) {
    //     ret_code_t err_code;
    //     uint8_t cmd = SHT41_MEASURE_HIGH_PRECISION;
    
    //    m_xfer_done = false;
    //    m_xfer_success = false;
    //    err_code = nrf_drv_twi_tx(&m_twi, SHT41_ADDR, &cmd, 1, false);
    //    APP_ERROR_CHECK(err_code);
    
    //    uint32_t timeout = 10000;
    //    while (!m_xfer_done && timeout > 0) {
    //        nrf_delay_us(10);
    //        timeout--;
    //    }
    
    //    if (timeout == 0 || !m_xfer_success) return; // Exit on NACK or timeout
    
    //    nrf_delay_ms(10); // Wait for the SHT41 to perform the physical measurement
    
    //    m_xfer_done = false;
    //    m_xfer_success = false;
    //    err_code = nrf_drv_twi_rx(&m_twi, SHT41_ADDR, m_buffer, 6);
    //    APP_ERROR_CHECK(err_code);
    
    //    timeout = 10000;
    //    while (!m_xfer_done && timeout > 0) {
    //        nrf_delay_us(10);
    //        timeout--;
    //    }
    
    //    if (timeout == 0 || !m_xfer_success) return;
    //}
    
    uint8_t battery_percentage(void);
    uint8_t battery_soc_update();
    
    uint8_t get_filtered_battery_soc(void);
    
    // void process_data() {
    //   uint16_t   rawTemp = (m_buffer[0] << 8) | m_buffer[1];
    //   uint16_t   rawHum  = (m_buffer[3] << 8) | m_buffer[4];
    
    //  temperature = -45 + 175 * ((float)rawTemp / 65535.0);
    //  humidity    = -6 + 125 * ((float)rawHum / 65535.0);
    
    //  //NRF_LOG_INFO("Temperature: %d C", (int)temperature);
    //  //NRF_LOG_INFO("Humidity: %d %%", (int)humidity);
    ////  uint8_t batt = battery_percentage();
    
    ////battery_level_update(batt);
    // uint8_t batt = get_filtered_battery_soc();
    //   battery_level_update(batt);
    
    //  nrf_delay_ms(600);
    // //NRF_LOG_INFO("Sending SOC: %d %%", soc);
    //  nrf_delay_ms(600);
    // // if (m_conn_handle != BLE_CONN_HANDLE_INVALID) {
    //  //  battery_level_update(soc);
    //    ble_cus_temperature_update(&m_cus, (int16_t )temperature);
    //    ble_cus_humidity_update(&m_cus, (int16_t )humidity);
    //  //}
    //}
    
    void twi_init(void) {
      ret_code_t err_code;
    
      const nrf_drv_twi_config_t config = {.scl                = ARDUINO_SCL_PIN,
                                           .sda                = ARDUINO_SDA_PIN,
                                           .frequency          = NRF_DRV_TWI_FREQ_100K,
                                           .interrupt_priority = APP_IRQ_PRIORITY_LOW,
                                           .clear_bus_init     = true};
    
      err_code = nrf_drv_twi_init(&m_twi, &config, twi_handler, NULL);
      APP_ERROR_CHECK(err_code);
    
      nrf_drv_twi_enable(&m_twi);
    }
    
    // void twi_init(void) {
    //     ret_code_t err_code;
    
    //    const nrf_drv_twi_config_t config = {
    //        .scl                = ARDUINO_SCL_PIN,
    //        .sda                = ARDUINO_SDA_PIN,
    //        .frequency          = NRF_DRV_TWI_FREQ_100K,
    //        .interrupt_priority = APP_IRQ_PRIORITY_HIGH,
    //        .clear_bus_init     = false
    //    };
    
    //    err_code = nrf_drv_twi_init(&m_twi, &config, twi_handler, NULL);
    //    APP_ERROR_CHECK(err_code);
    // nrf_drv_twi_enable(&m_twi);
    //    // Keep it disabled until actually needed
    //   // nrf_drv_twi_disable(&m_twi);
    //}
    
    #define WS2812_PIN 16
    #define LED_COUNT 1
    
    /* PWM instance */
    static nrfx_pwm_t pwm = NRFX_PWM_INSTANCE(0);
    
    /* WS2812 timing values (PWM duty cycles) */
    #define PWM_PERIOD 20
    
    static uint16_t pwm_buffer[24 * LED_COUNT + 50];
    
    /* Initialize PWM */
    void ws2812_init(void) {
      nrfx_pwm_config_t config = {
          .output_pins  = {WS2812_PIN, NRFX_PWM_PIN_NOT_USED, NRFX_PWM_PIN_NOT_USED, NRFX_PWM_PIN_NOT_USED},
          .irq_priority = APP_IRQ_PRIORITY_LOWEST,
          .base_clock   = NRF_PWM_CLK_16MHz,
          .count_mode   = NRF_PWM_MODE_UP,
          .top_value    = PWM_PERIOD,
          .load_mode    = NRF_PWM_LOAD_COMMON,
          .step_mode    = NRF_PWM_STEP_AUTO};
      nrfx_pwm_init(&pwm, &config, NULL);
    }
    
    #define WS2812_1 (14 | 0x8000)
    #define WS2812_0 (6 | 0x8000)
    #define WS2812_RESET (0 | 0x8000)
    
    void ws2812_set_color(uint8_t r, uint8_t g, uint8_t b) {
      // WS2812 expects GRB
      uint32_t color = ((uint32_t)g << 16) | ((uint32_t)r << 8) | b;
    
      for (int i = 0; i < 24; i++) {
        if (color & (1UL << (23 - i)))
          pwm_buffer[i] = WS2812_1;
        else
          pwm_buffer[i] = WS2812_0;
      }
    
      for (int i = 24; i < 24 + 50; i++)
        pwm_buffer[i] = WS2812_RESET;
    
      nrf_pwm_sequence_t seq = {.values.p_common = pwm_buffer, .length = 24 + 50, .repeats = 0, .end_delay = 0};
    
      nrfx_pwm_simple_playback(&pwm, &seq, 1, NRFX_PWM_FLAG_STOP);
    }
    
    uint16_t ina226_read_register(uint8_t reg) {
      ret_code_t err;
      uint8_t data[2];
    
      m_xfer_done = false;
    
      err = nrf_drv_twi_tx(&m_twi, INA226_ADDR, &reg, 1, true);
      if (err != NRF_SUCCESS) {
        NRF_LOG_ERROR("INA226 TX FAILED");
        return 0;
      }
    
      nrf_delay_ms(2);
    
      m_xfer_done = false;
    
      err = nrf_drv_twi_rx(&m_twi, INA226_ADDR, data, 2);
      if (err != NRF_SUCCESS) {
        NRF_LOG_ERROR("INA226 RX FAILED");
        return 0;
      }
    
      uint16_t raw = (data[0] << 8) | data[1];
    
      return raw;
    }
    
    float ina226_get_bus_voltage(void) {
      NRF_LOG_INFO("INA226 BUS VOLTAGE READ START");
    
      uint16_t raw = ina226_read_register(0x02);
    
      NRF_LOG_INFO("RAW BUS VOLTAGE REGISTER: %d", raw);
    
      float voltage = raw * 1.25f / 1000.0f;
    
      NRF_LOG_INFO("CALCULATED BUS VOLTAGE: %.2f V", voltage);
      //  if(voltage==0) ws2812_set_color(255,0,0);
      return voltage;
    }
    
    uint8_t battery_percentage(void) {
      float v = ina226_get_bus_voltage();
      if (v < 2.5 || v > 5.5) {
        NRF_LOG_ERROR("INVALID VOLTAGE RANGE -> SENSOR FAULT");
        return 0;
      }
      float percent = ((v - 3.3) / (4.2 - 3.3)) * 100;
      if (percent < 0)
        percent = 0;
      if (percent > 100)
        percent = 100;
    
      return (uint8_t)percent;
    }
    
    // bool i2c_write(uint8_t addr, uint8_t *data, uint8_t len)
    //{
    //     ret_code_t err;
    //     m_xfer_done = false; // Reset the flag
    
    //    // Start the transfer
    //    err = nrf_drv_twi_tx(&m_twi, addr, data, len, false);
    //    if(err != NRF_SUCCESS) return false;
    
    //    // CRITICAL: Wait for the hardware to finish sending bits
    //    uint32_t timeout = 100000;
    //    while(!m_xfer_done && timeout--)
    //    {
    //      //  __WFE(); // Wait for event to save power while hardware works
    //    }
    
    //    return m_xfer_done; // Only return true if transfer actually completed
    //}
    
    // bool i2c_write(uint8_t addr, uint8_t *data, uint8_t len)
    //{
    //     ret_code_t err;
    //     m_xfer_done = false;
    
    //    err = nrf_drv_twi_tx(&m_twi, addr, data, len, false);
    //    if(err != NRF_SUCCESS) return false;
    
    //    // Timeout: 10,000 iterations * 10 microseconds = 100 ms max wait
    //    uint32_t timeout = 10000;
    //    while(!m_xfer_done && timeout > 0)
    //    {
    //        nrf_delay_us(10); // Forces the loop to respect real-world time
    //        timeout--;
    //    }
    
    //    return m_xfer_done;
    //}
    
    bool i2c_write(uint8_t addr, uint8_t* data, uint8_t len) {
      ret_code_t err;
      m_xfer_done    = false;
      m_xfer_success = false;
    
      err = nrf_drv_twi_tx(&m_twi, addr, data, len, false);
      if (err != NRF_SUCCESS)
        return false;
    
      uint32_t timeout = 10000;
      while (!m_xfer_done && timeout > 0) {
        nrf_delay_us(10);
        timeout--;
      }
    
      return (m_xfer_done && m_xfer_success);    // Must actually succeed, not just timeout
    }
    
    // bool i2c_write(uint8_t addr, uint8_t *data, uint8_t len)
    //{
    //     ret_code_t err;
    //     for(int i = 0; i < 3; i++)
    //     {
    //         m_xfer_done = false;
    //         err = nrf_drv_twi_tx(&m_twi, addr, data, len, false);
    
    //        if(err != NRF_SUCCESS) return false;
    
    //        // CRITICAL: You MUST wait for the interrupt to fire
    //        uint32_t timeout = 100000;
    //        while(!m_xfer_done && timeout--) {
    //            __WFE(); // Wait for event to save power
    //        }
    
    //        if(m_xfer_done) return true;
    //    }
    //    NRF_LOG_ERROR("I2C WRITE TIMEOUT");
    //    return false;
    //}
    
    // bool i2c_read(uint8_t addr, uint8_t reg, uint8_t *rx, uint8_t len)
    //{
    //     ret_code_t err;
    
    //    NRF_LOG_INFO("---------------------------------");
    //    NRF_LOG_INFO("I2C READ START");
    //    NRF_LOG_INFO("Device Addr: 0x%02X", addr);
    //    NRF_LOG_INFO("Register: 0x%02X", reg);
    // nrf_delay_ms(600);
    //    for(int i = 0; i < 3; i++)
    //    {
    //        NRF_LOG_INFO("I2C TX Attempt %d", i+1);
    // nrf_delay_ms(200);
    //        m_xfer_done = false;
    
    //        err = nrf_drv_twi_tx(&m_twi, addr, &reg, 1, true);
    
    //        if(err != NRF_SUCCESS)
    //        {
    //            NRF_LOG_ERROR("TX START FAILED: %d", err);
    //            continue;
    //        }
    
    //        uint32_t timeout = 1000000;
    
    //        while(!m_xfer_done && timeout--)
    //        {
    //        }
    
    //        if(timeout == 0)
    //        {
    //            NRF_LOG_ERROR("TX TIMEOUT");
    //            continue;
    //        }
    
    //        NRF_LOG_INFO("TX COMPLETE");
    // nrf_delay_ms(300);
    //        m_xfer_done = false;
    
    //        err = nrf_drv_twi_rx(&m_twi, addr, rx, len);
    
    //        if(err != NRF_SUCCESS)
    //        {
    //            NRF_LOG_ERROR("RX START FAILED: %d", err);
    //            continue;
    //        }
    
    //        timeout = 1000000;
    
    //        while(!m_xfer_done && timeout--)
    //        {
    //        }
    
    //        if(timeout == 0)
    //        {
    //            NRF_LOG_ERROR("RX TIMEOUT");
    //            continue;
    //        }
    
    //        NRF_LOG_INFO("RX COMPLETE");
    // nrf_delay_ms(600);
    //        for(int j=0;j<len;j++)
    //        {
    //            NRF_LOG_INFO("RX[%d] = 0x%02X", j, rx[j]);
    //        }
    
    //        return true;
    //    }
    // nrf_delay_ms(600);
    //    NRF_LOG_ERROR("I2C READ FAILED AFTER RETRIES");
    
    //    return false;
    //}
    
    // bool i2c_read(uint8_t addr, uint8_t reg, uint8_t *rx, uint8_t len)
    //{
    //     ret_code_t err;
    
    //    for(int i = 0; i < 3; i++) // Retries
    //    {
    //        m_xfer_done = false;
    
    //        err = nrf_drv_twi_tx(&m_twi, addr, &reg, 1, true);
    //        if(err != NRF_SUCCESS) continue;
    
    //        uint32_t timeout = 10000;
    //        while(!m_xfer_done && timeout > 0)
    //        {
    //            nrf_delay_us(10);
    //            timeout--;
    //        }
    
    //        if(timeout == 0) continue; // TX Timeout
    
    //        m_xfer_done = false;
    //        err = nrf_drv_twi_rx(&m_twi, addr, rx, len);
    //        if(err != NRF_SUCCESS) continue;
    
    //        timeout = 10000;
    //        while(!m_xfer_done && timeout > 0)
    //        {
    //            nrf_delay_us(10);
    //            timeout--;
    //        }
    
    //        if(timeout == 0) continue; // RX Timeout
    
    //        return true; // Success
    //    }
    
    //    return false; // Failed after retries
    //}
    
    bool i2c_read(uint8_t addr, uint8_t reg, uint8_t* rx, uint8_t len) {
      ret_code_t err;
    
      for (int i = 0; i < 3; i++)    // Retries
      {
        m_xfer_done    = false;
        m_xfer_success = false;
    
        err = nrf_drv_twi_tx(&m_twi, addr, &reg, 1, true);
        if (err != NRF_SUCCESS)
          continue;
    
        uint32_t timeout = 10000;
        while (!m_xfer_done && timeout > 0) {
          nrf_delay_us(10);
          timeout--;
        }
    
        if (timeout == 0 || !m_xfer_success)
          continue;    // Retry if timed out or NACKed
    
        m_xfer_done    = false;
        m_xfer_success = false;
    
        err = nrf_drv_twi_rx(&m_twi, addr, rx, len);
        if (err != NRF_SUCCESS)
          continue;
    
        timeout = 10000;
        while (!m_xfer_done && timeout > 0) {
          nrf_delay_us(10);
          timeout--;
        }
    
        if (timeout == 0 || !m_xfer_success)
          continue;     // Retry if timed out or NACKed
    
        return true;    // Success!
      }
    
      return false;     // Failed after 3 retries
    }
    
    uint16_t ina_read(uint8_t reg) {
      uint8_t buf[2];
    
      NRF_LOG_INFO("=================================");
      NRF_LOG_INFO("INA226 REGISTER READ");
      NRF_LOG_INFO("Register Address: 0x%02X", reg);
      if (!i2c_read(INA226_ADDR, reg, buf, 2)) {
        NRF_LOG_ERROR("INA226 REGISTER READ FAILED");
        return 0;
      }
    
      NRF_LOG_INFO("INA226 RAW DATA HIGH BYTE: 0x%02X", buf[0]);
      NRF_LOG_INFO("INA226 RAW DATA LOW BYTE : 0x%02X", buf[1]);
      uint16_t value = (buf[0] << 8) | buf[1];
    
      NRF_LOG_INFO("INA226 REGISTER VALUE: %d", value);
    
      NRF_LOG_INFO("=================================");
      return value;
    }
    
    float ina_current() {
      uint16_t raw = ina_read(0x04);
    
      float current_lsb = 0.001f;
      return raw * current_lsb;
    }
    
    float ina_bus_voltage() {
      NRF_LOG_INFO("INA226 BUS VOLTAGE READ START");
      uint16_t raw = ina_read(0x02);
    
      NRF_LOG_INFO("RAW BUS VOLTAGE REGISTER: %d", raw);
      float voltage = raw * 1.25f / 1000.0f;
      NRF_LOG_INFO("CALCULATED BUS VOLTAGE: " NRF_LOG_FLOAT_MARKER " V", NRF_LOG_FLOAT(voltage));
      voltage *= 4.0f;
      NRF_LOG_INFO("CALCULATED BUS VOLTAGE: " NRF_LOG_FLOAT_MARKER " V", NRF_LOG_FLOAT(voltage));
      NRF_LOG_INFO("INA226 BUS VOLTAGE READ END");
      return voltage;
    }
    
    float soc_from_voltage(float v) {
      if (v >= 4.20)
        return 100;
    
      if (v >= 4.00)
        return 85 + (v - 4.00) * 75;
    
      if (v >= 3.90)
        return 75 + (v - 3.90) * 100;
    
      if (v >= 3.80)
        return 60 + (v - 3.80) * 150;
    
      if (v >= 3.70)
        return 50 + (v - 3.70) * 100;
    
      if (v >= 3.60)
        return 35 + (v - 3.60) * 150;
    
      if (v >= 3.50)
        return 25 + (v - 3.50) * 100;
    
      if (v >= 3.40)
        return 10 + (v - 3.40) * 150;
    
      if (v >= 3.30)
        return (v - 3.30) * 100;
    
      return 0;
    }
    
    // --- INA226 Registers & Config ---
    #define INA226_REG_CONFIG 0x00
    #define INA226_REG_SHUNTVOLTAGE 0x01
    #define INA226_REG_BUSVOLTAGE 0x02
    #define INA226_REG_POWER 0x03
    #define INA226_REG_CURRENT 0x04
    #define INA226_REG_CALIBRATION 0x05
    
    #define INA226_CONFIG_4_SAMPLES 0x4327
    #define INA226_CALIBRATION_VAL 1004
    
    // --- Battery Filter Constants ---
    #define SPIKE_CLAMP_PCT 8.0f
    #define TREND_DEADBAND_PCT 0.3f
    #define RATE_UP_PCT_SAMPLE 1.0f
    #define RATE_DOWN_PCT_SAMPLE 2.0f
    
    static float s_ema_soc     = 0.0f;
    static float s_display_soc = 0.0f;
    static float s_hist[TREND_WINDOW];
    static uint8_t s_hist_idx = 0;
    static bool s_initialised = false;
    
    float soc_filter(float raw) {
      if (!initialized) {
        ema_soc     = raw;
        display_soc = raw;
    
        for (int i = 0; i < TREND_WINDOW; i++)
          hist[i] = raw;
    
        initialized = true;
      }
    
      float clean = (fabsf(raw - ema_soc) > SPIKE_CLAMP) ? ema_soc : raw;
    
      ema_soc = EMA_ALPHA * clean + (1 - EMA_ALPHA) * ema_soc;
    
      hist[hist_index] = ema_soc;
    
      hist_index = (hist_index + 1) % TREND_WINDOW;
    
      float oldest = hist[hist_index];
    
      float trend_delta = ema_soc - oldest;
    
      int8_t trend = 0;
    
      if (trend_delta > TREND_DEADBAND)
        trend = 1;
      if (trend_delta < -TREND_DEADBAND)
        trend = -1;
    
      float error = ema_soc - display_soc;
    
      if (trend >= 0 && error > 0)
        display_soc += fminf(error, RATE_UP);
    
      else if (trend <= 0 && error < 0)
        display_soc += fmaxf(error, -RATE_DOWN);
    
      if (display_soc > 100)
        display_soc = 100;
      if (display_soc < 0)
        display_soc = 0;
    
      return display_soc;
    }
    
    bool ina226_write_register(uint8_t reg, uint16_t value) {
      uint8_t data[3];
      data[0] = reg;
      data[1] = (value >> 8) & 0xFF;
      data[2] = value & 0xFF;
      return i2c_write(INA226_ADDR, data, 3);
    }
    
    void ina226_init(void) {
      // Write Configuration (Averaging: 4 samples)
      if (ina226_write_register(INA226_REG_CONFIG, INA226_CONFIG_4_SAMPLES)) {
        NRF_LOG_INFO("INA226 Configured for 4 Samples.");
      } else {
        NRF_LOG_ERROR("INA226 Config FAILED.");
      }
      nrf_delay_ms(5);
    
      // Write Calibration (Max Current: 2A, Shunt: 0.051 Ohm)
      if (ina226_write_register(INA226_REG_CALIBRATION, INA226_CALIBRATION_VAL)) {
        NRF_LOG_INFO("INA226 Calibration Set.");
      } else {
        NRF_LOG_ERROR("INA226 Calibration FAILED.");
      }
      nrf_delay_ms(5);
    }
    
    float CAL_SoC(float v) {
      if (v >= 4.20f)
        return 100.0f;
      if (v >= 4.00f)
        return 85.0f + (v - 4.00f) * 75.0f;
      if (v >= 3.90f)
        return 75.0f + (v - 3.90f) * 100.0f;
      if (v >= 3.80f)
        return 60.0f + (v - 3.80f) * 150.0f;
      if (v >= 3.70f)
        return 50.0f + (v - 3.70f) * 100.0f;
      if (v >= 3.60f)
        return 35.0f + (v - 3.60f) * 150.0f;
      if (v >= 3.50f)
        return 25.0f + (v - 3.50f) * 100.0f;
      if (v >= 3.40f)
        return 10.0f + (v - 3.40f) * 150.0f;
      if (v >= 3.30f)
        return (v - 3.30f) * 100.0f;
      return 0.0f;
    }
    
    uint8_t get_filtered_battery_soc(void) {
      float battVoltage = ina226_get_bus_voltage();
      battVoltage *= 1.3;
      NRF_LOG_INFO("battVoltage     = " NRF_LOG_FLOAT_MARKER " %%", NRF_LOG_FLOAT(battVoltage));
    
      if (battVoltage < 2.5f || battVoltage > 5.5f) {
        NRF_LOG_ERROR("INVALID VOLTAGE RANGE -> SENSOR FAULT");
        //   ws2812_set_color(0,0,255);
        return battVoltage;
        return (uint8_t)s_display_soc;    // Return last known good state
      }
      float raw_soc = CAL_SoC(battVoltage);
    
      if (!s_initialised) {
        s_ema_soc     = raw_soc;
        s_display_soc = raw_soc;
        for (uint8_t i = 0; i < TREND_WINDOW; i++)
          s_hist[i] = raw_soc;
        s_initialised = true;
      }
    
      float clean_soc = (fabsf(raw_soc - s_ema_soc) > SPIKE_CLAMP_PCT) ? s_ema_soc : raw_soc;
      s_ema_soc       = EMA_ALPHA * clean_soc + (1.0f - EMA_ALPHA) * s_ema_soc;
    
      s_hist[s_hist_idx] = s_ema_soc;
      s_hist_idx         = (s_hist_idx + 1) % TREND_WINDOW;
    
      float oldest_ema  = s_hist[s_hist_idx];
      float trend_delta = s_ema_soc - oldest_ema;
    
      int8_t trend = 0;
      if (trend_delta > TREND_DEADBAND_PCT)
        trend = +1;
      if (trend_delta < -TREND_DEADBAND_PCT)
        trend = -1;
    
      float error = s_ema_soc - s_display_soc;
      if (trend >= 0 && error > 0.0f)
        s_display_soc += fminf(error, RATE_UP_PCT_SAMPLE);
      else if (trend <= 0 && error < 0.0f)
        s_display_soc += fmaxf(error, -RATE_DOWN_PCT_SAMPLE);
    
      s_display_soc = fmaxf(0.0f, fminf(100.0f, s_display_soc));
    
      NRF_LOG_INFO("Raw SoC     = " NRF_LOG_FLOAT_MARKER " %%", NRF_LOG_FLOAT(raw_soc));
      NRF_LOG_INFO("EMA SoC     = " NRF_LOG_FLOAT_MARKER " %%", NRF_LOG_FLOAT(s_ema_soc));
      NRF_LOG_INFO("Display SoC = " NRF_LOG_FLOAT_MARKER " %%", NRF_LOG_FLOAT(s_display_soc));
      NRF_LOG_INFO("Trend       = %s", trend > 0 ? "CHARGING" : trend < 0 ? "DISCHARGING" : "STABLE");
    
      return (uint8_t)s_display_soc;
    }
    uint8_t battery_soc_update() {
      // ws2812_set_color(100,0,100);
      NRF_LOG_INFO("=================================");
      NRF_LOG_INFO("BATTERY CHECK START");
      float v = ina_bus_voltage();
      NRF_LOG_INFO("CALCULATED BUS VOLTAGE: " NRF_LOG_FLOAT_MARKER " V", NRF_LOG_FLOAT(v));
      if (v < 2.5 || v > 5.5) {
        NRF_LOG_ERROR("INVALID VOLTAGE RANGE -> SENSOR FAULT");
        return 0;
      }
      float raw_soc = soc_from_voltage(v);
      NRF_LOG_INFO("RAW SOC CALCULATED: " NRF_LOG_FLOAT_MARKER " %%", NRF_LOG_FLOAT(raw_soc));
      ;
      float soc = soc_filter(raw_soc);
      NRF_LOG_INFO("FILTERED SOC: " NRF_LOG_FLOAT_MARKER " %%", NRF_LOG_FLOAT(soc));
      ;
      //  NRF_LOG_INFO("FILTERED SOC: %.2f %%", soc);
      if (v < BAT_LOW_LIMIT) {
        NRF_LOG_WARNING("BATTERY LOW CONDITION");
        return 0;
      }
      NRF_LOG_INFO("BATTERY STATUS NORMAL");
      NRF_LOG_INFO("BATTERY CHECK END");
      NRF_LOG_INFO("=================================");
      return (uint8_t)soc;
    }
    
    // void ina226_init()
    //{
    ////nrf_drv_twi_enable(&m_twi);
    //    uint8_t config[3];
    
    //    config[0] = 0x00;
    //    config[1] = 0x45;
    //    config[2] = 0x27;
    
    //  //  NRF_LOG_INFO("Initializing INA226...");
    // nrf_delay_ms(100);
    //    if(i2c_write(INA226_ADDR, config, 3))
    //    {
    //       // NRF_LOG_INFO("INA226 config write SUCCESS");
    //        nrf_delay_ms(100);
    //        }
    //    else
    //    {
    //        NRF_LOG_ERROR("INA226 config write FAILED");
    //         nrf_delay_ms(100);
    //        }
    //    //    nrf_drv_twi_disable(&m_twi); // <--- Add this
    //}
    
    /**
     * @brief Industrial-safe I2C Scan with Bus Recovery
     */
    
    // void perform_system_update(void) {
    //    // 1. Power up the bus
    //   // nrf_drv_twi_enable(&m_twi);
    
    //    // 2. Physical reads (Keep these fast)
    //    sht41_read();            // Climate
    //    float v_bus = ina_bus_voltage(); // Battery raw
    
    //    // 3. Power down the bus immediately
    //    nrf_drv_twi_disable(&m_twi);
    
    //    // 4. Data Processing (CPU only, no I2C needed here)
    //    uint16_t rawTemp = (m_buffer[0] << 8) | m_buffer[1];
    //    uint16_t rawHum  = (m_buffer[3] << 8) | m_buffer[4];
    //    temperature = -45 + 175 * ((float)rawTemp / 65535.0);
    //    humidity    = -6 + 125 * ((float)rawHum / 65535.0);
    
    //    float raw_soc = soc_from_voltage(v_bus);
    //    uint8_t soc = (uint8_t)soc_filter(raw_soc);
    
    //    // 5. Update BLE Services
    //    ble_cus_temperature_update(&m_cus, (int16_t)temperature);
    //    ble_cus_humidity_update(&m_cus, (int16_t)humidity);
    //    battery_level_update(soc);
    
    //    NRF_LOG_INFO("Update: %d C, %d%% Hum, %d%% Batt", (int)temperature, (int)humidity, soc);
    //}
    
    void perform_system_update(void) {
      // REMOVE: nrf_drv_twi_enable(&m_twi);
    
      sht41_read();
      float v_bus = ina_bus_voltage();
    
      // REMOVE: nrf_drv_twi_disable(&m_twi); <--- THIS WAS THE KILLER
    
      uint16_t rawTemp = (m_buffer[0] << 8) | m_buffer[1];
      uint16_t rawHum  = (m_buffer[3] << 8) | m_buffer[4];
      temperature      = -45 + 175 * ((float)rawTemp / 65535.0);
      humidity         = -6 + 125 * ((float)rawHum / 65535.0);
    
      float raw_soc = soc_from_voltage(v_bus);
      uint8_t soc   = (uint8_t)soc_filter(raw_soc);
    
      ble_cus_temperature_update(&m_cus, (int16_t)temperature);
      ble_cus_humidity_update(&m_cus, (int16_t)humidity);
      battery_level_update(soc);
    
      NRF_LOG_INFO("Update: %d C, %d%% Hum, %d%% Batt", (int)temperature, (int)humidity, soc);
    }
    
    // void idle_state_handle(void) {
    //   if (NRF_LOG_PROCESS() == false) {
    //   }
    //    //  __WFE();
    //  //  ws2812_set_color(0,0,0);     // OFF
    //  //ws2812_set_color(255,0,0);
    //  //nrf_delay_ms(300);
    //  ws2812_set_color(255,0,0);
    //  nrf_delay_ms(300);
    //     nrf_pwr_mgmt_run();
    
    // //    ws2812_set_color(0,0,255);
    // //nrf_delay_ms(300);
    //  //NRF_LOG_INFO("Sleep check: session_done=%d sensor=%d ack=%d",
    //    //         g_session_done, g_do_sensor_read, g_ack_received);
    //  //if (!g_do_sensor_read &&
    //  //    !g_ack_received &&
    //  //    !g_session_done &&
    //  //    NRF_LOG_PROCESS() == false) {
    //  //  nrf_pwr_mgmt_run();
    //  //}
    // // }
    //}
    
    //void idle_state_handle(void) {
    //  //  if (!NRF_LOG_PROCESS()) {
    //  nrf_pwr_mgmt_run();
    //  // }
    //}
    
    static void idle_state_handle(void)
    {
        if (NRF_LOG_PROCESS() == false)
        {
            nrf_pwr_mgmt_run();
        }
    }
    
    #define HTU21D_ADDR 0x40
    #define HTU21D_TEMP_CMD 0xE3
    #define HTU21D_HUM_CMD 0xE5
    #define INA226_ADDR 0x40
    #define SHT41_ADDR 0x44
    
    bool htu21d_read(float* temp, float* hum) {
      ret_code_t err;
      uint8_t cmd;
      uint8_t data[3];
    
      /* ---------- TEMPERATURE ---------- */
    
      cmd = HTU21D_TEMP_CMD;
    
      err = nrf_drv_twi_tx(&m_twi, HTU21D_ADDR, &cmd, 1, false);
      if (err != NRF_SUCCESS)
        return false;
      err = nrf_drv_twi_rx(&m_twi, HTU21D_ADDR, data, 3);
      if (err != NRF_SUCCESS)
        return false;
    
      uint16_t rawTemp = (data[0] << 8) | data[1];
      rawTemp &= 0xFFFC;
    
      *temp = -46.85 + 175.72 * ((float)rawTemp / 65536.0);
    
      /* ---------- HUMIDITY ---------- */
    
      cmd = HTU21D_HUM_CMD;
    
      err = nrf_drv_twi_tx(&m_twi, HTU21D_ADDR, &cmd, 1, false);
      if (err != NRF_SUCCESS)
        return false;
    
      //nrf_delay_ms(60);
    
      err = nrf_drv_twi_rx(&m_twi, HTU21D_ADDR, data, 3);
      if (err != NRF_SUCCESS)
        return false;
    
      uint16_t rawHum = (data[0] << 8) | data[1];
      rawHum &= 0xFFFC;
    
      *hum = -6 + 125 * ((float)rawHum / 65536.0);
    
      return true;
    }
    
    void process_data() {
      float temp, hum;
    
      if (!htu21d_read(&temp, &hum)) {
        NRF_LOG_ERROR("HTU21D read failed");
        return;
      }
      temperature = temp;
      humidity    = hum;
      NRF_LOG_INFO("Temperature: %d C", (int)temperature);
      NRF_LOG_INFO("Humidity: %d %%", (int)humidity);
    
      uint8_t batt = get_filtered_battery_soc();
    
      battery_level_update(batt);
      ble_cus_temperature_update(&m_cus, (int16_t)(temperature * 100));
      ble_cus_humidity_update(&m_cus, (int16_t)(humidity * 100));
      // ble_cus_temperature_update(&m_cus,(int16_t)temperature);
      // ble_cus_humidity_update(&m_cus,(int16_t)humidity);
    }
    
    /**@brief Function for application main entry.
     */
    int main(void) {
      bool erase_bonds;
      // Initialize.
       log_init();
      timers_init();
       buttons_leds_init(&erase_bonds);
       power_management_init();
      // NRF_LOG_INFO("before the blestack function.");
       ble_stack_init();
       ret_code_t err_code = sd_power_dcdc_mode_set(NRF_POWER_DCDC_ENABLE);
       APP_ERROR_CHECK(err_code);
       gap_params_init();
       gatt_init();
       services_init();
       advertising_init();
       err_code = sd_ble_gap_tx_power_set(BLE_GAP_TX_POWER_ROLE_ADV, m_advertising.adv_handle, -8);
       APP_ERROR_CHECK(err_code);
       conn_params_init();
       peer_manager_init();
       //nrf_delay_ms(200);
        //twi_init();
        ws2812_init();
       //ina226_init();
      application_timers_start();
       advertising_start(erase_bonds);
       NRF_LOG_INFO("After all initialization");
       //sht41_read();
       //process_data();
      int cnt               = 0;
      bool interval_changed = false; 
      for (;;) {
        if (updtmrexp) {
          //check_why_i_woke_up();
          updtmrexp        = false;
          g_do_sensor_read = false;
           if (tx_enable == 1) {
             NRF_LOG_INFO("TX enabled -> reading sensor");
             //sht41_read();
             //battery_level_update(70);
             //process_data();
           } else {
             NRF_LOG_WARNING("TX disabled -> skipping sensor read");
           }
            if (g_interval_changed) {
              g_interval_changed = false;
              // Prevent invalid interval (safety guard)
              if (new_tx_interval == 0)
                new_tx_interval = 8;    // fallback to 8 seconds
              tx_interval = new_tx_interval;
              NRF_LOG_INFO("Applying new tx_interval: %d sec", tx_interval);
              uint32_t ticks = APP_TIMER_TICKS(tx_interval * 1000UL);
              ret_code_t err;
              // === SAFE STOP ===
              err = app_timer_stop(m_timer_id);
              if (err != NRF_SUCCESS && err != NRF_ERROR_INVALID_STATE) {
                NRF_LOG_WARNING("app_timer_stop() failed: 0x%08X", err);
                // Do NOT reset device in production
              }
              // === SAFE RESTART ===
              err = app_timer_start(m_timer_id, ticks, NULL);
              if (err != NRF_SUCCESS) {
                NRF_LOG_ERROR("app_timer_start() failed: 0x%08X", err);
                // Optional: try again once
                //nrf_delay_ms(10);
                err = app_timer_start(m_timer_id, ticks, NULL);
              }
              if (err == NRF_SUCCESS) {
                NRF_LOG_INFO("Timer successfully restarted with new interval");
              } else
                NRF_LOG_ERROR("Failed to restart timer!");
            }
          }
        //  ws2812_set_color(100,0,0); // Cyan
        idle_state_handle();
        }
    }
    /**
     * Copyright (c) 2016 - 2019, Nordic Semiconductor ASA
     *
     * All rights reserved.
     *
     * Redistribution and use in source and binary forms, with or without modification,
     * are permitted provided that the following conditions are met:
     *
     * 1. Redistributions of source code must retain the above copyright notice, this
     *    list of conditions and the following disclaimer.
     *
     * 2. Redistributions in binary form, except as embedded into a Nordic
     *    Semiconductor ASA integrated circuit in a product or a software update for
     *    such product, must reproduce the above copyright notice, this list of
     *    conditions and the following disclaimer in the documentation and/or other
     *    materials provided with the distribution.
     *
     * 3. Neither the name of Nordic Semiconductor ASA nor the names of its
     *    contributors may be used to endorse or promote products derived from this
     *    software without specific prior written permission.
     *
     * 4. This software, with or without modification, must only be used with a
     *    Nordic Semiconductor ASA integrated circuit.
     *
     * 5. Any software provided in binary form under this license must not be reverse
     *    engineered, decompiled, modified and/or disassembled.
     *
     * THIS SOFTWARE IS PROVIDED BY NORDIC SEMICONDUCTOR ASA "AS IS" AND ANY EXPRESS
     * OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
     * OF MERCHANTABILITY, NONINFRINGEMENT, AND FITNESS FOR A PARTICULAR PURPOSE ARE
     * DISCLAIMED. IN NO EVENT SHALL NORDIC SEMICONDUCTOR ASA OR CONTRIBUTORS BE
     * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
     * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
     * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
     * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
     * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
     * OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
     *
     */
    #include "sdk_common.h"
    #if NRF_MODULE_ENABLED(NRF_PWR_MGMT)
    
    #include "nrf_pwr_mgmt.h"
    #include "nrf.h"
    #include "nrf_mtx.h"
    #include "nrf_power.h"
    #include "app_error.h"
    #include "nrf_assert.h"
    #include "nrf_log_ctrl.h"
    #include "app_util_platform.h"
    
    #define NRF_LOG_MODULE_NAME pwr_mgmt
    #if NRF_PWR_MGMT_CONFIG_LOG_ENABLED
        #define NRF_LOG_LEVEL       NRF_PWR_MGMT_CONFIG_LOG_LEVEL
        #define NRF_LOG_INFO_COLOR  NRF_PWR_MGMT_CONFIG_INFO_COLOR
        #define NRF_LOG_DEBUG_COLOR NRF_PWR_MGMT_CONFIG_DEBUG_COLOR
    #else
        #define NRF_LOG_LEVEL       0
    #endif // NRF_PWR_MGMT_CONFIG_LOG_ENABLED
    #include "nrf_log.h"
    NRF_LOG_MODULE_REGISTER();
    
    #ifdef SOFTDEVICE_PRESENT
        #include "nrf_soc.h"
        #include "nrf_sdh.h"
    #endif // SOFTDEVICE_PRESENT
    
    
    #if NRF_PWR_MGMT_CONFIG_USE_SCHEDULER
        #if (APP_SCHEDULER_ENABLED != 1)
            #error "APP_SCHEDULER is required."
        #endif
        #include "app_scheduler.h"
    #endif // NRF_PWR_MGMT_CONFIG_USE_SCHEDULER
    
    
    // Create section "pwr_mgmt_data".
    NRF_SECTION_SET_DEF(pwr_mgmt_data,
                        nrf_pwr_mgmt_shutdown_handler_t,
                        NRF_PWR_MGMT_CONFIG_HANDLER_PRIORITY_COUNT);
    
    static nrf_pwr_mgmt_evt_t   m_pwr_mgmt_evt;     /**< Event type which will be passed to the shutdown
                                                         handlers.*/
    static nrf_mtx_t            m_sysoff_mtx;       /**< Module API lock.*/
    static bool                 m_shutdown_started; /**< True if application started the shutdown preparation. */
    static nrf_section_iter_t   m_handlers_iter;    /**< Shutdown handlers iterator. */
    
    #if (NRF_PWR_MGMT_CONFIG_FPU_SUPPORT_ENABLED && __FPU_PRESENT)
        #define PWR_MGMT_FPU_SLEEP_PREPARE()     pwr_mgmt_fpu_sleep_prepare()
    
         __STATIC_INLINE void pwr_mgmt_fpu_sleep_prepare(void)
         {
            uint32_t original_fpscr;
    
            CRITICAL_REGION_ENTER();
            original_fpscr = __get_FPSCR();
            /*
             * Clear FPU exceptions.
             * Without this step, the FPU interrupt is marked as pending,
             * preventing system from sleeping. Exceptions cleared:
             * - IOC - Invalid Operation cumulative exception bit.
             * - DZC - Division by Zero cumulative exception bit.
             * - OFC - Overflow cumulative exception bit.
             * - UFC - Underflow cumulative exception bit.
             * - IXC - Inexact cumulative exception bit.
             * - IDC - Input Denormal cumulative exception bit.
             */
            __set_FPSCR(original_fpscr & ~0x9Fu);
            __DMB();
            NVIC_ClearPendingIRQ(FPU_IRQn);
            CRITICAL_REGION_EXIT();
    
            /*
             * The last chance to indicate an error in FPU to the user 
             * as the FPSCR is now cleared
             *
             * This assert is related to previous FPU operations 
             * and not power management.
             *
             * Critical FPU exceptions signaled:
             * - IOC - Invalid Operation cumulative exception bit.
             * - DZC - Division by Zero cumulative exception bit.
             * - OFC - Overflow cumulative exception bit.
             */
            ASSERT((original_fpscr & 0x7) == 0);
         }
    #else
        #define PWR_MGMT_FPU_SLEEP_PREPARE()
    #endif // NRF_PWR_MGMT_CONFIG_FPU_SUPPORT_ENABLED
    
    
    #if NRF_PWR_MGMT_CONFIG_DEBUG_PIN_ENABLED
        #undef  PWR_MGMT_SLEEP_IN_CRITICAL_SECTION_REQUIRED
        #define PWR_MGMT_SLEEP_IN_CRITICAL_SECTION_REQUIRED
    
        #include "nrf_gpio.h"
        #define PWR_MGMT_DEBUG_PINS_INIT()   pwr_mgmt_debug_pins_init()
        #define PWR_MGMT_DEBUG_PIN_CLEAR()   nrf_gpio_pin_clear(NRF_PWR_MGMT_SLEEP_DEBUG_PIN)
        #define PWR_MGMT_DEBUG_PIN_SET()     nrf_gpio_pin_set(NRF_PWR_MGMT_SLEEP_DEBUG_PIN)
    
        __STATIC_INLINE void pwr_mgmt_debug_pins_init(void)
        {
            nrf_gpio_pin_clear(NRF_PWR_MGMT_SLEEP_DEBUG_PIN);
            nrf_gpio_cfg_output(NRF_PWR_MGMT_SLEEP_DEBUG_PIN);
        }
    
    #else
        #define PWR_MGMT_DEBUG_PIN_CLEAR()
        #define PWR_MGMT_DEBUG_PIN_SET()
        #define PWR_MGMT_DEBUG_PINS_INIT()
    #endif
    
    
    #if NRF_PWR_MGMT_CONFIG_CPU_USAGE_MONITOR_ENABLED
        #undef  PWR_MGMT_SLEEP_IN_CRITICAL_SECTION_REQUIRED
        #define PWR_MGMT_SLEEP_IN_CRITICAL_SECTION_REQUIRED
    
        #undef  PWR_MGMT_TIMER_REQUIRED
        #define PWR_MGMT_TIMER_REQUIRED
        #include "app_timer.h"
    
        #define PWR_MGMT_CPU_USAGE_MONITOR_INIT()    pwr_mgmt_cpu_usage_monitor_init()
        #define PWR_MGMT_CPU_USAGE_MONITOR_UPDATE()  pwr_mgmt_cpu_usage_monitor_update()
        #define PWR_MGMT_CPU_USAGE_MONITOR_SUMMARY() NRF_LOG_INFO("Maximum CPU usage: %u%%", \
                                                                  m_max_cpu_usage)
        #define PWR_MGMT_CPU_USAGE_MONITOR_SECTION_ENTER()  \
            {                                               \
                uint32_t sleep_start = app_timer_cnt_get()
    
        #define PWR_MGMT_CPU_USAGE_MONITOR_SECTION_EXIT()                       \
                uint32_t sleep_end = app_timer_cnt_get();                       \
                uint32_t sleep_duration;                                        \
                sleep_duration = app_timer_cnt_diff_compute(sleep_end,          \
                                                           sleep_start);        \
                m_ticks_sleeping += sleep_duration;                             \
            }
    
        static uint32_t m_ticks_sleeping;    /**< Number of ticks spent in sleep mode (__WFE()). */
        static uint32_t m_ticks_last;        /**< Number of ticks from the last CPU usage computation. */
        static uint8_t  m_max_cpu_usage;     /**< Maximum observed CPU usage (0 - 100%). */
    
        __STATIC_INLINE void pwr_mgmt_cpu_usage_monitor_init(void)
        {
            m_ticks_sleeping    = 0;
            m_ticks_last        = 0;
            m_max_cpu_usage     = 0;
        }
    
        __STATIC_INLINE void pwr_mgmt_cpu_usage_monitor_update(void)
        {
            uint32_t delta;
            uint32_t ticks;
            uint8_t  cpu_usage;
    
            ticks = app_timer_cnt_get();
            delta = app_timer_cnt_diff_compute(ticks, m_ticks_last);
            cpu_usage = 100 * (delta - m_ticks_sleeping) / delta;
    
            NRF_LOG_INFO("CPU Usage: %u%%", cpu_usage);
            if (m_max_cpu_usage < cpu_usage)
            {
                m_max_cpu_usage = cpu_usage;
            }
    
            m_ticks_last        = ticks;
            m_ticks_sleeping    = 0;
        }
    
    #else
        #define PWR_MGMT_CPU_USAGE_MONITOR_INIT()
        #define PWR_MGMT_CPU_USAGE_MONITOR_UPDATE()
        #define PWR_MGMT_CPU_USAGE_MONITOR_SUMMARY()
        #define PWR_MGMT_CPU_USAGE_MONITOR_SECTION_ENTER()
        #define PWR_MGMT_CPU_USAGE_MONITOR_SECTION_EXIT()
    #endif // NRF_PWR_MGMT_CONFIG_CPU_USAGE_MONITOR_ENABLED
    
    
    #if NRF_PWR_MGMT_CONFIG_STANDBY_TIMEOUT_ENABLED
        #undef  PWR_MGMT_TIMER_REQUIRED
        #define PWR_MGMT_TIMER_REQUIRED
    
        #define PWR_MGMT_STANDBY_TIMEOUT_INIT()  pwr_mgmt_standby_timeout_clear()
        #define PWR_MGMT_STANDBY_TIMEOUT_CLEAR() pwr_mgmt_standby_timeout_clear()
        #define PWR_MGMT_STANDBY_TIMEOUT_CHECK() pwr_mgmt_standby_timeout_check()
    
        static uint16_t m_standby_counter;     /**< Number of seconds from the last activity
                                                    (@ref pwr_mgmt_feed). */
    
        __STATIC_INLINE void pwr_mgmt_standby_timeout_clear(void)
        {
            m_standby_counter = 0;
        }
    
        __STATIC_INLINE void pwr_mgmt_standby_timeout_check(void)
        {
            if (m_standby_counter < NRF_PWR_MGMT_CONFIG_STANDBY_TIMEOUT_S)
            {
                m_standby_counter++;
            }
            else if (m_shutdown_started == false)
            {
                nrf_pwr_mgmt_shutdown(NRF_PWR_MGMT_SHUTDOWN_GOTO_SYSOFF);
            }
        }
    
    #else
        #define PWR_MGMT_STANDBY_TIMEOUT_INIT()
        #define PWR_MGMT_STANDBY_TIMEOUT_CLEAR()
        #define PWR_MGMT_STANDBY_TIMEOUT_CHECK()
    #endif // NRF_PWR_MGMT_CONFIG_STANDBY_TIMEOUT_ENABLED
    
    
    #if NRF_PWR_MGMT_CONFIG_AUTO_SHUTDOWN_RETRY
        #undef  PWR_MGMT_TIMER_REQUIRED
        #define PWR_MGMT_TIMER_REQUIRED
    
        #define PWR_MGMT_AUTO_SHUTDOWN_RETRY() pwr_mgmt_auto_shutdown_retry()
    
        __STATIC_INLINE void pwr_mgmt_auto_shutdown_retry(void)
        {
            if (m_shutdown_started)
            {
                // Try to continue the shutdown procedure.
                nrf_pwr_mgmt_shutdown(NRF_PWR_MGMT_SHUTDOWN_CONTINUE);
            }
        }
    
    #else
        #define PWR_MGMT_AUTO_SHUTDOWN_RETRY()
    #endif // NRF_PWR_MGMT_CONFIG_AUTO_SHUTDOWN_RETRY
    
    
    #ifdef PWR_MGMT_SLEEP_IN_CRITICAL_SECTION_REQUIRED
        #define PWR_MGMT_SLEEP_INIT()           pwr_mgmt_sleep_init()
        #define PWR_MGMT_SLEEP_LOCK_ACQUIRE()   CRITICAL_REGION_ENTER()
        #define PWR_MGMT_SLEEP_LOCK_RELEASE()   CRITICAL_REGION_EXIT()
    
        __STATIC_INLINE void pwr_mgmt_sleep_init(void)
        {
        #ifdef SOFTDEVICE_PRESENT
            ASSERT(current_int_priority_get() >= APP_IRQ_PRIORITY_LOW);
        #endif
            SCB->SCR |= SCB_SCR_SEVONPEND_Msk;
        }
    
    #else
        #define PWR_MGMT_SLEEP_INIT()
        #define PWR_MGMT_SLEEP_LOCK_ACQUIRE()
        #define PWR_MGMT_SLEEP_LOCK_RELEASE()
    #endif // PWR_MGMT_SLEEP_IN_CRITICAL_SECTION_REQUIRED
    
    
    #ifdef PWR_MGMT_TIMER_REQUIRED
        #include "app_timer.h"
        #define PWR_MGMT_TIMER_CREATE()     pwr_mgmt_timer_create()
    
        APP_TIMER_DEF(m_pwr_mgmt_timer);    /**< Timer used by this module. */
    
        /**@brief Handle events from m_pwr_mgmt_timer.
         */
        static void nrf_pwr_mgmt_timeout_handler(void * p_context)
        {
            PWR_MGMT_CPU_USAGE_MONITOR_UPDATE();
            PWR_MGMT_AUTO_SHUTDOWN_RETRY();
            PWR_MGMT_STANDBY_TIMEOUT_CHECK();
        }
    
        __STATIC_INLINE ret_code_t pwr_mgmt_timer_create(void)
        {
            ret_code_t ret_code = app_timer_create(&m_pwr_mgmt_timer,
                                                   APP_TIMER_MODE_REPEATED,
                                                   nrf_pwr_mgmt_timeout_handler);
            if (ret_code != NRF_SUCCESS)
            {
                return ret_code;
            }
    
            return app_timer_start(m_pwr_mgmt_timer, APP_TIMER_TICKS(1000), NULL);
        }
    #else
        #define PWR_MGMT_TIMER_CREATE() NRF_SUCCESS
    #endif // PWR_MGMT_TIMER_REQUIRED
    
    ret_code_t nrf_pwr_mgmt_init(void)
    {
        NRF_LOG_INFO("Init");
    
        m_shutdown_started = false;
        nrf_mtx_init(&m_sysoff_mtx);
        nrf_section_iter_init(&m_handlers_iter, &pwr_mgmt_data);
    
        PWR_MGMT_SLEEP_INIT();
        PWR_MGMT_DEBUG_PINS_INIT();
        PWR_MGMT_STANDBY_TIMEOUT_INIT();
        PWR_MGMT_CPU_USAGE_MONITOR_INIT();
    
        return PWR_MGMT_TIMER_CREATE();
    }
    void ws2812_set_color(uint8_t r, uint8_t g, uint8_t b);
    #include "nrf_delay.h"
    
    void check_why_i_woke_up(void) {
        for (uint8_t i = 0; i < 48; i++) {
            if (NVIC_GetPendingIRQ((IRQn_Type)i)) {
                NRF_LOG_INFO("Woke up due to IRQ ID: %d", i);
            }
        }
    }
    
    void nrf_pwr_mgmt_run(void)
    {
    //__set_FPSCR(__get_FPSCR() & ~(0x0000001F));
    //    (void) __get_FPSCR();
    //    NVIC_ClearPendingIRQ(FPU_IRQn);
    //__disable_irq();
    
    //ws2812_set_color(0,0,0);
    // nrf_delay_ms(300);
        PWR_MGMT_FPU_SLEEP_PREPARE();
      //  ws2812_set_color(0,0,100);
       //nrf_delay_ms(300);
        PWR_MGMT_SLEEP_LOCK_ACQUIRE();
         //ws2812_set_color(100,0,0);
         //nrf_delay_ms(300);
        PWR_MGMT_CPU_USAGE_MONITOR_SECTION_ENTER();
         //ws2812_set_color(100,100,100);
         //nrf_delay_ms(300);
      //   ws2812_set_color(150,150,150);
    // cnt=10000;
    //while(cnt--);
        PWR_MGMT_DEBUG_PIN_SET();
        //ws2812_set_color(150,150,0);
        // nrf_delay_ms(300);
    //     ws2812_set_color(150,150,0);
    // cnt=10000;
    //while(cnt--);
    
        // Wait for an event.
    #ifdef SOFTDEVICE_PRESENT
        if (nrf_sdh_is_enabled())
        {
        sd_nvic_ClearPendingIRQ(SD_EVT_IRQn);
            ret_code_t ret_code = sd_app_evt_wait();
         //    ws2812_set_color(0,100,100);
         //nrf_delay_ms(300);
            ASSERT((ret_code == NRF_SUCCESS) || (ret_code == NRF_ERROR_SOFTDEVICE_NOT_ENABLED));
            UNUSED_VARIABLE(ret_code);
            check_why_i_woke_up();
            // NRF_LOG_INFO("AFTER NRF SD APP EVENT WAIT");
           // ws2812_set_color(100,0,0);
            //nrf_delay_ms(600);
        }
        else
    #endif // SOFTDEVICE_PRESENT
        {
            // Wait for an event.
            __WFE();
            // Clear the internal event register.
            __SEV();
            __WFE();
        }
        PWR_MGMT_DEBUG_PIN_CLEAR();
        PWR_MGMT_CPU_USAGE_MONITOR_SECTION_EXIT();
        PWR_MGMT_SLEEP_LOCK_RELEASE();
    }
    
    void nrf_pwr_mgmt_feed(void)
    {
        NRF_LOG_DEBUG("Feed");
        // It does not stop started shutdown process.
        PWR_MGMT_STANDBY_TIMEOUT_CLEAR();
    }
    
    /**@brief Function runs the shutdown procedure.
     */
    static void shutdown_process(void)
    {
        NRF_LOG_INFO("Shutdown started. Type %d", m_pwr_mgmt_evt);
        // Executing all callbacks.
        for (/* m_handlers_iter is initialized in nrf_pwr_mgmt_init(). Thanks to that each handler is
                called only once.*/;
             nrf_section_iter_get(&m_handlers_iter) != NULL;
             nrf_section_iter_next(&m_handlers_iter))
        {
            nrf_pwr_mgmt_shutdown_handler_t * p_handler =
                (nrf_pwr_mgmt_shutdown_handler_t *) nrf_section_iter_get(&m_handlers_iter);
            if ((*p_handler)(m_pwr_mgmt_evt))
            {
                NRF_LOG_INFO("SysOff handler 0x%08X => ready", (unsigned int)*p_handler);
            }
            else
            {
                // One of the modules is not ready.
                NRF_LOG_INFO("SysOff handler 0x%08X => blocking", (unsigned int)*p_handler);
                return;
            }
        }
    
        PWR_MGMT_CPU_USAGE_MONITOR_SUMMARY();
        NRF_LOG_INFO("Shutdown complete.");
        NRF_LOG_FINAL_FLUSH();
    
        if ((m_pwr_mgmt_evt == NRF_PWR_MGMT_EVT_PREPARE_RESET)
         || (m_pwr_mgmt_evt == NRF_PWR_MGMT_EVT_PREPARE_DFU))
        {
            NVIC_SystemReset();
        }
        else
        {
            // Enter System OFF.
    #ifdef SOFTDEVICE_PRESENT
            if (nrf_sdh_is_enabled())
            {
                ret_code_t ret_code = sd_power_system_off();
                ASSERT((ret_code == NRF_SUCCESS) || (ret_code == NRF_ERROR_SOFTDEVICE_NOT_ENABLED));
                UNUSED_VARIABLE(ret_code);
    #ifdef DEBUG
                while (true)
                {
                    /* Since the CPU is kept on in an emulated System OFF mode, it is recommended
                     * to add an infinite loop directly after entering System OFF, to prevent
                     * the CPU from executing code that normally should not be executed. */
                    __WFE();
    
                }
    #endif
            }
    #endif // SOFTDEVICE_PRESENT
            nrf_power_system_off();
        }
    }
    
    #if NRF_PWR_MGMT_CONFIG_USE_SCHEDULER
    /**@brief Handle events from app_scheduler.
     */
    static void scheduler_shutdown_handler(void * p_event_data, uint16_t event_size)
    {
        UNUSED_PARAMETER(p_event_data);
        UNUSED_PARAMETER(event_size);
        shutdown_process();
    }
    #endif // NRF_PWR_MGMT_CONFIG_USE_SCHEDULER
    
    void nrf_pwr_mgmt_shutdown(nrf_pwr_mgmt_shutdown_t shutdown_type)
    {
        // Check if shutdown procedure is not started.
        if (!nrf_mtx_trylock(&m_sysoff_mtx))
        {
            return;
        }
    
        if (shutdown_type != NRF_PWR_MGMT_SHUTDOWN_CONTINUE)
        {
            if (m_shutdown_started)
            {
                nrf_mtx_unlock(&m_sysoff_mtx);
                return;
            }
            else
            {
                m_pwr_mgmt_evt      = (nrf_pwr_mgmt_evt_t)shutdown_type;
                m_shutdown_started  = true;
            }
        }
    
        ASSERT(m_shutdown_started);
        NRF_LOG_INFO("Shutdown request %d", shutdown_type);
    
    #if NRF_PWR_MGMT_CONFIG_USE_SCHEDULER
        ret_code_t ret_code = app_sched_event_put(NULL, 0, scheduler_shutdown_handler);
        APP_ERROR_CHECK(ret_code);
    #else
        shutdown_process();
    #endif // NRF_PWR_MGMT_CONFIG_USE_SCHEDULER
    
        nrf_mtx_unlock(&m_sysoff_mtx);
    }
    
    #endif // NRF_MODULE_ENABLED(NRF_PWR_MGMT)
    

Children
No Data
Related