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

variables
{
	const word gMaxPacketLength = __size_of(struct ModbusReqWriteRegisters);
	
	msTimer gtRobin;				// Timer that sends the packets and watches for timeouts
	word gTxID = 0x0000;			// Transaction Identifier for Modbus. Used as index for gQueue

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

	char ModbusExceptions[11][72] = {
		"Illegal func code (0x01). The function code is unknown by the server",
		"Illegal data address (0x02). Please check your configuration",
		"Illegal data value (0x03)",
		"Server failure (0x04). The server failed during execution",
		"Acknowledge (0x05). The server needs more time to generate the response",
		"Server busy (0x06). The request could not be accepted",
		"",
		"",
		"",
		"Gateway problem (0x0A). Gateway paths not available",
		"Gateway problem (0x0B). The targeted device failed to respond"
	};
}

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

	writeDbg(MbInfo, "Connecting to %s:%d", ip, @sysvar::Config::Modbus::Port);
	ModbusConnectTo(ip, @sysvar::Config::Modbus::Port);
}



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

	mbr.Address = address;
	mbr.Count = count;

	writeDbg(MbDebug, "Sending 'Read Bits' (0x01) command. Addr: 0x%04X, Count: %d", address, count);

	memcpy_h2n(buffer, mbr);
	ModbusSend(buffer, length, funcCode);
}

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

	memcpy_n2h(mbres, buffer);
	memcpy_n2h(mbreq, gQueueAck[mbres.Header.TxID].Buffer);

	writeDbg(MbDebug, "Received %d bits from 0x%04X", mbreq.Count, mbreq.Address);

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

	OnModbusReadBitsSuccess(mbres, bitStatus, mbreq);
}

/// <ModbusReadBits>
void OnModbusReceiveBitsException(struct ModbusApHeader mbap, enum ModbusException ex)
{
	writeDbg(MbError, "Received an Exception while reading bits: %s", ModbusExceptions[ex-1]);
	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;

	mbr.Address = address;			// [2] Start address
	mbr.Count = count;				// [2] Number of items; 1:max 125=0x7D

	writeDbg(MbDebug, "Sending 'Read Registers' (0x03) command. Addr: 0x%04X, Count: %d", address, count);

	memcpy_h2n(buffer, mbr);
	ModbusSend(buffer, length, funcCode);
}

/// <ModbusReadRegisters>
void OnModbusReceiveRegisters(byte buffer[])
{
	struct ModbusResReceiveRegisters mbres;
	struct ModbusReqRead mbreq;

	memcpy_n2h(mbres, buffer);
	memcpy_n2h(mbreq, gQueueAck[mbres.Header.TxID].Buffer);

	writeDbg(MbDebug, "Received %d registers from 0x%04X", mbreq.Count, mbreq.Address);

	OnModbusReadRegistersSuccess(mbres, mbreq);
}

/// <ModbusReadRegisters>
void OnModbusReceiveRegistersException(struct ModbusApHeader mbap, enum ModbusException ex)
{
	writeDbg(MbError, "Received an Exception while reading registers: %s", ModbusExceptions[ex-1]);
	OnModbusReadRegistersFailed(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;

	mbw.Address = address;			// [2] Output address
	mbw.Value = value << 8;			// [2] Output value (0x0000: Off, 0xFF00: On)

	writeDbg(Debug, "Sending 'Write Bit' (0x05) command. Addr: 0x%04X, Value: 0x%02X", address, value);
	
	memcpy_h2n(buffer, mbw);
	ModbusSend(buffer, length, funcCode);
}

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

	memcpy_n2h(mbc, buffer);

	writeDbg(MbDebug, "Set bit at 0x%04X to %d", mbc.Address, mbc.Value);

	OnModbusWriteBitSuccess(mbc);
}

/// <ModbusWriteBit>
void OnModbusConfirmBitException(struct ModbusApHeader mbap, enum ModbusException ex)
{
	writeDbg(MbError, "Received an Exception while writing bit: %s", ModbusExceptions[ex-1]);
	OnModbusWriteBitFailed(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;

	mbw.Address = address;			// [2] Output address
	mbw.Value = value;				// [2] Output value

	writeDbg(MbDebug, "Sending 'Write Register' (0x06) command. Addr: 0x%04X, Value: 0x%02X", address, value);

	memcpy_h2n(buffer, mbw);
	ModbusSend(buffer, length, funcCode);
}

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

	memcpy_n2h(mbc, buffer);

	writeDbg(MbDebug, "Set register at 0x%04X to %d", mbc.Address, mbc.Value);

	OnModbusWriteRegisterSuccess(mbc);
}

