/**
  ******************************************************************************
  * @file          : app_tof.c
  * @author        : IMG SW Application Team
  * @brief         : This file provides code for the configuration
  *                  of the STMicroelectronics.X-CUBE-TOF1.3.2.0 instances.
  ******************************************************************************
  *
  * @attention
  *
  * Copyright (c) 2022 STMicroelectronics.
  * All rights reserved.
  *
  * This software is licensed under terms that can be found in the LICENSE file
  * in the root directory of this software component.
  * If no LICENSE file comes with this software, it is provided AS-IS.
  *
  ******************************************************************************
  */

#ifdef __cplusplus
extern "C" {
#endif

/* Includes ------------------------------------------------------------------*/
#include "app_tof.h"
#include "main.h"
#include "sys_app.h"
#include <stdio.h>

#include "VL53L1X_ULP_api.h"
//#include "53l1a2_ranging_sensor.h"
#include "stm32wlxx_nucleo.h"

/* Private typedef -----------------------------------------------------------*/

/* Private define ------------------------------------------------------------*/
#define TIMING_BUDGET (30U) /* 16 ms < TimingBudget < 500 ms */
#define POLLING_PERIOD (250U) /* refresh rate for polling mode (milliseconds, shall be consistent with TimingBudget value) */

/* Private variables ---------------------------------------------------------*/
//static RANGING_SENSOR_Capabilities_t Cap;
//static RANGING_SENSOR_ProfileConfig_t Profile;
//static int32_t status = 0;

volatile uint8_t ToF_EventDetected = 0;


/* Private function prototypes -----------------------------------------------*/

int sts_vl53lx_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);
uint8_t IsInterruptDetected(uint16_t dev);

void MX_TOF_Init(void)
{
  /* USER CODE BEGIN SV */

  /* USER CODE END SV */

  /* USER CODE BEGIN TOF_Init_PreTreatment */

  /* USER CODE END TOF_Init_PreTreatment */

  /* Initialize the peripherals and the TOF components */
  APP_LOG(TS_OFF,VLEVEL_L,"\r\n######################    MX TOF Init... \r\n");

  //MX_53L1A2_SimpleRanging_Init();
  STS_TOF_VL53LX_PeopleCounting_Process();
  /* USER CODE BEGIN TOF_Init_PostTreatment */

  /* USER CODE END TOF_Init_PostTreatment */
}

/*
 * LM background task
 */
void MX_TOF_Process(void)
{
  /* USER CODE BEGIN TOF_Process_PreTreatment */

  /* USER CODE END TOF_Process_PreTreatment */


	//APP_LOG(TS_OFF,VLEVEL_L,"\r\n######################    MX TOF Process... \r\n");
	//STS_TOF_VL53LX_Range_Process();
	//STS_TOF_VL53LX_PeopleCounting_Process();

	sts_tof_vl53lx_peoplecount_subprocess();

  /* USER CODE BEGIN TOF_Process_PostTreatment */

  /* USER CODE END TOF_Process_PostTreatment */
}
typedef enum {
	STS_TOF_SHORT_RANGE=0,
	STS_TOF_LONG_RANGE,
	STS_TOF_LOW_POWER_RANGE,
	STS_TOF_PP_RANGE
} range_mode_t;
typedef enum {
	STS_TOF_VL53L0X=0,
	STS_TOF_VL53L1X,
	STS_TOF_VL53L2X,
	STS_TOF_VL53L3X,
	STS_TOF_VL53L4X,
	STS_TOF_VL53L5X,
	STS_TOF_VL53L6X,
	STS_TOF_VL53L7X
} vl53lx_model;

uint8_t IsInterruptDetected(uint16_t dev)
{
	// To be filled with customer HW. This function should
	// return 1 when an interrupt is raised by the ToF on GPIO1 pin (pin7)
	if (ToF_EventDetected )
	{
		APP_LOG(TS_OFF, VLEVEL_L,"###############   TOF EVENT DETECTED \r\n");
		ToF_EventDetected =0;
		return 1;
	} else {
		return 0;
	}
}

void STS_TOF_VL53LX_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_vl53lx_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;
}


#ifdef __cplusplus
}
#endif