/*******************************************************************************
 * @file    imu_i2c.c
 * @brief   ICM42670P IMU Driver (Zephyr I2C API)

 * I2C 핀: SCL=P1.14, SDA=P1.15 (overlay에서 설정)
 ******************************************************************************/

#include <zephyr/kernel.h>
#include <zephyr/drivers/i2c.h>
#include <zephyr/devicetree.h>
#include <zephyr/sys/util.h>
#include <errno.h>
#include <limits.h>
#include <string.h>

#include "imu_i2c.h"
#include "debug_print.h"

/*==============================================================================
 * 디바이스트리 / I2C 설정
 *============================================================================*/
#define IMU_I2C_NODE    DT_NODELABEL(i2c0)
#define IMU_I2C_ADDR    0x68   // ICM42670P 기본 I2C 주소 (AD0=LOW)

/*==============================================================================
 * ICM42670P 레지스터 주소
 *============================================================================*/
#define REG_PWR_MGMT0     0x1F      // 전원 관리: accel/gyro 동작 모드
#define REG_GYRO_CONFIG0  0x20      // 자이로 FSR + ODR 설정
#define REG_ACCEL_CONFIG0 0x21      // 가속도 FSR + ODR 설정
#define REG_GYRO_CONFIG1  0x23      // 자이로 LN 필터 BW
#define REG_ACCEL_CONFIG1 0x24      // 가속도 LN 필터 BW
#define REG_FIFO_CONFIG1  0x28      // FIFO bypass/mode
#define REG_FIFO_CONFIG2  0x29      // FIFO watermark LSB
#define REG_FIFO_CONFIG3  0x2A      // FIFO watermark MSB
#define REG_INTF_CONFIG0  0x35      // FIFO count/endian 설정
#define REG_FIFO_COUNTH   0x3D      // FIFO count high
#define REG_FIFO_DATA     0x3F      // FIFO data pop
#define REG_BLK_SEL_W     0x79      // MREG write block
#define REG_MADDR_W       0x7A      // MREG write addr
#define REG_M_W           0x7B      // MREG write data
#define REG_BLK_SEL_R     0x7C      // MREG read block
#define REG_MADDR_R       0x7D      // MREG read addr
#define REG_M_R           0x7E      // MREG read data
#define REG_TEMP_DATA1    0x09      // IMU 내부 온도 상위 바이트
#define REG_ACCEL_DATA_X1 0x0B      // 가속도 X축 상위 바이트 (시작 레지스터)
#define REG_WHO_AM_I      0x75      // WHOAMI 레지스터
#define REG_MCLK_RDY      0x00      // MCLK ready
#define REG_SIGNAL_PATH_RESET 0x02  // FIFO flush
#define ICM_WHOAMI        0x67      // ICM42670P 식별값

/* 자이로 스타트업 대기 시간 (ms) — 스펙 최소 45ms, 80ms로 여유 확보 */
#define IMU_GYRO_STARTUP_MS   80
#define IMU_TEMP_AVG_SAMPLES  4U

/* FIFO 설정값: 50Hz, ±4g, ±500dps, Low Noise, 16Hz 필터 */
#define IMU_CFG_50HZ_4G       0x4A
#define IMU_CFG_50HZ_500DPS   0x4A
#define IMU_CFG_LN_BW_16HZ    0x07
#define IMU_PWR_ACCEL_GYRO_LN 0x0F

#define IMU_PWR_IDLE_MASK     BIT(4)
#define IMU_MCLK_RDY_MASK     BIT(3)
#define IMU_FIFO_FLUSH_MASK   BIT(2)

