/*@!Encoding:1252*/
// Additionally include ModbusTcpCommon.cin or ModbusUdpCommon.cin
includes
{
	#include "ModbusCommonStructs.cin"
}

variables
{
	const word gMaxPacketLength = __size_of(struct ModbusReqWriteRegisters);
	
	msTimer gtRobin;
	word gTxID = 0x0000;			// Transaction Identifier for Modbus. Used as index for gQueue

	// Global storage for pending and sent requests, associated by TxID
	struct QueueElement
	{
		word TimeoutTicks;
		byte Timeouts;
		word Length;
		byte Buffer[gMaxPacketLength];
	};
	struct QueueElement gQueuePending[long, 2];
	struct QueueElement gQueueSent[long, 2];
	struct QueueElement gQueueAck[long, 2];
}

void ModbusInit()
{
	char ip[16];
	sysGetVariableString("%BUS_TYPE%%CHANNEL%::%NODE_NAME%::Config", "IP", ip, elCount(ip));

	ModbusConnectTo(ip, @sysvar::Config::Modbus::Port);
}

void ModbusMakeHeader(struct ModbusApHeader mbap, word length)
{
	mbap.TxID = gTxID++;		// [2] Transaction ID
	mbap.Protocol = 0x0000;		// [2] Protocol ID = 0 = Modbus
	mbap.Length = length - __offset_of(struct ModbusApHeader, UnitID);	// [2] Length; Number of bytes following
	mbap.UnitID = 0xFF;			// [1] Unit identifier; not relevant
}



// REGION: ModbusReadBits -------------------------------------------------------------
/// <ModbusReadBits>
void ModbusReadBits(word address, word count)
{
	const byte length = __size_of(struct ModbusReqRead);
	const byte funcCode = 0x01;
	byte buffer[length];
	struct ModbusReqRead mbr;

	ModbusMakeHeader(mbr.Header, length);
	// Payload
	mbr.Header.FuncCode = funcCode;	// [1] Function Code; 1: Read Coils (DI), 2: Read Discret Inputs (DIO), seems to be the same for WAGO 750-881
	mbr.Address = address;			// [2] Start address
	mbr.Count = count;				// [2] Number of items; 1:max 2000=0x7D0
	
	memcpy_h2n(buffer, mbr);
	ModbusSend(buffer, length, mbr.Header.TxID);
}

/// <ModbusReadBits>
void OnModbusReceiveBits(byte buffer[])
{
	struct ModbusResReceiveBits mbr;
	byte bitStatus[1968];
	word numBits;
	byte i, j;

	memcpy_n2h(mbr, buffer);

	numBits = (gQueueAck[mbr.Header.TxID].Buffer[10] << 8) + gQueueAck[mbr.Header.TxID].Buffer[11];

	for (i = 0; i < mbr.ByteCount; i++)
	{
		for (j = 0; j < 8; j++)
		{
			bitStatus[8*i+j] = (mbr.Data[i] >> j) & 0x01;
		}
	}

	OnModbusReadBitsSuccess(mbr, bitStatus, numBits);
}

/// <ModbusReadBits>
void OnModbusReceiveBitsException(struct ModbusApHeader mbap, enum ModbusException ex)
{
	OnModbusReadBitsFailed(Exception, ex, mbap);
}





// REGION: ModbusReadRegisters -------------------------------------------------------
/// <ModbusReadRegisters>
void ModbusReadRegisters(word address, word count)	// 16 bit
{
	const byte length = __size_of(struct ModbusReqRead);
	const byte funcCode = 0x03;
	byte buffer[length];
	struct ModbusReqRead mbr;
	
	ModbusMakeHeader(mbr.Header, length);
	// Payload
	mbr.Header.FuncCode = funcCode;	// [1] Function Code; 3: Read Holding Registers (AI), 4: Read Input Registers (AIO), seems to be the same for WAGO 750-881
	mbr.Address = address;			// [2] Start address
	mbr.Count = count;				// [2] Number of items; 1:max 125=0x7D
	
	memcpy_h2n(buffer, mbr);
	ModbusSend(buffer, length, mbr.Header.TxID);
}

/// <ModbusReadRegisters>
void OnModbusReceiveRegisters(byte buffer[])
{
	struct ModbusResReceiveRegisters mbr;
	word numRegs;

	memcpy_n2h(mbr, buffer);
	numRegs = (gQueueAck[mbr.Header.TxID].Buffer[10] << 8) + gQueueAck[mbr.Header.TxID].Buffer[11];

	OnModbusReadRegistersSuccess(mbr, numRegs);
}

