#include "ModbusLibrary.h"

void sleep(clock_t x_ms)
{
   clock_t goal;
   goal = x_ms + clock();
   while( goal > clock() )
      ;
}

int InitializeLibrary()
{
	WSADATA data;
	FLibraryInitialized = false;

	if (WSAStartup(MAKEWORD(1,1), &data) != 0)
	{
		return EL_INITIALIZE;
	}
	FLibraryInitialized = true;
	return 0;
}

int ReleaseLibrary()
{
	if (FLibraryInitialized == false)
	{
		return 1;
	}
	WSACleanup();
	return 0;
}

char* GetErrorMessage(WORD errorCode)
{
	switch(errorCode){
		case 0:return "ModbusLibraryError: No error.";break;
		case EC_NEW_PARAMETERS:return "ModbusLibraryError: Client - New: Unexpected parameters.";break;
		case ES_NEW_PARAMETERS:return "ModbusLibraryError: Server - New: Unexpected parameters.";break;
		case EL_INITIALIZE:return "ModbusLibraryError: Cant initialize WSA library.";break;
		case EC_CONNECT_HOST:return "ModbusLibraryError: Client - Connecting: Cant find host. Probably not exist.";break;		
		case EC_CONNECT_SOCKET:return "ModbusLibraryError: Client - Connecting: Cant create socket.";break;
		case EC_CONNECT_CONNECTED:return "ModbusLibraryError: Client - Connecting: Already connected.";break;
		case EC_CONNECT_PROBLEM:return "ModbusLibraryError: Client - Connecting: Cant connect to host.";break;
		case EC_DISCONNECT_DISCONNECTED:return "ModbusLibraryError: Client - Disconnecting: Not Connected.";break;
		case EC_SENDING_DISCONNECTED:return "ModbusLibraryError: Client - Sending: Not Connected.";break;
		case EC_SENDING_PROBLEM:return "ModbusLibraryError: Client - Sending: Cant send message.";break;
		case EC_READING_DISCONNECTED:return "ModbusLibraryError: Client - Reading: Not Connected.";break;
		case EC_READING_PROBLEM:return "ModbusLibraryError: Client - Reading: Cant read message.";break;
		case EC_REQUEST_EXCEPTION:return "ModbusLibraryError: Client - Send request: Unhandled exception.";break;
		case ES_ESTABLISH_SOCKET:return "ModbusLibraryError: Server - Establishing: Cant create socket.";break;
		//case 15:return "ModbusLibraryError: Establishing: Wrong type of connection.";break;
		case ES_ESTABLISH_RUNNING:return "ModbusLibraryError: Server - Establishing: Already running.";break;
		case ES_ESTABLISH_NAMING:return "ModbusLibraryError: Server - Establishing: Cant name socket.";break;
		case ES_ESTABLISH_LISTENING:return "ModbusLibraryError: Server - Establishing: Cant listen to socket.";break;
		case ES_ESTABLISH_TREAD:return "ModbusLibraryError: Server - Establishing: Cant run server routine.";break;
		case ES_HANDLING_ACCEPT:return "ModbusLibraryError: Server - Handling: Cant accept connection.";break;
		case ES_SEND_CONNECTION:return "ModbusLibraryError: Server - Sending: Problem in connection.";break;
		case ES_SEND_PROBLEM:return "ModbusLibraryError: Server - Sending: Cant send message.";break;
		case ES_READ_CONNECTION:return "ModbusLibraryError: Server - Reading: Problem in connection.";break;
		case ES_READ_PROBLEM:return "ModbusLibraryError: Server - Reading: Cant read message.";break;
		case ES_SHUTDOWN_STOPPED:return "ModbusLibraryError: Server - Shutdown: Server is not running.";break;
		case ES_RESPONSE_EXCEPTION:return "ModbusLibraryError: Server - Send response: Unhandled exception.";break;
		case ES_REQUEST_EXCEPTION:return "ModbusLibraryError: Server - Get request: Unhandled exception.";break;
		case ES_REQUEST_DISCONNECT:return "ModbusLibraryError: Server - Get request: Not connected.";break;
		case EC_ANSWER_EXCEPTION:return "ModbusLibraryError: Client - Get answer: Unhandled exception.";break;
		case EC_ANSWER_DISCONNECT:return "ModbusLibraryError: Client - Get answer: Not connected.";break;
		case EC_SERVER_ERROR_FUNCCODE:return "ModbusLibraryError: Client - Function not supported.";break;
		case EC_SERVER_ERROR_ADDRESS:return "ModbusLibraryError: Client - Address is not valid.";break;
		case EC_SERVER_ERROR_DATA:return "ModbusLibraryError: Client - Data is not valid.";break;
		case EC_SERVER_ERROR_EXECUTION:return "ModbusLibraryError: Client - Command not executed.";break;
		default:return "ModbusLibraryError: Unhandled error.";break;
	}
}

int LastClientError(PClient client)
{
	int error = client->FLastError;
	client->FLastError = 0;
	return error;
}

int LastServerError(PServer server)
{
	int error = server->FLastError;
	server->FLastError = 0;
	return error;
}

// ***** CLIENT *****

PClient NewClient(char *ip, int port)
{
	unsigned char libError = 0;
	if (FLibraryInitialized != true)
	{
		libError = InitializeLibrary();
	}
	
	PClient newClient = (PClient)malloc(sizeof(TClient));
	newClient->FConnected = false;
	newClient->FSocket = 0;
	newClient->FLastError = libError;
	newClient->FPort = port;
	newClient->FMsgCounter = 0;
	newClient->FReadMessageSize = 0;
	newClient->FSendMessageSize = 0;
	strcpy(newClient->FIP, ip);
	if ((strcmp(ip, "") == 0) || (port <=0))
		newClient->FLastError = EC_NEW_PARAMETERS;
	return newClient;
}

int DestroyClient(PClient client)
{
	DisconnectClient(client);
	free(client);
	client = NULL;
	return 0;
}

int ConnectClient(PClient client)
{
	sockaddr_in sockName;

	if (FLibraryInitialized != true)
	{
		client->FLastError = EL_INITIALIZE;
		return EL_INITIALIZE;
	}
	if (client->FConnected == true)
	{
		client->FLastError = EC_CONNECT_CONNECTED;
		return EC_CONNECT_CONNECTED;
	}
	
	hostent *host = gethostbyname(client->FIP);
	if (host ==  NULL)
	{
		client->FLastError = EC_CONNECT_HOST;
		return EC_CONNECT_HOST;
	}
	
	if((client->FSocket = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP)) == INVALID_SOCKET)
	{
		client->FLastError = EC_CONNECT_SOCKET;
		return EC_CONNECT_SOCKET;
	}

	//char *ip = inet_ntoa(*(in_addr *)H->h_addr_list[0]) ;

	sockName.sin_family = AF_INET ;
	sockName.sin_addr.s_addr = inet_addr(client->FIP) ;
	sockName.sin_port = htons(client->FPort) ;

	if((connect(client->FSocket, (sockaddr*)&sockName, sizeof(sockName))) == SOCKET_ERROR)
	{
		closesocket(client->FSocket);
		client->FLastError = EC_CONNECT_PROBLEM;
		return EC_CONNECT_PROBLEM;
	}
	client->FConnected = true;
	return 0;
}

int DisconnectClient(PClient client)
{
	client->FLastError = 0;
	if (client->FConnected == false)
	{
		client->FLastError = EC_DISCONNECT_DISCONNECTED;
		return EC_DISCONNECT_DISCONNECTED;
	}
	closesocket(client->FSocket);
	
	client->FConnected = false;
	return 0;
}

