/* Includes ------------------------------------------------------------------*/
#include "main.h"
#include "sys_app.h"
/* USER CODE BEGIN Includes */
#include "app_tof_peoplecount.h"
#ifdef	STS_P2
#include "VL53L1X_API.h"
#include "VL53l1X_calibration.h"
#include "X-NUCLEO-53L1A1.h"
#endif
#include "app_tof.h"
/* USER CODE END Includes */
/* Private variables ---------------------------------------------------------*/
/* USER CODE BEGIN PV */
/* Private variables ---------------------------------------------------------*/
extern I2C_HandleTypeDef hi2c2;
volatile sts_people_count_sensor_data_t sts_people_count_sensor_data={0,0,0,2,'M',0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1};
extern volatile uint32_t STS_TOFScanPeriod_sec, STS_TxPeriod_sec, STS_HeartBeatTimerPeriod_sec;
volatile uint8_t sts_tof_presence_state=0, sts_tof_presence_state_changed=0;

/* TIMING_BUDGET, in ms possible values [15, 20, 50, 100, 200, 500] */
/* DISTANCE_THRESHOLD = MAX - ASSUME_CHILD_HEIGHT(800mm)            */
volatile sts_ppc_cfg_type_t ppc_cfg[6] = {
		{DOOR_JAM_2000, 16, 2000,0, 1200,16, tbms_33,DISTANCE_MODE_LONG,199,199},
		{DOOR_JAM_2400, 10, 2400,0, 1600,8, tbms_50,DISTANCE_MODE_LONG,175,231},
		{DOOR_JAM_3000, 10, 3000,0, 2200,8,tbms_100,DISTANCE_MODE_LONG,175,231},
		{DOOR_JAM_3500, 10, 3500,0, 2700,8,tbms_100,DISTANCE_MODE_LONG,175,231},
		{DOOR_JAM_4000, 10, 4000,0, 3200,8,tbms_200,DISTANCE_MODE_LONG,175,231},
		{DOOR_SIDE_1600, 8, 1600,0,  300,8, tbms_33,DISTANCE_MODE_SHORT,175,231},
};
volatile sts_zone_center_by_rows_of_spads_t zone_center[4]={
		{4, 151, 247},
		{6, 159, 239},
		{8, 175, 231},
		{16, 191,191},
};
#if	(defined(STS_P2))
extern volatile uint8_t sts_door_jam_profile;
volatile uint8_t sts_presence_state=0;
static int PresenceState=0, PrevPresenceState=0;
volatile uint8_t sts_distance_mode = 0; //sts_door_jam_profile; //DOOR_JAM_3000;
#endif
#if	(defined(STS_P2))
volatile uint8_t sts_people_count_number_changed = 0;
static uint32_t cnt_today=0, cnt_week=0;
static uint8_t last_year=0, this_year=0,last_month=0, this_month=0,last_week=0, this_week=0,last_day=0, this_day=0;
#endif
uint16_t	dev=0x52;
extern uint16_t sensor_id;
int status = 0;
volatile int IntCount;
#define isInterrupt 1 /* If isInterrupt = 1 then device working in interrupt mode, else device working in polling mode */
/* USER CODE END PV */
static int PplCounter;
static int center[2] = {FRONT_ZONE_CENTER,BACK_ZONE_CENTER}; //{ppc_cfg[sts_distance_mode].front_zone_center, ppc_cfg[sts_distance_mode].back_zone_center}; /* these are the spad center of the 2 4*16 zones */
static int Zone = 0;
static int PathTrack[] = {0,0,0,0};
static int PathTrackFillingSize = 1; // init this to 1 as we start from state where nobody is any of the zones
static int LeftPreviousStatus = NOBODY;
static int RightPreviousStatus = NOBODY;
//static int PeopleCount = 0;
volatile int PeopleCount = 0;
static uint16_t Distances[2][DISTANCES_ARRAY_SIZE];
static uint8_t DistancesTableSize[2] = {0,0};
static int ProcessPeopleCountingData(int16_t Distance, uint8_t zone, uint8_t RangeStatus);
/* Private function prototypes -----------------------------------------------*/
/* USER CODE BEGIN PFP */
/* Private function prototypes -----------------------------------------------*/

/* USER CODE END PFP */
//void sts_tof_vl53lx_peoplecount(void);
//int sts_tof_vl53lx_peoplecount_subprocess(void);
/* USER CODE BEGIN 0 */

