// ==========================================================================
// usb.c
// (c) 2020, Aurel Dumitru
//
// Description:
// USB functionality
// =========================================================================


#include "usb.h"
#include "calibrations.h"

#include "usbd_cdc_if.h"
#include "camera.h"
#include "sensor_shtc3.h"
#include "sensor_nfc.h"
#include "custom_nfc04a1_nfctag.h"
#include "rgbled.h"
#include "file.h"
#include "utils.h"
#include "modem.h"
#include "motor_test.h"
#include "flap.h"
#include "sensor_rs485.h"


volatile uint8_t Usb_HostPresent = 0;
volatile uint8_t Usb_BufferCmd[8] = {0};
uint8_t Usb_BufferStat[16];
const uint16_t Usb_HyperRamTestPattern[] = {0xA5A5, 0x5A5A, 0xA5A5, 0x5A5A, 0xA5A5, 0x5A5A, 0xA5A5, 0x5A5A, 0xA5A5, 0x5A5A, 0xA5A5};
__IO uint16_t* Usb_HyperRamWriteAddr;
__IO uint16_t* Usb_HyperRamReadAddr;
__IO uint16_t* Usb_HyperRamEndAddr;


void Usb_SetupStage(void)
{
	Usb_HostPresent = 1;
}


void Usb_GetCmd(uint8_t* Buf, uint32_t Len)
{
	for (uint32_t I=0; I<Len; I++)
		Usb_BufferCmd[I] = Buf[I];
}



