/*******************************************************************************
 * @file    parser.c
 * @brief   BLE command parser (Zephyr port)
 *
 * 원본: pc_firm/parser.c
 * 패킷 포맷: [TAG 4B] [DATA NB] [CRC16 2B]
 *
 * 현재 구현된 커맨드: msn? (배터리 측정), mls? (LED 상태 설정)
 ******************************************************************************/
#include <zephyr/kernel.h>
#include <zephyr/sys/util.h>
#include <string.h>
#include <limits.h>

#include "parser.h"
#include "app_nvs.h"
#include "main.h"
#include "debug_print.h"
#include "ble_service.h"
#include "battery_adc.h"
#include "led_control.h"
#include "echo_adc.h"
#include "imu_i2c.h"
#include "piezo.h"
#include "tmp235.h"

/*==============================================================================
 * Piezo / echo measurement constants
 *============================================================================*/
#define PIEZO_AVERAGE_COUNT   10
#define ECHO_STATUS_OK        0x0000
#define ECHO_STATUS_PIEZO     0x0001
#define ECHO_STATUS_ADC_INIT  0x0002
#define ECHO_STATUS_MUX       0x0003
#define ECHO_STATUS_CAPTURE   0x0004
#define ECHO_STATUS_BATT      0x0005
#define ECHO_STATUS_IMU       0x0006
#define ECHO_STATUS_TEMP      0x0007

#define PIEZO_CFG_FREQ_DEFAULT     1
#define PIEZO_CFG_CYCLES_DEFAULT   7
#define PIEZO_CFG_DELAY_DEFAULT    10
#define PIEZO_CFG_SAMPLES_DEFAULT  100
#define PIEZO_CFG_AVG_DEFAULT      10
#define PIEZO_CFG_AVG_MAX          20
#define PIEZO_CFG_DELAY_MAX_US     100
#define PIEZO_POST_SELECT_SETTLE_US  500
#define PIEZO_AVG_INTER_BURST_GAP_US 500
#define PIEZO_DUMMY_CAPTURE_COUNT    5

static uint16_t piezo_channels[PIEZO_NUM_CHANNELS][ECHO_ADC_MAX_SAMPLES];
static uint16_t echo_capture[ECHO_ADC_MAX_SAMPLES];
static uint32_t echo_accum[ECHO_ADC_MAX_SAMPLES];
static uint8_t tx_u16_buf[8];
static uint8_t tx_imu_buf[18];
static uint8_t tx_echo_buf[4 + 2 + 2 + (ECHO_ADC_MAX_SAMPLES * 2) + 2];
static uint8_t tx_bundle_buf[22];
static uint8_t tx_cfg_buf[16];
static uint8_t tx_ascii_buf[4 + HW_NO_LENGTH + 2];
static uint8_t tx_rim_samples[IMU_FIFO_RIM_TARGET_SAMPLES * IMU_FIFO_SAMPLE_BYTES];
static uint8_t tx_rim_buf[4 + 2 + (IMU_FIFO_RIM_TARGET_SAMPLES * IMU_FIFO_SAMPLE_BYTES) + 2];
static uint8_t g_echo_session;
static piezo_config_t g_piezo_config = {
    .freq = PIEZO_CFG_FREQ_DEFAULT,
    .cycles = PIEZO_CFG_CYCLES_DEFAULT,
    .avg = PIEZO_CFG_AVG_DEFAULT,
    .delay_us = PIEZO_CFG_DELAY_DEFAULT,
    .samples = PIEZO_CFG_SAMPLES_DEFAULT,
};

static bool piezo_config_validate(const piezo_config_t *cfg)
{
    if (cfg == NULL)
    {
        return false;
    }

    if (cfg->freq != PIEZO_CFG_FREQ_DEFAULT)
    {
        return false;
    }

    if ((cfg->cycles < 3U) || (cfg->cycles > 7U))
    {
        return false;
    }

    if ((cfg->avg == 0U) || (cfg->avg > PIEZO_CFG_AVG_MAX))
    {
        return false;
    }

    if (cfg->delay_us > PIEZO_CFG_DELAY_MAX_US)
    {
        return false;
    }

    if ((cfg->samples == 0U) || (cfg->samples > ECHO_ADC_MAX_SAMPLES))
    {
        return false;
    }

    return true;
}

static void piezo_config_log(const char *prefix, const piezo_config_t *cfg)
{
    DBG_PRINTF("%s freq=%u cycles=%u avg=%u delay=%u samples=%u\r\n",
               prefix,
               cfg->freq,
               cfg->cycles,
               cfg->avg,
               cfg->delay_us,
               cfg->samples);
}

const piezo_config_t *piezo_config_get(void)
{
    return &g_piezo_config;
}

int piezo_config_init(void)
{
    int err = app_nvs_init(&g_piezo_config, piezo_config_validate, piezo_config_log);
    if (err)
    {
        return err;
    }

    piezo_config_log("[CFG] piezo active", &g_piezo_config);
    return 0;
}

/*==============================================================================
 * CRC16 (CRC-CCITT, Nordic SDK 호환)
 *============================================================================*/