#define VL53L1X_INT_Pin    (GPIO_PIN_3)					// WL55JC GPIO_PIN_10, F401xE ==>(GPIO_PIN_4)
#if 0
void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin)
{
  if (GPIO_Pin==VL53L1X_INT_Pin)
  {
    IntCount++;
  }
}
#endif
/* USER CODE END 0 */
int ProcessPresenceDetectionData(int16_t Distance, uint8_t zone, uint8_t RangeStatus)
{
	  uint16_t MinDistance;
	  uint8_t i;

	#ifdef TRACE_PPC
	#define TIMES_WITH_NO_EVENT 10// was 40
	  static uint32_t trace_count = TIMES_WITH_NO_EVENT;  // replace by 0 if you want to trace the first TIMES_WITH_NO_EVENT values
	#endif

	  int CurrentZoneStatus = NOBODY;
	  int AllZonesCurrentStatus = 0;
	  int AnEventHasOccured = 0;

	  // Add just picked distance to the table of the corresponding zone
	  if (DistancesTableSize[zone] < ppc_cfg[sts_distance_mode].distance_array_size) {
	    Distances[zone][DistancesTableSize[zone]] = Distance;
	    DistancesTableSize[zone] ++;
	  }
	  else {
	    for (i=1; i<ppc_cfg[sts_distance_mode].distance_array_size; i++)
	      Distances[zone][i-1] = Distances[zone][i];
	    Distances[zone][ppc_cfg[sts_distance_mode].distance_array_size-1] = Distance;
	  }

	  // pick up the min distance
	  MinDistance = Distances[zone][0];
	  if (DistancesTableSize[zone] >= 2) {
	    for (i=1; i<DistancesTableSize[zone]; i++) {
	      if (Distances[zone][i] < MinDistance)
	        MinDistance = Distances[zone][i];
	    }
	  }

	  if (MinDistance < ppc_cfg[sts_distance_mode].dist_threshold) {
	    // Someone is in !
	    CurrentZoneStatus = SOMEONE;
	  }

	  //return CurrentZoneStatus;
#if 1
	  // left zone
	  if (zone == LEFT) {
	    if (CurrentZoneStatus != LeftPreviousStatus) {
	      // event in left zone has occured
	      AnEventHasOccured = 1;

	      if (CurrentZoneStatus == SOMEONE) {
	        AllZonesCurrentStatus += 1;
	      }
	      // need to check right zone as well ...
	      if (RightPreviousStatus == SOMEONE) {
	        // event in left zone has occured
	        AllZonesCurrentStatus += 2;
	      }
	      // remember for next time
	      LeftPreviousStatus = CurrentZoneStatus;
	    }
	  }
	  // right zone
	  else {

	    if (CurrentZoneStatus != RightPreviousStatus) {

	      // event in left zone has occured
	      AnEventHasOccured = 1;
	      if (CurrentZoneStatus == SOMEONE) {
	        AllZonesCurrentStatus += 2;
	      }
	      // need to left right zone as well ...
	      if (LeftPreviousStatus == SOMEONE) {
	        // event in left zone has occured
	        AllZonesCurrentStatus += 1;
	      }
	      // remember for next time
	      RightPreviousStatus = CurrentZoneStatus;
	    }
	  }

	#ifdef TRACE_PPC
	  // print debug data only when someone is within the field of view
	  trace_count++;
	  if ((CurrentZoneStatus == SOMEONE) || (LeftPreviousStatus == SOMEONE) || (RightPreviousStatus == SOMEONE))
	    trace_count = 0;
	#if 1
	  if (trace_count < TIMES_WITH_NO_EVENT)
	    printf ("%d,%d,%d,%d,%d\n", zone, Distance, MinDistance, RangeStatus, PeopleCount);
	#endif
	#endif

	  // if an event has occured
	  if (AnEventHasOccured) {
	    if (PathTrackFillingSize < 4) {
	      PathTrackFillingSize ++;
	    }

	    // if nobody anywhere lets check if an exit or entry has happened
	    if ((LeftPreviousStatus == NOBODY) && (RightPreviousStatus == NOBODY)) {

	      // check exit or entry only if PathTrackFillingSize is 4 (for example 0 1 3 2) and last event is 0 (nobobdy anywhere)
	      if (PathTrackFillingSize == 4) {
	        // check exit or entry. no need to check PathTrack[0] == 0 , it is always the case

	        if ((PathTrack[1] == 1)  && (PathTrack[2] == 3) && (PathTrack[3] == 2)) {
	          // This an entry
	          PeopleCount ++;
	          // reset the table filling size in case an entry or exit just found
	          DistancesTableSize[0] = 0;
	          DistancesTableSize[1] = 0;
	          APP_LOG(TS_OFF, VLEVEL_M,"Walk In, People Count=%d\n", PeopleCount);
	          sts_people_count_sensor_data.Walk_In_People_Count ++;
	          sts_people_count_number_changed = 1;
	        } else if ((PathTrack[1] == 2)  && (PathTrack[2] == 3) && (PathTrack[3] == 1)) {
	          // This an exit
	          PeopleCount --;
	          // reset the table filling size in case an entry or exit just found
	          DistancesTableSize[0] = 0;
	          DistancesTableSize[1] = 0;
	          APP_LOG(TS_OFF, VLEVEL_M,"Walk Out, People Count=%d\n", PeopleCount);
	          sts_people_count_sensor_data.Walk_Out_People_Count ++;
	          sts_people_count_number_changed = 1;
	        } else {
	          // reset the table filling size also in case of unexpected path
	          DistancesTableSize[0] = 0;
	          DistancesTableSize[1] = 0;
	          APP_LOG(TS_OFF, VLEVEL_M,"Walk Around ie. Wrong path\n");
	          sts_people_count_sensor_data.Walk_Around_People_Count ++;
	        }
	      }

	      PathTrackFillingSize = 1;
	    }
	    else {
	      // update PathTrack
	      // example of PathTrack update
	      // 0
	      // 0 1
	      // 0 1 3
	      // 0 1 3 1
	      // 0 1 3 3
	      // 0 1 3 2 ==> if next is 0 : check if exit
	      PathTrack[PathTrackFillingSize-1] = AllZonesCurrentStatus;
	    }

	#if 1
	#ifdef TRACE_PPC
	    if (AnEventHasOccured) {
	      for (int j=0; j<PathTrackFillingSize; j++)
	        printf ("%d ", PathTrack[j]);
	    }
	    printf("\n");
	#endif
	#endif
	  }
	  // if SOMEONE IN no matter LEFT or RIGHT or MIDDLE
	  return(AnEventHasOccured);
#endif
}
int ProcessPeopleCountingData(int16_t Distance, uint8_t zone, uint8_t RangeStatus) {

  uint16_t MinDistance;
  uint8_t i;

#ifdef TRACE_PPC
#define TIMES_WITH_NO_EVENT 10// was 40
  static uint32_t trace_count = TIMES_WITH_NO_EVENT;  // replace by 0 if you want to trace the first TIMES_WITH_NO_EVENT values
#endif
  
  int CurrentZoneStatus = NOBODY;
  int AllZonesCurrentStatus = 0;
  int AnEventHasOccured = 0;
  
  // Add just picked distance to the table of the corresponding zone
  if (DistancesTableSize[zone] < ppc_cfg[sts_distance_mode].distance_array_size) {
    Distances[zone][DistancesTableSize[zone]] = Distance;
    DistancesTableSize[zone] ++;
  }
  else {
    for (i=1; i<ppc_cfg[sts_distance_mode].distance_array_size; i++)
      Distances[zone][i-1] = Distances[zone][i];
    Distances[zone][ppc_cfg[sts_distance_mode].distance_array_size-1] = Distance;
  }
  
  // pick up the min distance
  MinDistance = Distances[zone][0];
  if (DistancesTableSize[zone] >= 2) {
    for (i=1; i<DistancesTableSize[zone]; i++) {
      if (Distances[zone][i] < MinDistance)
        MinDistance = Distances[zone][i];
    }
  }
  
  if (MinDistance < ppc_cfg[sts_distance_mode].dist_threshold) {
    // Someone is in !
    CurrentZoneStatus = SOMEONE;
  }
  
  // left zone
  if (zone == LEFT) {
    if (CurrentZoneStatus != LeftPreviousStatus) {
      // event in left zone has occured
      AnEventHasOccured = 1;
      
      if (CurrentZoneStatus == SOMEONE) {
        AllZonesCurrentStatus += 1;
      }
      // need to check right zone as well ...
      if (RightPreviousStatus == SOMEONE) {
        // event in left zone has occured
        AllZonesCurrentStatus += 2;
      }
      // remember for next time
      LeftPreviousStatus = CurrentZoneStatus;
    }
  }
  // right zone
  else {
    
    if (CurrentZoneStatus != RightPreviousStatus) {
      
      // event in left zone has occured
      AnEventHasOccured = 1;
      if (CurrentZoneStatus == SOMEONE) {
        AllZonesCurrentStatus += 2;
      }
      // need to left right zone as well ...
      if (LeftPreviousStatus == SOMEONE) {
        // event in left zone has occured
        AllZonesCurrentStatus += 1;
      }
      // remember for next time
      RightPreviousStatus = CurrentZoneStatus;
    }
  }
  
#ifdef TRACE_PPC
  // print debug data only when someone is within the field of view
  trace_count++;
  if ((CurrentZoneStatus == SOMEONE) || (LeftPreviousStatus == SOMEONE) || (RightPreviousStatus == SOMEONE))
    trace_count = 0;
#if 1
  if (trace_count < TIMES_WITH_NO_EVENT)
    printf ("%d,%d,%d,%d,%d\n", zone, Distance, MinDistance, RangeStatus, PeopleCount);
#endif
#endif
  
  // if an event has occured
  if (AnEventHasOccured) {
    if (PathTrackFillingSize < 4) {
      PathTrackFillingSize ++;
    }
    
    // if nobody anywhere lets check if an exit or entry has happened
    if ((LeftPreviousStatus == NOBODY) && (RightPreviousStatus == NOBODY)) {
      
      // check exit or entry only if PathTrackFillingSize is 4 (for example 0 1 3 2) and last event is 0 (nobobdy anywhere)
      if (PathTrackFillingSize == 4) {
        // check exit or entry. no need to check PathTrack[0] == 0 , it is always the case
        
        if ((PathTrack[1] == 1)  && (PathTrack[2] == 3) && (PathTrack[3] == 2)) {
          // This an entry
          PeopleCount ++;
          // reset the table filling size in case an entry or exit just found
          DistancesTableSize[0] = 0;
          DistancesTableSize[1] = 0;
          APP_LOG(TS_OFF, VLEVEL_M,"Walk In, People Count=%d\n", PeopleCount);
          sts_people_count_sensor_data.Walk_In_People_Count ++;
          sts_people_count_number_changed = 1;
        } else if ((PathTrack[1] == 2)  && (PathTrack[2] == 3) && (PathTrack[3] == 1)) {
          // This an exit
          PeopleCount --;
          // reset the table filling size in case an entry or exit just found
          DistancesTableSize[0] = 0;
          DistancesTableSize[1] = 0;
          APP_LOG(TS_OFF, VLEVEL_M,"Walk Out, People Count=%d\n", PeopleCount);
          sts_people_count_sensor_data.Walk_Out_People_Count ++;
          sts_people_count_number_changed = 1;
        } else {
          // reset the table filling size also in case of unexpected path
          DistancesTableSize[0] = 0;
          DistancesTableSize[1] = 0;
          APP_LOG(TS_OFF, VLEVEL_M,"Walk Around ie. Wrong path\n");
          sts_people_count_sensor_data.Walk_Around_People_Count ++;
        }
      }
      
      PathTrackFillingSize = 1;
    }
    else {
      // update PathTrack
      // example of PathTrack update
      // 0
      // 0 1
      // 0 1 3
      // 0 1 3 1
      // 0 1 3 3
      // 0 1 3 2 ==> if next is 0 : check if exit
      PathTrack[PathTrackFillingSize-1] = AllZonesCurrentStatus;
    }

#if 0
#ifdef TRACE_PPC
    if (AnEventHasOccured) {
      for (int j=0; j<PathTrackFillingSize; j++)
        printf ("%d ", PathTrack[j]);
    }
    printf("\n");
#endif
#endif
  }
  //printf ("Inside PeopleCount = %d , returned\n", PeopleCount);
  // output debug data to main host machine
  return(PeopleCount);
}

