#include "vl53l0x_platform.h"
#include "vl53l0x_api.h"

#if VL53_USE_FREERTOS_DELAY
#include "FreeRTOS.h"
#include "task.h"
#endif

#if defined(configSUPPORT_STATIC_ALLOCATION) || defined(configUSE_MUTEXES)
#include "semphr.h"
#define VL53_USE_FREERTOS_MUTEX 1
#else
#define VL53_USE_FREERTOS_MUTEX 0
#endif

static VL53L0X_Error vl53_hal_to_pal(HAL_StatusTypeDef hal_status)
{
    switch (hal_status) {
    case HAL_OK:
        return VL53L0X_ERROR_NONE;
    case HAL_TIMEOUT:
        return VL53L0X_ERROR_TIME_OUT;
    default:
        return VL53L0X_ERROR_CONTROL_INTERFACE;
    }
}

static VL53L0X_Error vl53_validate_dev(VL53L0X_DEV Dev)
{
    if ((Dev == NULL) || (Dev->hi2c == NULL)) {
        return VL53L0X_ERROR_CONTROL_INTERFACE;
    }
    return VL53L0X_ERROR_NONE;
}

static VL53L0X_Error vl53_bus_lock(VL53L0X_DEV Dev)
{
#if VL53_USE_FREERTOS_MUTEX
    if ((Dev != NULL) && (Dev->bus_lock != NULL)) {
        if (xTaskGetSchedulerState() != taskSCHEDULER_NOT_STARTED) {
            const TickType_t wait_ticks = pdMS_TO_TICKS((Dev->io_timeout_ms == 0u) ? 10u : Dev->io_timeout_ms);
            if (xSemaphoreTake((SemaphoreHandle_t)Dev->bus_lock, wait_ticks) != pdTRUE) {
                return VL53L0X_ERROR_TIME_OUT;
            }
        }
    }
#else
    (void)Dev;
#endif
    return VL53L0X_ERROR_NONE;
}

static void vl53_bus_unlock(VL53L0X_DEV Dev)
{
#if VL53_USE_FREERTOS_MUTEX
    if ((Dev != NULL) && (Dev->bus_lock != NULL)) {
        if (xTaskGetSchedulerState() != taskSCHEDULER_NOT_STARTED) {
            (void)xSemaphoreGive((SemaphoreHandle_t)Dev->bus_lock);
        }
    }
#else
    (void)Dev;
#endif
}

VL53L0X_Error VL53L0X_LockSequenceAccess(VL53L0X_DEV Dev)
{
    (void)Dev;
    /* Shared I2C bus is already serialized per I/O transaction below. */
    return VL53L0X_ERROR_NONE;
}

VL53L0X_Error VL53L0X_UnlockSequenceAccess(VL53L0X_DEV Dev)
{
    (void)Dev;
    return VL53L0X_ERROR_NONE;
}

void VL53L0X_PlatformAttachBus(VL53L0X_DEV Dev,
                               I2C_HandleTypeDef *hi2c,
                               uint8_t i2c_addr_8bit,
                               uint16_t io_timeout_ms,
                               void *bus_lock)
{
    if (Dev == NULL) {
        return;
    }
    Dev->hi2c = hi2c;
    Dev->I2cDevAddr = i2c_addr_8bit;
    Dev->io_timeout_ms = (io_timeout_ms == 0u) ? 100u : io_timeout_ms;
    Dev->bus_lock = bus_lock;
}

void VL53L0X_PlatformAttachPins(VL53L0X_DEV Dev,
                                GPIO_TypeDef *xshut_port,
                                uint16_t xshut_pin,
                                GPIO_TypeDef *gpio1_port,
                                uint16_t gpio1_pin)
{
    if (Dev == NULL) {
        return;
    }
    Dev->xshut_port = xshut_port;
    Dev->xshut_pin = xshut_pin;
    Dev->gpio1_port = gpio1_port;
    Dev->gpio1_pin = gpio1_pin;
}

