/*******************************************************************************
 * @file    cmd.c
 * @brief   Command handlers and command table
 * @author  Charles KWON <charleskwon@medithings.co.kr>
 * @date    2025-01-30
 * @copyright (c) 2025 Medithings Inc. All rights reserved.
 *
 * @details Platform-specific command implementations.
 *          Command table is exported to parser for dispatch.
 ******************************************************************************/

#include "cmd.h"
#include "parser.h"
#include "dr_util.h"
#include <string.h>

#include "nrf_gpio.h"
#include "nrf_delay.h"
#include "../drivers/w25q32/w25q32.h"  /* Flash driver */
#include "../storage/dr_mem.h"         /* Name-based memory API */
#include "../fstorage.h"               /* m_config */
#include "../debug_print.h"

/*==============================================================================
 * HARDWARE DEFINES
 *============================================================================*/

#define GAIN_SW_PIN        NRF_GPIO_PIN_MAP(0,20)
#define AGC_GAIN_SW(x)     do { \
    if ((x) == true)  nrf_gpio_pin_set(GAIN_SW_PIN); \
    else              nrf_gpio_pin_clear(GAIN_SW_PIN); \
} while(0)

/*==============================================================================
 * EXTERNAL FUNCTION DECLARATIONS
 *============================================================================*/

/* Sensor functions */
extern void battery_level_meas(void);
extern void pressure_all_level_meas(void);
extern void tmp235_voltage_level_meas(void);
extern void simple_mesurement_start(void);
extern void full_agc_mesurement_start(void);

/* Device control functions */
extern int device_activated(void);
extern int device_sleep_mode(void);

/* Error handling */
extern void param_error(const char *cmd);

/* BLE transmission */
extern void single_format_data(uint8_t *buffer, const char *tag, uint16_t value);
extern void format_data(uint8_t *buffer, const char *tag, const uint16_t *data_array, size_t length);
extern void format_data_byte(uint8_t *buffer, const char *tag, const uint8_t *data_array, size_t length);
extern void ascii_format_data(uint8_t *buffer, const char *tag, const char *data_ascii, size_t length);
extern void binary_tx_handler(const uint8_t *buffer, uint16_t length);

/* Timer functions */
extern void battery_timer_stop(void);
extern void main_timer_start(void);

/*==============================================================================
 * EXTERNAL VARIABLES
 *============================================================================*/

extern volatile bool processing;
extern bool device_status;
extern uint8_t resetCount;
extern uint8_t ble_bin_buffer[];

extern uint8_t ADC_PD_MODE;
extern bool low_battery_check;
extern bool info4;
extern bool ble_got_new_data;
extern uint8_t m48_samples_in_buffer;
extern uint8_t  m_pd_adc_cnt;
extern uint16_t m_pd_delay_us;
extern uint16_t led_pd_dac_v[];
extern uint32_t m_life_cycle;
extern bool pd_adc_m48_start;
extern bool go_batt;
extern bool motion_data_once;

/*==============================================================================
 * UTILITY FUNCTIONS
 *============================================================================*/

bool cmd_get_u16(const ParsedCmd *cmd, uint8_t word_index, uint16_t *out)
{
    uint8_t pos = (uint8_t)(word_index * 2);
    if (cmd->data_len < (uint8_t)(pos + 2)) {
        return false;
    }
    *out = (uint16_t)cmd->data[pos]
         | (uint16_t)((uint16_t)cmd->data[pos + 1] << 8);
    return true;
}

void cmd_get_ascii(const ParsedCmd *cmd, uint8_t offset, char *out, uint8_t max_len)
{
    uint8_t i;
    uint8_t remain;

    if (offset >= cmd->data_len) {
        out[0] = '\0';
        return;
    }

    remain = (uint8_t)(cmd->data_len - offset);
    if (remain > max_len) {
        remain = max_len;
    }

    for (i = 0; i < remain; i++) {
        out[i] = (char)cmd->data[offset + i];
    }
    out[remain] = '\0';
}

/*==============================================================================
 * COMMAND HANDLER PROTOTYPES
 *============================================================================*/

/* A. Device Status */
static int Cmd_mta(const ParsedCmd *cmd);
static int Cmd_sta(const ParsedCmd *cmd);
static int Cmd_mqq(const ParsedCmd *cmd);
static int Cmd_str(const ParsedCmd *cmd);

/* B. AGC / Gain Measurement */
static int Cmd_mag(const ParsedCmd *cmd);
static int Cmd_sar(const ParsedCmd *cmd);

/* C. LED DP Value */
static int Cmd_ssa(const ParsedCmd *cmd);
static int Cmd_sab(const ParsedCmd *cmd);
static int Cmd_ssb(const ParsedCmd *cmd);
static int Cmd_srb(const ParsedCmd *cmd);

/* D. LED On/Off & Gain Control */
static int Cmd_ssc(const ParsedCmd *cmd);
static int Cmd_ssd(const ParsedCmd *cmd);
static int Cmd_sse(const ParsedCmd *cmd);
static int Cmd_ssf(const ParsedCmd *cmd);
static int Cmd_sif(const ParsedCmd *cmd);
static int Cmd_ssg(const ParsedCmd *cmd);

