//	==========================================================================
//	Utils.c
//	(c) 2020, Aurel Dumitru
//
//	Description:
//  MainLoop operations
//	==========================================================================

// Includes
#include "control.h"
#include <string.h>
#include "main.h"
#include "devstatus.h"
#include "utils.h"
#include "modem.h"
#include "file.h"
#include "camera.h"
#include "motor_test.h"
#include "rtc.h"
#include "flash.h"
#include "rgbled.h"
#include "sensor_shtc3.h"
#include "sensor_nfc.h"
#include "sensors.h"
#include "jpg.h"
#include "proc.h"
#include "detect_lowpower.h"
#include "rgbled.h"
#include "config.h"
#include "calibrations.h"

/* Defines */
#define CONTROL_IDC_IDLE_STATE								0
#define CONTROL_IDC_START_CALIB_FRAME_CAPTURE_STATE			1
#define CONTROL_IDC_WAIT_CALIB_FRAME_CAPTURE_STATE			2
#define CONTROL_IDC_WAIT_FOREGROUND_FRAME_CAPTURE_STATE		3
#define CONTROL_IDC_MOTORS_FORWARD_STATE					4
#define CONTROL_IDC_MOTORS_FORWARD_COAST_STATE				5
#define CONTROL_IDC_MOTOR1_BACKWARD_STATE					6
#define CONTROL_IDC_MOTORS_BACKWARD_STATE					7
#define CONTROL_IDC_MOTORS_BACKWARD_COAST_STATE				8
#define CONTROL_IDC_WAIT_BACKGROUND_FRAME_CAPTURE_STATE		9
#define CONTROL_IDC_IDLE_WAIT_OTHER_PROCESS_STATE			10
#define CONTROL_IDC_PRE_IDLE_STATE							11
#define CONTROL_IDC_MAX_STATE								11	// always the last state


#define CONTROL_SFS_IDLE_STATE								0
#define CONTROL_SFS_WAIT_POWER_ON_MODEM_STATE				1
#define CONTROL_SFS_WAIT_FIRMWARE_UPDATE_CHECK_STATE		2
#define CONTROL_SFS_WAIT_VALIDTIME_STATE					3
#define CONTROL_SFS_SEND_FILE_STATE							4
#define CONTROL_SFS_WAIT_SENDING_FILE_STATE					5
#define CONTROL_SFS_WAIT_GPS_STATE							6
#define CONTROL_SFS_WAIT_STOP_MODEM_ACTIVITY_STATE			7
#define CONTROL_SFS_POWER_OFF_MODEM_STATE					8
#define CONTROL_SFS_MAX_STATE								8	// always the last state

#define CONTROL_SNS_IDLE_STATE								0
#define CONTROL_SNS_START_ACQUISITION_STATE					1
#define CONTROL_SNS_WAIT_RESULTS_STATE						2
#define CONTROL_SNS_WAIT_VALID_TIME_STATE					3
#define CONTROL_SNS_MAX_STATE								3	// always the last state


/* Data */
uint8_t  Control_IdcState;
uint32_t Control_IdcTimeStamp;
char     Control_IdcFileName[] = "2000-01-01_00h00m00s";  //20 chars
char     Control_FileNameJpg[256];
char     Control_FileNameText[256];
char     Control_InsectNameText[128];

uint8_t  Control_SfsState;
uint32_t Control_SfsTimeStamp;
char	 Control_SfsGpsResult[128];

uint8_t  Control_SnsState;
uint32_t Control_SnsTimeStamp;

uint8_t  Control_ErrorFlag;
uint8_t  Control_RequestType;

char*    Control_TextStatus;

uint32_t Control_ImgWidth;
uint32_t Control_ImgHeight;

uint32_t Control_BatteryVoltage;
double   Control_GpsLatitude, Control_GpsLongitude;
int32_t  Control_GpsYear, Control_GpsMonth, Control_GpsDay, Control_GpsTime;
uint8_t  Control_GpsDaysInMonth[12] = {31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31};
uint32_t Control_MonitorTimestamp;

int32_t  Control_TemperatureMeas;
uint32_t Control_TemperatureMeasValidity;

DIR		 Control_PicturesDirectory;
FILINFO  Control_PictureFileName;


/* Private operations */
void Control_CreateTimeDateString(char* String)
{
	RTC_TimeTypeDef sTime;
	RTC_DateTypeDef sDate;
	HAL_RTC_GetTime(&hrtc, &sTime, RTC_FORMAT_BCD);
	HAL_RTC_GetDate(&hrtc, &sDate, RTC_FORMAT_BCD);
	Utils_IntBcdToString(0x2000 + sDate.Year,    String,    4);
	Utils_IntBcdToString(		  sDate.Month,   String+5,  2);
	Utils_IntBcdToString(		  sDate.Date,    String+8,  2);
	Utils_IntBcdToString(		  sTime.Hours,   String+11, 2);
	Utils_IntBcdToString(		  sTime.Minutes, String+14, 2);
	Utils_IntBcdToString(		  sTime.Seconds, String+17, 2);
	String[21] = '\0';
}

void Control_SetTimeOnNitz(void)
{
	DevStatus[DEVSTATUS_TIME_SHIFT_IDX] = NC_INVALID_INTEGER;
}


