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

// Includes
#include <stdlib.h>
#include "utils.h"
#include "main.h"
#include "calibrations.h"

struct Utils_MemZone
{
	uint8_t* StartAddr;
	uint8_t* EndAddr;
};

// Data
uint8_t  Utils_Heap[NC_HEAP_SIZE];
struct Utils_MemZone Utils_MemZones[8];
uint8_t Utils_MemZonesNumber;

uint32_t Utils_BatteryVoltage;

char Utils_FtoaResult[64];

__attribute__((section(".psram_data_sec"))) uint16_t Utils_ImgFg[(2*1024*1024)/2];
__attribute__((section(".psram_data_sec"))) uint16_t Utils_ImgBg[(2*1024*1024)/2];
__attribute__((section(".psram_data_sec"))) uint8_t  Utils_ImgRgb[3*1024*1024];
__attribute__((section(".psram_data_sec"))) uint8_t  Utils_SparePsram[1*1024*1024];


/* Public operations */
uint8_t* Utils_GetForegroundImgStatPtr(void)
{
	return (uint8_t*)Utils_ImgFg;
}

uint8_t* Utils_GetForegroundImgPtr(void)
{
	return (uint8_t*)Utils_ImgFg + (C_CameraHorizontalPixels+4)*2*2;
}

uint8_t* Utils_GetBackgroundImgStatPtr(void)
{
	return (uint8_t*)Utils_ImgBg;
}

uint8_t* Utils_GetBackgroundImgPtr(void)
{
	return (uint8_t*)Utils_ImgBg + (C_CameraHorizontalPixels+4)*2*2;
}

uint8_t* Utils_GetDemosaicImgPtr(void)
{
	return (uint8_t*)Utils_ImgRgb;
}

uint8_t* Utils_GetSparePsramPtr(void)
{
	return (uint8_t*)Utils_SparePsram;
}


void Utils_InitMem(void)
{
	Utils_MemZones[0].StartAddr = (uint8_t*)0xFACECAFE;
	Utils_MemZones[0].EndAddr   = Utils_Heap;
	Utils_MemZones[1].StartAddr = Utils_Heap+NC_HEAP_SIZE;
	Utils_MemZones[1].EndAddr   = (uint8_t*)0xFACECAFE;
	Utils_MemZonesNumber = 2;
}


uint8_t* Utils_AllocMem(uint32_t Size)
{
	uint8_t* Addr;
	for (uint32_t I=1; I<Utils_MemZonesNumber; I++)
		if ((Utils_MemZones[I].StartAddr - Utils_MemZones[I-1].EndAddr) >= Size)
		{
			for (uint32_t J=Utils_MemZonesNumber; J>I; J--)
				Utils_MemZones[J] = Utils_MemZones[J-1];
			Addr = Utils_MemZones[I-1].EndAddr;
			Utils_MemZones[I].StartAddr = Addr;
			Utils_MemZones[I].EndAddr   = Addr + Size;
			Utils_MemZonesNumber++;
			return Addr;
		}
	return NULL;
}

void Utils_FreeMem(uint8_t* Addr)
{
	for (uint32_t I=0; I<Utils_MemZonesNumber; I++)
		if (Utils_MemZones[I].StartAddr == Addr)
		{
			for (uint32_t J=I+1; J<Utils_MemZonesNumber; J++)
				Utils_MemZones[J-1] = Utils_MemZones[J];
			Utils_MemZonesNumber--;
			break;
		}
}


void Utils_IntBcdToString(uint32_t IntBcd, char* String, uint8_t Length)
{
	String += (Length-1);
	while (Length--)
	{
		*String-- = (char)((IntBcd & 0x0F) + '0');
		IntBcd >>= 4;
	}
}


