/**
  ******************************************************************************
  * @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 "usart.h"
#include <stdlib.h>
#if	(defined(STS_P2)||defined(STS_T6))
#include "VL53L1X_api.h"
#elif defined(L8)
#include "53l8a1_ranging_sensor.h"
#include "app_tof_pin_conf.h"
#include "yunhorn_sts_sensors.h"
#include "sts_lamp_bar.h"
volatile uint8_t fhmos_fall=0, fhmos_human_movement=0, fhmos_occupancy=0, fhmos_sos_alarm=0;
volatile uint32_t fhmos_fall_counter=0;
volatile uint32_t sts_low_threshold=1500, sts_high_threshold=2800, sts_occupancy_threshold=2300;

volatile sts_fhmos_sensor_data_t fhmos={0x0};
volatile sts_fhmos_sensor_config_t fhmos_cfg;
volatile sts_fhmos_sensor_ambient_height_t fhmos_bg={0x0}, fhmos_gesture={0x0};
volatile sts_fhmos_sensor_data_t fhmos_data;
extern volatile uint8_t sts_fhmos_result;
volatile uint8_t sts_mask_bitmap[8]={0x0}, fhmos_gesture_bitmap[8]={0x0};
extern volatile uint16_t sts_sensor_install_height; //in mm
extern volatile uint8_t sts_lamp_bar_color;
extern volatile uint8_t sts_status_color;
extern volatile uint8_t sts_hall1_read,sts_hall2_read; // Above hall1_read == reed_hall_result, hall2_read == emergency_button
extern volatile uint8_t sts_pir_result;
extern volatile uint8_t sts_fhmos_state_changed;
extern volatile uint8_t sts_fhmos_bitmap_pending;
#endif
#include "stm32wlxx_nucleo.h"

/* Private typedef -----------------------------------------------------------*/
/*
 * The application is to showcase the threshold detection
 * functionality of this device.
 *
 * When the device detects a target that match the configuration an IT is generated
 * and the host will start printing the measurement information on the serial connection (UART).
 *
 * Here is the default configuration:
 *
 * ITConfig.Criteria = RS_IT_IN_WINDOW;
 * ITConfig.LowThreshold = 200; // distance in mm
 * ITConfig.HighThreshold = 600; // distance in mm
 *
 * Other availables interrupt generation criteria for this device are:
 *
 * - RS_IT_DEFAULT          // IT if new measurement is ready (no thresholds)
 * - RS_IT_IN_WINDOW        // IT if distance > thresh_high
 * - RS_IT_OUT_OF_WINDOW    // IT if distance < LowThreshold OR distance > HighThreshold
 * - RS_IT_BELOW_LOW        // IT if distance <= LowThreshold
 * - RS_IT_ABOVE_HIGH       // IT if distance > HighThreshold
 * - RS_IT_EQUAL_LOW        // IT if distance == LowThreshold
 * - RS_IT_NOT_EQUAL_LOW    // IT if distance != LowThreshold
 */

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

#ifdef L8
/* for VL53L8A1			*/
#define RANGING_FREQUENCY (10U) /* Ranging frequency Hz (shall be consistent with TimingBudget value) */
#define LOW_THRESHOLD  (600U)
#define HIGH_THRESHOLD (2000U)
// floor, eg. 2200
#define OCCUPANCY_THRESHOLD (1500)  // assume high people 2000-450 = 1550

/*	ceiling  -------------------- zero - 00            ref. 3000 mm high
 *
 *
 *  people high   --------------- 2100    mm           high people
 *
 *
 *
 *
 * people low  ------------------- 900 mm			child normal
 *
 *
 *
 * other things   ----------------- 400 mm
 *
 *
 * floor       ----------------------00000
 *
 *
 */
/* Private variables ---------------------------------------------------------*/
static RANGING_SENSOR_Capabilities_t Cap;
static RANGING_SENSOR_ProfileConfig_t Profile;
static RANGING_SENSOR_Result_t Result;
//static VL53L8CX_ResultsData L8CXResult;
//static VL53L8CX_Motion_Configuration motion_config;	/* Motion configuration*/

//static VL53L8CX_Configuration	Dev;

static void MX_53L8A1_ThresholdDetection_Init(void);
static void MX_53L8A1_ThresholdDetection_Process(void);
static void print_result(RANGING_SENSOR_Result_t *Result);
//static void sts_generate_fall_gesture_map(void);
#endif
static int32_t status = 0;

volatile uint8_t ToF_EventDetected = 0;
uint16_t sensor_id=0;

/* Private function prototypes -----------------------------------------------*/
#if	(defined(STS_P2)||defined(STS_T6))
uint8_t sts_vl53lx_ranging(uint16_t *ranged_distance, 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);
#endif
uint8_t IsInterruptDetected(uint16_t dev);
#if	(defined(STS_P2)||defined(STS_T6))
void STS_TOF_VL53LX_Range_Process(uint8_t range_mode, uint16_t *range_distance);
#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");

  //MX_53L1A2_SimpleRanging_Init();
  //STS_TOF_VL53LX_PeopleCounting_Process();
