/* 
 * 2025-12-08 power loop bug fix 
 * 2025-12-07 msn, mta, mqq
 * 2025-12-04 by Charles KWON
 * parser.c : Common parser + command table + handlers
 *  - Firmware/PC shared
 *  - Hardware-dependent parts left as TODO
 *  - Added CRC16 validation support
 */

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

#include "nrf_gpio.h"
#include "nrf_delay.h"
#include "dr_piezo.h"
#include "dr_util.h"
#include "dr_adc121s051.h"



// ========================================
// 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);


/* 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 ascii_format_data(uint8_t *buffer, const char *tag, const char *ascii, uint8_t len);
extern void binary_tx_handler(const uint8_t *buffer, uint16_t length);
extern void dr_sd_delay_ms(uint32_t ms);  /* Softdevice-friendly delay */

/* FDS config (fstorage) */
#include "fstorage.h"
extern char SERIAL_NO[12];
extern char HW_NO[12];            




// ========================================
// External variables
// ========================================

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

extern uint8_t simple_samples_in_buffer;  
extern uint8_t m_pd_adc_cnt;              
extern bool con_single;                   
extern bool lock_check;   


extern bool info4;                          // addtional info
extern bool ble_got_new_data;               // BLE data flag
extern uint8_t m_pd_adc_cnt;                // PD ADC count
extern bool go_batt;                        // battery
extern bool motion_data_once;               // IMU data flag
extern bool motion_raw_data_enabled;        // IMU continuous flag
extern int  imu_read_direct(void);           // IMU direct register read + BLE send
extern uint8_t ADC_PD_MODE;                 // PD ADC mode
extern bool pd_adc_m48_start;               // PD ADC M48 start flag
extern uint8_t m48_samples_in_buffer;       // M48 sample count

extern void pressure_all_level_meas(void);  // pressure sensor
extern void battery_timer_stop(void);       // battery timer
extern void main_timer_start(void);         // main timer
extern void hw_i2c_init_once(void);         // I2C init for IMU

/* AGC_GAIN_SW is a macro in measurements.h - replicate here */
#include "nrf_gpio.h"
#define GAIN_SW_PIN  NRF_GPIO_PIN_MAP(0, 20)
#define AGC_GAIN_SW(x) do { if(x) nrf_gpio_pin_set(GAIN_SW_PIN); else nrf_gpio_pin_clear(GAIN_SW_PIN); } while(0)


extern void dr_piezo_power_on( void );
extern void dr_piezo_burst_sw(uint8_t cycles);
extern void dr_piezo_burst_sw_18mhz(uint8_t cycles);
extern void dr_piezo_burst_sw_20mhz(uint8_t cycles);
extern void dr_piezo_burst_sw_17mhz(uint8_t cycles);

/* ---- Global variable definitions (extern in header) ---- */
dr_platform_if_t g_plat = { 0, 0, 0 };
bool g_log_enable = false;

/* ---- Internal constants/structures ---- */

#define DR_MAX_DATA 128  /* Max data length after TAG */

typedef struct {
    char    tag[5];               /* "sta?" etc 4 chars + '\0' */
    uint8_t data[DR_MAX_DATA];    /* Raw data after TAG */
    uint8_t data_len;             /* Length of data[] */
} ParsedCmd;

/* ---- Internal utility functions ---- */

/* Copy TAG */
static void dr_copy_tag(const uint8_t *buf, char *tag_out)
{
    tag_out[0] = (char)buf[0];
    tag_out[1] = (char)buf[1];
    tag_out[2] = (char)buf[2];
    tag_out[3] = (char)buf[3];
    tag_out[4] = '\0';
}

/* TAG comparison (4 chars) */
static bool dr_tag_eq(const char *tag, const char *key4)
{
    return (tag[0] == key4[0] &&
            tag[1] == key4[1] &&
            tag[2] == key4[2] &&
            tag[3] == key4[3]);
}

/* Extract uint16 little endian: word_index based (0 -> data[0], data[1]) */
/* Extract uint16 BIG endian: word_index based (0 -> data[0], data[1]) */
/* Extract uint16 LITTLE endian: word_index based (0 -> data[0], data[1]) */
static bool dr_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;
    }
    
    // Little Endian: data[pos] = low byte, data[pos+1] = high byte
    *out = (uint16_t)cmd->data[pos]
         | (uint16_t)((uint16_t)cmd->data[pos + 1] << 8);
    
    return true;
}

/* Extract ASCII: data[offset..offset+len] -> out, '\0' terminated */
/* EEPROM에서 텍스트 쓸 때 ASCII 문자열 추출 -> 추후 Flash Memory 커맨드에서 재활용 가능 */
static void dr_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';
}

/* ---- CRC16 functions ---- */

/* CRC16 computation - matches Nordic SDK crc16_compute */
static uint16_t dr_crc16_compute(const uint8_t *p_data, uint32_t size, const uint16_t *p_crc)
{
    uint32_t i;
    uint16_t crc = (p_crc == NULL) ? 0xFFFF : *p_crc;

    for (i = 0; i < size; i++)
    {
        crc = (uint8_t)(crc >> 8) | (crc << 8);
        crc ^= p_data[i];
        crc ^= (uint8_t)(crc & 0xFF) >> 4;
        crc ^= (crc << 8) << 4;
        crc ^= ((crc & 0xFF) << 4) << 1;
    }

    return crc;
}

