/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * @file           : main.c
  * @brief          : Main program body
  ******************************************************************************
  * @attention
  *
  * <h2><center>&copy; Copyright (c) 2021 STMicroelectronics.
  * All rights reserved.</center></h2>
  *
  * This software component is licensed by ST under BSD 3-Clause license,
  * the "License"; You may not use this file except in compliance with the
  * License. You may obtain a copy of the License at:
  *                        opensource.org/licenses/BSD-3-Clause
  *
  ******************************************************************************
  */
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "main.h"
#include "i2c.h"
#include "rtc.h"
#include "tim.h"
#include "usart.h"
#include "gpio.h"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include <stdio.h>

#include "lora.h"
#include "user_tim.h"
#include "usart_user.h"
#include "user_tim.h"
#include "user_data_send.h"
#include "user_data_process.h"

#include "VL53L1X_API.h"
#include "VL53l1X_calibration.h"
#include "X-NUCLEO-53L1A1.h"
#include "VL53l1X_Config.h"

/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */
uint16_t	dev=0x52;
volatile uint8_t TIME_TO_SEND = DISABLE;
volatile uint8_t JUDGE_LORA_JOINED = DISABLE;
extern volatile uint8_t LORA_STATE;
/* USER CODE END PTD */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */
/* USER CODE END PD */

/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */

/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/

/* USER CODE BEGIN PV */

/* USER CODE END PV */

/* Private function prototypes -----------------------------------------------*/
void SystemClock_Config(void);
/* USER CODE BEGIN PFP */
void clean_peoplecount(PeopleCount *peoplecount);

int ProcessPeopleCountingData(int16_t Distance, uint8_t zone, uint8_t RangeStatus, PeopleCount *peopleinout);
/* USER CODE END PFP */

/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */

/* USER CODE END 0 */

/**
  * @brief  The application entry point.
  * @retval int
  */
