/**
  ******************************************************************************
  * File Name          : app_tof_vl53l1x_range.c
  * Description        : Main program body
  ******************************************************************************
  *
  * COPYRIGHT(c) 2015 STMicroelectronics
  *
  * Redistribution and use in source and binary forms, with or without modification,
  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  *
  ******************************************************************************
  */
/* Includes ------------------------------------------------------------------*/
#include "stm32xxx_hal.h"

/* USER CODE BEGIN Includes */
#include <string.h>
#include "X-NUCLEO-53L1A1.h"
#include "vl53l1x_api.h"
#include <limits.h>

/**
 * @defgroup Configuration Static configuration
 * @{
 */
#define HAVE_ALARM_DEMO 0

/** Time the initial 53L0 message is shown at power up */
#define WelcomeTime 660

/** Time the initial 53L0 message is shown at power up */
#define ModeChangeDispTime  500

/**
 * Time considered as  a "long push" on push button
 */
#define PressBPSwicthTime   1000

/** @}  */ /* config group */

#ifndef MIN
#   define MIN(a,b) ((a) < (b) ? (a) : (b))
#endif

#define B1_Pin GPIO_PIN_13
#define B1_GPIO_Port GPIOC

#ifndef ARRAY_SIZE
#   define ARRAY_SIZE(x) (sizeof((x))/sizeof((x)[0]))
#endif

/**
 * @defgroup ErrCode Errors code shown on display
 * @{
 */
#define ERR_DETECT             -1
#define ERR_DEMO_RANGE_ONE     1
#define ERR_DEMO_RANGE_MULTI   2


/** }@} */ /* defgroup ErrCode */


/* USER CODE END Includes */

/* Private variables ---------------------------------------------------------*/

/* USER CODE BEGIN PV */
/* Private variables ---------------------------------------------------------*/

typedef enum {
	LONG_RANGE 		= 0, /*!< Long range mode */
	HIGH_SPEED 		= 1, /*!< High speed mode */
	HIGH_ACCURACY	= 2, /*!< High accuracy mode */
} RangingConfig_e;



/**
 * Global ranging struct
 */
VL53L0X_RangingMeasurementData_t RangingMeasurementData;


/** leaky factor for filtered range
 *
 * r(n) = averaged_r(n-1)*leaky +r(n)(1-leaky)
 *
 * */
int LeakyFactorFix8 = (int)( 0.6 *256);
/** How many device detect set by @a DetectSensors()*/
int nDevPresent=0;

int Distance_data;

VL53L0X_Dev_t VL53L0XDevs={
	//.Id=XNUCLEO53L0A1_DEV_CENTER, 
	//.DevLetter='c', 
	.I2cHandle=&XNUCLEO53L0A1_hi2c, 
	.I2cDevAddr=0x52};

/** Timer
 *
 * Used to get time stamp for UART logging
 */
TIM_HandleTypeDef htim5;

/* TIM5 init function */
void MX_TIM5_Init(void)
{

  TIM_MasterConfigTypeDef sMasterConfig;
  TIM_OC_InitTypeDef sConfigOC;

  htim5.Instance = TIM5;
  htim5.Init.Prescaler = 83;
  htim5.Init.CounterMode = TIM_COUNTERMODE_UP;
  htim5.Init.Period = 0xFFFFFFFF;
  htim5.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
  HAL_TIM_OC_Init(&htim5);

  sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
  sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
  HAL_TIMEx_MasterConfigSynchronization(&htim5, &sMasterConfig);

  sConfigOC.OCMode = TIM_OCMODE_TIMING;
  sConfigOC.Pulse = 0;
  sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH;
  sConfigOC.OCFastMode = TIM_OCFAST_DISABLE;
  HAL_TIM_OC_ConfigChannel(&htim5, &sConfigOC, TIM_CHANNEL_1);

}

void TimeStamp_Init(){
    MX_TIM5_Init();
}