static uint16_t dr_crc16_compute(const uint8_t *p_data, uint32_t size)
{
    uint16_t crc = 0xFFFF;

    for (uint32_t 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;
}

static bool get_data_u16_be(const uint8_t *data, uint8_t data_len,
                            uint8_t word_index, uint16_t *out)
{
    uint8_t offset = (uint8_t)(word_index * 2U);

    if ((offset + 1U) >= data_len)
    {
        return false;
    }

    *out = ((uint16_t)data[offset] << 8) | (uint16_t)data[offset + 1U];
    return true;
}

static void copy_fixed_ascii(char *dst, size_t dst_len, const char *src, size_t src_len)
{
    if (src_len > dst_len)
    {
        src_len = dst_len;
    }

    memset(dst, 0, dst_len);
    memcpy(dst, src, src_len);
}

static char ascii_to_lower(char ch)
{
    if ((ch >= 'A') && (ch <= 'Z'))
    {
        return (char)(ch - 'A' + 'a');
    }

    return ch;
}

/*==============================================================================
 * 응답 패킷 전송
 *============================================================================*/

/* TAG(4B) + uint16 값(2B) + CRC16(2B) = 8바이트 전송 */
static int send_response_u16(const char *tag, uint16_t value)
{
    uint8_t *buf = tx_u16_buf;

    buf[0] = tag[0];
    buf[1] = tag[1];
    buf[2] = tag[2];
    buf[3] = tag[3];
    buf[4] = (uint8_t)(value >> 8);
    buf[5] = (uint8_t)(value & 0xFF);

    uint16_t crc = dr_crc16_compute(buf, 6);
    buf[6] = (uint8_t)(crc & 0xFF);
    buf[7] = (uint8_t)(crc >> 8);

    return ble_data_send(buf, 8);
}

static int send_response_ascii(const char *tag, const char *value, uint8_t value_len)
{
    uint8_t *buf = tx_ascii_buf;

    buf[0] = tag[0];
    buf[1] = tag[1];
    buf[2] = tag[2];
    buf[3] = tag[3];
    memcpy(&buf[4], value, value_len);

    uint16_t crc = dr_crc16_compute(buf, (uint32_t)(4U + value_len));
    buf[4 + value_len] = (uint8_t)(crc & 0xFF);
    buf[5 + value_len] = (uint8_t)(crc >> 8);

    return ble_data_send(buf, (uint16_t)(6U + value_len));
}

static int send_response_tag_echo(const char *tag, const char *echo_tag)
{
    uint8_t *buf = tx_ascii_buf;

    buf[0] = tag[0];
    buf[1] = tag[1];
    buf[2] = tag[2];
    buf[3] = tag[3];
    buf[4] = echo_tag[0];
    buf[5] = echo_tag[1];
    buf[6] = echo_tag[2];
    buf[7] = echo_tag[3];

    uint16_t crc = dr_crc16_compute(buf, 8);
    buf[8] = (uint8_t)(crc & 0xFF);
    buf[9] = (uint8_t)(crc >> 8);

    return ble_data_send(buf, 10);
}

/*==============================================================================
 * 응답 패킷 전송 (IMU)
 *============================================================================*/

/* TAG(4B) + int16×6 빅엔디안(12B) + CRC16(2B) = 18바이트 전송
 * 기존 format_data() + dr_binary_tx_safe(buf, 8) 방식과 동일한 레이아웃 */
static int send_response_imu(const int16_t accel[3], const int16_t gyro[3])
{
    uint8_t *buf = tx_imu_buf;

    buf[0] = 'r'; buf[1] = 's'; buf[2] = 'p'; buf[3] = ':';

    const int16_t vals[6] ={
        accel[0], accel[1], accel[2],
        gyro[0],  gyro[1],  gyro[2]
    };
    for (int i = 0; i < 6; i++)
    {
        buf[4 + i * 2]     = (uint8_t)((uint16_t)vals[i] >> 8);   /* MSB */
        buf[4 + i * 2 + 1] = (uint8_t)((uint16_t)vals[i] & 0xFF); /* LSB */
    }

    uint16_t crc = dr_crc16_compute(buf, 16);
    buf[16] = (uint8_t)(crc & 0xFF);
    buf[17] = (uint8_t)(crc >> 8);

    return ble_data_send(buf, 18);
}

static void send_response_echo(uint8_t session, uint8_t channel, const uint16_t *samples, uint16_t num_samples)
{
    /*
     * reb: 패킷은 최대 210바이트라서 스택 지역변수로 두면
     * mbb?처럼 반복 호출할 때 워커 스택을 꽤 먹는다.
     * 현재는 한 번에 하나의 명령만 처리하므로 정적 버퍼를 재사용한다.
     */
    uint8_t *buf = tx_echo_buf;

    buf[0] = 'r'; buf[1] = 'e'; buf[2] = 'b'; buf[3] = ':';
    buf[4] = session;
    buf[5] = channel;
    buf[6] = (uint8_t)(num_samples >> 8);
    buf[7] = (uint8_t)(num_samples & 0xFF);

    for (uint16_t i = 0; i < num_samples; i++)
    {
        buf[8 + i * 2] = (uint8_t)(samples[i] >> 8);
        buf[9 + i * 2] = (uint8_t)(samples[i] & 0xFF);
    }

    uint16_t payload_len = 4 + 2 + 2 + (num_samples * 2);
    uint16_t crc = dr_crc16_compute(buf, payload_len);
    buf[payload_len] = (uint8_t)(crc & 0xFF);
    buf[payload_len + 1] = (uint8_t)(crc >> 8);

    ble_data_send(buf, payload_len + 2);
}

static void send_response_bundle(uint16_t batt_mv,
                                 const int16_t accel[3],
                                 const int16_t gyro[3],
                                 int16_t temp_cdeg)
{
    uint8_t *buf = tx_bundle_buf;

    buf[0] = 'r'; buf[1] = 'b'; buf[2] = 'b'; buf[3] = ':';
    buf[4] = (uint8_t)(batt_mv >> 8);
    buf[5] = (uint8_t)(batt_mv & 0xFF);

    const int16_t imu_vals[6] = {
        accel[0], accel[1], accel[2],
        gyro[0],  gyro[1],  gyro[2]
    };

    for (int i = 0; i < 6; i++)
    {
        buf[6 + i * 2] = (uint8_t)((uint16_t)imu_vals[i] >> 8);
        buf[7 + i * 2] = (uint8_t)((uint16_t)imu_vals[i] & 0xFF);
    }

    buf[18] = (uint8_t)((uint16_t)temp_cdeg >> 8);
    buf[19] = (uint8_t)((uint16_t)temp_cdeg & 0xFF);

    uint16_t crc = dr_crc16_compute(buf, 20);
    buf[20] = (uint8_t)(crc & 0xFF);
    buf[21] = (uint8_t)(crc >> 8);

    ble_data_send(buf, sizeof(tx_bundle_buf));
}

static void send_response_rim(const uint8_t *sample_bytes, uint16_t sample_count)
{
    uint8_t *buf = tx_rim_buf;
    uint16_t payload_len;

    if (sample_count > IMU_FIFO_RIM_TARGET_SAMPLES)
    {
        sample_count = IMU_FIFO_RIM_TARGET_SAMPLES;
    }

    buf[0] = 'r'; buf[1] = 'i'; buf[2] = 'm'; buf[3] = ':';
    buf[4] = (uint8_t)(sample_count >> 8);
    buf[5] = (uint8_t)(sample_count & 0xFF);

    memcpy(&buf[6], sample_bytes, (size_t)sample_count * IMU_FIFO_SAMPLE_BYTES);

    payload_len = (uint16_t)(6U + (sample_count * IMU_FIFO_SAMPLE_BYTES));
    uint16_t crc = dr_crc16_compute(buf, payload_len);
    buf[payload_len] = (uint8_t)(crc & 0xFF);
    buf[payload_len + 1U] = (uint8_t)(crc >> 8);

    DBG_PRINTF("[MTB] tx rim samples=%u len=%u\r\n",
               sample_count,
               (uint16_t)(payload_len + 2U));
    ble_data_send(buf, (uint16_t)(payload_len + 2U));
}

static void send_response_piezo_config(const char *tag,
                                       uint16_t freq,
                                       uint16_t cycles,
                                       uint16_t avg,
                                       uint16_t delay_us,
                                       uint16_t samples)
{
    uint8_t *buf = tx_cfg_buf;

    buf[0] = tag[0];
    buf[1] = tag[1];
    buf[2] = tag[2];
    buf[3] = tag[3];
    buf[4] = (uint8_t)(freq >> 8);
    buf[5] = (uint8_t)(freq & 0xFF);
    buf[6] = (uint8_t)(cycles >> 8);
    buf[7] = (uint8_t)(cycles & 0xFF);
    buf[8] = (uint8_t)(avg >> 8);
    buf[9] = (uint8_t)(avg & 0xFF);
    buf[10] = (uint8_t)(delay_us >> 8);
    buf[11] = (uint8_t)(delay_us & 0xFF);
    buf[12] = (uint8_t)(samples >> 8);
    buf[13] = (uint8_t)(samples & 0xFF);

    uint16_t crc = dr_crc16_compute(buf, 14);
    buf[14] = (uint8_t)(crc & 0xFF);
    buf[15] = (uint8_t)(crc >> 8);

    ble_data_send(buf, sizeof(tx_cfg_buf));
}

static int start_piezo_session(void)
{
    DBG_PRINTF("[MBB] piezo session start\r\n");

    int err = piezo_init();
    if (err)
    {
        DBG_PRINTF("[PIEZO] init fail err=%d\r\n", err);
        return ECHO_STATUS_PIEZO;
    }

    piezo_power_on();
    k_msleep(PIEZO_POWER_STABILIZE_MS);

    err = echo_adc_init();
    if (err)
    {
        DBG_PRINTF("[ECHO] init fail err=%d\r\n", err);
        return ECHO_STATUS_ADC_INIT;
    }

    err = echo_adc_wake();
    if (err)
    {
        DBG_PRINTF("[ECHO] wake fail err=%d\r\n", err);
        return ECHO_STATUS_ADC_INIT;
    }

    DBG_PRINTF("[MBB] piezo session ready\r\n");
    return ECHO_STATUS_OK;
}

static int perform_piezo_sweep(void)
{
    const piezo_config_t *cfg = piezo_config_get();
    uint16_t capture_delay_us = cfg->delay_us;

    if (capture_delay_us < PIEZO_BURST_TO_ADC_DELAY_US)
    {
        capture_delay_us = PIEZO_BURST_TO_ADC_DELAY_US;
    }

    for (uint8_t ch = 0; ch < PIEZO_NUM_CHANNELS; ch++)
    {
        DBG_PRINTF("[MBB] sweep ch=%d start\r\n", ch);

        int err = piezo_select_channel(ch);
        if (err)
        {
            DBG_PRINTF("[PIEZO] mux fail ch=%d err=%d\r\n", ch, err);
            return ECHO_STATUS_MUX;
        }

        k_busy_wait(PIEZO_POST_SELECT_SETTLE_US);

        err = echo_adc_wake();
        if (err)
        {
            DBG_PRINTF("[ECHO] dummy read fail ch=%d err=%d\r\n", ch, err);
            return ECHO_STATUS_CAPTURE;
        }

        for (uint8_t dummy = 0; dummy < PIEZO_DUMMY_CAPTURE_COUNT; dummy++)
        {
            k_busy_wait(PIEZO_AVG_INTER_BURST_GAP_US);
            piezo_burst_sw((uint8_t)cfg->cycles);
            k_busy_wait(capture_delay_us);

            err = echo_adc_capture(echo_capture, cfg->samples);
            if (err)
            {
                DBG_PRINTF("[ECHO] dummy capture fail ch=%d dummy=%d err=%d\r\n", ch, dummy, err);
                return ECHO_STATUS_CAPTURE;
            }
        }

        memset(echo_accum, 0, sizeof(echo_accum));

        for (uint16_t avg = 0; avg < cfg->avg; avg++)
        {
            if (avg > 0U)
            {
                k_busy_wait(PIEZO_AVG_INTER_BURST_GAP_US);
            }

            unsigned int key = irq_lock();
            piezo_burst_sw((uint8_t)cfg->cycles);
            k_busy_wait(capture_delay_us);

            err = echo_adc_capture(echo_capture, cfg->samples);
            irq_unlock(key);
            if (err)
            {
                DBG_PRINTF("[ECHO] capture fail ch=%d avg=%d err=%d\r\n", ch, avg, err);
                return ECHO_STATUS_CAPTURE;
            }

            for (uint16_t i = 0; i < cfg->samples; i++)
            {
                echo_accum[i] += echo_capture[i];
            }
        }

        for (uint16_t i = 0; i < cfg->samples; i++)
        {
            piezo_channels[ch][i] = (uint16_t)(echo_accum[i] / cfg->avg);
        }

        DBG_PRINTF("[MBB] sweep ch=%d done\r\n", ch);
    }

    DBG_PRINTF("[MBB] sweep done\r\n");
    return ECHO_STATUS_OK;
}

static int perform_single_piezo_capture(uint8_t cycles,
                                        uint16_t delay_us,
                                        uint16_t num_samples,
                                        uint16_t averaging,
                                        uint8_t channel)
{
    if (channel >= PIEZO_NUM_CHANNELS)
    {
        return ECHO_STATUS_MUX;
    }

    if ((num_samples == 0U) || (num_samples > ECHO_ADC_MAX_SAMPLES))
    {
        return ECHO_STATUS_CAPTURE;
    }

    if (averaging == 0U)
    {
        averaging = 1U;
    }

    int err = piezo_select_channel(channel);
    if (err)
    {
        return ECHO_STATUS_MUX;
    }

    k_busy_wait(PIEZO_POST_SELECT_SETTLE_US);

    err = echo_adc_wake();
    if (err)
    {
        return ECHO_STATUS_CAPTURE;
    }

    for (uint8_t dummy = 0; dummy < PIEZO_DUMMY_CAPTURE_COUNT; dummy++)
    {
        k_busy_wait(PIEZO_AVG_INTER_BURST_GAP_US);
        piezo_burst_sw(cycles);
        k_busy_wait(delay_us);

        err = echo_adc_capture(echo_capture, num_samples);
        if (err)
        {
            return ECHO_STATUS_CAPTURE;
        }
    }

    memset(echo_accum, 0, sizeof(echo_accum));

    for (uint16_t avg = 0; avg < averaging; avg++)
    {
        if (avg > 0U)
        {
            k_busy_wait(PIEZO_AVG_INTER_BURST_GAP_US);
        }

        unsigned int key = irq_lock();
        piezo_burst_sw(cycles);
        k_busy_wait(delay_us);

        err = echo_adc_capture(echo_capture, num_samples);
        irq_unlock(key);
        if (err)
        {
            return ECHO_STATUS_CAPTURE;
        }

        for (uint16_t i = 0; i < num_samples; i++)
        {
            echo_accum[i] += echo_capture[i];
        }
    }

    for (uint16_t i = 0; i < num_samples; i++)
    {
        echo_capture[i] = (uint16_t)(echo_accum[i] / averaging);
    }

    return ECHO_STATUS_OK;
}

/*==============================================================================
 * 커맨드 핸들러
 *============================================================================*/

/* msn? → 배터리 전압 측정 → rsn: + mV */

static int perform_adc_only_capture(uint16_t num_samples,
                                    uint16_t averaging,
                                    uint8_t channel)
{
    if (channel >= PIEZO_NUM_CHANNELS)
    {
        return ECHO_STATUS_MUX;
    }

    if ((num_samples == 0U) || (num_samples > ECHO_ADC_MAX_SAMPLES))
    {
        return ECHO_STATUS_CAPTURE;
    }

    if (averaging == 0U)
    {
        averaging = 1U;
    }

    int err = piezo_select_channel(channel);
    if (err)
    {
        return ECHO_STATUS_MUX;
    }

    k_busy_wait(PIEZO_POST_SELECT_SETTLE_US);

    err = echo_adc_wake();
    if (err)
    {
        return ECHO_STATUS_CAPTURE;
    }

    memset(echo_accum, 0, sizeof(echo_accum));

    for (uint16_t avg = 0; avg < averaging; avg++)
    {
        if (avg > 0U)
        {
            k_busy_wait(PIEZO_AVG_INTER_BURST_GAP_US);
        }

        unsigned int key = irq_lock();
        err = echo_adc_capture(echo_capture, num_samples);
        irq_unlock(key);
        if (err)
        {
            return ECHO_STATUS_CAPTURE;
        }

        for (uint16_t i = 0; i < num_samples; i++)
        {
            echo_accum[i] += echo_capture[i];
        }
    }

    for (uint16_t i = 0; i < num_samples; i++)
    {
        echo_capture[i] = (uint16_t)(echo_accum[i] / averaging);
    }

    return ECHO_STATUS_OK;
}

static int cmd_msn(const uint8_t *data, uint8_t data_len)
{
    ARG_UNUSED(data);
    ARG_UNUSED(data_len);

    int mv = battery_read_mv();
    if (mv < 0)
    {
        mv = 0;
    }

    send_response_u16("rsn:", (uint16_t)mv);
    DBG_PRINTF("[CMD] msn -> %d mV\r\n", mv);
    return 1;
}

/* msp? → IMU 1회 측정 → rsp: + accel XYZ + gyro XYZ (각 int16 빅엔디안) */
static int cmd_msp(const uint8_t *data, uint8_t data_len)
{
    ARG_UNUSED(data);
    ARG_UNUSED(data_len);

    int16_t accel[3], gyro[3];

    int ret = imu_read(accel, gyro);
    if (ret != 0)
    {
        send_response_u16("rsp:", 0xFFFF);
        DBG_PRINTF("[CMD] msp: FAIL (imu_read ret=%d) -> rsp: 0xFFFF\r\n", ret);
        return 1;
    }

    send_response_imu(accel, gyro);
    return 1;
}

/* mst? → 피에조 전원 ON → TMP235 온도 측정 → 전원 OFF → rso: + 온도(°C × 100, BE)
 *
 * TMP235가 피에조 레일을 공유하므로 ON/OFF 시퀀스를 한 커맨드에서 처리.
 * 안정화 대기 10ms: TMP235 start-up(~2ms) + 레일 RC 필터 여유분.
 * 에러 응답: 0xFFFF = ADC 읽기 실패 */
static int cmd_mst(const uint8_t *data, uint8_t data_len)
{
    ARG_UNUSED(data);
    ARG_UNUSED(data_len);

    /*
     * mst?는 "온도만 읽는 명령"처럼 보이지만,
     * 실제로는 TMP235가 piezo 전원 레일을 같이 쓰기 때문에
     * 전원 ON/OFF 시퀀스까지 같이 처리해야 한다.
     */
    power_button_suspend(true);

    if (piezo_init() != 0)
    {
        power_button_suspend(false);
        send_response_u16("rso:", 0xFFFF);
        DBG_PRINTF("[CMD] mst: piezo init fail\r\n");
        return 1;
    }

    /* 전원 ON → 센서 안정화 대기 */
    piezo_power_on();
    k_msleep(10);

    int16_t t_cdeg = temp_read_cdeg();

    /* 전원 OFF (측정 완료, 레일 끄기) */
    piezo_power_off();
    power_button_suspend(false);

    /* ADC 읽기 실패 → 에러 코드 0xFFFF */
    if (t_cdeg == INT16_MIN)
    {
        send_response_u16("rso:", 0xFFFF);
        DBG_PRINTF("[CMD] mst: temp read fail\r\n");
        return 1;
    }

    /* 음수 온도도 2's complement로 그대로 전송 (앱이 int16로 해석) */
    send_response_u16("rso:", (uint16_t)t_cdeg);
    DBG_PRINTF("[CMD] mst -> %d.%02d C\r\n", t_cdeg / 100, (t_cdeg < 0 ? -t_cdeg : t_cdeg) % 100);
    return 1;
}

/**
 * 테스트용
 * mpa?: piezo TX/RX 전원 레일만 켬
 */
static int cmd_mpa(const uint8_t *data, uint8_t data_len)
{
    ARG_UNUSED(data);
    ARG_UNUSED(data_len);

    DBG_CORE("[MPA] enter\r\n");
    if (piezo_init() != 0)
    {
        DBG_ERR("[MPA] piezo_init failed\r\n");
        send_response_u16("rpa:", 0);
        return 1;
    }

    DBG_CORE("[MPA] piezo_init ok\r\n");
    piezo_power_on();
    DBG_CORE("[MPA] power on\r\n");
    send_response_u16("rpa:", 1);
    DBG_CORE("[MPA] response sent\r\n");
    return 1;
}

static int cmd_mpb(const uint8_t *data, uint8_t data_len)
{
    ARG_UNUSED(data);
    ARG_UNUSED(data_len);

    /* mpb?: piezo TX/RX 전원 레일을 끈다. */
    DBG_CORE("[MPB] enter\r\n");
    if (piezo_init() != 0)
    {
        DBG_ERR("[MPB] piezo_init failed\r\n");
        send_response_u16("rpb:", 0);
        return 1;
    }

    DBG_CORE("[MPB] piezo_init ok\r\n");
    piezo_power_off();
    DBG_CORE("[MPB] power off\r\n");
    send_response_u16("rpb:", 1);
    DBG_CORE("[MPB] response sent\r\n");
    return 1;
}

/**
 * 테스트용
 * mpc?: burst만 한 번 발생시키는 테스트 명령
 * echo를 읽지 않고 초음파가 나가는지만 볼 때 사용
 */
static int cmd_mpc(const uint8_t *data, uint8_t data_len)
{
    const piezo_config_t *cfg = piezo_config_get();
    uint16_t cycles = cfg->cycles;
    uint16_t freq_option = cfg->freq;
    uint16_t piezo_ch = 0;

    get_data_u16_be(data, data_len, 0, &cycles);
    get_data_u16_be(data, data_len, 1, &freq_option);
    get_data_u16_be(data, data_len, 2, &piezo_ch);

    ARG_UNUSED(freq_option);

    if ((cycles < 3U) || (cycles > 7U))
    {
        send_response_u16("rpc:", 2);
        return 1;
    }

    power_button_suspend(true);

    if (piezo_init() != 0)
    {
        power_button_suspend(false);
        send_response_u16("rpc:", 0);
        return 1;
    }

    piezo_power_on();

    if (piezo_select_channel((uint8_t)(piezo_ch % PIEZO_NUM_CHANNELS)) != 0)
    {
        piezo_power_off();
        power_button_suspend(false);
        send_response_u16("rpc:", 0);
        return 1;
    }

    /*
     * 현재 Zephyr 포팅본은 2.1MHz SW burst 하나만 구현되어 있음
     * 레거시의 freq_option 값은 받아두되, 아직은 같은 burst 함수로 처리
     */
    piezo_burst_sw((uint8_t)cycles);
    piezo_power_off();
    power_button_suspend(false);

    send_response_u16("rpc:", cycles);
    return 1;
}

static int cmd_mec(const uint8_t *data, uint8_t data_len)
{
    const piezo_config_t *cfg = piezo_config_get();
    uint16_t freq_option = cfg->freq;
    uint16_t delay_us = cfg->delay_us;
    uint16_t num_samples = cfg->samples;
    uint16_t cycles = cfg->cycles;
    uint16_t averaging = cfg->avg;
    uint16_t piezo_ch = 0;
    uint8_t session = g_echo_session++;

    get_data_u16_be(data, data_len, 0, &freq_option);
    get_data_u16_be(data, data_len, 1, &delay_us);
    get_data_u16_be(data, data_len, 2, &num_samples);
    get_data_u16_be(data, data_len, 3, &cycles);
    get_data_u16_be(data, data_len, 4, &averaging);
    get_data_u16_be(data, data_len, 5, &piezo_ch);

    ARG_UNUSED(freq_option);

    /*
     * 테스트용
     * mec?: 단일 채널 burst + echo capture
     */
    if (num_samples > ECHO_ADC_MAX_SAMPLES)
    {
        num_samples = ECHO_ADC_MAX_SAMPLES;
    }
    if ((cycles < 3U) || (cycles > 7U))
    {
        cycles = PIEZO_SW_BURST_CYCLES;
    }
    if (averaging == 0U)
    {
        averaging = 1U;
    }

    processing = true;
    power_button_suspend(true);

    int status = start_piezo_session();
    if (status == ECHO_STATUS_OK)
    {
        status = perform_single_piezo_capture((uint8_t)cycles, delay_us, num_samples, averaging, (uint8_t)(piezo_ch % PIEZO_NUM_CHANNELS));
    }

    if (status == ECHO_STATUS_OK)
    {
        send_response_echo(session, (uint8_t)(piezo_ch % PIEZO_NUM_CHANNELS), echo_capture, num_samples);
    }

    piezo_power_off();
    power_button_suspend(false);
    send_response_u16("raa:", (uint16_t)status);
    processing = false;
    return 1;
}

/**
 * 테스트용(이전 정렬모드)
 * maa?:
 * - piezo 6채널 burst 순서대로 쏘고
 * - echo 샘플을 채널별로 모은 뒤
 * - reb: 패킷 6개 전송
 * - raa: 상태값은 전체 작업 성공/실패 요약
 */
static int cmd_mad(const uint8_t *data, uint8_t data_len)
{
    const piezo_config_t *cfg = piezo_config_get();
    uint16_t num_samples = cfg->samples;
    uint16_t averaging = cfg->avg;
    uint16_t piezo_ch = 0;
    uint8_t session = g_echo_session++;

    get_data_u16_be(data, data_len, 2, &num_samples);
    get_data_u16_be(data, data_len, 4, &averaging);
    get_data_u16_be(data, data_len, 5, &piezo_ch);

    if (num_samples > ECHO_ADC_MAX_SAMPLES)
    {
        num_samples = ECHO_ADC_MAX_SAMPLES;
    }
    if (averaging == 0U)
    {
        averaging = 1U;
    }

    processing = true;
    power_button_suspend(true);

    int status = start_piezo_session();
    if (status == ECHO_STATUS_OK)
    {
        status = perform_adc_only_capture(num_samples, averaging, (uint8_t)(piezo_ch % PIEZO_NUM_CHANNELS));
    }

    if (status == ECHO_STATUS_OK)
    {
        send_response_echo(session, (uint8_t)(piezo_ch % PIEZO_NUM_CHANNELS), echo_capture, num_samples);
    }

    piezo_power_off();
    power_button_suspend(false);
    send_response_u16("raa:", (uint16_t)status);
    DBG_PRINTF("[CMD] mad status=0x%04X ch=%u samples=%u avg=%u\r\n",
               status,
               (uint16_t)(piezo_ch % PIEZO_NUM_CHANNELS),
               num_samples,
               averaging);

    processing = false;
    return 1;
}

static int cmd_maa(const uint8_t *data, uint8_t data_len)
{
    ARG_UNUSED(data);
    ARG_UNUSED(data_len);

    uint8_t session = g_echo_session++;

    processing = true;
    power_button_suspend(true);

    int status = start_piezo_session();
    if (status == ECHO_STATUS_OK)
    {
        status = perform_piezo_sweep();
    }

    if (status == ECHO_STATUS_OK)
    {
        for (uint8_t ch = 0; ch < PIEZO_NUM_CHANNELS; ch++)
        {
            send_response_echo(session, ch, piezo_channels[ch], piezo_config_get()->samples);
        }
    }

    piezo_power_off();
    power_button_suspend(false);
    send_response_u16("raa:", (uint16_t)status);
    DBG_PRINTF("[CMD] maa status=0x%04X\r\n", status);

    processing = false;
    return 1;
}

/**
 * 전체 측정 시 사용
 */
static int cmd_mbb(const uint8_t *data, uint8_t data_len)
{
    ARG_UNUSED(data);
    ARG_UNUSED(data_len);

    uint8_t session = g_echo_session++;

    int16_t accel[3];
    int16_t gyro[3];

    /*
     * mbb?는 이 프로젝트에서 가장 무거운 명령 중 하나다.
     *
     * 순서:
     * 1. piezo/echo ADC 준비
     * 2. 6채널 echo sweep
     * 3. battery / imu / temp 추가 측정
     * 4. rbb: 1개 전송
     * 5. reb: 6개 전송
     * 6. raa: 최종 상태 전송
     *
     * 중간에 하나라도 실패하면 status에 에러 코드를 넣고,
     * 성공한 경우에만 묶음 응답(rbb + reb)을 보낸다.
     */
    processing = true;
    power_button_suspend(true);
    DBG_PRINTF("[MBB] cmd start\r\n");

    int status = start_piezo_session();
    if (status == ECHO_STATUS_OK)
    {
        status = perform_piezo_sweep();
    }

    int batt_mv = -1;
    int16_t temp_cdeg = INT16_MIN;

    if (status == ECHO_STATUS_OK)
    {
        /* info 성격 데이터는 echo sweep이 정상 끝났을 때만 읽음 */
        DBG_PRINTF("[MBB] battery read\r\n");
        batt_mv = battery_read_mv();
        if (batt_mv < 0)
        {
            status = ECHO_STATUS_BATT;
        }
    }

    if (status == ECHO_STATUS_OK)
    {
        DBG_PRINTF("[MBB] imu read\r\n");
        if (imu_read(accel, gyro) != 0)
        {
            status = ECHO_STATUS_IMU;
        }
    }

    if (status == ECHO_STATUS_OK)
    {
        DBG_PRINTF("[MBB] temp read\r\n");
        temp_cdeg = temp_read_cdeg();
        if (temp_cdeg == INT16_MIN)
        {
            status = ECHO_STATUS_TEMP;
        }
    }

    if (status == ECHO_STATUS_OK)
    {
        /* rbb: 센서(배터리+IMU+온도) 정보 패킷 */
        DBG_PRINTF("[MBB] response tx start\r\n");
        send_response_bundle((uint16_t)batt_mv, accel, gyro, temp_cdeg);

        for (uint8_t ch = 0; ch < PIEZO_NUM_CHANNELS; ch++)
        {
            /* reb: 채널별 raw echo 파형 */
            DBG_PRINTF("[MBB] tx reb ch=%d\r\n", ch);
            send_response_echo(session, ch, piezo_channels[ch], piezo_config_get()->samples);
        }
    }

    piezo_power_off();
    power_button_suspend(false);
    DBG_PRINTF("[MBB] power off\r\n");
    send_response_u16("raa:", (uint16_t)status);
    DBG_PRINTF("[CMD] mbb status=0x%04X\r\n", status);

    processing = false;
    return 1;
}

/**
 * mtb?: piezo sweep + IMU FIFO
 */
static int cmd_mtb(const uint8_t *data, uint8_t data_len)
{
    ARG_UNUSED(data);
    ARG_UNUSED(data_len);

    uint8_t session = g_echo_session++;
    uint16_t rim_count = 0U;
    bool fifo_started = false;

    processing = true;
    power_button_suspend(true);
    DBG_PRINTF("[MTB] cmd start\r\n");

    int status = ECHO_STATUS_OK;
    int imu_ret = imu_fifo_start();
    if (imu_ret != 0)
    {
        DBG_PRINTF("[MTB] fifo start fail ret=%d\r\n", imu_ret);
        status = ECHO_STATUS_IMU;
    }
    else
    {
        fifo_started = true;
    }

    if (status == ECHO_STATUS_OK)
    {
        status = start_piezo_session();
    }

    if (status == ECHO_STATUS_OK)
    {
        status = perform_piezo_sweep();
    }

    if (status == ECHO_STATUS_OK)
    {
        for (uint8_t ch = 0; ch < PIEZO_NUM_CHANNELS; ch++)
        {
            send_response_echo(session, ch, piezo_channels[ch], piezo_config_get()->samples);
        }
    }

    if (fifo_started)
    {
        int imu_ret = imu_fifo_read_latest(tx_rim_samples,
                                           IMU_FIFO_RIM_TARGET_SAMPLES,
                                           &rim_count);
        if (imu_ret != 0)
        {
            DBG_PRINTF("[MTB] fifo read fail ret=%d\r\n", imu_ret);
            status = ECHO_STATUS_IMU;
            rim_count = 0U;
        }
    }

    send_response_rim(tx_rim_samples, rim_count);

    piezo_power_off();
    power_button_suspend(false);
    send_response_u16("raa:", (uint16_t)status);
    DBG_PRINTF("[CMD] mtb status=0x%04X rim=%u\r\n", status, rim_count);

    processing = false;
    return 1;
}

/**
 * mcf?: piezo 측정 파라미터 읽기
 * 응답 rcf: + 설정값 echo back
 */
static int cmd_mcf(const uint8_t *data, uint8_t data_len)
{
    ARG_UNUSED(data);
    ARG_UNUSED(data_len);

    const piezo_config_t *cfg = piezo_config_get();
    send_response_piezo_config("rcf:", cfg->freq, cfg->cycles, cfg->avg, cfg->delay_us, cfg->samples);
    piezo_config_log("[CMD] mcf ->", cfg);
    return 1;
}

/**
 * mcs?: piezo 측정 파라미터 쓰기
 */
static int cmd_mcs(const uint8_t *data, uint8_t data_len)
{
    piezo_config_t cfg;

    if (data_len < 10U)
    {
        send_response_piezo_config("rcs:", 0xFFFF, 0U, 0U, 0U, 0U);
        DBG_PRINTF("[CMD] mcs: insufficient data len=%u\r\n", data_len);
        return 1;
    }

    get_data_u16_be(data, data_len, 0, &cfg.freq);
    get_data_u16_be(data, data_len, 1, &cfg.cycles);
    get_data_u16_be(data, data_len, 2, &cfg.avg);
    get_data_u16_be(data, data_len, 3, &cfg.delay_us);
    get_data_u16_be(data, data_len, 4, &cfg.samples);

    if (!piezo_config_validate(&cfg))
    {
        send_response_piezo_config("rcs:", 0xFFFF, cfg.cycles, cfg.avg, cfg.delay_us, cfg.samples);
        piezo_config_log("[CMD] mcs invalid", &cfg);
        return 1;
    }

    g_piezo_config = cfg;
    int err = app_nvs_save_piezo(&g_piezo_config);
    if (err)
    {
        send_response_piezo_config("rcs:", 0xFFFD, cfg.cycles, cfg.avg, cfg.delay_us, cfg.samples);
        piezo_config_log("[CMD] mcs save fail", &g_piezo_config);
        return 1;
    }

    send_response_piezo_config("rcs:", g_piezo_config.freq, g_piezo_config.cycles, g_piezo_config.avg, g_piezo_config.delay_us, g_piezo_config.samples);
    piezo_config_log("[CMD] mcs saved", &g_piezo_config);
    return 1;
}

/* mls? → LED 상태 변경 → rls: + state echo back
 * 파라미터: [state(2B LE)] — led_state_t enum 값
 *   0=OFF, 4=DETACH_WARNING, 5=ALIGN_SEARCHING, 6=ALIGN_COMPLETE, 7=ERROR
 * 에러 응답: 0xFFFF=파라미터 없음, 0xFFFE=범위 초과 */
static int cmd_mfv(const uint8_t *data, uint8_t data_len)
{
    char fw_version[SERIAL_NO_LENGTH];
    int err;

    ARG_UNUSED(data);
    ARG_UNUSED(data_len);

    copy_fixed_ascii(fw_version, sizeof(fw_version), FIRMWARE_VERSION, strlen(FIRMWARE_VERSION));
    err = send_response_ascii("rfv:", fw_version, sizeof(fw_version));
    if (err)
    {
        DBG_ERR("[CMD] mfv tx failed err=%d\r\n", err);
    }
    DBG_PRINTF("[CMD] mfv read\r\n");
    return 1;
}

static int cmd_mwh(const uint8_t *data, uint8_t data_len)
{
    if (data_len < HW_NO_LENGTH)
    {
        send_response_u16("rwh:", 0xFFFF);
        DBG_PRINTF("[CMD] mwh: insufficient data len=%u\r\n", data_len);
        return 1;
    }

    memset(HW_NO, 0, sizeof(HW_NO));
    memcpy(HW_NO, data, HW_NO_LENGTH);
    int err = app_nvs_save_hw_no(HW_NO);
    if (err)
    {
        send_response_u16("rwh:", 0xFFFD);
        return 1;
    }

    send_response_ascii("rwh:", HW_NO, HW_NO_LENGTH);
    DBG_PRINTF("[CMD] mwh updated\r\n");
    return 1;
}

static int cmd_mrh(const uint8_t *data, uint8_t data_len)
{
    int err;

    ARG_UNUSED(data);
    ARG_UNUSED(data_len);

    err = send_response_ascii("rrh:", HW_NO, HW_NO_LENGTH);
    if (err)
    {
        DBG_ERR("[CMD] mrh tx failed err=%d\r\n", err);
    }
    DBG_PRINTF("[CMD] mrh read\r\n");
    return 1;
}

static int cmd_mws(const uint8_t *data, uint8_t data_len)
{
    if (data_len < SERIAL_NO_LENGTH)
    {
        send_response_u16("rws:", 0xFFFF);
        DBG_PRINTF("[CMD] mws: insufficient data len=%u\r\n", data_len);
        return 1;
    }

    memset(SERIAL_NO, 0, sizeof(SERIAL_NO));
    memcpy(SERIAL_NO, data, SERIAL_NO_LENGTH);
    int err = app_nvs_save_serial_no(SERIAL_NO);
    if (err)
    {
        send_response_u16("rws:", 0xFFFD);
        return 1;
    }

    send_response_ascii("rws:", SERIAL_NO, SERIAL_NO_LENGTH);
    DBG_PRINTF("[CMD] mws updated\r\n");
    return 1;
}

static int cmd_mrs(const uint8_t *data, uint8_t data_len)
{
    int err;

    ARG_UNUSED(data);
    ARG_UNUSED(data_len);

    err = send_response_ascii("rrs:", SERIAL_NO, SERIAL_NO_LENGTH);
    if (err)
    {
        DBG_ERR("[CMD] mrs tx failed err=%d\r\n", err);
    }
    DBG_PRINTF("[CMD] mrs read\r\n");
    return 1;
}

static int cmd_mpz(const uint8_t *data, uint8_t data_len)
{
    if (data_len < PASSKEY_LENGTH)
    {
        send_response_u16("rpz:", 0xFFFF);
        DBG_PRINTF("[CMD] mpz: insufficient data len=%u\r\n", data_len);
        return 1;
    }

    memset(m_static_passkey, 0, sizeof(m_static_passkey));
    memcpy(m_static_passkey, data, PASSKEY_LENGTH);
    int err = app_nvs_save_passkey(m_static_passkey);
    if (err)
    {
        send_response_u16("rpz:", 0xFFFD);
        return 1;
    }

    send_response_ascii("rpz:", m_static_passkey, PASSKEY_LENGTH);
    DBG_PRINTF("[CMD] mpz updated\r\n");
    return 1;
}

static int cmd_mqz(const uint8_t *data, uint8_t data_len)
{
    ARG_UNUSED(data);
    ARG_UNUSED(data_len);

    send_response_ascii("rqz:", m_static_passkey, PASSKEY_LENGTH);
    DBG_PRINTF("[CMD] mqz read\r\n");
    return 1;
}

static int cmd_mls(const uint8_t *data, uint8_t data_len)
{
    /* 파라미터 부족 → 에러 코드 0xFFFF 에코 */
    if (data_len < 2)
    {
        send_response_u16("rls:", 0xFFFF);
        DBG_PRINTF("[CMD] mls: no data\r\n");
        return 1;
    }

    /* state 2B BE */
    uint16_t state = ((uint16_t)data[0] << 8) | (uint16_t)data[1];

    /* 범위 초과 → 에러 코드 0xFFFE 에코 */
    if (state > LED_STATE_ERROR)
    {
        send_response_u16("rls:", 0xFFFE);
        DBG_PRINTF("[CMD] mls: invalid state %d\r\n", state);
        return 1;
    }

    led_set_state((led_state_t)state);
    if (state == LED_STATE_OFF)
    {
        int imu_ret = imu_fifo_stop();
        DBG_PRINTF("[CMD] mls: fifo stop ret=%d\r\n", imu_ret);
    }

    send_response_u16("rls:", state);
    DBG_PRINTF("[CMD] mls -> LED state=%d\r\n", state);
    return 1;
}

/*==============================================================================
 * 커맨드 테이블
 *============================================================================*/
typedef struct
{
    char tag[5];
    int (*handler)(const uint8_t *data, uint8_t data_len);
} cmd_entry_t;

static const cmd_entry_t cmd_table[] =
{
    { "msn?", cmd_msn },
    { "mls?", cmd_mls },
    { "msp?", cmd_msp },
    { "mst?", cmd_mst },
    { "mpa?", cmd_mpa },
    { "mpb?", cmd_mpb },
    { "mpc?", cmd_mpc },
    { "mec?", cmd_mec },
    { "mad?", cmd_mad },
    { "maa?", cmd_maa },
    { "mbb?", cmd_mbb },
    { "mtb?", cmd_mtb },
    { "mcf?", cmd_mcf },
    { "mcs?", cmd_mcs },
    { "mfv?", cmd_mfv },
    { "mwh?", cmd_mwh },
    { "mrh?", cmd_mrh },
    { "mws?", cmd_mws },
    { "mrs?", cmd_mrs },
    { "mpz?", cmd_mpz },
    { "mqz?", cmd_mqz },
};

#define CMD_TABLE_SIZE  (sizeof(cmd_table) / sizeof(cmd_table[0]))

/*==============================================================================
 * 파서 엔트리
 *============================================================================*/
int dr_parser(const uint8_t *buf, uint16_t len)
{
    DBG_CORE("[PARSER] enter len=%u\r\n", len);
    DBG_CORE("[CMD] RX len=%u\r\n", len);
    /* 최소 4바이트 TAG 필요 */
    if (len < 4)
    {
        DBG_ERR("[CMD] Too short (%u)\r\n", len);
        return -1;
    }

    char raw_tag[5] = { buf[0], buf[1], buf[2], buf[3], '\0' };

    /* CRC16 검증 (6바이트 이상이면 마지막 2바이트가 CRC) */
    if (len >= 6)
    {
        uint16_t calc_crc = dr_crc16_compute(buf, len - 2);
        uint16_t recv_crc = (uint16_t)(buf[len - 2]) | ((uint16_t)(buf[len - 1]) << 8);

        if (calc_crc != recv_crc)
        {
            DBG_ERR("[CMD] CRC fail tag=%s calc=0x%04X recv=0x%04X\r\n", raw_tag, calc_crc, recv_crc);
            if (send_response_tag_echo("rxc:", raw_tag) != 0)
            {
                DBG_ERR("[CMD] rxc tx failed\r\n");
            }
            return -1;
        }
    }

    /* TAG 추출 (4바이트) */
    char tag[5] =
    {
        ascii_to_lower((char)buf[0]),
        ascii_to_lower((char)buf[1]),
        ascii_to_lower((char)buf[2]),
        ascii_to_lower((char)buf[3]),
        '\0'
    };
    DBG_CORE("[CMD] tag=%s\r\n", tag);

    /* 데이터 부분 (TAG 이후, CRC 이전) */
    const uint8_t *data = buf + 4;
    uint8_t data_len = (len >= 6) ? (len - 4 - 2) : (len - 4);

    /* 테이블 검색 + 디스패치 */
    for (int i = 0; i < CMD_TABLE_SIZE; i++)
    {
        if (memcmp(tag, cmd_table[i].tag, 4) == 0)
        {
            DBG_CORE("[CMD] dispatch -> %s\r\n", cmd_table[i].tag);
            return cmd_table[i].handler(data, data_len);
        }
    }

    DBG_ERR("[CMD] Unknown: raw=%s normalized=%s\r\n", raw_tag, tag);
    if (send_response_tag_echo("rxx:", raw_tag) != 0)
    {
        DBG_ERR("[CMD] rxx tx failed\r\n");
    }
    return 0;
}