#ifdef	L8
  MX_53L8A1_ThresholdDetection_Init();
  MX_53L8A1_ThresholdDetection_Process();
#elif	defined(STS_R1D)
  XWL55_WLE5_53L0X_Init();
#endif

  /* USER CODE BEGIN TOF_Init_PostTreatment */

  /* USER CODE END TOF_Init_PostTreatment */
}


/*
 * LM background task
 */
void STS_LMZ_Ambient_Height_Scan_Process(void)
{

#if 1
	uint8_t i=0, repeat=1;
	uint32_t range_distance =0;

	for (i=0; i<64; i++)
	{
		fhmos_bg.h2cm[i] = 0;
		fhmos_bg.maskoff[i] = 0;
	}
	for (i=0;i<8;i++)
	sts_mask_bitmap[i]=0;

//	printf("sts sensor install height = %4d \r\n", (int)sts_sensor_install_height);


	sts_high_threshold=sts_sensor_install_height;
	sts_low_threshold=sts_sensor_install_height-1400;
	STS_TOF_L8_Reconfig();
	//printf("%c[2H", 27); /* clear screen */
	for (uint8_t k=0; k<repeat; k++)
	{
		STS_TOF_L8_Process();
		for (uint8_t i = 0; i < 64; i++)
		{
			  /* Print distance and status */
				if ((Result.ZoneResult[i].NumberOfTargets > 0))
				{
					range_distance = (uint32_t)Result.ZoneResult[i].Distance[0];

					//if (sts_sensor_install_height > range_distance)
						fhmos_bg.h2cm[i] +=  ((uint32_t)sts_sensor_install_height - range_distance);
						if (abs(fhmos_bg.h2cm[i])<100){
							fhmos_bg.maskoff[i] = 0;
						} else {
							fhmos_bg.maskoff[i] = 1;

						}
						sts_mask_bitmap[(uint8_t)(i/8)] |= (fhmos_bg.maskoff[i])<<(7-i%8);
						if (i%8==0) printf("\r\n");
					printf("|%4d %4d ", range_distance, fhmos_bg.h2cm[i]);

				}
				else {
					fhmos_bg.h2cm[i] += 0;
					//printf(" .%d. ", i);
				}

		}

	}
	for (i=0; i<64; i++)
	{
		if (i%8==0) printf("\r\n");
		printf("|%d ", (uint8_t)fhmos_bg.maskoff[i]);
	}
	for (i=0; i<8; i++)
		printf("%02X\r\n",sts_mask_bitmap[i]);
#endif
}

void sts_generate_fall_gesture_map(void)
{
	uint8_t i=0,head_xy=0;
	uint32_t range_distance =0, sts_fall_head_position=8000; // assume max range 8000 mm, actual 4500mm

	for (i=0; i<64; i++)
	{
		fhmos_gesture.h2cm[i] = 0;
		fhmos_gesture.maskoff[i] = 0;
	}
	fhmos_gesture.head_level =1000;  // 1000 mm
	fhmos_gesture.head_xy = 31; //center of FOV

	for (i=0;i<8;i++)
		fhmos_gesture_bitmap[i]=0;

	for (uint8_t i = 0; i < 64; i++)
	{
			if ((Result.ZoneResult[i].NumberOfTargets > 0))
			{
				range_distance = (uint32_t)Result.ZoneResult[i].Distance[0];
				if (range_distance < sts_fall_head_position)
				{
					sts_fall_head_position = range_distance;  // simply find out the head level
					head_xy = i;					// head x, y coordination in 8x8 matrix
				}
				fhmos_gesture.h2cm[i] +=  ((uint32_t)sts_sensor_install_height - range_distance);

				fhmos_gesture.maskoff[i]= (abs(fhmos_gesture.h2cm[i])<100)? 0:1;
				fhmos_gesture_bitmap[(uint8_t)(i/8)] |= (fhmos_gesture.maskoff[i])<<(7-i%8);

				// debug
				if (i%8==0) printf("\r\n");
				printf("|%4d %4d ", range_distance, fhmos_gesture.h2cm[i]);
			}
			else {
				fhmos_gesture.h2cm[i] += 0;
			}
	}

	fhmos_gesture.head_level = 0xff&((sts_sensor_install_height - sts_fall_head_position)/10); // head level from floor in CM
	fhmos_gesture.head_xy = head_xy&0xff;
#if 0
		for (i=0; i<64; i++)
		{
			if (i%8==0) printf("\r\n");
			printf("|%d ", (uint8_t)fhmos_gesture.maskoff[i]);
		}
		for (i=0; i<8; i++)
			printf("%02X\r\n",fhmos_gesture_bitmap[i]);
#endif

}
uint16_t MX_TOF_Ranging_Process(void)
{

#if	(defined(STS_P2)||defined(STS_T6))
	uint16_t range_distance=0;
	uint8_t range_mode = 2; //STS_TOF_LONG_RANGE;
	STS_TOF_VL53LX_Range_Process(range_mode, &range_distance);
	APP_LOG(TS_OFF, VLEVEL_M, "\n VL53L1 Range distance =%u mm \n\r", (uint16_t)range_distance);

	return (uint16_t) range_distance;
#elif defined(L8)
	uint32_t range_distance=0;
	uint8_t center_roi[4] = {27,28,35,36};
	// uint8_t range_mode = 2; //STS_TOF_LONG_RANGE;
	// int status = VL53L8A1_RANGING_SENSOR_GetDistance(VL53L8A1_DEV_CENTER, &Result);

	// printf("\r|    27    |    28    |    35    |    36    |\r\n");
	for (uint8_t k=0; k<10; k++)
	{
		STS_TOF_L8_Process();

		// int status = VL53L8A1_RANGING_SENSOR_GetDistance(VL53L8A1_DEV_CENTER, &Result);
		// printf("\r\n status =%d \r\n", status);


		for (uint8_t zone_nbr = 0; zone_nbr < 4; zone_nbr++)
		{
		  /* Print distance and status */
			if (Result.ZoneResult[center_roi[zone_nbr]].NumberOfTargets > 0)
			{
			  printf("|  %04ld  %2ld",
					 (long)Result.ZoneResult[center_roi[zone_nbr]].Distance[RANGING_SENSOR_NB_TARGET_PER_ZONE-1],
					 (long)Result.ZoneResult[center_roi[zone_nbr]].Status[RANGING_SENSOR_NB_TARGET_PER_ZONE-1]);
			}
			else {
				printf("|    --    ");
			}

			range_distance += Result.ZoneResult[center_roi[zone_nbr]].Distance[RANGING_SENSOR_NB_TARGET_PER_ZONE-1];
		}
	}

    printf("\n\r");

    range_distance /=40;
	printf("| %u mm\r\n", (uint16_t)range_distance);

	return (uint16_t) range_distance;
#endif

}
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();
#ifdef L8
	MX_53L8A1_ThresholdDetection_Process();
	//printf("\r\n Tof Process\r\n");
	//STS_TOF_L8_Process();
