/*******************************************************************************
 * @file    full_agc.c
 * @brief   Automatic Gain Control (AGC) for NIRS LED/PD measurement system
 * @author  Charles KWON <charleskwon@medithings.co.kr>
 * @version V2.0.0
 * @date    2025-12-31
 *
 * Copyright (c) 2025 Medithings Inc.
 * All rights reserved.
 *
 * This module implements automatic gain calibration for the NIRS system.
 * It measures photodetector response for each LED and calculates optimal
 * DAC values for MCP4725 to normalize signal amplitudes across all channels.
 *
 * AGC Process Overview:
 * 1. Sequentially activate each LED-PD pair
 * 2. Sample ADC with gain switch toggling (high/low mode)
 * 3. Calculate voltage difference and derive DAC calibration value
 * 4. Store results in EEPROM for persistent calibration
 *
 * Hardware Dependencies:
 * - MCP4725 DAC for gain control
 * - SAADC for photodetector voltage measurement
 * - FSA5157P6X analog switch for signal routing
 ******************************************************************************/

#include "sdk_common.h"

#include <stdbool.h>
#include <stdint.h>
#include <stdio.h>
#include <string.h>
#include <math.h>
#include "nrf.h"
#include "boards.h"
#include "app_error.h"
#include "nrf_drv_saadc.h"
#include "nrfx_gpiote.h"
#include "nrf_drv_ppi.h"
#include "app_timer.h"
#include "nrf_drv_timer.h"
#include "nrf_delay.h"
#include "measurements.h"
#include "main_timer.h"
#include "battery_saadc.h"
#include "main.h"
#include "full_agc.h"
#include "mcp4725_i2c.h"
//#include "ad5272_i2c.h"
#include "ble_nus.h"
#include "cat_interface.h"
#include <cmd_parse.h>
#include "debug_print.h"
#include "i2c_manager.h"
#include "storage/dr_mem.h"

/*============================================================================*/
/* ADC Configuration Constants - Charles KWON                                 */
/*============================================================================*/

#if FEATURE_AGC_FLOAT
/** @brief Reference voltage in millivolts for ADC conversion (float mode) */
#define FULL_AGC_REF_VOLTAGE_IN_MILLIVOLTS      600.0f

/** @brief ADC prescaling compensation factor (VDD/3 input scaling) */
#define FULL_AGC_PRE_SCALING_COMPENSATION       6.0f

/** @brief Maximum digital value for 10-bit ADC resolution */
#define FULL_AGC_ADC_RES_10BITS                 1024.0f
#else
/** @brief Reference voltage in millivolts for ADC conversion (integer mode) */
#define FULL_AGC_REF_VOLTAGE_IN_MILLIVOLTS      600

/** @brief ADC prescaling compensation factor (VDD/3 input scaling) */
#define FULL_AGC_PRE_SCALING_COMPENSATION       6

/** @brief Maximum digital value for 10-bit ADC resolution */
#define FULL_AGC_ADC_RES_10BITS                 1024
#endif

/** @brief Total number of LEDs in the system */
#define LED_NUM     24

/**
 * @brief Convert ADC value to millivolts
 * - Charles KWON
 *
 * @param ADC_VALUE Raw ADC reading
 * @return Voltage in millivolts
 */
#define FULL_AGC_VOUT_IN_MILLI_VOLTS(ADC_VALUE) \
    ((((ADC_VALUE) * FULL_AGC_REF_VOLTAGE_IN_MILLIVOLTS) / FULL_AGC_ADC_RES_10BITS) * FULL_AGC_PRE_SCALING_COMPENSATION)

/*============================================================================*/
/* DAC Range Limits - Charles KWON                                            */
/*============================================================================*/

/** @brief Minimum DAC value for MCP4725 (prevents under-amplification) */
#define DAC_MIN     125

/** @brief Maximum DAC value for MCP4725 (prevents saturation) */
#define DAC_MAX     1365

/*============================================================================*/
/* External Variables - Charles KWON                                          */
/*============================================================================*/

/** @brief DAC values for each LED-PD pair stored in EEPROM */
extern uint16_t led_pd_dac_v[LED_NUM];

/** @brief Flag indicating command processing in progress */
extern volatile bool processing;

/*============================================================================*/
/* LED/PD Configuration Arrays - Charles KWON                                 */
/*============================================================================*/

/** @brief Working list of LEDs for current AGC scan */
uint8_t full_agc_LED_list[48];

/** @brief Working list of PDs for current AGC scan */
uint8_t full_agc_PD_list[1];

/**
 * @brief LED indices for AGC Part A (LEDs 0-23, PD0)
 * - Charles KWON
 */
uint8_t full_agc_LED_a[24] = {0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19,20,21,22,23};

