#include "nrf_log.h"
#include "app_twi.h"

//TWI instance handle
APP_TWI_DEF(m_app_twi, APP_TWI_QUEUE_SIZE, 0);

//TWI slave address
#define TWI_SLAVE_ADDR 0x1D

//how many bytes to read from slave
#define TWI_SLAVE_REG_LEN 7

static ret_code_t twi_init(void)
{
	ret_code_t err_code;
	nrf_drv_twi_config_t twi_config=NRF_DRV_TWI_DEFAULT_CONFIG;

	twi_config.sda=26;
	twi_config.scl=27;

	err_code=app_twi_init(&m_app_twi, &twi_config);

	APP_ERROR_CHECK(err_code);

	return err_code;
}

static void read_reg(const uint8_t reg_addr, uint8_t* const p_buff, const uint8_t len)
{
    ret_code_t err_code;
	
	static uint8_t tx_buff[1];
	tx_buff[0]=reg_addr;
	app_twi_transfer_t const twi_transfers[] = {
		APP_TWI_WRITE(TWI_SLAVE_ADDR, tx_buff, sizeof(tx_buff), APP_TWI_NO_STOP),
		APP_TWI_READ(TWI_SLAVE_ADDR, p_buff, len, 0)
	};
	
	err_code = app_twi_perform(&m_app_twi, twi_transfers, sizeof(twi_transfers) / sizeof(twi_transfers[0]), 0);
    APP_ERROR_CHECK(err_code);
}

static void write_reg(const uint8_t reg_addr, uint8_t* const p_buff, const uint8_t len, const uint8_t stop)
{
    ret_code_t err_code;
    uint8_t tx_buff[TWI_SLAVE_REG_LEN+1]={0};
    tx_buff[0] = reg_addr;
    memcpy(&tx_buff[1], p_buff, len);
	
	app_twi_transfer_t const twi_transfers[] = {
		APP_TWI_WRITE(TWI_SLAVE_ADDR, tx_buff, len + 1, (stop == 1) ? 0 : APP_TWI_NO_STOP)
	};
	
	err_code = app_twi_perform(&m_app_twi, twi_transfers, sizeof(twi_transfers) / sizeof(twi_transfers[0]), 0);
    APP_ERROR_CHECK(err_code);
}

static void test_case(uint8_t tx_stop)
{
	uint8_t rx_buff[TWI_SLAVE_REG_LEN] = { 0 };
    uint8_t tx_buff[] = { 0 };

    write_reg(0, tx_buff, sizeof(tx_buff), tx_stop);
    read_reg(0, rx_buff, sizeof(rx_buff));

    NRF_LOG_HEXDUMP_DEBUG(rx_buff, sizeof(rx_buff));
}

void twi_slave_tiny_main_unwrap(void)
{
    APP_ERROR_CHECK(twi_init());
	
	//this will complete just fine
	test_case(1);
	
	//this will pull SCL low and get stuck forever
	test_case(0);
}