/* E. Simple PD Measurement */
static int Cmd_ssh(const ParsedCmd *cmd);

/* F. PD-ADC M48 Full Measurement Series */
static int Cmd_mcj(const ParsedCmd *cmd);
static int Cmd_sdj(const ParsedCmd *cmd);
static int Cmd_sej(const ParsedCmd *cmd);
static int Cmd_sfj(const ParsedCmd *cmd);
static int Cmd_ssj(const ParsedCmd *cmd);
static int Cmd_szj(const ParsedCmd *cmd);

/* G. IMM ADC */
static int Cmd_saj(const ParsedCmd *cmd);

/* H. PD-ADC Count & Delay */
static int Cmd_ssk(const ParsedCmd *cmd);
static int Cmd_srk(const ParsedCmd *cmd);
static int Cmd_ssl(const ParsedCmd *cmd);
static int Cmd_srl(const ParsedCmd *cmd);

/* I. Sensor Measurements */
static int Cmd_msn(const ParsedCmd *cmd);
static int Cmd_spn(const ParsedCmd *cmd);
static int Cmd_sso(const ParsedCmd *cmd);
static int Cmd_ssp(const ParsedCmd *cmd);

/* J. Power / Reset / Version / Security */
static int Cmd_ssq(const ParsedCmd *cmd);
static int Cmd_ssr(const ParsedCmd *cmd);
static int Cmd_sss(const ParsedCmd *cmd);
static int Cmd_sst(const ParsedCmd *cmd);
static int Cmd_ssv(const ParsedCmd *cmd);

/* K. Serial / Passkey System */
static int Cmd_ssz(const ParsedCmd *cmd);
static int Cmd_spz(const ParsedCmd *cmd);
static int Cmd_sqz(const ParsedCmd *cmd);
static int Cmd_srz(const ParsedCmd *cmd);

/* L. EEPROM UInt16 Array */
static int Cmd_sez(const ParsedCmd *cmd);
static int Cmd_sfz(const ParsedCmd *cmd);
static int Cmd_sgz(const ParsedCmd *cmd);

/* M. Hardware No / Life Cycle */
static int Cmd_siz(const ParsedCmd *cmd);
static int Cmd_shz(const ParsedCmd *cmd);
static int Cmd_sxz(const ParsedCmd *cmd);
static int Cmd_syz(const ParsedCmd *cmd);

/* N. Memory Read (dr_mem) */
static int Cmd_mrh(const ParsedCmd *cmd);
static int Cmd_mrs(const ParsedCmd *cmd);
static int Cmd_mrp(const ParsedCmd *cmd);
static int Cmd_mrb(const ParsedCmd *cmd);
static int Cmd_mrr(const ParsedCmd *cmd);
static int Cmd_mrc(const ParsedCmd *cmd);
static int Cmd_mrd(const ParsedCmd *cmd);
static int Cmd_mrg(const ParsedCmd *cmd);
static int Cmd_mrl(const ParsedCmd *cmd);

/* O. Memory Write (dr_mem) */
static int Cmd_mwh(const ParsedCmd *cmd);
static int Cmd_mws(const ParsedCmd *cmd);
static int Cmd_mwp(const ParsedCmd *cmd);
static int Cmd_mwb(const ParsedCmd *cmd);
static int Cmd_mwr(const ParsedCmd *cmd);
static int Cmd_mwc(const ParsedCmd *cmd);
static int Cmd_mwd(const ParsedCmd *cmd);
static int Cmd_mwg(const ParsedCmd *cmd);
static int Cmd_mwl(const ParsedCmd *cmd);

/* P. Debug / Test */
static int Cmd_cmd(const ParsedCmd *cmd);

/*==============================================================================
 * COMMAND TABLE
 *============================================================================*/