/** @brief PD index for AGC Part A */
uint8_t full_agc_PD_a[1] = {0};

/**
 * @brief LED indices for AGC Part B (LEDs 24-47, PD1)
 * - Charles KWON
 */
const uint8_t full_agc_LED_b[24] = {24,25,26,27,28,29,30,31,32,33,34,35,36,37,38,39,40,41,42,43,44,45,46,47};

/** @brief PD index for AGC Part B */
uint8_t full_agc_PD_b[1] = {1};

/*============================================================================*/
/* State Machine Variables - Charles KWON                                     */
/*============================================================================*/

/** @brief Number of LEDs to process in current scan */
static int8_t LED_NO = 24;

/** @brief Current PD index (-1 = not started) */
static int8_t pd_no = -1;

/** @brief Current LED index (-1 = not started) */
static int8_t led_no = -1;

/*============================================================================*/
/* ADC Buffer Configuration - Charles KWON                                    */
/*============================================================================*/

/** @brief Number of samples per ADC conversion (averaging) */
#define SAMPLES_IN_BUFFER   4

/** @brief Double-buffered ADC sample storage */
static nrf_saadc_value_t full_agc_buf[2][SAMPLES_IN_BUFFER];

/*============================================================================*/
/* AGC State Flags - Charles KWON                                             */
/*============================================================================*/

/** @brief AGC Part A (PD0, LEDs 0-23) in progress */
bool full_agc_a_start = false;

/** @brief AGC Part B (PD1, LEDs 24-47) in progress */
bool full_agc_b_start = false;

/** @brief AGC Part C (reserved for future use) in progress */
bool full_agc_c_start = false;

/** @brief Full AGC sequence completed successfully */
bool full_agc_completed = false;

/*============================================================================*/
/* Timer Definitions - Charles KWON                                           */
/*============================================================================*/

/** @brief Timer for AGC measurement loop */
APP_TIMER_DEF(m_full_agc_loop_timer_id);

/** @brief AGC loop interval in milliseconds */
#define FULL_AGC_LOOP_INTERVAL  2

/** @brief Timer for AGC data transmission */
APP_TIMER_DEF(m_full_agc_send_timer_id);

/** @brief AGC send interval (feature-dependent) */
#if FEATURE_DELAY
#define FULL_AGC_SEND_INTERVAL  500
#else
#define FULL_AGC_SEND_INTERVAL  100
#endif

/*============================================================================*/
/* Measurement Variables - Charles KWON                                       */
/*============================================================================*/

#if FEATURE_AGC_FLOAT
/** @brief High-mode ADC reading in millivolts */
static double agc_read_h = 0;

/** @brief Low-mode ADC reading in millivolts */
static double agc_read_l = 0;

/** @brief Reference voltage difference */
static double agc_read_ref = 0;

/** @brief Current ADC reading in millivolts */
static double m_full_agc_in_milli_volts = 0.0f;
#else
/** @brief High-mode ADC reading in millivolts */
static uint16_t agc_read_h = 0;

/** @brief Low-mode ADC reading in millivolts */
static uint16_t agc_read_l = 0;

/** @brief Reference voltage difference */
static uint16_t agc_read_ref = 0;

/** @brief Current ADC reading in millivolts */
static uint16_t m_full_agc_in_milli_volts = 0;
#endif

/** @brief Sample skip counter (skip first few unstable samples) */
static uint16_t cnt_js = 0;

/** @brief Current gain mode (true=high, false=low) */
static bool agc_mode = false;

/** @brief Buffer for voltage difference analysis */
static uint16_t array_buff[48] = {0,};

/** @brief Current index in array_buff */
static uint16_t idx = 0;

/** @brief Order counter for BLE transmission sequence */
static uint8_t order_cnt = 0;

/*============================================================================*/
/* AGC Result Storage Arrays - Charles KWON                                   */
/*============================================================================*/

/**
 * @brief AGC read values for all LED-PD pairs
 * - Charles KWON
 *
 * Stores calculated DAC values after AGC calibration
 */
uint16_t agc_read_v[LED_NUM] = {1000, 1000, 1000, 1000, 1000, 1000,
                                 1000, 1000, 1000, 1000, 1000, 1000,
                                 1000, 1000, 1000, 1000, 1000, 1000,
                                 1000, 1000, 1000, 1000, 1000, 1000};

/** @brief AGC Part A, PD0 results */
uint16_t agc_A_0[LED_NUM] = {1000, 1000, 1000, 1000, 1000, 1000,
                              1000, 1000, 1000, 1000, 1000, 1000,
                              1000, 1000, 1000, 1000, 1000, 1000,
                              1000, 1000, 1000, 1000, 1000, 1000};