#define IMU_FIFO_CONFIG1_STREAM 0x00
#define IMU_FIFO_CONFIG1_BYPASS 0x01
#define IMU_INTF_FIFO_COUNT_FORMAT_MASK BIT(6)
#define IMU_INTF_FIFO_COUNT_ENDIAN_MASK BIT(5)
#define IMU_INTF_SENSOR_DATA_ENDIAN_MASK BIT(4)
#define IMU_INTF_FIFO_COUNT_RECORD BIT(6)
#define IMU_INTF_FIFO_COUNT_LITTLE_ENDIAN 0x00
#define IMU_INTF_SENSOR_DATA_BIG_ENDIAN BIT(4)
#define IMU_FIFO_CONFIG5_MREG1 0x01
#define IMU_TMST_CONFIG1_MREG1 0x00
#define IMU_FIFO_CONFIG5_ACCEL_GYRO_TMST 0x27
#define IMU_FIFO_PACKET_BYTES  16U
#define IMU_FIFO_MAX_RECORDS   258U
#define IMU_FIFO_READ_BURST_RECORDS 14U
#define IMU_FIFO_ACCEL_OFFSET  1U
#define IMU_FIFO_GYRO_OFFSET   7U
#define IMU_FIFO_INVALID_HEADER 0x80
#define IMU_FIFO_INVALID_AXIS  0x8000

static const struct device *i2c_bus;
static bool fifo_active;
static uint8_t fifo_raw[IMU_FIFO_PACKET_BYTES * IMU_FIFO_MAX_RECORDS];

/* 내부 I2C 래퍼 */

/* 레지스터 1바이트 쓰기: [reg, val] 2바이트 TX */
static int imu_write_reg(uint8_t reg, uint8_t val)
{
    uint8_t buf[2] = { reg, val };
    return i2c_write(i2c_bus, buf, 2, IMU_I2C_ADDR);
}

/* 레지스터 연속 읽기: reg 주소 TX → data RX */
static int imu_read_regs(uint8_t reg, uint8_t *data, uint8_t len)
{
    return i2c_write_read(i2c_bus, IMU_I2C_ADDR, &reg, 1, data, len);
}

static int16_t imu_temp_raw_to_cdeg(int16_t temp_raw)
{
    int32_t cdeg = 2500 + (((int32_t)temp_raw * 100) / 128);

    if (cdeg > INT16_MAX)
    {
        cdeg = INT16_MAX;
    }
    else if (cdeg < INT16_MIN)
    {
        cdeg = INT16_MIN;
    }

    return (int16_t)cdeg;
}

static int imu_read_temp_raw(int16_t *temp_raw)
{
    uint8_t raw[2];
    int ret;

    if (temp_raw == NULL)
    {
        return -EINVAL;
    }

    ret = imu_read_regs(REG_TEMP_DATA1, raw, sizeof(raw));
    if (ret)
    {
        return ret;
    }

    *temp_raw = (int16_t)(((uint16_t)raw[0] << 8) | raw[1]);
    return 0;
}

static int imu_update_reg(uint8_t reg, uint8_t clear_mask, uint8_t set_mask)
{
    uint8_t val;
    int ret = imu_read_regs(reg, &val, 1);

    if (ret)
    {
        return ret;
    }

    val = (uint8_t)((val & ~clear_mask) | set_mask);
    return imu_write_reg(reg, val);
}

static int imu_mclk_on(void)
{
    uint8_t val = 0;
    int ret = imu_update_reg(REG_PWR_MGMT0, 0U, IMU_PWR_IDLE_MASK);

    if (ret)
    {
        return ret;
    }

    for (uint16_t i = 0; i < 1000U; i++)
    {
        ret = imu_read_regs(REG_MCLK_RDY, &val, 1);
        if ((ret == 0) && ((val & IMU_MCLK_RDY_MASK) != 0U))
        {
            return 0;
        }
        k_busy_wait(10);
    }

    return -ETIMEDOUT;
}

static int imu_mclk_off(void)
{
    return imu_update_reg(REG_PWR_MGMT0, IMU_PWR_IDLE_MASK, 0U);
}