int SendMsg(PClient client, char *buffer, DWORD length)
{
	if (client->FConnected == false)
	{
		client->FLastError = EC_SENDING_DISCONNECTED;
		return EC_SENDING_DISCONNECTED;
	}
	if((send(client->FSocket, buffer, length, 0)) == SOCKET_ERROR)
	{
		client->FLastError = EC_SENDING_PROBLEM;
		return EC_SENDING_PROBLEM;
	}
	memcpy(client->FSendMessage,buffer,length);
	client->FSendMessageSize = length;
	return 0;
}

int ReadMsg(PClient client, char *buffer, DWORD *read)
{
	if (client->FConnected == false)
	{
		client->FLastError = EC_READING_DISCONNECTED;
		*read = 0;
		return EC_READING_DISCONNECTED;
	}
	if((*read = recv(client->FSocket, buffer, MSGSIZE, 0)) == SOCKET_ERROR)
	{
		client->FLastError = EC_READING_PROBLEM;
		*read = 0;
		return EC_READING_PROBLEM;       
	}
	memcpy(client->FReadMessage,buffer,*read);
	client->FReadMessageSize = *read;
	return 0;
}

int SendRequest(PClient client, unsigned char functionType, WORD baseAddress, WORD count, unsigned char *data, unsigned char dataLength)
{
	char buffer[MSGSIZE];
	unsigned char lengthData = dataLength/sizeof(unsigned char);
	if (data == NULL) lengthData = 0;
	WORD lengthPDU = 7 + lengthData;
	WORD lengthMsg = 13 + lengthData;
	WORD i = 0;
	if (lengthData == 0)
	{
		lengthPDU--;
		lengthMsg--;
	}

	if (client->FMsgCounter == 255) client->FMsgCounter = 1;
	else client->FMsgCounter++;
	try{
		buffer[0] = client->FMsgCounter >> 8;
		buffer[1] = client->FMsgCounter & 0xFF;
		buffer[2] = 0;
		buffer[3] = 0;
		buffer[4] = lengthPDU >> 8;
		buffer[5] = lengthPDU & 0xFF;
		buffer[6] = atoi(&(strrchr(client->FIP, '.'))[1]);
		buffer[7] = functionType;
		buffer[8] = baseAddress >> 8;
		buffer[9] = baseAddress & 0xFF;
		buffer[10] = count >> 8;
		buffer[11] = count & 0xFF;
		buffer[12] = lengthData;
		for (i=0; i<lengthData; i++)
		{
			buffer[13+i] = data[i];
		}

		SendMsg(client, buffer, lengthMsg);
	}
	catch(...){
		client->FLastError = EC_REQUEST_EXCEPTION;
		return EC_REQUEST_EXCEPTION;
	}
	
	return 0;
}

int GetAnswer(PClient client, WORD *msgNo, unsigned char *deviceId, unsigned char *functionCode, WORD *baseAddress, WORD *count, unsigned char **data, unsigned char *dataLength)
{
	char buffer[MSGSIZE] = "";
	DWORD msgLength = 0;
	WORD lengthPDU = 0;
	WORD i = 0;

	ReadMsg(client, buffer, &msgLength);
	if (msgLength ==0)
	{
		client->FLastError = EC_ANSWER_DISCONNECT;
		return EC_ANSWER_DISCONNECT;
	}
	try
	{
		*msgNo = ((unsigned char)(buffer[0] << 8) | (unsigned char)buffer[1]);
		lengthPDU = (buffer[4] << 8) | buffer[5];
		*deviceId = buffer[6];
		*functionCode = buffer[7];
		if ((*functionCode > 0x80) && (lengthPDU == 3))
		{
			*dataLength = 0;
			client->FLastError = EC_SERVER_ERROR + buffer[8];
			return client->FLastError;
		}
		if (*functionCode <= FC_READ_INPUT_REGISTERS)
		{
			*dataLength = buffer[8];
			if (*functionCode <= FC_READ_DISCRETE_INPUTS)
				*count = *dataLength * 8;
			else *count = *dataLength/2;

			if (*data != NULL)
			{
				free(*data);
			}
			*data = (unsigned char*)malloc(sizeof(unsigned char)*(*dataLength));
			for (i=0; i<(*dataLength); i++)
			{
				(*data)[i] = buffer[9+i];
			}
			return 0;
		}

		*baseAddress = (buffer[8] << 8) | buffer[9];
		if ((*functionCode == FC_WRITE_COIL) || (*functionCode == FC_WRITE_HOLDING_REGISTER))
		{
			*count = 1;
			*dataLength = 2;
			*data = (unsigned char*)malloc(sizeof(unsigned char)*(*dataLength));
			(*data)[0] = buffer[10];
			(*data)[1] = buffer[11];
		}
		else
		{
			*count = (buffer[10] << 8) | buffer[11];
			*dataLength = buffer[12];
			if (msgLength < 13) *dataLength = 0;

			if ((*dataLength) > 0)
			{
				if (*data != NULL)
				{
					free(*data);
				}
				*data = (unsigned char*)malloc(sizeof(unsigned char)*(*dataLength));
				for (i=0; i<(*dataLength); i++)
				{
					(*data)[i] = buffer[13+i];
				}
			}
			else *data = NULL;
		}
	}
	catch(...)
	{
		client->FLastError = EC_ANSWER_EXCEPTION;
		if (*data != NULL)
		{
			free(*data);
			*data = NULL;
		}
		return EC_ANSWER_EXCEPTION;
	}
	return 0;
}



int WriteCoil(PClient client, WORD number, bool stateon)
{
	WORD state = 0x0000;
	WORD msgNo = 0;
	unsigned char deviceId = 0;
	unsigned char functionCode = 0;
	WORD baseAddress = 0;
	WORD count = 0;
	unsigned char *data = NULL;
	unsigned char dataLength = 0;

	if (stateon == true) state = 0xFF00;
	if (SendRequest(client, FC_WRITE_COIL, number-1, state, NULL, 0) != 0)
	{
		return client->FLastError;
	}
	if (GetAnswer(client, &msgNo, &deviceId, &functionCode, &baseAddress, &count, &data, &dataLength) != 0)
	{
		if (data != NULL) free(data);
		return client->FLastError;
	}
	if ((msgNo == client->FMsgCounter) && (functionCode == FC_WRITE_COIL) && (baseAddress == number-1))
	{
		if ((dataLength == 2) && (count == 1))
		{
			if ((data[0] == state >> 8) && (data[1] == state | 0xFF))
			{
				free(data);
				return 0;
			}
		}
	}
	free(data);

	client->FLastError = EC_PARSE_ERROR;
	return client->FLastError;
}

int ReadCoil(PClient client, WORD number, bool *stateon)
{
	WORD msgNo = 0;
	unsigned char deviceId = 0;
	unsigned char functionCode = 0;
	WORD baseAddress = 0;
	WORD count = 0;
	unsigned char *data = NULL;
	unsigned char dataLength = 0;

	if (SendRequest(client, FC_READ_COILS, number-1, 1, NULL, 0) != 0)
	{
		return client->FLastError;
	}
	if (GetAnswer(client, &msgNo, &deviceId, &functionCode, &baseAddress, &count, &data, &dataLength) != 0)
	{
		if (data != NULL) free(data);
		return client->FLastError;
	}
	if ((msgNo == client->FMsgCounter) && (functionCode == FC_READ_COILS))
	{
		if ((dataLength == 1) && (count == 8))
		{
			*stateon = (bool)data[0];
			free(data);
			return 0;
		}
	}
	free(data);

	client->FLastError = EC_PARSE_ERROR;
	return client->FLastError;
}

