//#include <proc/p32mx440f256h.h>

#include "BootloaderInterface.h"
#include "BootloaderProtocol.h"
#include "ProtocolDefs.h"
#include "BoardCfg.h"
#include "timer.h"
#include "WiFiCtrl.h"

char BootloaderBuffer[200];
int BootloaderInterfaceState;

int BootloaderFlashErased;
int ReadyToReceiveData;
int DataChunkWritten;
int CurDataChunkIndex;
int FirmwareUploaded;

int BootloaderInterfaceInit()
{
	BootloaderProtocolInit();
	BootloaderResetStateMachine();
	return 1;
}

void BootloaderInterfaceTick()
{
	BootloaderIterfaceStateMachine(BOOTLOADER_TICK_EVENT,0);
}

void BootloaderExecuteCmd(char Cmd)
{
	unsigned char *DataBufPtr = BootloaderProtocolGetDataBufferPtr();
	switch(Cmd)
	{
		case BOOTLOADER_HEARTBEAT_REQUEST:
		{ 
			printf("Bootloader Heartbeat Request\n");
			*DataBufPtr = 1;
			BootloaderProtocolSendFrame(BOOTLOADER_HEARTBEAT_RESPONSE,1);
			break;
		}
		case BOOTLOADER_ERASE_BOOTLOADER_FLASH_REQUEST:
		{
			printf("BOOTLOADER_ERASE_BOOTLOADER_FLASH_REQUEST\n");
			BootloaderIterfaceStateMachine(BOOTLOADER_NEW_CMD_EVENT,BOOTLOADER_SM_ERASE_FLASH_CMD);
			break;
		}
		case BOOTLOADER_INIT_UPLOAD_REQUEST:
		{
			printf("BOOTLOADER_INIT_UPLOAD_REQUEST\n");
			BootloaderIterfaceStateMachine(BOOTLOADER_NEW_CMD_EVENT,BOOTLOADER_SM_INIT_UPLOAD_CMD);
			break;
		}
		case BOOTLOADER_GET_STATE_REQUEST:
		{
			printf("BOOTLOADER_GET_STATE_REQUEST\n");
			*DataBufPtr = (char)BootloaderInterfaceState;
			BootloaderProtocolSendFrame(BOOTLOADER_GET_STATE_RESPONSE,1);
			break;
		}
		case BOOTLOADER_SEND_DATA_CHUNK_REQUEST:
		{
			printf("BOOTLOADER_SEND_DATA_CHUNK_REQUEST\n");
			BootloaderIterfaceStateMachine(BOOTLOADER_NEW_CMD_EVENT,BOOTLOADER_SM_NEW_DATA_CHUNK_CMD);
			break;
		}
		case BOOTLOADER_UPLOAD_FINISHED_REQUEST:
		{
			printf("BOOTLOADER_UPLOAD_FINISHED_REQUEST\n");
			BootloaderIterfaceStateMachine(BOOTLOADER_NEW_CMD_EVENT,BOOTLOADER_SM_UPLOAD_FINISHED_CMD);
			break;
		}
		case BOOTLOADER_EXECUTE_UPGRAGE_REQUEST:
		{
			printf("BOOTLOADER_EXECUTE_UPGRAGE_REQUEST\n");
			BootloaderIterfaceStateMachine(BOOTLOADER_NEW_CMD_EVENT,BOOTLOADER_SM_EXECUTE_UPGRAGE_CMD);
			break;
		}
		case BOOTLOADER_ABORT_OPERATION_REQUEST:
		{
			printf("BOOTLOADER_ABORT_OPERATION_REQUEST\n");
			BootloaderIterfaceStateMachine(BOOTLOADER_NEW_CMD_EVENT,BOOTLOADER_SM_ABORT_CMD);
		}
		default:
		{
			break;
		}
	}
}

void BootloaderCRCError(char Cmd, int RxCRC, int ExpectedCRC)
{
	printf("BootloaderProtocol detected a CRC error.  Cmd: %d, RxCRC:0x%x, Expected:[0x%x]\n",Cmd,RxCRC,ExpectedCRC);
	BootloaderIterfaceStateMachine(BOOTLOADER_NEW_CMD_EVENT,BOOTLOADER_SM_INVALID_CRC_CMD);
}