CmdEntry g_cmd_table[] = {
    /* Debug command */
    { "cmd?", true,  Cmd_cmd },   // GPIO Test

    /* A. Device Status */
    { "mta?", true,  Cmd_mta },
    { "sta?", true,  Cmd_sta },
    { "str?", false, Cmd_str },
    { "mqq?", true,  Cmd_mqq },

    /* B. AGC / Gain Measurement */
    { "mag?", true,  Cmd_mag },
    { "sag?", true,  Cmd_mag },
    { "sar?", false, Cmd_sar },

    /* C. LED Power / DP Value */
    { "ssa?", false, Cmd_ssa },
    { "sab?", false, Cmd_sab },
    { "ssb?", false, Cmd_ssb },
    { "srb?", false, Cmd_srb },

    /* D. LED On/Off & Gain Control */
    { "ssc?", false, Cmd_ssc },
    { "ssd?", false, Cmd_ssd },
    { "sse?", false, Cmd_sse },
    { "ssf?", false, Cmd_ssf },
    { "sif?", false, Cmd_sif },
    { "ssg?", false, Cmd_ssg },

    /* E. Simple PD Measurement */
    { "ssh?", false, Cmd_ssh },

    /* F. PD-ADC M48 Full Measurement Series */
    { "mcj?", true,  Cmd_mcj },
    { "scj?", true,  Cmd_mcj },
    { "sdj?", false, Cmd_sdj },
    { "sej?", false, Cmd_sej },
    { "sfj?", false, Cmd_sfj },
    { "ssj?", false, Cmd_ssj },
    { "szj?", false, Cmd_szj },

    /* G. IMM ADC */
    { "saj?", false, Cmd_saj },

    /* H. PD-ADC Count & Delay */
    { "ssk?", false, Cmd_ssk },
    { "srk?", false, Cmd_srk },
    { "ssl?", false, Cmd_ssl },
    { "srl?", false, Cmd_srl },

    /* I. Sensor Measurements */
    { "msn?", true,  Cmd_msn },
    { "ssn?", true,  Cmd_msn },
    { "spn?", false, Cmd_spn },
    { "sso?", false, Cmd_sso },
    { "ssp?", false, Cmd_ssp },

    /* J. Power / Reset / Version / Security */
    { "ssq?", false, Cmd_ssq },
    { "ssr?", false, Cmd_ssr },
    { "sss?", false, Cmd_sss },
    { "sst?", false, Cmd_sst },
    { "ssv?", false, Cmd_ssv },

    /* K. Serial / Passkey System */
    { "ssz?", false, Cmd_ssz },
    { "spz?", false, Cmd_spz },
    { "sqz?", false, Cmd_sqz },
    { "srz?", false, Cmd_srz },

    /* L. EEPROM UInt16 Array */
    { "sez?", false, Cmd_sez },
    { "sfz?", false, Cmd_sfz },
    { "sgz?", false, Cmd_sgz },

    /* M. Hardware No / Life Cycle */
    { "siz?", true,  Cmd_siz },
    { "shz?", true,  Cmd_shz },
    { "sxz?", false, Cmd_sxz },
    { "syz?", false, Cmd_syz },

    /* N. Memory Read (dr_mem) */
    { "mrh?", true,  Cmd_mrh },    /* hw_no        → rrh: + 12B ASCII  */
    { "mrs?", true,  Cmd_mrs },    /* serial_no    → rrs: + 12B ASCII  */
    { "mrp?", true,  Cmd_mrp },    /* passkey      → rrp: + 6B         */
    { "mrb?", true,  Cmd_mrb },    /* bond_delete  → rrb: + uint16     */
    { "mrr?", true,  Cmd_mrr },    /* reset_status → rrr: + uint16     */
    { "mrc?", true,  Cmd_mrc },    /* pd_adc_cnt   → rrc: + uint16     */
    { "mrd?", true,  Cmd_mrd },    /* pd_delay     → rrd: + uint16     */
    { "mrg?", true,  Cmd_mrg },    /* agc_gain     → rrg: + 48*uint16  */
    { "mrl?", true,  Cmd_mrl },    /* life_cycle   → rrl: + uint32     */

    /* O. Memory Write (dr_mem) */
    { "mwh?", true,  Cmd_mwh },    /* hw_no        ← 12B ASCII → rwh: + status */
    { "mws?", true,  Cmd_mws },    /* serial_no    ← 12B ASCII → rws: + status */
    { "mwp?", true,  Cmd_mwp },    /* passkey      ← 6B        → rwp: + status */
    { "mwb?", true,  Cmd_mwb },    /* bond_delete  ← uint16    → rwb: + status */
    { "mwr?", true,  Cmd_mwr },    /* reset_status ← uint16    → rwr: + status */
    { "mwc?", true,  Cmd_mwc },    /* pd_adc_cnt   ← uint16    → rwc: + status */
    { "mwd?", true,  Cmd_mwd },    /* pd_delay     ← uint16    → rwd: + status */
    { "mwg?", true,  Cmd_mwg },    /* agc_gain     ← 48*uint16 → rwg: + status */
    { "mwl?", true,  Cmd_mwl },    /* life_cycle   ← uint32    → rwl: + status */
};

const uint16_t g_cmd_count = (uint16_t)(sizeof(g_cmd_table) / sizeof(g_cmd_table[0]));

/*==============================================================================
 * COMMAND HANDLER IMPLEMENTATIONS
 *============================================================================*/

/* A. Device Status */
static int Cmd_mta(const ParsedCmd *cmd)
{
    uint16_t mode = 0;

    resetCount = 0;
    (void)cmd_get_u16(cmd, 0, &mode);

    if (g_plat.log && g_log_enable) {
        g_plat.log("[Cmd_mta] mode=%u\r\n", mode);
    }

    if (mode == 1) {
        if (device_activated() == 0) {
            device_status = true;
        }
    }
    else if (mode == 0) {
        if (device_status == true) {
            if (device_sleep_mode() == 0) {
                device_status = false;
            }
        }
    }

    if (g_plat.tx_bin) {
        single_format_data(ble_bin_buffer, "rta:", mode);
        binary_tx_handler(ble_bin_buffer, 3);
    }

    return 1;
}