void Control_UpdateClockOnGps(int32_t TimeShift)
{
	char Text[20];
	int32_t Time = TimeShift - (DevStatus[DEVSTATUS_TIME_SHIFT_IDX] & 0xFFFFFFFE);

	if (TimeShift != NC_INVALID_INTEGER)
	{
		// Check if the time was taken from NITZ or same shift (then return, the time does not requires shift)
		if ((DevStatus[DEVSTATUS_TIME_SHIFT_IDX] == NC_INVALID_INTEGER) || (Time == 0) || (Rtc_GetRtcTimeSetStatus() == 0))
			return;
		Rtc_GetCurrentTimeAndDate(&Control_GpsTime, &Control_GpsYear, &Control_GpsMonth, &Control_GpsDay);
		DevStatus[DEVSTATUS_TIME_SHIFT_IDX] = TimeShift;
	}
	else
		// rough approximation
		DevStatus[DEVSTATUS_TIME_SHIFT_IDX] = Time = (((int32_t)round((Control_GpsLongitude*24)/360))*3600) | 0x01;

	Control_GpsTime += Time;
	if ((Control_GpsYear & 0x03) == 0)		Control_GpsDaysInMonth[1] = 29;

	if (Control_GpsTime < 0)
	{
		Control_GpsTime += 24*3600;
		if (Control_GpsDay != 1)
			Control_GpsDay--;
		else
		{
			if (--Control_GpsMonth == 0)
			{
				Control_GpsMonth = 12;
				Control_GpsDay   = 31;
				Control_GpsYear--;
			}
			else
				Control_GpsDay = Control_GpsDaysInMonth[Control_GpsMonth-1];
		}
	}
	if (Control_GpsTime >= (24*3600))
	{
		Control_GpsTime -= 24*3600;
		if (Control_GpsDay != Control_GpsDaysInMonth[Control_GpsMonth-1])
			Control_GpsDay++;
		else
		{
			Control_GpsDay = 1;
			if (++Control_GpsMonth == 13)
			{
				Control_GpsMonth = 1;
				Control_GpsYear++;
			}
		}
	}

	Text[0]  = '0'+(Control_GpsYear/10);
	Text[1]  = '0'+(Control_GpsYear%10);
	Text[2]  = '/';
	Text[3]  = '0'+(Control_GpsMonth/10);
	Text[4]  = '0'+(Control_GpsMonth%10);
	Text[5]  = '/';
	Text[6]  = '0'+(Control_GpsDay/10);
	Text[7]  = '0'+(Control_GpsDay%10);
	Text[8]  = ',';

	Time 	 =  Control_GpsTime/3600;
	Text[9]  = '0'+(Time/10);
	Text[10] = '0'+(Time%10);
	Text[11] = ':';
	Time 	 =  (Control_GpsTime%3600)/60;
	Text[12] = '0'+(Time/10);
	Text[13] = '0'+(Time%10);
	Text[14] = ':';
	Time 	 =  Control_GpsTime%60;
	Text[15] = '0'+(Time/10);
	Text[16] = '0'+(Time%10);

	(void)Rtc_UpdateTimeDate((uint8_t*)Text);
	Rtc_CheckAndUpdateNextWakeUp();
}



void Control_GpsPositionBuild(void)
{
	char* Src  = Control_SfsGpsResult;
	char* SrcPoint  = Src;
	char* Dest = Control_SfsGpsResult;

	if (strcmp(Control_SfsGpsResult, "N/A") == 0)
		return;

	// Replace all ',' with '\0'
	while (*SrcPoint != '\0')
	{
		if (*SrcPoint == ',')
			*SrcPoint = '\0';
		SrcPoint++;
	}

	// Get latitude
	SrcPoint = Src;
	while (*SrcPoint != '.')
		SrcPoint++;
	// Get the fractional part
	Control_GpsLatitude = (atof(SrcPoint-2))/60;
	*(SrcPoint-2) = '\0';
	Control_GpsLatitude += atof(Src);

	// Get the N/S position
	while (*SrcPoint++ != '\0');
	if (*SrcPoint == 'S')
		Control_GpsLatitude = -Control_GpsLatitude;

	// Get longitude
	Src  = SrcPoint+2;
	SrcPoint = Src;
	while (*SrcPoint != '.')
		SrcPoint++;
	// Get the fractional part
	Control_GpsLongitude = (atof(SrcPoint-2))/60;
	*(SrcPoint-2) = '\0';
	Control_GpsLongitude += atof(Src);

	// Get the E/V position
	while (*SrcPoint++ != '\0');
	if (*SrcPoint == 'V')
		Control_GpsLongitude = -Control_GpsLongitude;

	// Get date
	Src = SrcPoint+2;
	Control_GpsDay   = (Src[0]-'0')*10 + (Src[1]-'0');
	Control_GpsMonth = (Src[2]-'0')*10 + (Src[3]-'0');
	Control_GpsYear  = (Src[4]-'0')*10 + (Src[5]-'0');
	while (*Src++ != '\0'); // jump over date
	// Get time
	Control_GpsTime  = ((Src[0]-'0')*10 + (Src[1]-'0'))*3600 + ((Src[2]-'0')*10 + (Src[3]-'0'))*60 + ((Src[4]-'0')*10 + (Src[5]-'0'));
	while (*Src++ != '\0'); // jump over time

	// Build GPS string: Latitude,Longitude,Altitude
	Dest = Utils_CopyString(Utils_Ftoa5(Control_GpsLatitude), Dest);
	*Dest++ = ',';
	Dest = Utils_CopyString(Utils_Ftoa5(Control_GpsLongitude), Dest);
	*Dest++ = ',';
	Dest = Utils_CopyString(Src, Dest);
	*Dest = '\0';
}


