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

/*******************************************************************************
 * [모듈 개요] ICM42670P IMU 센서 I2C 통신 인터페이스
 *
 * nRF52840의 TWI(I2C) 하드웨어를 사용하여 ICM42670P IMU 센서와 통신하는
 * 저수준 인터페이스 모듈이다.
 *
 * - I2C 슬레이브 주소: 0x68 (ICM42670P 기본 주소)
 * - I2C 핀 설정: SCL=P1.14, SDA=P1.15 (system_interface.h에서 정의)
 * - TWI 인스턴스: NRFX_TWI_INSTANCE(0) 사용
 * - 통신 속도: 100kHz (NRF_TWI_FREQ_100K)
 *
 * 주요 함수 흐름:
 *   inv_io_hal_init()      → I2C 또는 SPI 초기화 (현재 I2C만 구현)
 *   inv_io_hal_read_reg()  → 레지스터 읽기 (TX로 주소 전송 → RX로 데이터 수신)
 *   inv_io_hal_write_reg() → 레지스터 쓰기 (주소+데이터를 한번에 TX)
 *
 * 에러 처리: 모든 I2C 읽기/쓰기에서 실패 시 1회 재시도 후 에러 출력
 *
 * 참고: SPI4 인터페이스 코드 경로도 존재하지만 현재 미구현 상태이며,
 *       실제 운용에서는 I2C(UI_I2C)만 사용한다.
 ******************************************************************************/

/* board driver */
#include <stdio.h>
#include <string.h>
#include <stdarg.h>

#include <stdbool.h>
#include "nrf.h"
#include "app_error.h"
#include "boards.h"
#include "nrfx_gpiote.h"
#include "nrfx_twi.h"

#include "system_interface.h"
#include "nrf_delay.h"

/* ICM42670P I2C 슬레이브 주소 및 직렬 쓰기 최대 바이트 수 */
#define ICM_I2C_ADDR     0x68
#define INV_MAX_SERIAL_WRITE 16

/* TWI(I2C) 인스턴스 생성 — system_interface.h의 ICM42670_I2C_INSTANCE(0)을 사용 */
const nrfx_twi_t m_twi_icm42670 = NRFX_TWI_INSTANCE(ICM42670_I2C_INSTANCE);

/*
 * inv_i2c_master_uninitialize()
 * I2C 버스를 비활성화하고 TWI 인스턴스를 해제한다.
 * 슬립 모드 진입 전 또는 재초기화 전에 호출된다.
 */
void inv_i2c_master_uninitialize(void){
    nrfx_twi_disable(&m_twi_icm42670);
    nrfx_twi_uninit(&m_twi_icm42670);
}

/*
 * inv_i2c_master_initialize()
 * nRF52840 TWI 하드웨어를 초기화하고 활성화한다.
 * - SCL: P1.14, SDA: P1.15
 * - 속도: 100kHz
 * - 인터럽트 우선순위: 최고 (APP_IRQ_PRIORITY_HIGH)
 * - 이벤트 핸들러 없음 (블로킹 모드로 동작)
 */
void inv_i2c_master_initialize(void){
    ret_code_t err_code;

    const nrfx_twi_config_t twi_icm42670_config = {
       .scl                = ICM42670_I2C_SCL_PIN,
       .sda                = ICM42670_I2C_SDA_PIN,
       .frequency          = NRF_TWI_FREQ_100K,
       .interrupt_priority = APP_IRQ_PRIORITY_HIGH,
    };

    /* TWI 드라이버 초기화 (이벤트 핸들러=NULL → 블로킹 모드) */
    err_code = nrfx_twi_init(&m_twi_icm42670, &twi_icm42670_config, NULL, NULL);
    APP_ERROR_CHECK(err_code);

    /* TWI 하드웨어 활성화 — 이후 tx/rx 가능 */
    nrfx_twi_enable(&m_twi_icm42670);
}


/*
 * icm42670_twi_tx()
 * I2C 전송 래퍼 함수. nrfx_twi_tx를 호출하여 데이터를 송신한다.
 * no_stop=true이면 STOP 컨디션을 보내지 않음 (Repeated START를 위해 사용)
 */