/** @brief AGC Part A, PD1 results */
uint16_t agc_A_1[LED_NUM] = {1000, 1000, 1000, 1000, 1000, 1000,
                              1000, 1000, 1000, 1000, 1000, 1000,
                              1000, 1000, 1000, 1000, 1000, 1000,
                              1000, 1000, 1000, 1000, 1000, 1000};

/** @brief AGC Part A, MOD1 results */
uint16_t agc_A_2[LED_NUM] = {1000, 1000, 1000, 1000, 1000, 1000,
                              1000, 1000, 1000, 1000, 1000, 1000,
                              1000, 1000, 1000, 1000, 1000, 1000,
                              1000, 1000, 1000, 1000, 1000, 1000};

/** @brief AGC Part B, PD0 results */
uint16_t agc_B_0[LED_NUM] = {1000, 1000, 1000, 1000, 1000, 1000,
                              1000, 1000, 1000, 1000, 1000, 1000,
                              1000, 1000, 1000, 1000, 1000, 1000,
                              1000, 1000, 1000, 1000, 1000, 1000};

/** @brief AGC Part B, PD1 results */
uint16_t agc_B_1[LED_NUM] = {1000, 1000, 1000, 1000, 1000, 1000,
                              1000, 1000, 1000, 1000, 1000, 1000,
                              1000, 1000, 1000, 1000, 1000, 1000,
                              1000, 1000, 1000, 1000, 1000, 1000};

/** @brief AGC Part B, MOD1 results */
uint16_t agc_B_2[LED_NUM] = {1000, 1000, 1000, 1000, 1000, 1000,
                              1000, 1000, 1000, 1000, 1000, 1000,
                              1000, 1000, 1000, 1000, 1000, 1000,
                              1000, 1000, 1000, 1000, 1000, 1000};

/*============================================================================*/
/* Communication Buffers - Charles KWON                                       */
/*============================================================================*/

/** @brief Command source type */
extern which_cmd_t cmd_type_t;

/** @brief AGC ASCII transmission buffer */
char agc_tx_buffer[BLE_NUS_MAX_DATA_LEN];

/** @brief AGC response buffer */
char agc_tx_buffer_r[BLE_NUS_MAX_DATA_LEN];

/** @brief Flag to control BLE send sequence order */
bool agc_ble_send_order = false;

/** @brief BLE connection state */
extern volatile bool ble_connection_st;

/** @brief AGC binary transmission buffer */
uint8_t agc_bin_buffer[BLE_NUS_MAX_DATA_LEN];

/*============================================================================*/
/* ADC Interrupt Handler - Charles KWON                                       */
/*============================================================================*/

/**
 * @brief SAADC event handler for AGC voltage measurement
 * - Charles KWON
 *
 * Processes ADC conversion results from photodetector.
 * Averages multiple samples for noise reduction and converts
 * to millivolts for DAC calculation.
 *
 * @param[in] p_event Pointer to SAADC event structure
 */
static void full_agc_voltage_handler(nrf_drv_saadc_evt_t const * p_event)
{
    uint16_t min_data = 0;

    if (p_event->type == NRF_DRV_SAADC_EVT_DONE)
    {
        uint32_t err_code;

        err_code = nrf_drv_saadc_buffer_convert(p_event->data.done.p_buffer, SAMPLES_IN_BUFFER);
        APP_ERROR_CHECK(err_code);

        /* Sum all samples for averaging */
        for (uint8_t i = 0; i < SAMPLES_IN_BUFFER; i++) {
            min_data += p_event->data.done.p_buffer[i];
        }

        /* Convert averaged ADC value to millivolts */
        m_full_agc_in_milli_volts = (uint16_t)(FULL_AGC_VOUT_IN_MILLI_VOLTS(min_data) * 0.25);
    }
}

/*============================================================================*/
/* AGC State Machine - Charles KWON                                           */
/*============================================================================*/

/**
 * @brief Main AGC measurement state machine
 * - Charles KWON
 *
 * Implements the core AGC algorithm:
 * 1. Activate LED-PD pair
 * 2. Toggle gain switch and sample ADC in both modes
 * 3. Calculate voltage difference
 * 4. Derive DAC value: Dn = (Vout/Vref) x 4096
 * 5. Store result and advance to next LED-PD pair
 *
 * Uses delay_ticks_counter for non-blocking LED settling time.
 */