int WriteHoldingRegister(PClient client, WORD number, WORD value)
{
	WORD msgNo = 0;
	unsigned char deviceId = 0;
	unsigned char functionCode = 0;
	WORD baseAddress = 0;
	WORD count = 0;
	unsigned char *data = NULL;
	unsigned char dataLength = 0;

	if (SendRequest(client, FC_WRITE_HOLDING_REGISTER, number-1, value, NULL, 0) != 0)
	{
		return client->FLastError;
	}
	if (GetAnswer(client, &msgNo, &deviceId, &functionCode, &baseAddress, &count, &data, &dataLength) != 0)
	{
		if (data != NULL) free(data);
		return client->FLastError;
	}
	if ((msgNo == client->FMsgCounter) && (functionCode == FC_WRITE_HOLDING_REGISTER) && (baseAddress == number-1))
	{
		if ((dataLength == 2) && (count == 1))
		{
			if ((data[0] == value >> 8) && (data[1] == value | 0xFF))
			{
				free(data);
				return 0;
			}
		}
	}
	free(data);

	client->FLastError = EC_PARSE_ERROR;
	return client->FLastError;
}

int ReadHoldingRegister(PClient client, WORD number, WORD *value)
{
	WORD msgNo = 0;
	unsigned char deviceId = 0;
	unsigned char functionCode = 0;
	WORD baseAddress = 0;
	WORD count = 0;
	unsigned char *data = NULL;
	unsigned char dataLength = 0;

	if (SendRequest(client, FC_READ_HOLDING_REGISTERS, number-1, 1, NULL, 0) != 0)
	{
		return client->FLastError;
	}
	if (GetAnswer(client, &msgNo, &deviceId, &functionCode, &baseAddress, &count, &data, &dataLength) != 0)
	{
		if (data != NULL) free(data);
		return client->FLastError;
	}
	if ((msgNo == client->FMsgCounter) && (functionCode == FC_READ_HOLDING_REGISTERS))
	{
		if ((dataLength == 2) && (count == 1))
		{
			*value = (data[0] << 8) | data[1];
			free(data);
			return 0;
		}
	}
	free(data);

	client->FLastError = EC_PARSE_ERROR;
	return client->FLastError;
}

int ReadCoils(PClient client, WORD from, WORD count, bool **stateons)
{
	WORD msgNo = 0;
	unsigned char deviceId = 0;
	unsigned char functionCode = 0;
	WORD baseAddress = 0;
	WORD pocet = 0;
	unsigned char *data = NULL;
	unsigned char dataLength = 0;
	unsigned char i = 0;

	if (SendRequest(client, FC_READ_COILS, from-1, count, NULL, 0) != 0)
	{
		return client->FLastError;
	}
	if (GetAnswer(client, &msgNo, &deviceId, &functionCode, &baseAddress, &pocet, &data, &dataLength) != 0)
	{
		if (data != NULL) free(data);
		return client->FLastError;
	}
	if ((msgNo == client->FMsgCounter) && (functionCode == FC_READ_COILS))
	{
		if (dataLength == count/8 + (count%8 != 0))
		{
			if (*stateons == NULL) *stateons = (bool*)malloc(sizeof(bool)*count);
			for (i=0; i<count; i++)
			{
				(*stateons)[i] = ((data[(i/8)] & (1 << (i%8))) == (1 << (i%8)));
			}
			free(data);
			return 0;
		}
	}
	free(data);

	client->FLastError = EC_PARSE_ERROR;
	return client->FLastError;
}

int ReadHoldingRegisters(PClient client, WORD from, WORD count, WORD **values)
{
	WORD msgNo = 0;
	unsigned char deviceId = 0;
	unsigned char functionCode = 0;
	WORD baseAddress = 0;
	WORD pocet = 0;
	unsigned char *data = NULL;
	unsigned char dataLength = 0;
	unsigned char i = 0;

	if (SendRequest(client, FC_READ_HOLDING_REGISTERS, from-1, count, NULL, 0) != 0)
	{
		return client->FLastError;
	}
	if (GetAnswer(client, &msgNo, &deviceId, &functionCode, &baseAddress, &pocet, &data, &dataLength) != 0)
	{
		if (data != NULL) free(data);
		return client->FLastError;
	}
	if ((msgNo == client->FMsgCounter) && (functionCode == FC_READ_HOLDING_REGISTERS))
	{
		if (dataLength == 2*count)
		{
			if (*values == NULL) *values = (WORD*)malloc(sizeof(WORD)*count);
			for (i=0; i<count; i++)
			{
				(*values)[i] = (data[2*i] << 8) | data[2*i+1];
			}
			free(data);
			return 0;
		}
	}
	free(data);

	client->FLastError = EC_PARSE_ERROR;
	return client->FLastError;
}

int ReadDiscreteInputs(PClient client, WORD from, WORD count, bool **stateons)
{
	WORD msgNo = 0;
	unsigned char deviceId = 0;
	unsigned char functionCode = 0;
	WORD baseAddress = 0;
	WORD pocet = 0;
	unsigned char *data = NULL;
	unsigned char dataLength = 0;
	unsigned char i = 0;

	if (SendRequest(client, FC_READ_DISCRETE_INPUTS, from-1, count, NULL, 0) != 0)
	{
		return client->FLastError;
	}
	if (GetAnswer(client, &msgNo, &deviceId, &functionCode, &baseAddress, &pocet, &data, &dataLength) != 0)
	{
		if (data != NULL) free(data);
		return client->FLastError;
	}
	if ((msgNo == client->FMsgCounter) && (functionCode == FC_READ_DISCRETE_INPUTS))
	{
		if (dataLength == count/8 + (count%8 != 0))
		{
			if (*stateons == NULL) *stateons = (bool*)malloc(sizeof(bool)*count);
			for (i=0; i<count; i++)
			{
				(*stateons)[i] = ((data[(i/8)] & (1 << (i%8))) == (1 << (i%8)));
			}
			free(data);
			return 0;
		}
	}
	free(data);

	client->FLastError = EC_PARSE_ERROR;
	return client->FLastError;
}

int ReadInputRegisters(PClient client, WORD from, WORD count, WORD **values)
{
	WORD msgNo = 0;
	unsigned char deviceId = 0;
	unsigned char functionCode = 0;
	WORD baseAddress = 0;
	WORD pocet = 0;
	unsigned char *data = NULL;
	unsigned char dataLength = 0;
	unsigned char i = 0;

	if (SendRequest(client, FC_READ_INPUT_REGISTERS, from-1, count, NULL, 0) != 0)
	{
		return client->FLastError;
	}
	if (GetAnswer(client, &msgNo, &deviceId, &functionCode, &baseAddress, &pocet, &data, &dataLength) != 0)
	{
		if (data != NULL) free(data);
		return client->FLastError;
	}
	if ((msgNo == client->FMsgCounter) && (functionCode == FC_READ_INPUT_REGISTERS))
	{
		if (dataLength == 2*count)
		{
			if (*values == NULL) *values = (WORD*)malloc(sizeof(WORD)*count);
			for (i=0; i<count; i++)
			{
				(*values)[i] = (data[2*i] << 8) | data[2*i+1];
			}
			free(data);
			return 0;
		}
	}
	free(data);

	client->FLastError = EC_PARSE_ERROR;
	return client->FLastError;
}

