/*******************************************************************************
 * @file    parser.c
 * @brief   BLE command parser
 *
 * Packet format : [TAG 4B] [DATA NB] [CRC16 2B]
 ******************************************************************************/
#include <zephyr/kernel.h>
#include <zephyr/sys/util.h>
#include <zephyr/sys/reboot.h>
#if IS_ENABLED(CONFIG_BT_SMP)
#include <zephyr/bluetooth/bluetooth.h>
#include <zephyr/bluetooth/conn.h>
#endif
#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 "imu_i2c.h"
#include "piezo.h"
#include "piezo_measure.h"

/*==============================================================================
 * Piezo / echo response state
 *============================================================================*/
static uint8_t tx_u16_buf[8];
static uint8_t tx_imu_buf[18];
static uint8_t tx_echo_buf[4 + 2 + 2 + (PIEZO_MEASURE_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_id_buf[4 + HW_NO_LENGTH + SERIAL_NO_LENGTH + SERIAL_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 void reboot_after_response(void)
{
    k_msleep(100);
    sys_reboot(SYS_REBOOT_COLD);
}

/*==============================================================================
 * 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_identity(void)
{
    uint8_t *buf = tx_id_buf;
    char fw_version[SERIAL_NO_LENGTH];

    copy_fixed_ascii(fw_version, sizeof(fw_version), FIRMWARE_VERSION, strlen(FIRMWARE_VERSION));

    buf[0] = 'r';
    buf[1] = 'i';
    buf[2] = 'd';
    buf[3] = ':';
    memcpy(&buf[4], HW_NO, HW_NO_LENGTH);
    memcpy(&buf[4 + HW_NO_LENGTH], SERIAL_NO, SERIAL_NO_LENGTH);
    memcpy(&buf[4 + HW_NO_LENGTH + SERIAL_NO_LENGTH], fw_version, sizeof(fw_version));

    uint16_t crc = dr_crc16_compute(buf, (uint32_t)(sizeof(tx_id_buf) - 2U));
    buf[sizeof(tx_id_buf) - 2U] = (uint8_t)(crc & 0xFF);
    buf[sizeof(tx_id_buf) - 1U] = (uint8_t)(crc >> 8);

    return ble_data_send(buf, sizeof(tx_id_buf));
}

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)
{
    /* 정적 버퍼 사용 (한 번에 한 명령만 처리) */
    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));
}

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

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

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);
    return 1;
}

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

    int err = send_response_u16("rsq:", 0x0000);
    if (err)
    {
        DBG_ERR("[CMD] msq tx failed err=%d\r\n", err);
    }

    DBG_PRINTF("[CMD] msq -> power off\r\n");
    sleep_mode_enter_reason("cmd-msq");
    return 1;
}

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

    int err = send_response_u16("rss:", 0x0000);
    if (err)
    {
        DBG_ERR("[CMD] mss tx failed err=%d\r\n", err);
    }

    DBG_PRINTF("[CMD] mss -> reboot\r\n");
    reboot_after_response();
    return 1;
}

static int cmd_msr(const uint8_t *data, uint8_t data_len)
{
    uint16_t status = 0x0000;

    ARG_UNUSED(data);
    ARG_UNUSED(data_len);

    int tx_err = send_response_u16("rsr:", status);
    if (tx_err)
    {
        DBG_ERR("[CMD] msr tx failed err=%d\r\n", tx_err);
        return 1;
    }

    /*
     * 현재 연결의 bond/security 정보를 먼저 지우면 같은 연결에서 rsr: 응답을
     * 보낼 수 없음. 응답 전송 완료 후 bond 삭제와 reboot 진행 필요
     */
    k_msleep(100);

#if IS_ENABLED(CONFIG_BT_SMP)
    int err = bt_unpair(BT_ID_DEFAULT, NULL);
    if (err)
    {
        status = (uint16_t)(-err);
        DBG_ERR("[CMD] msr bt_unpair failed err=%d\r\n", err);
    }
    else
    {
        bond_data_delete = true;
        DBG_PRINTF("[CMD] msr bond data deleted\r\n");
    }
#else
    bond_data_delete = true;
    DBG_PRINTF("[CMD] msr bond delete skipped (BT_SMP disabled)\r\n");
#endif

    if (status == 0x0000)
    {
        DBG_PRINTF("[CMD] msr complete -> reboot\r\n");
    }

    reboot_after_response();
    return 1;
}

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

    uint16_t status = ble_dfu_advertising_is_enabled() ? 0xFFFF : 0x0000;

    /* DFU advertising 전환 중 연결이 끊겨도 전원 래치가 유지되도록 먼저 ON 상태를 확정한다. */
    device_power_keep_on();

    int tx_err = send_response_u16("rdf:", status);
    if (tx_err)
    {
        DBG_ERR("[CMD] mdf tx failed err=%d\r\n", tx_err);
        return 1;
    }

    if (status == 0x0000)
    {
        /* rdf: 응답이 나간 뒤 연결 해제와 SMP 광고 전환 실행 */
        k_msleep(100);
        int err = ble_dfu_advertising_enable();
        if (err)
        {
            DBG_ERR("[CMD] mdf enable failed err=%d\r\n", err);
        }
    }
    else
    {
        DBG_PRINTF("[CMD] mdf rejected: already in DFU advertising\r\n");
    }

    return 1;
}

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? -> IMU internal temperature -> rso: + temperature (degC x 100, BE)
 * Error response: 0xFFFF = IMU temperature read failed. */
