/*******************************************************************************
 * @file	app_raw.c
 * @author	CandyPops Co.
 * @version	V1.0.0
 * @date	2022-09-05
 * @brief
 ******************************************************************************/

/*******************************************************************************
 * [Module overview] ICM42670P IMU driver application layer
 *
 * Application layer module responsible for initialization, configuration,
 * and data reading of the ICM42670P IMU sensor. Wraps InvenSense driver API.
 *
 * Key functions:
 *   1) setup_imu_device()     - IMU init and WHOAMI verification (0x67 = ICM42670P)
 *   2) configure_imu_device() - Sensor parameter configuration
 *      - Accelerometer: +/-4g FSR, 100Hz (low-power) or 800Hz (low-noise)
 *      - Gyroscope:     +/-2000dps FSR, 100Hz or 800Hz
 *      - FIFO disabled (direct register read mode)
 *   3) get_imu_data()         - Read sensor data from FIFO or registers
 *   4) imu_callback()         - Sensor data receive callback
 *      - Keeps raw sensor axes (ICM42670P datasheet coordinate system)
 *      - info4 mode: stores data in info_imu[6]
 *      - BLE mode:   sends 6-axis data via BLE with "rsp:" tag
 *      - UART mode:  outputs text format to serial
 *   5) imu_read_direct()      - Direct I2C register read bypassing driver API
 *      - Configure sensor -> power ON -> wait 80ms -> read temp + 6-axis -> sleep
 *
 * Axis convention:
 *   Raw ICM42670P datasheet X/Y/Z axes. No mounting matrix is applied.
 ******************************************************************************/

#include "sdk_config.h"
#include "app_raw.h"
#include "inv_imu_extfunc.h"
#include "inv_imu_driver.h"
#include "inv_imu_transport.h"
#include "ble_nus.h"
#include "nrf_log.h"
#include "nrf_log_ctrl.h"
#include "nrf_log_default_backends.h"

#include "app_util_platform.h"
#include "main.h"
#include "debug_print.h"
#include "nrf_delay.h"
#include <string.h>


/*
 * SCALED_DATA_G_DPS : Data output format selection
 * 0 : Raw data output (raw accel, gyro, temp)
 * 1 : Scaled data output (g, dps, Celsius)
 */
#define SCALED_DATA_G_DPS 0


/* --------------------------------------------------------------------------------------
 *  Static and extern variables
 * -------------------------------------------------------------------------------------- */

/* IMU driver object — always passed to driver API calls */
static struct inv_imu_device icm_driver;

/* Binary buffer for BLE transmission */
 uint8_t imu_bin_buffer[BLE_NUS_MAX_DATA_LEN] = {0};

bool custom_add_data;                              		/* Custom data append flag (BLE transmission control) */
extern bool motion_raw_data_enabled;               		/* Flag requesting raw data read from external module */
extern char		ble_tx_buffer[BLE_NUS_MAX_DATA_LEN]; 	/* BLE text transmit buffer */
extern which_cmd_t cmd_type_t;                     		/* Current command source (BLE or UART) */
uint16_t ssp_data[6]={0,};                         		/* 6-axis data array for BLE (accel XYZ + gyro XYZ) */
extern bool info4;                                 		/* info4 mode flag (set by cmd_parse) */
 volatile uint16_t info_imu[6];                    		/* Global array storing IMU data in info4 mode */
volatile uint16_t info_temp;                		/* Global temperature cache (deg C x 100) */

/* --------------------------------------------------------------------------------------
 *  Functions definition
 * -------------------------------------------------------------------------------------- */

/*
 * setup_imu_device()
 * IMU device initialization and identification verification.
 *
 * Flow:
 *   1) Initialize driver via inv_imu_init() (serial interface + callback registration)
 *   2) Read WHOAMI register for device identification
 *   3) Verify WHOAMI value matches ICM_WHOAMI (0x67)
 *
 * Returns: 0=success, negative=error
 */
int setup_imu_device(struct inv_imu_serif *icm_serif)
{
	int rc = 0;
	uint8_t who_am_i;

	/* Initialize IMU driver — connect serial interface and register callback */
	rc = inv_imu_init(&icm_driver, icm_serif, imu_callback);
	if (rc != INV_ERROR_SUCCESS) {
		DBG_PRINTF("!!! ERROR : Failed to initialize IMU!\r\n");
		return rc;
	}

	/* Read WHOAMI register — verify device presence and communication */
	rc = inv_imu_get_who_am_i(&icm_driver, &who_am_i);
	if (rc != INV_ERROR_SUCCESS) {
		DBG_PRINTF("!!! ERROR : Failed to read whoami!\r\n");
		return rc;
	}

	/* Verify WHOAMI value — must be 0x67 for ICM42670P */
	if (who_am_i != ICM_WHOAMI) {
		DBG_PRINTF("!!! ERROR : Bad WHOAMI value! Read 0x%02x, expected 0x%02x\r\n", who_am_i, ICM_WHOAMI);
		return INV_ERROR;
	}

	return rc;
}