void STS_tof_presence_detection_sensor_Read(sts_tof_presence_detection_sensor_data_t *sts_t6_sensor_data)
{
	sts_t6_sensor_data->tof_range_presence_state = sts_tof_presence_state;
}

void STS_people_count_sensor_Read(sts_people_count_sensor_data_t *sts_p2_sensor_data)
{

	sts_p2_sensor_data->Count_Valid					= sts_people_count_sensor_data.Count_Valid;

	sts_p2_sensor_data->Walk_In_People_Count 		= sts_people_count_sensor_data.Walk_In_People_Count;
	sts_p2_sensor_data->Walk_Out_People_Count 		= sts_people_count_sensor_data.Walk_Out_People_Count;
	sts_p2_sensor_data->Walk_Around_People_Count 	= sts_people_count_sensor_data.Walk_Around_People_Count;
	sts_p2_sensor_data->Count_Period 				= sts_people_count_sensor_data.Count_Period;
	sts_p2_sensor_data->Count_Period_Unit 			= sts_people_count_sensor_data.Count_Period_Unit;

	// Sum Day counting
	// if not end of day ---
	sts_people_count_sensor_data.Sum_Day_Walk_In_People_Count 			+= sts_people_count_sensor_data.Walk_In_People_Count;
	sts_people_count_sensor_data.Sum_Day_Walk_Out_People_Count 			+= sts_people_count_sensor_data.Walk_Out_People_Count;
	sts_people_count_sensor_data.Sum_Day_Walk_Around_People_Count 		+= sts_people_count_sensor_data.Walk_Around_People_Count;

	sts_p2_sensor_data->Sum_Day_Walk_In_People_Count 		= sts_people_count_sensor_data.Sum_Day_Walk_In_People_Count;
	sts_p2_sensor_data->Sum_Day_Walk_Out_People_Count 		= sts_people_count_sensor_data.Sum_Day_Walk_Out_People_Count;
	sts_p2_sensor_data->Sum_Day_Walk_Around_People_Count 	= sts_people_count_sensor_data.Sum_Day_Walk_Around_People_Count;


	// else reset day counter
#if 0
	sts_people_count_sensor_data.Sum_Day_Walk_In_People_Count 			= 0;
	sts_people_count_sensor_data.Sum_Day_Walk_Out_People_Count 			= 0;
	sts_people_count_sensor_data.Sum_Day_Walk_Around_People_Count 		= 0;
#endif

	// Sum Week counting
	// if not end of week ---
	sts_people_count_sensor_data.Sum_Week_Walk_In_People_Count 			+= sts_people_count_sensor_data.Sum_Day_Walk_In_People_Count;
	sts_people_count_sensor_data.Sum_Week_Walk_Out_People_Count 		+= sts_people_count_sensor_data.Sum_Day_Walk_Out_People_Count;
	sts_people_count_sensor_data.Sum_Week_Walk_Around_People_Count 		+= sts_people_count_sensor_data.Sum_Day_Walk_Around_People_Count;
	// else reset week counter
#if 0
	sts_people_count_sensor_data.Sum_Week_Walk_In_People_Count 			= 0;
	sts_people_count_sensor_data.Sum_Week_Walk_Out_People_Count 		= 0;
	sts_people_count_sensor_data.Sum_Week_Walk_Around_People_Count 		= 0;
#endif

	// Sum Month counting
	// if not end of month ---
	sts_people_count_sensor_data.Sum_Month_Walk_In_People_Count 		+= sts_people_count_sensor_data.Sum_Day_Walk_In_People_Count;
	sts_people_count_sensor_data.Sum_Month_Walk_Out_People_Count 		+= sts_people_count_sensor_data.Sum_Day_Walk_Out_People_Count;
	sts_people_count_sensor_data.Sum_Month_Walk_Around_People_Count 	+= sts_people_count_sensor_data.Sum_Day_Walk_Around_People_Count;
	// else reset month counter
#if 0
	sts_people_count_sensor_data.Sum_Month_Walk_In_People_Count 		= 0;
	sts_people_count_sensor_data.Sum_Month_Walk_Out_People_Count 		= 0;
	sts_people_count_sensor_data.Sum_Month_Walk_Around_People_Count 	= 0;
#endif

	// Sum Year counting
	// if not end of year ---
	sts_people_count_sensor_data.Sum_Year_Walk_In_People_Count 			+= sts_people_count_sensor_data.Sum_Day_Walk_In_People_Count;
	sts_people_count_sensor_data.Sum_Year_Walk_Out_People_Count 		+= sts_people_count_sensor_data.Sum_Day_Walk_Out_People_Count;
	sts_people_count_sensor_data.Sum_Year_Walk_Around_People_Count 		+= sts_people_count_sensor_data.Sum_Day_Walk_Around_People_Count;
	// else reset year counter
#if 0
	sts_people_count_sensor_data.Sum_Year_Walk_In_People_Count 			= 0;
	sts_people_count_sensor_data.Sum_Year_Walk_Out_People_Count 		= 0;
	sts_people_count_sensor_data.Sum_Year_Walk_Around_People_Count 		= 0;
#endif

	// Sum LifeCycle counting
	sts_people_count_sensor_data.Sum_LifeCycle_Walk_In_People_Count 	+= sts_people_count_sensor_data.Sum_Day_Walk_In_People_Count;
	sts_people_count_sensor_data.Sum_LifeCycle_Walk_Out_People_Count 	+= sts_people_count_sensor_data.Sum_Day_Walk_Out_People_Count;
	sts_people_count_sensor_data.Sum_LifeCycle_Walk_Around_People_Count += sts_people_count_sensor_data.Sum_Day_Walk_Around_People_Count;


	sts_people_count_sensor_data.Walk_In_People_Count 			= 0;
	sts_people_count_sensor_data.Walk_Out_People_Count 			= 0;
	sts_people_count_sensor_data.Walk_Around_People_Count 		= 0;

	//reset counter of counting period
	cnt_today ++;
	if (cnt_today*STS_TxPeriod_sec > 86400)
	{
		cnt_today = 0;
		sts_people_count_sensor_data.Walk_In_People_Count 			= 0;
		sts_people_count_sensor_data.Walk_Out_People_Count 			= 0;
		sts_people_count_sensor_data.Walk_Around_People_Count 		= 0;
	}

	if (cnt_today ==0)
	{	// new day, then check week/month/year, not check every time
		// reset counter at end of day TODO XXX
		if (last_day != this_day) {
			cnt_today = 0;

			sts_people_count_sensor_data.Sum_Day_Walk_In_People_Count 			= 0;
			sts_people_count_sensor_data.Sum_Day_Walk_Out_People_Count 			= 0;
			sts_people_count_sensor_data.Sum_Day_Walk_Around_People_Count 		= 0;
		}
		if (cnt_week)
		// reset counter at end of week TODO XXX
		if (last_week != this_week) {


			sts_people_count_sensor_data.Sum_Week_Walk_In_People_Count 			= 0;
			sts_people_count_sensor_data.Sum_Week_Walk_Out_People_Count 		= 0;
			sts_people_count_sensor_data.Sum_Week_Walk_Around_People_Count 		= 0;
		}

		// reset counter at end of month TODO XXX
		if (last_month != this_month) {

			sts_people_count_sensor_data.Sum_Month_Walk_In_People_Count 			= 0;
			sts_people_count_sensor_data.Sum_Month_Walk_Out_People_Count 		= 0;
			sts_people_count_sensor_data.Sum_Month_Walk_Around_People_Count 		= 0;
		}

		if (last_year != this_year) {

			sts_people_count_sensor_data.Sum_Year_Walk_In_People_Count 			= 0;
			sts_people_count_sensor_data.Sum_Year_Walk_Out_People_Count 		= 0;
			sts_people_count_sensor_data.Sum_Year_Walk_Around_People_Count 		= 0;
		}
	}

	// reset counter at end of year TODO XXX
	// reset counter at end of year TODO XXX

	// write to NVM store	 TODO XXX

}