void Control_BuildSaveAndSendFileMsgStatus()
{
	char* Dest = Control_TextStatus;
	char Tmp[128];
	char* Ptr;
	uint32_t Size;

	// Check if SMS frost alarm should be sent (frost and SMS phone number present)
	Ptr = Sensor_Shtc3_GetFrostAlarm();
	if (Config_FreezeAlert && Ptr[0] && Config_PhoneNumberSms[0])
		Modem_SendSms(Ptr, Config_PhoneNumberSms);

	Dest = Utils_CopyString("Device SN: ", Dest);
	Dest = Utils_CopyString(Sensor_Nfc_GetDevSN(), Dest);

	Dest = Utils_CopyString("\nTime: ", Dest);
	Ptr  = Dest;
	Control_CreateTimeDateString(Control_IdcFileName);
	Dest = Utils_CopyString(Control_IdcFileName, Dest) - 1;
	// Format date more readable
	Ptr[4]  = '/';	Ptr[7]  = '/';	Ptr[10] = ' ';	Ptr[13] = ':';	Ptr[16] = ':';
	Dest = Utils_CopyString("\nLocal time: ", Dest);
	if (Rtc_GetRtcTimeSetStatus())
	{
		if (DevStatus[DEVSTATUS_TIME_SHIFT_IDX] == NC_INVALID_INTEGER)
			Dest = Utils_CopyString("NITZ", Dest);
		else
		{
			if (DevStatus[DEVSTATUS_TIME_SHIFT_IDX] & 0x01)	Dest = Utils_CopyString("GPS", Dest);
			else							Dest = Utils_CopyString("GPSC", Dest);
		}
	}
	else
		Dest = Utils_CopyString("INV", Dest);

	Dest = Utils_CopyString("\nFirmware V", Dest);
	Dest = Utils_CopyString(C_FirmwareId, Dest);
	Dest = Utils_CopyString(" (B", Dest);
	*Dest++ = '0' + (char)Flash_GetCurrentBank();
	Dest = Utils_CopyString("-FTP", Dest);
	if (Modem_GetFtpsType())
		*Dest++ = 'S';
	*Dest++ = ')';

	Dest = Utils_CopyString("\nConfiguration: ", Dest);
	Dest = Utils_CopyString(Config_AllStatString, Dest);

	Dest = Utils_CopyString("\nSim ICCID: ", Dest);
	Dest = Utils_CopyString(Modem_GetIccid(), Dest);
	Dest = Utils_CopyString("\nSignal quality: ", Dest);
	Dest = Utils_CopyString(Modem_GetRssiBer(), Dest);
	Control_GpsPositionBuild();
	Dest = Utils_CopyString("\nGPS position: ", Dest);
	Dest = Utils_CopyString(Control_SfsGpsResult, Dest);

	Dest = Utils_CopyString("\n\nBattery Voltage: ", Dest);
	*Dest++ = '0' + (char)(Control_BatteryVoltage/100);
	*Dest++ = '.';
	*Dest++ = '0' + (char)((Control_BatteryVoltage%100)/10);
	*Dest++ = '0' + (char)(Control_BatteryVoltage%10);
	*Dest++ = 'V';

	if (DevStatus_IsDeviceTypeInsectTrap())
	{
		Dest = Utils_CopyString("\nIR quality: ", Dest);
		uint32_t IrAverage = Detect_GetIrAverage();
		if (IrAverage == 0xFF)
			Dest = Utils_CopyString("N/A", Dest);
		else
		{
			IrAverage = (100*IrAverage)/NC_IR_BASE_VALUE;
			IrAverage = (IrAverage<=100) ? IrAverage:100;
			Dest = Utils_CopyString(itoa(IrAverage, Tmp, 10), Dest);
			*Dest++ = '%';
		}

		Dest = Utils_CopyString("\nMotor1 status: ", Dest);
		if (DevStatus[DEVSTATUS_MOTOR1_STATUS_IDX] == DEVSTATUS_MOTOR_OK)				Dest = Utils_CopyString("ok", Dest);
		else if (DevStatus[DEVSTATUS_MOTOR1_STATUS_IDX] == DEVSTATUS_MOTOR_OPENLOAD)	Dest = Utils_CopyString("open load", Dest);
		else if (DevStatus[DEVSTATUS_MOTOR1_STATUS_IDX] == DEVSTATUS_MOTOR_JAMMED)		Dest = Utils_CopyString("jammed", Dest);
		Dest = Utils_CopyString("\nMotor2 status: ", Dest);
		if (DevStatus[DEVSTATUS_MOTOR2_STATUS_IDX] == DEVSTATUS_MOTOR_OK)				Dest = Utils_CopyString("ok", Dest);
		else if (DevStatus[DEVSTATUS_MOTOR2_STATUS_IDX] == DEVSTATUS_MOTOR_OPENLOAD)	Dest = Utils_CopyString("open load", Dest);
		else if (DevStatus[DEVSTATUS_MOTOR2_STATUS_IDX] == DEVSTATUS_MOTOR_JAMMED)		Dest = Utils_CopyString("jammed", Dest);

		Dest = Utils_CopyString("\nCamera status: ", Dest);
		if (DevStatus[DEVSTATUS_CAMERA_STATUS_IDX] == DEVSTATUS_CAMERA_OK)					Dest = Utils_CopyString("ok", Dest);
		else if (DevStatus[DEVSTATUS_CAMERA_STATUS_IDX] == DEVSTATUS_CAMERA_COM_FAIL)		Dest = Utils_CopyString("communication fail", Dest);
		else if (DevStatus[DEVSTATUS_CAMERA_STATUS_IDX] == DEVSTATUS_CAMERA_PICTURE_FAIL)	Dest = Utils_CopyString("picture fail", Dest);

		if (DevStatus[DEVSTATUS_HYPERRAM_STATUS_IDX] == DEVSTATUS_HYPERRAM_FAIL)
			Dest = Utils_CopyString("\nHyperram status: fail", Dest);
	}

	if (DevStatus[DEVSTATUS_OSC32_STATUS_IDX] == DEVSTATUS_OSC32_FAIL)
		Dest = Utils_CopyString("\nOsc32 status: fail", Dest);
	if (DevStatus[DEVSTATUS_TEMPHUMID_STATUS_IDX] == DEVSTATUS_TEMPHUMID_FAIL)
		Dest = Utils_CopyString("\nTempHumAirSensor status: fail", Dest);

	if (DevStatus_IsDeviceTypeInsectTrap())
	{
		Dest = Utils_CopyString("\nMotor counter: ", Dest);
		Dest = Utils_CopyString(itoa(DevStatus[DEVSTATUS_MOTORS_COUNTER_IDX], Tmp, 10), Dest);
		Dest = Utils_CopyString("\nPictures counter: ", Dest);
		Dest = Utils_CopyString(itoa(DevStatus[DEVSTATUS_PICTURES_COUNTER_IDX], Tmp, 10), Dest);
		if (DevStatus[DEVSTATUS_RAIN_DETECTED_STATUS_IDX] == DEVSTATUS_RAIN_DETECTED_YES)
		{
			Dest = Utils_CopyString("\nRain detected: yes", Dest);
			DevStatus[DEVSTATUS_RAIN_DETECTED_STATUS_IDX] = DEVSTATUS_RAIN_DETECTED_NO;
		}
	}

	Dest = Utils_CopyString("\nReset counter: ", Dest);
	Dest = Utils_CopyString(itoa(DevStatus[DEVSTATUS_RESET_COUNTER_IDX], Tmp, 10), Dest);
	if (DevStatus[DEVSTATUS_RESET_COUNTER_IDX])
	{
		Dest = Utils_CopyString("\nReset Rx: ", Dest);
		Utils_IntToHexString(DevStatus[DEVSTATUS_R0_FAULT_STATUS_IDX],  Dest, 8); Dest += 8; *Dest++ = ' ';
		Utils_IntToHexString(DevStatus[DEVSTATUS_R1_FAULT_STATUS_IDX],  Dest, 8); Dest += 8; *Dest++ = ' ';
		Utils_IntToHexString(DevStatus[DEVSTATUS_R2_FAULT_STATUS_IDX],  Dest, 8); Dest += 8; *Dest++ = ' ';
		Utils_IntToHexString(DevStatus[DEVSTATUS_R3_FAULT_STATUS_IDX],  Dest, 8); Dest += 8; *Dest++ = ' ';
		Utils_IntToHexString(DevStatus[DEVSTATUS_R12_FAULT_STATUS_IDX], Dest, 8); Dest += 8; *Dest++ = ' ';
		Utils_IntToHexString(DevStatus[DEVSTATUS_LR_FAULT_STATUS_IDX],  Dest, 8); Dest += 8; *Dest++ = ' ';
		Utils_IntToHexString(DevStatus[DEVSTATUS_PC_FAULT_STATUS_IDX],  Dest, 8); Dest += 8; *Dest++ = ' ';
		Utils_IntToHexString(DevStatus[DEVSTATUS_PSR_FAULT_STATUS_IDX], Dest, 8); Dest += 8;
	}
	if (DevStatus[DEVSTATUS_POWERON_STATUS_IDX] == DEVSTATUS_POWERON_TRUE)
		Dest = Utils_CopyString("\nPower on reset: yes", Dest);
	DevStatus[DEVSTATUS_RESET_COUNTER_IDX]  = 0;
	DevStatus[DEVSTATUS_POWERON_STATUS_IDX] = DEVSTATUS_POWERON_FALSE;

	if (DevStatus_IsDeviceTypeInsectTrap())
	{
		Dest = Utils_CopyString("\n\nDetected insects:\n", Dest);
		Size = File_ReadFile("InsectsToSend.txt", Dest);
		Dest += Size;
		if (Size == 0) Dest = Utils_CopyString("None\n", Dest);
	}
	else
		*Dest++ = '\n';

	Dest = Utils_CopyString("\nSensors values\n", Dest);
	Size = File_ReadFile("Sensors.log", Dest);
	if (Size)		Dest += Size;
	else			Dest -= 16;

	File_WriteFile("status.tmp", (const uint8_t*)Control_TextStatus, Dest-Control_TextStatus);
	strcpy(Tmp, "status/");
	strcat(Tmp, Control_IdcFileName);
	strcat(Tmp, "_status.txt");
	Modem_SendFileFtp("status.tmp", Tmp, 1);
}


