#include "vl53l0x.h"


bool checkResult(VL53L0X_Error error, VL53L0XResult* result, VL53L0XPosition position)
{
  if (error != VL53L0X_ERROR_NONE)
  {
    result->error = error;
    result->position = position;
    return false;
  }

  return true;
}


bool VL53L0X::init(uint8_t address, VL53L0XResult* result)
{
  device.I2cDevAddr = 0x29;
  device.comms_type = 1;
  device.comms_speed_khz = 400;
  device.i2c = &Wire;

  if (!checkResult(VL53L0X_DataInit(&device), result, VL53L0X_POSITION_DATAINIT))
    return false;


  auto setAddress = address & 0x7F;

  // * 2 to convert from 7 to 8 bits
  if (!checkResult(VL53L0X_SetDeviceAddress(&device, setAddress * 2), result, VL53L0X_POSITION_SETDEVICEADDRESS))
    return false;

  delay(10);
  device.I2cDevAddr = setAddress;

  VL53L0X_DeviceInfo_t deviceInfo;
  if (!checkResult(VL53L0X_GetDeviceInfo(&device, &deviceInfo), result, VL53L0X_POSITION_GETDEVICEINFO))
    return false;

  if (!checkResult(VL53L0X_StaticInit(&device), result, VL53L0X_POSITION_STATICINIT))
    return false;

  uint32_t refSpadCount;
  uint8_t isApertureSpads;

  if (!checkResult(VL53L0X_PerformRefSpadManagement(&device, &refSpadCount, &isApertureSpads), result, VL53L0X_POSITION_REFSPAD))
    return false;

  // TODO expose as API to be run before changing desk positions
  uint8_t vhvSettings;
  uint8_t phaseCal;

  if (!checkResult(VL53L0X_PerformRefCalibration(&device, &vhvSettings, &phaseCal), result, VL53L0X_POSITION_REFCAL))
    return false;

  if (!checkResult(VL53L0X_SetDeviceMode(&device, VL53L0X_DEVICEMODE_SINGLE_RANGING), result, VL53L0X_POSITION_DEVICEMODE))
    return false;

  if (!checkResult(VL53L0X_SetLimitCheckEnable(&device, VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, 1), result, VL53L0X_POSITION_LIMITRANGE))
    return false;

  if (!checkResult(VL53L0X_SetLimitCheckEnable(&device, VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE, 1), result, VL53L0X_POSITION_LIMITRATE))
    return false;

  if (!checkResult(VL53L0X_SetLimitCheckEnable(&device, VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD, 1), result, VL53L0X_POSITION_TRESHOLD1))
    return false;

  if (!checkResult(VL53L0X_SetLimitCheckValue(&device, VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD, (FixPoint1616_t)( 1.5 * 0.023 * 65536)), result, VL53L0X_POSITION_TRESHOLD2))
    return false;

  result->error = VL53L0X_ERROR_NONE;
  result->position = VL53L0X_POSITION_UNKNOWN;
  return true;
}


bool VL53L0X::setMeasurementTimingBudget(uint32_t budget_us, VL53L0X_Error* error)
{
  *error = VL53L0X_SetMeasurementTimingBudgetMicroSeconds(&device, budget_us);
  return (*error == VL53L0X_ERROR_NONE);
}


bool VL53L0X::getSingleRangingMeasurement(VL53L0X_RangingMeasurementData_t *data, VL53L0X_Error* error)
{
  *error = VL53L0X_PerformSingleRangingMeasurement(&device, data);
  return (*error == VL53L0X_ERROR_NONE);
}