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

/*******************************************************************************
 * [모듈 개요] ICM42670P IMU 드라이버 상위 레이어
 *
 * ICM42670P IMU 센서의 초기화, 설정, 데이터 읽기를 담당하는 애플리케이션 레이어 모듈
 * InvenSense 드라이버 API를 래핑하여 사용
 *
 * 주요 기능:
 *   1) setup_imu_device()     - IMU 초기화 및 WHOAMI 확인 (0x67 = ICM42670P)
 *   2) configure_imu_device() - 센서 파라미터 설정
 *      - 가속도계: ±4g FSR, 100Hz(저전력) 또는 800Hz(저잡음)
 *      - 자이로:   ±2000dps FSR, 100Hz 또는 800Hz
 *      - FIFO 비활성화 (레지스터 직접 읽기 모드)
 *   3) get_imu_data()         - FIFO 또는 레지스터에서 센서 데이터 읽기
 *   4) imu_callback()         - 센서 데이터 수신 콜백
 *      - 마운팅 매트릭스 적용 (보드 방향 보정)
 *      - info4 모드: info_imu[6]에 데이터 저장
 *      - BLE 모드:  "rsp:" 태그로 6축 데이터 BLE 전송
 *      - UART 모드: 텍스트 형식으로 시리얼 출력
 *   5) imu_read_direct()      - 드라이버 API를 우회한 직접 I2C 레지스터 읽기
 *      - 센서 설정 → 전원 ON → 80ms 대기 → 12바이트 읽기 → 슬립
 *
 * 마운팅 매트릭스:
 *   Q30 고정소수점 형식의 3x3 회전 매트릭스로, 보드에 장착된 센서의물리적 방향을 소프트웨어 좌표계에 맞춰 보정
 ******************************************************************************/

#include "sdk_config.h"
#include "app_raw.h"
#include "inv_imu_extfunc.h"
#include "inv_imu_driver.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"


/*
 * 데이터 출력 형식 선택
 * 0 : 원시 데이터 (raw accel, gyro, temp) 출력
 * 1 : 스케일링된 데이터 (g, dps, 섭씨) 출력
 */
#define SCALED_DATA_G_DPS 0


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

/* IMU 드라이버 객체 — 드라이버 API 호출 시 항상 이 구조체 전달 */
static struct inv_imu_device icm_driver;

/* BLE 전송용 바이너리 버퍼 */
 uint8_t imu_bin_buffer[BLE_NUS_MAX_DATA_LEN];

/*
 * ICM42670P 마운팅 매트릭스 (Q30 고정소수점)
 *
 * 센서가 보드에 장착된 물리적 방향에 따라 좌표 변환
 * Q30 형식: 1.0 = (1 << 30) = 0x40000000
 *
 * SM_REVB_DB (개발보드): X→-Y, Y→X 변환 (90도 회전)
 * 기본 (SmartMotion): 단위 행렬 (변환 없음)
 */
#if (SM_BOARD_REV == SM_REVB_DB) /* when DB or EVB are used */
static int32_t icm_mounting_matrix[9] = {  0,     -(1<<30), 0,
                                          (1<<30), 0,       0,
                                           0,      0,      (1<<30) };
#else /* For SmartMotion */
static int32_t icm_mounting_matrix[9] = {(1<<30),  0,       0,
                                          0,      (1<<30),  0,
                                          0,       0,      (1<<30)};
#endif

bool custom_add_data;                              		/* 커스텀 데이터 추가 플래그 (BLE 전송 제어) */
extern bool motion_raw_data_enabled;               		/* 외부에서 원시 데이터 읽기를 요청하는 플래그 */
extern char		ble_tx_buffer[BLE_NUS_MAX_DATA_LEN]; 	/* BLE 텍스트 전송 버퍼 */
extern which_cmd_t cmd_type_t;                     		/* 현재 명령 소스 (BLE 또는 UART) */
uint16_t ssp_data[6]={0,};                         		/* BLE 전송용 6축 데이터 배열 (accel XYZ + gyro XYZ) */
extern bool info4;                                 		/* info4 모드 플래그 (cmd_parse에서 설정) */
 volatile uint16_t info_imu[6];                    		/* info4 모드에서 IMU 데이터를 저장하는 전역 배열 */

/* --------------------------------------------------------------------------------------
 *  static function declaration
 * -------------------------------------------------------------------------------------- */
static void apply_mounting_matrix(const int32_t matrix[9], int32_t raw[3]);

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