static int Cmd_sta(const ParsedCmd *cmd)
{
    uint16_t mode = 0;

    resetCount = 0;
    (void)cmd_get_u16(cmd, 0, &mode);

    if (g_plat.log && g_log_enable) {
        g_plat.log("[Cmd_sta] mode=%u\r\n", mode);
    }

    if (mode == 1) {
        if (device_activated() == 0) {
            device_status = true;
        }
    }
    else if (mode == 0) {
        if (device_status == true) {
            if (device_sleep_mode() == 0) {
                device_status = false;
            }
        }
    }

    if (g_plat.tx_bin) {
        single_format_data(ble_bin_buffer, "sta:", mode);
        binary_tx_handler(ble_bin_buffer, 3);
    }

    return 1;
}

static int Cmd_str(const ParsedCmd *cmd)
{
    (void)cmd;
    uint8_t status = 1;

    if (g_plat.log && g_log_enable) {
        g_plat.log("[Cmd_str] read status=%u\n", status);
    }

    if (g_plat.tx_bin) {
        uint8_t resp[4] = { 'r','t','r', status };
        g_plat.tx_bin(resp, 4);
    }
    return 1;
}

static int Cmd_mqq(const ParsedCmd *cmd)
{
    (void)cmd;
    simple_mesurement_start();
    return 1;
}

/* B. AGC / Gain Measurement */
static int Cmd_mag(const ParsedCmd *cmd)
{
    (void)cmd;

    if (g_plat.log && g_log_enable) {
        g_plat.log("[Cmd_mag] Full AGC measurement request\r\n");
    }

    if (device_status != true) {
        if (g_plat.log && g_log_enable) {
            g_plat.log("[Cmd_mag] ERROR: Device not activated\r\n");
        }
        if (g_plat.tx_bin) {
            param_error("mag?");
        }
        return 1;
    }

    processing = true;

    if (g_plat.log && g_log_enable) {
        g_plat.log("[Cmd_mag] full_agc_mesurement_start()\r\n");
    }

    full_agc_mesurement_start();

    return 1;
}

static int Cmd_sar(const ParsedCmd *cmd)
{
    (void)cmd;
    if (g_plat.log && g_log_enable) {
        g_plat.log("[Cmd_sar] Read LED-PD gain array\n");
    }
    return 1;
}

/* C. LED Power / DP Value */
static int Cmd_ssa(const ParsedCmd *cmd)
{
    uint16_t led_index = 0;
    (void)cmd_get_u16(cmd, 0, &led_index);

    if (g_plat.log && g_log_enable) {
        g_plat.log("[Cmd_ssa] LED index=%u\n", led_index);
    }
    return 1;
}

static int Cmd_sab(const ParsedCmd *cmd)
{
    (void)cmd;
    if (g_plat.log && g_log_enable) {
        g_plat.log("[Cmd_sab] Read AGC-related LED data\n");
    }
    return 1;
}

static int Cmd_ssb(const ParsedCmd *cmd)
{
    uint16_t led_index = 0;
    uint16_t power = 0;
    (void)cmd_get_u16(cmd, 0, &led_index);
    (void)cmd_get_u16(cmd, 1, &power);

    if (g_plat.log && g_log_enable) {
        g_plat.log("[Cmd_ssb] LED index=%u, power=%u\n", led_index, power);
    }
    return 1;
}

static int Cmd_srb(const ParsedCmd *cmd)
{
    (void)cmd;
    if (g_plat.log && g_log_enable) {
        g_plat.log("[Cmd_srb] Read all 48 LED DP values\n");
    }
    return 1;
}

/* D. LED On/Off & Gain Control */
static int Cmd_ssc(const ParsedCmd *cmd)
{
    uint16_t led_index = 0;
    (void)cmd_get_u16(cmd, 0, &led_index);

    if (g_plat.log && g_log_enable) {
        g_plat.log("[Cmd_ssc] LED On/Off index=%u\n", led_index);
    }
    return 1;
}

static int Cmd_ssd(const ParsedCmd *cmd)
{
    uint16_t on_off = 0;
    (void)cmd_get_u16(cmd, 0, &on_off);

    if (g_plat.log && g_log_enable) {
        g_plat.log("[Cmd_ssd] AGC on_off=%u\n", on_off);
    }
    return 1;
}

static int Cmd_sse(const ParsedCmd *cmd)
{
    (void)cmd;
    if (g_plat.log && g_log_enable) {
        g_plat.log("[Cmd_sse] Measure DAC voltage\n");
    }
    return 1;
}

static int Cmd_ssf(const ParsedCmd *cmd)
{
    uint16_t led_index = 0;
    uint16_t dac_value = 0;
    (void)cmd_get_u16(cmd, 0, &led_index);
    (void)cmd_get_u16(cmd, 1, &dac_value);

    if (g_plat.log && g_log_enable) {
        g_plat.log("[Cmd_ssf] Set LED DP DAC: index=%u, dac=%u\n", led_index, dac_value);
    }
    return 1;
}

static int Cmd_sif(const ParsedCmd *cmd)
{
    uint16_t dac_value = 0;
    (void)cmd_get_u16(cmd, 0, &dac_value);

    if (g_plat.log && g_log_enable) {
        g_plat.log("[Cmd_sif] Immediate gain set: dac=%u\n", dac_value);
    }
    return 1;
}