void BootloaderIterfaceStateMachine(int Event, int Param)
{
	
	switch(BootloaderInterfaceState)
	{
		case BOOTLOADER_STANDBY_STATE:
		{
			switch(Event)
			{
				case BOOTLOADER_TICK_EVENT:
				{
					break;
				}
				case BOOTLOADER_NEW_CMD_EVENT:
				{
					if(Param == BOOTLOADER_SM_ACTIVATE_CMD)
					{
						BootloaderInterfaceState = BOOTLOADER_ACTIVE_STATE;
						printf("Bootloader Interface going into active state\n");
						
					}
					break;
				}
				case BOOTLOADER_TIMEOUT_EVENT:
				{
					break;
				}
			}

			break;
		}
		case BOOTLOADER_ACTIVE_STATE:
		{
			switch(Event)
			{
				case BOOTLOADER_TICK_EVENT:
				{
					break;
				}
				case BOOTLOADER_NEW_CMD_EVENT:
				{
					switch(Param)
					{
						case BOOTLOADER_SM_ERASE_FLASH_CMD:
						{
							//TODO: Start flash erase process
							BootloaderProtocolSendACK(BOOTLOADER_ERASE_BOOTLOADER_FLASH_REQUEST);
							BootloaderInterfaceState = BOOTLOADER_ERASE_FLASH_STATE;
							printf("Bootloader Interface going into Erase Flash state\n");
							break;
						}
						case BOOTLOADER_SM_INIT_UPLOAD_CMD:
						{
							BootloaderProtocolSendInitUploadResponse();
							BootloaderInterfaceState = BOOTLOADER_RECEIVING_FIRMWARE_STATE;
							printf("Bootloader Interface going into Firmware RX state\n");
							ReadyToReceiveData = 1;		//TODO: Manage this flag in flash SM
							break;
						}
						case BOOTLOADER_SM_ABORT_CMD:
						{
							//TODO invalidate data in Flash
							printf("Aborting upload, going into STANDBY mode\n");
							BootloaderResetStateMachine();
							break;
						}
					}
					default:
					{
						//SEND NACK
					}
					break;
				}
				case BOOTLOADER_TIMEOUT_EVENT:
				{
					break;
				}
			}
			
			break;
		}
		case BOOTLOADER_ERASE_FLASH_STATE:
		{
			switch(Event)
			{
				case BOOTLOADER_TICK_EVENT:
				{
					//TODO: Check flash erase operation.
					BootloaderFlashErased = 1;
					printf("Flash erase finished.  Bootloader Interface going into Active state\n");
					BootloaderProtocolSendACK(BOOTLOADER_ERASE_BOOTLOADER_FLASH_RESULT_RESPONSE);		//TODO: send result instead
					BootloaderInterfaceState = BOOTLOADER_ACTIVE_STATE;
					break;
				}
				case BOOTLOADER_NEW_CMD_EVENT:
				{
					switch(Param)
					{
						case BOOTLOADER_SM_ABORT_CMD:
						{
							//TODO: stop erasing and reset SM.
							//TODO invalidate data in Flash
							printf("Aborting upload, going into STANDBY mode\n");
							BootloaderResetStateMachine();
							break;
						}
					}
					break;
				}
				case BOOTLOADER_TIMEOUT_EVENT:
				{
					break;
				}
			}
			
			break;
		}
		case BOOTLOADER_RECEIVING_FIRMWARE_STATE:
		{
			switch(Event)
			{
				case BOOTLOADER_TICK_EVENT:
				{
					//TODO: Manage flash writing process
					if(ReadyToReceiveData == 1)		//TODO: replace this with flash SM result
					{
						ReadyToReceiveData = 0;
						printf("Bootloader Interface ready to receive data\n");
						BootloaderProtocolSendACK(BOOTLOADER_READY_FOR_DATA_RESPONSE);	
					}
					if(DataChunkWritten == 1)
					{
						DataChunkWritten = 0;
						BootloaderProtocolSendDataChunkResult(BOOTLOADER_CHUNK_TRANSFER_SUCCESS,CurDataChunkIndex);
						CurDataChunkIndex++;
					}
					
					break;
				}
				case BOOTLOADER_NEW_CMD_EVENT:
				{
					switch(Param)
					{
						case BOOTLOADER_SM_NEW_DATA_CHUNK_CMD:
						{
							
							//TODO:Check data validity
							//TODO: Write data to flash
							
							//Extract index from buffer
							int DataChunkIndex = 0;
							int DataChunkSize = 0;
							DataChunkIndex = BootloaderBuffer[0];
							DataChunkIndex <<= 8;
							DataChunkIndex += BootloaderBuffer[1];
							DataChunkIndex <<= 8;
							DataChunkIndex += BootloaderBuffer[2];
							DataChunkIndex <<= 8;
							DataChunkIndex += BootloaderBuffer[3];	
							
							DataChunkSize = BootloaderBuffer[4];
							DataChunkSize <<= 8;
							DataChunkSize += BootloaderBuffer[5];
							DataChunkSize <<= 8;
							DataChunkSize += BootloaderBuffer[6];
							DataChunkSize <<= 8;
							DataChunkSize += BootloaderBuffer[7];
							
							//Check CRC
							

							
							//if(CurDataChunkIndex != DataChunkIndex)
							if(0)
							{
								//Error... abort.
								BootloaderProtocolSendDataChunkResult(BOOTLOADER_CHUNK_TRANSFER_ERROR_INVALID_CHUNK_INDEX,CurDataChunkIndex);
								printf("Bootloader Interface ABORTING UPLOAD. Received invalid chunk index.  Rx: [%d] - Expected: [%d]\n", DataChunkIndex,CurDataChunkIndex);
								BootloaderResetStateMachine();
							}
							
							
							DataChunkWritten = 1;	//For debug only.
							
							break;
						}
						case BOOTLOADER_SM_INVALID_CRC_CMD:
						{
							//BootloaderProtocol determined the CRC of the chunk was invalid.
							BootloaderProtocolSendDataChunkResult(BOOTLOADER_CHUNK_TRANSFER_ERROR_RESEND,CurDataChunkIndex);
							printf("Bootloader Interface invalid chunk CRC. Requesting resend chunk index [%d]\n", CurDataChunkIndex);
						}
						case BOOTLOADER_SM_UPLOAD_FINISHED_CMD:
						{
							printf("Bootloader Interface firmware upload finished. Going into active state.\n");
							BootloaderInterfaceState = BOOTLOADER_ACTIVE_STATE;
							FirmwareUploaded = 1;
						}
						case BOOTLOADER_SM_ABORT_CMD:
						{
							//TODO invalidate data in Flash
							printf("Aborting upload, going into STANDBY mode\n");
							BootloaderResetStateMachine();
							break;
						}
						default:
						{
							//WHAT TO DO???
							
							break;
						}
					}
					break;
				}
				case BOOTLOADER_TIMEOUT_EVENT:
				{
					break;
				}
			}
			
			break;
		}
		case BOOTLOADER_SENDING_FIRMWARE_COPY_STATE:
		{
			switch(Event)
			{
				case BOOTLOADER_TICK_EVENT:
				{
					break;
				}
				case BOOTLOADER_NEW_CMD_EVENT:
				{
					switch(Param)
					{
						case BOOTLOADER_SM_ABORT_CMD:
						{
							//TODO invalidate data in Flash
							printf("Aborting upload, going into STANDBY mode\n");
							BootloaderResetStateMachine();
							break;
						}
					}
					break;
				}
				case BOOTLOADER_TIMEOUT_EVENT:
				{
					break;
				}
			}
			
			break;
		}
	}
	
}

void BootloaderResetStateMachine()
{
	BootloaderInterfaceState = BOOTLOADER_STANDBY_STATE;
	BootloaderFlashErased = 0;
	ReadyToReceiveData = 0;
	DataChunkWritten = 0;
	CurDataChunkIndex = 0;
	FirmwareUploaded = 0;
}

void BootloaderActivateBootloader()
{
	OpenBootloaderServer();
	BootloaderIterfaceStateMachine(BOOTLOADER_NEW_CMD_EVENT,BOOTLOADER_SM_ACTIVATE_CMD);
}
void BootloaderDeactivateBootloader()
{
	CloseBootloaderServer();
	BootloaderIterfaceStateMachine(BOOTLOADER_SM_ABORT_CMD,0);
}