/* Public operations */
void Control_StartInsectDetectionAndCapture(void)
{
	if ((DevStatus[DEVSTATUS_HYPERRAM_STATUS_IDX] == DEVSTATUS_HYPERRAM_OK) &&
		(DevStatus[DEVSTATUS_CAMERA_STATUS_IDX] == DEVSTATUS_CAMERA_OK) &&
		DevStatus[DEVSTATUS_BAT_STATUS_IDX] == DEVSTATUS_BAT_OK)
		Control_IdcState = CONTROL_IDC_START_CALIB_FRAME_CAPTURE_STATE;
	else
		Control_IdcState = CONTROL_IDC_IDLE_WAIT_OTHER_PROCESS_STATE;
}

void Control_InsectDetectionAndCaptureServer(void)
{
	uint32_t FileSize;
	uint32_t Time = HAL_GetTick() - Control_IdcTimeStamp;
	uint32_t Contours;

	switch (Control_IdcState)
	{
	case CONTROL_IDC_START_CALIB_FRAME_CAPTURE_STATE:
		HAL_DCMI_Stop(&hdcmi);
		Rgbled_SetColor(0, 0x000000);
		Camera_ExitStandby();
		Camera_ActivateFlash(0);
		Camera_AdaptExposure(0);
		Utils_DelayMicroseconds(11000);
		Control_IdcState = CONTROL_IDC_WAIT_CALIB_FRAME_CAPTURE_STATE;
		Control_IdcTimeStamp = HAL_GetTick();
		Camera_StartDmaGetFrame((uint32_t*)Utils_GetForegroundImgStatPtr());
		break;

	case CONTROL_IDC_WAIT_CALIB_FRAME_CAPTURE_STATE:
		// calibration frame received?
		if (Camera_GetFrameRcvFlag())
		{
			HAL_DCMI_Stop(&hdcmi);
			Utils_DelayMicroseconds(20000);
			Camera_ActivateFlash(1);
			Camera_AdaptExposure(1);
			Utils_DelayMicroseconds(11000);
			Control_IdcState = CONTROL_IDC_WAIT_FOREGROUND_FRAME_CAPTURE_STATE;
			Control_IdcTimeStamp = HAL_GetTick();
			Camera_StartDmaGetFrame((uint32_t*)Utils_GetForegroundImgStatPtr());
		}
		// check timeout (something wrong with camera)
		else
			if (Time > 500)
			{
				HAL_DCMI_Stop(&hdcmi);
				DevStatus[DEVSTATUS_CAMERA_STATUS_IDX] = DEVSTATUS_CAMERA_PICTURE_FAIL;
				Control_IdcState = CONTROL_IDC_IDLE_WAIT_OTHER_PROCESS_STATE;
			}
		break;


	case CONTROL_IDC_WAIT_FOREGROUND_FRAME_CAPTURE_STATE:
		// foreground frame received?
		if (Camera_GetFrameRcvFlag())
		{
			HAL_DCMI_Stop(&hdcmi);
			Control_IdcTimeStamp = HAL_GetTick();
			Control_IdcState = CONTROL_IDC_MOTORS_FORWARD_STATE;

			if (C_DeactivateMotors)
			{
				Control_IdcState = CONTROL_IDC_MOTORS_BACKWARD_COAST_STATE;
				break;
			}

			// Start both motors forward
			HAL_GPIO_WritePin(MOTOR1_PH_GPIO_Port, MOTOR1_PH_Pin,  GPIO_PIN_SET);
			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_GPIO_WritePin(MOTOR1_SLP_GPIO_Port, MOTOR1_SLP_Pin, GPIO_PIN_SET);
			DevStatus[DEVSTATUS_MOTORS_COUNTER_IDX]++;
		}
		// check timeout (something wrong with camera)
		else
			if (Time > 500)
			{
				HAL_DCMI_Stop(&hdcmi);
				DevStatus[DEVSTATUS_CAMERA_STATUS_IDX] = DEVSTATUS_CAMERA_PICTURE_FAIL;
				Control_IdcState = CONTROL_IDC_IDLE_WAIT_OTHER_PROCESS_STATE;
			}
		break;

	case CONTROL_IDC_MOTORS_FORWARD_STATE:
		if (Time > 400)
		{
			// Set motors to coast
			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);
			Control_IdcState = CONTROL_IDC_MOTORS_FORWARD_COAST_STATE;
		}
		break;

	case CONTROL_IDC_MOTORS_FORWARD_COAST_STATE:
		if (Time > 500)
		{
			// Start motor1 backward
			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);
			Control_IdcState = CONTROL_IDC_MOTOR1_BACKWARD_STATE;
		}
		break;

	case CONTROL_IDC_MOTOR1_BACKWARD_STATE:
		if (Time > 600)
		{
			// Start motor2 backward
			HAL_GPIO_WritePin(MOTOR2_PH_GPIO_Port, MOTOR2_PH_Pin,  GPIO_PIN_RESET);
			HAL_GPIO_WritePin(MOTOR2_SLP_GPIO_Port, MOTOR2_SLP_Pin, GPIO_PIN_SET);
			Control_IdcState = CONTROL_IDC_MOTORS_BACKWARD_STATE;
		}
		break;

	case CONTROL_IDC_MOTORS_BACKWARD_STATE:
		if (Time > 850)
		{
			// Set motors to coast
			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);
			// Start temperature measurement
			Sensor_Shtc3_StartMeasurement();
			HAL_Delay(15);
			uint32_t Humidity;
			Control_TemperatureMeasValidity = Sensor_Shtc3_GetTemperatureHumidity(&Control_TemperatureMeas, &Humidity);
			Control_IdcState = CONTROL_IDC_MOTORS_BACKWARD_COAST_STATE;
		}
		break;

	case CONTROL_IDC_MOTORS_BACKWARD_COAST_STATE:
		if (Time > 1100)
		{
			Control_IdcTimeStamp = HAL_GetTick();
			// Start Capture background image
			Camera_StartDmaGetFrame((uint32_t*)Utils_GetBackgroundImgStatPtr());
			File_Init(); // init files after to avoid picture delay
			Control_IdcState = CONTROL_IDC_WAIT_BACKGROUND_FRAME_CAPTURE_STATE;
		}
		break;

	case CONTROL_IDC_WAIT_BACKGROUND_FRAME_CAPTURE_STATE:
		// check if background image is received
		if (Camera_GetFrameRcvFlag())
		{
			HAL_DCMI_Stop(&hdcmi);
			// Deactivate camera
			Camera_EnterStandby();
			// Reactivate RGB LED if necessary
			Rgbled_SetColor(100, Config_TimeEventsTable[Rtc_GetCurrentHour()].RgbLedColor);

			// call object detection algorithm (if temperature is above the threshold ~ bellow the insects should be inactive)
			if ((Control_TemperatureMeasValidity == 0) || (Control_TemperatureMeas > (int32_t)NC_MIN_TEMPERATURE_DETECTION))
			{
				if (Proc_BuildMaskDmzMalvarHeCutler() > 0)
				{
					(void)HAL_IWDG_Refresh(&hiwdg1);
					if (Proc_ParseContours() > 0)
					{
						Proc_RemoveCloseContours();
						if (Proc_RemoveThinContoursBranches())
						{
							Proc_AdjustImageToValidContours();
							(void)HAL_IWDG_Refresh(&hiwdg1);
							Proc_AdjustImageToOrigin();
							Proc_FastVectorMedianFilter();
							Proc_ApplyBrightness(20);

							if (Proc_AnalyzeContours())
							{
								if (C_ActivateContourDraw)
								{
									Proc_DrawContours();
									Proc_AddContoursDataToImage();
								}
								(void)HAL_IWDG_Refresh(&hiwdg1);
								uint8_t* Addr = Jpg_StartConversion(Proc_GetImgSizeX(), Proc_GetImgSizeY(), &FileSize);

								Control_CreateTimeDateString(Control_IdcFileName);
								strcpy(Control_FileNameText, Control_IdcFileName);
								strcpy(Control_FileNameJpg, Control_IdcFileName);
								Contours = Proc_GetNumberOfContours();
								for (uint32_t I=0; I<Contours; I++)
								{
									strcat(Control_FileNameText, "_");
									strcat(Control_FileNameText, Proc_GetContour(I)->ExtraData->Name);
									strcat(Control_FileNameJpg, "_");
									strcat(Control_FileNameJpg, Proc_GetContour(I)->ExtraData->Name);
									strcpy(Control_InsectNameText, Control_IdcFileName);
									strcat(Control_InsectNameText, "_");
									strcat(Control_InsectNameText, Proc_GetContour(I)->ExtraData->Name);
									File_WriteAppendFileNwm("InsectsToSend.txt", Control_InsectNameText);
								}
								strcat(Control_FileNameText, ".txt");
								File_SaveText(Control_FileNameText);
								strcat(Control_FileNameJpg, ".jpg");
								File_SaveJpg(Control_FileNameJpg, Addr, FileSize);
								DevStatus[DEVSTATUS_PICTURES_COUNTER_IDX]++;
							}
						}
					}
				}
			}
			Control_IdcTimeStamp = HAL_GetTick();
			Control_IdcState = CONTROL_IDC_IDLE_WAIT_OTHER_PROCESS_STATE;
		}
		// check timeout (something wrong with camera)
		else
			if (Time > 500)
			{
				HAL_DCMI_Stop(&hdcmi);
				DevStatus[DEVSTATUS_CAMERA_STATUS_IDX] = DEVSTATUS_CAMERA_PICTURE_FAIL;
				Control_IdcState = CONTROL_IDC_IDLE_WAIT_OTHER_PROCESS_STATE;
			}
		break;

	case CONTROL_IDC_IDLE_WAIT_OTHER_PROCESS_STATE:
		// check if files are saved and no firmware update in progress
		(void)Detect_Inclination_Server(DETECT_NORMAL_POWER_SYS_FREQ_FACTOR);
		if ((Control_SfsState == CONTROL_SFS_IDLE_STATE) && (Control_SnsState == CONTROL_SNS_IDLE_STATE))
		{
			Control_IdcTimeStamp = HAL_GetTick();
			Control_IdcState = CONTROL_IDC_PRE_IDLE_STATE;
		}
		break;

	case CONTROL_IDC_PRE_IDLE_STATE:
		if (Time >= NC_TIME_TO_MICROSD_POWER_OFF)
		{
			// if firmware was updated or was a communication error, request a communication restart
			uint32_t Status = Modem_GetFirmwareUpdateStatus();
			if ((Status == MODEM_UPDATE_FIRMWARE_STATUS) ||	(Status == MODEM_UPDATE_CNF_STATUS) || Control_ErrorFlag)
			{
				uint32_t CommCycles = HAL_RTCEx_BKUPRead(&hrtc, RTC_BKP_DR1);
				// request transmission (3x) to announce the firmware/configuration update or error
				__HAL_RTC_WRITEPROTECTION_DISABLE(&hrtc);
				if (CommCycles == 0)	HAL_RTCEx_BKUPWrite(&hrtc, RTC_BKP_DR1, 3);
				else					HAL_RTCEx_BKUPWrite(&hrtc, RTC_BKP_DR1, CommCycles-1);
				__HAL_RTC_WRITEPROTECTION_ENABLE(&hrtc);
			}

			// Write NFC status
			DevStatus_WriteNfcStatus();

			if ((Status == MODEM_UPDATE_FIRMWARE_STATUS) ||	(Status == MODEM_UPDATE_CNF_STATUS))
				Rgbled_SetColor(0, 0x000000);

			if (Status == MODEM_UPDATE_FIRMWARE_STATUS)
			{
				DevStatus[DEVSTATUS_SENSORS_VALID_KEY1_IDX] = 0x00000000;
				DevStatus[DEVSTATUS_SENSORS_VALID_KEY2_IDX] = 0x00000000;
				Flash_ChangeBootBank();
			}

			// One of the followings requires reset
			if ((Status == MODEM_UPDATE_FIRMWARE_STATUS) ||
				(Status == MODEM_UPDATE_CNF_STATUS) ||
				(DevStatus[DEVSTATUS_CAMERA_STATUS_IDX] == DEVSTATUS_CAMERA_COM_FAIL) ||
				(DevStatus[DEVSTATUS_CAMERA_STATUS_IDX] == DEVSTATUS_CAMERA_PICTURE_FAIL) ||
				(DevStatus[DEVSTATUS_HYPERRAM_STATUS_IDX] == DEVSTATUS_HYPERRAM_FAIL) ||
				(DevStatus[DEVSTATUS_OSC32_STATUS_IDX] == DEVSTATUS_OSC32_FAIL))
				HAL_NVIC_SystemReset();

			Control_IdcState = CONTROL_IDC_IDLE_STATE;
		}
		break;
	}
}


