/*==============================================================================
 * cmd_piezo.c - Piezo ultrasound measurement handlers
 *
 * mpa? -> rpa:           TX/RX power ON
 * mpb? -> rpb:           TX/RX power OFF
 * mpc? -> rpc:           burst generation (test)
 * mec? -> reb:+raa:      single-channel burst + echo capture
 * maa? -> reb:+raa:      6-channel asynchronous capture
 * mbb? -> rbb:+reb:+raa: bulk sensor measurement + 6-channel capture
 * mcf? -> rcf:           read piezo parameters
 * mcs? -> rcs:           write piezo parameters
 *============================================================================*/

#include "cmd_common.h"
#include "cmd_piezo.h"
#include "cmd_sensor.h"           /* all_sensors() */
#include "dr_piezo.h"
#include "dr_adc121s051.h"
#include "led_control.h"

static void mtb_send_rim_after_piezo(void)
{
    send_imu_rim_fifo();
}

static void mab_led_off_after_piezo(void)
{
    led_set_state(LED_STATE_OFF);
}

/*------------------------------------------------------------------------------
 * Internal clamp helpers for persisted piezo configuration
 *----------------------------------------------------------------------------*/
static uint8_t clamp_piezo_freq_option(uint16_t raw_freq)
{
    /* Keep the existing option-code protocol:
     *   0=1.8MHz, 1=2.1MHz, 2=2.0MHz, 3=1.7MHz, 4=2.2MHz, 9=1.9MHz
     * Unknown values fall back to 2.1MHz (code 1).
     */
    switch (raw_freq)
    {
        case 3:   /* 1.7MHz */
            return 3;

        case 0:   /* 1.8MHz */
            return 0;

        case 9:   /* 1.9MHz */
            return 9;

        case 2:   /* 2.0MHz */
            return 2;

        case 1:   /* 2.1MHz */
            return 1;

        case 4:   /* 2.2MHz */
            return 4;

        default:
            return 1;      /* unknown code -> default to 2.1MHz */
    }
}

static uint8_t clamp_piezo_cycles(uint16_t cycles)
{
    if (cycles < 3)
    {
        return 3;
    }
    if (cycles > 7)
    {
        return 7;
    }
    return (uint8_t)cycles;
}

static uint16_t clamp_piezo_averaging(uint16_t averaging)
{
    if (averaging < 1)
    {
        return 1;
    }
    if (averaging > 10)
    {
        return 10;
    }
    return averaging;
}

static uint16_t clamp_piezo_delay_us(uint16_t delay_us)
{
    if (delay_us > 50)
    {
        return 50;
    }
    return delay_us;
}

static uint16_t clamp_piezo_num_samples(uint16_t num_samples)
{
    if (num_samples < 80)
    {
        return 80;
    }
    if (num_samples > 119)
    {
        return 119;
    }
    return num_samples;
}

/*==============================================================================
 * mpa? -> rpa:  Enable piezo TX/RX circuit
 *
 * Request:  [TAG 4B "mpa?"] [CRC 2B]
 * Response: [TAG 4B "rpa:"] [1 2B] [CRC 2B]
 *============================================================================*/