void Usb_InitialCheckTest(void)
{
	uint32_t I, SendSize;
	uint32_t SizeBuf = 0;
	uint8_t* Ptr = NULL;
	int32_t Temperature;
	uint32_t Humidity;
	uint32_t SensorsLastTime = 0;
	uint32_t MotorsLastTime = 0;
	uint32_t UsbLastTime;
	uint32_t RgbColor[] = {0x0E0000, 0x000800, 0x000014, 0x080808};
	uint8_t SettingsDone = 0;
	uint8_t RgbColorIndex=0;
	uint32_t LastTimeRx = 0;

	// Start RGB LED to show that device is started
	Rgbled_SetColor(100, 0xFFFFFF);

	// Wait for USB connection for 3 seconds; if not, return
	UsbLastTime = HAL_GetTick();
	while (1)
	{
		(void)HAL_IWDG_Refresh(&hiwdg1);
		if ((HAL_GetTick() - UsbLastTime) > 3000)
		{
			Rgbled_SetColor(0, 0x000000);
			return;
		}
		if (Usb_HostPresent)
			break;
	}

	Usb_HyperRamWriteAddr = (__IO uint16_t*)Utils_GetSparePsramPtr();
	Usb_HyperRamReadAddr  = (__IO uint16_t*)Utils_GetSparePsramPtr();
	Usb_HyperRamEndAddr   = (__IO uint16_t*)Utils_GetSparePsramPtr() + 128*1024;
	for (I=0; I<16; I++)
		Usb_BufferStat[I] = 0;
	// Enable motors PWM
	HAL_GPIO_WritePin(MOTOR_EN_GPIO_Port, MOTOR_EN_Pin, GPIO_PIN_SET);
	Modem_PowerUpHwModule();
	Sensor_RS485_Init();

	while ((Usb_BufferCmd[0] != 0xA5) && (Usb_BufferCmd[0] != 0xF5)) (void)HAL_IWDG_Refresh(&hiwdg1);
	if (Usb_BufferCmd[0] == 0xF5)
	{
		while (1)
		{
			(void)HAL_IWDG_Refresh(&hiwdg1);
			if ((Usb_BufferCmd[0] == 0xF5) && (Usb_BufferCmd[1] != 0x00))
			{
				Usb_BufferStat[0] = Sensor_RS485_SetSensorAddress(Usb_BufferCmd[1]);
				Usb_BufferCmd[0] = 0x00; Usb_BufferCmd[1] = 0x00;
				CDC_Transmit_HS(Usb_BufferStat, 16);
			}
		}
	}


	// USB test loop
	while (1)
	{
		(void)HAL_IWDG_Refresh(&hiwdg1);
		Modem_Server();
		MotorTest_Server();

		for (I=0; I<11; I++)
		{
			*Usb_HyperRamWriteAddr++ = Usb_HyperRamTestPattern[I];
			if (Usb_HyperRamWriteAddr >= Usb_HyperRamEndAddr)
				Usb_HyperRamWriteAddr = (__IO uint16_t*)Utils_GetSparePsramPtr();
		}
		for (I=0; I<11; I++)
		{
			if ((*Usb_HyperRamReadAddr++) != Usb_HyperRamTestPattern[I])
				Usb_BufferStat[0] = 1;
			if (Usb_HyperRamReadAddr >= Usb_HyperRamEndAddr)
				Usb_HyperRamReadAddr = (__IO uint16_t*)Utils_GetSparePsramPtr();
		}

		// Sensors test
		if (HAL_GetTick() - SensorsLastTime > 2000)
		{
			SensorsLastTime = HAL_GetTick();
			Rgbled_SetColor(10, RgbColor[RgbColorIndex]);
			(void)Sensor_Shtc3_GetTemperatureHumidity(&Temperature, &Humidity);
			Sensor_Shtc3_StartMeasurement();

			Usb_BufferStat[1] = (uint8_t)Temperature;
			Usb_BufferStat[2] = (uint8_t)(Temperature>>8);
			Usb_BufferStat[3] = (uint8_t)Humidity;
			Usb_BufferStat[4] = (uint8_t)(Humidity>>8);
			Usb_BufferStat[5] = RgbColorIndex;
			Usb_BufferStat[7] = Modem_GetTestStatus();
			Usb_BufferStat[11]= Sensor_Nfc_ReadFieldDetectedFlag();
			Usb_BufferStat[12]= Modem_GetRssiInt();

			RgbColorIndex = (RgbColorIndex+1) & 0x03;
		}

		// Motors test
		if (HAL_GetTick() - MotorsLastTime > 4000)
		{
			MotorsLastTime = HAL_GetTick();
			if (Usb_BufferCmd[5] != 0)
				MotorTest_Start();
		}

		// USB communication + camera + commands
		if (Usb_BufferCmd[0] == 0xA5)
		{
			Usb_BufferCmd[0] = 0;
			LastTimeRx = HAL_GetTick();
			if (SettingsDone == 0)
			{
				SettingsDone = 1;
				if (Usb_BufferCmd[1] == 0)
					HAL_GPIO_WritePin(LED_PWM_GPIO_Port, LED_PWM_Pin, GPIO_PIN_RESET); // Disable FLASH PWM
				else
					HAL_GPIO_WritePin(LED_PWM_GPIO_Port, LED_PWM_Pin, GPIO_PIN_SET); // Enable FLASH PWM

				if (Usb_BufferCmd[2] == 0)
				{
					RgbColor[0] = 0;
					RgbColor[1] = 0;
					RgbColor[2] = 0;
					RgbColor[3] = 0;
				}
				C_CameraVerticalPixels = 960;
				C_CameraHorizontalPixels = 960;
				Camera_Init();
				HAL_Delay(50);
				Camera_AdaptExposure(0);
				Camera_StartDmaGetFrame((uint32_t*)Utils_GetForegroundImgStatPtr());
				while (Camera_GetFrameRcvFlag() == 0);
				HAL_DCMI_Stop(&hdcmi);
				HAL_Delay(50);
				Camera_AdaptExposure(1);
				Camera_ActivateFlash(1);
				Camera_DisableEmbeddedStatistics();
				C_CameraHorizontalPixels = (((uint16_t)Usb_BufferCmd[3])<<8) + Usb_BufferCmd[4];
				C_CameraVerticalPixels   = C_CameraHorizontalPixels;
				Camera_SetWindowCamera();
			}

			if (SizeBuf == 0)
			{
				Camera_StartDmaGetFrameNoStatistics((uint32_t*)Utils_GetForegroundImgPtr());
				uint32_t BatV = Utils_GetAdcBatteryVoltage();
				Usb_BufferStat[6] = (uint8_t)(BatV>>8);
				Flap_SetIrState(1);
				uint16_t IrCur = Flap_GetIrCurrent();
				Usb_BufferStat[8] = (uint8_t)(IrCur>>8);
				Usb_BufferStat[9] = (uint8_t)IrCur;
				Flap_SetIrState(0);
				Usb_BufferStat[10] = Flap_GetTiltSensorValue();

				CDC_Transmit_HS(Usb_BufferStat, 16);

				while (Camera_GetFrameRcvFlag() == 0);
				HAL_DCMI_Stop(&hdcmi);

				Ptr = Utils_GetForegroundImgPtr();
				SizeBuf = ((C_CameraHorizontalPixels+4)*(C_CameraVerticalPixels+4)*2 + 1024) & 0xFFFFF400 ;
			}

			if (SizeBuf > 60*1024)
				SendSize = 60*1024;
			else
				SendSize = SizeBuf;
			SizeBuf -= SendSize;
			CDC_Transmit_HS(Ptr, (uint16_t)SendSize);
			Ptr += SendSize;
		}
		else
		{
			if ((HAL_GetTick() - LastTimeRx) > 4000)
				SettingsDone = 0;
		}
	  }
}
