//#############################################################################################################################################################################################################
#include "RSLink.h"
//-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
void RSLink_Init(USART_INTERFACE_TYPE *USART_INTERFACE, USART_IDIBUS_ROUTINE_TYPE *USART_ID_INTERFACE)
{
	RSLINK.USI=USART_INTERFACE;								//	USART POINTER INIT
	RSLINK.USIID=USART_ID_INTERFACE;
	RSLINK.NeedSpeedChangeFlag=0;
	RSLINK.LastMMES_TimeInstance=System_GetTimeInstance();
	RSLink_StatusLedInit();									//	Init LEDs
	RSLink_DipsInit();										//	Init DIP switches 
	RSLink_SpeedCheckAndUpdate();							//	Set speed
	RSLINK.USIID->SetIdiBusBoudrate(RSLINK.SpeedCode);		//	Set speed usart
	RSLink_AddressCheckAndUpdate();							//	Set adr
	RSLINK.USI->RxTransferRestart();						//	Restart RX
}
//-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
uint8_t RSLink_SpeedCheckAndUpdate(void)
{
	uint8_t SpeedDSW_Code = RSLink_SpeedDecode();
	if (RSLINK.SpeedCode != SpeedDSW_Code) { RSLINK.SpeedCode = SpeedDSW_Code; return 1; }
	return 0;
}
//-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
void RSLink_AddressCheckAndUpdate(void)
{
	RSLINK.Address = RSLink_AddrDecode();
	if ((RSLINK.Address>=IDIBUS_SLAVE_ADDR_MIN)&&(RSLINK.Address<=IDIBUS_SLAVE_ADDR_MAX)) { RSLINK.Address_Error=0; }
	else { RSLINK.Address_Error=1; }
}
//-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
void RSLink_Handler(void)	
{
	// Address and Speed Change Check Routine =======================================
	if ( (RSLINK.NeedSpeedChangeFlag == 1) || RSLink_SpeedCheckAndUpdate() )
	{
		if ( RSLINK.USI->IsTxActive() ) { RSLINK.NeedSpeedChangeFlag = 1; }
		else  						     { RSLINK.USIID->SetIdiBusBoudrate(RSLINK.SpeedCode); RSLINK.NeedSpeedChangeFlag = 0; }
	}
	RSLink_AddressCheckAndUpdate();


	// Timeout Led Routine ==========================================================
	if ( IDIBUS_MODULE.STATUS.STATE.B1S.TimeoutLed == 0 )
	{
		if (
				( (IDIBUS_MODULE.STATUS.STATE.B1S.NoMMESTimeout == 0) && (System_GetSysTickDifference( RSLINK.LastMMES_TimeInstance) >= IDIBUS_LINK_LED_NO_MMES_TIMEOUT_0_MS) ) ||
				( (IDIBUS_MODULE.STATUS.STATE.B1S.NoMMESTimeout == 1) && (System_GetSysTickDifference( RSLINK.LastMMES_TimeInstance) >= IDIBUS_LINK_LED_NO_MMES_TIMEOUT_1_MS) )
		   )
		{
			IDIBUS_MODULE.STATUS.STATE.B1S.TimeoutLed=1;
			//RSLink_StatusLedSetOn();
		}
		else { RSLink_StatusLedSetOff(); }
	}
	
	// Background Error Handler and Long Operations =================================
	IDIBUS_ModuleBackgroundHandler();
	IDIBUS_ChannelBackgroundHandler();

	// Message Handling Routine =====================================================
	if ( RSLINK.USI->IsNewRxMessage() == 0 ) { return; }
	
	if ( RSLINK.USI->IsRxError() == 1) { 
		RSLINK.USI->RxTransferRestart();
		return;
	}
	
	uint16_t RxMessageSize;
	uint8_t *TxMesageBuf = RSLINK.USI->getTxBuf();

	// Check incorrect and Developers DIP Switch Address ---------------------------->
	if ( RSLINK.Address_Error )
	{
		switch ( RSLINK.Address )
		{
			case ( IDIBUS_DEVELOPER_ADDR_0 ) : { break; }
			case ( IDIBUS_DEVELOPER_ADDR_1 ) : { break; }
			case ( IDIBUS_DEVELOPER_ADDR_2 ) : { break; }
			case ( IDIBUS_DEVELOPER_ADDR_3 ) : { break; }
			default : {}
		}
		RSLINK.USI->RxTransferRestart();
		return;
	}


	// Check normal Message --------------------------------------------------------->

	// Check Message size
	uint8_t RcvAddress;
	RxMessageSize = RSLINK.USI->getRxBufSize();
	if ( (RxMessageSize < (MODBUS_CRC16_SIZE+1) ) || (RxMessageSize > IDIMMES_MAX_MES_SIZE) ) { RSLINK.USI->RxTransferRestart(); return; }
	else
	{
		RSLINK.USI->copyRxBuf( &RxMesageBuf[IDIMMES_ADDR_Pos], IDIMMES_ADDR_Pos, 1 ); 	// Need for CRC
		RcvAddress = RxMesageBuf[IDIMMES_ADDR_Pos];
	}

	// MMESG(Module) or MMES ------------->
	if ( RcvAddress == RSLINK.Address )
	{
		// Check CRC, copy RxBuf and Restart Transfer
		RSLINK.USI->copyRxBuf( &RxMesageBuf[IDIMMES_MMPS_Pos], IDIMMES_MMPS_Pos, (uint16_t)(RxMessageSize-1) );
		RSLINK.USI->RxTransferRestart();
		uint16_t CalculatedCRC = MODBUS_CRC16_T( RxMesageBuf, (uint16_t)(RxMessageSize-2) );
		uint16_t ReceivedCRC   = (uint16_t)( ((uint16_t)RxMesageBuf[RxMessageSize-2] << 8) | RxMesageBuf[RxMessageSize-1] );
		if ( CalculatedCRC != ReceivedCRC ) { return; }
		IDIBUS_MODULE.STATUS.STATE.B1S.TimeoutLed = 0;
		RSLINK.LastMMES_TimeInstance = System_GetTimeInstance();		//FOR STM, (reset led)

		IDIBUS_FARG_TYPE FARG;
		FARG.OutData = &TxMesageBuf[0];		//set RX addr
		FARG.OutDataLength = IDISMES_ERROR_Pos;	//Set length 2 (adr+cmd)
		FARG.ErrorPos = IDISMES_ERROR_Pos;		//set pos err
		FARG.OutDataPos = IDISMES_ERROR_Pos + 1;	//set pos data
		FARG.ErrorState = 0;
		FARG.OutLongMesState = 0;
		if ( IDIBUS_MODULE.LONG_OP.State == IDILONGOP_STATE_IN_PROC ) { FARG.LongOpState = 1; }
		else                                                          { FARG.LongOpState = 0; }


		// Check if too short for MMESG message
		if ( RxMessageSize < (IDIMMES_MMPS_Pos + 1 + MODBUS_CRC16_SIZE) ) { IDIBUS_ResponseProtectedWrite(&FARG, NULL, 0, IDIERSLV_INVALID_RX_REQUEST_FORMAT); RSLink_SendSMES(&FARG); return;}

		// Get MMPS
		uint8_t MMPS_FastFunc    = (RxMesageBuf[IDIMMES_MMPS_Pos] & IDIMMES_MMPS_FAST_FUNC_Msk) >> IDIMMES_MMPS_FAST_FUNC_Pos;
		uint8_t MMPS_MesType     = (RxMesageBuf[IDIMMES_MMPS_Pos] & IDIMMES_MMPS_MES_TYPE_Msk)  >> IDIMMES_MMPS_MES_TYPE_Pos;
		uint8_t MMPS_LongMessage =  RxMesageBuf[IDIMMES_MMPS_Pos] & IDIMMES_MMPS_LONG_MES_Msk;
		uint8_t MMPS_AlarmFrame  =  RxMesageBuf[IDIMMES_MMPS_Pos] & IDIMMES_MMPS_ALARM_FRAME_Msk;
		uint8_t MMPS_EncryptedAes = RxMesageBuf[IDIMMES_MMPS_Pos] & IDIMMES_MMPS_ENCRYPTED_AES_Msk;
		if ( MMPS_AlarmFrame != 0 )
		{
			// Do Alarm
		}
		if ( MMPS_EncryptedAes != 0 )
		{
			if ( IDIBUS_MODULE.STATUS.STATE.B0S.AesSupported == 0 )      { IDIBUS_ResponseProtectedWrite(&FARG, NULL, 0, IDIERSLV_ENCRYPTION_NOT_SUPPORTED); RSLink_SendSMES(&FARG); return; }
			else if ( IDIBUS_MODULE.STATUS.STATE.B0S.AesInstalled == 0 ) { IDIBUS_ResponseProtectedWrite(&FARG, NULL, 0, IDIERSLV_ENCRYPTION_NOT_INSTALLED); RSLink_SendSMES(&FARG); return; }
			else
			{
				// Decrypt ...
			}
		}
		if ( MMPS_LongMessage != 0 )
		{
			IDIBUS_ResponseProtectedWrite(&FARG, NULL, 0, IDIERSLV_JUMBO_NOT_SUPPORTED);
			RSLink_SendSMES(&FARG);
			return;
		}
		// If Module Function
		if ( MMPS_MesType == IDIMMES_MMPS_MES_TYPE_MMESG )
		{
			if ( (MMPS_FastFunc != 0) || (RxMessageSize < IDIMMESG_MODULE_MIN_MES_SIZE) ) { IDIBUS_ResponseProtectedWrite(&FARG, NULL, 0, IDIERSLV_INVALID_RX_REQUEST_FORMAT); }
			else
			{
				FARG.ComFunc =  RxMesageBuf[IDIMMESG_DATA_COM_FUNC_Pos];
				FARG.InpData = &RxMesageBuf[IDIMMESG_DATA_COM_FUNC_Pos + 1];
				FARG.InpDataLength = (uint16_t)(RxMessageSize - (IDIMMESG_DATA_COM_FUNC_Pos + 1) - MODBUS_CRC16_SIZE);
				IDIBUS_ModuleCommandHandler(&FARG);
			}
			RSLink_SendSMES(&FARG);
			return;
		}

		// IF NOT Module Function
		if ( IDIBUS_MODULE.STATUS.STATE.B0S.StState == IDISTATUS_B0S_ST_STATE_StFreeze ) { IDIBUS_ResponseProtectedWrite(&FARG, NULL, 0, IDIERSLV_IN_FREEZE);       RSLink_SendSMES(&FARG); return; }
		else if ( IDIBUS_MODULE.Error != IDIER_NOPE ) 								   { IDIBUS_ResponseProtectedWrite(&FARG, NULL, 0, IDIBUS_MODULE.Error);      RSLink_SendSMES(&FARG); return; }
		else if ( IDIBUS_MODULE.LONG_OP.State == IDILONGOP_STATE_IN_PROC )               { IDIBUS_ResponseProtectedWrite(&FARG, NULL, 0, IDIERSLV_LONG_OP_IN_PROC); RSLink_SendSMES(&FARG); return; }

		if ( MMPS_FastFunc == 0 )
		{
			if ( (RxMessageSize < (IDIMMES_MIN_MES_SIZE + 1)) || (RxMesageBuf[IDIMMES_DATA_FUNC_COM_DATA_Pos] <= IDIMMES_MAX_FAST_FUNC_NUM) )	//CHECK LEN ON MSG
			{
				IDIBUS_ResponseProtectedWrite(&FARG, NULL, 0, IDIERSLV_INVALID_RX_REQUEST_FORMAT); RSLink_SendSMES(&FARG);
				return;
			}
			FARG.ComFunc = RxMesageBuf[IDIMMES_DATA_FUNC_COM_DATA_Pos];
			FARG.InpData = &RxMesageBuf[IDIMMES_DATA_FUNC_COM_DATA_Pos + 1];
			FARG.InpDataLength = (uint16_t)(RxMessageSize - (IDIMMES_DATA_FUNC_COM_DATA_Pos + 1) - MODBUS_CRC16_SIZE);
		}
		else
		{
			if ( RxMessageSize < IDIMMES_MIN_MES_SIZE ) { IDIBUS_ResponseProtectedWrite(&FARG, NULL, 0, IDIERSLV_INVALID_RX_REQUEST_FORMAT); RSLink_SendSMES(&FARG); return; }
			FARG.ComFunc = MMPS_FastFunc;
			FARG.InpData = &RxMesageBuf[IDIMMES_DATA_FUNC_COM_DATA_Pos];
			FARG.InpDataLength = (uint16_t)(RxMessageSize - IDIMMES_DATA_FUNC_COM_DATA_Pos - MODBUS_CRC16_SIZE);
		}
		// Channels command and funcs handling
		uint8_t DeviceNum      = (RxMesageBuf[IDIMMES_DEV_Pos]  & IDIMMES_DEV_NUM_Msk) >> IDIMMES_DEV_NUM_Pos;
		uint8_t AllChannels    =  RxMesageBuf[IDIMMES_DEV_Pos]  & IDIMMES_DEV_ALLCH_Msk;
		uint8_t ChannelNum     = (RxMesageBuf[IDIMMES_CHNL_Pos] & IDIMMES_CHNL_NUM_Msk) >> IDIMMES_CHNL_NUM_Pos;
		uint8_t ChannelAllSame =  RxMesageBuf[IDIMMES_CHNL_Pos] & IDIMMES_CHNL_ALLSAME_Msk;

		if ( DeviceNum >= IDIBUS_MODULE.DevicesCount )                                              { IDIBUS_ResponseProtectedWrite(&FARG, NULL, 0, IDIERDEV_INVALID_DEV_NUM); RSLink_SendSMES(&FARG); return; }
		if ( (ChannelNum >= IDIBUS_MODULE.Devices[DeviceNum].ChannelsCount) && (AllChannels == 0) ) { IDIBUS_ResponseProtectedWrite(&FARG, NULL, 0, IDIERDEV_INVALID_CHN_NUM); RSLink_SendSMES(&FARG); return; }
		if ( AllChannels != 0 )
		{
			uint8_t ChannelsCount = IDIBUS_MODULE.Devices[DeviceNum].ChannelsCount;
			FARG.OutDataPos = (uint16_t)( FARG.OutDataPos + ChannelsCount - 1 );	// SMES = ADDR SMPS ERR1 ERR2 ERR3 DATA1 DATA2 DATA3 CRC
			if ( ChannelAllSame == 0 )
			{
				uint16_t DataPortionLength = (uint16_t)( FARG.InpDataLength / ChannelsCount );	// If DataLength==0 -> OK  (Byte count on one channel)
				// SMES = ADDR SMPS ERR1 ERR2!=0 ERR3 DATA1 DATA3 CRC
				if ( FARG.InpDataLength != (DataPortionLength * ChannelsCount) )
				{
					for (uint8_t I=0; I < ChannelsCount; I++ ) { IDIBUS_ResponseProtectedWrite(&FARG, NULL, 0, IDIERSLV_INVALID_RX_REQUEST_FORMAT); }
				}
				else {
					FARG.InpDataLength = DataPortionLength;
					for (uint8_t I=0; I < ChannelsCount; I++ )
					{
						IDIBUS_CHANNEL *CH = &IDIBUS_MODULE.Devices[DeviceNum].Channels[I];
						CH->CH_Func( CH, &FARG );
						FARG.InpData += DataPortionLength;		//Pointer offset
					}
				}
			}
			else { for (uint8_t I=0; I < IDIBUS_MODULE.Devices[DeviceNum].ChannelsCount; I++ )
				{
					IDIBUS_CHANNEL *CH = &IDIBUS_MODULE.Devices[DeviceNum].Channels[I];
					CH->CH_Func( CH, &FARG );
				}
			}
		}
		else
		{
			IDIBUS_CHANNEL *CH = &IDIBUS_MODULE.Devices[DeviceNum].Channels[ChannelNum];
			CH->CH_Func( CH, &FARG );
		}
		RSLink_SendSMES(&FARG);
		return;
	} //if ( (RcvAddress == RSLINK.Address) && (RxMessageSize >= IDIMMES_MIN_MES_SIZE) )


	// MMESG ------------->
	// NO RESPONSE!!!
	if ( (RcvAddress >= IDIBUS_GROUP_0_ADDR) && (RcvAddress <= IDIBUS_GROUP_15_ADDR) )
	{
		// Check CRC, copy RxBuf and Restart Transfer
		RSLINK.USI->copyRxBuf( &RxMesageBuf[IDIMMES_MMPS_Pos], IDIMMES_MMPS_Pos, (uint16_t)(RxMessageSize-1) );
		RSLINK.USI->RxTransferRestart();
		uint16_t CalculatedCRC = MODBUS_CRC16_T( RxMesageBuf, (uint16_t)(RxMessageSize-2) );
		uint16_t ReceivedCRC   = (uint16_t)( ((uint16_t)RxMesageBuf[RxMessageSize-2] << 8) | RxMesageBuf[RxMessageSize-1] );
		if ( CalculatedCRC != ReceivedCRC ) { return; }
		IDIBUS_MODULE.STATUS.STATE.B1S.TimeoutLed = 0;
		RSLINK.LastMMES_TimeInstance = System_GetTimeInstance();

		IDIBUS_FARG_TYPE FARG;
		FARG.OutData = NULL;

		// Check if too short for MMESG message
		if ( RxMessageSize < (IDIMMES_MMPS_Pos + 1 + MODBUS_CRC16_SIZE) ) { return; }

		// Get MMPS
		uint8_t MMPS_FastFunc    = (RxMesageBuf[IDIMMES_MMPS_Pos] & IDIMMES_MMPS_FAST_FUNC_Msk) >> IDIMMES_MMPS_FAST_FUNC_Pos;
		uint8_t MMPS_MesType     = (RxMesageBuf[IDIMMES_MMPS_Pos] & IDIMMES_MMPS_MES_TYPE_Msk)  >> IDIMMES_MMPS_MES_TYPE_Pos;
		uint8_t MMPS_LongMessage =  RxMesageBuf[IDIMMES_MMPS_Pos] & IDIMMES_MMPS_LONG_MES_Msk;
		uint8_t MMPS_AlarmFrame  =  RxMesageBuf[IDIMMES_MMPS_Pos] & IDIMMES_MMPS_ALARM_FRAME_Msk;
		uint8_t MMPS_EncryptedAes = RxMesageBuf[IDIMMES_MMPS_Pos] & IDIMMES_MMPS_ENCRYPTED_AES_Msk;
		if ( MMPS_AlarmFrame != 0 )
		{
			// Do Alarm
		}
		if ( MMPS_EncryptedAes != 0 )
		{
			if ( (IDIBUS_MODULE.STATUS.STATE.B0S.AesSupported == 0) || (IDIBUS_MODULE.STATUS.STATE.B0S.AesInstalled == 0) ) { return; }
			else
			{
				// Decrypt ...
			}
		}
		if ( MMPS_LongMessage != 0 )
		{
			return;
		}
		if (
				( MMPS_MesType != IDIMMES_MMPS_MES_TYPE_MMESG ) ||
				( IDIBUS_MODULE.STATUS.STATE.B0S.StState == IDISTATUS_B0S_ST_STATE_StFreeze ) ||	 // There are no Module commands in group
				( IDIBUS_MODULE.Error != IDIER_NOPE ) ||
				( IDIBUS_MODULE.LONG_OP.State == IDILONGOP_STATE_IN_PROC )
		   ) { return; }

		if ( MMPS_FastFunc == 0 )
		{
			if ( RxMessageSize < (IDIMMESG_GROUP_MIN_MES_SIZE + 1) ) { return; }
			FARG.ComFunc = RxMesageBuf[IDIMMESG_DATA_COM_FUNC_Pos];
			if ( FARG.ComFunc <= IDIMMES_MAX_FAST_FUNC_NUM )         { return; }
			FARG.InpData = &RxMesageBuf[IDIMMESG_DATA_COM_FUNC_Pos + 1];
			FARG.InpDataLength = (uint16_t)(RxMessageSize - (IDIMMESG_DATA_COM_FUNC_Pos + 1) - MODBUS_CRC16_SIZE);
		}
		else
		{
			if ( RxMessageSize < IDIMMESG_GROUP_MIN_MES_SIZE ) { return; }
			FARG.ComFunc = MMPS_FastFunc;
			FARG.InpData = &RxMesageBuf[IDIMMESG_DATA_COM_FUNC_Pos];
			FARG.InpDataLength = (uint16_t)(RxMessageSize - IDIMMESG_DATA_COM_FUNC_Pos - MODBUS_CRC16_SIZE);
		}
		for (uint8_t Dev = 0; Dev < IDIBUS_MODULE.DevicesCount; Dev++)
		{
			for (uint8_t Ch = 0; Ch<IDIBUS_MODULE.Devices[Dev].ChannelsCount; Ch++)
			{
				IDIBUS_CHANNEL *CH = &IDIBUS_MODULE.Devices[Dev].Channels[Ch];
				if ( (RcvAddress == IDIBUS_GROUP_0_ADDR) || (CH->BcastAddr == RcvAddress) ) { CH->CH_Func(CH, &FARG); }
			}
		}
		return;
	} //if ( ((RcvAddress >= IDIBUS_GROUP_0_ADDR) && (RcvAddress <= IDIBUS_GROUP_15_ADDR)) )

	RSLINK.USI->RxTransferRestart(); // NOT MMES AND MMESG ------------->
	return;
}
//-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
void RSLink_SendSMES(IDIBUS_FARG_TYPE *farg)
{
	if ( (RSLINK.USIID->ResponseTimeoutComplete == 0) && (farg->OutDataLength > IDISMES_ERROR_Pos) )
	{
		farg->OutData[IDISMES_ADDR_Pos] = RSLINK.Address;
		farg->OutData[IDISMES_SMPS_Pos] = 0;
		if ( farg->ErrorState   != 0 )    { farg->OutData[IDISMES_SMPS_Pos] |= (1U << IDISMES_SMPS_ERROR_BIT_Pos); }
		if ( farg->OutLongMesState != 0 ) { farg->OutData[IDISMES_SMPS_Pos] |= (1U << IDISMES_SMPS_LONG_MES_Pos);  }
		if ( farg->LongOpState != 0 )     { farg->OutData[IDISMES_SMPS_Pos] |= (1U << IDISMES_SMPS_LONG_OP_Pos);  }
		uint16_t CRC16 = MODBUS_CRC16_T(&farg->OutData[0], farg->OutDataLength);
		farg->OutData[farg->OutDataLength++] = (uint8_t)(CRC16 >> 8);
		farg->OutData[farg->OutDataLength++] = (uint8_t) CRC16;
		RSLINK.USI->SendTxBuf(farg->OutDataLength);
	}
}
//#############################################################################################################################################################################################################