/*==============================================================================
 * cmd_sensor.c - Sensor measurement handlers
 *
 * msn? -> rsn:  battery ADC measurement
 * mso? -> rso:  TMP235 temperature reading
 * msp? -> rsp:  IMU 6-axis single read
 * all_sensors()  bulk-measurement helper used by the mbb? handler
 *============================================================================*/

#include "cmd_common.h"
#include "cmd_sensor.h"
#include "dr_piezo.h"

/*==============================================================================
 * msn? -> rsn:  Battery level ADC measurement
 *
 * Request:  [TAG 4B "msn?"] [CRC 2B]
 * Response: rsn: + battery voltage in mV (sent from battery_saadc.c callback)
 *============================================================================*/
int Cmd_msn(const ParsedCmd *cmd)
{
    (void)cmd;
    battery_level_meas();
    return 1;
}

/*==============================================================================
 * mst? -> rso:  Temperature with piezo power cycle
 *
 * Request:  [TAG 4B "mst?"] [CRC 2B]
 * Response: [TAG 4B "rso:"] [temp_x100 2B BE] [CRC 2B]
 *
 * TMP235 shares the piezo TX/RX power rail. This command handles the full
 * sequence: power ON -> measure -> power OFF, so the caller doesn't need
 * to send mpa?/mpb? separately.
 * Response is sent from the TMP235 SAADC callback (tmp235_voltage_handler).
 *============================================================================*/
int Cmd_mst(const ParsedCmd *cmd)
{
    uint32_t timeout_cnt;
    (void)cmd;

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

    tmp235_saadc_done = false;
    tmp235_voltage_level_meas();
    for (timeout_cnt = 0; !tmp235_saadc_done && timeout_cnt < 100; timeout_cnt++)
    {
        dr_sd_delay_ms(1);
    }

    dr_piezo_power_off();

    return 1;
}

/*==============================================================================
 * msp? -> rsp:  IMU 6-axis single read
 *
 * Request:  [TAG 4B "msp?"] [CRC 2B]
 * Response: rsp: + accel(xyz) + gyro(xyz)  (transmitted inside imu_read_direct)
 *
 * Reads IMU registers directly over I2C and transmits over BLE. Synchronous.
 *============================================================================*/
int Cmd_msp(const ParsedCmd *cmd)
{
    (void)cmd;
    hw_i2c_init_once();
    imu_read_direct();
    return 1;
}

/*==============================================================================
 * all_sensors() - Bulk-measurement helper for the mbb? handler
 *
 * Stores sensor values into globals via info4 mode, then transmits them as a
 * single rbb: packet. SAADC measurements run asynchronously (callback), so
 * dr_sd_delay_ms() is used to wait for completion.
 *
 * Order: battery -> IMU -> (Piezo TX/RX ON) -> temperature
 * Response: rbb: [batt 2B] [IMU 6x2B] [temp 2B] = 20 bytes = 10 words
 *============================================================================*/
void all_sensors(void)
{
    uint8_t *buf;
    uint32_t timeout_cnt;

    info4 = true;

    /* 1. Battery voltage -> info_batt */
    battery_saadc_done = false;
    battery_level_meas();
    for (timeout_cnt = 0; !battery_saadc_done && timeout_cnt < 100; timeout_cnt++)
    {
        dr_sd_delay_ms(1);
    }

    /* 2. IMU 6-axis single read -> info_imu[6] */
    hw_i2c_init_once();
    imu_read_direct();

    /* 3. Temperature -> info_temp  (TMP235 needs Piezo TX/RX power) */
    if (!dr_piezo_is_power_on())
    {
        dr_piezo_power_on();
    }

    tmp235_saadc_done = false;
    tmp235_voltage_level_meas();
    for (timeout_cnt = 0; !tmp235_saadc_done && timeout_cnt < 100; timeout_cnt++)
    {
        dr_sd_delay_ms(1);
    }

    info4 = false;

    /* Assemble and transmit the rbb: packet (dedicated buffer to avoid
       collision with ble_bin_buffer used by the async ADC state machine) */
    static uint8_t rbb_buf[20];
    buf = rbb_buf;
    buf[0] = 'r'; buf[1] = 'b'; buf[2] = 'b'; buf[3] = ':';
    buf[4] = (uint8_t)(info_batt >> 8);
    buf[5] = (uint8_t)(info_batt & 0xFF);

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

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

    dr_binary_tx_safe(buf, 10);  /* 20 bytes = 10 words */
}
