/*==============================================================================
 * cmd_sensor.c - Sensor measurement handlers
 *
 * msn? -> rsn:  battery ADC measurement
 * mst? -> rso:  IMU die 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"

/*==============================================================================
 * 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:  IMU die temperature
 *
 * Request:  [TAG 4B "mst?"] [CRC 2B]
 * Response: [TAG 4B "rso:"] [temp_x100 2B BE] [CRC 2B]
 *============================================================================*/
int Cmd_mst(const ParsedCmd *cmd)
{
    uint16_t imu_temp_x100 = 0xFFFF;
    (void)cmd;

    if (imu_read_temperature_x100(&imu_temp_x100, NULL) != 0)
    {
        imu_temp_x100 = 0xFFFF;
    }

    single_format_data(ble_bin_buffer, "rso:", imu_temp_x100);
    dr_binary_tx_safe(ble_bin_buffer, 3);

    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 6-axis + IMU 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 + IMU die temperature -> info_imu[6], info_temp */
    info_temp = 0xFFFF;
    hw_i2c_init_once();
    imu_read_direct();

    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 */
}

/*==============================================================================
 * all_sensors_batt_temp() - Battery + temperature only, then short rbb:
 *
 * Emits rbb: [batt 2B] [temp 2B] = 8 bytes = 4 words (no IMU).
 * Not used by mtb? anymore; kept for optional host/tests.
 *
 * Order: battery -> IMU temperature
 * Response: rbb: [batt 2B] [temp 2B] = 8 bytes = 4 words
 *           TX layer appends CRC 2B, so the BLE packet is 10B total.
 *============================================================================*/
void all_sensors_batt_temp(void)
{
    uint8_t *buf;
    uint32_t timeout_cnt;
    uint16_t imu_temp_x100;

    info4 = true;

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

    if (imu_read_temperature_x100(&imu_temp_x100, NULL) == 0)
    {
        info_temp = imu_temp_x100;
    }
    else
    {
        info_temp = 0xFFFF;
    }

    info4 = false;

    static uint8_t rbb_buf[8];
    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);
    buf[6] = (uint8_t)(info_temp >> 8);
    buf[7] = (uint8_t)(info_temp & 0xFF);

    dr_binary_tx_safe(buf, 4);  /* 8 bytes = 4 words, CRC appended by TX layer */
}

void send_imu_rim_fifo(void)
{
    (void)imu_fifo_capture_stop_and_send_rim();
}
