/* 
 * 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_util.h"
#include "imu_stub.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_binary_tx_safe(const uint8_t *buffer, uint16_t word_count);
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
extern int  imu_read_cached(void);          // IMU cached memory read + BLE send
extern volatile bool     g_imu_active;      // IMU active streaming flag
extern void imu_active_timer_start(void);   // start 1-sec amu: timer
extern void imu_active_timer_stop(void);    // stop  1-sec amu: timer

/* 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)


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

/* ---- 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_msp(const ParsedCmd *cmd);  /* IMU 6-axis raw data (single shot) */
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 */
static int Cmd_mrc(const ParsedCmd *cmd);  /* Read Measurement Config from FDS */
static int Cmd_mwc(const ParsedCmd *cmd);  /* Write Measurement Config to FDS */
static int Cmd_mas(const ParsedCmd *cmd);  /* IMU Active Streaming Start */
static int Cmd_max(const ParsedCmd *cmd);  /* IMU Active Streaming Stop  */

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

static CmdEntry g_cmd_table[] = {
	
	/* sudo command  */
	{ "cmd?", true,  Cmd_cmd },  // Piezo Activate
	
	{ "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
	{ "mrc?", true,  Cmd_mrc },  // Read Measurement Config (freq_idx, cycles, avg_count)
	{ "mwc?", true,  Cmd_mwc },  // Write Measurement Config (freq_idx, cycles, avg_count)
	{ "mas?", true,  Cmd_mas },  // IMU Active Streaming Start (1-sec amu: timer)
	{ "max?", true,  Cmd_max },  // IMU Active Streaming Stop

    /* 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;
}



/*

// 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;
}

*/



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;
}


/*==============================================================================
 * 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;
}


/*==============================================================================
 * Measurement Config: Read/Write (FDS)
 *
 * mrc? - Read measurement config
 *   Response: rrc: freq_idx(uint16), cycles(uint16), avg_count(uint16)
 *     freq_idx : Piezo frequency index
 *                0=2.1MHz, 1=1.8MHz, 2=2.0MHz, 3=1.7MHz, 4=2.2MHz, 5=1.9MHz
 *     cycles   : Piezo burst cycles (3~7: 3,4,5,6,7)
 *     avg_count: mec measurement averaging count (1~20)
 *
 * mwc? - Write measurement config
 *   Data: 6 bytes (3 x uint16_t LE)
 *     data[0-1]: freq_idx  (0~5, see above)
 *     data[2-3]: cycles    (3~10)
 *     data[4-5]: avg_count (1~20)
 *   Response: rwc: freq_idx(uint16), cycles(uint16), avg_count(uint16)
 *============================================================================*/

/* mrc? - Read Measurement Config from FDS */
static int Cmd_mrc(const ParsedCmd *cmd)
{
    (void)cmd;

    /* Return current measurement config: freq_idx, cycles, avg_count */
    dr_ble_return_3("rrc:",
                    (uint16_t)meas_config_get_freq_idx(),
                    (uint16_t)meas_config_get_cycles(),
                    (uint16_t)meas_config_get_avg_count());

    if (g_plat.log && g_log_enable) {
        g_plat.log("[Cmd_mrc] freq=%u cycles=%u avg=%u\r\n",
                   meas_config_get_freq_idx(),
                   meas_config_get_cycles(),
                   meas_config_get_avg_count());
    }
    return 1;
}

/* mwc? - Write Measurement Config to FDS
 * Data: 6 bytes = 3 x uint16_t (LE)
 *   [0-1] freq_idx  : 0=2.1MHz, 1=1.8MHz, 2=2.0MHz, 3=1.7MHz, 4=2.2MHz, 5=1.9MHz
 *   [2-3] cycles    : Piezo burst cycles (3~7: 3,4,5,6,7)
 *   [4-5] avg_count : mec averaging count (1~20)
 */
static int Cmd_mwc(const ParsedCmd *cmd)
{
    uint16_t freq_idx, cycles, avg_count;

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

    /* Parse Little Endian uint16_t */
    freq_idx  = (uint16_t)cmd->data[0] | ((uint16_t)cmd->data[1] << 8);
    cycles    = (uint16_t)cmd->data[2] | ((uint16_t)cmd->data[3] << 8);
    avg_count = (uint16_t)cmd->data[4] | ((uint16_t)cmd->data[5] << 8);

    /* Validate and set */
    meas_config_set_freq_idx((uint8_t)freq_idx);
    meas_config_set_cycles((uint8_t)cycles);
    meas_config_set_avg_count((uint8_t)avg_count);

    /* Save to FDS */
    meas_config_save();

    if (g_plat.log && g_log_enable) {
        g_plat.log("[Cmd_mwc] freq=%u cycles=%u avg=%u saved\r\n",
                   meas_config_get_freq_idx(),
                   meas_config_get_cycles(),
                   meas_config_get_avg_count());
    }

    /* Response: confirmed values (after validation) */
    dr_ble_return_3("rwc:",
                    (uint16_t)meas_config_get_freq_idx(),
                    (uint16_t)meas_config_get_cycles(),
                    (uint16_t)meas_config_get_avg_count());
    return 1;
}


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

/*==============================================================================
 * msp? — Read IMU accel(xyz) + gyro(xyz) raw data
 *
 * Behavior depends on active streaming state (g_imu_active):
 *   - g_imu_active == true  : read from cached memory (g_imu_latest[])
 *                              No I2C access. Fast. No conflict with amu: timer.
 *   - g_imu_active == false : direct I2C register read (original behavior)
 *                              Initializes I2C, reads sensor, responds.
 *
 * Response tag : "rsp:" + 6 x uint16_t BE (accel_x/y/z, gyro_x/y/z) + CRC16
 * Packet size  : 18 bytes
 *
 * Author  : Charles Kwon (Medithings)
 * Date    : 2026-03-17
 *
 * Usage   : Client sends "msp?" via BLE NUS
 *           Firmware responds with "rsp:" packet (same format regardless of source)
 *============================================================================*/
static int Cmd_msp(const ParsedCmd *cmd)
{
    (void)cmd;

    if (g_imu_active) {
        /* Active timer running — read from cached memory */
        if (g_plat.log) g_plat.log("[MSP] cached read (active mode)\r\n");
        int rc = imu_read_cached();
        if (rc < 0) {
            if (g_plat.log) g_plat.log("[MSP] no cache yet, fallback to direct\r\n");
            hw_i2c_init_once();
            imu_read_direct();
        }
    } else {
        /* No active timer — direct I2C read */
        if (g_plat.log) g_plat.log("[MSP] direct read\r\n");
        hw_i2c_init_once();
        imu_read_direct();
    }

    return 1;
}


/*==============================================================================
 * mas? — Measure Active Start
 *
 * Starts the 1-second periodic IMU active streaming timer.
 * After this command, firmware sends "amu:" packets every 1 second via BLE NUS
 * Notification containing accel(xyz) + gyro(xyz) raw data.
 *
 * Sets g_imu_active = true so that:
 *   - imu_active_timer_handler() fires every 1 sec
 *   - msp? reads from cached memory instead of direct I2C
 *
 * Request  : "mas?" (4 bytes + CRC16)
 * Response : "ras:" (4 bytes + CRC16) — confirmation that streaming started
 * Then     : "amu:" packets every 1 second (until max? is received)
 *
 * Author   : Charles Kwon (Medithings)
 * Date     : 2026-03-17
 *
 * Usage    :
 *   1. Client connects via BLE
 *   2. Client sends "mas?" + CRC16
 *   3. Firmware responds "ras:" + CRC16
 *   4. Firmware begins sending "amu:" every 1 sec
 *   5. Client sends "max?" + CRC16 to stop
 *============================================================================*/
static int Cmd_mas(const ParsedCmd *cmd)
{
    (void)cmd;
    uint16_t status;

    if (g_imu_active) {
        if (g_plat.log) g_plat.log("[MAS] already active\r\n");
        status = 0x0002;  /* already running */
    } else {
        g_imu_active = true;
        imu_active_timer_start();
        if (g_plat.log) g_plat.log("[MAS] streaming started\r\n");
        status = 0x0001;  /* newly started */
    }

    /* Response: "ras:" + status (0x0001=started, 0x0002=already active) */
    dr_ble_return_1("ras:", status);
    return 1;
}


/*==============================================================================
 * max? — Measure Active eXit (stop)
 *
 * Stops the 1-second periodic IMU active streaming timer.
 * After this command, firmware stops sending "amu:" packets.
 * IMU is powered off and I2C is released for power saving.
 *
 * Sets g_imu_active = false so that:
 *   - Timer stops firing
 *   - msp? reverts to direct I2C read
 *
 * Request  : "max?" (4 bytes + CRC16)
 * Response : "rax:" (4 bytes + CRC16) — confirmation that streaming stopped
 *
 * Author   : Charles Kwon (Medithings)
 * Date     : 2026-03-17
 *
 * Usage    :
 *   1. Client sends "max?" + CRC16 while streaming is active
 *   2. Firmware stops amu: timer
 *   3. Firmware responds "rax:" + CRC16
 *   4. No more "amu:" packets sent
 *   5. msp? now does direct I2C read again
 *============================================================================*/
static int Cmd_max(const ParsedCmd *cmd)
{
    (void)cmd;
    uint16_t status;

    if (!g_imu_active) {
        if (g_plat.log) g_plat.log("[MAX] already stopped\r\n");
        status = 0x0002;  /* already stopped */
    } else {
        g_imu_active = false;
        imu_active_timer_stop();
        if (g_plat.log) g_plat.log("[MAX] streaming stopped\r\n");
        status = 0x0001;  /* newly stopped */
    }

    /* Response: "rax:" + status (0x0001=stopped, 0x0002=already stopped) */
    dr_ble_return_1("rax:", status);
    return 1;
}