/*
 * configure_imu_device()
 * Configures IMU sensor operating parameters.
 *
 * Settings:
 *   - FIFO: disabled (when USE_FIFO=0, direct register read mode)
 *   - Accel FSR: +/-4g (when USE_HIGH_RES_MODE=0)
 *   - Gyro FSR: +/-2000dps
 *   - ODR (output data rate):
 *     - Low-noise mode (USE_LOW_NOISE_MODE=1): 800Hz
 *     - Low-power mode (USE_LOW_NOISE_MODE=0): 100Hz
 *   - Gyro always operates in low-noise mode
 *   - Waits for gyro startup time when FIFO is not used
 *
 * Returns: 0=success, negative=error
 */
int configure_imu_device(void)
{
	int rc = 0;

	/* Disable FIFO — read data directly from registers */
	if (!USE_FIFO)
		rc |= inv_imu_configure_fifo(&icm_driver, INV_IMU_FIFO_DISABLED);

	if (USE_HIGH_RES_MODE) {
		/* High-resolution FIFO mode: 20-bit data, FSR locked to 16g/2000dps */
		rc |= inv_imu_enable_high_resolution_fifo(&icm_driver);
	} else {
		/* Standard mode: accel +/-4g, gyro +/-500dps FSR */
		rc |= inv_imu_set_accel_fsr(&icm_driver, ACCEL_CONFIG0_FS_SEL_4g);
		rc |= inv_imu_set_gyro_fsr(&icm_driver, GYRO_CONFIG0_FS_SEL_500dps);
	}

	if (USE_LOW_NOISE_MODE) {
		/* Low-noise mode: 800Hz ODR, enable accel low-noise mode */
		rc |= inv_imu_set_accel_frequency(&icm_driver, ACCEL_CONFIG0_ODR_800_HZ);
		rc |= inv_imu_set_gyro_frequency(&icm_driver, GYRO_CONFIG0_ODR_800_HZ);
		rc |= inv_imu_enable_accel_low_noise_mode(&icm_driver);
	} else {
		/* Low-power mode: 100Hz ODR, enable accel low-power mode */
		rc |= inv_imu_set_accel_frequency(&icm_driver, ACCEL_CONFIG0_ODR_100_HZ);
		rc |= inv_imu_set_gyro_frequency(&icm_driver, GYRO_CONFIG0_ODR_100_HZ);
		rc |= inv_imu_enable_accel_low_power_mode(&icm_driver);
	}

	/* Gyro always operates in low-noise mode regardless of setting */
	rc |= inv_imu_enable_gyro_low_noise_mode(&icm_driver);

	/* When FIFO is not used, wait for gyro startup time (delay until first valid data) */
	if (!USE_FIFO)
		inv_imu_sleep_us(GYR_STARTUP_TIME_US);

	return rc;
}


/*
 * get_imu_data()
 * Reads sensor data from the IMU.
 * Fetches data from FIFO or registers depending on USE_FIFO setting.
 * Read data is processed via imu_callback().
 */
int get_imu_data(void)
{
#if USE_FIFO
	return inv_imu_get_data_from_fifo(&icm_driver);
#else

	return inv_imu_get_data_from_registers(&icm_driver);
#endif
}

#if SCALED_DATA_G_DPS
/*
 * get_accel_and_gyr_fsr()
 * Retrieves the currently configured FSR (Full Scale Range) for accel and gyro.
 * Used for converting to scaled data (g, dps).
 */
static void get_accel_and_gyr_fsr(int16_t * accel_fsr_g, int16_t * gyro_fsr_dps)
{
	ACCEL_CONFIG0_FS_SEL_t accel_fsr_bitfield;
	GYRO_CONFIG0_FS_SEL_t gyro_fsr_bitfield;

	inv_imu_get_accel_fsr(&icm_driver, &accel_fsr_bitfield);
	switch(accel_fsr_bitfield) {
	case ACCEL_CONFIG0_FS_SEL_2g:   *accel_fsr_g = 2;
	break;
	case ACCEL_CONFIG0_FS_SEL_4g:   *accel_fsr_g = 4;
	break;
	case ACCEL_CONFIG0_FS_SEL_8g:   *accel_fsr_g = 8;
	break;
	case ACCEL_CONFIG0_FS_SEL_16g:  *accel_fsr_g = 16;
	break;
	default:                        *accel_fsr_g = -1;
	}

	inv_imu_get_gyro_fsr(&icm_driver, &gyro_fsr_bitfield);
	switch(gyro_fsr_bitfield) {
	case GYRO_CONFIG0_FS_SEL_250dps:  *gyro_fsr_dps = 250;
	break;
	case GYRO_CONFIG0_FS_SEL_500dps:  *gyro_fsr_dps = 500;
	break;
	case GYRO_CONFIG0_FS_SEL_1000dps: *gyro_fsr_dps = 1000;
	break;
	case GYRO_CONFIG0_FS_SEL_2000dps: *gyro_fsr_dps = 2000;
	break;
	default:                          *gyro_fsr_dps = -1;
	}
}
#endif