/// <ModbusReadRegisters>
void OnModbusReceiveRegistersException(struct ModbusApHeader mbap, enum ModbusException ex)
{
	OnModbusReadBitsFailed(Exception, ex, mbap);
}





// REGION: ModbusWriteBit -------------------------------------------------------------
/// <ModbusWriteBit>
void ModbusWriteBit(word address, byte value)
{
	const byte length = __size_of(struct ModbusReqWriteSingle);
	const byte funcCode = 0x05;
	byte buffer[length];
	struct ModbusReqWriteSingle mbw;

	if (value >= 1)
		value = 0xFF;

	ModbusMakeHeader(mbw.Header, length);
	// Payload
	mbw.Header.FuncCode = funcCode;	// [1] Function Code; 5: Write Single Coil (DO)
	mbw.Address = address;			// [2] Output address
	mbw.Value = value << 8;			// [2] Output value (0x0000: Off, 0xFF00: On)
	
	memcpy_h2n(buffer, mbw);
	ModbusSend(buffer, length, mbw.Header.TxID);
}

/// <ModbusWriteBit>
void OnModbusConfirmBit(byte buffer[])
{
	struct ModbusResConfirmSingle mbc;

	memcpy_n2h(mbc, buffer);

	OnModbusWriteBitSuccess(mbc);
}

/// <ModbusWriteBit>
void OnModbusConfirmBitException(struct ModbusApHeader mbap, enum ModbusException ex)
{
	OnModbusReadBitsFailed(Exception, ex, mbap);
}





// REGION: ModbusWriteRegister ------------------------------------------------------
/// <ModbusWriteRegister>
void ModbusWriteRegister(word address, int value)
{
	const byte length = __size_of(struct ModbusReqWriteSingle);
	const byte funcCode = 0x06;
	byte buffer[length];
	struct ModbusReqWriteSingle mbw;

	ModbusMakeHeader(mbw.Header, length);
	// Payload
	mbw.Header.FuncCode = funcCode;	// [1] Function Code; 5: Write Single Register (AO)
	mbw.Address = address;			// [2] Output address
	mbw.Value = value;				// [2] Output value
	
	memcpy_h2n(buffer, mbw);
	ModbusSend(buffer, length, mbw.Header.TxID);
}

/// <ModbusWriteRegister>
void OnModbusConfirmRegister(byte buffer[])
{
	struct ModbusResConfirmSingle mbc;

	memcpy_n2h(mbc, buffer);

	OnModbusWriteRegisterSuccess(mbc);
}

/// <ModbusWriteRegister>
void OnModbusConfirmRegisterException(struct ModbusApHeader mbap, enum ModbusException ex)
{
	OnModbusReadBitsFailed(Exception, ex, mbap);
}







// REGION: ModbusWriteBits ----------------------------------------------------------
/// <ModbusWriteBits>
void ModbusWriteBits(word address, word count, byte values[])
{
	const word maxLength = __size_of(struct ModbusReqWriteBits);
	const byte funcCode = 0x0F;
	byte buffer[maxLength];
	struct ModbusReqWriteBits mbw;
	byte dataLength;
	byte overallLength;
	word i;

	dataLength = _ceil(count / 8.0);
	overallLength = maxLength - 1968/8 + dataLength;
	ModbusMakeHeader(mbw.Header, overallLength);
	// Payload
	mbw.Header.FuncCode = funcCode;	// [1] Function Code; 15: Write Multiple Bits (DOs)
	mbw.Address = address;			// [2] Output address
	mbw.Count = count;				// [2] Number of items; 1:max 1968=0x7B0
	mbw.ByteCount = dataLength;		// [1] Number of bytes; = ceil(count/8)
	memcpy(mbw.Data, values, dataLength);	// this is 1 memcpy too much -.-

	memcpy_h2n(buffer, mbw);
	ModbusSend(buffer, overallLength, mbw.Header.TxID);
}