/* CRC16 check: compare computed vs expected */
static bool dr_crc16_check(const uint8_t *p_data, uint32_t data_len, uint16_t expected_crc)
{
    uint16_t computed_crc = dr_crc16_compute(p_data, data_len, NULL);
    return (computed_crc == expected_crc);
}

/* CRC16 packet check: last 2 bytes are CRC (little endian) */
static bool dr_crc16_check_packet(const uint8_t *packet, uint32_t packet_len)
{
    uint16_t expected_crc;
    uint32_t data_len;

    if (packet_len < 2) {
        return false;
    }

    data_len = packet_len - 2;
    
    /* Extract CRC: little endian (low byte first) */
    expected_crc = (uint16_t)packet[packet_len - 2] 
                 | ((uint16_t)packet[packet_len - 1] << 8);

    if (g_plat.log && g_log_enable) {
        g_plat.log("CRC check: expected=0x%04X\n", expected_crc);
    }

    return dr_crc16_check(packet, data_len, expected_crc);
}

/* ---- Raw buffer -> ParsedCmd ---- */

static bool dr_parse_cmd(const uint8_t *buffer, uint8_t length, ParsedCmd *out)
{
    uint8_t i;

    if (length < 4) {
        return false;  /* Not even TAG received */
    }

    /* CRC check if enabled */
    if (g_plat.crc_check) {
        if (!dr_crc16_check_packet(buffer, length)) {
            if (g_plat.log && g_log_enable) {
                g_plat.log("CRC check FAILED!\n");
            }
            return false;
        }
        
        /* CRC validated - remove CRC bytes from data */
        length = (uint8_t)(length - 2);
        
        if (g_plat.log && g_log_enable) {
            g_plat.log("CRC check OK\n");
        }
    }

    dr_copy_tag(buffer, out->tag);

    out->data_len = (length > 4) ? (uint8_t)(length - 4) : 0;
    if (out->data_len > DR_MAX_DATA) {
        out->data_len = DR_MAX_DATA;
    }

    for (i = 0; i < out->data_len; i++) {
        out->data[i] = buffer[4 + i];
    }

    if (g_plat.log && g_log_enable) {
        g_plat.log("parse_cmd: TAG='%s', data_len=%u\n",
                   out->tag, out->data_len);
    }

    return true;
}



/* ---- Handler prototypes (Harbour style: int return) ---- */

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

/* F. PD-ADC M48 Full Measurement Series */
static int Cmd_mcj(const ParsedCmd *cmd);
static int Cmd_sej(const ParsedCmd *cmd);
static int Cmd_ssj(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);

static int Cmd_mpa(const ParsedCmd *cmd);
static int Cmd_msp(const ParsedCmd *cmd);  /* IMU 6-axis raw data (single shot) */
static int Cmd_mpc(const ParsedCmd *cmd);
static int Cmd_mdc(const ParsedCmd *cmd);
static int Cmd_mec(const ParsedCmd *cmd);
static int Cmd_maa(const ParsedCmd *cmd);  /* 8-channel all capture */
static int Cmd_cmd(const ParsedCmd *cmd);
static int Cmd_mwh(const ParsedCmd *cmd);  /* Write HW Number to FDS */
static int Cmd_mws(const ParsedCmd *cmd);  /* Write Serial Number to FDS */
static int Cmd_mrh(const ParsedCmd *cmd);  /* Read HW Number from FDS */
static int Cmd_mrs(const ParsedCmd *cmd);  /* Read Serial Number from FDS */

/* ---- Command Table ---- */

typedef struct {
    char tag[5];                  /* "sta?" */
    bool enabled;                 /* false = handler won't be called */
    int  (*handler)(const ParsedCmd *cmd); /* 1=success, 0=fail */
} CmdEntry;

static CmdEntry g_cmd_table[] = {
	
	/* sudo command  */
	{ "cmd?", true,  Cmd_cmd },  // Piezo Activate
	
      /* Piezo command  */
	{ "mpa?", true,  Cmd_mpa },  // Piezo Activate
	{ "mpc?", true,  Cmd_mpc },  // Piezo Cycles control command, 3,4,5,6,7
	{ "mdc?", true,  Cmd_mdc },  // Piezo burst + Echo capture (12-bit packed)
	{ "mec?", true,  Cmd_mec },  // Piezo burst + Echo capture (16-bit raw)
	{ "maa?", true,  Cmd_maa },  // 8-channel all capture (mode: 0=raw, 1=delta)
	{ "msp?", true,  Cmd_msp },  // IMU 6-axis raw data (single shot)

	/* Config: HW/Serial Number (FDS) */
	{ "mwh?", true,  Cmd_mwh },  // Write HW Number
	{ "mws?", true,  Cmd_mws },  // Write Serial Number
	{ "mrh?", true,  Cmd_mrh },  // Read HW Number
	{ "mrs?", true,  Cmd_mrs },  // Read Serial Number

    /* A. Device Status */
    { "mta?", true,  Cmd_mta },
	{ "sta?", true,  Cmd_sta },
    { "str?", false, Cmd_str },
    /* F. PD-ADC M48 Full Measurement Series */
    { "mcj?", true, Cmd_mcj },
    { "scj?", true, Cmd_mcj },
	
    { "sej?", true,  Cmd_sej },
    { "ssj?", false, Cmd_ssj },
    /* I. Sensor Measurements */
    { "msn?", true,  Cmd_msn },
	{ "ssn?", true,  Cmd_msn },  // snn compatible command for battery check

    { "spn?", false, Cmd_spn },
    { "sso?", false, Cmd_sso },
    { "ssp?", true,  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 },

};

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

