//#############################################################################################################################################################################################################
#include "RSLink.h"
#include "idibus_hw.h"
#include "System.h"
#include "MEMORY.h"
#include "boot.h"
#include "bootFunctions.h"
#include "USART1.h"
#include "IDIBUS_IMPL.h"

static uint8_t 			   RS_Address;
static uint8_t 			   RS_SpeedCode;

static uint8_t  mmes_buf[IDIMMES_MAX_MES_SIZE];		//RX buffer for copy. Store last msg here

static IDIBUS_FARG_TYPE FARG;

 flash_datablock_t dataBoot;

static void RSLink_SendSMES()
{
	if (FARG.OutDataLength > IDISMES_ERROR_Pos)
	{
		FARG.OutData[IDISMES_ADDR_Pos] = RS_Address;
		FARG.OutData[IDISMES_SMPS_Pos] = FARG.SMPS;
		uint16_t crc16 = CRC16(&FARG.OutData[0], FARG.OutDataLength);
		FARG.OutData[FARG.OutDataLength++] = (uint8_t)(crc16 >> 8);
		FARG.OutData[FARG.OutDataLength++] = (uint8_t) crc16;
		USART1_SendTxBuf(FARG.OutDataLength);
	}
}
static void IDIBUS_ResponseProtectedWrite(uint8_t *Data, uint16_t DataLength, uint8_t ErrorCode)
{
	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_DATA_Pos], DataLength);
		FARG.OutDataLength = (uint16_t)(FARG.OutDataLength + DataLength);
	}
    RSLink_SendSMES();
}

static void IDIBUS_ModuleCommandHandler()
{
	if (FARG.ComFunc == IDIMMES_COM_C_Init)
	{
		IDIBUS_ResponseProtectedWrite(NULL,0,IDIER_NOPE);
	}
	else if (FARG.ComFunc == IDIMMES_COM_C_ReadDevFullSN_MS)
	{
        flash_datablock_t dataBootR = dataBoot;
        dataBootR.SW[0] = 0;dataBootR.SW[1] = 0;
		IDIBUS_ResponseProtectedWrite((uint8_t*)&dataBootR,sizeof(dataBootR),IDIER_NOPE);
	}
    else if (FARG.ComFunc == IDIMMES_COM_C_FmwBootloaderInfo)
    {
		IDIBUS_ResponseProtectedWrite((uint8_t*)&dataBoot.SW,sizeof(dataBoot.SW),IDIER_NOPE);
    }
    else if (FARG.ComFunc == IDIMMES_COM_C_FmwBootloaderStart)
    {
       if (FARG.InpDataLength == sizeof(FMW_INFO)){
            //Some input came with MMES
           FMW_INFO fw_info;
           _memcopy(&mmes_buf[3], (uint8_t*)&fw_info,sizeof(FMW_INFO));
           if (BootloaderUpdateInit(fw_info))
           {
                IDIBUS_ResponseProtectedWrite(NULL, 0,IDIERSLV_NO_FIRMWARE);
           }
           else
           {
                IDIBUS_ResponseProtectedWrite(NULL, 0, IDIER_NOPE);
           }
        }
       else
       {
           //No input came with MMES
            IDIBUS_ResponseProtectedWrite(NULL, 0,
                                        IDIERSLV_INVALID_RX_REQUEST_FORMAT);
       }
    }
    else if (FARG.ComFunc == IDIMMES_COM_C_FmwBootloaderWrite)
    {
        if (FARG.InpDataLength != SPM_PAGESIZE)
		{
			IDIBUS_ResponseProtectedWrite(NULL, 0, IDIERSLV_INVALID_RX_REQUEST_FORMAT);
		}
		else
		{
            BootloaderUpdateHandler(&mmes_buf[3]);
			IDIBUS_ResponseProtectedWrite(NULL,0,IDIER_NOPE);
		}
    }
    else if (FARG.ComFunc == IDIMMES_COM_C_FmwBootloaderEnd)
    {
           if(BootloaderUpdateEnd())
           {
                IDIBUS_ResponseProtectedWrite(NULL, 0, IDIERSLV_NO_FIRMWARE);
           }
           else
           {
                IDIBUS_ResponseProtectedWrite(NULL, 0, IDIER_NOPE);
                //RSLink_SendSMES();
                while(USART1_IsTxActive());
                //Gotta restart now
                LaunchApplication();
           }
    }
    else if (FARG.ComFunc == IDIMMES_COM_C_GotoApp)
    {
        IDIBUS_ResponseProtectedWrite(NULL, 0, IDIER_NOPE);
        //RSLink_SendSMES();
        while(USART1_IsTxActive());
        LaunchApplication();
    }
	else
	{
		IDIBUS_ResponseProtectedWrite(NULL, 0, IDIERSLV_UNSUPPORTED_FUNC_NUM);
	}

}
//-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
void RSLink_Init()
{
    readDeviceData(&dataBoot, locationInBoot);
	FARG.OutData = USART1_getTxBuf();               //set RX addr
	USART1_RxTransferRestart();						//	Restart RX
    USART1_SetIdiBusBoudrate(RSLink_SpeedDecode());
    RS_Address = RSLink_AddrDecode();
}
//-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------