/// <ModbusWriteRegister>
void OnModbusConfirmRegisterException(struct ModbusApHeader mbap, enum ModbusException ex)
{
	writeDbg(MbError, "Received an Exception while writing register: %s", ModbusExceptions[ex-1]);
	OnModbusWriteRegisterFailed(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;

	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 -.-

	writeDbg(MbDebug, "Sending 'Write Bits' (0x0F) command. Addr: 0x%04X, Count: %d", address, count);

	memcpy_h2n(buffer, mbw);
	ModbusSend(buffer, overallLength, funcCode);
}

/// <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);
	writeDbg(AlgoDebug, "ModbusWriteBitsB: 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;
			writeDbg(AlgoDebug, "ModbusWriteBitsB: j: %d; indx: %d; value: %d; mask: %X", j, i*8 + j, values[i*8 + j], (0x01 << j));
		}
		writeDbg(AlgoDebug, "ModbusWriteBitsB: %d: %X", i, buffer[i]);
	}
	for (j = 0; j < count % 8; j++)	// wont be executed if there is no remainder
	{
		writeDbg(AlgoDebug, "ModbusWriteBitsB: 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;
	}
	writeDbg(AlgoDebug, "ModbusWriteBitsB: %d: %X", length-1, buffer[length-1]);

	hbin_to_strhex(values, str1);
	bin_to_strhex(buffer, str2);
	writeDbg(AlgoDebug, "ModbusWriteBitsB: Encoded %s to %s", str1, str2);
	ModbusWriteBits(address, count, buffer);
}

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

	memcpy_n2h(mbc, buffer);

	writeDbg(MbDebug, "Updated &d bits at 0x%04X", mbc.Count, mbc.Address);

	OnModbusWriteBitsSuccess(mbc);
}

/// <ModbusWriteBits>
void OnModbusConfirmBitsException(struct ModbusApHeader mbap, enum ModbusException ex)
{
	writeDbg(MbError, "Received an Exception while writing bits: %s", ModbusExceptions[ex-1]);
	OnModbusWriteBitsFailed(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;

	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, funcCode);
}

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

	memcpy_n2h(mbc, buffer);

	writeDbg(MbDebug, "Updated &d registers at 0x%04X", mbc.Count, mbc.Address);

	OnModbusWriteRegistersSuccess(mbc);
}

/// <ModbusWriteRegisters>
void OnModbusConfirmRegistersException(struct ModbusApHeader mbap, enum ModbusException ex)
{
	writeDbg(MbError, "Received an Exception while writing registers: %s", ModbusExceptions[ex-1]);
	OnModbusWriteRegistersFailed(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;

	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, funcCode);
}

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

	memcpy_n2h(mbc, buffer);

	writeDbg(MbDebug, "Applied masks at 0x%04X", mbc.Address);

	OnModbusWriteMasksSuccess(mbc);
}

/// <ModbusWriteMasks>
void OnModbusConfirmMasksException(struct ModbusApHeader mbap, enum ModbusException ex)
{
	writeDbg(MbError, "Received an Exception while applying masks: %s", ModbusExceptions[ex-1]);
	OnModbusWriteMasksFailed(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;

	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, funcCode);
}

/// <ModbusReadWriteRegisters>
void OnModbusReceiveConfirmRegisters(byte buffer[])
{
	struct ModbusResReceiveRegisters mbres;
	struct ModbusReqRead mbreq;

	memcpy_n2h(mbres, buffer);
	memcpy_n2h(mbreq, gQueueAck[mbres.Header.TxID].Buffer);

	writeDbg(MbDebug, "Wrote some registers and received %d registers from 0x%04X", mbreq.Count, mbreq.Address);

	OnModbusReadRegistersSuccess(mbres, mbreq);
}

/// <ModbusReadWriteRegisters>
void OnModbusReceiveConfirmRegistersException(struct ModbusApHeader mbap, enum ModbusException ex)
{
	writeDbg(MbError, "Received an Exception while reading and writing registers: %s", ModbusExceptions[ex-1]);
	OnModbusWriteRegistersFailed(Exception, ex, mbap);
}







