/* FreeRTOS */
#include "FreeRTOS.h"
#include "task.h"

/* SDK drivers */
#include "fsl_lpi2c.h"

/* Board */
#include "board.h"
#include "clock_config.h"
#include "peripherals.h"
#include "pin_mux.h"

/* App */
#include "app_impl_sensors.h"

#define APP_IMPL_I2C_INST LPI2C1
#define APP_IMPL_I2C_CLK  kCLOCK_Lpi2c1

int app_impl_sensors_init(void) {
    CLOCK_EnableClock(APP_IMPL_I2C_CLK);

    lpi2c_master_config_t i2c_cfg;
    LPI2C_MasterGetDefaultConfig(&i2c_cfg);

    i2c_cfg.baudRate_Hz = 400000; /* Fast mode */

    LPI2C_MasterInit(APP_IMPL_I2C_INST, &i2c_cfg, CLOCK_GetClockRootFreq(kCLOCK_Lpi2cClkRoot));

    return 0;
}

ims_ret_t app_impl_sensors_i2c_xfer(void *pdev, ims_i2c_xfer_desc_t *xfer) {
    status_t ret;

    lpi2c_master_transfer_t master_xfer = {
        .slaveAddress   = (uint32_t)pdev & 0xFFU,
        .subaddressSize = 0U,
        .flags          = kLPI2C_TransferDefaultFlag,
    };

    if (xfer->tx_data) {
        master_xfer.direction = kLPI2C_Write;

        master_xfer.data     = xfer->tx_data;
        master_xfer.dataSize = xfer->tx_size;

        ret = LPI2C_MasterTransferBlocking(APP_IMPL_I2C_INST, &master_xfer);
        if (ret != kStatus_Success) {
            return IMS_FAIL;
        }
    }

    if (xfer->rx_data) {
        master_xfer.direction = kLPI2C_Read;

        if (xfer->tx_data) {
            master_xfer.flags = kLPI2C_TransferRepeatedStartFlag;
        }

        master_xfer.data     = xfer->rx_data;
        master_xfer.dataSize = xfer->rx_size;

        ret = LPI2C_MasterTransferBlocking(APP_IMPL_I2C_INST, &master_xfer);
        if (ret != kStatus_Success) {
            return IMS_FAIL;
        }
    }

    return IMS_SUCCESS;
}

ims_ret_t app_impl_sensors_delay(void *pdev, uint32_t delay_ms) {
    vTaskDelay(pdMS_TO_TICKS(delay_ms));
    return IMS_SUCCESS;
}