/*******************************************************************************
 * @file    cmd_piezo.c
 * @brief   BLE command handlers
 ******************************************************************************/
#include <zephyr/sys/util.h>
#include <limits.h>

#include "cmd_common.h"
#include "main.h"
#include "debug_print.h"
#include "battery_adc.h"
#include "imu_i2c.h"
#include "piezo.h"
#include "piezo_measure.h"
#include "cmd_piezo.h"

static uint8_t tx_rim_samples[IMU_FIFO_RIM_TARGET_SAMPLES * IMU_FIFO_SAMPLE_BYTES];

/*
 * 주기 측정 명령
 * mbb?: piezo echo sweep + 배터리 + IMU + IMU 온도 측정
 * - 성공 시 rbb 1개, reb 6개, raa 상태값 전송
 * - processing=true 구간에서는 내부 모니터의 I2C 접근을 막는 기준으로 사용
 * 
 * - 순서:
 * 1. battery / imu / temp 측정
 * 2. rbb: 1개 전송
 * 3. piezo / echo ADC 준비
 * 4. piezo 6채널 burst
 * 5. echo 샘플 채널별로 모으기
 * 6. reb: 6개 전송
 * 7. raa: 상태값 전송
 */
int cmd_mbb(const uint8_t *data, uint8_t data_len)
{
    ARG_UNUSED(data);
    ARG_UNUSED(data_len);

    uint8_t session = cmd_next_echo_session();

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

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

    int batt_mv = -1;
    int16_t temp_cdeg = INT16_MIN;

    /* Read battery first (idle measurement) so mbb? result matches msn? */
    DBG_PRINTF("[MBB] battery read (idle)\r\n");
    batt_mv = battery_read_mv();

    int status = piezo_measure_start_session();

    if (status == ECHO_STATUS_OK) // Piezo GPIO, power, ADC initialization/wake succeeded
    {
        DBG_PRINTF("[MBB] imu/temp read\r\n");
        imu_read_with_temperature(accel, gyro, &temp_cdeg);

        battery_protection_record_continuous(batt_mv, temp_cdeg); // update protection counters for continuous monitoring

        DBG_PRINTF("[MBB] response rbb: tx start\r\n");
        cmd_send_response_bundle((uint16_t)batt_mv, accel, gyro, temp_cdeg);

        DBG_PRINTF("[MBB] piezo sweep start\r\n");
        status = piezo_measure_sweep();
        if (status == ECHO_STATUS_OK)
        {
            for (uint8_t ch = 0; ch < PIEZO_NUM_CHANNELS; ch++)
            {
                DBG_PRINTF("[MBB] response reb: tx start ch=%d\r\n", ch);
                cmd_send_response_echo(session, ch, piezo_measure_channel_buffer(ch), piezo_config_get()->samples);
            }
        }
    }

    piezo_power_off();
    DBG_PRINTF("[MBB] done\r\n");
    cmd_send_response_u16("raa:", (uint16_t)status); // 최종 상태 raa: 전송
    DBG_PRINTF("[CMD] mbb status=0x%04X\r\n", status);

    processing = false;
    return 1;
}

/*
 * 정렬모드
 * mtb?: IMU FIFO + piezo echo sweep
 * - 순서:
 * 1. IMU FIFO start
 * 2. piezo 6채널 burst
 * 3. echo 샘플 채널별로 모으기
 * 4. reb: 패킷 6개 전송
 * 5. raa: 상태값 전송
 * 6. rim: IMU FIFO 15 샘플 전송
 */
int cmd_mtb(const uint8_t *data, uint8_t data_len)
{
    ARG_UNUSED(data);
    ARG_UNUSED(data_len);

    uint8_t session = cmd_next_echo_session();
    uint16_t rim_count = 0U;
    bool fifo_started = false;

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

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

    int status = piezo_measure_start_session();
    
    if (status == ECHO_STATUS_OK) // Piezo GPIO, power, ADC initialization/wake succeeded
    {
        DBG_PRINTF("[MTB] piezo sweep start\r\n");
        status = piezo_measure_sweep();
        if (status == ECHO_STATUS_OK)
        {
            for (uint8_t ch = 0; ch < PIEZO_NUM_CHANNELS; ch++)
            {
                DBG_PRINTF("[MTB] response reb: tx start ch=%d\r\n", ch);
                cmd_send_response_echo(session, ch, piezo_measure_channel_buffer(ch), piezo_config_get()->samples);
            }
        }
    }
    
    cmd_send_response_u16("raa:", (uint16_t)status); // 최종 상태 raa: 전송

    if (status == ECHO_STATUS_OK)
    {
        if (fifo_started)
        {
            imu_ret = imu_fifo_wait_samples(IMU_FIFO_RIM_TARGET_SAMPLES, 500U);
            if (imu_ret != 0)
            {
                DBG_PRINTF("[MTB] fifo wait ret=%d\r\n", imu_ret);
            }

            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);
                rim_count = 0U;
            }
        }

        cmd_send_response_rim(tx_rim_samples, rim_count); // IMU FIFO 샘플 rim: 전송
    }
    DBG_PRINTF("[CMD] mtb status=0x%04X\r\n", status);

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

    processing = false;
    return 1;
}