#endif

  /* USER CODE BEGIN TOF_Process_PostTreatment */

  /* USER CODE END TOF_Process_PostTreatment */
}
#ifdef L8
void STS_TOF_L8_Init(void)
{
	MX_53L8A1_ThresholdDetection_Init();
	MX_53L8A1_ThresholdDetection_Process();
}
void STS_TOF_L8_Process(void)
{
	  //while (1)
	  {
	    /* interrupt mode */
	    if (ToF_EventDetected != 0)
	    {
	      ToF_EventDetected = 0;

	      status = VL53L8A1_RANGING_SENSOR_GetDistance(VL53L8A1_DEV_CENTER, &Result);
	      //printf("\r\n status =%d \r\n", status);
	      if (status == BSP_ERROR_NONE)
	      {
	        print_result(&Result);
	      } else {
	    	  printf("\r\n x \r\n");
	      }
	    }
	  }
}

void STS_TOF_L8_Reconfig(void)
{
	MX_53L8A1_ThresholdDetection_ConfigIT(sts_low_threshold, sts_high_threshold);
}


/*   VL53L8A1    */
static void MX_53L8A1_ThresholdDetection_Init(void)
{
  /* Initialize Virtual COM Port */
  //BSP_COM_Init(COM1);
  MX_USART2_UART_Init();

  /* Sensor reset */
  HAL_GPIO_WritePin(VL53L8A1_PWR_EN_C_PORT, VL53L8A1_PWR_EN_C_PIN, GPIO_PIN_RESET);
  HAL_Delay(2);
  HAL_GPIO_WritePin(VL53L8A1_PWR_EN_C_PORT, VL53L8A1_PWR_EN_C_PIN, GPIO_PIN_SET);
  HAL_Delay(2);

  HAL_GPIO_WritePin(VL53L8A1_LPn_C_PORT, VL53L8A1_LPn_C_PIN, GPIO_PIN_RESET);
  HAL_Delay(2);
  HAL_GPIO_WritePin(VL53L8A1_LPn_C_PORT, VL53L8A1_LPn_C_PIN, GPIO_PIN_SET);
  HAL_Delay(2);


  status = VL53L8A1_RANGING_SENSOR_Init(VL53L8A1_DEV_CENTER);

  if (status != BSP_ERROR_NONE)
  {
    printf("VL53L8A1_RANGING_SENSOR_Init failed\n");
    //while (1);
  }

}
void MX_53L8A1_ThresholdDetection_ConfigIT(uint32_t low_threshold, uint32_t high_threshold)
{

	  RANGING_SENSOR_ITConfig_t ITConfig;

	  status = VL53L8A1_RANGING_SENSOR_Stop(VL53L8A1_DEV_CENTER);

	  /* threshold parameters */
	  ITConfig.Criteria = RS_IT_IN_WINDOW;
	  ITConfig.LowThreshold = low_threshold; /* mm */
	  ITConfig.HighThreshold = high_threshold; /* mm */

	  VL53L8A1_RANGING_SENSOR_ConfigIT(VL53L8A1_DEV_CENTER, &ITConfig);

	  status = VL53L8A1_RANGING_SENSOR_Start(VL53L8A1_DEV_CENTER, RS_MODE_ASYNC_CONTINUOUS);

	    if (status != BSP_ERROR_NONE)
	    {
	      printf("VL53L8A1_RANGING_SENSOR_Start failed\n");
	      while (1);
	    }
}

