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

variables
{
	struct ModbusApHeader {
		word TxID;
		word Protocol;
		word Length;
		byte UnitID;
		byte FuncCode;
	};
	struct ModbusRead {
		struct ModbusApHeader Header;
		word Address;
		word Count;
	};
	struct ModbusWriteSingle {
		struct ModbusApHeader Header;
		word Address;
		word Value;
	};
	struct ModbusWriteMultiple {
		struct ModbusApHeader Header;
		word Address;
		word Count;
		byte ByteCount;
	};
}

// TODO: Add request timeout timers

void ModbusInit(char Remote_IP[], word Remote_Port)
{
	ModbusConnectTo("192.168.1.3", 502);
}

void ModbusMakeHeader(struct ModbusApHeader mbap, byte length)
{
	mbap.TxID = 0x1234;			// [2] Transaction ID. Not needed here?
	mbap.Protocol = 0x0000;		// [2] Protocol ID = 0 = Modbus
	mbap.Length = length-6;		// [2] Length; Number of bytes following
	mbap.UnitID = 0xFF;			// [1] Unit identifier; not relevant

	// 2:Read DIO; 3:Read AIO; 4:Read AI; 5:Write 1 DO; 6:Write 1 AO; 7: Read Exeption, 15:Write n DO; 16:Write n AO;
}



void ModbusReadBits(word address, word count)
{
	const byte length = 12;
	byte buffer[length];
	struct ModbusRead mbr;
	
	ModbusMakeHeader(mbr.Header, length);
	// Payload
	mbr.Header.FuncCode = 0x01;		// [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);
	ModbusRecv();
}

void OnModbusReceiveBits(byte response[], dword size)
{
	writeLineEx(0, 1, "<%BASE_FILE_NAME%> OnModbusReceiveBits: Received %d bytes: %d...", response[0], response[1]);
}


void ModbusReadRegisters(word address, word count)	// 16 bit
{
	const byte length = 12;
	byte buffer[length];
	struct ModbusRead mbr;
	
	ModbusMakeHeader(mbr.Header, length);
	// Payload
	mbr.Header.FuncCode = 0x03;		// [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 2000=0x7D0
	
	memcpy_h2n(buffer, mbr);
	ModbusSend(buffer);
	ModbusRecv();
}

void OnModbusReceiveRegisters(byte response[], dword size)
{
	writeLineEx(0, 1, "<%BASE_FILE_NAME%> OnModbusReceiveRegisters: Received %d bytes: %d...", response[0]/2, response[1]<<8+response[2]);
}


void ModbusWriteBit(word address, byte value)
{
	const byte length = 12;
	byte buffer[length];
	struct ModbusWriteSingle mbw;

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

	ModbusMakeHeader(mbw.Header, length);
	// Payload
	mbw.Header.FuncCode = 0x05;		// [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);
	ModbusRecv();
}

void OnModbusConfirmBit(byte response[], dword size)
{
	writeLineEx(0, 1, "<%BASE_FILE_NAME%> OnModbusConfirmBit: Set bit 0x%X to %d", (response[0]<<8) + response[1], ((response[2]<<8) + response[3] > 0 ? 1 : 0));
}


void ModbusWriteRegister(word address, word value)
{
	const byte length = 12;
	byte buffer[length];
	struct ModbusWriteSingle mbw;

	ModbusMakeHeader(mbw.Header, length);
	// Payload
	mbw.Header.FuncCode = 0x06;		// [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);
	ModbusRecv();
}

void OnModbusConfirmRegister(byte response[], dword size)
{
	writeLineEx(0, 1, "<%BASE_FILE_NAME%> OnModbusConfirmRegister: Set register 0x%X to %d", (response[0]<<8) + response[1], (response[2]<<8) + response[3]);
}


void ModbusWriteBits(word address, word count, byte values[])
{
	const byte length = 13 + 2;	//_ceil(count / 8.0)
	byte buffer[length];
	struct ModbusWriteMultiple mbw;
	word i;

	ModbusMakeHeader(mbw.Header, length);
	// Payload
	mbw.Header.FuncCode = 0x0F;		// [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 = 2;				// [1] Number of bytes; = ceil(count/8)

	memcpy_h2n(buffer, mbw);
	
	for (i = 0; i < length-13; i++)
	{
		buffer[i+13] = values[i];
	}
	ModbusSend(buffer);
	ModbusRecv();
}

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

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

	if (ellCount % 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 < ellCount % 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]);

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

void OnModbusConfirmBits(byte response[], dword size)
{
	writeLineEx(0, 1, "<%BASE_FILE_NAME%> OnModbusConfirmBits: Set %d bits beginning with 0x%X", (response[2]<<8) + response[3], (response[0]<<8) + response[1]);
}