/*
 * setup_imu_device()
 * IMU 디바이스 초기화 및 식별 확인
 *
 * 처리 흐름:
 *   1) inv_imu_init()으로 드라이버 초기화 (시리얼 인터페이스 + 콜백 등록)
 *   2) WHOAMI 레지스터 읽기로 디바이스 확인
 *   3) WHOAMI 값이 ICM_WHOAMI(0x67)와 일치하는지 검증
 *
 * 반환값: 0=성공, 음수=에러
 */
int setup_imu_device(struct inv_imu_serif *icm_serif)
{
	int rc = 0;
	uint8_t who_am_i;

	/* IMU 드라이버 초기화 — 시리얼 인터페이스 연결 및 콜백 함수 등록 */
	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;
	}

	/* WHOAMI 레지스터 읽기 — 디바이스 존재 및 통신 확인 */
	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;
	}

	/* WHOAMI 값 검증 — ICM42670P의 경우 0x67이어야 함 */
	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()
 * IMU 센서 동작 파라미터 설정
 *
 * 설정 항목:
 *   - FIFO: 비활성화 (USE_FIFO=0일 때, 레지스터 직접 읽기 모드)
 *   - 가속도계 FSR: ±4g (USE_HIGH_RES_MODE=0일 때)
 *   - 자이로 FSR: ±2000dps
 *   - ODR(출력 데이터율):
 *     - 저잡음 모드(USE_LOW_NOISE_MODE=1): 800Hz
 *     - 저전력 모드(USE_LOW_NOISE_MODE=0): 100Hz
 *   - 자이로는 항상 저잡음 모드로 동작
 *   - FIFO 미사용 시 자이로 스타트업 시간만큼 대기
 *
 * 반환값: 0=성공, 음수=에러
 */
int configure_imu_device(void)
{
	int rc = 0;

	/* FIFO 비활성화 — 레지스터에서 직접 데이터를 읽기 */
	if (!USE_FIFO)
		rc |= inv_imu_configure_fifo(&icm_driver, INV_IMU_FIFO_DISABLED);

	if (USE_HIGH_RES_MODE) {
		/* 고해상도 FIFO 모드: 20비트 데이터, FSR은 16g/2000dps로 고정됨 */
		rc |= inv_imu_enable_high_resolution_fifo(&icm_driver);
	} else {
		/* 표준 모드: 가속도계 ±4g, 자이로 ±2000dps 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_2000dps);
	}

	if (USE_LOW_NOISE_MODE) {
		/* 저잡음 모드: 800Hz ODR, 가속도계 저잡음 모드 활성화 */
		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 {
		/* 저전력 모드: 100Hz ODR, 가속도계 저전력 모드 활성화 */
		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);
	}

	/* 자이로는 모드에 관계없이 항상 저잡음 모드로 동작 */
	rc |= inv_imu_enable_gyro_low_noise_mode(&icm_driver);

	/* FIFO 미사용 시 자이로 스타트업 시간만큼 대기 (첫 유효 데이터까지의 지연) */
	if (!USE_FIFO)
		inv_imu_sleep_us(GYR_STARTUP_TIME_US);

	return rc;
}


