#include "stm32f4xx_hal.h"
#include <string.h>
#include "debugTools.h"
#include "globals.h"
#include "serial.h"


#ifdef DEBUG_WIFI_PARSER
// les commandes wifi sont envoyées sur la liaison série usb-cdc, et les réponses wifi sont aussi attendues de usb-cdc
// pour debug avec console sur pc
extern UART_HandleTypeDef huart2;
extern DMA_HandleTypeDef hdma_usart2_rx;
#define lhuart_Wifi huart2
#define lhdma_usart_Wifi_rx hdma_usart2_rx

#else
// les commandes sont envoyées sur usart1 connecté au module esp8266
// fonctionnement nominal
extern UART_HandleTypeDef huart1;
extern DMA_HandleTypeDef hdma_usart1_rx;
#define lhuart_Wifi huart1
#define lhdma_usart_Wifi_rx hdma_usart1_rx

#endif


enum WF_STREAM_PARSER_STATES {WF_SP_PREAMBLE_SEARCH=0, WF_SP_CMD_SEARCH, WF_SP_CMD_ID, WF_SP_RAW_IMAGE, WF_SP_RWC_IMAGE};
enum WF_STREAM_PARSER_STATES wfStreamParserState = WF_SP_CMD_SEARCH;

float timeRAWStreamStart = 0; // Mémorize when a RAW command started in order to manage a time-out. If we don't do that, next commands will be interpreted as part of the image.
// Démarre la gestion USART1 par DMA circulaire

// rx side
// this dma uses a circular buffer
// one should retrieve manually the new characters received by DMA 
#define RXBUFFERSIZE (1024+RAW_SIZE)
uint8_t rx_dma_circular_buffer[RXBUFFERSIZE];
// buffer to receive in one or a few UDP frames the raw image
uint16_t i_next_rx; // index in the circular buffer where the next data received
uint16_t i_next_dmarx; // index in the circular buffer to where the DMA should write
uint16_t dmarx_pcount; // number of bytes that have been received by DMA and remains to be taken

uint8_t raw_buffer[RAW_SIZE+4*4];
uint16_t raw_buffer_count;


// DMA current advance
int getDMA_RxCnt(void)
{
	return (RXBUFFERSIZE-lhdma_usart_Wifi_rx.Instance->NDTR);
}

typedef struct 
{
	int cmd_id;
	int pending;
	float f[1]; // commands could send up to X floats
	uint32_t i[1]; // commands could send up to X 32 bits integers
	void *pData;
} pendingCommands;

// Could accept up to 50 pending commands before commiting suicide
#define MAX_PENDING_COMMANDS 50

pendingCommands pendingCommand[MAX_PENDING_COMMANDS];


/*
Commande init translation, exprimée en unité de tour : 
POS:%+01.4f$$
exemple : "LEDTUBE:POS:+0.2445$$"
SPD:%+01.4f\r\n
exemple : "LEDTUBE:SPD:-0.1112$$"
RAW:binary...\r\n
exemple : "LEDTUBE:RAW:RGBRGBRGBRGB...(60*20x)$$"
MOD:%04X\r\n
exemple : "LEDTUBE:MOD:000A$$"
INT:%04X\r\n
exemple : "LEDTUBE:INT:008A$$"
RWC:binary with CRC..\r\n
exemple : "LEDTUBE:RWC:RGBRGBRGBRGB...(60*20x)(crc32*4x)$$"
*/

// this list of masks must follow the order in enum CMD_LIST in serial.h
uint8_t *commandMask[CMD_LIST_SIZE] = 
{
	"LEDTUBE:POS:???????$$",
	"LEDTUBE:SPD:???????$$",
	"LEDTUBE:MOD:????$$",
	"LEDTUBE:INT:????$$",
	"LEDTUBE:RAW:",
	"LEDTUBE:OFF:$$",
	"LEDTUBE:RWC:"
};

int commandMaskLen[CMD_LIST_SIZE];

