// ==========================================================================
// detect_lowpower.c
// (c) 2020, Aurel Dumitru
//
// Description:
// IR detection and low power management
// =========================================================================

#include "detect_lowpower.h"
#include "main.h"
#include "control.h"
#include "rtc.h"
#include "file.h"
#include "rgbled.h"
#include "sensors.h"
#include "utils.h"
#include "devstatus.h"
#include "calibrations.h"


uint32_t Detect_CurrentBaseMaxCounter 	= 0;
int32_t  Detect_CurrentBaseIr 			= 0;
int32_t  Detect_CurrentBaseMax[4] 		= {0, 0, 0, 0};
volatile uint32_t Dummy;
uint32_t Detect_IndexOfIrSample = 0;
uint8_t  Detect_IrSamples[1024];

uint32_t Detect_GetIrAverage(void)
{
	uint32_t NumberOfSamples = 0;
	uint32_t Sum = 0;

	while (NumberOfSamples < 1024)
		if (Detect_IrSamples[NumberOfSamples] != 0xFF)		Sum += Detect_IrSamples[NumberOfSamples++];
		else												break;
	Sum = (NumberOfSamples) ? (Sum / NumberOfSamples) : 0xFF;
	return (uint8_t)Sum;
}

uint32_t Detect_Inclination_Server(double FreqFactor)
{
	uint32_t Flag;
	if ((Flag = (TILT_GPIO_Port->IDR & TILT_Pin)))
	{
		DevStatus[DEVSTATUS_MOTORS_COUNTER_IDX]++;
		if (FreqFactor == DETECT_LOW_POWER_SYS_FREQ_FACTOR)
		{
			HAL_GPIO_WritePin(EN_6V_GPIO_Port, 		EN_6V_Pin,		GPIO_PIN_SET);		// Set EN_6V to active level (HIGH)
			HAL_GPIO_WritePin(MOTOR_EN_GPIO_Port,	MOTOR_EN_Pin,	GPIO_PIN_SET);		// Enable motors PWM
		}
		HAL_Delay((uint32_t)(150.0f/FreqFactor));

		// Set MOTOR2 FORWARD
		HAL_GPIO_WritePin(MOTOR2_PH_GPIO_Port,	MOTOR2_PH_Pin,	GPIO_PIN_SET);
		HAL_GPIO_WritePin(MOTOR2_SLP_GPIO_Port, MOTOR2_SLP_Pin, GPIO_PIN_SET);
		HAL_Delay((uint32_t)(200.0f/FreqFactor));

		// Set MOTOR1 BACKWARD, MOTOR 2 COAST
		HAL_GPIO_WritePin(MOTOR2_SLP_GPIO_Port, MOTOR2_SLP_Pin, GPIO_PIN_RESET);
		HAL_GPIO_WritePin(MOTOR1_PH_GPIO_Port,	MOTOR1_PH_Pin,	GPIO_PIN_RESET);
		HAL_GPIO_WritePin(MOTOR1_SLP_GPIO_Port, MOTOR1_SLP_Pin,	GPIO_PIN_SET);
		HAL_Delay((uint32_t)(200.0f/FreqFactor));

		// Set MOTOR2 BACKWARD
		HAL_GPIO_WritePin(MOTOR2_SLP_GPIO_Port, MOTOR2_SLP_Pin, GPIO_PIN_SET);
		HAL_GPIO_WritePin(MOTOR2_PH_GPIO_Port,	MOTOR2_PH_Pin,	GPIO_PIN_RESET);
		HAL_Delay((uint32_t)(100.0f/FreqFactor));

		// Disable both motors
		HAL_GPIO_WritePin(MOTOR2_SLP_GPIO_Port, MOTOR2_SLP_Pin, GPIO_PIN_RESET);
		HAL_GPIO_WritePin(MOTOR1_SLP_GPIO_Port, MOTOR1_SLP_Pin, GPIO_PIN_RESET);
		if (FreqFactor == DETECT_LOW_POWER_SYS_FREQ_FACTOR)
		{
			HAL_GPIO_WritePin(EN_6V_GPIO_Port, 		EN_6V_Pin,		GPIO_PIN_RESET);		// Set EN_6V to inactive level (LOW)
			HAL_GPIO_WritePin(MOTOR_EN_GPIO_Port,	MOTOR_EN_Pin,	GPIO_PIN_RESET);		// Disable motors PWM
		}
		HAL_Delay((uint32_t)(10.0f/FreqFactor));
	}
	return Flag;
}