/*
 * get_imu_data()
 * IMU에서 센서 데이터 읽기
 * USE_FIFO 설정에 따라 FIFO 또는 레지스터에서 데이터를 가져옴
 * 읽은 데이터는 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()
 * 현재 설정된 가속도계와 자이로의 FSR(Full Scale Range) 값을 가져온다.
 * 스케일링된 데이터(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()
 * IMU 드라이버가 새 센서 데이터를 읽을 때마다 호출되는 콜백 함수
 *
 * 처리 흐름:
 *   1) 이벤트에서 가속도/자이로 원시 데이터 추출
 *      - FIFO 모드: 타임스탬프 롤오버 처리, 고해상도(20비트) 지원
 *      - 레지스터 모드: 16비트 데이터 직접 사용
 *   2) 마운팅 매트릭스 적용 (보드 장착 방향 보정)
 *   3) 데이터 출력 (모드에 따라 분기):
 *      - info4 모드: info_imu[6] 전역 배열에 저장 (외부에서 폴링)
 *      - UART 모드: "Tp" 접두사로 6축 데이터 텍스트 출력
 *      - BLE 모드:  "rsp:" 태그로 바이너리 패킷 전송 + UART 텍스트 동시 출력
 */
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 타임스탬프 롤오버 처리 (16비트 → 64비트 확장) */
	if (last_fifo_timestamp > event->timestamp_fsync)
		rollover_num++;
	last_fifo_timestamp = event->timestamp_fsync;

	/* 타임스탬프를 마이크로초 단위로 변환 (Q24 해상도 적용) */
	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) {
		/* 고해상도 모드: 16비트 데이터를 4비트 좌측 시프트 + 하위 4비트 추가 → 20비트 */
		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 {
		/* 표준 해상도: 16비트 데이터 그대로 사용 */
		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

	/* 레지스터 직접 읽기 모드: 16비트 원시 데이터 추출 */
	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];

	/* 레지스터 모드에서는 센서 마스크를 강제 설정하여 아래 출력 로직이 동작하도록 함 */
	event->sensor_mask |= (1 << INV_SENSOR_TEMPERATURE);
	event->sensor_mask |= (1 << INV_SENSOR_ACCEL);
	event->sensor_mask |= (1 << INV_SENSOR_GYRO);
#endif

	/* 마운팅 매트릭스 적용 — 센서의 물리적 장착 방향을 소프트웨어 좌표계로 보정 */
	apply_mounting_matrix(icm_mounting_matrix, accel);
	apply_mounting_matrix(icm_mounting_matrix, gyro);

#if SCALED_DATA_G_DPS
	/*
	 * 원시 데이터를 물리 단위(g, dps)로 변환
	 * 변환 공식: 물리값 = 원시값 * 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;

	/* 온도 변환: 고해상도/레지스터 모드는 /128, FIFO 표준 모드는 /2 */
	if (USE_HIGH_RES_MODE || !USE_FIFO)
		temp_degc = 25 + ((float)event->temperature / 128);
	else
		temp_degc = 25 + ((float)event->temperature / 2);

	/*
	 * 스케일링된 데이터를 UART로 출력
	 */
	if (event->sensor_mask & (1 << INV_SENSOR_ACCEL) && event->sensor_mask & (1 << INV_SENSOR_GYRO))
		DBG_PRINTF("%u: %.3f, \t%.3f, \t%.3f, \t%.3f, \t%.3f, \t%.3f, \t%.3f\r\n",
		        (uint32_t)timestamp,
		        accel_g[0], accel_g[1], accel_g[2],
		        temp_degc,
		        gyro_dps[0], gyro_dps[1], gyro_dps[2]);
#else

	/*
	 * 원시 데이터 출력 — 명령 소스(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 모드: 전역 배열 info_imu[6]에 데이터 저장, 외부 모듈에서 이 배열을 폴링하여 데이터 사용 */
		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];
		}

		/* UART 모드: "Tp" 접두사로 6축 데이터를 텍스트 형식으로 출력 */
		else if(cmd_type_t == CMD_UART) {
			//DBG_PRINTF("Tp%d,%d,%d,%d,%d,%d\r\n\r\n", accel[0], accel[1], accel[2], gyro[0], gyro[1], gyro[2]);
		}

		/*
			* BLE 모드: 6축 데이터를 바이너리 패킷으로 BLE 전송
			* ssp_data[0~2] = 가속도 XYZ, ssp_data[3~5] = 자이로 XYZ
			* format_data()로 "rsp:" 태그 + 12바이트 데이터를 패킷화
			* dr_binary_tx_safe()로 8바이트 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);
			//DBG_PRINTF("Tp%d,%d,%d,%d,%d,%d\r\n\r\n", accel[0], accel[1], accel[2], gyro[0], gyro[1], gyro[2]);
			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
}

/* --------------------------------------------------------------------------------------
 *  Static functions definition
 * -------------------------------------------------------------------------------------- */

/*
 * apply_mounting_matrix()
 * Q30 고정소수점 회전 매트릭스를 3축 벡터에 적용한다.
 *
 * 계산 방식:
 *   result[i] = matrix[i*3+0]*raw[0] + matrix[i*3+1]*raw[1] + matrix[i*3+2]*raw[2]
 *   결과를 30비트 우측 시프트하여 Q30 → 정수 변환
 *
 * 이를 통해 센서의 물리적 장착 방향에 관계없이 일관된 좌표계를 사용할 수 있다.
 */
static void apply_mounting_matrix(const int32_t matrix[9], int32_t raw[3])
{
	unsigned i;
	int64_t data_q30[3];

	for(i = 0; i < 3; i++) {
		data_q30[i] =  ((int64_t)matrix[3*i+0] * raw[0]);
		data_q30[i] += ((int64_t)matrix[3*i+1] * raw[1]);
		data_q30[i] += ((int64_t)matrix[3*i+2] * raw[2]);
	}
	/* Q30 → 정수 변환: 30비트 우측 시프트 */
	raw[0] = (int32_t)(data_q30[0]>>30);
	raw[1] = (int32_t)(data_q30[1]>>30);
	raw[2] = (int32_t)(data_q30[2]>>30);
}