/// <ModbusWriteBits>
void ModbusWriteBitsB(word address, word count, byte values[])
{
	byte buffer[2];	// length
	word length;
	dword ellCount;
	dword i;
	dword j;
	///char str1[20*2], str2[20*3];

	length = (word)_ceil(count / 8.0);
	///writeLineEx(0, 3, "count: %d; length: %d", count, length);

	if (count % 8 != 0)
		length--;

	for (i = 0; i < length; i++)
	{
		buffer[i] = 0;
		for (j = 0; j < 8; j++)
		{
			buffer[i] |= (values[i*8 + j] & 0x01) << j;
			///writeLineEx(0, 3, "j: %d; indx: %d; value: %d; mask: %X", j, i*8 + j, values[i*8 + j], (0x01 << j));
		}
		///writeLineEx(0, 3, "%d: %X", i, buffer[i]);
	}
	for (j = 0; j < count % 8; j++)	// wont be executed if there is no remainder
	{
		///writeLineEx(0, 3, "j: %d; indx: %d; value: %d; mask: %X", j, (length-1)*8 + j, values[(length-1)*8 + j], (0x01 << j));
		buffer[length] |= (values[(length)*8 + j] & 0x01) << j;
	}
	///writeLineEx(0, 3, "%d: %X", length-1, buffer[length-1]);

	///hbin_to_strhex(values, str1);
	///bin_to_strhex(buffer, str2);
	///writeLineEx(0, 1, "<%NODE_NAME%> ModbusWriteBitsB: Encoded %s to %s", str1, str2);
	ModbusWriteBits(address, count, buffer);
}

/// <ModbusWriteBits>
void OnModbusConfirmBits(byte buffer[])
{
	struct ModbusResConfirmMultiple mbc;

	memcpy_n2h(mbc, buffer);

	OnModbusWriteBitsSuccess(mbc);
}

/// <ModbusWriteBits>
void OnModbusConfirmBitsException(struct ModbusApHeader mbap, enum ModbusException ex)
{
	OnModbusReadBitsFailed(Exception, ex, mbap);
}






// REGION: ModbusWriteRegisters -------------------------------------------------------
/// <ModbusWriteRegisters>
void ModbusWriteRegisters(word address, word count, word values[])
{
	const word maxLength = __size_of(struct ModbusReqWriteRegisters);
	const byte funcCode = 0x10;
	byte buffer[maxLength];
	struct ModbusReqWriteRegisters mbw;
	byte dataLength;
	word overallLength;
	word i;

	dataLength = 2 * count;
	overallLength = maxLength - 2*123 + dataLength;

	ModbusMakeHeader(mbw.Header, overallLength);
	// Payload
	mbw.Header.FuncCode = funcCode;	// [1] Function Code; 16: Write Multiple Registers (AOs)
	mbw.Address = address;			// [2] Output address
	mbw.Count = count;				// [2] Number of items; 1:max 123=0x7B
	mbw.ByteCount = dataLength;		// [1] Number of bytes; = 2 * count

	for (i = 0; i < dataLength; i++)
		mbw.Data[i] = values[i];

	memcpy_h2n(buffer, mbw);
	ModbusSend(buffer, overallLength, mbw.Header.TxID);
}

/// <ModbusWriteRegisters>
void OnModbusConfirmRegisters(byte buffer[])
{
	struct ModbusResConfirmMultiple mbc;

	memcpy_n2h(mbc, buffer);

	OnModbusWriteRegistersSuccess(mbc);
}

/// <ModbusWriteRegisters>
void OnModbusConfirmRegistersException(struct ModbusApHeader mbap, enum ModbusException ex)
{
	OnModbusReadBitsFailed(Exception, ex, mbap);
}






// REGION: ModbusWriteMasks ------------------------------------------------------------
/// <ModbusWriteMasks>
void ModbusWriteMasks(word address, word and, word or)
{
	const word length = __size_of(struct ModbusReqWriteMasks);
	const byte funcCode = 0x16;
	byte buffer[length];
	struct ModbusReqWriteMasks mbw;

	ModbusMakeHeader(mbw.Header, length);
	// Payload
	mbw.Header.FuncCode = funcCode;	// [1] Function Code; 22: Mask Write Registers (AO)
	mbw.Address = address;			// [2] Output address
	mbw.And = and;					// [2] AND mask
	mbw.Or = or;					// [2] OR mask

	memcpy_h2n(buffer, mbw);
	ModbusSend(buffer, length, mbw.Header.TxID);
}

/// <ModbusWriteMasks>
void OnModbusConfirmMasks(byte buffer[])
{
	struct ModbusResConfirmMasks mbc;

	memcpy_n2h(mbc, buffer);

	OnModbusWriteMasksSuccess(mbc);
}

/// <ModbusWriteMasks>
void OnModbusConfirmMasksException(struct ModbusApHeader mbap, enum ModbusException ex)
{
	OnModbusReadBitsFailed(Exception, ex, mbap);
}





