//#############################################################################################################################################################################################################
#include "IDIBUS_IMPL.h"
//=============================================================================================================================================================================================================
//-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
//=============================================================================================================================================================================================================
void IDIBUS_ModuleBackgroundHandler(void)
{
	if (BootloaderInfo.Status == IDIFMW_STATUS_Busy){
		BootloaderDataHandler();
	} else if (BootloaderInfo.Status >= IDIFMW_STATUS_Reboot) {
		uint8_t TimeOut = 200; //ms
		while(USART1STR.TxComplete == 0 && TimeOut != 0) {_delay_ms(1);TimeOut--;}
		if ((BootloaderInfo.Reason & IDIFMW_REASON_NoFmwDetected) == 0){
			LaunchApplication();
		} else {
			System_SWReboot();			
		}
	}
}
//=============================================================================================================================================================================================================
//-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
//=============================================================================================================================================================================================================
void IDIBUS_ModuleCommandHandler()
{
	if (FARG.ComFunc == IDIMMES_COM_C_Init) {
		BootloaderInfo.Status += IDIFMW_STATUS_Reboot;
		IDIBUS_ResponseProtectedWrite(NULL,0,IDIER_NOPE);	
	} else if (FARG.ComFunc == IDIMMES_COM_C_FmwBootloaderWrite) {
		uint16_t BlockSize = GetNextBlockSize();
		if (FARG.InpDataLength != BlockSize || FARG.InpDataLength == 0) {IDIBUS_ResponseProtectedWrite(NULL, 0, IDIERSLV_INVALID_RX_REQUEST_FORMAT);}	
		else {
			if (BootloaderInfo.Status < IDIFMW_STATUS_Busy){
				BootloaderInfo.Status = IDIFMW_STATUS_Busy;
				_memcopy(&RxMesageBuf[3],BootloaderData,BlockSize);
			}
			//volatile uint8_t Response[3];	
			//GetBootloaderStatus(Response);
			IDIBUS_ResponseProtectedWrite(NULL,0,IDIER_NOPE);	
		}
	} else if (FARG.ComFunc == IDIMMES_COM_C_FmwBootloaderStatus) {
		if (FARG.InpDataLength != 0) {IDIBUS_ResponseProtectedWrite(NULL, 0, IDIERSLV_INVALID_RX_REQUEST_FORMAT);}	
		else{
			uint8_t Response[5];	
			GetBootloaderStatus(Response);
			IDIBUS_ResponseProtectedWrite(Response,5,IDIER_NOPE);	
		}
	} else if (FARG.ComFunc == IDIMMES_COM_C_FmwBootloaderInfo) {
		if (FARG.InpDataLength != 0 && FARG.InpDataLength != 1) {IDIBUS_ResponseProtectedWrite(NULL, 0, IDIERSLV_INVALID_RX_REQUEST_FORMAT);}	
		else{
			uint8_t Response[IDIMMES_COM_C_FmwBootloaderInfo_Length];	
			if (FARG.InpDataLength == 1) DisableAES();	//Disable AES if we get something
			GetBootloaderInfo(Response);
			IDIBUS_ResponseProtectedWrite(Response,IDIMMES_COM_C_FmwBootloaderInfo_Length,IDIER_NOPE);	
		}
	} else { IDIBUS_ResponseProtectedWrite(NULL, 0, IDIERSLV_NO_FIRMWARE); }	//TODO!!!
	/*
	switch ( FARG.ComFunc )
	{
		case (IDIMMES_COM_C_Init) : {
			BootloaderInfo.Status += IDIFMW_STATUS_Reboot;
			IDIBUS_ResponseProtectedWrite(NULL,0,IDIER_NOPE);	
			break;
		}
		case (IDIMMES_COM_C_FmwBootloaderWrite) : {
			uint16_t BlockSize = GetNextBlockSize();
			if (FARG.InpDataLength != BlockSize || FARG.InpDataLength == 0) {IDIBUS_ResponseProtectedWrite(NULL, 0, IDIERSLV_INVALID_RX_REQUEST_FORMAT);}	
			else {
				if (BootloaderInfo.Status < IDIFMW_STATUS_Busy){
					BootloaderInfo.Status = IDIFMW_STATUS_Busy;
					_memcopy(&RxMesageBuf[3],BootloaderData,BlockSize);
				}
				//volatile uint8_t Response[3];	
				//GetBootloaderStatus(Response);
				IDIBUS_ResponseProtectedWrite(NULL,0,IDIER_NOPE);	
			}
			break;	
		}
		case (IDIMMES_COM_C_FmwBootloaderStatus) : {
			if (FARG.InpDataLength != 0) {IDIBUS_ResponseProtectedWrite(NULL, 0, IDIERSLV_INVALID_RX_REQUEST_FORMAT);}	
			else{
				uint8_t Response[5];	
				GetBootloaderStatus(Response);
				IDIBUS_ResponseProtectedWrite(Response,5,IDIER_NOPE);	
			}
			break;
		}
		case (IDIMMES_COM_C_FmwBootloaderInfo) : {
			if (FARG.InpDataLength != 0) {IDIBUS_ResponseProtectedWrite(NULL, 0, IDIERSLV_INVALID_RX_REQUEST_FORMAT);}	
			else{
				uint8_t Response[IDIMMES_COM_C_FmwBootloaderInfo_Length];	
				GetBootloaderInfo(Response);
				IDIBUS_ResponseProtectedWrite(Response,IDIMMES_COM_C_FmwBootloaderInfo_Length,IDIER_NOPE);	
			}
			break;
		}
		default : { IDIBUS_ResponseProtectedWrite(NULL, 0, IDIERSLV_NO_FIRMWARE); }	//TODO!!!
	}
*/
}
//-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
//=============================================================================================================================================================================================================
void IDIBUS_ResponseProtectedWrite(uint8_t *Data, uint16_t DataLength, uint8_t ErrorCode)
{
		//TODO!!!!!
		if ( ErrorCode != IDIER_NOPE ) { FARG.SMPS |= 1; }
		else 							{ FARG.SMPS &= ~(1); }
		FARG.OutData[IDISMES_ERROR_Pos] = ErrorCode;
		FARG.OutDataLength = (uint16_t)(FARG.OutDataLength + 1);

		if (Data != NULL)
		{
			_memcopy(Data, &FARG.OutData[IDISMES_ERROR_Pos+1], DataLength);
			FARG.OutDataLength = (uint16_t)(FARG.OutDataLength + DataLength);
		}
}
//=============================================================================================================================================================================================================
//-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
//=============================================================================================================================================================================================================
//-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
//#############################################################################################################################################################################################################