static int Cmd_ssg(const ParsedCmd *cmd)
{
    uint16_t pd_index = 0;
    (void)cmd_get_u16(cmd, 0, &pd_index);

    if (g_plat.log && g_log_enable) {
        g_plat.log("[Cmd_ssg] Set PD channel=%u\n", pd_index);
    }
    return 1;
}

/* E. Simple PD Measurement */
static int Cmd_ssh(const ParsedCmd *cmd)
{
    (void)cmd;
    if (g_plat.log && g_log_enable) {
        g_plat.log("[Cmd_ssh] Simple PD measurement start\n");
    }
    return 1;
}

/* F. PD-ADC M48 Full Measurement Series */
static int Cmd_mcj(const ParsedCmd *cmd)
{
    (void)cmd;

    if (g_plat.log && g_log_enable) {
        g_plat.log("[Cmd_mcj] PD-ADC M48 MODE=2 (Press + M48)\r\n");
    }

    if (device_status != true) {
        if (g_plat.log && g_log_enable) {
            g_plat.log("[Cmd_mcj] ERROR: Device not activated\r\n");
        }
        if (g_plat.tx_bin) {
            param_error("mcj?");
        }
        return 1;
    }

    ADC_PD_MODE = 2;
    info4 = true;
    ble_got_new_data = false;
    processing = true;

    pressure_all_level_meas();
    AGC_GAIN_SW(false);

    m48_samples_in_buffer = m_pd_adc_cnt;
    pd_adc_m48_start = true;
    battery_timer_stop();

    go_batt = true;
    motion_data_once = true;
    main_timer_start();

    if (g_plat.log && g_log_enable) {
        g_plat.log("[Cmd_mcj] Measurement started\r\n");
    }

    return 1;
}

static int Cmd_sdj(const ParsedCmd *cmd)
{
    (void)cmd;
    if (g_plat.log && g_log_enable) {
        g_plat.log("[Cmd_sdj] MODE=3 (M48 only)\n");
    }
    return 1;
}

static int Cmd_sej(const ParsedCmd *cmd)
{
    (void)cmd;
    if (g_plat.log && g_log_enable) {
        g_plat.log("[Cmd_sej] MODE=4 (M48 + batt + IMU)\n");
    }
    return 1;
}

static int Cmd_sfj(const ParsedCmd *cmd)
{
    (void)cmd;
    if (g_plat.log && g_log_enable) {
        g_plat.log("[Cmd_sfj] MODE=5 (M48 + init)\n");
    }
    return 1;
}

static int Cmd_ssj(const ParsedCmd *cmd)
{
    (void)cmd;
    if (g_plat.log && g_log_enable) {
        g_plat.log("[Cmd_ssj] MODE=0 (M48 + batt/IMU combined)\n");
    }
    return 1;
}

static int Cmd_szj(const ParsedCmd *cmd)
{
    uint16_t off_dac = 0;
    uint16_t off_pd = 0;
    (void)cmd_get_u16(cmd, 0, &off_dac);
    (void)cmd_get_u16(cmd, 1, &off_pd);

    if (g_plat.log && g_log_enable) {
        g_plat.log("[Cmd_szj] FAST mode: off_dac=%u, off_pd=%u\n", off_dac, off_pd);
    }
    return 1;
}

/* G. IMM ADC */
static int Cmd_saj(const ParsedCmd *cmd)
{
    uint16_t led0 = 0, led1 = 0, led2 = 0, led3 = 0;
    (void)cmd_get_u16(cmd, 0, &led0);
    (void)cmd_get_u16(cmd, 1, &led1);
    (void)cmd_get_u16(cmd, 2, &led2);
    (void)cmd_get_u16(cmd, 3, &led3);

    if (g_plat.log && g_log_enable) {
        g_plat.log("[Cmd_saj] IMM ADC LEDs: %u,%u,%u,%u\n", led0, led1, led2, led3);
    }
    return 1;
}

/* H. PD-ADC Count & Delay */
static int Cmd_ssk(const ParsedCmd *cmd)
{
    uint16_t count = 0;
    (void)cmd_get_u16(cmd, 0, &count);

    if (g_plat.log && g_log_enable) {
        g_plat.log("[Cmd_ssk] Set PD ADC count=%u\n", count);
    }
    return 1;
}

static int Cmd_srk(const ParsedCmd *cmd)
{
    (void)cmd;
    if (g_plat.log && g_log_enable) {
        g_plat.log("[Cmd_srk] Read PD ADC count\n");
    }
    return 1;
}

static int Cmd_ssl(const ParsedCmd *cmd)
{
    uint16_t delay_us = 0;
    (void)cmd_get_u16(cmd, 0, &delay_us);

    if (g_plat.log && g_log_enable) {
        g_plat.log("[Cmd_ssl] Set PD delay=%u us\n", delay_us);
    }
    return 1;
}

static int Cmd_srl(const ParsedCmd *cmd)
{
    (void)cmd;
    if (g_plat.log && g_log_enable) {
        g_plat.log("[Cmd_srl] Read PD delay\n");
    }
    return 1;
}

/* I. Sensor Measurements */
static int Cmd_msn(const ParsedCmd *cmd)
{
    (void)cmd;
    if (g_plat.log && g_log_enable) {
        g_plat.log("[Cmd_msn] Measure battery level\n");
    }
    low_battery_check = false;  /* Ensure response is sent via BLE */
    battery_level_meas();
    return 1;
}