void Utils_IntToHexString(uint32_t Number, char* String, uint8_t Length)
{
	uint8_t Digit;

	String += (Length-1);
	while (Length--)
	{
		Digit = Number & 0x0F;
		if (Digit < 10)
			Digit += (uint8_t)'0';
		else
			Digit += (uint8_t)'A' - 10;
		*String-- = (char)Digit;
		Number >>= 4;
	}
}


void Utils_FtoaComplex(double F, uint8_t* Buf, uint8_t Precision, uint8_t Sign, uint8_t EndString)
{
	uint8_t* Ptr = Buf;
	uint8_t* P = Ptr;
	uint8_t* P1;
	uint8_t C;
	uint32_t IntPart;
	uint8_t Exp;

	// sign stuff
	if (Sign)
	{
		if (F < 0)
		{
			F = -F;
			*Ptr++ = '-';
		}
		else
			*Ptr++ = ' ';
	}

	for (Exp=0; ((F < 1.0) && (F != 0.0)); Exp++)
		F *= 10;

	// integer part...
	IntPart = (uint32_t)F;
	F -= (float)IntPart;

	if (!IntPart)
		*Ptr++ = '0';
	else
	{
		// save start pointer
		P = Ptr;
		// convert (reverse order)
		while (IntPart)
		{
			*P++ = '0' + IntPart % 10;
			IntPart /= 10;
		}
		// save end pos
		P1 = P;

		// reverse result
		while (P > Ptr)
		{
			C = *--P;
			*P = *Ptr;
			*Ptr++ = C;
		}
		// restore end pos
		Ptr = P1;
	}

	// decimal part
	// place decimal point
	*Ptr++ = '.';
	// convert
	for (uint32_t I=0; I<Precision; I++)
	{
		F *= 10.0;
		C = (uint8_t)F;
		*Ptr++ = '0' + C;
		F -= C;
	}

	if (Exp != 0)
	{
		*Ptr++ = 'e';
		*Ptr++ = '-';
		Ptr[2] = '0' + (Exp%10);
		Exp = Exp/10;
		Ptr[1] = '0' + (Exp%10);
		Exp = Exp/10;
		Ptr[0] = '0' + Exp;
		Ptr += 3;
	}
	// terminating zero
	if (EndString)
		*Ptr = 0;
}


char* Utils_Ftoa5(double F)
{
	char* Ptr = Utils_FtoaResult;
	uint32_t IntPart;

	if (F<0)	{*Ptr++ = '-'; F=-F;}
	// integer part...
	IntPart = (uint32_t)F;
	(void)itoa(IntPart, Ptr, 10);
	while (*Ptr++ != 0);
	*(Ptr-1) = '.';
	Ptr[5] = '\0';

	//Fractional part
	F -= (double)IntPart;
	IntPart = (uint32_t)(F*100000);
	for (int I=4; I>=0; I--)	{Ptr[I] = (IntPart%10) + '0';	IntPart /= 10;}
	return Utils_FtoaResult;
}


char* Utils_Ftoa1(double F)
{
	char* Ptr = Utils_FtoaResult;
	uint32_t IntPart;

	if (F<0)	{*Ptr++ = '-'; F=-F;}
	// integer part...
	IntPart = (uint32_t)F;
	(void)itoa(IntPart, Ptr, 10);
	while (*Ptr++ != 0);
	*(Ptr-1) = '.';
	Ptr[1] = '\0';

	//Fractional part
	F -= (double)IntPart;
	IntPart = (uint32_t)(F*10);
	Ptr[0] = IntPart + '0';
	return Utils_FtoaResult;
}


char* Utils_Ftoa0(double F)
{
	(void)itoa((int32_t)F, Utils_FtoaResult, 10);
	return Utils_FtoaResult;
}


void Utils_ClearMsb(uint32_t* Val)
{
	*Val ^= 0x80000000 >> __CLZ(*Val);
}

uint8_t Utils_CheckMemPattern(uint8_t* Src, uint8_t* Pattern)
{
	while (*Pattern != 0)
		if (*Src++ != *Pattern++)
			return 0;
	return 1;
}