void TimeStamp_Reset(){
    HAL_TIM_Base_Start(&htim5);
    htim5.Instance->CNT=0;
}

uint32_t TimeStamp_Get(){
    return htim5.Instance->CNT;
}

/* USER CODE END PV */

/* Private function prototypes -----------------------------------------------*/
void SystemClock_Config(void);
static void MX_GPIO_Init(void);

/* USER CODE BEGIN PFP */
/* Private function prototypes -----------------------------------------------*/
void ResetAndDetectSensor(int SetDisplay);

/* USER CODE END PFP */

/* USER CODE BEGIN 0 */

#define debug_printf    trace_printf
char WelcomeMsg[]="Hi I am Ranging VL53L0X mcu " MCU_NAME "\n";

#define BSP_BP_PORT GPIOC
#define BSP_BP_PIN  GPIO_PIN_13

int BSP_GetPushButton(void){
    GPIO_PinState state ;
    state = HAL_GPIO_ReadPin(BSP_BP_PORT, BSP_BP_PIN);
    return state;
}

/**
 * When button is already pressed it waits for user to release it.
 * if button remains pressed for a given time it returns true.
 * This is used to detect mode switch by long press on blue Push Button
 *
 * As soon as time is elapsed -rb- is displayed to let user know the mode
 * switch is taken into account
 *
 * @return True if button remains pressed more than specified time
 */
int PusbButton_WaitUnPress(void){
    uint32_t TimeStarted;
    TimeStarted = HAL_GetTick();
    while( !BSP_GetPushButton() ){ ; /* debounce */
        if(HAL_GetTick()- TimeStarted> PressBPSwicthTime){
          //  XNUCLEO53L0A1_SetDisplayString (" rb ");
        }
    }
    return  HAL_GetTick() - TimeStarted>PressBPSwicthTime;

}

/**
 * Handle Error
 *
 * Set err on display and loop forever
 * @param err Error case code
 */
void HandleError(int err){
    char msg[16];
    sprintf(msg,"Er%d", err);
    //XNUCLEO53L0A1_SetDisplayString(msg);
    while(1){};
}


/**
 * Reset all sensor then goes true presence detection
 *
 * All present devices are data initiated and assigned with their final address
 * @return
 */
int sts_tof_vl53l0x_DetectSensors(void) {
    int i;
    uint16_t Id;
    int status;
    int FinalAddress;


    /* detect all sensors (even on-board)*/
				
        VL53L0X_Dev_t *pDev;
        pDev = &VL53L0XDevs;
        pDev->I2cDevAddr = 0x52;
        pDev->Present = 0;
        //status = XNUCLEO53L0A1_ResetId(pDev->Id, 1);
        //HAL_Delay(2);
        FinalAddress=0x52;
				do {
					/* Set I2C standard mode (400 KHz) before doing the first register access */
        	//if (status == VL53L0X_ERROR_NONE)
        		status = VL53L0X_WrByte(pDev, 0x88, 0x00);
					
        	/* Try to read one register using default 0x52 address */
            status = VL53L0X_RdWord(pDev, VL53L0X_REG_IDENTIFICATION_MODEL_ID, &Id);
            if (status) {
                debug_printf("#%d Read id fail\n", i);
                break;
            }
            if (Id == 0xEEAA) {
            	/* Sensor is found => Change its I2C address to final one */
                status = VL53L0X_SetDeviceAddress(pDev,FinalAddress);
                if (status != 0) {
                    debug_printf("#i VL53L0X_SetDeviceAddress fail\n", i);
                    break;
                }
                pDev->I2cDevAddr = FinalAddress;
                /* Check all is OK with the new I2C address and initialize the sensor */
                status = VL53L0X_RdWord(pDev, VL53L0X_REG_IDENTIFICATION_MODEL_ID, &Id);
                status = VL53L0X_DataInit(pDev);
                if( status == 0 ){
                    pDev->Present = 1;
                }
                else{
                    debug_printf("VL53L0X_DataInit %d fail\n", i);
                    break;
                }
                debug_printf("VL53L0X %d Present and initiated to final 0x%x\n", i, pDev->I2cDevAddr);
                pDev->Present = 1;
            }
            else {
                debug_printf("#%d unknown ID %x\n", i, Id);
                status = 1;
            }
 
        /* if fail r can't use for any reason then put the  device back to reset */
        //if (status) {
            //XNUCLEO53L0A1_ResetId(i, 0);
        //}
			} while(0);
    /* Display detected sensor(s) */


    return 1;
}