int WriteCoils(PClient client, WORD from, WORD count, bool stateon[])
{
	WORD state = 0x0000;
	WORD msgNo = 0;
	unsigned char deviceId = 0;
	unsigned char functionCode = 0;
	WORD baseAddress = 0;
	WORD pocet = 0;
	WORD i = 0;
	unsigned char tmpByte = 0;
	unsigned char *data = NULL;
	unsigned char dataLength = count/8 + (count%8 != 0);

	data = (unsigned char*)malloc(sizeof(unsigned char)*dataLength);
	for (i=0; i<count; i++)
	{
		if ((i%8 == 0) && (i != 0))
		{
			data[i/8] = tmpByte;
			tmpByte = 0;
		}
		tmpByte |= stateon[i] << (i%8);
	}
	if (i <= 8) data[0] = tmpByte;
	else data[i/8] = tmpByte;
	if (SendRequest(client, FC_WRITE_COILS, from-1, count, data, dataLength) != 0)
	{
		if (data != NULL) free(data);
		return client->FLastError;
	}
	free(data);
	data = NULL;
	if (GetAnswer(client, &msgNo, &deviceId, &functionCode, &baseAddress, &pocet, &data, &dataLength) != 0)
	{
		if (data != NULL) free(data);
		return client->FLastError;
	}
	if ((msgNo == client->FMsgCounter) && (functionCode == FC_WRITE_COILS) && (baseAddress == from-1) && (count == pocet))
	{
		return 0;
	}

	client->FLastError = EC_PARSE_ERROR;
	return client->FLastError;
}

int WriteHoldingRegisters(PClient client, WORD from, WORD count, WORD values[])
{
	WORD state = 0x0000;
	WORD msgNo = 0;
	unsigned char deviceId = 0;
	unsigned char functionCode = 0;
	WORD baseAddress = 0;
	WORD pocet = 0;
	WORD i = 0;
	unsigned char tmpByte = 0;
	unsigned char *data = NULL;
	unsigned char dataLength = 2*count;

	data = (unsigned char*)malloc(sizeof(unsigned char)*dataLength);
	for (i=0; i<count; i++)
	{
		data[2*i] = values[i] >> 8;
		data[2*i+1] = values[i] & 0xFF;
	}
	if (SendRequest(client, FC_WRITE_HOLDING_REGISTERS, from-1, count, data, dataLength) != 0)
	{
		if (data != NULL) free(data);
		return client->FLastError;
	}
	free(data);
	data = NULL;
	if (GetAnswer(client, &msgNo, &deviceId, &functionCode, &baseAddress, &pocet, &data, &dataLength) != 0)
	{
		if (data != NULL) free(data);
		return client->FLastError;
	}
	if ((msgNo == client->FMsgCounter) && (functionCode == FC_WRITE_HOLDING_REGISTERS) && (baseAddress == from-1) && (count == pocet))
	{
		return 0;
	}

	client->FLastError = EC_PARSE_ERROR;
	return client->FLastError;
}

int GetRWBuffer(PClient client, char **RBuffer, char **WBuffer, int *RBufferSize, int *WBufferSize)
{
	*RBufferSize = client->FReadMessageSize;
	*WBufferSize = client->FSendMessageSize;
	if (*RBuffer == NULL)
		*RBuffer = (char*)malloc(sizeof(char)*client->FReadMessageSize);
	if (*WBuffer == NULL)
		*WBuffer = (char*)malloc(sizeof(char)*client->FSendMessageSize);
	memcpy(*RBuffer, client->FReadMessage, client->FReadMessageSize);
	memcpy(*WBuffer, client->FSendMessage, client->FSendMessageSize);
	return 0;
}

// ****** SERVER *******

PServer NewServer(int port, WORD serverMaxClients, WORD nDiscreteInputs, WORD nCoils, WORD nInputRegisters, WORD nHoldingRegisters)
{
	unsigned short libError = 0;
	unsigned short i = 0;
	if (FLibraryInitialized != true)
	{
		libError = InitializeLibrary();
	}
	
	PServer newServer = (PServer)malloc(sizeof(TServer));
	newServer->FRunning = false;
	newServer->FMainSocket = 0;
	newServer->FClient = 0;
	newServer->FLastError = libError;
	newServer->FPort = port;
	newServer->FServerMaxClients = serverMaxClients;
	newServer->FServerThread = NULL;
	/*
	newServer->FClientThreads = (HANDLE*)malloc(sizeof(HANDLE)*serverMaxClients);
	for (i=0; i<serverMaxClients; i++)
	{
		newServer->FClientThreads[i] = NULL;
	}
	*/
	newServer->FData = (PServerData)malloc(sizeof(TServerData));
	newServer->FData->FDiscreteInputs = (bool*)malloc(sizeof(bool)*nDiscreteInputs);
	newServer->FData->FCoils = (bool*)malloc(sizeof(bool)*nCoils);
	newServer->FData->FInputRegisters = (WORD*)malloc(sizeof(WORD)*nInputRegisters);
	newServer->FData->FHoldingRegisters = (WORD*)malloc(sizeof(WORD)*nHoldingRegisters);
	for (i=0; i<nCoils; i++)
		newServer->FData->FCoils[i] = false;
	for (i=0; i<nDiscreteInputs; i++)
		newServer->FData->FDiscreteInputs[i] = false;
	for (i=0; i<nHoldingRegisters; i++)
		newServer->FData->FHoldingRegisters[i] = 0;
	for (i=0; i<nHoldingRegisters; i++)
		newServer->FData->FInputRegisters[i] = 0;
	newServer->FData->nDiscreteInputs = nDiscreteInputs;
	newServer->FData->nCoils = nCoils;
	newServer->FData->nInputRegisters = nInputRegisters;
	newServer->FData->nHoldingRegisters = nHoldingRegisters;
	if ((port <=0) || (nDiscreteInputs == 0) || (nCoils == 0) || (nInputRegisters == 0) || (nHoldingRegisters == 0))
		newServer->FLastError = ES_NEW_PARAMETERS;
	return newServer;
}

int DestroyServer(PServer server)
{
	ShutdownServer(server);
	free(server->FData->FCoils);
	free(server->FData->FDiscreteInputs);
	free(server->FData->FHoldingRegisters);
	free(server->FData->FInputRegisters);
	server->FData->FCoils = NULL;
	server->FData->FDiscreteInputs = NULL;
	server->FData->FHoldingRegisters = NULL;
	server->FData->FInputRegisters = NULL;
	free(server->FData);
	server->FData = NULL;
	free(server);
	server = NULL;
	return 0;
}