int Cmd_mpa(const ParsedCmd *cmd)
{
    (void)cmd;
    dr_piezo_power_on();

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

/*==============================================================================
 * mpb? -> rpb:  Disable piezo TX/RX circuit
 *
 * Request:  [TAG 4B "mpb?"] [CRC 2B]
 * Response: [TAG 4B "rpb:"] [1 2B] [CRC 2B]
 *
 * Power saving once measurement is complete.
 *============================================================================*/
int Cmd_mpb(const ParsedCmd *cmd)
{
    (void)cmd;
    dr_piezo_power_off();

    if (g_plat.tx_bin)
    {
        single_format_data(ble_bin_buffer, "rpb:", 1);
        dr_binary_tx_safe(ble_bin_buffer, 3);
    }
    return 1;
}

/*==============================================================================
 * mpc? -> rpc:  Piezo burst generation (test / debug)
 *
 * Request:  [TAG 4B "mpc?"] [cycles 2B] [freq_option 2B] [piezo_ch 2B] [CRC 2B]
 *   cycles      : 3..7 (default 5)
 *   freq_option : 0=1.8MHz, 1=2.1MHz(default), 2=2.0MHz, 3=1.7MHz, 4=2.2MHz
 *   piezo_ch    : 0..7
 * Response: [TAG 4B "rpc:"] [cycles 2B] [CRC 2B]
 * Error:    rpc: + 2  (cycles out of range)
 *
 * Generates the burst only - no echo capture.
 *============================================================================*/
int Cmd_mpc(const ParsedCmd *cmd)
{
    uint16_t cycles = 5;
    uint16_t freq_option = 1;
    uint16_t piezo_ch = 0;

    (void)dr_get_u16(cmd, 0, &cycles);
    (void)dr_get_u16(cmd, 1, &freq_option);
    (void)dr_get_u16(cmd, 2, &piezo_ch);

    if (piezo_ch >= MAA_NUM_CHANNELS)
    {
        piezo_ch = 0;
    }

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

    dr_piezo_select_channel((uint8_t)piezo_ch);

    switch (freq_option)
    {
        case 0:
            dr_piezo_burst_sw_18mhz((uint8_t)cycles);
            break;
        case 2:  
            dr_piezo_burst_sw_20mhz((uint8_t)cycles);
            break;
        case 3:  
            dr_piezo_burst_sw_17mhz((uint8_t)cycles);
            break;
        case 4:  
            dr_piezo_burst_sw_22mhz((uint8_t)cycles);
            break;
        default: 
            dr_piezo_burst_sw((uint8_t)cycles);
            break;
    }

    dr_ble_return_1("rpc:", (uint8_t)cycles);
    return 1;
}

/*==============================================================================
 * mec? -> reb:+raa:  Piezo burst + echo capture (16-bit raw)
 *
 * Request:  [TAG 4B "mec?"] [freq 2B] [delay_us 2B] [num_samples 2B]
 *           [cycles 2B] [averaging 2B] [piezo_ch 2B] [CRC 2B]
 * Response: reb: [num_samples 2B] [raw_data up to 238B]   (red: follow-up if needed)
 *           raa: [status 2B]
 * Error:    rer: + (0xEE00|err) + num_samples
 *
 * Sends raw 16-bit ADC values directly. Suited for short-range (~25cm).
 *============================================================================*/
int Cmd_mec(const ParsedCmd *cmd)
{
    uint16_t freq_option = 0;
    uint16_t delay_us = 20;
    uint16_t num_samples = 140;
    uint16_t cycles = 5;
    uint16_t averaging = 1;
    uint16_t piezo_ch = 0;

    if (!dr_piezo_is_power_on())
    {
        dr_piezo_power_on();
    }

    (void)dr_get_u16(cmd, 0, &freq_option);
    (void)dr_get_u16(cmd, 1, &delay_us);
    (void)dr_get_u16(cmd, 2, &num_samples);
    (void)dr_get_u16(cmd, 3, &cycles);
    (void)dr_get_u16(cmd, 4, &averaging);
    (void)dr_get_u16(cmd, 5, &piezo_ch);

    if (averaging == 0)
    {
        averaging = 1;
    }
    if (averaging > 1000)
    {
        averaging = 1000;
    }
    if (piezo_ch >= MAA_NUM_CHANNELS)
    {
        piezo_ch = 0;
    }

    dr_adc_err_t err = dr_adc_burst_capture_transmit(
        (uint8_t)freq_option, delay_us, num_samples, (uint8_t)cycles,
        (uint16_t)averaging, (uint8_t)piezo_ch, ble_bin_buffer, 0);

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

    dr_piezo_power_off();
    return 1;
}

/*==============================================================================
 * maa? -> reb:+raa:  6-channel asynchronous full capture
 *
 * Request:  [TAG 4B "maa?"] [CRC 2B]
 * Response: per channel  reb: [num_samples 2B] [raw_data...]
 *           final         raa: [status 2B]
 * Error:    raa: + 0xFFFE        (previous capture in progress)
 *           raa: + (0xFF00|err)  (start failed)
 *
 * Asynchronous state machine - the BLE_NUS_EVT_TX_RDY callback drives the
 * follow-up packet transmissions. Parameters are loaded from m_config (FDS).
 *============================================================================*/
int Cmd_maa(const ParsedCmd *cmd)
{
    dr_adc_err_t err;
    (void)cmd;

    if (maa_async_is_busy())
    {
        dr_ble_return_1("raa:", 0xFFFE);
        return 1;
    }

    err = maa_async_start(
        m_config.piezo_freq_option,
        m_config.piezo_delay_us,
        m_config.piezo_num_samples,
        m_config.piezo_cycles,
        m_config.piezo_averaging,
        ble_bin_buffer
    );

    if (err != DR_ADC_OK)
    {
        if (g_plat.log)
        {
            g_plat.log("[Cmd_maa] start failed err=%d\r\n", err);
        }
        single_format_data(ble_bin_buffer, "raa:", (uint16_t)(0xFF00 | err));
        dr_binary_tx_safe(ble_bin_buffer, 3);
        dr_piezo_power_off();
        return 1;
    }

    return 1;
}

/*==============================================================================
 * mbb? -> rbb:+reb:+raa:  Bulk sensor measurement + 6-channel full capture
 *
 * Request:  [TAG 4B "mbb?"] [CRC 2B]
 * Response: rbb: [batt 2B] [IMU 12B] [temp 2B]
 *           reb: [num_samples 2B] [raw_data...]   (per channel)
 *           raa: [status 2B]
 * Error:    raa: + 0xFFFE       (previous capture in progress)
 *           raa: + (0xFF00|err) (start failed)
 *
 * 1) all_sensors() measures battery / IMU / temperature and emits rbb:
 * 2) Starts 6-channel capture in pre-capture-all mode
 *============================================================================*/
int Cmd_mbb(const ParsedCmd *cmd)
{
    dr_adc_err_t err;
    (void)cmd;

    all_sensors();

    if (maa_async_is_busy())
    {
        dr_ble_return_1("raa:", 0xFFFE);
        return 1;
    }

    maa_async_set_pre_capture_all(true);

    err = maa_async_start(
        m_config.piezo_freq_option,
        m_config.piezo_delay_us,
        m_config.piezo_num_samples,
        m_config.piezo_cycles,
        m_config.piezo_averaging,
        ble_bin_buffer
    );

    if (err != DR_ADC_OK)
    {
        if (g_plat.log)
        {
            g_plat.log("[Cmd_mbb] start failed err=%d\r\n", err);
        }
        single_format_data(ble_bin_buffer, "raa:", (uint16_t)(0xFF00 | err));
        dr_binary_tx_safe(ble_bin_buffer, 3);
        dr_piezo_power_off();
        return 1;
    }

    return 1;
}

/*==============================================================================
 * mtb? -> reb:+raa:+rim:  Piezo ADC + IMU FIFO (no rbb:)
 *
 * Request:  [TAG 4B "mtb?"] [CRC 2B]
 * Response: reb: [num_samples 2B] [raw_data...]   (per channel; same maa_async as mbb?)
 *           raa: [status 2B]
 *           rim: [total_sample_count u16 BE] [samples: 12B each ax,ay,az,gx,gy,gz ...] (may span BLE packets)
 * Error:    raa: + 0xFFFE       (previous capture in progress)
 *           raa: + (0xFF00|err) (start failed)
 *
 * reb/raa use the same maa_async_start path as mbb?; no rbb: / all_sensors().
 *============================================================================*/
int Cmd_mtb(const ParsedCmd *cmd)
{
    dr_adc_err_t err;
    (void)cmd;

    if (maa_async_is_busy())
    {
        dr_ble_return_1("raa:", 0xFFFE);
        return 1;
    }

    (void)imu_fifo_capture_start();

    maa_async_set_pre_capture_all(true);

    err = maa_async_start(
        m_config.piezo_freq_option,
        m_config.piezo_delay_us,
        m_config.piezo_num_samples,
        m_config.piezo_cycles,
        m_config.piezo_averaging,
        ble_bin_buffer
    );

    if (err != DR_ADC_OK)
    {
        if (g_plat.log)
        {
            g_plat.log("[Cmd_mtb] start failed err=%d\r\n", err);
        }
        single_format_data(ble_bin_buffer, "raa:", (uint16_t)(0xFF00 | err));
        dr_binary_tx_safe(ble_bin_buffer, 3);
        dr_piezo_power_off();
        maa_async_set_on_complete(NULL);
        send_imu_rim_fifo();
        return 1;
    }

    maa_async_set_on_complete(mtb_send_rim_after_piezo);

    return 1;
}

/*==============================================================================
 * mab? -> reb:+raa:  6-channel asynchronous full capture + GR LED
 *
 * Request:  [TAG 4B "mab?"] [CRC 2B]
 * Response: per channel  reb: [num_samples 2B] [raw_data...]
 *           final         raa: [status 2B]
 * Error:    raa: + 0xFFFE        (previous capture in progress)
 *           raa: + (0xFF00|err)  (start failed)
 *
 * For B-Mode Test
 *============================================================================*/
int Cmd_mab(const ParsedCmd *cmd)
{
    dr_adc_err_t err;
    (void)cmd;

    if (maa_async_is_busy())
    {
        dr_ble_return_1("raa:", 0xFFFE);
        return 1;
    }

    led_set_state(LED_STATE_ALIGN_COMPLETE);

    err = maa_async_start(
        m_config.piezo_freq_option,
        m_config.piezo_delay_us,
        m_config.piezo_num_samples,
        m_config.piezo_cycles,
        m_config.piezo_averaging,
        ble_bin_buffer
    );

    if (err != DR_ADC_OK)
    {
        if (g_plat.log)
        {
            g_plat.log("[Cmd_mab] start failed err=%d\r\n", err);
        }
        single_format_data(ble_bin_buffer, "raa:", (uint16_t)(0xFF00 | err));
        dr_binary_tx_safe(ble_bin_buffer, 3);
        dr_piezo_power_off();
        led_set_state(LED_STATE_OFF);
        return 1;
    }

    maa_async_set_on_complete(mab_led_off_after_piezo);

    return 1;
}

/*==============================================================================
 * mcf? -> rcf:  Read piezo parameters from FDS
 *
 * Request:  [TAG 4B "mcf?"] [CRC 2B]
 * Response: [TAG 4B "rcf:"] [freq 2B] [cycles 2B] [avg 2B] [delay_us 2B] [num_samples 2B] [CRC 2B]
 *============================================================================*/
int Cmd_mcf(const ParsedCmd *cmd)
{
    (void)cmd;

    uint8_t *buf = ble_bin_buffer;
    buf[0] = 'r'; buf[1] = 'c'; buf[2] = 'f'; buf[3] = ':';
    buf[4] = 0;  buf[5] = m_config.piezo_freq_option;
    buf[6] = 0;  buf[7] = m_config.piezo_cycles;
    buf[8]  = (uint8_t)(m_config.piezo_averaging   >> 8); buf[9]  = (uint8_t)(m_config.piezo_averaging   & 0xFF);
    buf[10] = (uint8_t)(m_config.piezo_delay_us    >> 8); buf[11] = (uint8_t)(m_config.piezo_delay_us    & 0xFF);
    buf[12] = (uint8_t)(m_config.piezo_num_samples >> 8); buf[13] = (uint8_t)(m_config.piezo_num_samples & 0xFF);
    dr_binary_tx_safe(buf, 7);  /* 14 bytes = 7 words */
    return 1;
}

/*==============================================================================
 * mcs? -> rcs:  Write piezo parameters to FDS
 *
 * Request:  [TAG 4B "mcs?"] [freq 2B] [cycles 2B] [avg 2B] [delay_us 2B] [num_samples 2B] [CRC 2B]
 * Response: [TAG 4B "rcs:"] [stored 5 values] [CRC 2B]
 * Notes:
 *   - Stored values are clamped before saving
 *   - freq      : 1.8/1.9/2.0/2.1/2.2 MHz only
 *   - cycles    : 3..7
 *   - avg       : 1..10
 *   - delay_us  : 0..50
 *   - samples   : 80..119
 * Error:    rcs: + 0xFFFF  (insufficient data)
 *============================================================================*/
int Cmd_mcs(const ParsedCmd *cmd)
{
    if (cmd->data_len < 10)
    {
        if (g_plat.log)
        {
            g_plat.log("[Cmd_mcs] missing params (data_len=%u)\r\n", cmd->data_len);
        }
        dr_ble_return_1("rcs:", 0xFFFF);
        return 1;
    }

    uint16_t freq, cycles, averaging, delay_us, num_samples;
    dr_get_u16(cmd, 0, &freq);
    dr_get_u16(cmd, 1, &cycles);
    dr_get_u16(cmd, 2, &averaging);
    dr_get_u16(cmd, 3, &delay_us);
    dr_get_u16(cmd, 4, &num_samples);

    m_config.piezo_freq_option = clamp_piezo_freq_option(freq);
    m_config.piezo_cycles      = clamp_piezo_cycles(cycles);
    m_config.piezo_averaging   = clamp_piezo_averaging(averaging);
    m_config.piezo_delay_us    = clamp_piezo_delay_us(delay_us);
    m_config.piezo_num_samples = clamp_piezo_num_samples(num_samples);
    config_save();

    uint8_t *buf = ble_bin_buffer;
    buf[0] = 'r'; buf[1] = 'c'; buf[2] = 's'; buf[3] = ':';
    buf[4] = 0;  buf[5] = m_config.piezo_freq_option;
    buf[6] = 0;  buf[7] = m_config.piezo_cycles;
    buf[8]  = (uint8_t)(m_config.piezo_averaging   >> 8); buf[9]  = (uint8_t)(m_config.piezo_averaging   & 0xFF);
    buf[10] = (uint8_t)(m_config.piezo_delay_us    >> 8); buf[11] = (uint8_t)(m_config.piezo_delay_us    & 0xFF);
    buf[12] = (uint8_t)(m_config.piezo_num_samples >> 8); buf[13] = (uint8_t)(m_config.piezo_num_samples & 0xFF);
    dr_binary_tx_safe(buf, 7);
    return 1;
}