/**
 *  Setup all detected sensors for single shot mode and setup ranging configuration
 */
void sts_tof_vl53l0x_SetupSingleShot(RangingConfig_e rangingConfig)
{
    int status=VL53L0X_ERROR_NONE;
    uint8_t VhvSettings;
    uint8_t PhaseCal;
    uint32_t refSpadCount;
	uint8_t isApertureSpads;
	FixPoint1616_t signalLimit = (FixPoint1616_t)(0.25*65536);
	FixPoint1616_t sigmaLimit = (FixPoint1616_t)(18*65536);
	uint32_t timingBudget = 33000;
	uint8_t preRangeVcselPeriod = 14;
	uint8_t finalRangeVcselPeriod = 10;
  // uart_printf("\r\n#########  start setup for single shot \r\n");
	
  status=VL53L0X_StaticInit(&VL53L0XDevs);
  if( status ){
                debug_printf("VL53L0X_StaticInit failed\n");
  }

  status = VL53L0X_PerformRefCalibration(&VL53L0XDevs, &VhvSettings, &PhaseCal);
			if( status ){
			   debug_printf("VL53L0X_PerformRefCalibration failed\n");
			}

			status = VL53L0X_PerformRefSpadManagement(&VL53L0XDevs, &refSpadCount, &isApertureSpads);
			if( status ){
			   debug_printf("VL53L0X_PerformRefSpadManagement failed\n");
			}

            status = VL53L0X_SetDeviceMode(&VL53L0XDevs, VL53L0X_DEVICEMODE_SINGLE_RANGING); // Setup in single ranging mode
            if( status ){
               debug_printf("VL53L0X_SetDeviceMode failed\n");
            }

            status = VL53L0X_SetLimitCheckEnable(&VL53L0XDevs, VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, 1); // Enable Sigma limit
			if( status ){
			   debug_printf("VL53L0X_SetLimitCheckEnable failed\n");
			}

			status = VL53L0X_SetLimitCheckEnable(&VL53L0XDevs, VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE, 1); // Enable Signa limit
			if( status ){
			   debug_printf("VL53L0X_SetLimitCheckEnable failed\n");
			}
			/* Ranging configuration */
            switch(rangingConfig) {
            case LONG_RANGE:
            	signalLimit = (FixPoint1616_t)(0.1*65536);
            	sigmaLimit = (FixPoint1616_t)(60*65536);
            	timingBudget = 33000;
            	preRangeVcselPeriod = 18;
            	finalRangeVcselPeriod = 14;
            	break;
            case HIGH_ACCURACY:
				signalLimit = (FixPoint1616_t)(0.25*65536);
				sigmaLimit = (FixPoint1616_t)(18*65536);
				timingBudget = 200000;
				preRangeVcselPeriod = 14;
				finalRangeVcselPeriod = 10;
				break;
            case HIGH_SPEED:
				signalLimit = (FixPoint1616_t)(0.25*65536);
				sigmaLimit = (FixPoint1616_t)(32*65536);
				timingBudget = 20000;
				preRangeVcselPeriod = 14;
				finalRangeVcselPeriod = 10;
				break;
            default:
            	debug_printf("Not Supported");
            }

            status = VL53L0X_SetLimitCheckValue(&VL53L0XDevs,  VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE, signalLimit);
			if( status ){
			   debug_printf("VL53L0X_SetLimitCheckValue failed\n");
			}

			status = VL53L0X_SetLimitCheckValue(&VL53L0XDevs,  VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, sigmaLimit);
			if( status ){
			   debug_printf("VL53L0X_SetLimitCheckValue failed\n");
			}

            status = VL53L0X_SetMeasurementTimingBudgetMicroSeconds(&VL53L0XDevs,  timingBudget);
            if( status ){
               debug_printf("VL53L0X_SetMeasurementTimingBudgetMicroSeconds failed\n");
            }

            status = VL53L0X_SetVcselPulsePeriod(&VL53L0XDevs,  VL53L0X_VCSEL_PERIOD_PRE_RANGE, preRangeVcselPeriod);
			if( status ){
			   debug_printf("VL53L0X_SetVcselPulsePeriod failed\n");
			}

            status = VL53L0X_SetVcselPulsePeriod(&VL53L0XDevs,  VL53L0X_VCSEL_PERIOD_FINAL_RANGE, finalRangeVcselPeriod);
			if( status ){
			   debug_printf("VL53L0X_SetVcselPulsePeriod failed\n");
			}

			status = VL53L0X_PerformRefCalibration(&VL53L0XDevs, &VhvSettings, &PhaseCal);
			if( status ){
			   debug_printf("VL53L0X_PerformRefCalibration failed\n");
			}

			VL53L0XDevs.LeakyFirst =1;
  
		 // uart_printf("\r\n#########  end of setup for single shot \r\n");
}