static void full_agc_measurement(void)
{
    static bool mode_behind = false;
    static bool sampling_behind = false;
    static bool agc_processing_behind = false;

    static double Vref = 3.3f;          /* Reference voltage = Vdd */
    static double dac_value_h = 0.0f;
    static double dac_value_l = 0.0f;
#if FEATURE_DETAIL_VALUE_AGC
    static double avg_num = 0.0f;
#endif

    static uint16_t dac_reg_h = 0;
    static uint16_t dac_reg_l = 0;
    static uint16_t max_num = 0, zero_count = 0, sum_num = 0;
    static uint16_t diff_volt = 0;

    /* Non-blocking delay counter for LED settling */
    static int delay_ticks_counter = 0;

    /* Skip processing during delay period */
    if (delay_ticks_counter > 0)
    {
        delay_ticks_counter--;
        return;
    }

    /*------------------------------------------------------------------------*/
    /* Initial LED/PD activation                                              */
    /*------------------------------------------------------------------------*/
    if (led_no == -1) {
        led_no = 0;
        pd_no = 0;

        led_on(full_agc_LED_list[led_no]);
        pd_on(full_agc_PD_list[pd_no]);

        DBG_PRINTF("LED-1\r\n");
        delay_ticks_counter = 100;
        DBG_PRINTF("Finished part A. Pausing for ~108ms before part B...\r\n");

    } else if ((led_no >= 0) && (led_no <= 47)) {

        /*--------------------------------------------------------------------*/
        /* Step 1: Toggle gain mode                                           */
        /*--------------------------------------------------------------------*/
        if (mode_behind == false) {
            mode_behind = true;

            if (agc_mode == false) {
                agc_mode = true;
            } else if (agc_mode == true) {
                agc_mode = false;
            }

#if FEATURE_DETAIL_VALUE_AGC
            DBG_PRINTF("agc_mode [%d]\r\n", agc_mode);
#endif
            AGC_GAIN_SW(agc_mode);

            return;
        }

        /*--------------------------------------------------------------------*/
        /* Step 2: Trigger ADC sampling                                       */
        /*--------------------------------------------------------------------*/
        if (sampling_behind == false) {
            sampling_behind = true;

            ret_code_t err_code = nrf_drv_saadc_sample();
            cnt_js++;
            APP_ERROR_CHECK(err_code);

            return;
        }

        /*--------------------------------------------------------------------*/
        /* Step 3: Process ADC results and calculate DAC value                */
        /*--------------------------------------------------------------------*/
        if (agc_processing_behind == false) {
            if (agc_mode == true) {
                /* High-mode measurement */
                if (m_full_agc_in_milli_volts == 0) {
#if FEATURE_DETAIL_VALUE_AGC
                    DBG_PRINTF("<1>ReSampling +++++ mode = %d, led_no = %d(%d), pd_no = %d(%d)\r\n",
                               agc_mode, led_no, full_agc_LED_list[led_no], pd_no, full_agc_PD_list[pd_no]);
#endif
                    agc_mode = false;
                    mode_behind = false;
                    sampling_behind = false;
                    return;
                }

                agc_read_h = m_full_agc_in_milli_volts;
                m_full_agc_in_milli_volts = 0.0f;

                /* Calculate DAC value: Dn = (Vout/Vref) x 4096 */
                dac_value_h = (((agc_read_h / Vref) * 4096.0f) / 1000.0f);
                dac_reg_h = (uint16_t)(dac_value_h + 0.5f);  /* Round to nearest integer */

#if FEATURE_FOR_SCOPE
                nrf_gpio_pin_set(ADC_CLK_18);
                nrf_gpio_pin_clear(ADC_CLK_18);
                nrf_gpio_pin_set(ADC_CLK_18);
                nrf_gpio_pin_clear(ADC_CLK_18);
#endif

                led_pd_dac_v[full_agc_LED_list[led_no]] = dac_reg_h;

#if FEATURE_DETAIL_VALUE_AGC
#if FEATURE_AGC_FLOAT
                DBG_PRINTF("mode = %d, led_no = %d(%d), pd_no = %d(%d), \t%lf(mV)->DAC %d\r\n",
                           agc_mode, led_no, full_agc_LED_list[led_no], pd_no, full_agc_PD_list[pd_no],
                           agc_read_h, dac_reg_h);
#else
                DBG_PRINTF("mode = %d, led_no = %d(%d), pd_no = %d(%d), \t%d(mV)->DAC %d\r\n",
                           agc_mode, led_no, full_agc_LED_list[led_no], pd_no, full_agc_PD_list[pd_no],
                           agc_read_h, dac_reg_h);
#endif
#endif

                sw_i2c_init_once();
                pd_gain_set(full_agc_LED_list[led_no]);

                mode_behind = false;
                sampling_behind = false;

            } else if (agc_mode == false) {
                /* Low-mode measurement */
                if (m_full_agc_in_milli_volts == 0) {
#if FEATURE_DETAIL_VALUE_AGC
                    DBG_PRINTF("<2>ReSampling +++++ mode = %d, led_no = %d(%d), pd_no = %d(%d)\r\n",
                               agc_mode, led_no, full_agc_LED_list[led_no], pd_no, full_agc_PD_list[pd_no]);
#endif
                    agc_mode = true;
                    mode_behind = false;
                    sampling_behind = false;
                    return;
                }

                agc_read_l = m_full_agc_in_milli_volts;
                m_full_agc_in_milli_volts = 0.0f;

                /* Calculate DAC value for low mode */
                dac_value_l = (((agc_read_l / Vref) * 4096.0f) / 1000.0f);
                dac_reg_l = (uint16_t)(dac_value_l + 0.5f);

#if FEATURE_FOR_SCOPE
                nrf_gpio_pin_set(ADC_CLK_18);
                nrf_gpio_pin_clear(ADC_CLK_18);
#endif

                agc_read_v[full_agc_LED_list[led_no]] = dac_reg_l;

                /* Calculate voltage difference between high and low modes */
                agc_read_ref = fabsf(agc_read_h - agc_read_l);

                /* Store difference for analysis */
                diff_volt = (uint16_t)(agc_read_ref + 0.5f);
                array_buff[idx] = diff_volt;
                idx++;

#if FEATURE_DETAIL_VALUE_AGC
#if FEATURE_AGC_FLOAT
                DBG_PRINTF("mode = %d, led_no = %d(%d), pd_no = %d(%d), \t%f(mV), \t\t\tDiff = %d\r\n\r\n",
                           agc_mode, led_no, full_agc_LED_list[led_no], pd_no, full_agc_PD_list[pd_no],
                           agc_read_l, diff_volt);
#else
                DBG_PRINTF("mode = %d, led_no = %d(%d), pd_no = %d(%d), \t%d(mV), \t\t\tDiff = %d\r\n\r\n",
                           agc_mode, led_no, full_agc_LED_list[led_no], pd_no, full_agc_PD_list[pd_no],
                           agc_read_l, diff_volt);
#endif
#endif
                agc_processing_behind = true;
            }

            return;
        }

        /*--------------------------------------------------------------------*/
        /* Step 4: Advance to next LED-PD pair                                */
        /*--------------------------------------------------------------------*/
        mode_behind = false;
        sampling_behind = false;
        agc_processing_behind = false;

        agc_mode = false;
        dac_value_h = 0.0f;
        dac_reg_h = 0;
        diff_volt = 0;

        if (pd_no < PD_NO - 1) {
            /* Next PD for same LED */
            pd_no++;
            pd_on(full_agc_PD_list[pd_no]);
        }
        else if (pd_no >= PD_NO - 1) {
            pd_no = 0;

            if (led_no < LED_NO - 1) {
                /* Next LED */
                led_no++;
                led_on(full_agc_LED_list[led_no]);
                pd_on(full_agc_PD_list[pd_no]);

                /* Add delay at LED package boundaries */
                if (full_agc_LED_list[led_no] % 6 == 0) {
                    delay_ticks_counter = 5;
                    DBG_PRINTF("LED_PACKEAGE\r\n");
                }
                DBG_PRINTF("LED+1\r\n");
            } else if (led_no >= LED_NO - 1) {
                /*------------------------------------------------------------*/
                /* All LEDs processed - transition to next phase              */
                /*------------------------------------------------------------*/
                pd_no = -1;
                led_no = -1;

#if FEATURE_DETAIL_VALUE_AGC
                DBG_PRINTF("\r\nLED-PD Gain Array =\r\n");
                for (uint16_t i = 0; i < PD_NUM; i++) {
                    for (uint16_t j = 0; j < LED_NUM; j++) {
                        DBG_PRINTF("%d,\t", led_pd_dac_v[i][j]);
                    }
                    DBG_PRINTF("\r\n");
                }
                DBG_PRINTF("\r\n");
#endif
                if (ble_connection_st == 0) {
                    full_agc_send_timer_stop();
                    full_agc_end();
                    DBG_PRINTF("Full AGC STOP 1\r\n");
                }
                else if (full_agc_a_start == true) {
                    /* Part A complete, start Part B */
                    full_agc_a_start = false;
                    full_agc_b_start = true;
                    full_agc_start();
                } else if (full_agc_b_start == true) {
                    /* Part B complete, finalize AGC */
                    full_agc_b_start = false;

                    full_agc_completed = true;
                    full_agc_end();

#if FEATURE_DETAIL_VALUE_AGC
                    DBG_PRINTF("Full AGC, Completed!\r\n");
#endif

                    /* Analyze voltage difference statistics */
                    for (uint16_t i = 0; i < idx; i++) {
#if FEATURE_DETAIL_VALUE_AGC
                        if (i % 25 == 0) DBG_PRINTF("\r\n");
                        DBG_PRINTF("%d, \t", array_buff[i]);
#endif
                        sum_num += array_buff[i];
                        if (array_buff[i] == 0) zero_count++;
                        if (max_num < array_buff[i]) {
                            max_num = array_buff[i];
                        }
                    }

#if FEATURE_DETAIL_VALUE_AGC
                    avg_num = (double)sum_num / idx;
                    DBG_PRINTF("\r\n\r\n");
                    DBG_PRINTF("Max_number = %d\r\n", max_num);
                    DBG_PRINTF("Avg_number = %.2f\r\n", avg_num);
                    DBG_PRINTF("Zero_count = %d\r\n", zero_count);
                    DBG_PRINTF("Zero_Percentage = %.2f%%\r\n", (double)zero_count / idx * 100);
                    DBG_PRINTF("\r\n");
#endif

                    /* Clear analysis buffer */
                    for (uint16_t i = 0; i < 48; i++) {
                        array_buff[i] = 0;
                    }
#if FEATURE_DETAIL_VALUE_AGC
                    avg_num = 0.0f;
#endif
                    max_num = 0;
                    zero_count = 0;
                    sum_num = 0;
                    idx = 0;

#if FEATURE_DETAIL_VALUE_AGC
                    DBG_PRINTF("agc_read_v = \r\n");
                    for (uint16_t i = 0; i < PD_NUM; i++) {
                        for (uint16_t j = 0; j < LED_NUM; j++) {
                            DBG_PRINTF("%d, \t", agc_read_v[i][j]);
                        }
                        DBG_PRINTF("\r\n");
                    }
                    DBG_PRINTF("\r\n");
#endif

                    /*--------------------------------------------------------*/
                    /* Send AGC results via UART or BLE                       */
                    /*--------------------------------------------------------*/
                    if (cmd_type_t == CMD_UART) {
                        DBG_PRINTF("Tagc, ");
                        for (uint16_t j = 0; j < LED_NUM; j++) {
                            DBG_PRINTF("%d, ", led_pd_dac_v[j]);
                        }
                        DBG_PRINTF("\r\n");
                    } else if (cmd_type_t == CMD_BLE) {
                        battery_timer_stop();

                        /* Copy results to transmission arrays */
                        for (uint16_t j = 0; j < LED_NO; j++) {
                            agc_A_0[j] = led_pd_dac_v[j];
                        }
                        for (uint16_t j = 0; j < LED_NO; j++) {
                            agc_A_1[j] = led_pd_dac_v[j + LED_NO];
                        }

                        /* Format and send binary data */
                        format_data(agc_bin_buffer, "rag:", led_pd_dac_v, 96);
                        binary_tx_handler(agc_bin_buffer, 50);

                        for (uint16_t i = 0; i < 50; i++) {
                            DBG_PRINTF("%02X ", agc_bin_buffer[i]);
                        }
                        DBG_PRINTF("\r\n");

                        DBG_PRINTF("Tagc,");
                        for (uint16_t j = 0; j < LED_NUM; j++) {
                            DBG_PRINTF("%d, ", led_pd_dac_v[j]);
                        }

                        /* Save AGC results to W25Q32 */
                        dr_memWrite("agc_gain", led_pd_dac_v, 96);
                        nrf_delay_ms(10);
                        DBG_PRINTF("AGC_Gain Write!\r\n");
                    }
                }
            }
        }
    }
}