/* ---- Command dispatcher ---- */
static int dr_cmd_dispatch(const ParsedCmd *cmd)
{
    uint16_t i;
    char tag_lower[5];

    /* tag command convert to lower case */
    for (i = 0; i < 4 && cmd->tag[i]; i++) {
        tag_lower[i] = (cmd->tag[i] >= 'A' && cmd->tag[i] <= 'Z')
                       ? (cmd->tag[i] + 32) : cmd->tag[i];
    }
    tag_lower[i] = '\0';

    for (i = 0; i < g_cmd_count; i++) {
        if (dr_tag_eq(tag_lower, g_cmd_table[i].tag)) {

            if (!g_cmd_table[i].enabled) {
                if (g_plat.log && g_log_enable) {
                    g_plat.log("Command '%s' disabled\n", cmd->tag);
                }
                return 0;
            }

            if (g_plat.log && g_log_enable) {
                g_plat.log("Run handler '%s'\n", cmd->tag);
            }

            return g_cmd_table[i].handler(cmd);
        }
    }

    if (g_plat.log && g_log_enable) {
        g_plat.log("Unknown TAG '%s'\n", cmd->tag);
    }
    return 0;
}

/*
Main Parser called from external code
*/

int dr_cmd_parser(const uint8_t *buf, uint8_t len)
{
    ParsedCmd cmd;

    if (g_plat.log) g_plat.log("[PARSER] in len=%u crc=%u\r\n", len, g_plat.crc_check);

    if (!dr_parse_cmd(buf, len, &cmd)) {
        if (g_plat.log) g_plat.log("[PARSER] PARSE FAIL\r\n");

        /* CRC 실패 시 에러 응답 전송 */
        if (g_plat.crc_check && g_plat.tx_bin) {
            single_format_data(ble_bin_buffer, "crc!", 65530);
            binary_tx_handler(ble_bin_buffer, 3);
        }
        return -1;  /* CRC 실패 또는 파싱 실패 → 음수로 old parser에 위임 */
    }

    if (g_plat.log) g_plat.log("[PARSER] tag=%s\r\n", cmd.tag);
    return dr_cmd_dispatch(&cmd);
		
		
		
		
}

/* ---- Each Command Handler implementation (Stub) ---- */
/*  In actual firmware, replace TODO sections with hardware/EEPROM integration  */

/* A. Device Status */
static int Cmd_mta(const ParsedCmd *cmd)
{
    uint16_t mode = 0;
    
    // Reset count (sta? replacement )
    resetCount = 0;
    
    // Extract mode
    (void)dr_get_u16(cmd, 0, &mode);
    
    if (g_plat.log && g_log_enable) {
        g_plat.log("[Cmd_mta] mode=%u\r\n", mode);
    }
    
    // Process mode ( mta?1 )
    if (mode == 1) {
        if (device_activated() == 0) {
            device_status = true;
        }
    }
    else if (mode == 0) {                      //  mta?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;
}


/* ---- Each Command Handler implementation (Stub) ---- */
/*  In actual firmware, replace TODO sections with hardware/EEPROM integration  */

/* A. Device Status */
static int Cmd_sta(const ParsedCmd *cmd)
{
    uint16_t mode = 0;
    
    // Reset count (sta? replacement )
    resetCount = 0;
    
    // Extract mode
    (void)dr_get_u16(cmd, 0, &mode);
    
    if (g_plat.log && g_log_enable) {
        g_plat.log("[Cmd_mta] mode=%u\r\n", mode);
    }
    
    // Process mode ( mta?1 )
    if (mode == 1) {
        if (device_activated() == 0) {
            device_status = true;
        }
    }
    else if (mode == 0) {                      //  mta?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;
    /* TODO: read actual device_status */
    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;
}


/**
 * @brief PD-ADC M48 Full Measurement - MODE 2 (scj? ? mcj?)
 * 
 * Original: scj?
 * New:      mcj?
 * Response: rcj: (from m48 measurement callback)
 * 
 * MODE 2: Pressure + M48 Full Measurement
 *   - Pressure sensor measurement (pressure1 + pressure2)
 *   - 48 LED-PD ADC measurement
 *   - Battery, Temperature, IMU data
 * 
 * Preconditions:
 *   - Device must be activated (device_status == true)
 *   - Not currently processing
 */
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");
    }
    
    /* Check device activation status */
    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;
    }
    
    info4 = true;
    ble_got_new_data = false;
    processing = true;

    /* Start pressure measurement */
    pressure_all_level_meas();

    battery_timer_stop();
    
    /* Enable battery, temperature, IMU measurement */
    go_batt = true;
    motion_data_once = true;
    
    /* Start main timer */
    main_timer_start();
    
    if (g_plat.log && g_log_enable) {
        g_plat.log("[Cmd_mcj] Measurement started\r\n");
    }
    
    return 1;
}

