#include "VL53L0X.h"

bool VL53L0X::init(uint8_t address)
{
  device.I2cDevAddr = address;
  device.comms_type = 1;
  device.comms_speed_khz = 400;
  device.i2c = &Wire;

  if (!VL53L0X_DataInit(&device))
    return false;

  //if (!VL53L0X_GetDeviceInfo(&device, &deviceInfo))
  //  return false;

  if (!VL53L0X_StaticInit(&device))
    return false;


  uint32_t refSpadCount;
  uint8_t isApertureSpads;

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


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

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

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

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

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

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

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

  return true;
}


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