#define PARSER_BUFFER_SIZE 32

uint8_t parserWorkingBuffer[PARSER_BUFFER_SIZE];
uint8_t parserWorkingBufferSize;
int pendingCommandsCounter;

void initRxParser(void)
{
	int i;
	
	parserWorkingBufferSize = 0;
	
	for (i=0;i<CMD_LIST_SIZE;i++)
	{
		commandMaskLen[i] = strlen((char*)commandMask[i]);
		if (commandMaskLen[i]>parserWorkingBufferSize)
			parserWorkingBufferSize = commandMaskLen[i];
	}
	
	if (parserWorkingBufferSize>sizeof(parserWorkingBuffer))
		crash(CC_MUST_INCREASE_PARSER_BUFFER_SIZE);
  
	for (i=0;i<parserWorkingBufferSize;i++)
	    parserWorkingBuffer[i] = 0;
	
	pendingCommandsCounter = 0;
}


void commandsManagerReset(void)
{
	int i;
	for (i=0;i<MAX_PENDING_COMMANDS;i++)
	{
		pendingCommand[i].pending = 0;
	}
}


// Startup reception by DMA, circular buffer.
// Le buffer fait 4096 bytes. A 115200 bauds, ça se remplis en 355ms
// à 1Mbits/s, ça se remplis en 41ms

void startRxCmdFlux(void)
{
	commandsManagerReset();
	initRxParser();
	
	i_next_rx = 0;
	i_next_dmarx = 0;
	dmarx_pcount = 0;
	// start dma rx circular buffer
	HAL_UART_Receive_DMA(&lhuart_Wifi, rx_dma_circular_buffer,RXBUFFERSIZE);

	// Each time there is a roll over, it will call our callback XferCpltCallback()
	// used ???
	// Info for knowing where the dma is : 
	// hdma_usart2_rx.Instance->NDTR = nb of bytes remaining 
	// (RXBUFFERSIZE-hdma_usart2_rx.Instance->NDTR) == number of bytes received
	
}

uint8_t checkMatch(uint8_t *mask, uint8_t *buffer, int size)
{
	int i;
	
	for (i=0;i<size;i++)
	{
		if (mask[i]=='?')
			continue;
		if (mask[i]!=buffer[i])
			return 0;
	}
	
	return 1;
}

void addNewCommand(int cmd_id,void *pData)
{
	int i;
	
	if (pendingCommandsCounter<MAX_PENDING_COMMANDS)
	{
		for (i=0;i<MAX_PENDING_COMMANDS;i++)
		{
			if (pendingCommand[i].pending == 0)
			{
				pendingCommand[i].cmd_id = cmd_id;
				switch(cmd_id)
				{
					case CMD_POS:
					case CMD_SPD : pendingCommand[i].f[0] = *((float*)pData);
					break;
					case CMD_MOD :
					case CMD_INT : pendingCommand[i].i[0] = *((uint16_t*)pData); // even pendingCommand can get 32bits, pData contains 16 bits data
					break;
					case CMD_RAW : 
					case CMD_RWC :
													pendingCommand[i].pData  = pData; 
					break;
					case CMD_OFF : break;
				}
				pendingCommand[i].pending = 1;
				pendingCommandsCounter++;
				break;
			}
		}
		if (i==MAX_PENDING_COMMANDS)
			crash(CC_SW_ERROR_CMD_MNGR);
	}
	else
		crash(CC_TO_MANY_COMMANDS);
}