/*
 * imu_read_direct()
 * 드라이버 API를 우회하여 직접 I2C 레지스터를 읽는 함수.
 * DRDY(데이터 준비) 인터럽트를 기다리지 않고 즉시 데이터를 읽는다.
 *
 * 처리 흐름:
 *   1) TWI 초기화 확인 (최초 1회만)
 *   2) 자이로 설정: ±2000dps, 100Hz ODR (GYRO_CONFIG0 = 0x09)
 *   3) 가속도 설정: ±4g, 100Hz ODR (ACCEL_CONFIG0 = 0x29)
 *   4) 전원 ON: 가속도+자이로 저잡음 모드 (PWR_MGMT0 = 0x0F)
 *   5) 80ms 대기 (자이로 스타트업: 최소 45ms + 여유)
 *   6) ACCEL_DATA_X1(0x0B)부터 12바이트 연속 읽기 (accel 6 + gyro 6)
 *   7) 빅엔디안 → int16_t 변환
 *   8) 마운팅 매트릭스 적용
 *   9) "rsp:" 태그로 BLE 전송
 *  10) IMU 슬립 모드로 전환 (전력 절감)
 *
 * 반환값: 0=성공, -1=TX 실패, -2=RX 실패
 */
/* 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_ACCEL_X1   0x0B  /* ACCEL_DATA_X1 — 가속도 X축 상위 바이트 레지스터 */

/* --------------------------------------------------------------------------------------
 * Direct IMU register read — raw I2C, no DRDY, sends rsp: via BLE
 * 직접 I2C로 레지스터 읽는 방식 (인터럽트 X, IMU 드라이버 API X)
 * -------------------------------------------------------------------------------------- */
int imu_read_direct(void)
{
	uint8_t raw[12];  /* 가속도 6바이트 + 자이로 6바이트 */
	int32_t accel[3], gyro[3];
	uint8_t reg;
	uint32_t ret;

	static bool twi_ready = false;

	/* TWI(I2C) 초기화 — 최초 1회만 수행 (재초기화로 클린 상태 보장) */
	if (!twi_ready) {
		inv_i2c_master_uninitialize();
		inv_i2c_master_initialize();
		twi_ready = true;
	}

	/* 자이로 설정: 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_CONFIG0(0x21) = 0x29 → ±4g FSR, 100Hz ODR */
	{
		uint8_t accel_cfg[2] = { 0x21, 0x29 };
		icm42670_twi_tx(IMU_I2C_ADDR, accel_cfg, 2, false);
	}

	/* 전원 ON: PWR_MGMT0(0x1F) = 0x0F → 가속도(저잡음) + 자이로(저잡음) 활성화 */
	{
		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);  /* 자이로 스타트업: 최소 45ms + 안전 마진 */
		dr_sd_delay_ms(80);
	}

	/* ACCEL_DATA_X1(0x0B)부터 12바이트 연속 읽기 (0x0B~0x16) */
	reg = REG_ACCEL_X1;
	ret = icm42670_twi_tx(IMU_I2C_ADDR, &reg, 1, true);  /* 레지스터 주소 전송 (STOP 없음) */
	
	if (ret)
	{
		DBG_PRINTF("[IMU] tx FAIL %u\r\n", ret);
		return -1;
	}

	ret = icm42670_twi_rx(IMU_I2C_ADDR, raw, 12);  /* 12바이트 데이터 수신 */

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

	/*
	 * 빅엔디안 레지스터 레이아웃을 int16_t로 변환
	 * raw[0..5] = 가속도 X,Y,Z (각 2바이트, MSB first)
	 * raw[6..11] = 자이로 X,Y,Z (각 2바이트, MSB first)
	 */
	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]);

	/* 마운팅 매트릭스 적용 — 보드 장착 방향 보정 */
	apply_mounting_matrix(icm_mounting_matrix, accel);
	apply_mounting_matrix(icm_mounting_matrix, gyro);

	/* 데이터 패킹 */
	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)
	{
		/* info4 모드: 전역 배열에 저장 (mbb?에서 rbb: 패킷으로 일괄 전송) */
		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];
	}
	else
	{
		/* 일반 모드: "rsp:" 태그로 BLE 즉시 전송 */
		format_data(imu_bin_buffer, "rsp:", ssp_data, 12);
		dr_binary_tx_safe(imu_bin_buffer, 8);
		DBG_PRINTF("0");
	}

	/* IMU 슬립 모드: PWR_MGMT0 = 0x00 → 가속도/자이로 모두 OFF (전력 절감) */
	{
		uint8_t pwr_off[2] = { 0x1F, 0x00 };  /* reg=PWR_MGMT0, val=0x00 */
		icm42670_twi_tx(IMU_I2C_ADDR, pwr_off, 2, false);
	}

	return 0;
}
