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

/* 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"
#include "meas_pd_48.h"

/* I2C number and slave address for INV device */
#define ICM_I2C_ADDR     0x68
#define INV_MAX_SERIAL_WRITE 16

/* TWI instance. */
const nrfx_twi_t m_twi_icm42670 = NRFX_TWI_INSTANCE(ICM42670_I2C_INSTANCE);

void inv_i2c_master_uninitialize(void){
    nrfx_twi_disable(&m_twi_icm42670);
    nrfx_twi_uninit(&m_twi_icm42670);
}

static 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,
    };

    err_code = nrfx_twi_init(&m_twi_icm42670, &twi_icm42670_config, NULL, NULL);
    APP_ERROR_CHECK(err_code);

    nrfx_twi_enable(&m_twi_icm42670);
}


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;
}


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;
}


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;

	ret = icm42670_twi_tx(Address, &addr8, 1, true);
	if(ret != NRF_SUCCESS) {
		ret = icm42670_twi_tx(Address, &addr8, 1, true);
		if(ret != NRF_SUCCESS) {
		  	printf("ERR! i2c read-1\r\n");
		}
	}

	ret = icm42670_twi_rx(Address, RegisterValue, RegisterLen); 
	if(ret != NRF_SUCCESS) {
		ret = icm42670_twi_rx(Address, RegisterValue, RegisterLen); 
		if(ret != NRF_SUCCESS) {
		  	printf("ERR! i2c read-2\r\n");
		}
	}

	return ret;
}

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]; /* Addr + data */

	buffer[0] = (uint8_t)RegisterAddr;
	memcpy(buffer+1, RegisterValue, RegisterLen);
	ret = icm42670_twi_tx(Address, buffer, RegisterLen+1, false);
	if(ret != NRF_SUCCESS) {
		ret = icm42670_twi_tx(Address, buffer, RegisterLen+1, false);
		if(ret != NRF_SUCCESS) {
		  	printf("ERR! i2c write\r\n");
		}
	}

	return ret;
}





int inv_io_hal_init(struct inv_imu_serif *serif)
{

	switch (serif->serif_type) {
		case UI_SPI4:
		{
			break;
		}

		case UI_I2C:
			inv_i2c_master_initialize();
			break;
		
		default:
			return -1;
	}

	return 0;
}


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;
	}
}

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;
	}
}
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);
    err_code = nrfx_twi_tx(&m_twi_icm42670, device_id, &address, 1, true);
    if (err_code != NRF_SUCCESS) {
        // Handle error
       // return;
    }

    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;
		
		
		
		
		


 }

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);
		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.");


 		
 	}
	
}