int EstablishServer(PServer server)
{
	sockaddr_in sockName;

	if (FLibraryInitialized != true)
	{
		server->FLastError = EL_INITIALIZE;
		return EL_INITIALIZE;
	}
	if (server->FRunning == true)
	{
		server->FLastError = ES_ESTABLISH_RUNNING;
		return ES_ESTABLISH_RUNNING;
	}
	if ((server->FMainSocket = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP)) == INVALID_SOCKET)
    {
		server->FLastError = ES_ESTABLISH_SOCKET;
        return ES_ESTABLISH_SOCKET;
    }

    sockName.sin_family = AF_INET;
	sockName.sin_port = htons(server->FPort);
    sockName.sin_addr.s_addr = INADDR_ANY;

    if (bind(server->FMainSocket, (sockaddr *)&sockName, sizeof(sockName)) == SOCKET_ERROR)
    {
		server->FLastError = ES_ESTABLISH_NAMING;
        return ES_ESTABLISH_NAMING;
    }

	if (listen(server->FMainSocket, server->FServerMaxClients) == SOCKET_ERROR)
    {
		server->FLastError = ES_ESTABLISH_LISTENING;
        return ES_ESTABLISH_LISTENING;
    }
	server->FServerThread = (HANDLE)_beginthread(HandlingClients, 0, (void*)server);
	if (server->FServerThread == INVALID_HANDLE_VALUE)
	{
		server->FLastError = ES_ESTABLISH_TREAD;
        return ES_ESTABLISH_TREAD;
    }
	server->FRunning = true;

	return 0;
}

int ShutdownServer(PServer server)
{
	if (server->FRunning == false)
	{
		server->FLastError = ES_SHUTDOWN_STOPPED;
		return ES_SHUTDOWN_STOPPED;
	}
	if ((server->FServerThread != NULL) && (server->FServerThread != INVALID_HANDLE_VALUE))
	{
		TerminateThread(server->FServerThread, 0);
		CloseHandle(server->FServerThread);
		server->FServerThread = NULL;
	}
	if ((server->FClient != 0) && (server->FClient != INVALID_SOCKET))
	{
		closesocket(server->FClient);
		server->FClient = 0;
	}
	/*
	if ((server->FMainSocket != 0) && (server->FMainSocket != INVALID_SOCKET))
	{
		closesocket(server->server->FMainSocket);
	}
	*/
	closesocket(server->FMainSocket);
	server->FMainSocket = 0;
	server->FRunning = false;
	return 0;
}

void HandlingClients(void *param)
{
	sockaddr_in clientInfo;
    int addressLength = sizeof(clientInfo);
	PServer server = (PServer)param;
	//unsigned char clientCount = 0;

    do
    {
		server->FClient = accept(server->FMainSocket, (sockaddr*)&clientInfo, &addressLength);
        if (server->FClient == INVALID_SOCKET)
        {
			server->FLastError = ES_HANDLING_ACCEPT;
			continue;
        }
        ClientService(server);
    }
	while(1);
    _endthread();
}

int ClientService(PServer server)
{
	DWORD len = 0;
	WORD msgNo = 0;
	unsigned char deviceId = 0;
	unsigned char functionCode = 0;
	WORD baseAddress = 0;
	WORD count = 0;
	unsigned char *data = NULL;
	unsigned char responseData[3] = {1, 1, 5};
	unsigned char dataLength = 0;

	server->FLastError = 0;
	do
	{
		GetRequest(server, &msgNo, &deviceId, &functionCode, &baseAddress, &count, &data, &dataLength);
		if ((server->FLastError == 28) || (server->FLastError == 27)) continue;

		if (IsValidFunctionCode(functionCode) == false)
		{
			SendErrorResponse(server, msgNo, deviceId, functionCode, 1);
			if (data != NULL)
			{
				free(data);
				data = NULL;
			}
			continue;
		}
		if (IsValidAddress(server, functionCode, baseAddress, count) == false)
		{
			SendErrorResponse(server, msgNo, deviceId, functionCode, 2);
			if (data != NULL)
			{
				free(data);
				data = NULL;
			}
			continue;
		}
		/*
		SendResponse(server, msgNo, deviceId, functionCode, responseData, sizeof(responseData));
		if (data != NULL) free(data);
		continue;
		*/
		if (IsValidDataValue(server, functionCode, baseAddress, count, data, dataLength) == false)
		{
			SendErrorResponse(server, msgNo, deviceId, functionCode, 3);
			if (data != NULL)
			{
				free(data);
				data = NULL;
			}
			continue;
		}
		if (DoOperation(server, msgNo, deviceId, functionCode, baseAddress, count, data) != 0)
		{
			SendErrorResponse(server, msgNo, deviceId, functionCode, 4);
			if (data != NULL)
			{
				free(data);
				data = NULL;
			}
			continue;
		}
		if (data != NULL)
		{
			free(data);
			data = NULL;
		}
	}
	while((server->FLastError != 28) && (server->FLastError != 27));
	server->FLastError = 0;
	return 0;
}

int SendMsg(PServer server, char *buffer, DWORD length)
{
	if ((server->FRunning == false) || (server->FClient == 0)
		|| (server->FClient == INVALID_SOCKET) || (length == 0))
	{
		server->FLastError = ES_SEND_CONNECTION;
		return ES_SEND_CONNECTION;
	}
	if((send(server->FClient, buffer, length, 0)) == SOCKET_ERROR)
	{
		server->FLastError = ES_SEND_PROBLEM;
		return ES_SEND_PROBLEM;
	}
	return 0;
}

int ReadMsg(PServer server, char *buffer, DWORD *read)
{
	if ((server->FRunning == false) || (server->FClient == 0) || (server->FClient == INVALID_SOCKET))
	{
		server->FLastError = ES_READ_CONNECTION;
		*read = 0;
		return ES_READ_CONNECTION;
	}
	if((*read = recv(server->FClient, buffer, MSGSIZE, 0)) == SOCKET_ERROR)
	{
		server->FLastError = ES_READ_PROBLEM;
		*read = 0;
		return ES_READ_PROBLEM;       
	}
	return 0;
}

int GetRequest(PServer server, WORD *msgNo, unsigned char *deviceId, unsigned char *functionType, WORD *baseAddress, WORD *count, unsigned char **data, unsigned char *dataLength)
{
	char buffer[MSGSIZE] = "";
	DWORD msgLength = 0;
	unsigned char lengthData = *dataLength;
	WORD lengthPDU = 0;
	WORD i = 0;

	ReadMsg(server, buffer, &msgLength);
	if (msgLength ==0)
	{
		server->FLastError = ES_REQUEST_DISCONNECT;
		return ES_REQUEST_DISCONNECT;
	}
	try
	{
		*msgNo = (buffer[0] << 8) | buffer[1];
		lengthPDU = (buffer[4] << 8) | buffer[5];
		*deviceId = buffer[6];
		*functionType = buffer[7];
		*baseAddress = (buffer[8] << 8) | buffer[9];
		if ((*functionType == FC_WRITE_COIL) || (*functionType == FC_WRITE_HOLDING_REGISTER))
		{
			*count = 1;
			lengthData = 2;
			*data = (unsigned char*)malloc(sizeof(unsigned char)*lengthData);
			(*data)[0] = buffer[10];
			(*data)[1] = buffer[11];
		}
		else
		{
			*count = (buffer[10] << 8) | buffer[11];
			lengthData = buffer[12];
			if (msgLength < 13) *dataLength = 0;

			if (lengthData > 0)
			{
				if (*data != NULL)
				{
					free(*data);
				}
				*data = (unsigned char*)malloc(sizeof(unsigned char)*lengthData);
				for (i=0; i<lengthData; i++)
				{
					(*data)[i] = buffer[13+i];
				}
			}
			else *data = NULL;
		}
	}
	catch(...)
	{
		*dataLength = lengthData;
		server->FLastError = ES_REQUEST_EXCEPTION;
		if (*data != NULL)
		{
			free(*data);
			*data = NULL;
		}
		return ES_REQUEST_EXCEPTION;
	}
	*dataLength = lengthData;
	return 0;
}