// REGION: ModbusReadWriteRegisters -------------------------------------------------------
/// <ModbusReadWriteRegisters>
void ModbusReadWriteRegisters(word readAddress, word readCount, word writeAddress, word writeCount, int values[])
{
	const word maxLength = __size_of(struct ModbusReqReadWriteRegisters);
	const byte funcCode = 0x17;
	byte buffer[maxLength];
	struct ModbusReqReadWriteRegisters mbw;
	byte dataLength;
	word overallLength;
	word i;

	dataLength = 2 * writeCount;
	overallLength = maxLength - 2*121 + dataLength;

	ModbusMakeHeader(mbw.Header, overallLength);
	// Payload
	mbw.Header.FuncCode = funcCode;	// [1] Function Code; 16: Write Multiple Registers (AOs)
	mbw.ReadAddress = readAddress;	// [2] Input address
	mbw.ReadCount = readCount;		// [2] Number of items; 1:max 125=0x7D
	mbw.WriteAddress = writeAddress;// [2] Output address
	mbw.WriteCount = writeCount;	// [2] Number of items; 1:max 121=0x79
	mbw.ByteCount = dataLength;		// [1] Number of bytes; = 2 * count

	for (i = 0; i < dataLength; i++)
		mbw.Data[i] = values[i];

	memcpy_h2n(buffer, mbw);
	ModbusSend(buffer, overallLength, mbw.Header.TxID);
}

/// <ModbusReadWriteRegisters>
void OnModbusReceiveConfirmRegisters(byte buffer[])
{
	byte numRegs;
	struct ModbusResReceiveRegisters mbr;
	memcpy_n2h(mbr, buffer);

	numRegs = (gQueueAck[mbr.Header.TxID].Buffer[10] << 8) + gQueueAck[mbr.Header.TxID].Buffer[11];

	OnModbusReadRegistersSuccess(mbr, numRegs);
}

/// <ModbusReadWriteRegisters>
void OnModbusReceiveConfirmRegistersException(struct ModbusApHeader mbap, enum ModbusException ex)
{
	OnModbusReadBitsFailed(Exception, ex, mbap);
}







// ------------------------------------------------------------------------------------
// REGION: OnModbusReceive ------------------------------------------------------------
/// <-OnModbusReceive>
void OnModbusReceive(dword socket, long result, dword address, dword port, byte buffer[], dword size)
{
	if (result == 0)
	{
		if (size == 0)
		{
			// Size of zero indicates that the socket was closed by the communication peer.
			writeLineEx(0, 2, "<%NODE_NAME%> OnTcpReceive: Socket closed by peer");
			gSocket.Close();
			gSocketState = CLOSED;
		}
		else
		{
			// Sucessfully received some bytes over the TCP/IP connection.
			OnModbusReceive2(buffer, size);
		}
	}
	else
	{
		gIpLastErr = gSocket.GetLastSocketError();
		gSocket.GetLastSocketErrorAsString(gIpLastErrStr, elcount(gIpLastErrStr));
		writeLineEx(0, 2, "<%NODE_NAME%> OnTcpReceive error (%d): %s", gIpLastErr, gIpLastErrStr);
		gSocket.Close();
		gSocketState = CLOSED;
	}
}

/// <-OnModbusReceive>
void OnModbusReceive2(byte buffer[], dword size)
{
	struct ModbusApHeader mbap;
	int offset;
	char str[3*20];

	if (size < 8)						// No complete Modbus Application Header
		return;

	offset = 0;
	//write("OnModbusReceive2: size = %d", size);
	do
	{
		//write("OnModbusReceive2: offset pre  = %d", offset);
		memcpy_n2h(mbap, buffer, offset);
		OnModbusReceive2OnePacket(buffer, offset, mbap);
		
		offset += __offset_of(struct ModbusApHeader, UnitID) + mbap.Length;
		//write("OnModbusReceive2: offset post = %d. %d <= %d?", offset, offset, size-8);
	}
	while(offset <= size-8);			// We need at least 8 bytes for a new packet
	//write("OnModbusReceive2: yes. finished", offset);

	if (offset != size)	// Can be removed.
	{
		bin_to_strhex(buffer, str);
		writeLineEx(0, 3, "OnModbusReceive2: Error while going through receive buffer. Our final offset is %d, but the size of the buffer is %d! Buffer: %s", offset, size, str);
		runError(1002, 1);
	}
}