/*============================================================================*/
/* AGC Control Functions - Charles KWON                                       */
/*============================================================================*/

/**
 * @brief Initialize and start AGC sequence
 * - Charles KWON
 *
 * Configures LED and PD lists based on current phase (A or B).
 * Part A: LEDs 0-23 with PD0
 * Part B: LEDs 24-47 with PD1
 */
void full_agc_start(void)
{
    cnt_js = 0;

    if (full_agc_a_start == true) {
#if FEATURE_DETAIL_VALUE_AGC
        DBG_PRINTF("\r\n===== Full_AGC_A start ====================\r\n");
        DBG_PRINTF("LED : ");
#endif

        full_agc_PD_list[0] = full_agc_PD_a[0];

        for (uint8_t i = 0; i < LED_NO; i++) {
            full_agc_LED_list[i] = full_agc_LED_a[i];
        }

#if FEATURE_DETAIL_VALUE_AGC
        DBG_PRINTF("\r\n");
        DBG_PRINTF("PD  : ");
        DBG_PRINTF("%d ", full_agc_PD_list[i]);
        DBG_PRINTF("\r\n\r\n");
#endif
    }
    else if (full_agc_b_start == true) {
#if FEATURE_DETAIL_VALUE_AGC
        DBG_PRINTF("\r\n===== Full_AGC_B start ====================\r\n");
        DBG_PRINTF("LED : ");
#endif

        for (uint8_t i = 0; i < LED_NO; i++) {
            full_agc_LED_list[i] = full_agc_LED_b[i];
#if FEATURE_DETAIL_VALUE_AGC
            DBG_PRINTF("%d ", full_agc_LED_list[i]);
#endif
        }

#if FEATURE_DETAIL_VALUE_AGC
        DBG_PRINTF("\r\n");
        DBG_PRINTF("PD : ");
#endif

        full_agc_PD_list[0] = full_agc_PD_b[0];

#if FEATURE_DETAIL_VALUE_AGC
        DBG_PRINTF("\r\n\r\n");
#endif
    }

    pd_no = -1;
    led_no = -1;
}

