/**
  ******************************************************************************
  * @file          : app_tof.c
  * @author  Yunhorn (r) Technology Limited Application Team
  * @brief  configuration X-CUBE-TOF1.3.2.0 instances.
  ******************************************************************************
  *
  * @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.
  *
  ******************************************************************************
  */

#ifdef __cplusplus
extern "C" {
#endif

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

#ifdef		STS_USE_TOF_VL53L1X
#include "VL53L1X_ULP_api.h"
//#include "53l1a2_ranging_sensor.h"
#endif

#ifdef		STS_USE_TOF_VL53L0X
#include "VL53L0X_api.h"

#include "X-WL55_WLE5_53L0X.h"
#endif


#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;
extern volatile uint8_t sensor_data_ready;
extern volatile int sts_tof_distance_data[MAX_TOF_COUNT];
//volatile uint8_t sensor_data_ready=0;
/* Private function prototypes -----------------------------------------------*/
#ifdef	STS_USE_TOF_VL53L1X
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);
uint8_t IsInterruptDetected(uint16_t dev);
#endif

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");

  XWL55_WLE5_53L0X_Init();

  //MX_53L1A2_SimpleRanging_Init();

  /* 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");

#ifdef	STS_USE_TOF_VL53L1X
	STS_TOF_VL53L1X_Range_Process();
#endif

#ifdef	STS_USE_TOF_VL53L0X
	STS_SENSOR_Power_ON(1);
	HAL_Delay(100);
#if defined(TOF_1)||defined(TOF_2)
	STS_TOF_VL53L0X_Range_Process();
#endif
#ifdef TOF_3
	STS_TOF250_Range_Process();
#endif

	STS_SENSOR_Power_OFF(1);
#endif

#if defined(STS_TOF_VL53L1X_PeopleCount) && defined(STS_USE_TOF_VL53L1X)
	//STS_TOF_VL53LX_PeopleCounting_Process();
#endif


  /* USER CODE BEGIN TOF_Process_PostTreatment */

  /* USER CODE END TOF_Process_PostTreatment */
}
#if	defined(YUNHORN_STS_R0_ENABLED)
void STS_R0_SENSOR_Read(STS_R0_SensorDataTypeDef *r0_data)
{

	r0_data->distance_mm  = sts_tof_distance_data[0];
	r0_data->distance1_mm = sts_tof_distance_data[1];
	r0_data->distance2_mm = sts_tof_distance_data[2];
#ifdef ROCTEC_R5
	r0_data->distance1_mm = sts_tof_distance_data[0]+sts_tof_distance_data[1]+sts_tof_distance_data[2];
#endif

	sensor_data_ready = 1;
}
#endif

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 STS_SENSOR_MEMS_Get_ID(uint8_t *mems_Dev_ID)
{
	uint16_t Id = 0xEEAA; 	//0xeeaa for VL53L0X
	uint8_t cnt =0;

	cnt = sts_tof_vl53l0x_DetectSensors();

	if (cnt > 0)
	mems_Dev_ID[0] = (Id >>8)&0xff;
	mems_Dev_ID[1] = (Id & 0xff);

	APP_LOG(TS_OFF, VLEVEL_L,"MEMS ID=%02x %02x \r\n", mems_Dev_ID[0],mems_Dev_ID[1]);
	return cnt;
}

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)
#ifdef	STS_USE_TOF_VL53L1X
	if (ToF_EventDetected )
	{
		//APP_LOG(TS_OFF, VLEVEL_L,"###############   TOF EVENT DETECTED \r\n");
		ToF_EventDetected =0;
		return 1;
	} else {
		return 0;
	}
#endif
	return 0;
}


#ifdef __cplusplus
}
#endif