static int cmd_mst(const uint8_t *data, uint8_t data_len)
{
    ARG_UNUSED(data);
    ARG_UNUSED(data_len);

    power_button_suspend(true);

    int16_t t_cdeg = INT16_MIN;
    int ret = imu_read_temperature_cdeg(&t_cdeg);

    power_button_suspend(false);

    if ((ret != 0) || (t_cdeg == INT16_MIN))
    {
        send_response_u16("rso:", 0xFFFF);
        DBG_PRINTF("[CMD] mst: imu temp read fail ret=%d\r\n", ret);
        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;
}

/*
 * 테스트용
 * mec?: 단일 채널 burst + echo capture
 */
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);

    if (num_samples > PIEZO_MEASURE_MAX_SAMPLES)
    {
        num_samples = PIEZO_MEASURE_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 = piezo_measure_start_session();
    if (status == ECHO_STATUS_OK)
    {
        status = piezo_measure_single_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), piezo_measure_single_buffer(), num_samples);
    }

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

/*
 * 테스트용
 * mad?: 단일 채널 echo capture만 수행
 * - piezo burst는 하지 않고 echo ADC만 수행
 * - echo 샘플을 모은 뒤 reb: 패킷 1개 전송
 * - 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 > PIEZO_MEASURE_MAX_SAMPLES)
    {
        num_samples = PIEZO_MEASURE_MAX_SAMPLES;
    }
    if (averaging == 0U)
    {
        averaging = 1U;
    }

    processing = true;
    power_button_suspend(true);

    int status = piezo_measure_start_session();
    if (status == ECHO_STATUS_OK)
    {
        status = piezo_measure_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), piezo_measure_single_buffer(), 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;
}

/*
 * 테스트용(이전 정렬모드)
 * maa?:
 * - piezo 6채널 burst 순서대로 쏘고
 * - echo 샘플을 채널별로 모은 뒤
 * - reb: 패킷 6개 전송
 * - raa: 상태값은 전체 작업 성공/실패 요약
 */
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 = piezo_measure_start_session();
    if (status == ECHO_STATUS_OK)
    {
        status = piezo_measure_sweep();
    }

    if (status == ECHO_STATUS_OK)
    {
        for (uint8_t ch = 0; ch < PIEZO_NUM_CHANNELS; ch++)
        {
            send_response_echo(session, ch, piezo_measure_channel_buffer(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;
}

/*
 * 전체 측정 명령
 * mbb?: piezo echo sweep + 배터리 + IMU + IMU 온도 측정
 * - 성공 시 rbb 1개, reb 6개, raa 상태값 전송
 * - 실패 시 rbb/reb 생략, raa에 실패 원인 코드 전송
 * - processing=true 구간에서는 내부 모니터의 I2C 접근을 막는 기준으로 사용
 */
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 = piezo_measure_start_session();
    if (status == ECHO_STATUS_OK)
    {
        status = piezo_measure_sweep();
    }

    int batt_mv = -1;
    int16_t temp_cdeg = INT16_MIN;

    if (status == ECHO_STATUS_OK)
    {
        /* echo sweep 성공 후에만 부가 센서값 읽기
         * - rbb에 포함되는 배터리 전압
         * - 실패 시 ECHO_STATUS_BATT로 종료
         */
        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)
    {
        /* rbb에 포함되는 IMU 6축 값과 IMU 내부 온도 읽기
         * - direct read 경로 사용
         * - 실패 시 ECHO_STATUS_IMU로 종료
         */
        DBG_PRINTF("[MBB] imu/temp read\r\n");
        if (imu_read_with_temperature(accel, gyro, &temp_cdeg) != 0)
        {
            status = ECHO_STATUS_IMU;
        }
    }

    if (status == ECHO_STATUS_OK)
    {
        /* mbb? 성공 응답값으로 연속 측정 보호 판정 갱신
         * - 실패한 측정은 호출하지 않아 기존 저전압/고온 카운트 유지
         * - rbb에 실리는 배터리/IMU 온도 값과 같은 값을 사용
         */
        battery_protection_record_continuous(batt_mv, temp_cdeg);

        /* 측정 성공 응답 순서
         * - rbb: 배터리 + IMU + IMU 온도 요약
         * - reb: 채널별 echo raw 데이터
         * - raa: 마지막 상태 코드
         */
        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_measure_channel_buffer(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 = piezo_measure_start_session();
    }

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

    if (status == ECHO_STATUS_OK)
    {
        for (uint8_t ch = 0; ch < PIEZO_NUM_CHANNELS; ch++)
        {
            send_response_echo(session, ch, piezo_measure_channel_buffer(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);
    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);
        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);
        return 1;
    }

    int err = piezo_config_set(&cfg);
    if (err)
    {
        send_response_piezo_config("rcs:", 0xFFFD, cfg.cycles, cfg.avg, cfg.delay_us, cfg.samples);
        return 1;
    }

    const piezo_config_t *saved_cfg = piezo_config_get();
    send_response_piezo_config("rcs:", saved_cfg->freq, saved_cfg->cycles, saved_cfg->avg, saved_cfg->delay_us, saved_cfg->samples);
    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);
    }
    return 1;
}

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

    ARG_UNUSED(data);
    ARG_UNUSED(data_len);

    err = send_response_identity();
    if (err)
    {
        DBG_ERR("[CMD] mid tx failed err=%d\r\n", err);
    }
    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);
        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);
    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);
    }
    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);
        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);
    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);
    }
    return 1;
}

