#include "imsensors/common/sensors_common.h"

/* SDK drivers */
#include "fsl_i2c.h"
#include "fsl_pint.h"

/* Utils */
#include "util_systick.h"

ims_ret_t sensors_impl_i2c_xfer(void *pdev, ims_i2c_xfer_desc_t *xfer) {
    uint16_t addr = ((uint32_t)pdev & 0xFFU);

    if (xfer->tx_size > 0) {
        if (I2C_MasterStart(I2C0, addr, kI2C_Write) != kStatus_Success) {
            goto xfer_fail;
        }

        if (I2C_MasterWriteBlocking(I2C0, xfer->tx_data, xfer->tx_size, kI2C_TransferNoStopFlag) != kStatus_Success) {
            goto xfer_fail;
        }
    }

    if (xfer->rx_size > 0) {
        if (xfer->tx_size > 0) {
            if (I2C_MasterRepeatedStart(I2C0, addr, kI2C_Read) != kStatus_Success) {
                goto xfer_fail;
            }
        } else {
            if (I2C_MasterStart(I2C0, addr, kI2C_Read) != kStatus_Success) {
                goto xfer_fail;
            }
        }

        if (I2C_MasterReadBlocking(I2C0, xfer->rx_data, xfer->rx_size, kI2C_TransferNoStopFlag) != kStatus_Success) {
            goto xfer_fail;
        }
    }

    if (I2C_MasterStop(I2C0) != kStatus_Success) {
        goto xfer_fail;
    }

    return IMS_SUCCESS;

xfer_fail:
    /* Clean-up */
    I2C_MasterReset(I2C0);
    return IMS_FAIL;
}

ims_ret_t sensors_impl_delay(void *pdev, uint32_t msec) {
    util_systick_delay(msec);

    return IMS_SUCCESS;
}

void bmm_drdy_callback(pint_pin_int_t pintr, uint32_t pmatch_status) {}