void Control_StartSendingFiles(void)
{
	Modem_PowerUpHwModule();
	Control_SfsState = CONTROL_SFS_WAIT_POWER_ON_MODEM_STATE;
}

void Control_SendingFilesServer(void)
{
	int8_t FileSentFlag;

	if (Modem_GetModemErrorStatus())
	{
		Control_ErrorFlag = 1;
		Control_SfsState = CONTROL_SFS_POWER_OFF_MODEM_STATE;
	}

	switch (Control_SfsState)
	{

	case CONTROL_SFS_WAIT_POWER_ON_MODEM_STATE:
		if (Modem_GetPowerServerState() == 1)
		{
			Modem_OpenFtpConnection();
			if (C_DeactivateFirmwareCheck == 0)
				Modem_CheckFirmwareUpdate();
			Control_SfsTimeStamp = HAL_GetTick();
			Control_SfsState = CONTROL_SFS_WAIT_FIRMWARE_UPDATE_CHECK_STATE;
		}
		break;

	case CONTROL_SFS_WAIT_FIRMWARE_UPDATE_CHECK_STATE:
		if (Modem_GetFirmwareUpdateStatus() != MODEM_UPDATE_CHECK_STATUS)
		{
			Control_SfsTimeStamp = HAL_GetTick();
			Control_SfsState 	 = CONTROL_SFS_WAIT_VALIDTIME_STATE;
		}
		break;

	case CONTROL_SFS_WAIT_VALIDTIME_STATE:
		if (Rtc_GetRtcTimeSetStatus() || ((HAL_GetTick() - Control_SfsTimeStamp) > NC_GPS_TIME_WAIT_TIME))
		{
			if (f_opendir(&Control_PicturesDirectory, "/") == FR_OK)
				Control_SfsState = CONTROL_SFS_SEND_FILE_STATE;
			else
			{
				Control_SfsTimeStamp = HAL_GetTick();
				Control_SfsState = CONTROL_SFS_WAIT_GPS_STATE;
			}
		}
		else
		{
			strcpy(Control_SfsGpsResult, Modem_GpsResult());
			if (strcmp(Control_SfsGpsResult, "N/A") != 0)
			{
				Control_GpsPositionBuild();
				Control_UpdateClockOnGps(NC_INVALID_INTEGER);
				Rgbled_SetColor(0, 0x000000);
			}
		}
		break;

	case CONTROL_SFS_SEND_FILE_STATE:
		if ((f_readdir(&Control_PicturesDirectory, &Control_PictureFileName) != FR_OK) || (Control_PictureFileName.fname[0] == 0))
		{
			// All files sent with success
			Control_SfsTimeStamp = HAL_GetTick();
			Control_SfsState = CONTROL_SFS_WAIT_GPS_STATE;
		}
		else
			if (Control_PictureFileName.fname[0] == '2')
			{
				Modem_SendFileFtp(Control_PictureFileName.fname, Control_PictureFileName.fname, 0);
				Control_SfsState = CONTROL_SFS_WAIT_SENDING_FILE_STATE;
			}
		break;

	case CONTROL_SFS_WAIT_SENDING_FILE_STATE:
		FileSentFlag = Modem_GetSentFileStatus();
		if (FileSentFlag > 0)
		{
			// Delete from memory this file
			File_DeleteFile(Control_PictureFileName.fname);
			Control_SfsState = CONTROL_SFS_SEND_FILE_STATE;
		}
		if (FileSentFlag < 0)
		{
			// Error sending file
			Control_SfsTimeStamp = HAL_GetTick();
			Control_SfsState = CONTROL_SFS_WAIT_GPS_STATE;
		}
		break;

	case CONTROL_SFS_WAIT_GPS_STATE:
		strcpy(Control_SfsGpsResult, Modem_GpsResult());
		if (Modem_GetTestStatus() == MODEM_TESTSTATUS_SERVER_LOGIN)
			if (((strcmp(Control_SfsGpsResult, "N/A") != 0) || ((HAL_GetTick() - Control_SfsTimeStamp) > NC_GPS_WAIT_TIME)) &&
				(Control_SnsState == CONTROL_SNS_IDLE_STATE))
			{
				Control_TextStatus = (char*)Utils_AllocMem(192*1024);
				Control_BuildSaveAndSendFileMsgStatus();
				Modem_CloseFtpConnection();
				Control_SfsState = CONTROL_SFS_WAIT_STOP_MODEM_ACTIVITY_STATE;
			}
		break;


	case CONTROL_SFS_WAIT_STOP_MODEM_ACTIVITY_STATE:
		if (Modem_CommandsServerActivityStatus() == 0)
		{
			if (Modem_GetSentFileStatus() == 1)
			{
				DevStatus[DEVSTATUS_MOTORS_COUNTER_IDX]   = 0;
				DevStatus[DEVSTATUS_PICTURES_COUNTER_IDX] = 0;
				File_DeleteFile("InsectsToSend.txt");
				File_DeleteFile("Sensors.log");
				File_DeleteFile("FilesToSend.txt");			// it shall be removed at one point
				File_DeleteFile("thslog.txt");				// it shall be removed at one point
				__HAL_RTC_WRITEPROTECTION_DISABLE(&hrtc);
				HAL_RTCEx_BKUPWrite(&hrtc, RTC_BKP_DR1, 0);
				__HAL_RTC_WRITEPROTECTION_ENABLE(&hrtc);
			}
			Control_SfsState = CONTROL_SFS_POWER_OFF_MODEM_STATE;
		}
		break;

	case CONTROL_SFS_POWER_OFF_MODEM_STATE:
		Modem_PowerDownHwModule();
		Utils_FreeMem((uint8_t*)Control_TextStatus);
		Control_SfsState = CONTROL_SFS_IDLE_STATE;
		break;
	}
}