/// <-OnModbusReceive>
void OnModbusReceive2OnePacket(byte buffer[], int offset, struct ModbusApHeader mbap)
{
	// Test transaction identifier?
	// Test unit/device identifier?
	word i;					// counter
	word length;			// length of current packet
	byte mbuffer[__size_of(struct ModbusResReceiveRegisters)];		// second buffer where we copy the message. This way the user won't overwrite other packages.

	length = __offset_of(struct ModbusApHeader, UnitID) + mbap.Length;
	// We cannot check this properly anymore. We have to trust the TCP/UDP stack and the sender... *sigh*
	if (elCount(buffer) < offset + length)		// TCP packet not as large enough
	{
		writeLineEx(0, 3, "<%NODE_NAME%> OnModbusReceive Error: Modbus packet did not fit into Buffer: buffer length = %d, packet length = %d, offset = %d", elCount(buffer), __offset_of(struct ModbusApHeader, UnitID) + mbap.Length, offset);
		// I REALLY don't want to assemble the two package fragments.
		runError(1001,1);
		return;
	}
	if (mbap.Protocol != 0)						// Protocol is not Modbus (0x0000). Wayne.
	{
		writeLineEx(0, 2, "<%NODE_NAME%> OnModbusReceive Error: Tcp packet is no Modbus packet: Protocol = %d", mbap.Protocol);
		return;
	}
	// MBAP Header is OK :) Go on
	
	if (!gQueueSent.ContainsKey(mbap.TxID))		// We don't wait for this message!
		return;

	memcpy(gQueueAck[mbap.TxID], gQueueSent[mbap.TxID]);
	gQueueSent.Remove(mbap.TxID);

	if (mbap.FuncCode > 0x80)					// Oh no, we got a exception!
	{
		OnModbusReceive2Exceptions(buffer[offset+08], mbap);

		gQueueAck.Remove(mbap.TxID);			// Remove from acknowledge queue

		return;
	}


	// Copy the message
	memcpy_off(mbuffer, 0, buffer, offset, length);

	// Let's give the PDU to the corresponding function
	switch (mbap.FuncCode)
	{
		case 0x01:
		case 0x02:
			OnModbusReceiveBits(mbuffer);
			break;
		case 0x03:
		case 0x04:
			OnModbusReceiveRegisters(mbuffer);
			break;
		case 0x05:
			OnModbusConfirmBit(mbuffer);
			break;
		case 0x06:
			OnModbusConfirmRegister(mbuffer);
			break;
		case 0x0F:
			OnModbusConfirmBits(mbuffer);
			break;
		case 0x10:
			OnModbusConfirmRegisters(mbuffer);
			break;
		case 0x16:
			OnModbusConfirmMasks(mbuffer);
			break;
		case 0x17:
			OnModbusReceiveConfirmRegisters(mbuffer);
			break;
		default:
			writeLineEx(0, 3, "We received funcCode 0x%X?", mbap.FuncCode);
	}

	gQueueAck.Remove(mbap.TxID);		// Remove from acknowledge queue
}

/// <-OnModbusReceive>
void OnModbusReceive2Exceptions(byte exCode, struct ModbusApHeader mbap)
{
	enum ModbusException ex;
	ex = (enum ModbusException)exCode;
	switch(exCode)		// Let's have a look at the reason
	{
		case 0x01:		// Illegal function code: Implementation failure!
			writeLineEx(0, 3, "<%NODE_NAME%> Exception: Illegal function code (0x01). The function code %d is unknown by the server.", mbap.FuncCode & 0x0F);
			break;
		case 0x02:		// Illegal data address: Configuration failure!
			writeLineEx(0, 3, "<%NODE_NAME%> Exception: Illegal data address (0x02). Please check your configuration!");
			break;
		case 0x03:		// Illegal data value: Depends.
			writeLineEx(0, 3, "<%NODE_NAME%> Exception: Illegal data value (0x03).");
			break;
		case 0x04:		// Server failure: We can't do anything about that, can we?
			writeLineEx(0, 3, "<%NODE_NAME%> Exception: Server failure (0x04). The server failed during execution.");
			break;
		case 0x05:		// Acknowledge: That's ok.
			writeLineEx(0, 1, "<%NODE_NAME%> Exception: Acknowledge (0x05). The server simply needs more time to generate the response.");
			break;
		case 0x06:		// Server busy: We should resend the request.
			writeLineEx(0, 3, "<%NODE_NAME%> Exception: Server busy (0x06). The request could not be accepted.");
			break;
		case 0x0A:		// Gateway problem: We don't have gateways :)
			writeLineEx(0, 3, "<%NODE_NAME%> Exception: Gateway problem (0x0A). Gateway paths not available.");
			break;
		case 0x0B:		// Gateway problem: We don't have gateways :)
			writeLineEx(0, 3, "<%NODE_NAME%> Exception: Gateway problem (0x0B). The targeted device failed to respond.");
			break;
	}

	switch (mbap.FuncCode)
	{
		case 0x81:
		case 0x82:
			OnModbusReceiveBitsException(mbap, ex);
			break;
		case 0x83:
		case 0x84:
			OnModbusReceiveRegistersException(mbap, ex);
			break;
		case 0x85:
			OnModbusConfirmBitException(mbap, ex);
			break;
		case 0x86:
			OnModbusConfirmRegisterException(mbap, ex);
			break;
		case 0x8F:
			OnModbusConfirmBitsException(mbap, ex);
			break;
		case 0x90:
			OnModbusConfirmRegistersException(mbap, ex);
			break;
		case 0x96:
			OnModbusConfirmMasksException(mbap, ex);
			break;
		case 0x97:
			OnModbusReceiveConfirmRegistersException(mbap, ex);
			break;
	}
}