int SendResponse(PServer server, WORD msgNo, unsigned char deviceId, unsigned char functionType, unsigned char *data, unsigned char dataLength)
{
	char buffer[MSGSIZE];
	WORD lengthPDU = 2 + dataLength;
	WORD lengthMsg = 8 + dataLength;
	WORD i = 0;

	if (data == NULL)
	{
		server->FLastError = 26;
		return 26;
	}

	try{
		buffer[0] = msgNo >> 8;
		buffer[1] = msgNo & 0xFF;
		buffer[2] = 0;
		buffer[3] = 0;
		buffer[4] = lengthPDU >> 8;
		buffer[5] = lengthPDU & 0xFF;
		buffer[6] = deviceId;
		buffer[7] = functionType;
		for (i=0; i<dataLength; i++)
		{
			buffer[8+i] = data[i];
		}

		SendMsg(server, buffer, lengthMsg);
	}
	catch(...){
		server->FLastError = ES_RESPONSE_EXCEPTION;
		return ES_RESPONSE_EXCEPTION;
	}

	return 0;
}

int SendErrorResponse(PServer server, WORD msgNo, unsigned char deviceId, unsigned char functionType, unsigned char errorCode)
{
	char buffer[MSGSIZE];
	WORD lengthPDU = 3;
	WORD lengthMsg = 9;

	try{
		buffer[0] = msgNo >> 8;
		buffer[1] = msgNo & 0xFF;
		buffer[2] = 0;
		buffer[3] = 0;
		buffer[4] = lengthPDU >> 8;
		buffer[5] = lengthPDU & 0xFF;
		buffer[6] = deviceId;
		buffer[7] = functionType + 128;
		buffer[8] = errorCode;

		SendMsg(server, buffer, lengthMsg);
	}
	catch(...){
		server->FLastError = 26;
		return 26;
	}

	return 0;
}

bool IsValidFunctionCode(unsigned char functionCode)
{
	unsigned char validCodes[8] = {FC_READ_COILS, FC_READ_DISCRETE_INPUTS, FC_READ_HOLDING_REGISTERS,
		FC_READ_INPUT_REGISTERS, FC_WRITE_COIL, FC_WRITE_HOLDING_REGISTER, FC_WRITE_COILS, FC_WRITE_HOLDING_REGISTERS};
	unsigned char i =0;
	bool valid = false;
	
	for (i=0; i<sizeof(validCodes); i++)
	{
		if (validCodes[i] == functionCode) valid = true;
	}
	if (valid == false) return false;
	return true;
}

bool IsValidAddress(PServer server, unsigned char functionCode, WORD baseAddress, WORD count)
{
	if (count == 0) return false;
	if (((functionCode == FC_WRITE_COIL) || (functionCode == FC_WRITE_HOLDING_REGISTER)) && (count != 1)) return false;
	
	switch(functionCode)
	{
	case FC_READ_COILS:if (baseAddress+count > server->FData->nCoils) return false;break;
	case FC_READ_DISCRETE_INPUTS:if (baseAddress+count > server->FData->nDiscreteInputs) return false;break;
	case FC_READ_HOLDING_REGISTERS:if (baseAddress+count > server->FData->nHoldingRegisters) return false;break;
	case FC_READ_INPUT_REGISTERS:if (baseAddress+count > server->FData->nInputRegisters) return false;break;
	case FC_WRITE_COIL:if (baseAddress >= server->FData->nCoils) return false;break;
	case FC_WRITE_HOLDING_REGISTER:if (baseAddress >= server->FData->nHoldingRegisters) return false;break;
	case FC_WRITE_COILS:if (baseAddress+count > server->FData->nCoils) return false;break;
	case FC_WRITE_HOLDING_REGISTERS:if (baseAddress+count > server->FData->nHoldingRegisters) return false;break;
	}
	return true;
}

bool IsValidDataValue(PServer server, unsigned char functionCode, WORD baseAddress, WORD count, unsigned char *data, unsigned char dataLength)
{	
	//if (sizeof(data) != count) return false;
	if ((functionCode == FC_READ_COILS))
		if (data == NULL) return true;
		else return false;
	if ((functionCode == FC_READ_DISCRETE_INPUTS))
		if (data == NULL) return true;
		else return false;
	if ((functionCode == FC_READ_HOLDING_REGISTERS))
		if (data == NULL) return true;
		else return false;
	if ((functionCode == FC_READ_INPUT_REGISTERS))
		if (data == NULL) return true;
		else return false;
	if ((functionCode == FC_WRITE_COIL))
		if ((data != NULL) && (dataLength/sizeof(WORD) == count))
		{
			if ((((data[0]<<8) | data[1]) == 0x0000) || (((data[0]<<8) | data[1]) == 0xFF00)) return true;
			else return false;
		}
		else return false;
	if ((functionCode == FC_WRITE_HOLDING_REGISTER))
		if ((data != NULL) && (dataLength/sizeof(WORD) == count))
			return true;
		else return false;
	if ((functionCode == FC_WRITE_COILS))
		if (data != NULL)
		{
			if (dataLength/sizeof(unsigned char) == count/8 + (count%8 != 0))
				return true;
			else return false;
		}
		else return false;
	if ((functionCode == FC_WRITE_HOLDING_REGISTERS))
		if (data != NULL)
		{
			if (dataLength/sizeof(WORD) == count)
				return true;
			else return false;
		}
		else return false;
	
	return true;
}