/*
 * imu_callback()
 * Callback invoked each time the IMU driver reads new sensor data.
 *
 * Flow:
 *   1) Extract raw accel/gyro data from event
 *      - FIFO mode: handles timestamp rollover, supports high-res (20-bit)
 *      - Register mode: uses 16-bit data directly
 *   2) Keep raw ICM42670P datasheet axes
 *   3) Output data (branches by mode):
 *      - info4 mode: stores in info_imu[6] global array (polled externally)
 *      - UART mode: text output with "Tp" prefix for 6-axis data
 *      - BLE mode:  binary packet with "rsp:" tag + simultaneous UART output
 */
void imu_callback(inv_imu_sensor_event_t *event)
{
	int32_t accel[3], gyro[3];

#if SCALED_DATA_G_DPS
	float accel_g[3];
	float gyro_dps[3];
	float temp_degc;
	int16_t accel_fsr_g, gyro_fsr_dps;
#endif

#if USE_FIFO
	static uint64_t last_fifo_timestamp = 0;
	static uint32_t rollover_num = 0;

	/* FIFO timestamp rollover handling (16-bit -> 64-bit extension) */
	if (last_fifo_timestamp > event->timestamp_fsync)
		rollover_num++;
	last_fifo_timestamp = event->timestamp_fsync;

	/* Convert timestamp to microseconds (apply Q24 resolution) */
	timestamp = event->timestamp_fsync + rollover_num * UINT16_MAX;
	timestamp *= inv_imu_get_fifo_timestamp_resolution_us_q24(&icm_driver);
	timestamp /= (1UL << 24);

	if (icm_driver.fifo_highres_enabled) {
		/* High-res mode: left-shift 16-bit data by 4 + add lower 4 bits -> 20-bit */
		accel[0] = (((int32_t)event->accel[0] << 4)) | event->accel_high_res[0];
		accel[1] = (((int32_t)event->accel[1] << 4)) | event->accel_high_res[1];
		accel[2] = (((int32_t)event->accel[2] << 4)) | event->accel_high_res[2];

		gyro[0] = (((int32_t)event->gyro[0] << 4)) | event->gyro_high_res[0];
		gyro[1] = (((int32_t)event->gyro[1] << 4)) | event->gyro_high_res[1];
		gyro[2] = (((int32_t)event->gyro[2] << 4)) | event->gyro_high_res[2];

	} else {
		/* Standard resolution: use 16-bit data as-is */
		accel[0] = event->accel[0];
		accel[1] = event->accel[1];
		accel[2] = event->accel[2];

		gyro[0] = event->gyro[0];
		gyro[1] = event->gyro[1];
		gyro[2] = event->gyro[2];
	}
#else

	/* Direct register read mode: extract 16-bit raw data */
	accel[0] = event->accel[0];
	accel[1] = event->accel[1];
	accel[2] = event->accel[2];

	gyro[0] = event->gyro[0];
	gyro[1] = event->gyro[1];
	gyro[2] = event->gyro[2];

	/* In register mode, force sensor mask so the output logic below works */
	event->sensor_mask |= (1 << INV_SENSOR_TEMPERATURE);
	event->sensor_mask |= (1 << INV_SENSOR_ACCEL);
	event->sensor_mask |= (1 << INV_SENSOR_GYRO);
#endif

#if SCALED_DATA_G_DPS
	/*
	 * Convert raw data to physical units (g, dps)
	 * Formula: physical_value = raw_value * FSR / INT16_MAX
	*/
	get_accel_and_gyr_fsr(&accel_fsr_g, &gyro_fsr_dps);
	accel_g[0]  = (float)(accel[0] * accel_fsr_g)  / INT16_MAX;
	accel_g[1]  = (float)(accel[1] * accel_fsr_g)  / INT16_MAX;
	accel_g[2]  = (float)(accel[2] * accel_fsr_g)  / INT16_MAX;
	gyro_dps[0] = (float)(gyro[0]  * gyro_fsr_dps) / INT16_MAX;
	gyro_dps[1] = (float)(gyro[1]  * gyro_fsr_dps) / INT16_MAX;
	gyro_dps[2] = (float)(gyro[2]  * gyro_fsr_dps) / INT16_MAX;

	/* Temperature conversion: high-res/register mode uses /128, FIFO standard mode uses /2 */
	if (USE_HIGH_RES_MODE || !USE_FIFO)
		temp_degc = 25 + ((float)event->temperature / 128);
	else
		temp_degc = 25 + ((float)event->temperature / 2);

	(void)timestamp;
	(void)accel_g;
	(void)gyro_dps;
	(void)temp_degc;
#else

	/*
	 * Raw data output — branches by command source (info4/UART/BLE)
	 */
	if (event->sensor_mask & (1 << INV_SENSOR_ACCEL) && event->sensor_mask & (1 << INV_SENSOR_GYRO) || motion_raw_data_enabled)
	{
		motion_raw_data_enabled = false;

		/* info4 mode: store data in global array info_imu[6], polled by external modules */
		if (info4 == true)
		{
			info_imu[0] = (uint16_t)accel[0];
			info_imu[1] = (uint16_t)accel[1];
			info_imu[2] = (uint16_t)accel[2];
			info_imu[3] = (uint16_t)gyro[0];
			info_imu[4] = (uint16_t)gyro[1];
			info_imu[5] = (uint16_t)gyro[2];
		}

		/*
			* BLE mode: send 6-axis data as binary packet via BLE
			* ssp_data[0..2] = accel XYZ, ssp_data[3..5] = gyro XYZ
			* format_data() packs "rsp:" tag + 12-byte data
			* dr_binary_tx_safe() sends 8 bytes via BLE
		*/
		else if(cmd_type_t == CMD_BLE) {
			ssp_data[0] = (uint16_t)accel[0];
			ssp_data[1] = (uint16_t)accel[1];
			ssp_data[2] = (uint16_t)accel[2];
			ssp_data[3] = (uint16_t)gyro[0];
			ssp_data[4] = (uint16_t)gyro[1];
			ssp_data[5] = (uint16_t)gyro[2];

			format_data(imu_bin_buffer, "rsp:", ssp_data,12);
			dr_binary_tx_safe(imu_bin_buffer,8);

			if(custom_add_data==true) {
				custom_add_data = false;
			}
			else {
				//data_tx_handler(ble_tx_buffer);
			}
		}
	}
#endif
}