static int imu_mreg_write(uint8_t addr, uint8_t val)
{
    int ret = imu_mclk_on();

    if (ret)
    {
        return ret;
    }

    ret = imu_write_reg(REG_BLK_SEL_W, 0U);
    ret |= imu_write_reg(REG_MADDR_W, addr);
    ret |= imu_write_reg(REG_M_W, val);
    k_busy_wait(10);
    ret |= imu_write_reg(REG_BLK_SEL_W, 0U);
    ret |= imu_mclk_off();
    return ret;
}

static int imu_set_data_big_endian(void)
{
    return imu_update_reg(REG_INTF_CONFIG0,
                          IMU_INTF_SENSOR_DATA_ENDIAN_MASK,
                          IMU_INTF_SENSOR_DATA_BIG_ENDIAN);
}

static int imu_configure_fifo_count_format(void)
{
    return imu_update_reg(REG_INTF_CONFIG0,
                          IMU_INTF_FIFO_COUNT_FORMAT_MASK |
                          IMU_INTF_FIFO_COUNT_ENDIAN_MASK |
                          IMU_INTF_SENSOR_DATA_ENDIAN_MASK,
                          IMU_INTF_FIFO_COUNT_RECORD |
                          IMU_INTF_FIFO_COUNT_LITTLE_ENDIAN |
                          IMU_INTF_SENSOR_DATA_BIG_ENDIAN);
}

static int imu_fifo_reset(void)
{
    uint8_t val = IMU_FIFO_FLUSH_MASK;
    int ret = imu_mclk_on();

    if (ret)
    {
        return ret;
    }

    ret = imu_write_reg(REG_SIGNAL_PATH_RESET, val);
    for (uint16_t i = 0; (ret == 0) && (i < 1000U); i++)
    {
        ret = imu_read_regs(REG_SIGNAL_PATH_RESET, &val, 1);
        if ((val & IMU_FIFO_FLUSH_MASK) == 0U)
        {
            break;
        }
        k_busy_wait(10);
    }

    ret |= imu_mclk_off();
    return ret;
}

static int imu_fifo_read_count(uint16_t *count)
{
    uint8_t raw[2] = {0};
    int ret;

    if (count == NULL)
    {
        return -EINVAL;
    }

    ret = imu_mclk_on();
    if (ret)
    {
        return ret;
    }

    ret = imu_read_regs(REG_FIFO_COUNTH, raw, sizeof(raw));
    ret |= imu_mclk_off();

    if (ret)
    {
        return ret;
    }

    *count = (uint16_t)raw[0] | ((uint16_t)raw[1] << 8);
    if (*count > IMU_FIFO_MAX_RECORDS)
    {
        *count = IMU_FIFO_MAX_RECORDS;
    }

    return 0;
}

static int imu_fifo_read_records(uint16_t record_count)
{
    uint16_t record_idx = 0;
    int ret = imu_mclk_on();

    if (ret)
    {
        return ret;
    }

    while ((record_idx < record_count) && (ret == 0))
    {
        uint16_t burst_records = record_count - record_idx;
        uint16_t burst_bytes;

        if (burst_records > IMU_FIFO_READ_BURST_RECORDS)
        {
            burst_records = IMU_FIFO_READ_BURST_RECORDS;
        }

        burst_bytes = burst_records * IMU_FIFO_PACKET_BYTES;
        ret = imu_read_regs(REG_FIFO_DATA,
                            &fifo_raw[record_idx * IMU_FIFO_PACKET_BYTES],
                            (uint8_t)burst_bytes);
        record_idx = (uint16_t)(record_idx + burst_records);
    }

    ret |= imu_mclk_off();
    return ret;
}

static bool imu_fifo_record_is_placeholder(const uint8_t *rec)
{
    if (rec[0] != IMU_FIFO_INVALID_HEADER)
    {
        return false;
    }

    for (uint8_t i = 1U; i < IMU_FIFO_PACKET_BYTES; i++)
    {
        if (rec[i] != 0U)
        {
            return false;
        }
    }

    return true;
}

static int16_t imu_fifo_axis_be(const uint8_t *rec, uint8_t offset)
{
    return (int16_t)(((uint16_t)rec[offset] << 8) | rec[offset + 1U]);
}

