/* Includes ------------------------------------------------------------------*/
#include "main.h"
#include "sys_app.h"
/* USER CODE BEGIN Includes */
#include "app_tof_peoplecount.h"
#include "VL53L1X_API.h"
#include "VL53l1X_calibration.h"
#include "X-NUCLEO-53L1A1.h"
#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'};
volatile _Bool sts_people_count_number_changed = 0;
uint16_t	dev=0x52;
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}; /* 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;
static uint16_t Distances[2][DISTANCES_ARRAY_SIZE];
static uint8_t DistancesTableSize[2] = {0,0};

/* 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 ProcessPeopleCountingData(int16_t Distance, uint8_t zone, uint8_t RangeStatus) {
#if 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;
  static uint16_t Distances[2][DISTANCES_ARRAY_SIZE];
  static uint8_t DistancesTableSize[2] = {0,0};
#endif
  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] < DISTANCES_ARRAY_SIZE) {
    Distances[zone][DistancesTableSize[zone]] = Distance;
    DistancesTableSize[zone] ++;
  }
  else {
    for (i=1; i<DISTANCES_ARRAY_SIZE; i++)
      Distances[zone][i-1] = Distances[zone][i];
    Distances[zone][DISTANCES_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 < 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 0
  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);
          printf ("Walk In = %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);
          printf ("Walk Out = %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,"Wrong path\n");
          printf ("Walk Around = %d\n", PeopleCount);
          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
  }
  //printf ("Inside PeopleCount = %d , returned\n", PeopleCount);
  // output debug data to main host machine
  return(PeopleCount);
}


void STS_people_count_sensor_Read(sts_people_count_sensor_data_t *sts_p2_sensor_data)
{
	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;
}

void STS_TOF_VL53LX_PeopleCounting_Process_Init(void)
{
	APP_LOG(TS_OFF, VLEVEL_M,"###############   TOF  VL53LX_ PEOPLE COUNTING PROCESS INITIALIZATION\r\n");
	sts_tof_vl53lx_peoplecount();

}

void STS_TOF_VL53LX_PeopleCounting_Process_Start(void)
{
	//APP_LOG(TS_OFF, VLEVEL_M,"###############   TOF  VL53LX_ PEOPLE COUNTING SUB-PROCESS \r\n");
	{
		sts_tof_vl53lx_peoplecount_subprocess();
		//APP_LOG(TS_OFF, VLEVEL_M,"############### SUB-PROCESS running .... \r\n");
	}
}

void sts_tof_vl53lx_peoplecount(void)
{
  //int8_t error;
  uint8_t byteData, sensorState=0;
  uint16_t wordData;
  uint16_t Distance, Signal;
  uint8_t RangeStatus;
  uint8_t dataReady;
  char DisplayStr[5];
  int PplCounter;
  int center[2] = {FRONT_ZONE_CENTER, BACK_ZONE_CENTER}; /* these are the spad center of the 2 4*16 zones */
  int Zone = 0;
  /* 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_L,"XNUCLEO53L1A1_Init Start .......... \r\n");
  status = XNUCLEO53L1A1_Init();
  APP_LOG(TS_OFF, VLEVEL_L,"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_L,"VL53L1X Model_ID: %X\n", byteData);
  status = VL53L1_RdByte(dev, 0x0110, &byteData);
  APP_LOG(TS_OFF, VLEVEL_L,"VL53L1X Module_Type: %X\n", byteData);
  status = VL53L1_RdWord(dev, 0x010F, &wordData);
  APP_LOG(TS_OFF, VLEVEL_L,"VL53L1X: %X\n", wordData);
  while (sensorState == 0) {
    status = VL53L1X_BootState(dev, &sensorState);
    HAL_Delay(2);
  }
  APP_LOG(TS_OFF, VLEVEL_L,"Chip booted\n");
  
  /* Initialize and configure the device according to people counting need */
  status = VL53L1X_SensorInit(dev);
  status += VL53L1X_SetDistanceMode(dev, DISTANCE_MODE); /* 1=short, 2=long */
  status += VL53L1X_SetTimingBudgetInMs(dev, TIMING_BUDGET); /* in ms possible values [15, 20, 50, 100, 200, 500] */
  status += VL53L1X_SetInterMeasurementInMs(dev, TIMING_BUDGET);
  status += VL53L1X_SetROI(dev, ROWS_OF_SPADS, 16); /* minimum ROI 4,4 */
  if (status != 0) {
	  APP_LOG(TS_OFF, VLEVEL_L,"Initialization or configuration of the device\n");
    return (-1);
  }
  APP_LOG(TS_OFF, VLEVEL_L,"Start counting people with profile : %s...\n", PROFILE_STRING);
  status = VL53L1X_StartRanging(dev);   /* This function has to be called to enable the ranging */
#if 0
  while(1) { /* read and display data */
    while (dataReady == 0) {
      status = VL53L1X_CheckForDataReady(dev, &dataReady);
      HAL_Delay(1);
    }
    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 <= MIN_DISTANCE) // wraparound case see the explanation at the constants definition place
        Distance = MAX_DISTANCE + MIN_DISTANCE;
    }
    else // severe error cases
      Distance = MAX_DISTANCE;
    // inject the new ranged distance in the people counting algorithm
    PplCounter = ProcessPeopleCountingData(Distance, Zone, RangeStatus);
    //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;
  }
#endif
  if (status != 0) {
  	APP_LOG(TS_OFF, VLEVEL_L,"Error in start ranging\n");
    return (-1);
  }
}
void sts_tof_vl53lx_peoplecount_subprocess(void)
{
	  uint8_t byteData, sensorState=0;
	  uint16_t wordData;
	  uint16_t Distance, Signal;
	  uint8_t RangeStatus;
	  uint8_t dataReady;
	  char DisplayStr[5];
#if 0
	  int PplCounter;
	  int center[2] = {FRONT_ZONE_CENTER, BACK_ZONE_CENTER}; /* these are the spad center of the 2 4*16 zones */
	  int Zone = 0;
#endif
	 /* read and display data */

    while (dataReady == 0) {
      status = VL53L1X_CheckForDataReady(dev, &dataReady);
      HAL_Delay(1);
    }
    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 <= MIN_DISTANCE) // wraparound case see the explanation at the constants definition place
        Distance = MAX_DISTANCE + MIN_DISTANCE;
    }
    else // severe error cases
      Distance = MAX_DISTANCE;
    // inject the new ranged distance in the people counting algorithm
    PplCounter = ProcessPeopleCountingData(Distance, Zone, RangeStatus);
    //printf("\PplCounter =%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;

}