int main(void)
{
  /* USER CODE BEGIN 1 */

  /* USER CODE END 1 */

  /* MCU Configuration--------------------------------------------------------*/

  /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
  HAL_Init();

  /* USER CODE BEGIN Init */

  /* USER CODE END Init */

  /* Configure the system clock */
  SystemClock_Config();

  /* USER CODE BEGIN SysInit */

  /* USER CODE END SysInit */

  /* Initialize all configured peripherals */
  MX_GPIO_Init();
  MX_I2C1_Init();
  MX_USART2_UART_Init();
  MX_TIM11_Init();
  MX_USART1_UART_Init();
  MX_RTC_Init();
  MX_TIM10_Init();
  /* USER CODE BEGIN 2 */

//	uint32_t time;
//	while(1)
//	{
//		LL_mDelay(1000);
//		time = LL_RTC_TIME_Get(RTC);
//		printf("time:%02X:%02X:%02X\r\n",(time>>16)&0xff,(time>>8)&0xff,time&0xff);
//	}

//	LL_GPIO_ResetOutputPin(GPIOB, LL_GPIO_PIN_5);
	LL_GPIO_SetOutputPin(GPIOB, LL_GPIO_PIN_5);
	LL_mDelay(100);
	LL_GPIO_ResetOutputPin(GPIOB, LL_GPIO_PIN_5);
	LL_mDelay(100);
	LL_GPIO_SetOutputPin(GPIOB, LL_GPIO_PIN_5);
	
	uint8_t data_to_send[4] = {0};
	PeopleCount peopleinout;
	clean_peoplecount(&peopleinout);
	uint32_t people_idle_count = 0;
	
	InitLoraModule(&lora);
	UsartEnableIT(LORA_USART);
	
//	UsartEnableIT(DEBUG_USART);
	
	const uint8_t cmd_lora_status[13] = "AT+STATUS=?\r\n";
	
	int status = 0;
  uint8_t byteData, sensorState=0;
  uint16_t wordData;
  uint16_t Distance, Signal;
  uint8_t RangeStatus;
  uint8_t dataReady;
  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;
	
	status = XNUCLEO53L1A1_Init();
  printf("XNUCLEO53L1A1_Init Status : %X\n", status); 
	
	status = VL53L1_RdByte(dev, 0x010F, &byteData);
  printf("VL53L1X Model_ID: %X\n", byteData);
	
  status = VL53L1_RdByte(dev, 0x0110, &byteData);
  printf("VL53L1X Module_Type: %X\n", byteData);
	
  status = VL53L1_RdWord(dev, 0x010F, &wordData);
  printf("VL53L1X: %X\n", wordData);
	
	while (sensorState == 0) {
    status = VL53L1X_BootState(dev, &sensorState);
    HAL_Delay(2);
  }
	printf("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) {
    printf("Initialization or configuration of the device\n");
    return (-1);
  }
	
  printf("Start counting people with profile : %s...\n", PROFILE_STRING);
  status = VL53L1X_StartRanging(dev);   /* This function has to be called to enable the ranging */
		
  /* USER CODE END 2 */

  /* Infinite loop */
  /* USER CODE BEGIN WHILE */
  while (1)
  {
		if(LORA_STATE != LORA_JOINED)
		{
			StartTIM(TIM10);
			if(JUDGE_LORA_JOINED == ENABLE)
			{
				StopTIM(TIM10);
				JUDGE_LORA_JOINED = DISABLE;
				UsartxSendDataStr(LORA_USART,cmd_lora_status,sizeof(cmd_lora_status));
			}
		}
		
    if(LORA_RECV_STATE == ENABLE)
    {
      LORA_RECV_STATE = DISABLE;
      if(lora.recvRingBuff.Lenght != 0)
      {
        ReadLoraData(&lora.recvRingBuff);
        data_process((char *)lora.loraData);
        memset(lora.loraData, 0, sizeof(lora.loraData));
      }
      UsartEnableIT(LORA_USART);	
    }
		
		if( TIME_TO_SEND != SEND_DISABLE )
		{
			if( TIME_TO_SEND  == SEND_DATA)
			{
				if(( peopleinout.in != 0) || ( peopleinout.out != 0))
				{
					data_to_send[0] = (peopleinout.in>>8)&0xff;
					data_to_send[1] = peopleinout.in&0xff;
					data_to_send[2] = (peopleinout.out>>8)&0xff;
					data_to_send[3] = peopleinout.out&0xff;
					SendDate_Lora(SEND_DATA, data_to_send);
					clean_peoplecount(&peopleinout);
					memset(data_to_send, 0, sizeof(data_to_send));
					people_idle_count = 0;
				}
				else
				{
					people_idle_count++;
					if(people_idle_count >= 2)
					{
						TIME_TO_SEND  = SEND_HEART_DATA;
						people_idle_count = 0;
					}
				}
			}
			if( TIME_TO_SEND  == SEND_HEART_DATA)
			{
				SendDate_Lora(SEND_HEART_DATA, data_to_send);
			}
			
			TIME_TO_SEND = SEND_DISABLE;
		}
		
    status = VL53L1X_CheckForDataReady(dev, &dataReady);
    if(dataReady != 0)
    {
      dataReady = 0;
      status += VL53L1X_GetRangeStatus(dev, &RangeStatus);
      status += VL53L1X_GetDistance(dev, &Distance);
      status += VL53L1X_GetSignalPerSpad(dev, &Signal);
      status += VL53L1X_ClearInterrupt(dev);
			
      if (status != 0) {
        printf("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) {
        printf("Error in chaning the center of the ROI\n");
        return (-1);
      }
		
      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,&peopleinout);
			
//		if(Zone == 1)
//    printf("%d,%d,%d\n", Zone, Distance, Signal);
      Zone++;
      Zone = Zone%2;
		}
    /* USER CODE END WHILE */

    /* USER CODE BEGIN 3 */
  }
  /* USER CODE END 3 */
}

/**
  * @brief System Clock Configuration
  * @retval None
  */