static void MX_53L8A1_ThresholdDetection_Process(void)
{
  uint32_t Id;

  RANGING_SENSOR_ITConfig_t ITConfig;

  VL53L8A1_RANGING_SENSOR_ReadID(VL53L8A1_DEV_CENTER, &Id);
  VL53L8A1_RANGING_SENSOR_GetCapabilities(VL53L8A1_DEV_CENTER, &Cap);

  Profile.RangingProfile = RS_PROFILE_8x8_AUTONOMOUS;
  Profile.TimingBudget = TIMING_BUDGET;
  Profile.Frequency = RANGING_FREQUENCY; /* Ranging frequency Hz (shall be consistent with TimingBudget value) */
  Profile.EnableAmbient = 0; /* Enable: 1, Disable: 0 */
  Profile.EnableSignal = 0; /* Enable: 1, Disable: 0 */

  /* set the profile if different from default one */
  VL53L8A1_RANGING_SENSOR_ConfigProfile(VL53L8A1_DEV_CENTER, &Profile);

  /* threshold parameters */
  ITConfig.Criteria = RS_IT_IN_WINDOW;
  ITConfig.LowThreshold = LOW_THRESHOLD; /* mm */
  ITConfig.HighThreshold = HIGH_THRESHOLD; /* mm */

  VL53L8A1_RANGING_SENSOR_ConfigIT(VL53L8A1_DEV_CENTER, &ITConfig);


  status = VL53L8A1_RANGING_SENSOR_Start(VL53L8A1_DEV_CENTER, RS_MODE_ASYNC_CONTINUOUS);

  if (status != BSP_ERROR_NONE)
  {
    printf("VL53L8A1_RANGING_SENSOR_Start failed\n");
    while (1);
  }
#if 0
  printf("\033[2H\033[2J");
  printf("53L8A1 Threshold Detection demo application\n\r");
  printf("-------------------------------------------\n\r");
  printf("please put a target between %d and %d millimeters from the sensor\n\r",
		  LOW_THRESHOLD, HIGH_THRESHOLD);
#endif

#if 0
  	  while (1)
  {
    /* interrupt mode */
    if (ToF_EventDetected != 0)
    {
      ToF_EventDetected = 0;

      status = VL53L8A1_RANGING_SENSOR_GetDistance(VL53L8A1_DEV_CENTER, &Result);

      if (status == BSP_ERROR_NONE)
      {
        print_result(&Result);
      }
    }
  }
#endif
}