/* Store new ranging data into the device structure, apply leaky integrator if needed */
void sts_tof_vl53l0x_Sensor_SetNewRange(VL53L0X_Dev_t *pDev, VL53L0X_RangingMeasurementData_t *pRange){
    if( pRange->RangeStatus == 0 ){
        if( pDev->LeakyFirst ){
            pDev->LeakyFirst = 0;
            pDev->LeakyRange = pRange->RangeMilliMeter;
        }
        else{
            pDev->LeakyRange = (pDev->LeakyRange*LeakyFactorFix8 + (256-LeakyFactorFix8)*pRange->RangeMilliMeter)>>8;
        }
    }
    else{
        pDev->LeakyFirst = 1;
    }
}

/**
 * Implement the ranging function with all modes managed through the blue button (short and long press)
 * This function implements a while loop until the blue button is pressed
 * @param UseSensorsMask Mask of any sensors to use if not only one present
 * @param rangingConfig Ranging configuration to be used (same for all sensors)
 */
int RangeDemo(int UseSensorsMask, RangingConfig_e rangingConfig){
    int over=0;
    int status;

    /* Setup all sensors in Single Shot mode */
    sts_tof_vl53l0x_SetupSingleShot(rangingConfig);
	
    /* Start ranging until blue button is pressed */
    do{

        	/* Call All-In-One blocking API function */
            status = VL53L0X_PerformSingleRangingMeasurement(&VL53L0XDevs, &RangingMeasurementData);
            if( status ==0 ){
            	/* Push data logging to UART */
            	//trace_printf("%d,%u,%d,%d,%d\n", VL53L0XDevs[SingleSensorNo].Id, TimeStamp_Get(), RangingMeasurementData.RangeStatus, RangingMeasurementData.RangeMilliMeter, RangingMeasurementData.SignalRateRtnMegaCps);
            	sts_tof_vl53l0x_Sensor_SetNewRange(&VL53L0XDevs,&RangingMeasurementData);
                /* Display distance in cm */
            	if( RangingMeasurementData.RangeStatus == 0 ){
             
										uart_printf("########## LeakyRange Distance = %4d mm \r\n", (int)VL53L0XDevs.LeakyRange);
							
                }           
            }
            else{
                HandleError(ERR_DEMO_RANGE_ONE);
            }
        /* Check blue button */
        if( !BSP_GetPushButton() ){
            over=1;
            break;
        }
    }while( !over);
    /* Wait button to be un-pressed to decide if it is a short or long press */
    status=PusbButton_WaitUnPress();
    return status;
}





