/*
 * arduino_com.c
 *
 *  Created on: 26 okt. 2016
 *      Author: Philip
 */

#include "drivers/arduino_com.h"

#include "utilities.h"
#include "string.h"
#include "stm32f4xx_revo.h"

usart_dma_profile dmaHandler;
dma_usart_return raw_dma_data_t;

enum packet_ids {
	COMPASS_PACKET_ID 	= 0xA1,
	GPS_PACKET_ID 		= 0xB1,
};

typedef struct compass_data_t {
	uint8_t header;
	int16_t x;
	int16_t y;
	int16_t z;
	uint8_t crc;
} compass_data_t;

compass_data_t compass_data = {0};

typedef struct gps_data_t {
	uint8_t header;
	float latitude;
	float longitude;
	uint8_t crc;
} gps_data_t;

gps_data_t gps_data = {0};

typedef struct arduino_data_t {
	uint8_t size;	//Size of the data
	void * dataPtr;	//pointer to the data
} arduino_data_t ;

enum arduino_data_e {
	COMPASS_DATA_ID,
	GPS_DATA_ID,
	ARDUINO_DATA_COUNT,
};

arduino_data_t data_arr[ARDUINO_DATA_COUNT] = {
		[COMPASS_DATA_ID] = {
			.size = sizeof(compass_data),
			.dataPtr = &compass_data,
		},
		[GPS_DATA_ID] = {
			.size = sizeof(gps_data),
			.dataPtr = &gps_data,
		},
};


void arduinoCom_init(USART_TypeDef* usart_inst)
{
	usart_init_dma(usart_inst, &dmaHandler, ARDUINO_BAUD, STOP_BITS_2, PARITY_EVEN, ARDUINO_DMA_SIZE, 0);
}


bool arduino_frame_available()
{
	/* We read data from DMA */
	raw_dma_data_t = usart_get_dma_buffer(&dmaHandler);

	return raw_dma_data_t.new_data;
}

arduino_data_t find_packet_from_header(uint8_t header)
{
	arduino_data_t arduino_data = {
			.dataPtr = NULL,
			.size = 0,
	};

	switch (header)
	{
		case COMPASS_PACKET_ID:
			arduino_data = data_arr[COMPASS_DATA_ID];
			break;
		case GPS_PACKET_ID:
			arduino_data = data_arr[GPS_DATA_ID];
			break;
		default:
			break;
	}
	return arduino_data;
}

void arduino_read()
{
	static uint8_t arduino_arr[ARDUINO_DMA_SIZE];
	static uint8_t message_it = 0;
	static uint32_t missedMsg = 0;
	static uint8_t message_it_secondary_head = 0;
	static bool new_header = false;
	//static uint8_t current_packet_size = 0;
	static uint8_t current_header = 0;
	static uint8_t crc = 0;
	static arduino_data_t msg_header_and_size = {0};

	if (raw_dma_data_t.new_data)
	{

		for (int i = 0; i < ARDUINO_DMA_SIZE; i++)
		{

			uint8_t msg = raw_dma_data_t.buff[i];
			msg_header_and_size = find_packet_from_header(msg);

			// Look for the beginning of a frame
			if ( message_it == 0 )
			{



				if (msg_header_and_size.size != 0)
				{
					arduino_arr[(message_it)] = msg;
					message_it++;
					current_header = msg;
					new_header = false; // Just received one
					crc ^= msg;
				}
			}
			// Look for the end of sbus frame
			else
			{
				if (msg_header_and_size.size != 0  && new_header == false)
				{
					new_header = true;
					message_it_secondary_head = message_it;	//save the value of the position in The buffer array, not the dma array index
				}

				/* Reading the message */
				if ((message_it) < msg_header_and_size.size)
				{
					arduino_arr[(message_it)] = msg;
					crc ^= msg;
					message_it++;
				}

				if ((message_it) == msg_header_and_size.size)
				{
					missedMsg++;

					/* TODO: Replace with check for CRC */
					if (crc == msg)
					{
						message_it = 0;
						missedMsg--;
						arduino_data_t current_header_and_size = find_packet_from_header(current_header);

						uint8_t sizeof_data = current_header_and_size.size;
						uint8_t* tmp_ptr_to;
						tmp_ptr_to = current_header_and_size.dataPtr;

						memcpy(tmp_ptr_to,arduino_arr,sizeof_data);

					}

					/* If CRC does not match */
					else
					{
						int temp_secondaryHeader = message_it_secondary_head;
						message_it = message_it - temp_secondaryHeader;	//update the counter to the empty part of the updated array
						new_header = false;		//set new header to false, this is true if there is another header within the buffer

						//Move all the remaning messages in the buffer to the start of the buffer
						for (int i = temp_secondaryHeader; i < ARDUINO_DMA_SIZE; i++)
						{
							int innerCount = i-temp_secondaryHeader;
							arduino_arr[innerCount] = arduino_arr[i];

							//check if we find another possible header inside the rest of the buffer and save that
							if (((arduino_data_t)(find_packet_from_header(innerCount))).size != 0 && innerCount > 0 && new_header == false )
							{
								new_header = true;
								message_it_secondary_head = innerCount;
							}
						}

					}
				}
			}
		}
	}
}