void RSLink_Handler(void)
{
#ifdef _LONG_ADDR_SPACE_
    // No dynamic adr and speed change for 328pb
    if(!USART1_IsTxActive())
    {
        uint8_t SpeedDSW_Code = RSLink_SpeedDecode();
        if (RS_SpeedCode != SpeedDSW_Code)
        {
            RS_SpeedCode = SpeedDSW_Code;
            USART1_SetIdiBusBoudrate(RS_SpeedCode);
        }
    }
	RS_Address = RSLink_AddrDecode();
#endif
	//Manage transfer status=====================================================
	if ( !USART1_IsNewRxMessage() )
	{
		return;
	}
	if ( USART1_IsRxError())
	{
		USART1_RxTransferRestart();
		return;
	}
	//==========================================================================
	// Check normal Message
	// Check Message size
	static uint16_t RxMessageSize;
    RxMessageSize = USART1_getRxBufSize();
    
	if ( (RxMessageSize < (MODBUS_CRC16_SIZE+1) ) || (RxMessageSize > IDIMMES_MAX_MES_SIZE) )
	{
        //Bad message size. Restart------------------------------------------------------
		USART1_RxTransferRestart();
		return;
	}
	//Good message. Copy it to local buf and continue---------------------------
	USART1_copyRxBuf( &mmes_buf[IDIMMES_ADDR_Pos], IDIMMES_ADDR_Pos, RxMessageSize ); 	// Need for CRC
	USART1_RxTransferRestart();
    
    //Parse address
    uint8_t RcvAddress;
	RcvAddress = mmes_buf[IDIMMES_ADDR_Pos];
	// MMESG(Module) or MMES -------------
	//That's our address.
	if ( RcvAddress == RS_Address )
	{
		//Calculate msg's CRC
		uint16_t CalculatedCRC = CRC16( mmes_buf, (uint16_t)(RxMessageSize-2) );
		//Read msg's CRC. Last two bytes???
		uint16_t ReceivedCRC   = (uint16_t)( ((uint16_t)mmes_buf[RxMessageSize-2] << 8) | mmes_buf[RxMessageSize-1] );
		if ( CalculatedCRC != ReceivedCRC )
		{
			//CRC doesn't match. BAD
			return;
		}

		FARG.OutDataLength = IDISMES_ERROR_Pos;	//Set length 2 (adr+cmd)

		uint8_t MMPS = mmes_buf[IDIMMES_MMPS_Pos];

		if ( (MMPS != (IDIMMES_MMPS_MES_TYPE_Msk)) || (RxMessageSize < IDIMMESG_MODULE_MIN_MES_SIZE))
		{
			IDIBUS_ResponseProtectedWrite(NULL, 0, IDIERSLV_INVALID_RX_REQUEST_FORMAT);
		}
		else
		{
			FARG.ComFunc =  mmes_buf[IDIMMESG_DATA_COM_FUNC_Pos];
			FARG.InpDataLength = (uint16_t)(RxMessageSize - (IDIMMESG_DATA_COM_FUNC_Pos + 1) - MODBUS_CRC16_SIZE);
			IDIBUS_ModuleCommandHandler();
		}
	}
}
//-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------

//#############################################################################################################################################################################################################