VL53L0X_Error sts_tof_vl53l0x_range(VL53L0X_Dev_t *dev,VL53L0X_RangingMeasurementData_t *pdata, RangingConfig_e rangingConfig)
{
	int over=0;
	VL53L0X_Error status = VL53L0X_ERROR_NONE;
	
    /* Setup all sensors in Single Shot mode */
    sts_tof_vl53l0x_SetupSingleShot(rangingConfig);
   	/* Call All-In-One blocking API function */
    status = VL53L0X_PerformSingleRangingMeasurement(&VL53L0XDevs, &RangingMeasurementData);
    if( status ==0 ){
           sts_tof_vl53l0x_Sensor_SetNewRange(&VL53L0XDevs,&RangingMeasurementData);
            /* Display distance in mm */
           	if( RangingMeasurementData.RangeStatus == 0 ){								
							  
								Distance_data = (int)VL53L0XDevs.LeakyRange;
								//uart_printf("## MeasureData Distance = %4d mm \r\n", (int)Distance_data,(int) VL53L0XDevs.LeakyRange);
							  
              }
           
          }
            else{
                HandleError(ERR_DEMO_RANGE_ONE);
            }

    return status;
}

/* USER CODE END 0 */

void STS_TOF_VL53L1X_Range_Process(void)
{

  /* USER CODE BEGIN 1 */
	int ExitWithLongPress=0;
  RangingConfig_e RangingConfig = HIGH_ACCURACY;		//LONG_RANGE;
  //DemoMode_e DemoMode = RANGE_VALUE;
  int UseSensorsMask = 1<<XNUCLEO53L0A1_DEV_CENTER;
	
	static char buf[VL53L0X_MAX_STRING_LENGTH];
  /* USER CODE END 1 */

  /* MCU Configuration----------------------------------------------------------*/

  /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
  HAL_Init();

  /* Configure the system clock */
  SystemClock_Config();

  /* Initialize all configured peripherals */
  MX_GPIO_Init();

  /* Initialize timestamping for UART logging */
  TimeStamp_Init();

  /* USER CODE BEGIN 2 */
  XNUCLEO53L0A1_Init();			//i2c, usart
  uart_printf(WelcomeMsg);

	uart_printf("\n#####   53L0\r\n");
	
	sts_tof_vl53l0x_DetectSensors();					// confirm sensor online

  VL53L0X_trace_config(NULL, TRACE_MODULE_NONE, TRACE_LEVEL_NONE, TRACE_FUNCTION_NONE); // No Trace
  
  do {
		int Status=0;
		// RangingConfig == HIGH ACCURACY, LONG RANGE, HIGH SPEED
		
		Status = sts_tof_vl53l0x_range(&VL53L0XDevs,&RangingMeasurementData, RangingConfig);
		// status ==0, and ranging status for valid ranging value !!!!!!!!!!!!!!!!!
		if ((Status == 0) && (RangingMeasurementData.RangeStatus == 0 )) 
		{					
					//Distance_data = (int) VL53L0XDevs.LeakyRange;
					uart_printf("## LeakyRange = %4d mm\r\n", (int)Distance_data);
		}
		
		HAL_Delay(WelcomeTime/2);
		
	} while (1);

  /* USER CODE END 2 */

  /* Infinite loop */
  /* USER CODE BEGIN WHILE */
  while (1)
  {
		
	  /* Reset and Detect all sensors */

		uart_printf("Reset and Detect Sensor 0 \r\n");
		
      /* Reset Timestamping */
      TimeStamp_Reset();

      /* Start Ranging demo */
			uart_printf("Start Ranging demo\r\n");
     // ExitWithLongPress = RangeDemo(UseSensorsMask, RangingConfig);

      /* Blue button has been pressed (long or short press) */
        if(ExitWithLongPress){
    	  /* Short press : change ranging config */
    	  RangingConfig = (RangingConfig == LONG_RANGE) ? HIGH_SPEED : ((RangingConfig == HIGH_SPEED) ? HIGH_ACCURACY : LONG_RANGE);
				}

  /* USER CODE BEGIN 3 */

  }
  /* USER CODE END 3 */

}