// ------------------------------------------------------------------------------------
// REGION: OnModbusReceive ------------------------------------------------------------
/// <-OnModbusReceive>
void OnModbusReceive(dword socket, long result, dword address, dword port, byte buffer[], dword size)
{
	writeDbg(ConnDebug, "OnModbusReceive: Received %d bytes", size);
	if (result == 0)
	{
		if (size == 0)
		{
			// Size of zero indicates that the socket was closed by the communication peer.
			writeDbg(ConnWarning, "OnModbusReceive: Socket closed by peer");
			ModbusDisconnect();
		}
		else
		{
			// Sucessfully received some bytes over the TCP/IP connection.
			OnModbusReceive2(buffer, size);
		}
	}
	else
	{
		gIpLastErr = ModbusGetLastConnectionError(gIpLastErrStr);
		writeDbg(ConnError, "OnModbusReceive error (%d): %s", gIpLastErr, gIpLastErrStr);
		ModbusDisconnect();
	}
}

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

	if (offset != size)	// Can be removed.
	{
		bin_to_strhex(buffer, str);
		writeDbg(ConnError, "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);
		OnModbusClientPanics(ParsingBuffer);
	}
}

/// <-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 (mbap.Protocol != 0)						// Protocol is not Modbus (0x0000). Wayne.
	{
		writeDbg(ConnDebug, "OnModbusReceive2OnePacket: packet is no Modbus packet: Protocol = %d", mbap.Protocol);
		return;
	}
	if (elCount(buffer) < offset + length)		// packet larger than the (rest of the) buffer
	{
		writeDbg(ConnError, "OnModbusReceive2OnePacket: 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.
		OnModbusClientPanics(ModbusPackageWasSplit);
		return;
	}
	// MBAP Header is OK :) Go on
	
	if (!gQueueSent.ContainsKey(mbap.TxID))		// We don't wait for this message!
		return;

	//write("Received TxID: %d", mbap.TxID);
	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:
			writeDbg(MbError, "OnModbusReceive2OnePacket: 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 (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, byte funcCode)
{
	struct QueueElement qe;
	word TxID;

	TxID = gTxID++;

	qe.FuncCode = funcCode;
	qe.Length = length;
	memcpy(qe.Buffer, buffer, length);

	memcpy(gQueuePending[TxID], qe);
	writeDbg(ConnDebug, "Appended packet 0x%04X to pending queue", TxID);

	if (gQueuePending.Size() == 1 && gQueueSent.Size() == 0 && gSocketState == OK)	// start timer if connection established
		ModbusStartQueue();
}

void ModbusStartQueue()
{
	writeDbg(ConnDebug, "Starting Timer gtRobin");
	setTimerCyclic(gtRobin, 1);
}

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

	writeDbg(ConnDebug, "gtRobin: 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
		{
			writeDbg(ConnInfo, "Packet 0x%04X timed out! Retrying...", TxID);
			gQueueSent[TxID].TimeoutTicks = 0;
			EthSetTokenInt(gPacket,		"modbus",	"TxId",		TxID);
			EthSetTokenInt(gPacket,		"modbus",	"FuncCode",	gQueueSent[TxID].FuncCode);
			ModbusSnd(gQueueSent[TxID].Buffer, gQueueSent[TxID].Length);					// resend it
			reqError = Timeout;
			ModbusRecv();
		}
		else																				// we will NOT resend it
		{
			writeDbg(ConnWarning, "Packet 0x%04X timed out! Giving up", TxID);
			reqError = FinalTimeout;
		}

		memcpy_n2h(mbap, gQueueSent[TxID].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;

		// if packet was sent or the socket is not currently being opened
		if (ModbusSnd(gQueuePending[TxID].Buffer, gQueuePending[TxID].Length) == 0 || gSocketState != NULL)
		{
			memcpy(gQueueSent[TxID], gQueuePending[TxID]);				// move packet to sent queue
			gQueuePending.Remove(TxID);
			ModbusRecv();
		}
	}

	if (gSocketState == ERROR || gQueueSent.Size() == 0 && gQueuePending.Size() == 0)	// Stop timer to reduce latency of first packet
	{
		writeDbg(ConnDebug, "Stopping Timer gtRobin");
		this.Cancel();
	}
}