char* Utils_CopyString(const char* Src, char* Dest)
{
	while (*Src != 0)
		*Dest++ = *Src++;
	return Dest;
}


uint32_t Utils_GetMicroSecondsTime(void)
{
	return __HAL_TIM_GET_COUNTER(&htim5);
}


void Utils_DelayMicroseconds(uint32_t Microseconds)
{
	uint32_t TimeStamp = Utils_GetMicroSecondsTime();
	while ((Utils_GetMicroSecondsTime() - TimeStamp) < Microseconds);
}


uint8_t Utils_CheckSum8(uint8_t* Buffer, uint32_t Size)
{
	uint8_t Cks = 0;
	while (Size--)
		Cks += *Buffer++;
	return Cks;
}


void Utils_ClearMem(uint8_t* Buffer, uint32_t Size)
{
	while (Size--)
		*Buffer++ = 0;
}


uint32_t Utils_GetAdcBatteryVoltage(void)
{
	ADC_ChannelConfTypeDef AdcChConfig;

	AdcChConfig.Channel = ADC_CHANNEL_1;
	AdcChConfig.Rank = ADC_REGULAR_RANK_1;
	AdcChConfig.SamplingTime = ADC_SAMPLETIME_32CYCLES_5;
	AdcChConfig.SingleDiff = ADC_SINGLE_ENDED;
	AdcChConfig.OffsetNumber = ADC_OFFSET_NONE;
	AdcChConfig.Offset = 0;
	AdcChConfig.OffsetRightShift = DISABLE;
	AdcChConfig.OffsetSignedSaturation = DISABLE;
	HAL_ADC_ConfigChannel(&hadc2, &AdcChConfig);
	HAL_ADC_Start(&hadc2);
	while ((ADC2->ISR & ADC_FLAG_EOC) == 0);	// wait end of conversion
	return HAL_ADC_GetValue(&hadc2) & 0xFFFF;
}


void Utils_EnterHyperRamDeepSleep(void)
{
	  uint32_t TmpCcr  = hospi1.Instance->CCR;
	  uint32_t TmpWccr = hospi1.Instance->WCCR;

	  MODIFY_REG(hospi1.Instance->CR, OCTOSPI_CR_ABORT, OCTOSPI_CR_ABORT);
	  while ((hospi1.Instance->SR & OCTOSPI_SR_BUSY) || (hospi1.Instance->CR & OCTOSPI_CR_ABORT));
	  MODIFY_REG(hospi1.Instance->DCR1, OCTOSPI_DCR1_MTYP, OCTOSPI_DCR1_MTYP_2|OCTOSPI_DCR1_MTYP_0);
	  while (hospi1.Instance->SR & OCTOSPI_SR_BUSY);
	  hospi1.Instance->CCR  = TmpCcr;
	  hospi1.Instance->WCCR = TmpWccr;
	  while (hospi1.Instance->SR & OCTOSPI_SR_BUSY);

	  MODIFY_REG(hospi1.Instance->HLCR, OCTOSPI_HLCR_WZL,  OCTOSPI_HLCR_WZL);

	  // configure HyperRam = Deep power down enable, fixed latency, 4 CLK initial latency, 32 bytes burst, 34 ohm drive strength
	  *(__IO uint16_t*)0x90001000 = 0x0FFF;
	  // wait write to be completed before abort
	  Utils_DelayMicroseconds(10);

	  MODIFY_REG(hospi1.Instance->CR, OCTOSPI_CR_ABORT, OCTOSPI_CR_ABORT);
	  while ((hospi1.Instance->SR & OCTOSPI_SR_BUSY) || (hospi1.Instance->CR & OCTOSPI_CR_ABORT));
	  MODIFY_REG(hospi1.Instance->HLCR, OCTOSPI_HLCR_WZL,  0);
	  MODIFY_REG(hospi1.Instance->DCR1, OCTOSPI_DCR1_MTYP, OCTOSPI_DCR1_MTYP_2);
	  hospi1.Instance->CCR  = TmpCcr;
	  hospi1.Instance->WCCR = TmpWccr;
}