/*
 * imu_read_direct()
 * Reads IMU registers directly via I2C, bypassing the driver API.
 * Reads data immediately without waiting for DRDY interrupt.
 *
 * Flow:
 *   1) Check TWI initialization (first call only)
 *   2) Gyro config: +/-2000dps, 100Hz ODR (GYRO_CONFIG0 = 0x09)
 *   3) Accel config: +/-4g, 100Hz ODR (ACCEL_CONFIG0 = 0x49)
 *   4) Power ON: accel+gyro low-noise mode (PWR_MGMT0 = 0x0F)
 *   5) Wait 80ms (gyro startup: min 45ms + margin)
 *   6) Read 14 consecutive bytes from TEMP_DATA1 (0x09) (temp 2 + accel 6 + gyro 6)
 *   7) Big-endian -> int16_t conversion
 *   8) Keep raw ICM42670P datasheet axes
 *   9) Send via BLE with "rsp:" tag
 *  10) Switch IMU to sleep mode (power saving)
 *
 * Returns: 0=success, -1=TX failure, -2=RX failure
 */
/* Raw I2C read from ICM42670P — bypasses driver API entirely */
#include "system_interface.h"
#include "nrfx_twi.h"

extern const nrfx_twi_t m_twi_icm42670;

#define IMU_I2C_ADDR   0x68
#define REG_TEMP_DATA1 0x09  /* TEMP_DATA1 — temperature upper byte register */
#define IMU_TEMP_AVG_SAMPLES 4U

static bool s_direct_twi_ready = false;
static bool s_fifo_capture_active = false;

static void imu_direct_twi_init_once(void)
{
	/* TWI (I2C) init — performed only once (re-init ensures clean state) */
	if (!s_direct_twi_ready) {
		inv_i2c_master_uninitialize();
		inv_i2c_master_initialize();
		s_direct_twi_ready = true;
	}
}

static uint16_t imu_temp_raw_to_x100(int16_t temp_raw)
{
	float temp_c = 25.0f + ((float)temp_raw / 128.0f);
	return (uint16_t)(temp_c * 100.0f);
}

/* --------------------------------------------------------------------------------------
 * Direct IMU register read — raw I2C, no DRDY, sends rsp: via BLE
 * Direct I2C register read (no interrupt, no IMU driver API)
 * -------------------------------------------------------------------------------------- */
