#include "nrf_drv_twi.h"
#include "nrf_gpio.h"
#include "nrf_log.h"

#include "FreeRTOS.h"
#include "queue.h"
#include "semphr.h"
#include "task.h"

#define I2C_SDA NRF_GPIO_PIN_MAP(0, 11)
#define I2C_SCL NRF_GPIO_PIN_MAP(0, 22)

static const nrf_drv_twi_t twi0    = NRF_DRV_TWI_INSTANCE(0);
static SemaphoreHandle_t i2c_sem   = NULL;
static SemaphoreHandle_t i2c_mutex = NULL;
static QueueHandle_t i2c_evt_queue = NULL;

void nrf_drv_twi_evt_handler(nrf_drv_twi_evt_t const *p_event, void *p_context)
{
    BaseType_t xHigherPriorityTaskWoken = pdFALSE;
    xQueueSendFromISR(i2c_evt_queue, p_event, &xHigherPriorityTaskWoken);
    portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
}

void twi0_init(void)
{
    ret_code_t err_code;

    i2c_sem       = xSemaphoreCreateBinary();
    i2c_mutex     = xSemaphoreCreateMutex();
    i2c_evt_queue = xQueueCreate(2, sizeof(nrf_drv_twi_evt_t));

    const nrf_drv_twi_config_t twi0_config = {
        .scl                = I2C_SCL,
        .sda                = I2C_SDA,
        .frequency          = NRF_DRV_TWI_FREQ_100K,
        .interrupt_priority = APP_IRQ_PRIORITY_HIGH,
        .clear_bus_init     = true
    };

    err_code = nrf_drv_twi_init(&twi0, &twi0_config, nrf_drv_twi_evt_handler, NULL);
    APP_ERROR_CHECK(err_code);

    nrf_drv_twi_enable(&twi0);
}

void twi0_write_reg(uint8_t slave_addr, uint8_t reg_addr, uint8_t *data, uint8_t data_len)
{
    xSemaphoreTake(i2c_mutex, portMAX_DELAY);

    static uint8_t i2c_buf[255];
    static nrf_drv_twi_evt_t evt;
    ret_code_t err_code;

    memcpy(i2c_buf, &reg_addr, sizeof(reg_addr));
    memcpy(i2c_buf + sizeof(reg_addr), data, data_len);

    err_code = nrf_drv_twi_tx(&twi0, slave_addr, i2c_buf, data_len + 1, false);
    APP_ERROR_CHECK(err_code);

    xQueueReceive(i2c_evt_queue, &evt, portMAX_DELAY);

    switch (evt.type)
    {
        /* Transfer completed event. */
        case NRF_DRV_TWI_EVT_DONE:
            // TODO...
            break;
        /* Error event: NACK received after sending the address. */
        case NRF_DRV_TWI_EVT_ADDRESS_NACK:
            // TODO...
            __BKPT(255);
            break;
        /* Error event: NACK received after sending a data byte. */
        case NRF_DRV_TWI_EVT_DATA_NACK:
            // TODO...
            __BKPT(255);
            break;
        default:
            __BKPT(255);
            break;
    }

    xSemaphoreGive(i2c_mutex);

    NRF_LOG_INFO("i2c(W): slave_addr=0x%02x", slave_addr);
    NRF_LOG_HEXDUMP_INFO(i2c_buf, data_len + 1);
}

void twi0_read_reg(uint8_t slave_addr, uint8_t reg_addr, uint8_t *p_rx_buf, uint8_t rx_buffer_length)
{
    xSemaphoreTake(i2c_mutex, portMAX_DELAY);

    nrf_drv_twi_evt_t evt;
    ret_code_t err_code;

    err_code = nrf_drv_twi_tx(&twi0, slave_addr, &reg_addr, sizeof(reg_addr), false);
    APP_ERROR_CHECK(err_code);
    xQueueReceive(i2c_evt_queue, &evt, portMAX_DELAY);

    switch (evt.type)
    {
        /* Transfer completed event. */
        case NRF_DRV_TWI_EVT_DONE:
            // TODO...
            break;
        /* Error event: NACK received after sending the address. */
        case NRF_DRV_TWI_EVT_ADDRESS_NACK:
            // TODO...
            __BKPT(255);
            break;
        /* Error event: NACK received after sending a data byte. */
        case NRF_DRV_TWI_EVT_DATA_NACK:
            // TODO...
            __BKPT(255);
            break;
        default:
            break;
    }

    err_code = nrf_drv_twi_rx(&twi0, slave_addr, p_rx_buf, rx_buffer_length);
    APP_ERROR_CHECK(err_code);
    xQueueReceive(i2c_evt_queue, &evt, portMAX_DELAY);

    switch (evt.type)
    {
        /* Transfer completed event. */
        case NRF_DRV_TWI_EVT_DONE:
            // TODO...
            break;
        /* Error event: NACK received after sending the address. */
        case NRF_DRV_TWI_EVT_ADDRESS_NACK:
            // TODO...
            __BKPT(255);
            break;
        /* Error event: NACK received after sending a data byte. */
        case NRF_DRV_TWI_EVT_DATA_NACK:
            // TODO...
            __BKPT(255);
            break;
        default:
            break;
    }

    xSemaphoreGive(i2c_mutex);

    NRF_LOG_INFO("i2c(R): slave_addr=0x%02x reg_addr=0x%02x", slave_addr, reg_addr);
    NRF_LOG_HEXDUMP_INFO(p_rx_buf, rx_buffer_length);
}