/*
 * mcf?: piezo measurement parameter read
 */
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();
    cmd_send_response_piezo_config("rcf:", cfg->freq, cfg->cycles, cfg->avg, cfg->delay_us, cfg->samples);
    return 1;
}

/*
 * mcs?: piezo measurement parameter write
 */
int cmd_mcs(const uint8_t *data, uint8_t data_len)
{
    piezo_config_t cfg;

    if (data_len < 10U)
    {
        cmd_send_response_piezo_config("rcs:", 0xFFFF, 0U, 0U, 0U, 0U);
        return 1;
    }

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

    if (!piezo_config_validate(&cfg))
    {
        cmd_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)
    {
        cmd_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();
    cmd_send_response_piezo_config("rcs:", saved_cfg->freq, saved_cfg->cycles, saved_cfg->avg, saved_cfg->delay_us, saved_cfg->samples);
    return 1;
}

// =============================== Test Command ===============================
/* TEST
* mim? : IMU FIFO 15 samples
*/
int cmd_mim(const uint8_t *data, uint8_t data_len)
{
    ARG_UNUSED(data);
    ARG_UNUSED(data_len);

    if (processing)
    {
        return 1;
    }

    processing = true;

    uint16_t rim_count = 0U;
    int imu_ret = imu_fifo_start();
    if (imu_ret != 0)
    {
        DBG_PRINTF("[MTB] fifo start fail ret=%d\r\n", imu_ret);
        cmd_send_response_rim(tx_rim_samples, 0U);
        processing = false;
        return 1;
    }
     
    k_msleep(320);  // IMU FIFO is 50Hz, 15 samples need about 300ms

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

    cmd_send_response_rim(tx_rim_samples, rim_count); // IMU FIFO 15 samples rim: 전송
    imu_fifo_stop();
    processing = false;
    return 1;
}

/*
 * TEST (이전 정렬모드)
 * maa?: piezo echo sweep
 * - 순서:
 * 1. piezo 6채널 burst
 * 2. echo 샘플 채널별로 모으기
 * 3. reb: 패킷 6개 전송
 * 4. raa: 상태값 전송
 */
int cmd_maa(const uint8_t *data, uint8_t data_len)
{
    ARG_UNUSED(data);
    ARG_UNUSED(data_len);

    uint8_t session = cmd_next_echo_session();

    processing = true;

    int status = piezo_measure_start_session();

    if (status == ECHO_STATUS_OK) // Piezo GPIO, power, ADC initialization/wake succeeded
    {
        DBG_PRINTF("[MAA] piezo sweep start\r\n");
        status = piezo_measure_sweep();
        if (status == ECHO_STATUS_OK)
        {
            for (uint8_t ch = 0; ch < PIEZO_NUM_CHANNELS; ch++)
            {
                DBG_PRINTF("[MBB] response reb: tx start ch=%d\r\n", ch);
                cmd_send_response_echo(session, ch, piezo_measure_channel_buffer(ch), piezo_config_get()->samples);
            }
        }
    }

    piezo_power_off();
    cmd_send_response_u16("raa:", (uint16_t)status); // 최종 상태 raa: 전송
    DBG_PRINTF("[CMD] maa status=0x%04X\r\n", status);

    processing = false;
    return 1;
}

/*
 * TEST
 * mpa?: piezo power ON
 */
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");
        cmd_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");
    cmd_send_response_u16("rpa:", 1);
    DBG_CORE("[MPA] response sent\r\n");
    return 1;
}

/*
 * TEST
 * mpb?: piezo power OFF
 */
int cmd_mpb(const uint8_t *data, uint8_t data_len)
{
    ARG_UNUSED(data);
    ARG_UNUSED(data_len);

    DBG_CORE("[MPB] enter\r\n");
    if (piezo_init() != 0)
    {
        DBG_ERR("[MPB] piezo_init failed\r\n");
        cmd_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");
    cmd_send_response_u16("rpb:", 1);
    DBG_CORE("[MPB] response sent\r\n");
    return 1;
}

/*
 * TEST
 * mpc?: burst only
 */
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;

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

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


    if (piezo_init() != 0)
    {
        cmd_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();
        cmd_send_response_u16("rpc:", 0);
        return 1;
    }

    piezo_burst_sw_freq((uint8_t)freq_option, cycles);
    piezo_power_off();

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

/*
 * TEST
 * mec?: 1 channel burst + echo capture
 */
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 = cmd_next_echo_session();

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

    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;

    int status = piezo_measure_start_session();
    if (status == ECHO_STATUS_OK)
    {
        status = piezo_measure_single_capture((uint8_t)freq_option, cycles, delay_us, num_samples, averaging, (uint8_t)(piezo_ch % PIEZO_NUM_CHANNELS));
    }

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

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

/*
 * TEST
 * mad?: 1 channel echo capture only
 */
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 = cmd_next_echo_session();

    cmd_get_data_u16_be(data, data_len, 2, &num_samples);
    cmd_get_data_u16_be(data, data_len, 4, &averaging);
    cmd_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;

    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)
    {
        cmd_send_response_echo(session, (uint8_t)(piezo_ch % PIEZO_NUM_CHANNELS), piezo_measure_single_buffer(), num_samples);
    }

    piezo_power_off();
    cmd_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;
}