int imu_read_direct(void)
{
	uint8_t raw[14];  /* temp 2 bytes + accel 6 bytes + gyro 6 bytes */
	uint8_t temp_raw_bytes[2];
	int32_t accel[3], gyro[3];
	int16_t temp_raw;
	int32_t temp_sum;
	uint8_t reg;
	uint32_t ret;
	uint8_t i;

	imu_direct_twi_init_once();

	/* Gyro config: GYRO_CONFIG0(0x20) = 0x09 -> +/-2000dps FSR, 100Hz ODR */
	{
		uint8_t gyro_cfg[2] = { 0x20, 0x09 };
		icm42670_twi_tx(IMU_I2C_ADDR, gyro_cfg, 2, false);
	}

	/* Accel config: ACCEL_CONFIG0(0x21) = 0x49 -> +/-4g FSR, 100Hz ODR */
	{
		uint8_t accel_cfg[2] = { 0x21, 0x49 };
		icm42670_twi_tx(IMU_I2C_ADDR, accel_cfg, 2, false);
	}

	/* Power ON: PWR_MGMT0(0x1F) = 0x0F -> accel (low-noise) + gyro (low-noise) enabled */
	{
		uint8_t pwr_cmd[2] = { 0x1F, 0x0F };  /* reg=0x1F, val=0x0F */
		icm42670_twi_tx(IMU_I2C_ADDR, pwr_cmd, 2, false);
		//nrf_delay_ms(80);  /* Gyro startup: min 45ms + safety margin */
		dr_sd_delay_ms(80);
	}

	/* Read 14 consecutive bytes from TEMP_DATA1 (0x09~0x16): temp + accel + gyro */
	reg = REG_TEMP_DATA1;
	ret = icm42670_twi_tx(IMU_I2C_ADDR, &reg, 1, true);  /* Send register address (no STOP) */
	
	if (ret)
	{
		DBG_PRINTF("[IMU] tx FAIL %u\r\n", ret);
		return -1;
	}

	ret = icm42670_twi_rx(IMU_I2C_ADDR, raw, sizeof(raw));

	if (ret)
	{
		DBG_PRINTF("[IMU] rx FAIL %u\r\n", ret);
		return -2;
	}

	/*
	 * Convert big-endian register layout to int16_t
	 * raw[0..1] = temperature
	 * raw[2..7] = accel X,Y,Z (2 bytes each, MSB first)
	 * raw[8..13] = gyro X,Y,Z (2 bytes each, MSB first)
	 */
	temp_raw = (int16_t)((raw[0] << 8) | raw[1]);
	temp_sum = 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]);

	/* Pack data */
	ssp_data[0] = (uint16_t)accel[0];
	ssp_data[1] = (uint16_t)accel[1];
	ssp_data[2] = (uint16_t)accel[2];
	ssp_data[3] = (uint16_t)gyro[0];
	ssp_data[4] = (uint16_t)gyro[1];
	ssp_data[5] = (uint16_t)gyro[2];

	if (info4 == true)
	{
		reg = REG_TEMP_DATA1;
		for (i = 1; i < IMU_TEMP_AVG_SAMPLES; i++)
		{
			ret = icm42670_twi_tx(IMU_I2C_ADDR, &reg, 1, true);
			if (ret)
			{
				DBG_PRINTF("[IMU] temp avg tx FAIL %u\r\n", ret);
				break;
			}

			ret = icm42670_twi_rx(IMU_I2C_ADDR, temp_raw_bytes, sizeof(temp_raw_bytes));
			if (ret)
			{
				DBG_PRINTF("[IMU] temp avg rx FAIL %u\r\n", ret);
				break;
			}

			temp_sum += (int16_t)((temp_raw_bytes[0] << 8) | temp_raw_bytes[1]);
		}
		if (i == IMU_TEMP_AVG_SAMPLES)
		{
			temp_raw = (int16_t)(temp_sum / (int32_t)IMU_TEMP_AVG_SAMPLES);
		}

		uint16_t temp_x100 = imu_temp_raw_to_x100(temp_raw);

		/* info4 mode: store in global array (sent as rbb: packet by mbb?) */
		info_imu[0] = ssp_data[0];
		info_imu[1] = ssp_data[1];
		info_imu[2] = ssp_data[2];
		info_imu[3] = ssp_data[3];
		info_imu[4] = ssp_data[4];
		info_imu[5] = ssp_data[5];
		info_temp = temp_x100;
	}
	else
	{
		/* Normal mode: send immediately via BLE with "rsp:" tag */
		format_data(imu_bin_buffer, "rsp:", ssp_data, 12);
		dr_binary_tx_safe(imu_bin_buffer, 8);
	}

	/* IMU sleep mode: PWR_MGMT0 = 0x00 -> accel/gyro both OFF (power saving) */
	{
		uint8_t pwr_off[2] = { 0x1F, 0x00 };  /* reg=PWR_MGMT0, val=0x00 */
		icm42670_twi_tx(IMU_I2C_ADDR, pwr_off, 2, false);
	}

	return 0;
}

int imu_read_temperature_x100(uint16_t *temp_x100, float *temp_c)
{
	uint8_t raw[2];
	uint8_t reg = REG_TEMP_DATA1;
	int16_t temp_raw;
	int32_t temp_sum = 0;
	uint32_t ret;
	uint8_t i;

	if (temp_x100 == NULL && temp_c == NULL)
	{
		return -1;
	}

	if (s_fifo_capture_active)
	{
		return -4;
	}

	imu_direct_twi_init_once();

	/* Power ON briefly so the temperature register is refreshed before reading. */
	{
		uint8_t pwr_cmd[2] = { 0x1F, 0x0F };
		icm42670_twi_tx(IMU_I2C_ADDR, pwr_cmd, 2, false);
		dr_sd_delay_ms(80);
	}

	for (i = 0; i < IMU_TEMP_AVG_SAMPLES; i++)
	{
		ret = icm42670_twi_tx(IMU_I2C_ADDR, &reg, 1, true);
		if (ret)
		{
			uint8_t pwr_off[2] = { 0x1F, 0x00 };
			icm42670_twi_tx(IMU_I2C_ADDR, pwr_off, 2, false);
			DBG_PRINTF("[IMU] temp tx FAIL %u\r\n", ret);
			return -2;
		}

		ret = icm42670_twi_rx(IMU_I2C_ADDR, raw, sizeof(raw));
		if (ret)
		{
			uint8_t pwr_off[2] = { 0x1F, 0x00 };
			icm42670_twi_tx(IMU_I2C_ADDR, pwr_off, 2, false);
			DBG_PRINTF("[IMU] temp rx FAIL %u\r\n", ret);
			return -3;
		}

		temp_sum += (int16_t)((raw[0] << 8) | raw[1]);
	}

	temp_raw = (int16_t)(temp_sum / (int32_t)IMU_TEMP_AVG_SAMPLES);
	if (temp_x100 != NULL)
	{
		*temp_x100 = imu_temp_raw_to_x100(temp_raw);
	}
	if (temp_c != NULL)
	{
		*temp_c = 25.0f + ((float)temp_raw / 128.0f);
	}

	{
		uint8_t pwr_off[2] = { 0x1F, 0x00 };
		icm42670_twi_tx(IMU_I2C_ADDR, pwr_off, 2, false);
	}

	return 0;
}