static void print_result(RANGING_SENSOR_Result_t *Result)
{
  int8_t i;
  int8_t j;
  int8_t k;
  int8_t l;
  uint8_t zones_per_line;
  static uint8_t prev_occupy_state=0;
  uint32_t center_range_distance=0;
  uint8_t center_roi[4] = {27,28,35,36};
  static uint32_t prev_distance[64]={0};
  uint32_t motion_diff=0, motion_power=0;
  uint32_t motion_power_threshold = 3200; // 64*50mm

  SysTime_t sensor_event_time = SysTimeGetMcuTime();
  uint32_t time_stamp=STS_Get_Date_Time_Stamp();


  //printf("\r\n Motion level Calculation \r\n");

  for (j=0; j<64; j++)
  {
	  if (prev_distance[j] > (uint32_t)Result->ZoneResult[j].Distance[0])
			  motion_diff = (prev_distance[j] - (uint32_t)Result->ZoneResult[j].Distance[0]);
	  else motion_diff = (uint32_t)Result->ZoneResult[j].Distance[0] - prev_distance[j];
	  //printf("\r\nj=%d, diff=%d ", j, motion_diff);
		motion_power += motion_diff;
		prev_distance[j] = (uint16_t)Result->ZoneResult[j].Distance[0];
  }
  motion_power /=64;
  if (motion_power > fhmos_cfg.th_motion_power_level)
	  {
	  fhmos_human_movement =1;
	  } else {
		  fhmos_human_movement =0;
	  }
#if 0
  printf("\r\n Motion Power =%d Average=%d Human Movement =%d, th=%d \r\n", motion_power*64, motion_power, fhmos_human_movement, fhmos_cfg.th_motion_power_level);
#endif
  zones_per_line = ((Profile.RangingProfile == RS_PROFILE_8x8_AUTONOMOUS) ||
                    (Profile.RangingProfile == RS_PROFILE_8x8_CONTINUOUS)) ? 8 : 4;

  	  //printf("%c[2H", 27); /* clear screen */
  for (i=0; i<4; i++)
  {

	  center_range_distance +=(uint32_t)(Result->ZoneResult[center_roi[i]].Distance[0]);

  }
  	//printf("\n\r Center Range =%4d mm\r\n",center_range_distance );

	//int32_t roi_distance =(uint32_t)(Result->ZoneResult[j + k].Distance[l]);
  	int32_t roi_distance =(uint32_t)center_range_distance/4;

  	/* state tree */

  	if ((roi_distance < sts_high_threshold) && (roi_distance > (sts_high_threshold - fhmos_cfg.th_head_level_height_cm*10))) // TODO XXX
  	{
  		printf("\r\nThreshold: low =%d  occupy=%d  head level=%d  high=%d\r\n", sts_low_threshold, sts_occupancy_threshold, fhmos_cfg.th_head_level_height_cm*10, sts_high_threshold);
  		printf("\r\n YELLOW OR RED Distance=%d \r\n", roi_distance);
  		fhmos_data.state_fall = STS_FHMOS_FALL_STATE_NORMAL;
  		fhmos_data.state_human_movement = STS_FHMOS_HUMAN_MOVEMENT_NORMAL;
  		fhmos_data.status_color = STS_OCCUPY_COLOR; // HOLD THIS COLOR BEFORE ENTER YELLOW STATUS

  		OnSensorL8AStateChanged();
  		fhmos_fall_counter ++;				// TODO XXX  Timer for confirmation

  		if ((fhmos_fall_counter >= 30*fhmos_cfg.th_fall_duration_potential_15sec)&& (fhmos_fall_counter < 30*fhmos_cfg.th_fall_duration_confirm_15sec))
  		{
#if 0
  	  		printf("\r\n  [YELLOW **] Suspicious Fall: distance=%d  [high - head level=[%d]] <= distance <= [high-100 =%d] \r\n",
  	  				roi_distance, (sts_high_threshold - 10*fhmos_cfg.th_head_level_height_cm), (sts_high_threshold - 100));
  			printf("\r\n duration =%d sec, threshold=%d sec\r\n", fhmos_fall_counter, 15*fhmos_cfg.th_fall_duration_potential_15sec);
  			printf("\r\n Fall state --- Yellow \r\n");
#endif


		    OnSensorL8BStateChanged();

  			fhmos_fall =2;
  			fhmos_data.state_fall = STS_FHMOS_FALL_STATE_POTENTIAL;
  			fhmos_data.status_color = STS_FALL_SUSPICIOUS_COLOR;
  			fhmos_data.state_changed_fall = true;

  			sts_fhmos_state_changed |=1;

  		} else 	if (fhmos_fall_counter >= 30*fhmos_cfg.th_fall_duration_confirm_15sec)
  		{
#if 0
  			printf("\r\n  [RED **] Configmed Fall: distance=%d  [high - head level=[%d]] <= distance <= [high-100 =%d] \r\n",
  			  	  				roi_distance, (sts_high_threshold - 10*fhmos_cfg.th_head_level_height_cm), (sts_high_threshold - 100));
  			printf("\r\n duration =%d sec, threshold=%d sec\r\n", fhmos_fall_counter, 15*fhmos_cfg.th_fall_duration_confirm_15sec);
  			printf("\r\n Fall state --- Red \r\n");
  			printf("\r\n  [RED ***] Confirmed Fall state: roi_distance =%d   -----[high=%4d mm  Head Level =%d mm\r\n", roi_distance, sts_high_threshold, 10*fhmos_cfg.th_head_level_height_cm);
#endif

  			OnSensorL8CStateChanged();

  			fhmos_fall =3;
  			fhmos_data.state_fall = STS_FHMOS_FALL_STATE_CONFIRMED;
  			fhmos_data.status_color = STS_FALL_CONFIRMED_COLOR;
  			fhmos_data.state_changed_fall = true;
  			sts_fhmos_state_changed |=1;
  			sts_generate_fall_gesture_map();
  			sts_fhmos_bitmap_pending = 1;
  		}

  	}
  	else if ((roi_distance > sts_low_threshold)&&(roi_distance < sts_occupancy_threshold))
  	  	{
			fhmos_fall_counter = 0;
			fhmos_fall = 0;
			fhmos_human_movement = 1;
			OnSensorL8DStateChanged();
#if 0
			printf("\r\nThreshold: low =%d   occupy=%d   head level=%d high=%d\r\n", sts_low_threshold, sts_occupancy_threshold, fhmos_cfg.th_head_level_height_cm*10, sts_high_threshold);
			printf("\r\n [GREEN ** ]Normal Occupy state: rio distance =%d, low =%d   occupy=%d   head level=%d high=%d\r\n", roi_distance, sts_low_threshold, sts_occupancy_threshold, fhmos_cfg.th_head_level_height_cm*10, sts_high_threshold);
#endif
  	  		//fhmos_occupancy = STS_FHMOS_OCCUPANCY_NORMAL;
  	  		fhmos_data.state_occupancy = STS_FHMOS_OCCUPANCY_NORMAL;
  	  		fhmos_data.state_human_movement = STS_FHMOS_HUMAN_MOVEMENT_NORMAL;
  	  	    fhmos_data.state_changed_human_movemen = true;
  	  		if (fhmos_data.prev_occupancy != fhmos_data.state_occupancy)
  	  		{
  	  			fhmos_data.status_color = STS_OCCUPY_COLOR;
  	  			sts_fhmos_state_changed |=1;

  	  		}

  	  	}else
  	  	{
#if 0
  	  		printf("\r\nThreshold: low =%d   occupy=%d   head level=%d high=%d\r\n", sts_low_threshold, sts_occupancy_threshold, fhmos_cfg.th_head_level_height_cm*10, sts_high_threshold);
  	  		printf("\r\n [BLUE *] No occupy status , distance=%d \r\n", roi_distance);
#endif
  	  			fhmos_data.state_occupancy = STS_FHMOS_OCCUPANCY_NO_OCCUPY;
  	  			fhmos_fall_counter =0;
  	  			fhmos_data.state_human_movement = fhmos_human_movement | STS_FHMOS_HUMAN_MOVEMENT_NO_OCCUPY;
  	  			fhmos_data.state_fall = STS_FHMOS_FALL_STATE_NO_OCCUPY;

  	  			if (fhmos_data.prev_occupancy != fhmos_data.state_occupancy)
  	  			{
  	  				fhmos_data.status_color = STS_VACANT_COLOR;
  	  			    sts_fhmos_state_changed |=1;
  	  			}
  	  	}

  			fhmos_data.prev_occupancy = fhmos_data.state_occupancy;


  			sts_fhmos_result = fhmos_data.state_changed_fall||fhmos_data.state_changed_human_movemen||fhmos_data.state_changed_occupancy;

  			if (fhmos_data.status_color != fhmos_data.prev_status_color)
  			{
  				fhmos_data.prev_status_color = fhmos_data.status_color;
  				sts_lamp_bar_color = fhmos_data.status_color;
  				sts_status_color = sts_lamp_bar_color;
  				sts_fhmos_result = 1;
  				STS_Combined_Status_Processing();
  			}

#if 0
  printf("53L8A1 Threshold Detection demo application\n\r");
  printf("-------------------------------------------\n\r");
  printf("-------- Low= %4d High= %4d ------------\n\r", LOW_THRESHOLD, HIGH_THRESHOLD);
  printf("Cell Format :");
#endif
  //printf(" \033[38;5;10m%20s\033[0m : %20s\n", "Distance [mm]", "Status\r");
#if 0
  for (l = 0; l < RANGING_SENSOR_NB_TARGET_PER_ZONE; l++)
  {
    if ((Profile.EnableAmbient != 0) || (Profile.EnableSignal != 0))
    {
      printf(" %20s : %20s\n", "Signal [kcps/spad]", "Ambient [kcps/spad]\r");
    }
  }

  printf("\n\n");
#endif

#if 0
  for (j = 0; j < Result->NumberOfZones; j += zones_per_line)
  {
#if 0
    for (i = 0; i < zones_per_line; i++) /* number of zones per line */
    {
      printf(" -----------------");
    }
    printf("\n");

    for (i = 0; i < zones_per_line; i++)
    {
    	printf("|                 ");
    }
    printf("|\n");
#endif
    for (l = 0; l < RANGING_SENSOR_NB_TARGET_PER_ZONE; l++)
    {
#if 0
      /* Print distance and status */
      for (k = (zones_per_line - 1); k >= 0; k--)
      {
        if (Result->ZoneResult[j + k].NumberOfTargets > 0)
        {	uint16_t distance_i =  (long)Result->ZoneResult[j + k].Distance[l];
        	if ((distance_i >sts_low_threshold)&&(distance_i <sts_high_threshold))
        	printf("| \033[38;5;10m%5ld\033[0m  :%2d%5ld ", distance_i, (j+k),(long)Result->ZoneResult[j + k].Status[l]);
        	else printf("| %5s  :  %5s ", " ", " ");
        }
        else
        			 printf("| %5s  :  %5s ", " ", " ");
        }
        printf("\n");
#endif
        /*  ---- origin
          printf("| \033[38;5;10m%5ld\033[0m  :%2d%5ld ",
                 (long)Result->ZoneResult[j + k].Distance[l],(j+k),
                 (long)Result->ZoneResult[j + k].Status[l]);
        */
        	int32_t roi_distance =(uint32_t)(Result->ZoneResult[j + k].Distance[l]);
        	/* state tree */
        	if ((roi_distance > sts_low_threshold)&&(roi_distance < sts_occupancy_threshold))
        	{
        		//fhmos_occupancy = 1;
        		sts_fhmos_data.occupancy_state = 1;
        		sts_lamp_bar_color = STS_RED;
        	} else {
        		sts_fhmos_data.occupancy_state = 0;
        		sts_lamp_bar_color = STS_GREEN;

        	}
        	if ((roi_distance < sts_high_threshold) && (roi_distance > (sts_high_threshold - sts_fhmos_cfg.sts_head_level_height_threshold_cm*10))) // TODO XXX
        	{
        		sts_fhmos_data.fall_state = 1;
        		sts_fhmos_data.human_movement_state = 1;

        		fhmos_fall = 1;
        		fhmos_human_movement = 1;
        		fhmos_fall_counter ++;				// TODO XXX  Timer for confirmation
        		if (fhmos_fall_counter++>60)
        			fhmos_fall =2;
        		if (fhmos_fall_counter > 200)
        			fhmos_fall =3;
        		sts_lamp_bar_color = STS_YELLOW;
        	}



#if 0

        	int16_t roi_low = (roi_distance - sts_low_threshold)/10;
        	int16_t roi_high = (sts_high_threshold - roi_distance)/10;

        	if ((roi_low> 0)&&(roi_high>0))
        	{
        		//printf("| \033[38;5;10m%5ld\033[0m  :%2d%5ld ", (uint32_t)(roi_distance-LOW_THRESHOLD), (j+k), (uint32_t)Result->ZoneResult[j + k].Status[l]);
        		printf("| \033[38;5;10m%5d\033[0m  :        ", (uint16_t)roi_low);
        	}
        	else if ((roi_low< 0)|| (roi_high<0))
        	{
        		//printf("| \033[38;5;10m%5s\033[0m  :%2d%5ld ", ".",(j+k), (uint32_t)Result->ZoneResult[j + k].Status[l]);
        		printf("| \033[38;5;10m%5s\033[0m  :        ", ".");
        	}
#endif

#if 0
      if ((Profile.EnableAmbient != 0) || (Profile.EnableSignal != 0))
      {
        /* Print Signal and Ambient */
        for (k = (zones_per_line - 1); k >= 0; k--)
        {
          if (Result->ZoneResult[j + k].NumberOfTargets > 0)
          {
            if (Profile.EnableSignal != 0)
            {
              printf("| %5ld  :  ", (long)Result->ZoneResult[j + k].Signal[l]);
            }
            else
              printf("| %5s  :  ", "X");

            if (Profile.EnableAmbient != 0)
            {
              printf("%5ld ", (long)Result->ZoneResult[j + k].Ambient[l]);
            }
            else
              printf("%5s ", "X");
          }
          else
            printf("| %5s  :  %5s ", "X", "X");
        }
        printf("|\n");
      }
#endif
    }
  }
#endif
#if 0
  for (i = 0; i < zones_per_line; i++)
  {
    printf(" -----------------");
  }
  printf("\n");
#endif
}



