/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * @file    app_tof_vl53l0x_range.c
  * @author  Yunhorn (r) Technology Limited Application Team
  * @brief   Application of the LRWAN Middleware
  ******************************************************************************
  * @attention
  *
  * Copyright (c) 2023 Yunhorn Technology Limited.
  * Copyright (c) 2023 Shenzhen Yunhorn Technology Co., Ltd.
  * 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.
  *
  ******************************************************************************
  */

/* Includes ------------------------------------------------------------------*/

/* USER CODE BEGIN Includes */
#include <string.h>
#include "X-WL55_WLE5_53L0X.h"
#include "main.h"
#include "vl53l0x_api.h"
#include <limits.h>
#include "sys_app.h"
#include "yunhorn_sts_sensors.h"

#if	defined(VL53L0)||(VL53L0 == 1)
/**
 * @defgroup Configuration Static configuration
 * @{
 */

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

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

/**
 * @defgroup ErrCode Errors code shown on display
 * @{
 */
#if	defined(STS_R1)||defined(STS_R1x)||defined(STS_R1D)		//short, accuracy ranging for jumbo sensors
#define		STS_MAX_L0_RANGE		((uint32_t) 1000)
#endif

#if	defined(STS_R5)||defined(STS_R5x)		//long distance for waste bin sensor
#define		STS_MAX_L0_RANGE		((uint32_t) 2500)
#endif

#define ERR_DETECT             -1
#define ERR_DEMO_RANGE_ONE     1
#define ERR_DEMO_RANGE_MULTI   2
extern I2C_HandleTypeDef hi2c2;
#define X_WL55_WLE5_53L0X_hi2c hi2c2

/** }@} */ /* 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);				// 0.6 for ranging, 0.0 for gesture detect
/** How many device detect set by @a DetectSensors()*/
int nDevPresent=0;
/** bit is index in VL53L0XDevs that is not necessary the dev id of the BSP */
int nDevMask=0;

int nSensorPresent;

volatile int sts_tof_distance_data[MAX_TOF_COUNT]={0x0};
extern volatile uint8_t sensor_data_ready;

VL53L0X_Dev_t VL53L0XDevs[]={
		{.Id=XNUCLEO53L0X_LEFT, .DevLetter='l',.I2cHandle=&X_WL55_WLE5_53L0X_hi2c, .I2cDevAddr=0x52},
		{.Id=XNUCLEO53L0X_CENTER,.DevLetter='c', .I2cHandle=&X_WL55_WLE5_53L0X_hi2c, .I2cDevAddr=0x52},
		{.Id=XNUCLEO53L0X_RIGHT, .DevLetter='r',.I2cHandle=&X_WL55_WLE5_53L0X_hi2c, .I2cDevAddr=0x52},
};

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

void sts_tof_vl53l0x_Sensor_SetNewRange(VL53L0X_Dev_t *pDev, VL53L0X_RangingMeasurementData_t *pRange);
void sts_tof_vl53l0x_SetupSingleShot(RangingConfig_e rangingConfig);
/* USER CODE END PFP */

/* USER CODE BEGIN 0 */

/**
 * 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);
    APP_LOG(TS_OFF, VLEVEL_H,"Er %04u\r\n",err);
    //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;
    nDevPresent = 0;

    for (i=0; i < MAX_TOF_COUNT; i++) {
    	XWL55_WLE5_53L0X_ResetId(i,0);
    	//XWL55_WLE5_53L0X_SetIntrStateId(1,i);
    }

    /* detect all sensors (even on-board)*/
    for (i=0; i < MAX_TOF_COUNT; i++)
    {
		VL53L0X_Dev_t *pDev;
		pDev = &VL53L0XDevs[i];
		pDev->I2cDevAddr = 0x52;
		pDev->Present = 0;
		XWL55_WLE5_53L0X_ResetId(pDev->Id, 1);
		HAL_Delay(3);
		FinalAddress = 0x52+(i+1)*2;

	   //APP_LOG(TS_OFF, VLEVEL_M, "\r\n Detect TOF sensors #%u with I2CDevAddr=0x%02x \r\n", i, pDev->I2cDevAddr);

	   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);
		   //APP_LOG(TS_OFF, VLEVEL_H, "#%u read id = %04X I2C ADDR=0x%2X\r\n",i, Id, pDev->I2cDevAddr);
		   if (status) {
				 APP_LOG(TS_OFF, VLEVEL_H, "# Read id fail \r\n");
				 break;
		   }
		   if (Id == 0xEEAA) {
			   /* Sensor is found => Change its I2C address to final one */
			   //APP_LOG(TS_OFF, VLEVEL_H, "\n Set Final i2c addr to %02X \r\n", FinalAddress);
			   status = VL53L0X_SetDeviceAddress(pDev,FinalAddress);
			   if (status != 0) {
				   APP_LOG(TS_OFF, VLEVEL_H, "#i VL53L0X_SetDeviceAddress fail\r\n");
				   break;
			   }
			   pDev->I2cDevAddr = FinalAddress;
			   //APP_LOG(TS_OFF, VLEVEL_H, "\n pDev->I2CDevAddr set to: %02X \r\n", pDev->I2cDevAddr);
			   /* Check all is OK with the new I2C address and initialize the sensor */
			   status = VL53L0X_RdWord(pDev, VL53L0X_REG_IDENTIFICATION_MODEL_ID, &Id);
			   if (status !=0)
			   {
			  	   APP_LOG(TS_OFF, VLEVEL_H, "#i VL53L0X_RdWord fail\r\n");
			  	   break;
			  	}
			   status = VL53L0X_DataInit(pDev);
			   if( status == 0 ){
				   pDev->Present = 1;
				   nDevPresent++;
				   nDevMask |= 1 << i;
				   pDev->Present = 1;
			   }
			   else{
				   APP_LOG(TS_OFF, VLEVEL_H, "VL53L0X_DataInit %d fail\r\n", Id);
				   break;
			   }

		   } else {
			   APP_LOG(TS_OFF, VLEVEL_H, "# unknown ID %x\r\n", Id);
			   status = 1;
		   }
	   } while(0);
		/* Display detected sensor(s) */
	   if (status) {
		   XWL55_WLE5_53L0X_ResetId(i,0);
	   }
	}

    return nDevPresent;
}