void ModbusWriteRegisters(word address, word count, word values[])
{
	const byte length = 13 + 2;	//13 + 2*count
	byte buffer[length];
	struct ModbusWriteMultiple mbw;
	word i;

	ModbusMakeHeader(mbw.Header, length);
	// Payload
	mbw.Header.FuncCode = 0x10;		// [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 = 2 * count;		// [1] Number of bytes; = 2 * count

	memcpy_h2n(buffer, mbw);

	for (i = 0; i < length-13; i += 2)
	{
		buffer[i+13] = values[i] >> 8;
		buffer[i+14] = values[i] & 0xFF;
	}
	ModbusSend(buffer);
	ModbusRecv();
}

void OnModbusConfirmRegisters(byte response[], dword size)
{
	writeLineEx(0, 1, "<%BASE_FILE_NAME%> OnModbusConfirmRegisters: Set %d registers beginning with 0x%X", (response[2]<<8) + response[3], (response[0]<<8) + response[1]);
}






void OnModbusReceive(byte buffer[], dword size)
{
	// Test transaction identifier?
	// Test unit/device identifier?
	char str[20*3];
	byte x[2];
	byte mbuffer[8192];
	dword i;
	struct ModbusApHeader mbap;
	
	memcpy_n2h(mbap, buffer);

	if (size < 8)						// No complete Modbus Application Header
	{
		writeLineEx(0, 2, "<%BASE_FILE_NAME%> OnModbusReceive Error: Tcp packet is too short: Length = %d", size);
		return;
	}
	if (size != 6 + mbap.Length)		// TCP packet not as large as specified in length field
	{
		writeLineEx(0, 2, "<%BASE_FILE_NAME%> OnModbusReceive Error: Size of Tcp packet is incorrect: Length = %d, has to be %d", size, 6 + mbap.Length);
		return;
	}
	if (mbap.Protocol != 0)				// Protocol is not Modbus (0x0000)
	{
		writeLineEx(0, 2, "<%BASE_FILE_NAME%> OnModbusReceive Error: Tcp packet is no Modbus packet: Protocol = %d", mbap.Protocol);
		return;
	}
	// MBAP Header is OK :) Go on


	if (mbap.FuncCode > 0x80)			// Oh no, we got a exception!
	{
		switch(buffer[08])				// Let's have a look at the reason
		{
			case 0x01:		// Illegal function code: Implementation failure!
				writeLineEx(0, 3, "<%BASE_FILE_NAME%> OnModbusReceive Exception: Illegal function code (0x01). The function code %d is unknown by the server.", buffer[7] & 0x0F);
				return;
			case 0x02:		// Illegal data address: Configuration failure!
				writeLineEx(0, 3, "<%BASE_FILE_NAME%> OnModbusReceive Exception: Illegal data address (0x02). Please check your configuration!");
				return;
			case 0x03:		// Illegal data value: Depends.
				writeLineEx(0, 3, "<%BASE_FILE_NAME%> OnModbusReceive Exception: Illegal data value (0x03).");
				return;
			case 0x04:		// Server failure: We can't do anything about that, can we?
				writeLineEx(0, 3, "<%BASE_FILE_NAME%> OnModbusReceive Exception: Server failure (0x04). The server failed during execution.");
				return;
			case 0x05:		// Acknowledge: That's ok.
				writeLineEx(0, 1, "<%BASE_FILE_NAME%> OnModbusReceive Exception: Acknowledge (0x05). The server simply needs more time to generate the response.");
				return;
			case 0x06:		// Server busy: We should resend the request.
				writeLineEx(0, 3, "<%BASE_FILE_NAME%> OnModbusReceive Exception: Server busy (0x06). The request could not be accepted.");
				return;
			case 0x0A:		// Gateway problem: We don't have gateways :)
				writeLineEx(0, 3, "<%BASE_FILE_NAME%> OnModbusReceive Exception: Gateway problem (0x0A). Gateway paths not available.");
				return;
			case 0x0B:		// Gateway problem: We don't have gateways :)
				writeLineEx(0, 3, "<%BASE_FILE_NAME%> OnModbusReceive Exception: Gateway problem (0x0B). The targeted device failed to respond.");
				return;
		}
	}


	// Extract the PDU for further processing
	size -= 8;
	for (i = 0; i < size; i++)
		mbuffer[i] = buffer[i+8];

	// Let's give the PDU to the corresponding function
	switch (mbap.FuncCode)
	{
		case 0x01:
		case 0x02:
			OnModbusReceiveBits(mbuffer, size);
			break;
		case 0x03:
		case 0x04:
			OnModbusReceiveRegisters(mbuffer, size);
			break;
		case 0x05:
			OnModbusConfirmBit(mbuffer, size);
			break;
		case 0x0F:
			OnModbusConfirmBits(mbuffer, size);
			break;
		case 0x10:
			OnModbusConfirmRegisters(mbuffer, size);
			break;
	}
}