uint32_t icm42670_twi_tx(  uint8_t       device_id,
                          uint8_t const *       p_data,
                          uint8_t               length,
                          bool                  no_stop)
{
	ret_code_t ret;
	ret = nrfx_twi_tx(&m_twi_icm42670, device_id, p_data, length, no_stop);
	return ret;
}


/*
 * icm42670_twi_rx()
 * I2C 수신 래퍼 함수. nrfx_twi_rx를 호출하여 데이터를 수신한다.
 */
uint32_t icm42670_twi_rx(  uint8_t       device_id,
                          uint8_t *             p_data,
                          uint8_t               length)
{
	ret_code_t ret;
	ret = nrfx_twi_rx(&m_twi_icm42670, device_id, p_data, length);
	return ret;
}


/*
 * inv_i2c_master_read_register()
 * ICM42670P의 특정 레지스터에서 데이터를 읽는다.
 *
 * 동작 순서:
 *   1) TX: 레지스터 주소 1바이트 전송 (no_stop=true → Repeated START 준비)
 *   2) RX: 지정된 길이만큼 데이터 수신
 *
 * 에러 처리: TX, RX 각각 실패 시 1회 재시도한다.
 */
static unsigned long inv_i2c_master_read_register(unsigned char Address, unsigned char RegisterAddr, unsigned short RegisterLen, unsigned char *RegisterValue){
	//ret_code_t ret;
	uint32_t ret;
	uint8_t addr8 = (uint8_t)RegisterAddr;

	/* 1단계: 읽을 레지스터 주소를 전송 (STOP 없이 → Repeated START 사용) */
	ret = icm42670_twi_tx(Address, &addr8, 1, true);
	if(ret != NRF_SUCCESS) {
		/* 실패 시 1회 재시도 */
		ret = icm42670_twi_tx(Address, &addr8, 1, true);
		if(ret != NRF_SUCCESS) {
		  	printf("ERR! i2c read-1\r\n");
		}
	}

	/* 2단계: 해당 레지스터에서 데이터 수신 */
	ret = icm42670_twi_rx(Address, RegisterValue, RegisterLen);
	if(ret != NRF_SUCCESS) {
		/* 실패 시 1회 재시도 */
		ret = icm42670_twi_rx(Address, RegisterValue, RegisterLen);
		if(ret != NRF_SUCCESS) {
		  	printf("ERR! i2c read-2\r\n");
		}
	}

	return ret;
}

/*
 * inv_i2c_master_write_register()
 * ICM42670P의 특정 레지스터에 데이터를 쓴다.
 *
 * 동작 순서:
 *   1) 버퍼[0]에 레지스터 주소, 버퍼[1~N]에 쓸 데이터를 배치
 *   2) TX: 주소+데이터를 한번에 전송 (no_stop=false → STOP 컨디션 포함)
 *
 * 에러 처리: 실패 시 1회 재시도한다.
 */
static unsigned long inv_i2c_master_write_register(unsigned char Address, unsigned char RegisterAddr, unsigned short RegisterLen, const unsigned char *RegisterValue){
	uint32_t ret;
	uint8_t buffer[1 + INV_MAX_SERIAL_WRITE]; /* 레지스터 주소(1) + 데이터(최대 16바이트) */

	/* 버퍼 구성: [레지스터 주소][데이터 바이트들] */
	buffer[0] = (uint8_t)RegisterAddr;
	memcpy(buffer+1, RegisterValue, RegisterLen);

	/* 주소+데이터를 한번에 전송 */
	ret = icm42670_twi_tx(Address, buffer, RegisterLen+1, false);
	if(ret != NRF_SUCCESS) {
		/* 실패 시 1회 재시도 */
		ret = icm42670_twi_tx(Address, buffer, RegisterLen+1, false);
		if(ret != NRF_SUCCESS) {
		  	printf("ERR! i2c write\r\n");
		}
	}

	return ret;
}



/*
 * inv_io_hal_init()
 * IMU 드라이버가 사용하는 시리얼 인터페이스(I2C 또는 SPI)를 초기화한다.
 * serif->serif_type에 따라 분기하며, 현재는 I2C만 구현되어 있다.
 * 반환값: 0=성공, -1=지원하지 않는 인터페이스 타입
 */