#endif

#ifdef	L8
/*  Represent      	Color      Value    Definition
 *  Fall			Blue		0		The Room is not occupied by Human
 * 					Green		1		Human in room in normal level (head above 700 mmH from floor level)
 * 					Yellow		2		Human in low level position for short time potential (Head below 700mmH from floor level
 *					Red			3		Human in low level position with time longer than threshold, e.g. 4 mins.(Head below 700 mm from Floor level
 *	Human
 *  Movement		Blue		0		The Room is not occupied by Human
 *  				Green		1		The room is occupied by human, with movement
 *  				Yellow		2		The room is occupied by human, with motionless for short period of time (???)
 *  				Red			3		The room is occupied by human, with motionless for a period of time longer than threshold period, e.g. 5min.
 *
 *	Occupancy		Blue		0		The room is not occupied by Human
 *					Green		1		The room is occupied by human, with movement
 *					Red			2		The room is occupied by human, for a short period of time (???)
 *
 *  # height is configurable from 10cm to 90 cm, with interval of 1cm
 *  # the period of time is configurable, from 1 min to 30 mins, with 15 sec as interval
 *
 *  sts_height_threshold					(10cm --90 cm, 1cm interval)	(10cm=10, 90cm=90,										"P11T1XXX"
 *  sts_fall_duration_threshold_potential	(1 min to 30 min. 15 sec interval, 1800 sec.   1800=15*120, 60=15*4)	"P11T2XXX"
 *  sts_fall_duration_threshold_confirm		(1 min to 30 min. 15 sec interval, 1800 sec.   1800=15*120, 60=15*4)	"P11T3XXX"
 *
 *  sts_motionless_threshold_short			(1 min to 30 min. 15 sec interval, 1800 sec.   1800=15*120, 60=15*4)		"P11T4XXX"
 *  sts_motionless_threshold_long			(1 min to 30 min. 15 sec interval, 1800 sec.   1800=15*120, 60=15*4)		"P11T5XXX"
 *
 *  sts_occupancy_threshold_overstay		(1 min to 30 min. 15 sec interval, 1800 sec.   1800=15*120, 60=15*4)		"P11T6XXX"
 *
 */