void Detect_LowPower_Server(void)
{
	int32_t AdcValueAmbientIrLight, NowBaseIr;

	// Configure wake up timer
	// Wakeup Time Base = 16 /(~32 kHz RC) = ~0.5 ms
	// Wakeup Time = 0.5 ms  * WakeUpCounter
	// Deactivate -> clear INT flags -> set wake-up timer
	HAL_RTCEx_DeactivateWakeUpTimer(&hrtc);
	HAL_RTCEx_WakeUpTimerIRQHandler(&hrtc);
	if (DevStatus_IsDeviceTypeInsectTrap())
		HAL_RTCEx_SetWakeUpTimer_IT(&hrtc, NC_DETECTION_RECURRENCE*2, RTC_WAKEUPCLOCK_RTCCLK_DIV16);
	else
		HAL_RTCEx_SetWakeUpTimer_IT(&hrtc, 5000*2, RTC_WAKEUPCLOCK_RTCCLK_DIV16);

	while (1)
	{
		LL_ADC_EnableInternalRegulator(ADC2);		// Enable LDO
	    while (HAL_IS_BIT_CLR(ADC2->ISR, 1<<12));
	    LL_ADC_Enable(ADC2);						// Enable ADC
		while (HAL_IS_BIT_CLR(ADC2->ISR, ADC_ISR_ADRDY));

		if (DevStatus_IsDeviceTypeInsectTrap())
		{
			// Check inclination sensor
			if (Detect_Inclination_Server(DETECT_LOW_POWER_SYS_FREQ_FACTOR) == 0)
			{
				LL_ADC_REG_StartConversion(ADC2);
				while (HAL_IS_BIT_CLR(ADC2->ISR, ADC_FLAG_EOC));	// wait end of conversion
				AdcValueAmbientIrLight = (int32_t)ADC2->DR;

				// Do detection just if we are in the non saturated region
				if (AdcValueAmbientIrLight <= NC_IR_DETECTION_LIMIT)
				{
					// Activate IR light and wait for rise time
					EN_LED_IR_GPIO_Port->BSRR = EN_LED_IR_Pin;
					// Start conversion IR with infrared light
					LL_ADC_REG_StartConversion(ADC2);
					while (HAL_IS_BIT_CLR(ADC2->ISR, ADC_FLAG_EOC));	// wait end of conversion
					// Deactivate IR light
					EN_LED_IR_GPIO_Port->BSRR = ((uint32_t)EN_LED_IR_Pin) << 16;

					NowBaseIr = __USAT((int32_t)ADC2->DR - AdcValueAmbientIrLight + (((int32_t)C_IrDetectionThresholdOffset[AdcValueAmbientIrLight>>8])<<8), 16);

					//Check if the IR light increased
					if (NowBaseIr > Detect_CurrentBaseIr)
					{
						// Yes, store the new base as reference
						Detect_CurrentBaseMax[Detect_CurrentBaseMaxCounter++] = NowBaseIr;
						if (Detect_CurrentBaseMaxCounter == 4)
						{
							Detect_CurrentBaseMaxCounter = 0;
							Detect_CurrentBaseIr = Detect_CurrentBaseMax[0];
							for (uint32_t I = 1; I<4; I++)
								Detect_CurrentBaseIr = (Detect_CurrentBaseIr > Detect_CurrentBaseMax[I]) ? Detect_CurrentBaseMax[I]:Detect_CurrentBaseIr;
						}
					}
					else
					{
						Detect_CurrentBaseMaxCounter = 0;
						if ((Detect_CurrentBaseIr - NowBaseIr) >= NC_IR_DETECTION_THRESHOLD)
						{
							Detect_CurrentBaseIr = NowBaseIr;
							Control_SetRequest(CONTROL_REQ_INSECT_DETECTED);
							break;						// goes in normal mode to process insect detection
						}
					}

					Detect_IrSamples[Detect_IndexOfIrSample] = (NowBaseIr+0x80)>>8;
					Detect_IndexOfIrSample = (Detect_IndexOfIrSample + 1) & 0x3FF;
				}
			}
		}

		// Force modem communication to obtain the time or it was a request for communication / announcement
		if (((READ_REG(RTC->ICSR) & RTC_ICSR_INITS) == 0) || (READ_REG(TAMP->BKP1R)))
		{
			Control_SetRequest(CONTROL_REQ_MODEM_START);
			break;
		}

		// Next event occurred?
		uint32_t RtcHour = (READ_REG(RTC->TR)>>16) & 0x3F;
		Dummy = READ_REG(RTC->DR);

		RtcHour		-= 6*(RtcHour>>4);
		uint32_t EventHour = READ_REG(TAMP->BKP2R);
		EventHour	-= 6*(EventHour>>4);
		if (RtcHour < EventHour)	RtcHour += 24;
		if ((RtcHour - EventHour) < 20)
		{
			Control_SetRequest(CONTROL_REQ_EVENT_PROCESS);
			break;
		}

		// Disable ADC and LDO regulator to minimize power consumption
		LL_ADC_Disable(ADC2);
	    LL_ADC_ClearFlag_ADRDY(ADC2);
		LL_ADC_DisableInternalRegulator(ADC2);

		(void)HAL_IWDG_Refresh(&hiwdg1);
		HAL_SuspendTick();
		HAL_PWREx_EnterSTOP2Mode(PWR_MAINREGULATOR_ON, PWR_STOPENTRY_WFI);
		HAL_ResumeTick();
	}
	HAL_RTCEx_DeactivateWakeUpTimer(&hrtc);
}