static bool imu_fifo_record_has_valid_gyro(const uint8_t *rec)
{
    for (uint8_t axis = 0U; axis < 3U; axis++)
    {
        if (imu_fifo_axis_be(rec, (uint8_t)(IMU_FIFO_GYRO_OFFSET + axis * 2U)) !=
            (int16_t)IMU_FIFO_INVALID_AXIS)
        {
            return true;
        }
    }

    return false;
}

static uint16_t imu_fifo_compact_records(uint16_t record_count)
{
    uint16_t write_idx = 0;

    for (uint16_t read_idx = 0; read_idx < record_count; read_idx++)
    {
        uint8_t *rec = &fifo_raw[read_idx * IMU_FIFO_PACKET_BYTES];

        if (imu_fifo_record_is_placeholder(rec) || !imu_fifo_record_has_valid_gyro(rec))
        {
            continue;
        }

        if (write_idx != read_idx)
        {
            memcpy(&fifo_raw[write_idx * IMU_FIFO_PACKET_BYTES], rec, IMU_FIFO_PACKET_BYTES);
        }
        write_idx++;
    }

    return write_idx;
}

/* 공개 API */

int imu_init(void)
{
    i2c_bus = DEVICE_DT_GET(IMU_I2C_NODE);
    if (!device_is_ready(i2c_bus)) {
        DBG_PRINTF("[IMU] FAIL — I2C bus not ready\r\n");
        return -1;
    }

    uint8_t who_am_i = 0;
    if (imu_read_regs(REG_WHO_AM_I, &who_am_i, 1) != 0) {
        DBG_PRINTF("[IMU] FAIL — WHOAMI read error (check SCL=P1.14, SDA=P1.15)\r\n");
        return -2;
    }

    if (who_am_i != ICM_WHOAMI) {
        DBG_PRINTF("[IMU] FAIL — WHOAMI mismatch (got=0x%02X, expected=0x%02X)\r\n", who_am_i, ICM_WHOAMI);
        return -3;
    }

    //DBG_PRINTF("[IMU] OK — ICM42670P detected (WHOAMI=0x%02X, addr=0x%02X)\r\n", who_am_i, IMU_I2C_ADDR);
    return 0;
}

int imu_read(int16_t accel[3], int16_t gyro[3])
{
    uint8_t raw[12];
    int ret;

    // 자이로 설정: ±2000dps FSR, 100Hz ODR
    ret = imu_write_reg(REG_GYRO_CONFIG0, 0x09);
    if (ret) { DBG_PRINTF("[IMU] FAIL — gyro config write (ret=%d)\r\n", ret); return -1; }

    // 가속도 설정: ±4g FSR, 100Hz ODR
    ret = imu_write_reg(REG_ACCEL_CONFIG0, 0x29);
    if (ret) { DBG_PRINTF("[IMU] FAIL — accel config write (ret=%d)\r\n", ret); return -1; }

    ret = imu_set_data_big_endian();
    if (ret) { DBG_PRINTF("[IMU] FAIL — endian config write (ret=%d)\r\n", ret); return -1; }

    // 전원 ON: accel(저잡음) + gyro(저잡음) 모두 활성화
    ret = imu_write_reg(REG_PWR_MGMT0, 0x0F);
    if (ret) { DBG_PRINTF("[IMU] FAIL — power on write (ret=%d)\r\n", ret); return -1; }

    // 자이로 스타트업 대기
    k_msleep(IMU_GYRO_STARTUP_MS);

    // ACCEL_DATA_X1(0x0B)부터 12바이트 연속 읽기
    // [0..5]  = accel X,Y,Z (MSB first)
    // [6..11] = gyro  X,Y,Z (MSB first)
    ret = imu_read_regs(REG_ACCEL_DATA_X1, raw, 12);
    if (ret) { DBG_PRINTF("[IMU] FAIL — data read (ret=%d)\r\n", ret); return -2; }

    // 빅엔디안 → int16_t 변환
    accel[0] = (int16_t)((raw[0]  << 8) | raw[1]);
    accel[1] = (int16_t)((raw[2]  << 8) | raw[3]);
    accel[2] = (int16_t)((raw[4]  << 8) | raw[5]);
    gyro[0]  = (int16_t)((raw[6]  << 8) | raw[7]);
    gyro[1]  = (int16_t)((raw[8]  << 8) | raw[9]);
    gyro[2]  = (int16_t)((raw[10] << 8) | raw[11]);

    //DBG_PRINTF("[IMU] msp: A=(%6d,%6d,%6d) G=(%6d,%6d,%6d)\r\n", accel[0], accel[1], accel[2], gyro[0],  gyro[1],  gyro[2]);

    // 슬립 모드: 전력 절감
    imu_write_reg(REG_PWR_MGMT0, 0x00);

    return 0;
}