/**
 * @brief Terminate AGC sequence and cleanup
 * - Charles KWON
 *
 * Stops timers, uninitializes ADC, restores I2C mode,
 * turns off all LEDs/PDs, and clears processing flag.
 */
void full_agc_end(void)
{
    hw_i2c_init_once();
    full_agc_timer_stop();
    full_agc_uninit();

    battery_timer_start();

    led_off(99);    /* Turn off all LEDs */
    pd_off(99);     /* Turn off all PDs */

    DBG_PRINTF("end_process\r\n\r\n");
    processing = false;
}

/*============================================================================*/
/* SAADC Configuration - Charles KWON                                         */
/*============================================================================*/

/**
 * @brief Initialize SAADC for AGC voltage measurement
 * - Charles KWON
 *
 * Configures single-ended ADC on AIN6 (FSA5157P6X output)
 * with double-buffering for continuous sampling.
 */
void full_agc_adc_init(void)
{
    ret_code_t err_code = nrf_drv_saadc_init(NULL, full_agc_voltage_handler);
    APP_ERROR_CHECK(err_code);

    nrf_saadc_channel_config_t config =
        NRF_DRV_SAADC_DEFAULT_CHANNEL_CONFIG_SE(NRF_SAADC_INPUT_AIN6);

    err_code = nrf_drv_saadc_channel_init(0, &config);
    APP_ERROR_CHECK(err_code);

    err_code = nrf_drv_saadc_buffer_convert(full_agc_buf[0], SAMPLES_IN_BUFFER);
    APP_ERROR_CHECK(err_code);

    err_code = nrf_drv_saadc_buffer_convert(full_agc_buf[1], SAMPLES_IN_BUFFER);
    APP_ERROR_CHECK(err_code);
}