void Detect_LowPower_Enter(void)
{
	GPIO_InitTypeDef GPIO_InitStruct;

	// Configure and disable ADC
	WRITE_REG(ADC2->SMPR1, 0x0005);             // configure sample time of CH0 to 64.5 CLK(if not set is 2.5 CLK)
	WRITE_REG(ADC2->PCSEL, (1<<0));				// configure IN0 in pre-selection register
	WRITE_REG(ADC2->SQR1,  (0<<6));				// configure IN0 in group conversion
    LL_ADC_Disable(ADC2);
    LL_ADC_ClearFlag_ADRDY(ADC2);
    LL_ADC_DisableInternalRegulator(ADC2);

	HAL_GPIO_WritePin(CAMERA_STBY_GPIO_Port,	CAMERA_STBY_Pin,	GPIO_PIN_SET);		// Put camera in STANDBY
	HAL_GPIO_WritePin(MOTOR_EN_GPIO_Port,		MOTOR_EN_Pin,		GPIO_PIN_RESET);	// Disable motors PWM
	HAL_GPIO_WritePin(LED_PWM_GPIO_Port,		LED_PWM_Pin,		GPIO_PIN_RESET);	// Disable camera flash PWM
	if (Rgbled_GetStatus() == 0)
		HAL_GPIO_WritePin(EN_6V_GPIO_Port,		EN_6V_Pin,			GPIO_PIN_RESET);	// Set EN_6V to inactive level (LOW)
	HAL_GPIO_WritePin(EN_3V3_GPIO_Port,			EN_3V3_Pin,			GPIO_PIN_RESET);	// Set EN_3V3 to inactive level (LOW)
	HAL_GPIO_WritePin(EN_3V3AUX_GPIO_Port,		EN_3V3AUX_Pin,		GPIO_PIN_RESET);	// Set EN_3V3AUX to inactive level (LOW)
	HAL_GPIO_WritePin(VCC_NFC_GPIO_Port,		VCC_NFC_Pin,		GPIO_PIN_RESET);	// Set VCC_NFC to inactive level (LOW)
	HAL_GPIO_WritePin(EN_12V_GPIO_Port,			EN_12V_Pin,			GPIO_PIN_RESET);	// Set EN_12V to inactive level (LOW)
	HAL_GPIO_WritePin(EN_12V_SW_GPIO_Port,		EN_12V_SW_Pin,		GPIO_PIN_RESET);	// Set EN_12V_SW to inactive level (LOW)
	if (Sensor_Raing_GetStatus() == SENSOR_RAING_NOTPRESENT)
		HAL_GPIO_WritePin(EN_3V3S_GPIO_Port,	EN_3V3S_Pin, 		GPIO_PIN_RESET);	// Set 3V3S to inactive level (LOW)

	GPIO_InitStruct.Pin = MODEM_RXD_Pin;
	GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
	GPIO_InitStruct.Pull = GPIO_NOPULL;
	GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
	HAL_GPIO_Init(GPIOF, &GPIO_InitStruct);

	GPIO_InitStruct.Pin = RGB_SCK_Pin|RGB_SDI_Pin;
    GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
    GPIO_InitStruct.Pull = GPIO_NOPULL;
    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
    HAL_GPIO_Init(GPIOG, &GPIO_InitStruct);

	HAL_RTCEx_EnableBypassShadow(&hrtc);

	if (DevStatus_IsDeviceTypeInsectTrap())		Utils_EnterHyperRamDeepSleep();		// enter deep sleep

	// Unmount microSD
	File_PrepareSdPowerDown();

	// 3V3AUX	- microSD, USB
	// 3V3		- modem
	// 6V		- motors, RGB LEDs, illumination LEDs
}