VL53L0X_Error VL53L0X_PlatformSetXShut(VL53L0X_DEV Dev, GPIO_PinState state)
{
    if ((Dev == NULL) || (Dev->xshut_port == NULL)) {
        return VL53L0X_ERROR_INVALID_PARAMS;
    }
    HAL_GPIO_WritePin(Dev->xshut_port, Dev->xshut_pin, state);
    return VL53L0X_ERROR_NONE;
}

VL53L0X_Error VL53L0X_PlatformBootDevice(VL53L0X_DEV Dev, uint32_t reset_low_ms, uint32_t boot_wait_ms)
{
    VL53L0X_Error status = VL53L0X_PlatformSetXShut(Dev, GPIO_PIN_RESET);
    if (status != VL53L0X_ERROR_NONE) {
        return status;
    }

    if (xTaskGetSchedulerState() != taskSCHEDULER_NOT_STARTED) {
        vTaskDelay(pdMS_TO_TICKS(reset_low_ms));
    } else {
        HAL_Delay(reset_low_ms);
    }

    status = VL53L0X_PlatformSetXShut(Dev, GPIO_PIN_SET);
    if (status != VL53L0X_ERROR_NONE) {
        return status;
    }

    if (xTaskGetSchedulerState() != taskSCHEDULER_NOT_STARTED) {
        vTaskDelay(pdMS_TO_TICKS(boot_wait_ms));
    } else {
        HAL_Delay(boot_wait_ms);
    }

    return VL53L0X_ERROR_NONE;
}

uint32_t VL53L0X_PlatformGetTick(void)
{
    return HAL_GetTick();
}

VL53L0X_Error VL53L0X_WriteMulti(VL53L0X_DEV Dev, uint8_t index, uint8_t *pdata, uint32_t count)
{
    HAL_StatusTypeDef hal_status;
    VL53L0X_Error status;

    if ((pdata == NULL) || (count == 0u) || (count > 255u)) {
        return VL53L0X_ERROR_INVALID_PARAMS;
    }
    status = vl53_validate_dev(Dev);
    if (status != VL53L0X_ERROR_NONE) {
        return status;
    }
    status = vl53_bus_lock(Dev);
    if (status != VL53L0X_ERROR_NONE) {
        return status;
    }

    hal_status = HAL_I2C_Mem_Write(Dev->hi2c,
                                   Dev->I2cDevAddr,
                                   index,
                                   I2C_MEMADD_SIZE_8BIT,
                                   pdata,
                                   (uint16_t)count,
                                   Dev->io_timeout_ms);
    vl53_bus_unlock(Dev);
    return vl53_hal_to_pal(hal_status);
}

VL53L0X_Error VL53L0X_ReadMulti(VL53L0X_DEV Dev, uint8_t index, uint8_t *pdata, uint32_t count)
{
    HAL_StatusTypeDef hal_status;
    VL53L0X_Error status;

    if ((pdata == NULL) || (count == 0u) || (count > 255u)) {
        return VL53L0X_ERROR_INVALID_PARAMS;
    }
    status = vl53_validate_dev(Dev);
    if (status != VL53L0X_ERROR_NONE) {
        return status;
    }
    status = vl53_bus_lock(Dev);
    if (status != VL53L0X_ERROR_NONE) {
        return status;
    }

    hal_status = HAL_I2C_Mem_Read(Dev->hi2c,
                                  Dev->I2cDevAddr,
                                  index,
                                  I2C_MEMADD_SIZE_8BIT,
                                  pdata,
                                  (uint16_t)count,
                                  Dev->io_timeout_ms);
    vl53_bus_unlock(Dev);
    return vl53_hal_to_pal(hal_status);
}

VL53L0X_Error VL53L0X_WrByte(VL53L0X_DEV Dev, uint8_t index, uint8_t data)
{
    return VL53L0X_WriteMulti(Dev, index, &data, 1u);
}