void STS_TOF_VL53L1X_Range_Process(void)
{
  uint8_t vl53lx_model = STS_TOF_VL53L1X;
  uint8_t range_mode = STS_TOF_SHORT_RANGE;

  uint16_t i_distance_threshold_mm = 800;
  uint16_t i_inter_measurement_ms, i_macro_timing;
  uint16_t i_roi_width = 16, i_sigma_mm = 30, i_signal_kcps = 2000;
  while (1)
  {

                switch (range_mode)
                {
                case STS_TOF_SHORT_RANGE:
          // STS ---002  for short distance
          /* Example for robust and short distance measurements. Max distance reduced
           * but very low number of false-positives */
          // status |= VL53L1X_ULP_SetSigmaThreshold(dev, 30);
          // status |= VL53L1X_ULP_SetSignalThreshold(dev, 2000);
          i_sigma_mm = 30;      // increase this for longer distance, reduce for shorter distance
          i_signal_kcps = 2000; // 1000-  6000 kcps
          i_distance_threshold_mm = 900;
          i_inter_measurement_ms = 100; // 100 - 1000 ms
          i_macro_timing = 100;
          // i_roi_width = 16;

          break;

                case STS_TOF_LONG_RANGE:
          // STS --- 003 for long range
          /* Relax some limits. Be careful, it can create false-positives !*/
          // status |= VL53L1X_ULP_SetSigmaThreshold(dev, 60);
          // status |= VL53L1X_ULP_SetSignalThreshold(dev, 1200);
          i_sigma_mm = 85;                // increase this for longer distance, reduce for short distance
          i_signal_kcps = 1000;           // 1000-  6000 kcps
          i_distance_threshold_mm = 4000; // 4000;
          i_inter_measurement_ms = 200;   // 100 - 1000 ms
          i_macro_timing = 30;            // 1 - 100 ms
          i_roi_width = 8;

          break;

                case STS_TOF_LOW_POWER_RANGE:
          // STS---001   for ultra low power
          /* Reduce the macro timing to minimum. This is equivalent as reducing the integration time */
          // status = VL53L1X_ULP_SetMacroTiming(dev, 1);
          i_distance_threshold_mm = 4000;
          i_inter_measurement_ms = 100; // 100 - 1000 ms
          i_macro_timing = 1;
          /* Reduce at maximum the SPADS */
          // status = VL53L1X_ULP_SetROI(dev, 4);
          i_roi_width = 4;

          break;
                default:
          break;
                }

                sts_vl53lx_ranging(vl53lx_model, range_mode, i_distance_threshold_mm, i_inter_measurement_ms, i_macro_timing, i_roi_width, i_sigma_mm, i_signal_kcps);

                range_mode++;
                range_mode %= 3;
  }
}

void BSP_PB_Callback(Button_TypeDef Button)
{
  // PushButtonDetected = 1;
}