void injectOneByteInParser(uint8_t byte)
{
	int i;
	int cmd_id;
	float fx;
	uint16_t byteHigh, byteLow;	
	uint16_t integer;
	uint16_t offset;
	
	
	// shift left one byte and add to the end
	for (i=0;i<parserWorkingBufferSize-1;i++)
	{
		parserWorkingBuffer[i] = parserWorkingBuffer[i+1];
	}
	// i == parserWorkingBufferSize-1
	parserWorkingBuffer[i] = byte;
	
	for (cmd_id=0;cmd_id<CMD_LIST_SIZE;cmd_id++)
	{
		offset = parserWorkingBufferSize-commandMaskLen[cmd_id];
		// check match. 
		if ((checkMatch(commandMask[cmd_id],&parserWorkingBuffer[offset],commandMaskLen[cmd_id])))
		{
			switch(cmd_id)
			{
				case CMD_POS : // float in fixed format
				case CMD_SPD :  
					parserWorkingBuffer[offset+12+7] = 0; // we can dot it, we just overwrite the '\r'; No lost data
					fx = readFloatInHEXString(&parserWorkingBuffer[offset+12]);
					addNewCommand(cmd_id,(void*)&fx);
					break;
				case CMD_MOD :  // uint16 in hex format
				case CMD_INT :   
					byteHigh = readByteInHEXString(&parserWorkingBuffer[offset+12]);
					byteLow = readByteInHEXString(&parserWorkingBuffer[offset+14]);
					integer = (byteHigh << 16) | byteLow;
					addNewCommand(cmd_id,(void*)&integer);
					break;
				case CMD_RAW : // raw image
					wfStreamParserState=WF_SP_RAW_IMAGE;
					timeRAWStreamStart = getTime();
					raw_buffer_count = 0;
					break;					
				case CMD_RWC : // raw image with CRC
					wfStreamParserState=WF_SP_RWC_IMAGE;
					timeRAWStreamStart = getTime();
					raw_buffer_count = 0;
					break;					
				case CMD_OFF : // switch off
					addNewCommand(cmd_id,NULL);
					break;
			}			
		}
	}
}

// renvoie le nombre de bytes non traités car necessite un rappel du parser
// renvoie 0 si tous les bytes ont pu être absorbés
uint16_t injectInParser(uint8_t *data, uint16_t idx_from, uint16_t idx_to)
{
	int i;
	uint16_t amount_taken_for_raw;
	uint16_t amount_left;
	uint16_t amount_received;
	
	if (idx_from==idx_to)
		return 0;
	
	if ((wfStreamParserState==WF_SP_RAW_IMAGE)||(wfStreamParserState==WF_SP_RWC_IMAGE))
	{
		if (idx_to<idx_from)
		{
			amount_left = injectInParser(data,idx_from,RXBUFFERSIZE);
			
			if (amount_left)
				return (amount_left+idx_to);
			else
				return injectInParser(data,0,idx_to);
		}
		else
		{
				amount_received = idx_to-idx_from;
			
			  if (
					((wfStreamParserState==WF_SP_RAW_IMAGE)&&(amount_received+raw_buffer_count>=RAW_SIZE)) 
					||
					((wfStreamParserState==WF_SP_RWC_IMAGE)&&(amount_received+raw_buffer_count>=RAW_SIZE+16))
				)
				{
					if (wfStreamParserState==WF_SP_RAW_IMAGE)
						amount_taken_for_raw = RAW_SIZE-raw_buffer_count;
					else if (wfStreamParserState==WF_SP_RWC_IMAGE)
						amount_taken_for_raw = RAW_SIZE+16-raw_buffer_count;
					amount_left = amount_received-amount_taken_for_raw;
					memcpy(&raw_buffer[raw_buffer_count],&data[idx_from],amount_taken_for_raw);
					raw_buffer_count+=amount_taken_for_raw;
					if (wfStreamParserState==WF_SP_RAW_IMAGE)
					addNewCommand(CMD_RAW,(void*)raw_buffer);
					else
						addNewCommand(CMD_RWC,(void*)raw_buffer);
						
					wfStreamParserState = WF_SP_CMD_SEARCH;
					if (amount_left!=0)
						return injectInParser(data,idx_from+amount_taken_for_raw, idx_to);
					else
						return 0;
				}
				else
				{
					amount_taken_for_raw = amount_received;
					amount_left = 0;
					memcpy(&raw_buffer[raw_buffer_count],&data[idx_from],amount_taken_for_raw);
					raw_buffer_count+=amount_taken_for_raw;
					return amount_left; // 0
				}
		}		
	}
	else if (wfStreamParserState==WF_SP_CMD_SEARCH)
	{
		if (idx_to<idx_from)
		{
			amount_left = injectInParser(data,idx_from,RXBUFFERSIZE);
			if (amount_left)
				return amount_left+idx_to;
			return injectInParser(data,0,idx_to);
		}
		else
		{
			for (i=idx_from;i<idx_to;i++)
			{
				injectOneByteInParser(data[i]);
				if ((wfStreamParserState==WF_SP_RAW_IMAGE)||(wfStreamParserState==WF_SP_RWC_IMAGE))
				{
						return injectInParser(data,i+1,idx_to); // cas i+1==idx_to traité au début de  injectInParser
				}
			}
			return 0;
		}
	}
	else
		crash(CC_UNMANAGED_WF_SP_STATE);
	return 0;
}
	