/**
 *  Setup all detected sensors for single shot mode and setup ranging configuration
 */
void sts_tof_vl53l0x_SetupSingleShot(RangingConfig_e rangingConfig)
{	int i;
    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");
	for( i=0; i< MAX_TOF_COUNT; i++){

		if( VL53L0XDevs[i].Present){

		  //APP_LOG(TS_OFF, VLEVEL_H, "\r\n ######  Starting Range   #%u sensor \r\n",i);

		  status=VL53L0X_StaticInit(&VL53L0XDevs[i]);

		  status = VL53L0X_PerformRefCalibration(&VL53L0XDevs[i], &VhvSettings, &PhaseCal);

		  status = VL53L0X_PerformRefSpadManagement(&VL53L0XDevs[i], &refSpadCount, &isApertureSpads);

		  status = VL53L0X_SetDeviceMode(&VL53L0XDevs[i], VL53L0X_DEVICEMODE_SINGLE_RANGING); // Setup in single ranging mode

		  status = VL53L0X_SetLimitCheckEnable(&VL53L0XDevs[i], VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, 1); // Enable Sigma limit

		  status = VL53L0X_SetLimitCheckEnable(&VL53L0XDevs[i], VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE, 1); // Enable Signa limit

		  /* 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:
				  APP_LOG(TS_OFF, VLEVEL_H, "Not Supported");
			  break;
		  }

		  status = VL53L0X_SetLimitCheckValue(&VL53L0XDevs[i],  VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE, signalLimit);

		  status = VL53L0X_SetLimitCheckValue(&VL53L0XDevs[i],  VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, sigmaLimit);

		  status = VL53L0X_SetMeasurementTimingBudgetMicroSeconds(&VL53L0XDevs[i],  timingBudget);

		  status = VL53L0X_SetVcselPulsePeriod(&VL53L0XDevs[i],  VL53L0X_VCSEL_PERIOD_PRE_RANGE, preRangeVcselPeriod);

		  status = VL53L0X_SetVcselPulsePeriod(&VL53L0XDevs[i],  VL53L0X_VCSEL_PERIOD_FINAL_RANGE, finalRangeVcselPeriod);

		  status = VL53L0X_PerformRefCalibration(&VL53L0XDevs[i], &VhvSettings, &PhaseCal);

		  VL53L0XDevs[i].LeakyFirst =1;

		}
	}
  
}


/* 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;
    }
}

/* USER CODE END 0 */
void STS_TOF250_Range_Process(void)
{
#ifdef 	TOF_3

#define I2C_TIME_OUT_BASE   10
#define I2C_TIME_OUT_BYTE   1
	int status;
	uint8_t pdata[2]={0x0,0x0}; uint32_t count=2;
	int i2c_time_out = I2C_TIME_OUT_BASE + count*I2C_TIME_OUT_BYTE;

	sensor_data_ready = 0 ;

    sts_tof_distance_data[0] = 0;
	sts_tof_distance_data[1] = 0;

	sts_tof_distance_data[2] = 0;

	status = HAL_I2C_Master_Receive(&hi2c2, TOF250_I2C_ADDR, pdata, count, i2c_time_out);
	if (status == HAL_OK) {
		APP_LOG(TS_OFF, VLEVEL_H, "\r\n## TOF250 ranging value %02x %02x %4d ",pdata[0],pdata[1], *pdata);
		sts_tof_distance_data[2] = pdata[0]<<8|pdata[1];
		sensor_data_ready = 1;
		APP_LOG(TS_OFF, VLEVEL_H, "\r\n## Measured Range: \r\nTOF #0 = %4u mm, \r\nTOF #1 = %4u mm, \r\nTOF #2 = %4u mm\r\n",
				  (int)sts_tof_distance_data[0],(int)sts_tof_distance_data[1],(int)sts_tof_distance_data[2]);
	  }
#endif

}
void STS_TOF_VL53L0X_Range_Process(void)
{
  int status=0, i;
  /* HIGH_ACCURACY for STS_R1/R2/R3 short distance less than 1000mm  */
  /* LONG_RANGE    for STS_R5        long distance less than 2500mm  */
#ifdef	STS_R5
  RangingConfig_e RangingConfig = LONG_RANGE; //HIGH_ACCURACY;		//LONG_RANGE;
#else
  RangingConfig_e RangingConfig = HIGH_ACCURACY; //HIGH_ACCURACY;		//LONG_RANGE;
#endif
  XWL55_WLE5_53L0X_Init();
  if ((nDevMask ==0) || (nSensorPresent ==0))
  {
	  nSensorPresent = sts_tof_vl53l0x_DetectSensors();					// confirm sensor online
	  APP_LOG(TS_OFF, VLEVEL_H, "\r\n %u pcs sensor(s) online \r\n", nSensorPresent);
  }

  //APP_LOG(TS_OFF, VLEVEL_H, "\n\n nDevMask=%02X  nSensorPresent = %02x \n\n", nDevMask, nSensorPresent);

#ifdef	TOF_3
  sts_tof_distance_data[2] = 0;
#endif

  if (nSensorPresent > 0)
  {
	  // RangingConfig == HIGH ACCURACY, LONG RANGE, HIGH SPEED
	  sts_tof_vl53l0x_SetupSingleShot(RangingConfig);

	  sensor_data_ready = 0 ;
	  for (i=0; i< MAX_TOF_COUNT; i++)
	  //for (i=0; i< nSensorPresent; i++)
	  {
		  APP_LOG(TS_OFF, VLEVEL_H, "\n\n i=%u  Present=%u DevAdd=%02x \n", i, VL53L0XDevs[i].Present, VL53L0XDevs[i].I2cDevAddr);
		  if (VL53L0XDevs[i].Present ==1)
		  {
			uint8_t rep=0;
			do {
//				rep++;
				status = VL53L0X_PerformSingleRangingMeasurement(&VL53L0XDevs[i], &RangingMeasurementData);
				if (status ==0)
				{
					sts_tof_vl53l0x_Sensor_SetNewRange(&VL53L0XDevs[i],&RangingMeasurementData);
					if( RangingMeasurementData.RangeStatus == 0 )
					{
						sts_tof_distance_data[i] = (int)VL53L0XDevs[i].LeakyRange;
						nDevMask |= (1 << i);
						sensor_data_ready |= 1<<i;
					}  else
					{
						//HandleError(ERR_DEMO_RANGE_ONE);
						APP_LOG(TS_OFF, VLEVEL_H,"\r\n#%u sensor   ERR code = %04u\r\n",i, ERR_DEMO_RANGE_ONE);
					}
					// ##########     two conditions
					// ##########     1) return status ==0,
					// ##########     2) and ranging status for valid ranging value !!!!!!!!!!!!!!!!!
				}

				HAL_Delay(1);

				} while ((RangingMeasurementData.RangeStatus != 0)&&(rep++ < 10));
				sts_tof_distance_data[i] = (RangingMeasurementData.RangeStatus!=0)?STS_MAX_L0_RANGE:VL53L0XDevs[i].LeakyRange;
		  }

		    HAL_Delay(30);
	   }  // for i < MAX_TOF_COUNT

	   if (sensor_data_ready != 0)
	   {
	    	  APP_LOG(TS_OFF, VLEVEL_H, "\r\n## Measured Range: \r\nTOF #0 = %4u mm, \r\nTOF #1 = %4u mm, \r\nTOF #2 = %4u mm\r\n",
				  (int)sts_tof_distance_data[0],(int)sts_tof_distance_data[1],(int)sts_tof_distance_data[2]);
	   }

  } else {
	  // nSensorPresent >0
	  sts_tof_distance_data[0] = STS_MAX_L0_RANGE;
	  sts_tof_distance_data[1] = STS_MAX_L0_RANGE;
	  sts_tof_distance_data[2] = STS_MAX_L0_RANGE;
	  APP_LOG(TS_OFF, VLEVEL_H, "\n\n APP_LOG   no ToF sensor online \n\n");
  }

  // reset for next ranging
  nDevMask = 0;
  nSensorPresent = 0;
}

void STS_RR_Sensor_Read(sts_tof_range_data_t *sts_rr_sensor_data)
{
	sts_rr_sensor_data->tof_1_distance_mm = sts_tof_distance_data[0];
	sts_rr_sensor_data->tof_2_distance_mm = sts_tof_distance_data[1];
	sts_rr_sensor_data->tof_3_distance_mm = sts_tof_distance_data[2];
}

void STS_Prepare_Packet_Data(void)
{
	//AppData.Buffer = 0x0;
	//AppData.BufferSize = 0x0;
}

/* USER CODE BEGIN 4 */

/* USER CODE END 4 */

#endif
/**
  * @}
  */ 

/**
  * @}
*/ 

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