int sts_vl53l1x_ranging(uint8_t vl_model, uint8_t range_mode, uint16_t distance_threshold_mm, uint16_t inter_measurement_ms, uint16_t macro_timing,
                        uint16_t roi_width, uint16_t sigma_mm, uint16_t signal_kcps)
{
  /*********************************/
  /*   VL53L1X ranging variables  */
  /*********************************/
  uint8_t status, loop;
  uint8_t dev;
  uint16_t sensor_id;
  uint8_t measurement_status;
  uint16_t estimated_distance_mm, r_signal_kcps, r_sigma_mm, r_ambient_kcps;
  /*********************************/
  /*      Customer platform        */
  /*********************************/
  /* Default VL53L1X Ultra Low Power I2C address */
  dev = 0x52;

  /* (Optional) Change I2C address */
  // status = VL53L1X_ULP_SetI2CAddress(dev, 0x52);
  // dev = 0x20;

  /*********************************/
  /*   Power on sensor and init    */
  /*********************************/
  APP_LOG(TS_OFF, VLEVEL_L, "Range Mode =%d \r\n", range_mode);
  /* (Optional) Check if there is a VL53L1X sensor connected */
  status = VL53L1X_ULP_GetSensorId(dev, &sensor_id);
  APP_LOG(TS_OFF, VLEVEL_L, "VL53L1X address =%X\r\n", sensor_id);
  if (status || (sensor_id != 0xEACC))
  {
                APP_LOG(TS_OFF, VLEVEL_L, "VL53L1X not detected at requested address\n");
                return status;
  }
  /* (Mandatory) Init VL53L1X sensor */
  status = VL53L1X_ULP_SensorInit(dev);
  if (status)
  {
                APP_LOG(TS_OFF, VLEVEL_L, "VL53L1X ultra low power Loading failed\n");
                // HAL_Delay(100);
                return status;
  }
  APP_LOG(TS_OFF, VLEVEL_L, "VL53L1X ultra low power ready ! \r\n");
  /*********************************/
  /*     Sensor configuration      */
  /*********************************/
  /* (Optional) Program sensor to raise an interrupt ONLY below 300mm */
  status = VL53L1X_ULP_SetInterruptConfiguration(dev, distance_threshold_mm, 1); // i_distance_threshold_mm
  /* (Optional) Program a 10Hz ranging frequency */
  status = VL53L1X_ULP_SetInterMeasurementInMs(dev, inter_measurement_ms); // range_interval_ms
  /* Increase the macro timing. This is equivalent as increasing the integration time */
  status = VL53L1X_ULP_SetMacroTiming(dev, macro_timing); // micro_timing_ms
  /* Enable all the SPADS */
  status = VL53L1X_ULP_SetROI(dev, roi_width); // SPADS { 1 -- 16 }

  if (range_mode != STS_TOF_LOW_POWER_RANGE)
  {
                /* Example for robust and short distance measurements. Max distance reduced
                 * but very low number of false-positives */
                status |= VL53L1X_ULP_SetSigmaThreshold(dev, sigma_mm);
                status |= VL53L1X_ULP_SetSignalThreshold(dev, signal_kcps);
  }

  /*********************************/
  /*         Ranging loop          */
  /*********************************/
  status = VL53L1X_ULP_StartRanging(dev);
  if (status)
  {
                APP_LOG(TS_OFF, VLEVEL_L, "VL53L1X_ULP_StartRanging failed with status %u\n", status);
                return status;
  }
  APP_LOG(TS_OFF, VLEVEL_L, "Ranging started. Put your hand close to the sensor to generate an interrupt...\n");
  loop = 0;
  while (loop < 20)
  {
                /* Use this external function to detect when a hardware interrupt is generated on PIN 7 (GPIO1). It means that a new measurement is ready.
                 */

                if (IsInterruptDetected(dev))
                {
          /* (Mandatory) Clear HW interrupt to restart measurements */
          VL53L1X_ULP_ClearInterrupt(dev);
          /* Dump debug data */
          status = VL53L1X_ULP_DumpDebugData(dev, &measurement_status,
                                             &estimated_distance_mm, &r_sigma_mm, &r_signal_kcps, &r_ambient_kcps);

          APP_LOG(TS_OFF, VLEVEL_L, "Target detected! Interrupt raised by sensor, Distance =%d mm \r\n", estimated_distance_mm);
          loop++;
                }
  }

  status = VL53L1X_ULP_StopRanging(dev);
  APP_LOG(TS_OFF, VLEVEL_L, "End of VL53L1X ultra low power demo\n");
  return status;
}

/** System Clock Configuration
*/

/**
  * @}
  */ 

/**
  * @}
*/ 

/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/