static int Cmd_spn(const ParsedCmd *cmd)
{
    (void)cmd;
    if (g_plat.log && g_log_enable) {
        g_plat.log("[Cmd_spn] Measure pressure1 & 2\n");
    }
    return 1;
}

static int Cmd_sso(const ParsedCmd *cmd)
{
    (void)cmd;
    if (g_plat.log && g_log_enable) {
        g_plat.log("[Cmd_sso] Measure LED temperature\n");
    }
    return 1;
}

static int Cmd_ssp(const ParsedCmd *cmd)
{
    char mode = '1';
    if (cmd->data_len > 0) {
        mode = (char)cmd->data[0];
    }

    if (g_plat.log && g_log_enable) {
        g_plat.log("[Cmd_ssp] Motion sensor raw, mode='%c'\n", mode);
    }
    return 1;
}

/* J. Power / Reset / Version / Security */
static int Cmd_ssq(const ParsedCmd *cmd)
{
    (void)cmd;
    if (g_plat.log && g_log_enable) {
        g_plat.log("[Cmd_ssq] Power off\n");
    }
    return 1;
}

static int Cmd_ssr(const ParsedCmd *cmd)
{
    (void)cmd;
    if (g_plat.log && g_log_enable) {
        g_plat.log("[Cmd_ssr] Bond delete\n");
    }
    return 1;
}

static int Cmd_sss(const ParsedCmd *cmd)
{
    (void)cmd;
    if (g_plat.log && g_log_enable) {
        g_plat.log("[Cmd_sss] Device reset\n");
    }
    return 1;
}

static int Cmd_sst(const ParsedCmd *cmd)
{
    (void)cmd;
    if (g_plat.log && g_log_enable) {
        g_plat.log("[Cmd_sst] Ready\n");
    }
    return 1;
}

static int Cmd_ssv(const ParsedCmd *cmd)
{
    (void)cmd;
    if (g_plat.log && g_log_enable) {
        g_plat.log("[Cmd_ssv] Read firmware version\n");
    }
    return 1;
}

/* K. Serial / Passkey System */
static int Cmd_ssz(const ParsedCmd *cmd)
{
    char serial[13];
    cmd_get_ascii(cmd, 0, serial, 12);

    if (g_plat.log && g_log_enable) {
        g_plat.log("[Cmd_ssz] Write Serial='%s'\n", serial);
    }
    return 1;
}

static int Cmd_spz(const ParsedCmd *cmd)
{
    char pass[7];
    cmd_get_ascii(cmd, 0, pass, 6);

    if (g_plat.log && g_log_enable) {
        g_plat.log("[Cmd_spz] Write Passkey='%s'\n", pass);
    }
    return 1;
}

static int Cmd_sqz(const ParsedCmd *cmd)
{
    (void)cmd;
    if (g_plat.log && g_log_enable) {
        g_plat.log("[Cmd_sqz] Read Passkey\n");
    }
    return 1;
}

static int Cmd_srz(const ParsedCmd *cmd)
{
    (void)cmd;
    if (g_plat.log && g_log_enable) {
        g_plat.log("[Cmd_srz] Read Serial Number\n");
    }
    return 1;
}

/* L. EEPROM UInt16 Array */
static int Cmd_sez(const ParsedCmd *cmd)
{
    uint16_t addr = 0;
    (void)cmd_get_u16(cmd, 0, &addr);

    if (g_plat.log && g_log_enable) {
        g_plat.log("[Cmd_sez] Write 48*uint16 at addr=0x%04X\n", addr);
    }
    return 1;
}

static int Cmd_sfz(const ParsedCmd *cmd)
{
    uint16_t addr = 0;
    (void)cmd_get_u16(cmd, 0, &addr);

    if (g_plat.log && g_log_enable) {
        g_plat.log("[Cmd_sfz] Read 48*uint16 from addr=0x%04X\n", addr);
    }
    return 1;
}

static int Cmd_sgz(const ParsedCmd *cmd)
{
    uint16_t addr = 0;
    (void)cmd_get_u16(cmd, 0, &addr);

    if (g_plat.log && g_log_enable) {
        g_plat.log("[Cmd_sgz] Load DP preset from EEPROM addr=0x%04X\n", addr);
    }
    return 1;
}

/* M. Hardware No / Life Cycle */
static int Cmd_siz(const ParsedCmd *cmd)
{
    (void)cmd;
    char buf[12];
    dr_memRead("hw_no", buf, 12);
    ascii_format_data(ble_bin_buffer, "riz:", buf, 12);
    binary_tx_handler(ble_bin_buffer, 8);  /* (4+12)/2 = 8 words */
    return 1;
}

static int Cmd_shz(const ParsedCmd *cmd)
{
    char hw[13];
    memset(hw, 0, sizeof(hw));
    cmd_get_ascii(cmd, 0, hw, 12);
    DBG_PRINTF("[shz] hw='%s'\r\n", hw);
    ret_code_t err = dr_memWrite("hw_no", hw, 12);
    dr_ble_return_1("rhz:", err == NRF_SUCCESS ? 0 : 1);
    return 1;
}