int inv_io_hal_init(struct inv_imu_serif *serif)
{

	switch (serif->serif_type) {
		case UI_SPI4:
		{
			/* SPI4 초기화 — 현재 미구현 (I2C만 사용) */
			break;
		}

		case UI_I2C:
			inv_i2c_master_initialize();
			break;

		default:
			return -1;
	}

	return 0;
}


/*
 * inv_io_hal_read_reg()
 * IMU 드라이버 콜백: 지정된 레지스터에서 데이터를 읽는다.
 * 시리얼 타입에 따라 I2C 또는 SPI 읽기를 수행한다.
 */
int inv_io_hal_read_reg(struct inv_imu_serif *serif, uint8_t reg, uint8_t * rbuffer, uint32_t rlen)
{
	switch (serif->serif_type) {
		case UI_SPI4:
			return 0;

		case UI_I2C:
			return inv_i2c_master_read_register(ICM_I2C_ADDR, reg, rlen, rbuffer);

		default:
			return -1;
	}
}

/*
 * inv_io_hal_write_reg()
 * IMU 드라이버 콜백: 지정된 레지스터에 데이터를 쓴다.
 * 시리얼 타입에 따라 I2C 또는 SPI 쓰기를 수행한다.
 */
int inv_io_hal_write_reg(struct inv_imu_serif *serif, uint8_t reg, const uint8_t * wbuffer, uint32_t wlen)
{
	switch (serif->serif_type) {
		case UI_SPI4:
			return 0;

		case UI_I2C:
			return inv_i2c_master_write_register(ICM_I2C_ADDR, reg, wlen, wbuffer);

		default:
			return -1;
	}
}

/*
 * cat_read()
 * 범용 I2C 읽기 함수 (디버그/레거시용).
 * 8바이트를 읽어 첫 번째 바이트를 반환하고, 읽은 데이터를 콘솔에 출력한다.
 * 참고: 현재 실제 운용에서는 사용되지 않으며, 디버그 목적으로 남아 있다.
 */
uint8_t cat_read(uint8_t device_id, uint8_t address, uint8_t *data)
	{

				uint8_t read_data = 0;
		char adata[8];
		 ret_code_t err_code;
    //address = 1|(address<<1);
    address = (address & 0xFF);

    /* 레지스터 주소 전송 (STOP 없이, Repeated START 준비) */
    err_code = nrfx_twi_tx(&m_twi_icm42670, device_id, &address, 1, true);
    if (err_code != NRF_SUCCESS) {
        // Handle error
       // return;
    }

    /* 8바이트 데이터 수신 */
    err_code = nrfx_twi_rx(&m_twi_icm42670, device_id, data, 8);
    if (err_code != NRF_SUCCESS) {
        // Handle error
        return 0;
    }
		read_data = data[0];
		memcpy(adata,data,8);
		printf("Data %s  . \r\n", adata);
		 return read_data;







 }

/*
 * cat_write()
 * 범용 I2C 쓰기 함수 (디버그/레거시용).
 * 주소 1바이트 + 데이터 1바이트를 전송한다.
 * 참고: buffer에 6바이트를 복사하지만, 실제 전송은 2바이트만 수행한다.
 */
void cat_write(uint8_t device_id, uint8_t address, uint8_t *data){

		uint8_t buffer[7]={0x00,0x00,0x00,0x00,0x00,0x00,0x00};

    address = (address & 0xFF);

		    buffer[0] = (address);
		//buffer[1] =(data & 0xFF);
		memcpy(buffer+1,data,6);
    ret_code_t err_code;
		//err_code = nrf_drv_twi_tx(&m_twi_ir, device_id, 0x00, 1, false);

		/* 주소(1바이트) + 데이터(1바이트) = 2바이트 전송 */
		err_code = nrfx_twi_tx(&m_twi_icm42670, device_id, buffer, 2, false);
//    err_code = nrf_drv_twi_tx(&m_twi_ir, device_id, buffer, 2, false);
//	nrfx_twi_rx(&m_twi_icm42670, device_id, p_data, length);
//	nrfx_twi_tx(&m_twi_icm42670, device_id, p_data, length, no_stop);
		printf("Data %x  %x    %x   %x. \r\n", buffer[0], buffer[1], buffer[2], buffer[3]);

		//err_code = nrf_drv_twi_tx(&m_twi_ir, device_id, buffer, 6, false);
    if (err_code != NRF_SUCCESS) {

        printf("TWI  Error.");



 	}

}

