/* FreeRTOS includes. */
#include "FreeRTOS.h"
#include "task.h"
#include "queue.h"
#include "semphr.h"

#include "project.h"
#include "radar_manager_icd.h"
#include "ethernet.h"

void radar_manager_get_message( unsigned char * recv_buf, int n, unsigned char * msg, int * msg_ind, int * header_found)
{
	headerType *header;
	header = (headerType *)msg;

	for(int i = 0; i < n; i++)
	{
		// Copy Byte Into Message Buffer
		msg[*msg_ind] = recv_buf[i];
		(*msg_ind)++;

		if (*header_found == 0)
		{
			if (*msg_ind == sizeof(headerType))
			{
				// Check for valid header
				if (header->fsync == FSYNC)
				{
					// Mark that we have found a header
					*header_found = 1;
				}
				else
				{
					// Shift bytes to front of buffer until a header is found
					for (int j = 0; j < sizeof(headerType)-1; j++)
					{
						msg[j] = msg[j+1];
					}
					(*msg_ind)--;
				}
			}
		}

		if( (*header_found != 0) && (*msg_ind == header->length) )
		{
			// We now have a complete message, pass it to the message handling function.
//			DEBUG_PRINT("Got Msg 0x%04X\r\n", header->type);
			radar_manager_parse_message(msg);

			// Reset State
			*header_found = 0;
			*msg_ind = 0;
		}
	}
}

void send_data(unsigned char * data, int size)
{
//	uart_SendMessage(data, size);
	eth_sendMessage(data, size);
}

void send_ack(unsigned int flags)
{
    ackType resp;

	// Form the header
	resp.header.fsync = FSYNC;
	resp.header.type = ACK_MSG;
	resp.header.length = sizeof(resp);

	// Set the data
	resp.flags = flags;

	// Send the message
	send_data((u8 *)&resp, sizeof(resp));
}

void radar_manager_parse_message(u8 * msgBuffer)
{
	headerType *header;
	header = (headerType *)msgBuffer;

//	DEBUG_PRINT("Parse Msg 0x%04X\r\n", header->type);

	switch (header->type)
	{
		case AXI_WRITE_REG:
			{
				writeRegType *msg = (writeRegType *)msgBuffer;
//				DEBUG_PRINT("Write Reg 0x%08X, %d\r\n", msg->addr, msg->data);
				Xil_Out32(msg->addr, msg->data);
			}
			break;

		case AXI_WRITE_REG_BURST:
			{
				writeRegBurstType *msg = (writeRegBurstType *)msgBuffer;
//				DEBUG_PRINT("Write Reg 0x%08X, %d\r\n", msg->addr, msg->data);
				for (int i = 0; i < msg->length; i++) {
					Xil_Out32(msg->addr + i * 4, msg->data[i]);
				}
			}
			break;

		case AXI_READ_REG:
		    {
		    	readRegType *msg = (readRegType *)msgBuffer;
				readRespType resp;

				resp.header.fsync = FSYNC;
				resp.header.type = AXI_READ_RESP;
				resp.header.length = sizeof(resp);

				resp.data = Xil_In32(msg->addr);
//				DEBUG_PRINT("Read Reg 0x%08X, %d\r\n", msg->addr, resp.data);

				send_data((u8 *)&resp, sizeof(resp));
		    }
			break;

		case SET_AD9081_DAC_NCO:
		    {
		    	ncoConfigType *msg = (ncoConfigType *)msgBuffer;
		    	xil_printf("Set DAC NCO, Ch %d, Freq %d\r\n", msg->channel, (int)msg->frequency);
				adi_ad9081_dac_duc_nco_set(ad9081_dev_ptr, AD9081_DAC_0 << msg->channel, AD9081_DAC_CH_NONE, msg->frequency);
		    }
			break;

		case SET_AD9081_ADC_NCO:
		    {
		    	ncoConfigType *msg = (ncoConfigType *)msgBuffer;
		    	xil_printf("Set ADC NCO, Ch %d, Freq %d\r\n", msg->channel, (int)msg->frequency);
		    	adi_ad9081_adc_ddc_coarse_nco_set(ad9081_dev_ptr, AD9081_ADC_CDDC_0 << msg->channel, msg->frequency);
		    }
			break;

		case RF_SPI_WRITE:
		    {
		    	rfSpiWriteType *msg = (rfSpiWriteType *)msgBuffer;
		    	rf_spi_write(msg->dev_sel, msg->num_bits, msg->data);
		    }
			break;

		default:
			DEBUG_PRINT("Unknown Type 0x%04X!\r\n", header->type);
			break;
	}

	if ((header->flags & HDR_FLAG_REQ_ACK) > 0)
	{
		// A completion ACK had been requested
//		DEBUG_PRINT("Send Ack 0x%04X\r\n", header->type);
		send_ack(ACK_VALID_PACKET | ACK_VALID_EXECUTION);
	}

}
