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

/*******************************************************************************
 * [Module overview] ICM42670P IMU sensor I2C communication interface
 *
 * Low-level interface module for communicating with the ICM42670P IMU sensor
 * via the nRF52840 TWI (I2C) hardware.
 *
 * - I2C slave address: 0x68 (ICM42670P default)
 * - I2C pin config: SCL=P1.14, SDA=P1.15 (defined in system_interface.h)
 * - TWI instance: NRFX_TWI_INSTANCE(0)
 * - Bus speed: 100kHz (NRF_TWI_FREQ_100K)
 *
 * Main function flow:
 *   inv_io_hal_init()      -> Initialize I2C or SPI (only I2C implemented)
 *   inv_io_hal_read_reg()  -> Register read (TX address -> RX data)
 *   inv_io_hal_write_reg() -> Register write (TX address+data at once)
 *
 * Error handling: All I2C read/write operations retry once on failure
 *
 * Note: SPI4 code path exists but is not implemented;
 *       only I2C (UI_I2C) is used in production.
 ******************************************************************************/

/* 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 slave address and max serial write byte count */
#define ICM_I2C_ADDR     0x68
#define INV_MAX_SERIAL_WRITE 16

/* TWI (I2C) instance — uses ICM42670_I2C_INSTANCE(0) from system_interface.h */
const nrfx_twi_t m_twi_icm42670 = NRFX_TWI_INSTANCE(ICM42670_I2C_INSTANCE);

/*
 * inv_i2c_master_uninitialize()
 * Disables the I2C bus and releases the TWI instance.
 * Called before entering sleep mode or before re-initialization.
 */
void inv_i2c_master_uninitialize(void){
    nrfx_twi_disable(&m_twi_icm42670);
    nrfx_twi_uninit(&m_twi_icm42670);
}

/*
 * inv_i2c_master_initialize()
 * Initializes and enables the nRF52840 TWI hardware.
 * - SCL: P1.14, SDA: P1.15
 * - Speed: 100kHz
 * - Interrupt priority: highest (APP_IRQ_PRIORITY_HIGH)
 * - No event handler (blocking mode)
 */
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,
    };

    /* Initialize TWI driver (event handler=NULL -> blocking mode) */
    err_code = nrfx_twi_init(&m_twi_icm42670, &twi_icm42670_config, NULL, NULL);
    APP_ERROR_CHECK(err_code);

    /* Enable TWI hardware — tx/rx available after this */
    nrfx_twi_enable(&m_twi_icm42670);
}


/*
 * icm42670_twi_tx()
 * I2C transmit wrapper. Calls nrfx_twi_tx to send data.
 * If no_stop=true, STOP condition is omitted (used for 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 receive wrapper. Calls nrfx_twi_rx to receive data.
 */
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()
 * Reads data from a specific ICM42670P register.
 *
 * Sequence:
 *   1) TX: Send 1-byte register address (no_stop=true -> prepare Repeated START)
 *   2) RX: Receive data of specified length
 *
 * Error handling: Retries once on TX or RX failure.
 */
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;

	/* Step 1: Send register address to read (no STOP -> uses Repeated START) */
	ret = icm42670_twi_tx(Address, &addr8, 1, true);
	if(ret != NRF_SUCCESS) {
		/* Retry once on failure */
		ret = icm42670_twi_tx(Address, &addr8, 1, true);
		if(ret != NRF_SUCCESS) {
		  	printf("ERR! i2c read-1\r\n");
		}
	}

	/* Step 2: Receive data from the register */
	ret = icm42670_twi_rx(Address, RegisterValue, RegisterLen);
	if(ret != NRF_SUCCESS) {
		/* Retry once on failure */
		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()
 * Writes data to a specific ICM42670P register.
 *
 * Sequence:
 *   1) Place register address in buffer[0], data in buffer[1..N]
 *   2) TX: Send address+data at once (no_stop=false -> includes STOP condition)
 *
 * Error handling: Retries once on failure.
 */
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]; /* register address (1) + data (max 16 bytes) */

	/* Buffer layout: [register address][data bytes] */
	buffer[0] = (uint8_t)RegisterAddr;
	memcpy(buffer+1, RegisterValue, RegisterLen);

	/* Send address+data at once */
	ret = icm42670_twi_tx(Address, buffer, RegisterLen+1, false);
	if(ret != NRF_SUCCESS) {
		/* Retry once on failure */
		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()
 * Initializes the serial interface (I2C or SPI) used by the IMU driver.
 * Branches based on serif->serif_type; only I2C is currently implemented.
 * Returns: 0=success, -1=unsupported interface type
 */

int inv_io_hal_init(struct inv_imu_serif *serif)
{

	switch (serif->serif_type) {
		case UI_SPI4:
		{
			/* SPI4 init — not implemented (only I2C is used) */
			break;
		}

		case UI_I2C:
			inv_i2c_master_initialize();
			break;

		default:
			return -1;
	}

	return 0;
}


/*
 * inv_io_hal_read_reg()
 * IMU driver callback: reads data from the specified register.
 * Performs I2C or SPI read depending on the serial type.
 */
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 driver callback: writes data to the specified register.
 * Performs I2C or SPI write depending on the serial type.
 */
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()
 * Generic I2C read function (debug/legacy).
 * Reads 8 bytes, returns the first byte, and prints the data to console.
 * Note: Not used in production; kept for debug purposes.
 */
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);

    /* Send register address (no STOP, prepare Repeated START) */
    err_code = nrfx_twi_tx(&m_twi_icm42670, device_id, &address, 1, true);
    if (err_code != NRF_SUCCESS) {
        // Handle error
       // return;
    }

    /* Receive 8 bytes of data */
    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()
 * Generic I2C write function (debug/legacy).
 * Sends 1 byte address + 1 byte data.
 * Note: Copies 6 bytes into buffer, but only transmits 2 bytes.
 */
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);

		/* Address (1 byte) + data (1 byte) = 2 bytes transmitted */
		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.");



 	}

}