VL53L0X_Error VL53L0X_WrWord(VL53L0X_DEV Dev, uint8_t index, uint16_t data)
{
    uint8_t buffer[2];
    buffer[0] = (uint8_t)(data >> 8);
    buffer[1] = (uint8_t)(data & 0xFFu);
    return VL53L0X_WriteMulti(Dev, index, buffer, 2u);
}

VL53L0X_Error VL53L0X_WrDWord(VL53L0X_DEV Dev, uint8_t index, uint32_t data)
{
    uint8_t buffer[4];
    buffer[0] = (uint8_t)(data >> 24);
    buffer[1] = (uint8_t)(data >> 16);
    buffer[2] = (uint8_t)(data >> 8);
    buffer[3] = (uint8_t)(data & 0xFFu);
    return VL53L0X_WriteMulti(Dev, index, buffer, 4u);
}

VL53L0X_Error VL53L0X_RdByte(VL53L0X_DEV Dev, uint8_t index, uint8_t *data)
{
    if (data == NULL) {
        return VL53L0X_ERROR_INVALID_PARAMS;
    }
    return VL53L0X_ReadMulti(Dev, index, data, 1u);
}

VL53L0X_Error VL53L0X_RdWord(VL53L0X_DEV Dev, uint8_t index, uint16_t *data)
{
    uint8_t buffer[2] = {0u, 0u};
    VL53L0X_Error status;

    if (data == NULL) {
        return VL53L0X_ERROR_INVALID_PARAMS;
    }
    status = VL53L0X_ReadMulti(Dev, index, buffer, 2u);
    if (status == VL53L0X_ERROR_NONE) {
        *data = (((uint16_t)buffer[0]) << 8) | buffer[1];
    }
    return status;
}

VL53L0X_Error VL53L0X_RdDWord(VL53L0X_DEV Dev, uint8_t index, uint32_t *data)
{
    uint8_t buffer[4] = {0u, 0u, 0u, 0u};
    VL53L0X_Error status;

    if (data == NULL) {
        return VL53L0X_ERROR_INVALID_PARAMS;
    }
    status = VL53L0X_ReadMulti(Dev, index, buffer, 4u);
    if (status == VL53L0X_ERROR_NONE) {
        *data = (((uint32_t)buffer[0]) << 24)
              | (((uint32_t)buffer[1]) << 16)
              | (((uint32_t)buffer[2]) << 8)
              | ((uint32_t)buffer[3]);
    }
    return status;
}

VL53L0X_Error VL53L0X_UpdateByte(VL53L0X_DEV Dev, uint8_t index, uint8_t AndData, uint8_t OrData)
{
    VL53L0X_Error status;
    uint8_t data = 0u;

    status = VL53L0X_RdByte(Dev, index, &data);
    if (status != VL53L0X_ERROR_NONE) {
        return status;
    }
    data = (uint8_t)((data & AndData) | OrData);
    return VL53L0X_WrByte(Dev, index, data);
}

VL53L0X_Error VL53L0X_PollingDelay(VL53L0X_DEV Dev)
{
    (void)Dev;
#if VL53_USE_FREERTOS_DELAY
    if (xTaskGetSchedulerState() != taskSCHEDULER_NOT_STARTED) {
        vTaskDelay(pdMS_TO_TICKS(2u));
    } else {
        HAL_Delay(2u);
    }
#else
    HAL_Delay(2u);
#endif
    return VL53L0X_ERROR_NONE;
}

VL53L0X_Error VL53L0X_PlatformChangeAddress(VL53L0X_DEV Dev, uint8_t new_addr_8bit)
{
    VL53L0X_Error status;

    if (new_addr_8bit == 0u) {
        return VL53L0X_ERROR_INVALID_PARAMS;
    }
    status = VL53L0X_SetDeviceAddress(Dev, new_addr_8bit);
    if (status == VL53L0X_ERROR_NONE) {
        Dev->I2cDevAddr = new_addr_8bit;
        if (xTaskGetSchedulerState() != taskSCHEDULER_NOT_STARTED) {
            vTaskDelay(pdMS_TO_TICKS(2u));
        } else {
            HAL_Delay(2u);
        }
    }
    return status;
}