int imu_read_with_temperature(int16_t accel[3], int16_t gyro[3], int16_t *temp_cdeg)
{
    uint8_t raw[14];
    int16_t temp_raw;
    int16_t temp_samples[IMU_TEMP_AVG_SAMPLES];
    int32_t temp_sum;
    int ret;

    if ((accel == NULL) || (gyro == NULL) || (temp_cdeg == NULL))
    {
        return -EINVAL;
    }

    ret = imu_write_reg(REG_GYRO_CONFIG0, 0x09);
    if (ret) { DBG_PRINTF("[IMU] FAIL — gyro config write (ret=%d)\r\n", ret); return -1; }

    ret = imu_write_reg(REG_ACCEL_CONFIG0, 0x29);
    if (ret) { DBG_PRINTF("[IMU] FAIL — accel config write (ret=%d)\r\n", ret); return -1; }

    ret = imu_set_data_big_endian();
    if (ret) { DBG_PRINTF("[IMU] FAIL — endian config write (ret=%d)\r\n", ret); return -1; }

    ret = imu_write_reg(REG_PWR_MGMT0, 0x0F);
    if (ret) { DBG_PRINTF("[IMU] FAIL — power on write (ret=%d)\r\n", ret); return -1; }

    k_msleep(IMU_GYRO_STARTUP_MS);

    ret = imu_read_regs(REG_TEMP_DATA1, raw, sizeof(raw));
    if (ret) { DBG_PRINTF("[IMU] FAIL — temp/data read (ret=%d)\r\n", ret); return -2; }

    temp_raw = (int16_t)(((uint16_t)raw[0] << 8) | raw[1]);
    temp_sum = temp_raw;
    temp_samples[0] = temp_raw;
    for (uint8_t i = 1U; i < IMU_TEMP_AVG_SAMPLES; i++)
    {
        int16_t next_temp_raw;

        ret = imu_read_temp_raw(&next_temp_raw);
        if (ret)
        {
            DBG_PRINTF("[IMU] temp avg read fail ret=%d idx=%u\r\n", ret, i);
            return -3;
        }
        temp_samples[i] = next_temp_raw;
        temp_sum += next_temp_raw;
    }

    temp_raw = (int16_t)(temp_sum / (int32_t)IMU_TEMP_AVG_SAMPLES);
    *temp_cdeg = imu_temp_raw_to_cdeg(temp_raw);
    accel[0] = (int16_t)((raw[2]  << 8) | raw[3]);
    accel[1] = (int16_t)((raw[4]  << 8) | raw[5]);
    accel[2] = (int16_t)((raw[6]  << 8) | raw[7]);
    gyro[0]  = (int16_t)((raw[8]  << 8) | raw[9]);
    gyro[1]  = (int16_t)((raw[10] << 8) | raw[11]);
    gyro[2]  = (int16_t)((raw[12] << 8) | raw[13]);

    /*
    DBG_PRINTF("[IMU] mbb: A=(%6d,%6d,%6d) G=(%6d,%6d,%6d) T=%d.%02d C temp_raw=%d/%d/%d/%d avg=%d first=0x%02X%02X\r\n",
               accel[0], accel[1], accel[2],
               gyro[0], gyro[1], gyro[2],
               *temp_cdeg / 100,
               (*temp_cdeg < 0 ? -*temp_cdeg : *temp_cdeg) % 100,
               temp_samples[0],
               temp_samples[1],
               temp_samples[2],
               temp_samples[3],
               temp_raw,
               raw[0], raw[1]);
    */
    imu_write_reg(REG_PWR_MGMT0, 0x00);
    return 0;
}