void STS_FHMOS_sensor_read(sts_fhmos_sensor_data_t *sts_data)
{
	//uint8_t fhmos_fall=0, fhmos_human_movement=0, fhmos_occupancy=0, fhmos_sos_alarm=0;
	sts_data->state_occupancy 		= fhmos_data.state_occupancy;
	sts_data->state_fall 			= fhmos_data.state_fall;
	sts_data->state_human_movement 	= fhmos_data.state_human_movement;
	sts_data->state_sos_alarm 		= fhmos_data.state_sos_alarm;

	sts_data->lamp_bar_color		= sts_lamp_bar_color;
	sts_data->state_hall_1			= sts_hall1_read;
	sts_data->state_hall_2			= sts_hall2_read;
	sts_data->state_PIR				= sts_pir_result;;
}



#endif


#if	defined(STS_P2)||defined(STS_T6)||defined(L8)
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_M,"###############   TOF EVENT DETECTED \r\n");
		ToF_EventDetected =0;
		return 1;
	} else {
		return 0;
	}
}
#endif

#if	(defined(STS_P2)||defined(STS_T6))
void STS_TOF_VL53LX_Range_Process(uint8_t range_mode, uint16_t *range_distance)
{
	//uint8_t vl53lx_model = STS_TOF_VL53L1X;
	//uint8_t range_mode = STS_TOF_SHORT_RANGE;
	uint16_t	i_distance_measured = 0;
	uint16_t	i_distance_threshold_mm = 800;
	uint16_t	i_inter_measurement_ms=100, i_macro_timing=33;
	uint16_t	i_roi_width=16, i_sigma_mm=30, i_signal_kcps=2000;

	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(&i_distance_measured, range_mode, i_distance_threshold_mm, i_inter_measurement_ms,i_macro_timing, i_roi_width, i_sigma_mm, i_signal_kcps);
	APP_LOG(TS_OFF, VLEVEL_M, "I_DISTANCE_MEASURED = %d \n", i_distance_measured);
	*range_distance = i_distance_measured;

}

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