void Control_StartSensorsAcq()
{
	Control_SnsState = CONTROL_SNS_START_ACQUISITION_STATE;
}

void Control_SensorsServer(void)
{
	switch (Control_SnsState)
	{
	case CONTROL_SNS_START_ACQUISITION_STATE:
		Sensors_StartMeasurement();
		Control_SnsState = CONTROL_SNS_WAIT_RESULTS_STATE;
		break;
	case CONTROL_SNS_WAIT_RESULTS_STATE:
		if (Sensors_MeasurementServer())
			Control_SnsState = CONTROL_SNS_WAIT_VALID_TIME_STATE;
		break;
	case CONTROL_SNS_WAIT_VALID_TIME_STATE:
		if ((Control_SfsState == CONTROL_SFS_IDLE_STATE) || (Control_SfsState > CONTROL_SFS_WAIT_VALIDTIME_STATE))
		{
			Control_CreateTimeDateString(Control_IdcFileName);
			Sensors_GetValues(Control_IdcFileName);
			Control_SnsState = CONTROL_SNS_IDLE_STATE;
		}
		break;

	}
}


void Control_CheckEventsTimeTable(void)
{
	uint32_t Hour = Rtc_GetCurrentHour();
	Rtc_CheckAndUpdateNextWakeUp();
	Rgbled_SetColor(100, Config_TimeEventsTable[Hour].RgbLedColor);
	if (Config_TimeEventsTable[Hour].ModemReq != 0)			Control_StartSendingFiles();
	if (Config_TimeEventsTable[Hour].SensorsReq != 0)		Control_StartSensorsAcq();
	if (Config_TimeEventsTable[Hour].PheromoneFlapReq != 0)	MotorTest_PheromoneMix();
}