// ------------------------------------------------------------------------------------
// REGION: ModbusSend -----------------------------------------------------------------
/// <-ModbusSend>
void ModbusSend(byte buffer[], word length, word TxID)
{
	struct QueueElement qe;
	qe.Length = length;
	memcpy(qe.Buffer, buffer, length);

	memcpy(gQueuePending[TxID], qe);
	
	if (gQueuePending.Size() == 1 && gQueueSent.Size() == 0)			// start timer at beginning
		setTimerCyclic(gtRobin, 1);

}

/// <-ModbusSend>
on timer gtRobin
{
	struct ModbusApHeader mbap;
	struct QueueElement qe;
	enum ModbusRequestError reqError;

	//writeLineEx(0, 1, "Queue Sent: %d, Queue Pending: %d, Queue Ack: %d", gQueueSent.Size(), gQueuePending.Size(), gQueueAck.Size());

	// First: check timeouts = packets that were sent in previous run and not removed by response
	for (long TxID : gQueueSent)
	{
		if (++gQueueSent[TxID].TimeoutTicks < @sysvar::Config::Modbus::RequestTimeout)		// not timed out yet
			continue;
																							// timed out!
		if (++gQueueSent[TxID].Timeouts < @sysvar::Config::Modbus::MaxTransmissionCount)	// if we may resend it
		{
			gQueueSent[TxID].TimeoutTicks = 0;
			ModbusSnd(gQueueSent[TxID].Buffer, gQueueSent[TxID].Length);					// resend it
			reqError = Timeout;
			ModbusRecv();
		}
		else																				// we will NOT resend it
		{
			reqError = FinalTimeout;
		}

		memcpy_n2h(mbap, qe.Buffer);
		switch(mbap.FuncCode)											// throw an error in each case
		{
			case ReadBits1:
			case ReadBits2:
				OnModbusReadBitsFailed(reqError, None, mbap);
				break;
			case ReadRegisters1:
			case ReadRegisters2:
				OnModbusReadRegistersFailed(reqError, None, mbap);
				break;
			case WriteBit:
				OnModbusWriteBitFailed(reqError, None, mbap);
				break;
			case WriteRegister:
				OnModbusWriteRegisterFailed(reqError, None, mbap);
				break;
			case WriteBits:
				OnModbusWriteBitsFailed(reqError, None, mbap);
				break;
			case WriteRegisters:
				OnModbusWriteRegistersFailed(reqError, None, mbap);
				break;
			case MaskRegister:
				OnModbusWriteMasksFailed(reqError, None, mbap);
				break;
			case ReadWriteRegisters:
				OnModbusReadWriteRegistersFailed(reqError, None, mbap);
				break;
		}
		
		if (reqError == FinalTimeout)									// remove the packet from queue
			gQueueSent.Remove(TxID);									// wait until here to let the methods access the request
	}

	// Second: send new packets
	for (long TxID : gQueuePending)
	{
		if (gQueueSent.Size() > 4)		// Wago 750-881 cannot handle more than 5 messages at a time :(
			continue;

		ModbusSnd(gQueuePending[TxID].Buffer, gQueuePending[TxID].Length);	// send packet
		
		memcpy(gQueueSent[TxID], gQueuePending[TxID]);						// move packet to sent queue
		gQueuePending.Remove(TxID);
		ModbusRecv();
	}

	if (gQueueSent.Size() == 0)					// Stop timer to reduce latency of first packet
		this.Cancel();
}