static int cmd_mpz(const uint8_t *data, uint8_t data_len)
{
    if (data_len < PASSKEY_LENGTH)
    {
        send_response_u16("rpz:", 0xFFFF);
        return 1;
    }

    if (m_passkey_changed != 0U)
    {
        /* 패스키가 변경된 경우 다시 바꿀 수 없음(FFFF 반환) */
        send_response_u16("rpz:", 0xFFFF);
        DBG_PRINTF("[CMD] mpz: passkey already changed\r\n");
        return 1;
    }

    /* NVS 저장이 모두 성공한 뒤 RAM 값 확정 */
    char new_passkey[PASSKEY_BUF_SIZE];
    memset(new_passkey, 0, sizeof(new_passkey));
    memcpy(new_passkey, data, PASSKEY_LENGTH);

    int err = app_nvs_save_passkey(new_passkey);
    if (err)
    {
        send_response_u16("rpz:", 0xFFFD);
        return 1;
    }

    /* 변경 완료 플래그 세트 : 다음 mpz? 명령 차단 */
    err = app_nvs_save_passkey_changed(1U);
    if (err)
    {
        send_response_u16("rpz:", 0xFFFD);
        return 1;
    }

    memset(m_static_passkey, 0, sizeof(m_static_passkey));
    memcpy(m_static_passkey, new_passkey, PASSKEY_LENGTH);
    m_passkey_changed = 1U;

    send_response_ascii("rpz:", m_static_passkey, PASSKEY_LENGTH);
    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);
    return 1;
}

/*
 * LED 상태 변경 명령
 * mls?: 앱에서 전달한 LED 상태값을 적용하고 rls로 echo back
 * - 파라미터 부족: 0xFFFF
 * - 범위 초과: 0xFFFE
 * - LED_STATE_OFF 진입 시 IMU FIFO 정지
 */
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 필드: 2바이트 big-endian */
    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);

    /* 정렬 LED 상태와 보호 판정 정책 연결
     * - ALIGN_SEARCHING/ALIGN_COMPLETE 동안 보호 판정 차단
     * - 다른 LED 상태에서는 기존 조건에 따라 보호 판정 재개
     */
    battery_protection_set_alignment_mode((state == LED_STATE_ALIGN_SEARCHING) ||
                                          (state == LED_STATE_ALIGN_COMPLETE));

    /* OFF 상태에서는 정렬/측정용 FIFO를 함께 정리 */
    if (state == LED_STATE_OFF)
    {
        (void)imu_fifo_stop();
    }

    send_response_u16("rls:", 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[] =
{
    { "msq?", cmd_msq },
    { "mss?", cmd_mss },
    { "msr?", cmd_msr },
    { "mdf?", cmd_mdf },
    { "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 },
    { "mid?", cmd_mid },
    { "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 ble_cmd_dispatch(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);

    char raw_tag[5] = { '?', '?', '?', '?', '\0' };
    for (uint16_t i = 0; i < len && i < 4U; i++)
    {
        raw_tag[i] = (char)buf[i];
    }

    if (len < 7U)
    {
        DBG_ERR("[CMD] Too short (%u)\r\n", len);
        if (send_response_tag_echo("rxs:", raw_tag) != 0)
        {
            DBG_ERR("[CMD] rxs tx failed\r\n");
        }
        return -1;
    }

    uint16_t calc_crc = dr_crc16_compute(buf, len - 2U);
    uint16_t recv_crc = (uint16_t)(buf[len - 2U]) | ((uint16_t)(buf[len - 1U]) << 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;
    }

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

    const uint8_t *data = buf + 4;
    uint8_t data_len = (uint8_t)(len - 4U - 2U);

    for (int i = 0; i < CMD_TABLE_SIZE; i++)
    {
        if (memcmp(tag, cmd_table[i].tag, 4) == 0)
        {
            if (cmd_table[i].handler == NULL)
            {
                DBG_ERR("[CMD] Null handler: %s\r\n", cmd_table[i].tag);
                if (send_response_tag_echo("rxn:", raw_tag) != 0)
                {
                    DBG_ERR("[CMD] rxn tx failed\r\n");
                }
                return -1;
            }

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