int DoOperation(PServer server, WORD msgNo, unsigned char deviceId, unsigned char functionCode, WORD baseAddress, WORD count, unsigned char *data)
{
	unsigned char *responseData = NULL;
	unsigned char responseDataLength = 0;
	unsigned char i = 0;
	unsigned char tmpByte = 0;

	switch(functionCode)
	{
	case FC_READ_COILS:
		{
			try
			{
				responseDataLength = 1 + count/8 + (count%8 != 0);
				responseData = (unsigned char*)malloc(sizeof(unsigned char)*responseDataLength);
				responseData[0] = responseDataLength - 1;
				for (i=0; i<count; i++)
				{
					if ((i%8 == 0) && (i != 0))
					{
						responseData[i/8] = tmpByte;
						tmpByte = 0;
					}
					tmpByte |= (server->FData->FCoils[baseAddress+i] << (i%8));
				}
				if (i <= 8) responseData[1] = tmpByte;
				else responseData[1+i/8] = tmpByte;
				SendResponse(server, msgNo, deviceId, functionCode, responseData, responseDataLength);
			}
			catch(...)
			{
				if (responseData != NULL)
				{
					free(responseData);
					responseData = NULL;
				}
				return FC_READ_COILS;
			}
			if (responseData != NULL)
			{
				free(responseData);
				responseData = NULL;
			}
			break;
		}
	case FC_READ_DISCRETE_INPUTS:
		{
			try
			{
				responseDataLength = 1 + count/8 + (count%8 != 0);
				responseData = (unsigned char*)malloc(sizeof(unsigned char)*responseDataLength);
				responseData[0] = responseDataLength - 1;
				for (i=0; i<count; i++)
				{
					if ((i%8 == 0) && (i != 0))
					{
						responseData[i/8] = tmpByte;
						tmpByte = 0;
					}
					tmpByte |= (server->FData->FDiscreteInputs[baseAddress+i] << (i%8));
				}
				if (i <= 8) responseData[1] = tmpByte;
				else responseData[1+i/8] = tmpByte;
				SendResponse(server, msgNo, deviceId, functionCode, responseData, responseDataLength);
			}
			catch(...)
			{
				if (responseData != NULL)
				{
					free(responseData);
					responseData = NULL;
				}
				return FC_READ_DISCRETE_INPUTS;
			}
			if (responseData != NULL)
			{
				free(responseData);
				responseData = NULL;
			}
			break;
		}
	case FC_READ_HOLDING_REGISTERS:
		{
			try
			{
				responseDataLength = 1 + 2*count;
				responseData = (unsigned char*)malloc(sizeof(unsigned char)*responseDataLength);
				responseData[0] = responseDataLength - 1;
				for (i=0; i<count; i++)
				{
					responseData[1+(2*i)] = server->FData->FHoldingRegisters[baseAddress+i] >> 8;
					responseData[1+(2*i+1)] = server->FData->FHoldingRegisters[baseAddress+i] & 0xFF;
				}
				SendResponse(server, msgNo, deviceId, functionCode, responseData, responseDataLength);
			}
			catch(...)
			{
				if (responseData != NULL)
				{
					free(responseData);
					responseData = NULL;
				}
				return FC_READ_HOLDING_REGISTERS;
			}
			if (responseData != NULL)
			{
				free(responseData);
				responseData = NULL;
			}
			break;
		}
	case FC_READ_INPUT_REGISTERS:
		{
			try
			{
				responseDataLength = 1 + 2*count;
				responseData = (unsigned char*)malloc(sizeof(unsigned char)*responseDataLength);
				responseData[0] = responseDataLength - 1;
				for (i=0; i<count; i++)
				{
					responseData[1+(2*i)] = server->FData->FInputRegisters[baseAddress+i] >> 8;
					responseData[1+(2*i+1)] = server->FData->FInputRegisters[baseAddress+i] & 0xFF;
				}
				SendResponse(server, msgNo, deviceId, functionCode, responseData, responseDataLength);
			}
			catch(...)
			{
				if (responseData != NULL)
				{
					free(responseData);
					responseData = NULL;
				}
				return FC_READ_INPUT_REGISTERS;
			}
			if (responseData != NULL)
			{
				free(responseData);
				responseData = NULL;
			}
			break;
		}
	case FC_WRITE_COIL:
		{
			try
			{
				responseDataLength = 4;
				responseData = (unsigned char*)malloc(sizeof(unsigned char)*responseDataLength);
				responseData[0] = baseAddress >> 8;
				responseData[1] = baseAddress & 0xFF;
				responseData[2] = data[0];
				responseData[3] = data[1];
				if (((data[0] << 8) | data[1]) == 0xFF00)
					server->FData->FCoils[baseAddress] = true;
				if (((data[0] << 8) | data[1]) == 0x0000)
					server->FData->FCoils[baseAddress] = false;
				SendResponse(server, msgNo, deviceId, functionCode, responseData, responseDataLength);
			}
			catch(...)
			{
				if (responseData != NULL)
				{
					free(responseData);
					responseData = NULL;
				}
				return FC_WRITE_COIL;
			}
			if (responseData != NULL)
			{
				free(responseData);
				responseData = NULL;
			}
			break;
		}
	case FC_WRITE_HOLDING_REGISTER:
		{
			try
			{
				responseDataLength = 4;
				responseData = (unsigned char*)malloc(sizeof(unsigned char)*responseDataLength);
				responseData[0] = baseAddress >> 8;
				responseData[1] = baseAddress & 0xFF;
				responseData[2] = data[0];
				responseData[3] = data[1];
				server->FData->FHoldingRegisters[baseAddress] = ((data[0] << 8) | data[1]);
				SendResponse(server, msgNo, deviceId, functionCode, responseData, responseDataLength);
			}
			catch(...)
			{
				if (responseData != NULL)
				{
					free(responseData);
					responseData = NULL;
				}
				return FC_WRITE_HOLDING_REGISTER;
			}
			if (responseData != NULL)
			{
				free(responseData);
				responseData = NULL;
			}
			break;
		}
	case FC_WRITE_COILS:
		{
			try
			{
				responseDataLength = 4;
				//if (count%8 != 0)
				//	if (data[1+(count/8)] > 2^(count%8)) return FC_WRITE_COILS;
				responseData = (unsigned char*)malloc(sizeof(unsigned char)*responseDataLength);
				for (i=0; i<count; i++)
				{
					server->FData->FCoils[baseAddress+i] = (bool)((data[(i/8)] & (1 << (i%8))) == (1 << (i%8)));
				}
				responseData[0] = baseAddress >> 8;
				responseData[1] = baseAddress & 0xFF;
				responseData[2] = count >> 8;
				responseData[3] = count & 0xFF;
				SendResponse(server, msgNo, deviceId, functionCode, responseData, responseDataLength);
			}
			catch(...)
			{
				if (responseData != NULL)
				{
					free(responseData);
					responseData = NULL;
				}
				return FC_WRITE_COILS;
			}
			if (responseData != NULL)
			{
				free(responseData);
				responseData = NULL;
			}
			break;
		}
	case FC_WRITE_HOLDING_REGISTERS:
		{
			try
			{
				responseDataLength = 4;
				responseData = (unsigned char*)malloc(sizeof(unsigned char)*responseDataLength);
				for (i=0; i<count; i++)
				{
					server->FData->FHoldingRegisters[baseAddress+i] = ((data[2*i] << 8) | data[2*i+1]);
				}
				responseData[0] = baseAddress >> 8;
				responseData[1] = baseAddress & 0xFF;
				responseData[2] = count >> 8;
				responseData[3] = count & 0xFF;
				SendResponse(server, msgNo, deviceId, functionCode, responseData, responseDataLength);
			}
			catch(...)
			{
				if (responseData != NULL)
				{
					free(responseData);
					responseData = NULL;
				}
				return FC_WRITE_HOLDING_REGISTERS;
			}
			if (responseData != NULL)
			{
				free(responseData);
				responseData = NULL;
			}
			break;
		}
	}
	return 0;
}

int ChangeData(PServer server, bool discreteInputs[], WORD inputRegisters[], bool **coils, WORD **holdingRegisters)
{
	if (server == NULL)
	{
		return ES_CHANGEDATA_ERROR;
	}
	try
	{
		if (*coils == NULL) *coils = (bool*)malloc(sizeof(bool)*server->FData->nCoils);
		if (*holdingRegisters == NULL) *holdingRegisters = (WORD*)malloc(sizeof(WORD)*server->FData->nHoldingRegisters);

		WORD i = 0;
		for (i=0; i < server->FData->nCoils; i++)
			(*coils)[i] = server->FData->FCoils[i];
		for (i=0; i < server->FData->nDiscreteInputs; i++)
			if ((bool)discreteInputs[i] == true) server->FData->FDiscreteInputs[i] = (bool)true;
			else server->FData->FDiscreteInputs[i] = (bool)false;
		for (i=0; i < server->FData->nHoldingRegisters; i++)
			(*holdingRegisters)[i] = server->FData->FHoldingRegisters[i];
		for (i=0; i < server->FData->nInputRegisters; i++)
			server->FData->FInputRegisters[i] = inputRegisters[i];
	}
	catch(...)
	{
		server->FLastError = ES_CHANGEDATA_ERROR;
		return ES_CHANGEDATA_ERROR;
	}
	return 0;
}

#ifdef Develop
// ***** MAIN TEST *****