int imu_read_temperature_cdeg(int16_t *temp_cdeg)
{
    int16_t temp_raw;
    int32_t temp_sum = 0;
    int32_t cdeg;
    int ret;
    bool was_fifo_active = fifo_active;

    if (temp_cdeg == NULL)
    {
        return -EINVAL;
    }

    ret = imu_init();
    if (ret)
    {
        return ret;
    }

    if (!was_fifo_active)
    {
        ret = imu_write_reg(REG_PWR_MGMT0, IMU_PWR_ACCEL_GYRO_LN);
        if (ret)
        {
            DBG_PRINTF("[IMU] temp power on fail ret=%d\r\n", ret);
            return ret;
        }
        k_msleep(IMU_GYRO_STARTUP_MS);
    }

    for (uint8_t i = 0U; i < IMU_TEMP_AVG_SAMPLES; i++)
    {
        ret = imu_read_temp_raw(&temp_raw);
        if (ret)
        {
            DBG_PRINTF("[IMU] temp read fail ret=%d idx=%u\r\n", ret, i);
            if (!was_fifo_active)
            {
                (void)imu_write_reg(REG_PWR_MGMT0, 0x00);
            }
            return ret;
        }

        temp_sum += temp_raw;
    }

    temp_raw = (int16_t)(temp_sum / (int32_t)IMU_TEMP_AVG_SAMPLES);
    cdeg = imu_temp_raw_to_cdeg(temp_raw);
    *temp_cdeg = (int16_t)cdeg;
    /*
    DBG_PRINTF("[IMU] temp=%d.%02d C raw=%d\r\n",
               *temp_cdeg / 100,
               (*temp_cdeg < 0 ? -*temp_cdeg : *temp_cdeg) % 100,
               temp_raw);
    */
    if (!was_fifo_active)
    {
        (void)imu_write_reg(REG_PWR_MGMT0, 0x00);
    }

    return 0;
}

int imu_fifo_start(void)
{
    int ret = imu_init();

    if (ret)
    {
        return ret;
    }

    if (fifo_active)
    {
        DBG_PRINTF("[IMU FIFO] already running\r\n");
        return 0;
    }

    ret = imu_write_reg(REG_PWR_MGMT0, 0x00);
    ret |= imu_write_reg(REG_GYRO_CONFIG0, IMU_CFG_50HZ_500DPS);
    ret |= imu_write_reg(REG_ACCEL_CONFIG0, IMU_CFG_50HZ_4G);
    ret |= imu_write_reg(REG_GYRO_CONFIG1, IMU_CFG_LN_BW_16HZ);
    ret |= imu_write_reg(REG_ACCEL_CONFIG1, IMU_CFG_LN_BW_16HZ);
    ret |= imu_configure_fifo_count_format();
    ret |= imu_write_reg(REG_FIFO_CONFIG1, IMU_FIFO_CONFIG1_STREAM);
    ret |= imu_write_reg(REG_FIFO_CONFIG2, 0x01);
    ret |= imu_write_reg(REG_FIFO_CONFIG3, 0x00);
    ret |= imu_mreg_write(IMU_TMST_CONFIG1_MREG1, 0x01);
    ret |= imu_mreg_write(IMU_FIFO_CONFIG5_MREG1, IMU_FIFO_CONFIG5_ACCEL_GYRO_TMST);
    ret |= imu_fifo_reset();
    ret |= imu_write_reg(REG_PWR_MGMT0, IMU_PWR_ACCEL_GYRO_LN);

    k_msleep(IMU_GYRO_STARTUP_MS);
    ret |= imu_fifo_reset();

    if (ret)
    {
        DBG_PRINTF("[IMU FIFO] start fail ret=%d\r\n", ret);
        (void)imu_write_reg(REG_FIFO_CONFIG1, IMU_FIFO_CONFIG1_BYPASS);
        (void)imu_write_reg(REG_PWR_MGMT0, 0x00);
        return ret;
    }

    fifo_active = true;
    DBG_PRINTF("[IMU FIFO] start 50Hz LN\r\n");
    return 0;
}