uint8_t getNextCommand(int *cmd_id,void *vData)
{
	int i;

	if (pendingCommandsCounter==0)
		return 0;
	
	for (i=0;i<MAX_PENDING_COMMANDS;i++)
	{
		if (pendingCommand[i].pending == 1)
		{
			*cmd_id = pendingCommand[i].cmd_id;
			switch(*cmd_id)
			{
				case CMD_POS : 
				case CMD_SPD : *((float*)vData) = pendingCommand[i].f[0];
					break;
				case CMD_MOD :
				case CMD_INT : *((uint32_t*)vData) = pendingCommand[i].i[0];
					break;
				case CMD_RAW : memcpy(vData,pendingCommand[i].pData,RAW_SIZE);
					break;
				case CMD_RWC : memcpy(vData,pendingCommand[i].pData,RAW_SIZE+16);
					break;
				case CMD_OFF : break;
			}
			pendingCommand[i].pending = 0;
			pendingCommandsCounter--;
			return 1;
		}
	}
	return 0;
}

// gets data from DMA and injects what has been received into the command parser
void pullDMA_Rxdata(void)
{
	int dmaidx;
	uint16_t amount_injected;
	uint16_t amount_left;
	uint16_t amount_consummed;
	
	dmaidx = getDMA_RxCnt(); // for instance dmaidx=1 means one byte received and it is in [0].
	
	if (i_next_dmarx!=dmaidx)
	{
		do{
			amount_left=injectInParser(rx_dma_circular_buffer,i_next_dmarx,dmaidx); // buffer,from,to. If to<from, that mean a rollover in needed
			if (dmaidx<i_next_dmarx)
			{
					amount_injected	= RXBUFFERSIZE - i_next_dmarx + dmaidx;
			}
			else
			{
					amount_injected	= dmaidx - i_next_dmarx;
			}
			amount_consummed = amount_injected - amount_left;
			i_next_dmarx = i_next_dmarx + amount_consummed;
			if (i_next_dmarx>=RXBUFFERSIZE)
				i_next_dmarx-=RXBUFFERSIZE;
			if (i_next_dmarx>=RXBUFFERSIZE)
				crash(CC_SW_ERROR_ROLLOVER);
		}while(amount_left);
	}
}
// retrieve from rx_dma_circular_buffer to rxbuffer
// analyses commands, 
// returns 1 for each new command received
// returns 0 when there is no more commands received
// command format 

uint8_t commandRx(int *cmd_id,void *vData)
{
// in case raw image transfer failed partially, go back to normal command
// management after 5 seconds, else we would eat all commands to complete the raw image
	if (((wfStreamParserState==WF_SP_RAW_IMAGE)||(wfStreamParserState==WF_SP_RWC_IMAGE))&&(getTime()-timeRAWStreamStart>5))
		wfStreamParserState = WF_SP_CMD_SEARCH;
	pullDMA_Rxdata();
	return getNextCommand(cmd_id,vData);
}