/**
 * @brief Uninitialize SAADC
 * - Charles KWON
 */
void full_agc_uninit(void)
{
    nrf_drv_saadc_uninit();
    nrf_drv_saadc_channel_uninit(0);
}

/*============================================================================*/
/* Timer Callbacks - Charles KWON                                             */
/*============================================================================*/

/**
 * @brief AGC loop timer callback
 * - Charles KWON
 *
 * Called periodically to drive the AGC state machine.
 *
 * @param[in] p_context Unused timer context
 */
void full_agc_loop(void * p_context)
{
    UNUSED_PARAMETER(p_context);
    full_agc_measurement();
}

/*============================================================================*/
/* AGC Loop Timer Control - Charles KWON                                      */
/*============================================================================*/

/**
 * @brief Start AGC measurement loop timer
 * - Charles KWON
 */
void full_agc_timer_start(void)
{
    APP_ERROR_CHECK(app_timer_start(m_full_agc_loop_timer_id,
                                     APP_TIMER_TICKS(FULL_AGC_LOOP_INTERVAL), NULL));
}

/**
 * @brief Stop AGC measurement loop timer
 * - Charles KWON
 */
void full_agc_timer_stop(void)
{
    APP_ERROR_CHECK(app_timer_stop(m_full_agc_loop_timer_id));
}

/**
 * @brief Initialize AGC loop timer
 * - Charles KWON
 */
void full_agc_timer_init(void)
{
    APP_ERROR_CHECK(app_timer_create(&m_full_agc_loop_timer_id,
                                      APP_TIMER_MODE_REPEATED, full_agc_loop));
}

/*============================================================================*/
/* AGC Measurement Entry Point - Charles KWON                                 */
/*============================================================================*/

/**
 * @brief Start full AGC measurement sequence
 * - Charles KWON
 *
 * Entry point for AGC calibration. Initializes state,
 * stops battery monitoring, and begins Part A measurement.
 */
void full_agc_mesurement_start(void)
{
    agc_ble_send_order = false;
    full_agc_completed = false;
    battery_timer_stop();
    order_cnt = 0;

    full_agc_a_start = true;

    for (uint8_t i = 0; i < LED_NO; i++) {
        DBG_PRINTF("%d ", full_agc_LED_list[i]);
    }
    DBG_PRINTF("led/r/n ");

    full_agc_start();
    nrf_delay_ms(5);
    full_agc_adc_init();
    nrf_delay_ms(5);
    full_agc_timer_start();
}