bool imu_fifo_capture_is_active(void)
{
	return s_fifo_capture_active;
}

/* --------------------------------------------------------------------------------------
 * mtb? FIFO capture support
 *
 * Uses the ICM42670P internal FIFO at 50 Hz. The FIFO is started when mtb?
 * begins, then drained after piezo raa: completion and sent as rim: packets.
 * rim payload: u16 BE total_sample_count, then per sample accel(6B)+gyro(6B) from each 16B FIFO record.
 *
 * Timing (tune for MTB latency vs BLE reliability):
 *   - IMU_FIFO_ENABLE_SETTLE_MS: after accel/gyro on, before 2nd FIFO flush (first start only).
 *   - IMU_FIFO_RIM_POST_TX_MS: gap between rim notifications only (not after final packet).
 * -------------------------------------------------------------------------------------- */
#define IMU_FIFO_PACKET_SIZE_BYTES  FIFO_16BYTES_PACKET_SIZE
#define IMU_FIFO_MAX_PACKET_COUNT   258
#define IMU_FIFO_READ_RECORDS_PER_BURST 14
#define IMU_FIFO_ACCEL_OFFSET       1
#define IMU_FIFO_GYRO_OFFSET        7
#define IMU_FIFO_ENABLE_SETTLE_MS   20U
#define IMU_FIFO_RIM_POST_TX_MS     0U
#define RIM_SAMPLE_SIZE_BYTES       12
#define RIM_PACKET_HEADER_BYTES     6  /* "rim:" + u16 BE total_sample_count */
#define RIM_MAX_SAMPLE_BYTES        (BLE_NUS_MAX_DATA_LEN - 2 - RIM_PACKET_HEADER_BYTES)
#define RIM_SAMPLES_PER_PACKET      (RIM_MAX_SAMPLE_BYTES / RIM_SAMPLE_SIZE_BYTES)

static void imu_serif_make(struct inv_imu_serif *serif)
{
	serif->context    = 0;
	serif->read_reg   = inv_io_hal_read_reg;
	serif->write_reg  = inv_io_hal_write_reg;
	serif->max_read   = 1024 * 32;
	serif->max_write  = 1024 * 32;
	serif->serif_type = SERIF_TYPE;
}

static int imu_fifo_driver_prepare(void)
{
	struct inv_imu_serif icm_serif;
	int rc;

	inv_i2c_master_uninitialize();
	inv_i2c_master_initialize();

	imu_serif_make(&icm_serif);
	rc = setup_imu_device(&icm_serif);
	return rc;
}

static void imu_fifo_power_off(void)
{
	(void)inv_imu_disable_gyro(&icm_driver);
	(void)inv_imu_disable_accel(&icm_driver);
	(void)inv_imu_configure_fifo(&icm_driver, INV_IMU_FIFO_DISABLED);
	s_fifo_capture_active = false;
}

void imu_fifo_capture_disable(void)
{
	if (s_fifo_capture_active)
	{
		imu_fifo_power_off();
	}
}