void Utils_ExitHyperRamDeepSleep(void)
{
	  uint32_t TmpCcr  = hospi1.Instance->CCR;
	  uint32_t TmpWccr = hospi1.Instance->WCCR;

	  HAL_GPIO_WritePin(OCTOSPIM_RST_GPIO_Port, OCTOSPIM_RST_Pin, GPIO_PIN_RESET);
	  Utils_DelayMicroseconds(100);
	  HAL_GPIO_WritePin(OCTOSPIM_RST_GPIO_Port, OCTOSPIM_RST_Pin, GPIO_PIN_SET);
	  Utils_DelayMicroseconds(150);

	  MODIFY_REG(hospi1.Instance->CR, OCTOSPI_CR_ABORT, OCTOSPI_CR_ABORT);
	  while ((hospi1.Instance->SR & OCTOSPI_SR_BUSY) || (hospi1.Instance->CR & OCTOSPI_CR_ABORT));
	  MODIFY_REG(hospi1.Instance->DCR1, OCTOSPI_DCR1_MTYP, OCTOSPI_DCR1_MTYP_2|OCTOSPI_DCR1_MTYP_0);
	  while (hospi1.Instance->SR & OCTOSPI_SR_BUSY);
	  hospi1.Instance->CCR  = TmpCcr;
	  hospi1.Instance->WCCR = TmpWccr;
	  while (hospi1.Instance->SR & OCTOSPI_SR_BUSY);

	  MODIFY_REG(hospi1.Instance->HLCR, OCTOSPI_HLCR_WZL,  OCTOSPI_HLCR_WZL);

	  // configure HyperRam = Deep power down enable, fixed latency, 4 CLK initial latency, 32 bytes burst, 34 ohm drive strength
	  *(__IO uint16_t*)0x90001000 = 0x8FFF;
	  // wait write to be completed before abort
	  Utils_DelayMicroseconds(10);

	  MODIFY_REG(hospi1.Instance->CR, OCTOSPI_CR_ABORT, OCTOSPI_CR_ABORT);
	  while ((hospi1.Instance->SR & OCTOSPI_SR_BUSY) || (hospi1.Instance->CR & OCTOSPI_CR_ABORT));
	  MODIFY_REG(hospi1.Instance->HLCR, OCTOSPI_HLCR_WZL,  0);
	  MODIFY_REG(hospi1.Instance->DCR1, OCTOSPI_DCR1_MTYP, OCTOSPI_DCR1_MTYP_2);
	  hospi1.Instance->CCR  = TmpCcr;
	  hospi1.Instance->WCCR = TmpWccr;
}

void Utils_SetUartBaudrate(UART_HandleTypeDef* huart, uint32_t BaudRate)
{
	HAL_UART_Abort(huart);
	HAL_UART_DeInit(huart);
	huart->Init.BaudRate = BaudRate;
	(void)HAL_UART_Init(huart);
}



