#include <string.h>
#include "i2c.h"
#include "app_error.h"
#include "app_util_platform.h"
#include "bsp.h"
#include "error.h"
#include "nrf.h"
#include "nrf_delay.h"
#include "nrf_drv_twi.h"
#include "nrf_log.h"

static uint8_t slave_addr;
static const nrf_drv_twi_t m_twi_master = NRF_DRV_TWI_INSTANCE(1);

int I2C_Init(void) {
	ret_code_t ret;
	const nrf_drv_twi_config_t config =
	{
		.scl                = TWI_SCL_M,
		.sda                = TWI_SDA_M,
		.frequency          = (nrf_drv_twi_frequency_t) NRF_TWI_FREQ_400K,
		.interrupt_priority = APP_IRQ_PRIORITY_HIGH
	};

    ret = nrf_drv_twi_init(&m_twi_master, &config, NULL, NULL);
	if (NRF_SUCCESS != ret) {
	    NRF_LOG_WARNING("");
        return ERROR_I2C_INITIALIZE_FAIL;
	}
    
	nrf_drv_twi_enable(&m_twi_master);

	return 0;
}

void I2C_Uninit(void) {
	nrf_drv_twi_uninit(&m_twi_master);
}

void I2C_SlaveAddr(uint8_t addr) {
	slave_addr = addr;
}

ret_code_t I2C_WriteReg(uint8_t reg, uint8_t data) {
	static ret_code_t ret;
	uint8_t buff[2];
	
	buff[0] = reg;
	buff[1] = data;
	
	ret = nrf_drv_twi_tx(&m_twi_master, slave_addr, buff, sizeof(buff), false);
	
	return ret;
}

ret_code_t I2C_WriteData(uint8_t reg,uint8_t *data,int len) {
	static ret_code_t ret;
	uint8_t buff[256];

	buff[0] = reg;
	memcpy(&buff[1],data,len);
	
	ret = nrf_drv_twi_tx(&m_twi_master, slave_addr, buff, len+1, false);
	
	return ret;
}


ret_code_t I2C_ReadBatchData(uint8_t reg,uint8_t *data,int len) {
	static ret_code_t ret;
	
	ret = nrf_drv_twi_tx(&m_twi_master, slave_addr, &reg, 1, true);
	if(NRF_SUCCESS != ret)
		return ret;
	
	ret = nrf_drv_twi_rx(&m_twi_master, slave_addr, data,len);
	
	return ret;
}

ret_code_t I2C_ReadData(uint8_t reg,uint8_t *data,int len) {
	static ret_code_t ret;
	
	ret = nrf_drv_twi_tx(&m_twi_master, slave_addr, &reg, 1, true);
	if(NRF_SUCCESS != ret)
		return ret;
	
	ret = nrf_drv_twi_rx(&m_twi_master, slave_addr, data,len);
	
	return ret;
}

uint8_t I2C_ReadReg(uint8_t reg) {
	static ret_code_t ret;
	uint8_t data = 0xFD;
	
	ret = nrf_drv_twi_tx(&m_twi_master, slave_addr, &reg, 1, true);
	if(NRF_SUCCESS != ret)
		return data;
	ret = nrf_drv_twi_rx(&m_twi_master, slave_addr, &data,1);
	
	return data;
}