int imu_fifo_capture_start(void)
{
	int rc;

	if (s_fifo_capture_active)
	{
		return 0;
	}

	rc = imu_fifo_driver_prepare();
	if (rc != 0)
	{
		DBG_PRINTF("[IMU FIFO] prepare fail %d\r\n", rc);
		return rc;
	}

	rc |= inv_imu_set_accel_fsr(&icm_driver, ACCEL_CONFIG0_FS_SEL_4g);
	rc |= inv_imu_set_gyro_fsr(&icm_driver, GYRO_CONFIG0_FS_SEL_500dps);
	rc |= inv_imu_set_accel_frequency(&icm_driver, ACCEL_CONFIG0_ODR_50_HZ); // FIFO ODR Accel 50Hz Setting
	rc |= inv_imu_set_gyro_frequency(&icm_driver, GYRO_CONFIG0_ODR_50_HZ);	 // FIFO ODR Gyro 50Hz Setting
	rc |= inv_imu_set_accel_ln_bw(&icm_driver, IMU_FIFO_MTB_ACCEL_LN_BW);
	rc |= inv_imu_set_gyro_ln_bw(&icm_driver, IMU_FIFO_MTB_GYRO_LN_BW);
	rc |= inv_imu_disable_high_resolution_fifo(&icm_driver);
	rc |= inv_imu_configure_fifo(&icm_driver, INV_IMU_FIFO_ENABLED);
	{
		uint8_t fifo_cfg1;
		rc |= inv_imu_read_reg(&icm_driver, FIFO_CONFIG1, 1, &fifo_cfg1);
		fifo_cfg1 &= ~FIFO_CONFIG1_FIFO_MODE_MASK;
		fifo_cfg1 |= FIFO_CONFIG1_FIFO_MODE_STREAM;
		rc |= inv_imu_write_reg(&icm_driver, FIFO_CONFIG1, 1, &fifo_cfg1);
	}
	rc |= inv_imu_reset_fifo(&icm_driver);
	rc |= inv_imu_enable_accel_low_noise_mode(&icm_driver); // FIFO Accel Low Noise Mode
	//rc |= inv_imu_enable_accel_low_power_mode(&icm_driver);	// FIFO Accel Low Power Mode TEST
	rc |= inv_imu_enable_gyro_low_noise_mode(&icm_driver);	// FIFO Gyro Low Noise Mode
	dr_sd_delay_ms(IMU_FIFO_ENABLE_SETTLE_MS);
	rc |= inv_imu_reset_fifo(&icm_driver);

	if (rc != 0)
	{
		DBG_PRINTF("[IMU FIFO] start fail %d\r\n", rc);
		imu_fifo_power_off();
		return rc;
	}

	s_fifo_capture_active = true;
	return 0;
}

static void imu_fifo_send_rim_packets(uint16_t total_sample_count)
{
	static uint8_t rim_buf[BLE_NUS_MAX_DATA_LEN];
	uint16_t record_idx = 0;

	do
	{
		uint16_t sample_count = total_sample_count - record_idx;
		uint16_t dst_idx = RIM_PACKET_HEADER_BYTES;
		uint16_t i;

		if (sample_count > RIM_SAMPLES_PER_PACKET)
		{
			sample_count = RIM_SAMPLES_PER_PACKET;
		}

		rim_buf[0] = 'r'; rim_buf[1] = 'i'; rim_buf[2] = 'm'; rim_buf[3] = ':';
		rim_buf[4] = (uint8_t)(total_sample_count >> 8);
		rim_buf[5] = (uint8_t)(total_sample_count & 0xFF);

		for (i = 0; i < sample_count; i++)
		{
			const uint8_t *record = &icm_driver.fifo_data[(record_idx + i) * IMU_FIFO_PACKET_SIZE_BYTES];
			memcpy(&rim_buf[dst_idx], &record[IMU_FIFO_ACCEL_OFFSET], 6);
			dst_idx += 6;
			memcpy(&rim_buf[dst_idx], &record[IMU_FIFO_GYRO_OFFSET], 6);
			dst_idx += 6;
		}

		dr_binary_tx_safe(rim_buf, dst_idx / 2);
		record_idx += sample_count;
		if (record_idx < total_sample_count)
		{
			dr_sd_delay_ms(IMU_FIFO_RIM_POST_TX_MS);
		}
	} while (record_idx < total_sample_count);
}

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

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

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

		burst_bytes = burst_records * IMU_FIFO_PACKET_SIZE_BYTES;
		rc |= inv_imu_read_reg(&icm_driver,
							   FIFO_DATA,
							   burst_bytes,
							   &icm_driver.fifo_data[record_idx * IMU_FIFO_PACKET_SIZE_BYTES]);

		record_idx += burst_records;
	}

	return rc;
}

/* Invensense inv_imu_get_data_from_fifo(): header 0x80 with all payload bytes zero = invalid placeholder. */
static bool imu_fifo_record_is_invalid_placeholder(const uint8_t *rec)
{
	uint16_t i;

	if (rec[0] != 0x80u)
	{
		return false;
	}
	for (i = 1u; i < IMU_FIFO_PACKET_SIZE_BYTES; i++)
	{
		if (rec[i] != 0u)
		{
			return false;
		}
	}
	return true;
}

static uint16_t imu_fifo_compact_placeholder_records(uint8_t *fifo, uint16_t record_count)
{
	uint16_t w;
	uint16_t r;
	const uint16_t sz = IMU_FIFO_PACKET_SIZE_BYTES;

	for (r = 0, w = 0; r < record_count; r++)
	{
		uint8_t *rec = fifo + (r * sz);

		if (imu_fifo_record_is_invalid_placeholder(rec))
		{
			continue;
		}
		if (w != r)
		{
			memcpy(fifo + (w * sz), rec, sz);
		}
		w++;
	}
	return w;
}