static int Cmd_sxz(const ParsedCmd *cmd)
{
    uint16_t hi = 0, lo = 0;
    (void)cmd_get_u16(cmd, 0, &hi);
    (void)cmd_get_u16(cmd, 1, &lo);

    {
        uint32_t life = ((uint32_t)hi << 16) | (uint32_t)lo;
        if (g_plat.log && g_log_enable) {
            g_plat.log("[Cmd_sxz] Write Life Cycle=%lu\n", (unsigned long)life);
        }
    }
    return 1;
}

static int Cmd_syz(const ParsedCmd *cmd)
{
    (void)cmd;
    if (g_plat.log && g_log_enable) {
        g_plat.log("[Cmd_syz] Read Life Cycle\n");
    }
    return 1;
}

/* N. Memory Read (dr_mem) */

static int Cmd_mrh(const ParsedCmd *cmd)
{
    (void)cmd;
    char buf[12];
    char tmp[13] = {0};
    dr_memRead("hw_no", buf, 12);
    memcpy(tmp, buf, 12);
    DBG_PRINTF("[mrh] hw='%s' hex:%02X%02X%02X%02X\r\n", tmp,
               (uint8_t)buf[0], (uint8_t)buf[1], (uint8_t)buf[2], (uint8_t)buf[3]);
    ascii_format_data(ble_bin_buffer, "rrh:", buf, 12);
    binary_tx_handler(ble_bin_buffer, 8);   /* (4+12)/2 = 8 words */
    return 1;
}

static int Cmd_mrs(const ParsedCmd *cmd)
{
    (void)cmd;
    char buf[12];
    char tmp[13] = {0};
    dr_memRead("serial_no", buf, 12);
    memcpy(tmp, buf, 12);
    DBG_PRINTF("[mrs] sn='%s' hex:%02X%02X%02X%02X\r\n", tmp,
               (uint8_t)buf[0], (uint8_t)buf[1], (uint8_t)buf[2], (uint8_t)buf[3]);
    ascii_format_data(ble_bin_buffer, "rrs:", buf, 12);
    binary_tx_handler(ble_bin_buffer, 8);   /* (4+12)/2 = 8 words */
    return 1;
}

static int Cmd_mrp(const ParsedCmd *cmd)
{
    (void)cmd;
    uint8_t buf[6];
    dr_memRead("passkey", buf, 6);
    format_data_byte(ble_bin_buffer, "rrp:", buf, 6);
    binary_tx_handler(ble_bin_buffer, 5);   /* (4+6)/2 = 5 words */
    return 1;
}

static int Cmd_mrb(const ParsedCmd *cmd)
{
    (void)cmd;
    uint8_t val = 0;
    dr_memRead("bond_delete", &val, 1);
    dr_ble_return_1("rrb:", (uint16_t)val);
    return 1;
}

static int Cmd_mrr(const ParsedCmd *cmd)
{
    (void)cmd;
    uint8_t val = 0;
    dr_memRead("reset_status", &val, 1);
    dr_ble_return_1("rrr:", (uint16_t)val);
    return 1;
}

static int Cmd_mrc(const ParsedCmd *cmd)
{
    (void)cmd;
    uint8_t val = 0;
    dr_memRead("pd_adc_cnt", &val, 1);
    dr_ble_return_1("rrc:", (uint16_t)val);
    return 1;
}

static int Cmd_mrd(const ParsedCmd *cmd)
{
    (void)cmd;
    uint16_t val = 0;
    dr_memRead("pd_delay", &val, 2);
    dr_ble_return_1("rrd:", val);
    return 1;
}

static int Cmd_mrg(const ParsedCmd *cmd)
{
    (void)cmd;
    /* Read from RAM global (W25Q32 may not be initialized at boot) */
    format_data(ble_bin_buffer, "rrg:", led_pd_dac_v, 48);
    binary_tx_handler(ble_bin_buffer, 50);  /* (4+96)/2 = 50 words */
    return 1;
}

static int Cmd_mrl(const ParsedCmd *cmd)
{
    (void)cmd;
    /* Read from RAM global (W25Q32 may not be initialized at boot) */
    uint16_t hi = (uint16_t)(m_life_cycle >> 16);
    uint16_t lo = (uint16_t)(m_life_cycle & 0xFFFF);
    dr_ble_return_2("rrl:", hi, lo);
    return 1;
}

/* N. Config Write Commands (mw* → rw* responses) */

static int Cmd_mwh(const ParsedCmd *cmd)
{
    char hw[13];
    memset(hw, 0, sizeof(hw));
    cmd_get_ascii(cmd, 0, hw, 12);
    ret_code_t err = dr_memWrite("hw_no", hw, 12);
    dr_ble_return_1("rwh:", err == NRF_SUCCESS ? 0 : 1);
    return 1;
}

static int Cmd_mws(const ParsedCmd *cmd)
{
    char serial[13];
    memset(serial, 0, sizeof(serial));
    cmd_get_ascii(cmd, 0, serial, 12);
    DBG_PRINTF("[mws] serial='%s'\r\n", serial);

    ret_code_t err = dr_memWrite("serial_no", serial, 12);
    dr_ble_return_1("rws:", err == NRF_SUCCESS ? 0 : 1);
    return 1;
}