static int Cmd_sej(const ParsedCmd *cmd)
{
    (void)cmd;

    ADC_PD_MODE = 4;
    info4 = true;

    ble_got_new_data = false;
    processing = true;

    AGC_GAIN_SW(false);
    m48_samples_in_buffer = m_pd_adc_cnt;
    pd_adc_m48_start = true;
    battery_timer_stop();
    go_batt = true;
    main_timer_start();

    if (g_plat.log && g_log_enable) {
        g_plat.log("[Cmd_sej] MODE=4 (M48 + batt + IMU) started\r\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;
}

/* 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");
    }
		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)
{
    hw_i2c_init_once();

    motion_raw_data_enabled = true;
    ble_got_new_data = false;

    /* 'c' = continuous, otherwise single shot */
    if (cmd->data_len > 0 && (char)cmd->data[0] == 'c') {
        motion_data_once = false;
    } else {
        motion_data_once = true;
    }

    main_timer_start();

    if (g_plat.log && g_log_enable) {
        g_plat.log("[Cmd_ssp] Motion sensor raw, once=%u\r\n", motion_data_once);
    }
    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;
}

static int Cmd_mpa(const ParsedCmd *cmd)
{
    (void)cmd;

    if (g_plat.log && g_log_enable) {
        g_plat.log("[Cmd_mpa] Piezo Activation\n");
    }

		dr_piezo_power_on();
		dr_piezo_system_init();
		
	  if (g_plat.tx_bin) {
        single_format_data(ble_bin_buffer, "rpa:", 1);
        binary_tx_handler(ble_bin_buffer, 3);
    }  
		
    return 1;
}


/**
 * @brief Piezo burst command with frequency option
 * @param cmd->data[0-1]: cycles (3~9), default=5
 * @param cmd->data[2-3]: freq_option (0=1.8MHz, 1=2.1MHz, 2=2.0MHz, 3=1.7MHz), default=1
 * @return Response: rpc:cycles
 *
 * Usage from Harbour:
 *   mpc?[cycles][freq]  where freq: 0=1.8MHz, 1=2.1MHz (default), 2=2.0MHz, 3=1.7MHz, 4=2.2MHz
 *   Example: mpc?0x05,0x00,0x01,0x00 -> 5 cycles at 2.1MHz
 *   Example: mpc?0x05,0x00,0x00,0x00 -> 5 cycles at 1.8MHz
 *   Example: mpc?0x05,0x00,0x02,0x00 -> 5 cycles at 2.0MHz
 *   Example: mpc?0x05,0x00,0x03,0x00 -> 5 cycles at 1.7MHz
 *   Example: mpc?0x05,0x00,0x04,0x00 -> 5 cycles at 2.2MHz
 */
static int Cmd_mpc(const ParsedCmd *cmd)
{
    uint16_t cycles = 5;      /* default */
    uint16_t freq_option = 1; /* 0=1.8MHz, 1=2.1MHz, 2=2.0MHz, 3=1.7MHz, 4=2.2MHz */
    uint16_t piezo_ch = 0;    /* Piezo channel 0~7, default 0 */

    /* Extract cycles (word 0) */
    (void)dr_get_u16(cmd, 0, &cycles);

    /* Extract frequency option (word 1) */
    (void)dr_get_u16(cmd, 1, &freq_option);

    /* Extract piezo channel (word 2) */
    (void)dr_get_u16(cmd, 2, &piezo_ch);

    /* Validate piezo channel: 0~7     jhChun 26.01.29 */
    if (piezo_ch > 7) piezo_ch = 0;

    if (g_plat.log && g_log_enable) {
        const char *freq_str = (freq_option == 0) ? "1.8MHz" :
                               (freq_option == 1) ? "2.1MHz" :
                               (freq_option == 2) ? "2.0MHz" :
                               (freq_option == 3) ? "1.7MHz" :
                               (freq_option == 4) ? "2.2MHz" :
                               (freq_option == 9) ? "1.9MHz" : "unknown";
        g_plat.log("[Cmd_mpc] cycles=%u, freq=%u (%s), piezo=%u\r\n",
                   cycles, freq_option, freq_str, piezo_ch);
    }

    /* Range check: 3~9 */
    if (cycles < 3 || cycles > 9) {
        dr_ble_return_1("rpc:", 2);  /* Error: out of range */
        return 1;
    }

    /* Select piezo channel */
    dr_piezo_select_channel((uint8_t)piezo_ch);

    /* Execute burst based on frequency option */
    switch (freq_option) {
        case 0:
            dr_piezo_burst_sw_18mhz((uint8_t)cycles);
            break;
        case 2:
            dr_piezo_burst_sw_20mhz((uint8_t)cycles);
            break;
        case 3:
            dr_piezo_burst_sw_17mhz((uint8_t)cycles);
            break;
        case 4:
            dr_piezo_burst_sw_22mhz((uint8_t)cycles);
            break;
        case 9:
            dr_piezo_burst_sw_19mhz((uint8_t)cycles);
            break;
        case 1:
        default:
            dr_piezo_burst_sw((uint8_t)cycles);  /* 2.1MHz */
            break;
    }

    /* Response */
    dr_ble_return_1("rpc:", (uint8_t)cycles);

    return 1;
}


/**
 * @brief Piezo burst + Echo capture command
 * @param cmd->data[0]: cycles (3~9), default=5
 * @param cmd->data[1-2]: delay_us (0~65535), default=1000
 * @param cmd->data[3-4]: num_samples (1~8192), default=4096
 * @return Multiple packets with 12-bit packed data
 *
 * Response format (multi-packet, 12-bit packed):
 *
 * First packet ("rdc:"):
 *   [0-3]:   "rdc:"
 *   [4-5]:   peak_raw (uint16_t, little-endian)
 *   [6-7]:   peak_index (uint16_t)
 *   [8-9]:   baseline_raw (uint16_t)
 *   [10-11]: num_samples (uint16_t)
 *   [12-13]: total_packets (uint16_t)
 *   [14...]: 12-bit packed ADC data (first chunk)
 *
 * Continuation packets ("rdd:"):
 *   [0-3]:   "rdd:"
 *   [4-5]:   packet_index (uint16_t, 1-based)
 *   [6...]:  12-bit packed ADC data (continuation)
 *
 * Last packet marker ("rde:"):
 *   [0-3]:   "rde:"
 *   [4-5]:   total_bytes_sent (uint16_t)
 *
 * 12-bit packing: 2 samples (24 bits) -> 3 bytes
 *   Byte0 = sample0[7:0]
 *   Byte1 = sample1[3:0] << 4 | sample0[11:8]
 *   Byte2 = sample1[11:4]
 */
#define MDC_BLE_MTU_SIZE        240     /* Safe BLE packet size */
#define MDC_FIRST_HEADER_LEN    14      /* "rdc:" + header */
#define MDC_CONT_HEADER_LEN     6       /* "rdd:" + packet_index */
#define MDC_FIRST_DATA_LEN      (MDC_BLE_MTU_SIZE - MDC_FIRST_HEADER_LEN)
#define MDC_CONT_DATA_LEN       (MDC_BLE_MTU_SIZE - MDC_CONT_HEADER_LEN)

/**
 * @brief Piezo burst + Echo capture command (12-bit packed)
 *
 * PROTOCOL v3: Small header packet + separate data packets with delays
 * This avoids large packet loss in BLE stack.
 *
 * Response format:
 *   Packet 1 (rdb:): header only (14 bytes)
 *   Packet 2~N (rdd:): pkt_idx(2) + packed_data (up to 234 bytes)
 *   Final (rde:): total_packets(2)
 */
static int Cmd_mdc(const ParsedCmd *cmd)
{
    uint16_t cycles = 5;
    uint16_t delay_us = 1000;
    uint16_t num_samples = 4096;

    /* Extract parameters */
    (void)dr_get_u16(cmd, 0, &cycles);
    (void)dr_get_u16(cmd, 1, &delay_us);
    (void)dr_get_u16(cmd, 2, &num_samples);

    if (g_plat.log && g_log_enable) {
        g_plat.log("[Cmd_mdc] cycles=%u, delay=%uus, samples=%u\r\n",
                   cycles, delay_us, num_samples);
    }

    /* Range check */
    if (cycles < 3) cycles = 3;
    if (cycles > 9) cycles = 9;
    if (num_samples > 8192) num_samples = 8192;
    if (num_samples < 1) num_samples = 1;

    /* Execute burst + echo capture */
    dr_adc_echo_t echo;
    dr_adc_err_t err = dr_adc_burst_and_capture(
        (uint8_t)cycles, delay_us, num_samples, &echo);

    if (g_plat.log && g_log_enable) {
        g_plat.log("[Cmd_mdc] err=%d, peak=%u@%u, base=%u, n=%u\r\n",
                   err, echo.peak_raw, echo.peak_index, echo.baseline_raw, echo.num_samples);
    }

    if (g_plat.tx_bin) {
        const uint16_t *raw_buffer = dr_adc_get_echo_buffer();

        /* Calculate packed data size: 2 samples -> 3 bytes */
        uint16_t packed_data_bytes = ((echo.num_samples + 1) / 2) * 3;
        uint16_t data_per_packet = MDC_BLE_MTU_SIZE - MDC_CONT_HEADER_LEN;  /* 234 bytes */
        uint16_t data_packets = (packed_data_bytes + data_per_packet - 1) / data_per_packet;
        if (data_packets == 0) data_packets = 1;
        uint16_t total_packets = 1 + data_packets;  /* header + data packets */

        if (g_plat.log && g_log_enable) {
            g_plat.log("[Cmd_mdc] packed=%u bytes, packets=%u\r\n", packed_data_bytes, total_packets);
        }

        /* Packet 1: rdb: header only (14 bytes) */
        ble_bin_buffer[0] = 'r';
        ble_bin_buffer[1] = 'd';
        ble_bin_buffer[2] = 'b';
        ble_bin_buffer[3] = ':';
        ble_bin_buffer[4] = (uint8_t)(total_packets & 0xFF);
        ble_bin_buffer[5] = (uint8_t)(total_packets >> 8);
        ble_bin_buffer[6] = (uint8_t)(echo.peak_raw & 0xFF);
        ble_bin_buffer[7] = (uint8_t)(echo.peak_raw >> 8);
        ble_bin_buffer[8] = (uint8_t)(echo.peak_index & 0xFF);
        ble_bin_buffer[9] = (uint8_t)(echo.peak_index >> 8);
        ble_bin_buffer[10] = (uint8_t)(echo.baseline_raw & 0xFF);
        ble_bin_buffer[11] = (uint8_t)(echo.baseline_raw >> 8);
        ble_bin_buffer[12] = (uint8_t)(echo.num_samples & 0xFF);
        ble_bin_buffer[13] = (uint8_t)(echo.num_samples >> 8);
        binary_tx_handler(ble_bin_buffer, 7);  /* 14 bytes = 7 words */
        nrf_delay_ms(100);  /* Wait for BLE stack */

        /* Packet 2~N: rdd: data packets with packed 12-bit samples */
        uint16_t src_idx = 0;
        for (uint16_t pkt = 0; pkt < data_packets; pkt++) {
            ble_bin_buffer[0] = 'r';
            ble_bin_buffer[1] = 'd';
            ble_bin_buffer[2] = 'd';
            ble_bin_buffer[3] = ':';
            ble_bin_buffer[4] = (uint8_t)(pkt & 0xFF);
            ble_bin_buffer[5] = (uint8_t)(pkt >> 8);

            uint16_t dst_idx = 6;
            uint16_t bytes_this_pkt = 0;
            while (src_idx < echo.num_samples && bytes_this_pkt < data_per_packet) {
                uint16_t s0 = raw_buffer[src_idx++] & 0x0FFF;
                uint16_t s1 = (src_idx < echo.num_samples) ? (raw_buffer[src_idx++] & 0x0FFF) : 0;

                ble_bin_buffer[dst_idx++] = (uint8_t)(s0 & 0xFF);
                ble_bin_buffer[dst_idx++] = (uint8_t)(((s1 & 0x0F) << 4) | ((s0 >> 8) & 0x0F));
                ble_bin_buffer[dst_idx++] = (uint8_t)(s1 >> 4);
                bytes_this_pkt += 3;
            }

            /* Ensure even byte count for word alignment */
            if (dst_idx & 1) ble_bin_buffer[dst_idx++] = 0;
            binary_tx_handler(ble_bin_buffer, dst_idx / 2);  /* bytes to words */
            nrf_delay_ms(100);  /* Inter-packet delay */
        }

        /* Final packet: rde: end marker */
        ble_bin_buffer[0] = 'r';
        ble_bin_buffer[1] = 'd';
        ble_bin_buffer[2] = 'e';
        ble_bin_buffer[3] = ':';
        ble_bin_buffer[4] = (uint8_t)(total_packets & 0xFF);
        ble_bin_buffer[5] = (uint8_t)(total_packets >> 8);
        binary_tx_handler(ble_bin_buffer, 3);  /* 6 bytes = 3 words */

        if (g_plat.log && g_log_enable) {
            g_plat.log("[Cmd_mdc] sent rdb+rdd*%u+rde (%u samples)\r\n",
                       total_packets, echo.num_samples);
        }
    }

    return 1;
}


/*

// The following code demonstrates an example of how to receive data from a client 
// and echo the same data back to the client.



static int Cmd_cmd(const ParsedCmd *cmd)
{ 
    uint16_t v1, v2, v3;
    
    if (cmd->data_len < 6) {
        dr_ble_return_1("rmd:", 0);
        return 1;
    }
    
    // Little Endian ?? (PC? LE? ??)
    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);
    
    dr_ble_return_3("rmd:", v1, v2, v3);
    
    return 1;
}

*/

/**
 * @brief Piezo burst + Echo capture with 16-bit raw data (no compression)
 * @param cmd->data[0]: cycles (3~9), default=5
 * @param cmd->data[1-2]: delay_us (0~65535), default=100
 * @param cmd->data[3-4]: num_samples (1~200), default=140 (20cm target)
 * @return Multiple packets with 16-bit raw data
 *
 * Response format (multi-packet, 16-bit raw - no compression):
 *
 * First packet ("rec:"):
 *   [0-3]:   "rec:"
 *   [4-5]:   peak_raw (uint16_t, little-endian)
 *   [6-7]:   peak_index (uint16_t)
 *   [8-9]:   baseline_raw (uint16_t)
 *   [10-11]: num_samples (uint16_t)
 *   [12-13]: total_packets (uint16_t)
 *   [14...]: 16-bit raw ADC data (first chunk, little-endian)
 *
 * Continuation packets ("red:"):
 *   [0-3]:   "red:"
 *   [4-5]:   packet_index (uint16_t, 1-based)
 *   [6...]:  16-bit raw ADC data (continuation)
 *
 * Last packet marker ("ree:"):
 *   [0-3]:   "ree:"
 *   [4-5]:   total_bytes_sent (uint16_t)
 *
 * 16-bit raw format: Each sample = 2 bytes (uint16, little-endian)
 *   Byte0 = Sample[7:0]
 *   Byte1 = Sample[15:8]
 *
 * Example: 140 samples (20cm) = 280 bytes = 2 packets
 */
static int Cmd_mec(const ParsedCmd *cmd)
{
    uint16_t freq_option = 0;    /* 0=1.8MHz (default), 1=2.1MHz, 2=2.0MHz, 3=1.7MHz */
    uint16_t delay_us = 20;      /* Default 20us */
    uint16_t num_samples = 140;  /* Default for 20cm target */
    uint16_t cycles = 5;         /* Default 5 cycles (valid: 3~7) */
    uint16_t averaging = 1;      /* Default 1 (no averaging), max 1000 */
    uint16_t piezo_ch = 0;       /* Default piezo channel 0 (valid: 0~7) */

    /* Extract parameters: mec [freq_option] [delay_us] [num_samples] [cycles] [averaging] [piezo_ch] */
    (void)dr_get_u16(cmd, 0, &freq_option);
    (void)dr_get_u16(cmd, 1, &delay_us);
    (void)dr_get_u16(cmd, 2, &num_samples);
    (void)dr_get_u16(cmd, 3, &cycles);
    (void)dr_get_u16(cmd, 4, &averaging);
    (void)dr_get_u16(cmd, 5, &piezo_ch);

    /* Validate averaging: 1~1000, default 1 */
    if (averaging == 0) averaging = 1;
    if (averaging > 1000) averaging = 1000;

    /* Validate piezo channel: 0~7     jhChun 26.01.29 */
    if (piezo_ch > 7) piezo_ch = 0;

    if (g_plat.log && g_log_enable) {
        const char *freq_str = (freq_option == 0) ? "1.8MHz" :
                               (freq_option == 1) ? "2.1MHz" :
                               (freq_option == 2) ? "2.0MHz" :
                               (freq_option == 3) ? "1.7MHz" :
                               (freq_option == 4) ? "2.2MHz" :
                               (freq_option == 9) ? "1.9MHz" : "unknown";
        g_plat.log("[Cmd_mec] freq=%u (%s), delay=%uus, samples=%u, cycles=%u, avg=%u, piezo=%u\r\n",
                   freq_option, freq_str, delay_us, num_samples, cycles, averaging, piezo_ch);
    }

    /* Use integrated burst + capture + transmit function
     * This function handles:
     * 1. ADC power on
     * 2. Select piezo channel (0~7)
     * 3. Piezo burst (frequency based on freq_option, cycles from param)
     * 4. ADC capture (after delay_us) - repeated 'averaging' times
     * 5. Average the captured samples
     * 6. BLE transmission with proper packet timing
     */
    dr_adc_err_t err = dr_adc_burst_capture_transmit(
        (uint8_t)freq_option, delay_us, num_samples, (uint8_t)cycles,
        (uint16_t)averaging, (uint8_t)piezo_ch, ble_bin_buffer, 0);  /* 0=send raa */

    if (err != DR_ADC_OK) {
        dr_ble_return_2("rer:", 0xEE00 | (uint16_t)err, num_samples);
    }

    if (g_plat.log && g_log_enable) {
        g_plat.log("[Cmd_mec] result=%d\r\n", err);
    }

    return 1;
}


static int Cmd_cmd(const ParsedCmd *cmd)
{
    uint16_t v1, v2, v3;
    uint32_t pin_number;
    
    if (cmd->data_len < 6) {
        dr_ble_return_1("rmd:", 0);
        return 1;
    }
    
    // Little Endian from PC to LE
    v1 = (uint16_t)cmd->data[0] | ((uint16_t)cmd->data[1] << 8);  // port
    v2 = (uint16_t)cmd->data[2] | ((uint16_t)cmd->data[3] << 8);  // pin
    v3 = (uint16_t)cmd->data[4] | ((uint16_t)cmd->data[5] << 8);  // 1=HIGH, 0=LOW
    
    // -- GPIO Test
    
    // Pin No: NRF_GPIO_PIN_MAP(port, pin)
    pin_number = NRF_GPIO_PIN_MAP(v1, v2);
    
    // output
    nrf_gpio_cfg_output(pin_number);
    
    // HIGH or LOW 
    if (v3 == 1) {
        nrf_gpio_pin_set(pin_number);      // HIGH
    } else {
        nrf_gpio_pin_clear(pin_number);    // LOW
    }
    
    // return : port, pin, state
    dr_ble_return_3("rmd:", v1, v2, v3);

    return 1;
}


/**
 * @brief 4-Channel All Capture Command (maa?) - ASYNC VERSION
 * @param cmd->data[0-1]: mode (0=raw 16-bit async)
 * @return Immediately. Data sent asynchronously via BLE_NUS_EVT_TX_RDY
 *
 * ASYNC ARCHITECTURE:
 *   - maa_async_start() initiates capture and sends CH0 header
 *   - BLE_NUS_EVT_TX_RDY callback drives remaining transmission
 *   - No blocking - SoftDevice can process events normally
 *   - Prevents BLE TX buffer overflow that caused firmware brick
 *
 * Hardcoded parameters:
 *   freq_option = 0 (1.8MHz)
 *   delay_us = 10
 *   num_samples = 140
 *   cycles = 7
 *   averaging = 5
 *
 * Response format:
 *   For each channel (CH0~CH3):
 *     reb: [total_pkts(2)] [peak(2)] [idx(2)] [baseline(2)] [samples(2)]
 *     red: [pkt_idx(2)] [data...]
 *   Final:
 *     raa: [status(2)]
 *
 * Version marker: 0xA000 (vA) = async 8-channel
 */
#define MAA_FREQ_OPTION      0      /* 1.8MHz */
#define MAA_DELAY_US         10     /* 10us post-burst delay */
#define MAA_NUM_SAMPLES      140    /* 140 samples (~25cm) */
#define MAA_CYCLES           7      /* 7 cycles burst */
#define MAA_AVERAGING        5      /* 5x averaging */

static int Cmd_maa(const ParsedCmd *cmd)
{
    uint16_t mode = 0;
    dr_adc_err_t err;

    /* Extract mode parameter */
    (void)dr_get_u16(cmd, 0, &mode);

    /* Mode validation - only mode 0 (async raw) supported */
    if (mode > 0) {
        dr_ble_return_1("raa:", 0xFFFF);
        return 1;
    }

    /* Check if already busy */
    if (maa_async_is_busy()) {
        dr_ble_return_1("raa:", 0xFFFE);  /* Busy */
        return 1;
    }

    /*=======================================================================
     * ASYNC 4-CHANNEL CAPTURE
     * - maa_async_start() captures CH0 and sends first header
     * - Subsequent packets sent via BLE_NUS_EVT_TX_RDY callback
     * - No blocking delays - SoftDevice can process events normally
     *=======================================================================*/
    err = maa_async_start(
        (uint8_t)MAA_FREQ_OPTION,
        MAA_DELAY_US,
        MAA_NUM_SAMPLES,
        (uint8_t)MAA_CYCLES,
        (uint16_t)MAA_AVERAGING,
        ble_bin_buffer
    );

    if (err != DR_ADC_OK) {
        /* Start failed - error already sent by maa_async_start */
        return 1;
    }

    /* Return immediately - async transmission in progress */
    /* raa: will be sent by state machine when complete */
    return 1;
}


/*==============================================================================
 * CONFIG: HW/Serial Number (FDS)
 *============================================================================*/

/* mwh? - Write HW Number to FDS
 * Data: 12 bytes ASCII HW number
 */
static int Cmd_mwh(const ParsedCmd *cmd)
{
    char buf[13];

    if (cmd->data_len < 12) {
        dr_ble_return_1("rwh:", 0xFFFF);  /* Error: insufficient data */
        return 1;
    }

    dr_get_ascii(cmd, 0, buf, 12);
    memcpy(HW_NO, buf, 12);
    memcpy(m_config.hw_no, buf, 12);
    config_save();

    if (g_plat.log && g_log_enable) {
        g_plat.log("[Cmd_mwh] HW=%.12s saved to FDS\r\n", m_config.hw_no);
    }

    ascii_format_data(ble_bin_buffer, "rwh:", buf, 12);
    binary_tx_handler(ble_bin_buffer, 8);
    return 1;
}

/* mws? - Write Serial Number to FDS
 * Data: 12 bytes ASCII serial number
 */
static int Cmd_mws(const ParsedCmd *cmd)
{
    char buf[13];

    if (cmd->data_len < 12) {
        dr_ble_return_1("rws:", 0xFFFF);  /* Error: insufficient data */
        return 1;
    }

    dr_get_ascii(cmd, 0, buf, 12);
    memcpy(SERIAL_NO, buf, 12);
    memcpy(m_config.serial_no, buf, 12);
    config_save();

    if (g_plat.log && g_log_enable) {
        g_plat.log("[Cmd_mws] S/N=%.12s saved to FDS\r\n", m_config.serial_no);
    }

    ascii_format_data(ble_bin_buffer, "rws:", buf, 12);
    binary_tx_handler(ble_bin_buffer, 8);
    return 1;
}

/* mrh? - Read HW Number from FDS */
static int Cmd_mrh(const ParsedCmd *cmd)
{
    (void)cmd;
    memcpy(HW_NO, m_config.hw_no, 12);
    ascii_format_data(ble_bin_buffer, "rrh:", HW_NO, 12);
    binary_tx_handler(ble_bin_buffer, 8);
    return 1;
}

/* mrs? - Read Serial Number from FDS */
static int Cmd_mrs(const ParsedCmd *cmd)
{
    (void)cmd;
    memcpy(SERIAL_NO, m_config.serial_no, 12);
    ascii_format_data(ble_bin_buffer, "rrs:", SERIAL_NO, 12);
    binary_tx_handler(ble_bin_buffer, 8);
    return 1;
}


/*==============================================================================
 * IMU: 6-axis raw data (single shot)
 *============================================================================*/

/* msp? - Read IMU accel(xyz) + gyro(xyz) raw data, single shot
 * Response: rsp: + 6 x uint16_t (accel_x, accel_y, accel_z, gyro_x, gyro_y, gyro_z)
 */
static int Cmd_msp(const ParsedCmd *cmd)
{
    (void)cmd;

    if (g_plat.log) g_plat.log("[MSP] enter\r\n");

    hw_i2c_init_once();

    /* Direct register read — no timer, no DRDY, no callback */
    int rc = imu_read_direct();

    if (g_plat.log) g_plat.log("[MSP] rc=%d\r\n", rc);
    return 1;
}