/*============================================================================*/
/* AGC Data Transmission - Charles KWON                                       */
/*============================================================================*/

/**
 * @brief AGC data send loop timer callback
 * - Charles KWON
 *
 * Handles sequenced transmission of AGC results over BLE.
 * Transmits data in chunks to avoid BLE packet size limits.
 *
 * @param[in] p_context Unused timer context
 */
void full_agc_send_loop(void * p_context)
{
    UNUSED_PARAMETER(p_context);

    static bool send_step = false;

    full_agc_send_timer_stop();

    if (send_step == false) {
        /* Data preparation phase */
        if (order_cnt >= 6) {
            order_cnt = 0;
            send_step = true;
        } else if (order_cnt < 6) {
            switch (order_cnt) {
                case 0:
                    for (uint16_t j = 0; j < LED_NO; j++) {
                        agc_A_0[j] = led_pd_dac_v[j];
                        agc_B_0[j] = agc_read_v[j];
                    }
                    break;

                case 1:
                    for (uint16_t j = 0; j < LED_NO; j++) {
                        agc_A_1[j] = led_pd_dac_v[LED_NO + j];
                        agc_B_1[j] = agc_read_v[LED_NO + j];
                    }
                    break;

                case 2:
                case 3:
                case 4:
                case 5:
                    break;

                default:
                    DBG_PRINTF("ERR!!!, Out of range <1>\r\n");
                    break;
            }
            order_cnt++;
        }
    } else if (send_step == true) {
        /* Data transmission phase */
        if (order_cnt >= 6) {
            if (ble_connection_st == 1) {
                battery_timer_start();
            }

            order_cnt = 0;
            send_step = false;
            return;
        } else if (order_cnt < 3) {
            if (agc_ble_send_order == false) {
                switch (order_cnt) {
                    case 0:
                        {
                            int offset = sprintf(agc_tx_buffer, "Tagc-A0");
                            for (uint8_t j = 0; j < LED_NO; j++) {
                                offset += sprintf(agc_tx_buffer + offset, ",\t%d", agc_A_0[j]);
                            }
                            sprintf(agc_tx_buffer + offset, "\r\n");
                        }
                        break;

                    case 1:
                        break;

                    case 2:
                        {
                            int offset = sprintf(agc_tx_buffer, "Tagc-A1");
                            for (uint8_t j = 0; j < LED_NO; j++) {
                                offset += sprintf(agc_tx_buffer + offset, ",\t%d", agc_A_1[j]);
                            }
                            sprintf(agc_tx_buffer + offset, "\r\n");
                        }
                        break;

                    case 3:
                    case 4:
                    case 5:
                        break;

                    default:
                        DBG_PRINTF("ERR!!!, Out of range <2>\r\n");
                        break;
                }

                if (ble_connection_st == 0) {
                    full_agc_end();
                    full_agc_send_timer_stop();
                    DBG_PRINTF("Full AGC STOP 2\r\n");
                } else {
                    data_tx_handler(agc_tx_buffer);
                    order_cnt++;
                    if (order_cnt == 10) {
                        agc_ble_send_order = true;
                        order_cnt = 0;
                    }
                }
            } else if (agc_ble_send_order == true) {
                if (ble_connection_st == 0) {
                    full_agc_end();
                    full_agc_send_timer_stop();
                } else {
                    order_cnt++;
                }
            }
        }
    }

    if (ble_connection_st == 0) {
        order_cnt = 0;
        full_agc_send_timer_stop();
    } else {
        full_agc_send_timer_start();
    }
}

/*============================================================================*/
/* AGC Send Timer Control - Charles KWON                                      */
/*============================================================================*/

/**
 * @brief Start AGC data transmission timer
 * - Charles KWON
 */
void full_agc_send_timer_start(void)
{
    APP_ERROR_CHECK(app_timer_start(m_full_agc_send_timer_id,
                                     APP_TIMER_TICKS(FULL_AGC_SEND_INTERVAL), NULL));
}

/**
 * @brief Stop AGC data transmission timer
 * - Charles KWON
 */
void full_agc_send_timer_stop(void)
{
    APP_ERROR_CHECK(app_timer_stop(m_full_agc_send_timer_id));
}

/**
 * @brief Initialize AGC send timer
 * - Charles KWON
 */
void full_agc_send_timer_init(void)
{
    APP_ERROR_CHECK(app_timer_create(&m_full_agc_send_timer_id,
                                      APP_TIMER_MODE_SINGLE_SHOT, full_agc_send_loop));
}