static int Cmd_mwp(const ParsedCmd *cmd)
{
    char pass[7];
    memset(pass, 0, sizeof(pass));
    cmd_get_ascii(cmd, 0, pass, 6);
    ret_code_t err = dr_memWrite("passkey", pass, 6);
    dr_ble_return_1("rwp:", err == NRF_SUCCESS ? 0 : 1);
    return 1;
}

static int Cmd_mwb(const ParsedCmd *cmd)
{
    uint16_t val = 0;
    (void)cmd_get_u16(cmd, 0, &val);
    uint8_t u8val = (uint8_t)val;
    ret_code_t err = dr_memWrite("bond_delete", &u8val, 1);
    dr_ble_return_1("rwb:", err == NRF_SUCCESS ? 0 : 1);
    return 1;
}

static int Cmd_mwr(const ParsedCmd *cmd)
{
    uint16_t val = 0;
    (void)cmd_get_u16(cmd, 0, &val);
    uint8_t u8val = (uint8_t)val;
    ret_code_t err = dr_memWrite("reset_status", &u8val, 1);
    dr_ble_return_1("rwr:", err == NRF_SUCCESS ? 0 : 1);
    return 1;
}

static int Cmd_mwc(const ParsedCmd *cmd)
{
    uint16_t val = 0;
    (void)cmd_get_u16(cmd, 0, &val);
    uint8_t u8val = (uint8_t)val;
    ret_code_t err = dr_memWrite("pd_adc_cnt", &u8val, 1);
    m_pd_adc_cnt = u8val;
    dr_ble_return_1("rwc:", err == NRF_SUCCESS ? 0 : 1);
    return 1;
}

static int Cmd_mwd(const ParsedCmd *cmd)
{
    uint16_t val = 0;
    (void)cmd_get_u16(cmd, 0, &val);
    ret_code_t err = dr_memWrite("pd_delay", &val, 2);
    m_pd_delay_us = val;
    dr_ble_return_1("rwd:", err == NRF_SUCCESS ? 0 : 1);
    return 1;
}

static int Cmd_mwg(const ParsedCmd *cmd)
{
    uint16_t gains[48];
    uint8_t i;
    for (i = 0; i < 48; i++) {
        if (!cmd_get_u16(cmd, i, &gains[i])) {
            gains[i] = 0;
        }
    }
    ret_code_t err = dr_memWrite("agc_gain", gains, sizeof(gains));
    /* Update RAM copy */
    for (i = 0; i < 48; i++) {
        led_pd_dac_v[i] = gains[i];
    }
    dr_ble_return_1("rwg:", err == NRF_SUCCESS ? 0 : 1);
    return 1;
}

static int Cmd_mwl(const ParsedCmd *cmd)
{
    uint16_t hi = 0, lo = 0;
    (void)cmd_get_u16(cmd, 0, &hi);
    (void)cmd_get_u16(cmd, 1, &lo);
    uint32_t life = ((uint32_t)hi << 16) | (uint32_t)lo;
    ret_code_t err = dr_memWrite("life_cycle", &life, 4);
    m_life_cycle = life;
    dr_ble_return_1("rwl:", err == NRF_SUCCESS ? 0 : 1);
    return 1;
}

/* O. Debug / Test */
static int Cmd_cmd(const ParsedCmd *cmd)
{
    uint16_t v1, v2, v3;
    uint32_t pin_number;

    DBG_PRINTF("[CMD] cmd? received, data_len=%u\r\n", cmd->data_len);

    if (cmd->data_len < 6) {
        DBG_PRINTF("[CMD] data_len < 6, return 0\r\n");
        dr_ble_return_1("rmd:", 0);
        return 1;
    }

    v1 = (uint16_t)cmd->data[0] | ((uint16_t)cmd->data[1] << 8);
    v2 = (uint16_t)cmd->data[2] | ((uint16_t)cmd->data[3] << 8);
    v3 = (uint16_t)cmd->data[4] | ((uint16_t)cmd->data[5] << 8);

    DBG_PRINTF("[CMD] v1=%u v2=%u v3=%u\r\n", v1, v2, v3);

    /* v1=99: Flash Test */
    if (v1 == 99) {
        DBG_PRINTF("[CMD] Flash Test Start!\r\n");
        uint16_t result = 0;
        if (w25q32_init()) {
            result = 1;  /* Success */
            DBG_PRINTF("[CMD] Flash Test OK\r\n");
        } else {
            DBG_PRINTF("[CMD] Flash Test FAIL\r\n");
        }
        dr_ble_return_1("rmd:", result);
        return 1;
    }

    /* Default: GPIO Test */
    pin_number = NRF_GPIO_PIN_MAP(v1, v2);
    nrf_gpio_cfg_output(pin_number);

    if (v3 == 1) {
        nrf_gpio_pin_set(pin_number);
    } else {
        nrf_gpio_pin_clear(pin_number);
    }

    dr_ble_return_3("rmd:", v1, v2, v3);

    return 1;
}