//#############################################################################################################################################################################################################
#include "RSLink.h"

uint8_t  mmes_buf[IDIMMES_MAX_MES_SIZE];		//RX buffer for copy
struct RSLINK_STRUCT RSLINK;


static rslink_func_pack_t rslink_hw =
{
	.status_led_init = NULL,
	.status_led_on = NULL,
	.status_led_off = NULL,
	.dips_init = NULL,
	.disp_read_addr = NULL,
	.disp_read_speed = NULL,
};
static idibus_slave_module_t*  IDIBUS_MODULE;
//Device descriptor

void RSLlink_pass_module(idibus_slave_module_t*  MODULE)
{
	IDIBUS_MODULE = MODULE;
}

void RSLink_register_hw_functions(rslink_func_pack_t callback)
{
	rslink_hw.dips_init = callback.dips_init;
	rslink_hw.status_led_init = callback.status_led_init;
	rslink_hw.status_led_on = callback.status_led_on;
	rslink_hw.status_led_off = callback.status_led_off;
	rslink_hw.disp_read_addr = callback.disp_read_addr;
	rslink_hw.disp_read_speed = callback.disp_read_speed;
}
//-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
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_hw.status_led_init();									//	Init LEDs
	rslink_hw.dips_init();										//	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_hw.disp_read_speed();
	if (RSLINK.SpeedCode != SpeedDSW_Code)
	{
		RSLINK.SpeedCode = SpeedDSW_Code;
		return 1;
	}
	return 0;
}
//-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
void RSLink_AddressCheckAndUpdate(void)
{
	RSLINK.Address = rslink_hw.disp_read_addr();
	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_hw.status_led_on();
		}
		else
		{
			rslink_hw.status_led_off();
		}
	}

	// Background Error Handler and Long Operations =================================
	IDIBUS_ModuleBackgroundHandler();
	IDIBUS_ChannelBackgroundHandler();
    if ( RSLINK.USI->IsRxError() == 1)
	{
		RSLINK.USI->RxTransferRestart();
		return;
	}
	// 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;
	}
	IdiBusProccessMessage();
}
//-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
void IdiBusProccessMessage()
{
    // Message Handling Routine =====================================================
	if ( RSLINK.USI->IsNewRxMessage() == 0 )
	{
		return;
	}

	uint16_t RxMessageSize;
	uint8_t *TxMesageBuf = RSLINK.USI->getTxBuf();

	// 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( &mmes_buf[IDIMMES_ADDR_Pos], IDIMMES_ADDR_Pos, 1 ); 	// Need for CRC
		RcvAddress = mmes_buf[IDIMMES_ADDR_Pos];
	}

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

		idibus_farg_t 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    = (mmes_buf[IDIMMES_MMPS_Pos] & IDIMMES_MMPS_FAST_FUNC_Msk) >> IDIMMES_MMPS_FAST_FUNC_Pos;
		uint8_t MMPS_MesType     = (mmes_buf[IDIMMES_MMPS_Pos] & IDIMMES_MMPS_MES_TYPE_Msk)  >> IDIMMES_MMPS_MES_TYPE_Pos;
		uint8_t MMPS_LongMessage =  mmes_buf[IDIMMES_MMPS_Pos] & IDIMMES_MMPS_LONG_MES_Msk;
		uint8_t MMPS_AlarmFrame  =  mmes_buf[IDIMMES_MMPS_Pos] & IDIMMES_MMPS_ALARM_FRAME_Msk;
		uint8_t MMPS_EncryptedAes = mmes_buf[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 =  mmes_buf[IDIMMESG_DATA_COM_FUNC_Pos];
				FARG.InpData = &mmes_buf[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)) || (mmes_buf[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 = mmes_buf[IDIMMES_DATA_FUNC_COM_DATA_Pos];
			FARG.InpData = &mmes_buf[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 = &mmes_buf[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      = (mmes_buf[IDIMMES_DEV_Pos]  & IDIMMES_DEV_NUM_Msk) >> IDIMMES_DEV_NUM_Pos;
		uint8_t AllChannels    =  mmes_buf[IDIMMES_DEV_Pos]  & IDIMMES_DEV_ALLCH_Msk;
		uint8_t ChannelNum     = (mmes_buf[IDIMMES_CHNL_Pos] & IDIMMES_CHNL_NUM_Msk) >> IDIMMES_CHNL_NUM_Pos;
		uint8_t ChannelAllSame =  mmes_buf[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_t *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_t *CH = &IDIBUS_MODULE->Devices[DeviceNum].Channels[I];
					CH->CH_Func( CH, &FARG );
				}
			}
		}
		else
		{
			idibus_channel_t *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( &mmes_buf[IDIMMES_MMPS_Pos], IDIMMES_MMPS_Pos, (uint16_t)(RxMessageSize-1) );
		RSLINK.USI->RxTransferRestart();
		uint16_t CalculatedCRC = MODBUS_CRC16_T( mmes_buf, (uint16_t)(RxMessageSize-2) );
		uint16_t ReceivedCRC   = (uint16_t)( ((uint16_t)mmes_buf[RxMessageSize-2] << 8) | mmes_buf[RxMessageSize-1] );
		if ( CalculatedCRC != ReceivedCRC )
		{
			return;
		}
		IDIBUS_MODULE->STATUS.STATE.B1S.TimeoutLed = 0;
		RSLINK.LastMMES_TimeInstance = System_GetTimeInstance();

		idibus_farg_t 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    = (mmes_buf[IDIMMES_MMPS_Pos] & IDIMMES_MMPS_FAST_FUNC_Msk) >> IDIMMES_MMPS_FAST_FUNC_Pos;
		uint8_t MMPS_MesType     = (mmes_buf[IDIMMES_MMPS_Pos] & IDIMMES_MMPS_MES_TYPE_Msk)  >> IDIMMES_MMPS_MES_TYPE_Pos;
		uint8_t MMPS_LongMessage =  mmes_buf[IDIMMES_MMPS_Pos] & IDIMMES_MMPS_LONG_MES_Msk;
		uint8_t MMPS_AlarmFrame  =  mmes_buf[IDIMMES_MMPS_Pos] & IDIMMES_MMPS_ALARM_FRAME_Msk;
		uint8_t MMPS_EncryptedAes = mmes_buf[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 = mmes_buf[IDIMMESG_DATA_COM_FUNC_Pos];
			if ( FARG.ComFunc <= IDIMMES_MAX_FAST_FUNC_NUM )
			{
				return;
			}
			FARG.InpData = &mmes_buf[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 = &mmes_buf[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_t *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_t *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);
	}
}
//-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
//#############################################################################################################################################################################################################


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