void STS_TOF_VL53LX_PeopleCounting_Process_Init(void)
{
	APP_LOG(TS_OFF, VLEVEL_M,"\n\n# YUNHORN STS-P2 PEOPLE COUNTING PROCESS #\r\n");
	sts_tof_vl53lx_peoplecount_init();

}

void STS_TOF_VL53LX_PeopleCounting_Process_Start(void)
{

	//APP_LOG(TS_OFF, VLEVEL_M,"###############   TOF  VL53LX_ PEOPLE COUNTING SUB-PROCESS \r\n");
	{
		status = sts_tof_vl53lx_peoplecount_start();
		sts_people_count_sensor_data.Count_Valid = (status ==0)? 1:0;
		//APP_LOG(TS_OFF, VLEVEL_M,"############### SUB-PROCESS running .... \r\n");
	}

}

void STS_TOF_VL53LX_PresenceDetection_Process_Start(void)
{

	//APP_LOG(TS_OFF, VLEVEL_M,"###############   TOF  VL53LX_ PRESENCE DETECTION SUB-PROCESS \r\n");
	{
		status = sts_tof_vl53lx_presence_detection_start();
		sts_people_count_sensor_data.Count_Valid = (status ==0)? 1:0;
		//APP_LOG(TS_OFF, VLEVEL_M,"############### SUB-PROCESS running .... \r\n");
	}

}
int sts_tof_vl53lx_presence_detection_start(void)
{
	  //uint8_t byteData, sensorState=0;
	  uint16_t wordData=0;
	  uint16_t Distance, Signal;
	  uint8_t RangeStatus;
	  uint8_t dataReady;
	  //char DisplayStr[5];

	 /* read and display data */

  while (dataReady == 0) {
    status = VL53L1X_CheckForDataReady(dev, &dataReady);
    HAL_Delay(1);
    wordData++;
    if (wordData > 30) return -1;  // 50 ms timer, so make this 60% to fail back
  }
  dataReady = 0;
  status += VL53L1X_GetRangeStatus(dev, &RangeStatus);
  status += VL53L1X_GetDistance(dev, &Distance);
  status += VL53L1X_GetSignalPerSpad(dev, &Signal);
  status += VL53L1X_ClearInterrupt(dev); /* clear interrupt has to be called to enable next interrupt*/
  if (status != 0) {
  	APP_LOG(TS_OFF, VLEVEL_L,"Error in operating the device\n");
    return (-1);
  }
  //HAL_Delay(WAIT_BEFORE_PROGRAMMING_OTHER_ZONE_CENTER);  // 10, 8, 7, 6 tested OK

  status = VL53L1X_SetROICenter(dev, center[Zone]);
  if (status != 0) {
  	APP_LOG(TS_OFF, VLEVEL_L,"Error in chaning the center of the ROI\n");
    return (-1);
  }
  // check the status of the ranging. In case of error, lets assume the distance is the max of the use case
  // Value RangeStatus string Comment
  // 0 VL53L1_RANGESTATUS_RANGE_VALID Ranging measurement is valid
  // 1 VL53L1_RANGESTATUS_SIGMA_FAIL Raised if sigma estimator check is above the internal defined threshold
  // 2 VL53L1_RANGESTATUS_SIGNAL_FAIL Raised if signal value is below the internal defined threshold
  // 4 VL53L1_RANGESTATUS_OUTOFBOUNDS_ FAIL Raised when phase is out of bounds
  // 5 VL53L1_RANGESTATUS_HARDWARE_FAIL Raised in case of HW or VCSEL failure
  // 7 VL53L1_RANGESTATUS_WRAP_TARGET_ FAIL Wrapped target, not matching phases
  // 8 VL53L1_RANGESTATUS_PROCESSING_ FAIL Internal algorithm underflow or overflow
  // 14 VL53L1_RANGESTATUS_RANGE_INVALID The reported range is invalid
  if ((RangeStatus == 0) || (RangeStatus == 4) || (RangeStatus == 7)) {
    if (Distance <= ppc_cfg[sts_distance_mode].min_distance) //MIN_DISTANCE) // wraparound case see the explanation at the constants definition place
      Distance = ppc_cfg[sts_distance_mode].max_distance + ppc_cfg[sts_distance_mode].min_distance; //MAX_DISTANCE + MIN_DISTANCE;
  }
  else // severe error cases
    Distance = ppc_cfg[sts_distance_mode].max_distance;  //MAX_DISTANCE;
  // inject the new ranged distance in the people counting algorithm


  if (Distance < ppc_cfg[sts_distance_mode].dist_threshold)
  {
	  PresenceState = 1;
	  LED1_ON;
  } else {
	  PresenceState = 0;
	  LED1_OFF;
  }
  //PresenceState = ProcessPresenceDetectionData(Distance, Zone, RangeStatus);
  //printf("\nPresenceState =%d \n\r", PresenceState);
  if (PresenceState == PrevPresenceState)
  {
	  sts_tof_presence_state_changed = 0;
	  return 0;
  } else {
	  sts_tof_presence_state = PresenceState;
	  PrevPresenceState = PresenceState;
	  sts_tof_presence_state_changed = 1;
  }
  //APP_LOG(TS_OFF, VLEVEL_M,"%d,%d,%d\n", Zone, Distance, Signal);
  Zone++;
  Zone = Zone%2;

  return 0;

}
int sts_tof_vl53lx_peoplecount_init(void)
{
  //int8_t error;
  uint8_t byteData, sensorState=0;
  uint16_t wordData;

  /* int PplCounter = 0;*/
  
  /* MCU Configuration----------------------------------------------------------*/
  
  /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
  

  /* Configure the system clock */
  

  /* Initialize all configured peripherals */
  //APP_LOG(TS_OFF, VLEVEL_H,"XNUCLEO53L1A1_Init Start .......... \r\n");
  status = XNUCLEO53L1A1_Init();
  //APP_LOG(TS_OFF, VLEVEL_H,"XNUCLEO53L1A1_Init Status : %X\n", status);

  
  status = XNUCLEO53L1A1_ResetId(XNUCLEO53L1A1_DEV_CENTER, 0); // Reset ToF sensor
  HAL_Delay(2);
  status = XNUCLEO53L1A1_ResetId(XNUCLEO53L1A1_DEV_CENTER, 1); // Reset ToF sensor
  

  // Those basic I2C read functions can be used to check your own I2C functions */
  status = VL53L1_RdByte(dev, 0x010F, &byteData);
  APP_LOG(TS_OFF, VLEVEL_H,"VL53L1X Model_ID: %X\n", byteData);
  status = VL53L1_RdByte(dev, 0x0110, &byteData);
  APP_LOG(TS_OFF, VLEVEL_M,"VL53L1X Module_Type: %X\n", byteData);
  status = VL53L1_RdWord(dev, 0x010F, &wordData);
  APP_LOG(TS_OFF, VLEVEL_M,"VL53L1X: %X\n", wordData);
  sensor_id = wordData;
  wordData = 0;
  while (sensorState == 0) {
    status = VL53L1X_BootState(dev, &sensorState);
    HAL_Delay(2);
    wordData ++;
    if (wordData > 8000) {
    	APP_LOG(TS_OFF, VLEVEL_M,"\n\n ***Failed to boot Chip***\n\n\n");
    	return status;
    }
  }
  APP_LOG(TS_OFF, VLEVEL_M,"\nChip booted\n\n");
  
  /* Initialize and configure the device according to people counting need */
  status = VL53L1X_SensorInit(dev);
  status += VL53L1X_SetDistanceMode(dev, ppc_cfg[sts_distance_mode].distance_mode);  /* 1=short, 2=long, DISTANCE_MODE */
  status += VL53L1X_SetTimingBudgetInMs(dev, ppc_cfg[sts_distance_mode].timing_budget);  /* TIMING_BUDGET, in ms possible values [15, 20, 50, 100, 200, 500] */
  status += VL53L1X_SetInterMeasurementInMs(dev, ppc_cfg[sts_distance_mode].timing_budget);
  status += VL53L1X_SetROI(dev, ppc_cfg[sts_distance_mode].rows_of_SPADS, 16); /* minimum ROI 4,4 */
  center[0]=ppc_cfg[sts_distance_mode].front_zone_center;
  center[1]=ppc_cfg[sts_distance_mode].back_zone_center;
  if (status != 0) {
	  APP_LOG(TS_OFF, VLEVEL_H,"Initialization or configuration of the device\n");
    return (-1);
  }
  APP_LOG(TS_OFF, VLEVEL_M,"\n\nInit finished...\r\nStart counting people with profile : %s...\r\n\n\n\n", PROFILE_STRING);
  status = VL53L1X_StartRanging(dev);   /* This function has to be called to enable the ranging */

  if (status != 0) {
  	APP_LOG(TS_OFF, VLEVEL_H,"Error in start ranging\n");
    return (-1);
  }
  return 0;
}
int sts_tof_vl53lx_peoplecount_start(void)
{
	  //uint8_t byteData, sensorState=0;
	  uint16_t wordData=0;
	  uint16_t Distance, Signal;
	  uint8_t RangeStatus;
	  uint8_t dataReady;
	  //char DisplayStr[5];

	 /* read and display data */

    while (dataReady == 0) {
      status = VL53L1X_CheckForDataReady(dev, &dataReady);
      HAL_Delay(1);
      wordData++;
      if (wordData > 30) return -1;  // 50 ms timer, so make this 60% to fail back
    }
    dataReady = 0;
    status += VL53L1X_GetRangeStatus(dev, &RangeStatus);
    status += VL53L1X_GetDistance(dev, &Distance);
    status += VL53L1X_GetSignalPerSpad(dev, &Signal);
    status += VL53L1X_ClearInterrupt(dev); /* clear interrupt has to be called to enable next interrupt*/
    if (status != 0) {
    	APP_LOG(TS_OFF, VLEVEL_L,"Error in operating the device\n");
      return (-1);
    }
    //HAL_Delay(WAIT_BEFORE_PROGRAMMING_OTHER_ZONE_CENTER);  // 10, 8, 7, 6 tested OK
    status = VL53L1X_SetROICenter(dev, center[Zone]);
    if (status != 0) {
    	APP_LOG(TS_OFF, VLEVEL_L,"Error in chaning the center of the ROI\n");
      return (-1);
    }
    // check the status of the ranging. In case of error, lets assume the distance is the max of the use case
    // Value RangeStatus string Comment
    // 0 VL53L1_RANGESTATUS_RANGE_VALID Ranging measurement is valid
    // 1 VL53L1_RANGESTATUS_SIGMA_FAIL Raised if sigma estimator check is above the internal defined threshold
    // 2 VL53L1_RANGESTATUS_SIGNAL_FAIL Raised if signal value is below the internal defined threshold
    // 4 VL53L1_RANGESTATUS_OUTOFBOUNDS_ FAIL Raised when phase is out of bounds
    // 5 VL53L1_RANGESTATUS_HARDWARE_FAIL Raised in case of HW or VCSEL failure
    // 7 VL53L1_RANGESTATUS_WRAP_TARGET_ FAIL Wrapped target, not matching phases
    // 8 VL53L1_RANGESTATUS_PROCESSING_ FAIL Internal algorithm underflow or overflow
    // 14 VL53L1_RANGESTATUS_RANGE_INVALID The reported range is invalid
    if ((RangeStatus == 0) || (RangeStatus == 4) || (RangeStatus == 7)) {
      if (Distance <= ppc_cfg[sts_distance_mode].min_distance) //MIN_DISTANCE) // wraparound case see the explanation at the constants definition place
        Distance = ppc_cfg[sts_distance_mode].max_distance + ppc_cfg[sts_distance_mode].min_distance; //MAX_DISTANCE + MIN_DISTANCE;
    }
    else // severe error cases
      Distance = ppc_cfg[sts_distance_mode].max_distance;  //MAX_DISTANCE;
    // inject the new ranged distance in the people counting algorithm
    PplCounter = ProcessPeopleCountingData(Distance, Zone, RangeStatus);
    //printf("\nPplCounter =%d \n\r", PplCounter);
    //sprintf(DisplayStr, "%4d", PplCounter); // only use for special EVK with display
    //XNUCLEO53L1A1_SetDisplayString(DisplayStr);
    //APP_LOG(TS_OFF, VLEVEL_H,"%d,%d,%d\n", Zone, Distance, Signal);
    Zone++;
    Zone = Zone%2;

    return 0;
}