void SystemClock_Config(void)
{
  LL_FLASH_SetLatency(LL_FLASH_LATENCY_2);
  while(LL_FLASH_GetLatency()!= LL_FLASH_LATENCY_2)
  {
  }
  LL_PWR_SetRegulVoltageScaling(LL_PWR_REGU_VOLTAGE_SCALE2);
  LL_RCC_HSE_Enable();

   /* Wait till HSE is ready */
  while(LL_RCC_HSE_IsReady() != 1)
  {

  }
  LL_PWR_EnableBkUpAccess();
  LL_RCC_LSE_Enable();

   /* Wait till LSE is ready */
  while(LL_RCC_LSE_IsReady() != 1)
  {

  }
  if(LL_RCC_GetRTCClockSource() != LL_RCC_RTC_CLKSOURCE_LSE)
  {
    LL_RCC_ForceBackupDomainReset();
    LL_RCC_ReleaseBackupDomainReset();
    LL_RCC_SetRTCClockSource(LL_RCC_RTC_CLKSOURCE_LSE);
  }
  LL_RCC_EnableRTC();
  LL_RCC_PLL_ConfigDomain_SYS(LL_RCC_PLLSOURCE_HSE, LL_RCC_PLLM_DIV_8, 84, LL_RCC_PLLP_DIV_2);
  LL_RCC_PLL_Enable();

   /* Wait till PLL is ready */
  while(LL_RCC_PLL_IsReady() != 1)
  {

  }
  LL_RCC_SetAHBPrescaler(LL_RCC_SYSCLK_DIV_1);
  LL_RCC_SetAPB1Prescaler(LL_RCC_APB1_DIV_2);
  LL_RCC_SetAPB2Prescaler(LL_RCC_APB2_DIV_1);
  LL_RCC_SetSysClkSource(LL_RCC_SYS_CLKSOURCE_PLL);

   /* Wait till System clock is ready */
  while(LL_RCC_GetSysClkSource() != LL_RCC_SYS_CLKSOURCE_STATUS_PLL)
  {

  }
  LL_SetSystemCoreClock(84000000);

   /* Update the time base */
  if (HAL_InitTick (TICK_INT_PRIORITY) != HAL_OK)
  {
    Error_Handler();
  }
  LL_RCC_SetTIMPrescaler(LL_RCC_TIM_PRESCALER_TWICE);
}

/* USER CODE BEGIN 4 */
int ProcessPeopleCountingData(int16_t Distance, uint8_t zone, uint8_t RangeStatus, PeopleCount *peopleinout) {
  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};
  
  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 (trace_count < TIMES_WITH_NO_EVENT)
//    printf ("%d,%d,%d,%d,%d\n", zone, Distance, MinDistance, RangeStatus, PeopleCount);
#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 ++;
					peopleinout->in++;
          // reset the table filling size in case an entry or exit just found
          DistancesTableSize[0] = 0;
          DistancesTableSize[1] = 0;
          //printf("Walk In, People Count=%d\n", PeopleCount);
					printf("People in %d\r\n", peopleinout->in);
        } else if ((PathTrack[1] == 2)  && (PathTrack[2] == 3) && (PathTrack[3] == 1)) {
          // This an exit
          PeopleCount --;
					peopleinout->out++;
          // reset the table filling size in case an entry or exit just found
          DistancesTableSize[0] = 0;
          DistancesTableSize[1] = 0;
          //printf("Walk Out, People Count=%d\n", PeopleCount);
					printf("People out %d\r\n", peopleinout->out);
        } else {
          // reset the table filling size also in case of unexpected path
          DistancesTableSize[0] = 0;
          DistancesTableSize[1] = 0;
          printf("Wrong path\n");
        }
      }
      
      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;
    }
    
#ifdef TRACE_PPC
    if (AnEventHasOccured) {
      for (int j=0; j<PathTrackFillingSize; j++)
        printf ("%d ", PathTrack[j]);
    }
    printf("\n");
#endif
  }
  
  // output debug data to main host machine
  return(PeopleCount);
}




void clean_peoplecount(PeopleCount *peoplecount)
{
	peoplecount->in = 0;
	peoplecount->out = 0;
	
}
/* USER CODE END 4 */

/**
  * @brief  This function is executed in case of error occurrence.
  * @retval None
  */
void Error_Handler(void)
{
  /* USER CODE BEGIN Error_Handler_Debug */
  /* User can add his own implementation to report the HAL error return state */
  __disable_irq();
  while (1)
  {
  }
  /* USER CODE END Error_Handler_Debug */
}

#ifdef  USE_FULL_ASSERT
/**
  * @brief  Reports the name of the source file and the source line number
  *         where the assert_param error has occurred.
  * @param  file: pointer to the source file name
  * @param  line: assert_param error line source number
  * @retval None
  */
void assert_failed(uint8_t *file, uint32_t line)
{
  /* USER CODE BEGIN 6 */
  /* User can add his own implementation to report the file name and line number,
     ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
  /* USER CODE END 6 */
}
#endif /* USE_FULL_ASSERT */

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