uint8_t sts_vl53lx_ranging(uint16_t *ranged_distance, 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 status=0, dev=0x52;
	  uint16_t estimated_distance_mm=0;
	  status = VL53L1X_SensorInit(dev);
	  status += VL53L1X_SetDistanceMode(dev, 2);  /* 1=short, 2=long, DISTANCE_MODE */
	  status += VL53L1X_SetTimingBudgetInMs(dev, 100);  /* TIMING_BUDGET, in ms possible values [15, 20, 50, 100, 200, 500] */
	  status += VL53L1X_SetInterMeasurementInMs(dev, 50);
	  status += VL53L1X_SetROI(dev, 8, 8); /* minimum ROI 4,4 */
	  if (status != 0) {
		  APP_LOG(TS_OFF, VLEVEL_L,"Initialization or configuration of the device\n");
	    return (-1);
	  }
	  status = VL53L1X_StartRanging(dev);   /* This function has to be called to enable the ranging */
	  if (status != 0) {
	  		  APP_LOG(TS_OFF, VLEVEL_L,"Start Range failed\n");
	  	    return (-1);
	  }

		status = VL53L1X_GetDistance(dev, &estimated_distance_mm);
		APP_LOG(TS_OFF,VLEVEL_M,"Target detected! Distance =%d mm \r\n", estimated_distance_mm );
		*ranged_distance = estimated_distance_mm;
		status = VL53L1X_StopRanging(dev);
		APP_LOG(TS_OFF,VLEVEL_M,"End of VL53L1X Ranging Process\n");
		return status;

#if 0
	/*********************************/
	/*   VL53L1X ranging variables  */
	/*********************************/
	uint8_t 				status, loop;
	uint8_t 				dev;
	//uint16_t 				sensor_id=0;
	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_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_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_SetInterruptConfiguration(dev, distance_threshold_mm, 1);  //i_distance_threshold_mm
	status = VL53L1X_SetDistanceThreshold(dev, distance_threshold_mm,distance_threshold_mm, 0,1);  //i_distance_threshold_mm
	/* (Optional) Program a 10Hz ranging frequency */
	status = VL53L1X_SetInterMeasurementInMs(dev, inter_measurement_ms);			// range_interval_ms
	/* Increase the macro timing. This is equivalent as increasing the integration time */
	//status = VL53L1X_SetMacroTiming(dev, macro_timing);				// micro_timing_ms
	status = VL53L1X_SetTimingBudgetInMs(dev, macro_timing);
	/* Enable all the SPADS */
	status = VL53L1X_SetROI(dev, roi_width, 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_SetSigmaThreshold(dev, sigma_mm);
		status |= VL53L1X_SetSignalThreshold(dev, signal_kcps);
	}

	/*********************************/
	/*         Ranging loop          */
	/*********************************/
	status = VL53L1X_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_ClearInterrupt(dev);
			/* Dump debug data */
#if 0
			status = VL53L1X_DumpDebugData(dev, &measurement_status,
						&estimated_distance_mm, &r_sigma_mm, &r_signal_kcps, &r_ambient_kcps);
			*ranged_distance = estimated_distance_mm;
			APP_LOG(TS_OFF,VLEVEL_L,"Target detected! Interrupt raised by sensor, Distance =%d mm \r\n", estimated_distance_mm );
#endif
			status = VL53L1X_GetDistance(dev, &estimated_distance_mm);
			APP_LOG(TS_OFF,VLEVEL_L,"Target detected! Interrupt raised by sensor, Distance =%d mm \r\n", estimated_distance_mm );
			loop++;
		}
	}

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

#ifdef __cplusplus
}
#endif