void Control_MonitorServer(void)
{
	// Check if automata states out of ranges (memory corruption)
	// Check if one of automata is stuck in one state
	if ((Control_IdcState > CONTROL_IDC_MAX_STATE) ||
		(Control_SfsState > CONTROL_SFS_MAX_STATE) ||
		(Control_SnsState > CONTROL_SNS_MAX_STATE) ||
		((HAL_GetTick() - Control_MonitorTimestamp) > (50*60*1000))) // check if more than 50 minutes in normal mode
	{
		// Reset the device and request status transmission
		uint32_t CommCycles = HAL_RTCEx_BKUPRead(&hrtc, RTC_BKP_DR1);
		// request transmission (3x) to announce the firmware/configuration update or error
		__HAL_RTC_WRITEPROTECTION_DISABLE(&hrtc);
		if (CommCycles == 0)	HAL_RTCEx_BKUPWrite(&hrtc, RTC_BKP_DR1, 3);
		else					HAL_RTCEx_BKUPWrite(&hrtc, RTC_BKP_DR1, CommCycles-1);
		__HAL_RTC_WRITEPROTECTION_ENABLE(&hrtc);
		HAL_Delay(2000);
		HAL_NVIC_SystemReset();
	}
}


void Control_Start(void)
{
	Control_ErrorFlag   = 0;
	Control_IdcState    = CONTROL_IDC_IDLE_WAIT_OTHER_PROCESS_STATE;
	Control_SnsState    = CONTROL_SNS_IDLE_STATE;
	Control_SfsState	= CONTROL_SFS_IDLE_STATE;

	Control_MonitorTimestamp = HAL_GetTick();

	Modem_ResetFirmwareUpdateStatus();

	Control_BatteryVoltage = (Utils_GetAdcBatteryVoltage()*30564)/4574343;
	if (Control_BatteryVoltage < 330)
		DevStatus[DEVSTATUS_BAT_STATUS_IDX] = DEVSTATUS_BAT_LOW;
	if (Control_BatteryVoltage > 350)
		DevStatus[DEVSTATUS_BAT_STATUS_IDX] = DEVSTATUS_BAT_OK;

	if (Rtc_GetRtcTimeSetStatus() == 0)
		Rgbled_SetColor(100, 0xFF0000);

	switch (Control_RequestType)
	{
	case CONTROL_REQ_INSECT_DETECTED:
		Control_StartInsectDetectionAndCapture();
		break;
	case CONTROL_REQ_MODEM_START:
		File_Init();
		if (DevStatus_IsDeviceTypeInsectTrap())		MotorTest_Health();
		if (Rtc_GetRtcTimeSetStatus() == 0)		// request sensors acquisition if power up (or no time set)
			Control_StartSensorsAcq();
		Control_StartSendingFiles();
		break;
	case CONTROL_REQ_EVENT_PROCESS:
		File_Init();
		if (DevStatus_IsDeviceTypeInsectTrap())		MotorTest_Health();
		Control_CheckEventsTimeTable();
		break;
	}
}


void Control_SetRequest(uint8_t Request)
{
	Control_RequestType = Request;
}


uint8_t	Control_IsIdle(void)
{
	uint8_t Idle = 0;
	if ((Control_IdcState == CONTROL_IDC_IDLE_STATE) &&
		(Control_SfsState == CONTROL_SFS_IDLE_STATE) &&
		(Control_SnsState == CONTROL_SNS_IDLE_STATE))
		Idle = 1;
	return Idle;
}


void Control_Server(void)
{
	Control_InsectDetectionAndCaptureServer();
	Control_SendingFilesServer();
	Control_SensorsServer();
	Control_MonitorServer();
}