int main(int argc, char* argv[])
{
	PServer modbusX = NewServer(502, 5, 16, 16, 12, 12);
	printf("%s\n",GetErrorMessage(LastServerError(modbusX)));
	EstablishServer(modbusX);
	//PClient modbus1 = NewClient("10.0.0.15", 502);
	PClient modbus1 = NewClient("127.0.0.1", 502);
	printf("%s\n",GetErrorMessage(LastClientError(modbus1)));

	//EstablishServer(modbusX);
	//printf("%s\n",GetErrorMessage(LastServerError(modbusX)));
	
	WORD i = 0;
	WORD *values = NULL;
	WORD values1[12]={32768,32768,255,0,0,0,0,1,0,0,0,0};
	//WORD* holdings = NULL;
	//WORD inputs[12]={0,0,0,0,0,0,0,0,0,0,0,0};
	//bool* coils = NULL;
	//bool discretes[16]={false,false,true,false,true,false,true,false,false,false,false,false,false,false,false,false};
	unsigned short error = 0;
	int chyb = 0;
	//bool* tmp = (bool*)malloc(8*sizeof(bool));
	
	//ConnectClient(modbus1);
	//ChangeData(modbusX,discretes,inputs,&coils,&holdings);
	printf("%s\n",GetErrorMessage(LastClientError(modbus1)));
	int start = 4;
	int count = 8;
	//ReadDiscreteInputs(modbus1, start, count, &tmp);
	//for (i=0; i<count; i++)
	//	if (tmp[i] == true) printf("Discrete input %d is true\n", i+start);
	//	else printf("Discrete input %d is false\n", i+start);
	//DisconnectClient(modbus1);
	//while(1==1){//for (i=0;i<100;i++){
		
		ConnectClient(modbus1);
		if ((error = WriteHoldingRegisters(modbus1,1,12,values1)) != 0)
			printf("Error - write registers %d\n\n", error);
		sleep(1);
		if ((error = ReadHoldingRegisters(modbus1, 1, 12, &values)) != 0)
			printf("Error - read registers %d\n\n", error);
		else
		{
			for (i=0; i<12; i++)
				printf("Register %d is 0x%X\n", i+1, values[i]);
		}
		
		printf("\n");
		//ChangeData(modbusX,&coils,discretes,&holdings,inputs);
		//if ((error = WriteHoldingRegisters(modbus1,1,12,values1)) != 0)
		//	printf("Error - write registers %d\n\n", error);
		
		//if (error == 30) chyb++;
		DisconnectClient(modbus1);



		
	//}
		char* tmp1 = NULL;
		char* tmp2 = NULL;
		int size1 = 0;
		int size2 = 0;
		GetRWBuffer(modbus1,&tmp1, &tmp2, &size1, &size2);
	for (i=0; i<size1; i++)
				printf("0x%X ",(unsigned char)tmp1[i]);
	printf("\n\n");
	for (i=0; i<size2; i++)
				printf("0x%X ",(unsigned char)tmp2[i]);
	//printf("%d",chyb);
	

/*
	ConnectClient(modbus1);
	printf("%s\n",GetErrorMessage(LastClientError(modbus1)));

	char buffer[MSGSIZE];
	char answer[260];
	DWORD answer_len = 0;
	
	//sprintf(buffer, "%c%c%c%c%c%c%c%c%c%c%c%c", 0x00, 0x01, 0x00, 0x00, 0x00,
	//  0x06, 0x10, 0x03, 0x00, 0x00, 0x00, 0x12);
	//SendMsg(modbus1, buffer, 12);
	DWORD len;

	SendRequest(modbus1, 10, 0, 12, NULL, 0);//momentum - refresh (možná to je write holding register)
	ReadMsg(modbus1, buffer, &len);


	buffer[len] = '\0';
	answer_len = 0;
	for (i=0; i<len; i++)
	{
		sprintf(&answer[answer_len], "%X, ",(unsigned char)buffer[i]);
		answer_len = strlen(answer);
	}
	answer[answer_len] = '\0';
	printf("Prijato: %s\n\n", answer);


	//SendRequest(modbus1, 3, 0, 12, NULL);//Momentum - read holding registers

	// SERVER PODPORUJE: 1,2,3,4,  5,15,6,16
	//SendRequest(modbus1, 1, 0, 16, NULL);//read coils (bits)
	//SendRequest(modbus1, 2, 0, 16, NULL);//read discrete inputs (bits)
	//SendRequest(modbus1, 3, 0, 8, NULL);//read holding registers (bytes)
	//SendRequest(modbus1, 4, 0, 8, NULL);//read input registers (bytes)
	
	//unsigned char dat[6] = {00, 10,00, 20,00, 30};
	//SendRequest(modbus1, 16, 1, 3, dat, 6);//write holding registers

	//unsigned char dat[1] = {5};
	//SendRequest(modbus1, 15, 1, 8, dat, 1);//write coils

	//SendRequest(modbus1, 5, 3, 0xFF00, NULL, 0);//write coil
	//SendRequest(modbus1, 6, 3, 0x1234, NULL, 0);//write holding register

	unsigned short error = 0;
	if (WriteCoil(modbus1, 10, true) != 0)
		printf("Error - write coil\n\n");

	bool state = false;
	if ((error = ReadCoil(modbus1, 10, &state)) != 0)
		printf("Error - read coil %d\n\n", error);
	else printf("Coil 10 is %d\n\n", state);

	if ((error = WriteHoldingRegister(modbus1, 6, 0x1234)) != 0)
		printf("Error - write register %d\n\n", error);

	WORD value = 0;
	if ((error = ReadHoldingRegister(modbus1, 6, &value)) != 0)
		printf("Error - read register %d\n\n", error);
	else printf("Register 6 is 0x%X\n\n", value);

	WORD values1[2]={23,11};
	if ((error = WriteHoldingRegisters(modbus1,2,2,values1)) != 0)
		printf("Error - write registers %d\n\n", error);

	bool values2[3]={true,true,true};
	if ((error = WriteCoils(modbus1,4,3,values2)) != 0)
		printf("Error - write coils %d\n\n", error);

	bool *states = NULL;
	WORD pocet = 8;
	if ((error = ReadCoils(modbus1, 4, pocet, &states)) != 0)
		printf("Error - read coils %d\n\n", error);
	else
	{
		for (i=0; i<pocet; i++)
			printf("Coil %d is %d\n", i+4, states[i]);
	}
	printf("\n");

	WORD *values = NULL;
	pocet = 8;
	if ((error = ReadHoldingRegisters(modbus1, 1, pocet, &values)) != 0)
		printf("Error - read registers %d\n\n", error);
	else
	{
		for (i=0; i<pocet; i++)
			printf("Register %d is 0x%X\n", i+1, values[i]);
	}
	printf("\n");




	DisconnectClient(modbus1);
	printf("%s\n",GetErrorMessage(LastClientError(modbus1)));
	/*
	ConnectClient(modbus1);
	PrintLastError(modbus1);

	SendRequest(modbus1, 3, 0, 12, NULL);	
	PrintLastError(modbus1);

	ReadMsg(modbus1, buffer, 260, &len);
	PrintLastError(modbus1);

	buffer[len] = '\0';
	answer_len = 0;
	for (i=0; i<len; i++)
	{
		sprintf(&answer[answer_len], "%X, ",(unsigned char)buffer[i]);
		answer_len = strlen(answer);
	}
	answer[answer_len] = '\0';
	printf("Prijato: %s\n\n", answer);
*/
	DestroyClient(modbus1);
	DestroyServer(modbusX);
	ReleaseLibrary();

	getchar();
    return 0;
}
#endif