void __attribute__((section(".fast_code_sec"))) Utils_MdmaTransfer(MDMA_Channel_TypeDef* MdmaCh, uint32_t Src, uint32_t Dest, uint32_t Length)
{
	uint32_t Tmp;

	/* Disable the peripheral */
	MdmaCh->CCR &= ~MDMA_CCR_EN;
	/* Configure the MDMA Channel data length and block repeat count (1) */
	MODIFY_REG(MdmaCh->CBNDTR, MDMA_CBNDTR_BNDT|MDMA_CBNDTR_BRC, Length);
	/* Configure MDMA Channel destination address */
	MdmaCh->CDAR = Dest;
	/* Configure MDMA Channel Source address */
	MdmaCh->CSAR = Src;

	Tmp = Src & 0xFF000000;
	if ((Tmp == 0x20000000) || (Tmp == 0x00000000))
		MdmaCh->CTBR |= MDMA_CTBR_SBUS;		/*The AHBSbus is used as source */
	else
		MdmaCh->CTBR &= (~MDMA_CTBR_SBUS);	/*The AXI bus is used as source (read operation) on channel x */

	Tmp = Dest & 0xFF000000;
	if ((Tmp == 0x20000000) || (Tmp == 0x00000000))
		MdmaCh->CTBR |= MDMA_CTBR_DBUS;		/*The AHBSbus is used as source */
	else
		MdmaCh->CTBR &= (~MDMA_CTBR_DBUS);	/*The AXI bus is used as source (read operation) on channel x */

	/* Enable the Peripheral */
	MdmaCh->CCR |=  MDMA_CCR_EN;
	/* Trigger the SW request */
	MdmaCh->CCR |=  MDMA_CCR_SWRQ;
}

void __attribute__((section(".fast_code_sec"))) Utils_MdmaTransferKeepSrc(MDMA_Channel_TypeDef* MdmaCh, uint32_t Dest, uint32_t Length)
{
	/* Disable the peripheral */
	MdmaCh->CCR &= ~MDMA_CCR_EN;
	/* Configure the MDMA Channel data length and block repeat count (1) */
	MODIFY_REG(MdmaCh->CBNDTR, MDMA_CBNDTR_BNDT|MDMA_CBNDTR_BRC, Length);
	/* Configure MDMA Channel destination address */
	MdmaCh->CDAR = Dest;
	/* Enable the Peripheral */
	MdmaCh->CCR |=  MDMA_CCR_EN;
	/* Trigger the SW request */
	MdmaCh->CCR |=  MDMA_CCR_SWRQ;
}

void __attribute__((section(".fast_code_sec"))) Utils_MdmaTransferKeepDest(MDMA_Channel_TypeDef* MdmaCh, uint32_t Src, uint32_t Length)
{
	/* Disable the peripheral */
	MdmaCh->CCR &= ~MDMA_CCR_EN;
	/* Configure the MDMA Channel data length and block repeat count (1) */
	MODIFY_REG(MdmaCh->CBNDTR, MDMA_CBNDTR_BNDT|MDMA_CBNDTR_BRC, Length);
	/* Configure MDMA Channel Source address */
	MdmaCh->CSAR = Src;
	/* Enable the Peripheral */
	MdmaCh->CCR |=  MDMA_CCR_EN;
	/* Trigger the SW request */
	MdmaCh->CCR |=  MDMA_CCR_SWRQ;
}

void __attribute__((section(".fast_code_sec"))) Utils_MdmaTransferKeepSrcDest(MDMA_Channel_TypeDef* MdmaCh, uint32_t Length)
{
	/* Disable the peripheral */
	MdmaCh->CCR &= ~MDMA_CCR_EN;
	/* Configure the MDMA Channel data length and block repeat count (1) */
	MODIFY_REG(MdmaCh->CBNDTR, MDMA_CBNDTR_BNDT|MDMA_CBNDTR_BRC, Length);
	/* Enable the Peripheral */
	MdmaCh->CCR |=  MDMA_CCR_EN;
	/* Trigger the SW request */
	MdmaCh->CCR |=  MDMA_CCR_SWRQ;
}

void __attribute__((section(".fast_code_sec"))) Utils_MdmaClearFlags(MDMA_Channel_TypeDef* MdmaCh)
{
	/* Clear all flags */
	MdmaCh->CIFCR = 0x1F;
}

void __attribute__((section(".fast_code_sec"))) Utils_MdmaPoolTransferComplete(MDMA_Channel_TypeDef* MdmaCh)
{
	while (((MdmaCh->CISR) & MDMA_FLAG_BT) == 0);
}