void Detect_LowPower_Exit(void)
{
	RCC_OscInitTypeDef			RCC_OscInitStruct = {0};
	RCC_ClkInitTypeDef			RCC_ClkInitStruct = {0};
	uint32_t 					pFLatency = 0;
	RCC_PeriphCLKInitTypeDef	RCC_PeriphCLKInitStruct = {0};
	GPIO_InitTypeDef 			GPIO_InitStruct;

	HAL_RTCEx_DisableBypassShadow(&hrtc);

	HAL_GPIO_WritePin(EN_6V_GPIO_Port,			EN_6V_Pin,			GPIO_PIN_SET);	// Set EN_6V to active level (HIGH)

	/* Supply configuration update enable */
	HAL_PWREx_ConfigSupply(PWR_DIRECT_SMPS_SUPPLY);
	// The voltage scaling allows optimizing the power consumption when the device is
	// clocked below the maximum system frequency, to update the voltage scaling value
	// regarding system frequency refer to product datasheet
	__HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE0);
	while(!__HAL_PWR_GET_FLAG(PWR_FLAG_VOSRDY)) {}

	// Re-enable oscillators
	HAL_RCC_GetOscConfig(&RCC_OscInitStruct);
	RCC_OscInitStruct.OscillatorType 	= RCC_OSCILLATORTYPE_HSI | RCC_OSCILLATORTYPE_LSI | RCC_OSCILLATORTYPE_LSE;
	RCC_OscInitStruct.PLL.PLLState		= RCC_PLL_ON;
	if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
		Error_Handler();

	// Initializes the CPU, AHB and APB buses clocks
	// Get the Clocks configuration according to the internal RCC registers
	HAL_RCC_GetClockConfig(&RCC_ClkInitStruct, &pFLatency);
	RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK    | RCC_CLOCKTYPE_SYSCLK |
								  RCC_CLOCKTYPE_PCLK1	| RCC_CLOCKTYPE_PCLK2 |
								  RCC_CLOCKTYPE_D3PCLK1 | RCC_CLOCKTYPE_D1PCLK1;
    RCC_ClkInitStruct.SYSCLKSource  = RCC_SYSCLKSOURCE_PLLCLK;
	if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, pFLatency) != HAL_OK)
		Error_Handler();

	// Initialize the peripheral clock configuration
	HAL_RCCEx_GetPeriphCLKConfig(&RCC_PeriphCLKInitStruct);
	RCC_PeriphCLKInitStruct.PeriphClockSelection = RCC_PERIPHCLK_OSPI | RCC_PERIPHCLK_SDMMC | RCC_PERIPHCLK_UART7 | RCC_PERIPHCLK_CKPER;
	if (HAL_RCCEx_PeriphCLKConfig(&RCC_PeriphCLKInitStruct) != HAL_OK)
		Error_Handler();

	HAL_GPIO_WritePin(EN_3V3AUX_GPIO_Port,	EN_3V3AUX_Pin,	GPIO_PIN_SET);	// Set EN_3V3AUX to active level (HIGH)
	HAL_GPIO_WritePin(MOTOR_EN_GPIO_Port,	MOTOR_EN_Pin,	GPIO_PIN_SET);	// Enable motors PWM
	HAL_GPIO_WritePin(LED_PWM_GPIO_Port,	LED_PWM_Pin,	GPIO_PIN_SET);	// Enable camera flash PWM

    GPIO_InitStruct.Pin = RGB_SCK_Pin|RGB_SDI_Pin;
    GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
    GPIO_InitStruct.Pull = GPIO_NOPULL;
    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_MEDIUM;
    GPIO_InitStruct.Alternate = GPIO_AF5_SPI6;
    HAL_GPIO_Init(GPIOG, &GPIO_InitStruct);

    if (DevStatus_IsDeviceTypeInsectTrap())		Utils_ExitHyperRamDeepSleep();		// exit deep sleep (CS LOW and HIGH); 150us no activity
}


void Detect_Init(void)
{
	for (uint32_t I=0; I<1024; I++)
		Detect_IrSamples[I] = 0xFF;
}