static int16_t imu_fifo_record_gyro_axis(const uint8_t *rec, uint8_t axis)
{
	const uint8_t *p = &rec[IMU_FIFO_GYRO_OFFSET + ((uint16_t)axis * 2u)];

	if (icm_driver.endianness_data == INTF_CONFIG0_DATA_BIG_ENDIAN)
	{
		return (int16_t)((uint16_t)p[0] << 8 | p[1]);
	}
	return (int16_t)((uint16_t)p[1] << 8 | p[0]);
}

/* Invensense: gyro axis == INVALID_VALUE_FIFO (0x8000) when not valid in this FIFO record. */
static bool imu_fifo_record_is_invalid_gyro(const uint8_t *rec)
{
	uint8_t axis;

	for (axis = 0; axis < 3u; axis++)
	{
		if (imu_fifo_record_gyro_axis(rec, axis) != INVALID_VALUE_FIFO)
		{
			return false;
		}
	}
	return true;
}

static uint16_t imu_fifo_compact_invalid_gyro_records(uint8_t *fifo, uint16_t record_count)
{
	uint16_t w;
	uint16_t r;
	const uint16_t sz = IMU_FIFO_PACKET_SIZE_BYTES;

	for (r = 0, w = 0; r < record_count; r++)
	{
		uint8_t *rec = fifo + (r * sz);

		if (imu_fifo_record_is_invalid_gyro(rec))
		{
			continue;
		}
		if (w != r)
		{
			memcpy(fifo + (w * sz), rec, sz);
		}
		w++;
	}
	return w;
}

static uint16_t imu_fifo_normalize_for_rim(uint8_t *fifo, uint16_t n)
{
	const uint16_t sz = IMU_FIFO_PACKET_SIZE_BYTES;

#if (IMU_FIFO_RIM_TARGET_SAMPLES > 0)
	if (n > (uint16_t)IMU_FIFO_RIM_TARGET_SAMPLES)
	{
		const uint16_t drop = (uint16_t)(n - (uint16_t)IMU_FIFO_RIM_TARGET_SAMPLES);

		memmove(fifo, fifo + ((uint32_t)drop * sz), (uint32_t)IMU_FIFO_RIM_TARGET_SAMPLES * sz);
		n = (uint16_t)IMU_FIFO_RIM_TARGET_SAMPLES;
	}
	(void)sz;
#elif (IMU_FIFO_RIM_MIN_SAMPLES > 0)
	if (n < (uint16_t)IMU_FIFO_RIM_MIN_SAMPLES)
	{
		while (n < (uint16_t)IMU_FIFO_RIM_MIN_SAMPLES && n < IMU_FIFO_MAX_PACKET_COUNT)
		{
			memset(fifo + ((uint32_t)n * sz), 0, sz);
			n++;
		}
	}
#else
	(void)sz;
#endif
	return n;
}

int imu_fifo_capture_stop_and_send_rim(void)
{
	int rc = 0;
	uint8_t count_raw[2] = {0};
	uint16_t packet_count;

	if (!s_fifo_capture_active)
	{
		imu_fifo_send_rim_packets(0);
		return -1;
	}

	rc |= inv_imu_switch_on_mclk(&icm_driver);

	rc |= inv_imu_read_reg(&icm_driver, FIFO_COUNTH, 2, count_raw);
	packet_count = (uint16_t)count_raw[0] | ((uint16_t)count_raw[1] << 8);

	if (packet_count > IMU_FIFO_MAX_PACKET_COUNT)
	{
		packet_count = IMU_FIFO_MAX_PACKET_COUNT;
	}

	if ((rc == 0) && (packet_count > 0))
	{
		rc |= imu_fifo_read_records(packet_count);
		if (rc == 0)
		{
			packet_count = imu_fifo_compact_placeholder_records(icm_driver.fifo_data, packet_count);
			packet_count = imu_fifo_compact_invalid_gyro_records(icm_driver.fifo_data, packet_count);
		}
	}

	if ((rc == 0) && (IMU_FIFO_RIM_DROP_LEADING_SAMPLES > 0U) && (packet_count > 0U))
	{
		const uint16_t drop_req = (uint16_t)IMU_FIFO_RIM_DROP_LEADING_SAMPLES;
		const uint16_t drop = (drop_req < packet_count) ? drop_req : packet_count;
		const uint16_t sz = IMU_FIFO_PACKET_SIZE_BYTES;
		uint8_t *const fifo = icm_driver.fifo_data;

		packet_count = (uint16_t)(packet_count - drop);
		memmove(fifo, fifo + ((uint32_t)drop * sz), (uint32_t)packet_count * sz);
	}

	if (rc == 0)
	{
		packet_count = imu_fifo_normalize_for_rim(icm_driver.fifo_data, packet_count);
	}

	rc |= inv_imu_switch_off_mclk(&icm_driver);

	if (rc != 0)
	{
		DBG_PRINTF("[IMU FIFO] drain fail %d\r\n", rc);
		imu_fifo_send_rim_packets(0);
		imu_fifo_power_off();
		return rc;
	}

	imu_fifo_send_rim_packets(packet_count);
	return 0;
}