int imu_fifo_wait_samples(uint16_t target_samples, uint32_t timeout_ms)
{
    uint32_t waited_ms = 0U;
    const uint32_t poll_ms = 10U;

    if (target_samples == 0U)
    {
        return 0;
    }

    if (!fifo_active)
    {
        return -EALREADY;
    }

    while (waited_ms <= timeout_ms)
    {
        uint16_t record_count = 0U;
        int ret = imu_fifo_read_count(&record_count);

        if (ret)
        {
            return ret;
        }

        if (record_count >= target_samples)
        {
            return 0;
        }

        k_msleep(poll_ms);
        waited_ms += poll_ms;
    }

    return -ETIMEDOUT;
}

int imu_fifo_read_latest(uint8_t *sample_bytes,
                         uint16_t max_samples,
                         uint16_t *out_count)
{
    uint16_t record_count = 0;
    uint16_t start_idx = 0;
    int ret;

    if ((sample_bytes == NULL) || (out_count == NULL))
    {
        return -EINVAL;
    }

    *out_count = 0U;

    if (!fifo_active)
    {
        return -EALREADY;
    }

    ret = imu_fifo_read_count(&record_count);
    //DBG_PRINTF("[IMU FIFO] raw count=%u ret=%d target=%u\r\n", record_count, ret, max_samples);
    if ((ret == 0) && (record_count > 0U))
    {
        ret = imu_fifo_read_records(record_count);
        //DBG_PRINTF("[IMU FIFO] read records ret=%d\r\n", ret);
    }

    if (ret)
    {
        DBG_PRINTF("[IMU FIFO] drain fail ret=%d count=%u\r\n", ret, record_count);
        return ret;
    }

    {
        record_count = imu_fifo_compact_records(record_count);
        DBG_PRINTF("[IMU FIFO] compact %u -> %u\r\n", raw_count, record_count);
    }
    if (record_count > max_samples)
    {
        start_idx = (uint16_t)(record_count - max_samples);
        record_count = max_samples;
    }

    for (uint16_t i = 0; i < record_count; i++)
    {
        const uint8_t *rec = &fifo_raw[(start_idx + i) * IMU_FIFO_PACKET_BYTES];
        uint8_t *dst = &sample_bytes[i * IMU_FIFO_SAMPLE_BYTES];

        memcpy(dst, &rec[IMU_FIFO_ACCEL_OFFSET], 6U);
        memcpy(&dst[6], &rec[IMU_FIFO_GYRO_OFFSET], 6U);
    }

    *out_count = record_count;
    //DBG_PRINTF("[IMU FIFO] drained samples=%u\r\n", record_count);
    return 0;
}

bool imu_fifo_is_active(void)
{
    return fifo_active;
}

int imu_fifo_stop(void)
{
    int ret = 0;

    if (!fifo_active)
    {
        return 0;
    }

    ret |= imu_write_reg(REG_FIFO_CONFIG1, IMU_FIFO_CONFIG1_BYPASS);
    ret |= imu_mreg_write(IMU_FIFO_CONFIG5_MREG1, 0x00);
    ret |= imu_write_reg(REG_PWR_MGMT0, 0x00);
    fifo_active = false;

    DBG_PRINTF("[IMU FIFO] stop ret=%d\r\n", ret);
    return ret;
}