/******************************************************************
	Example for reading modbus registers over TCP in Pico-C
	Not perfect but working

	Check how your modbus device behaves when more than one client is connecting...
	
	Andrej Konkow, 01/2026
 
	...Pico-C is in somekind very special in syntax and possibilities...
******************************************************************/

/********** Main configuration block ****************
*/
// Do we configure this code from inside ourself or is a
// Textgenerator-component connected. Either keep the line or
// outcomment the next line like this:    // #define CODE...
#define CODECONFIG_BY_TEXTGENERATOR

// If not using the textgenerator specify your data here
#ifndef CODECONFIG_BY_TEXTGENERATOR
	#define MODBUS_INPUT_IP_PORT     "192.168.13.xx:1502,1"
	#define MODBUS_INPUT_ADDRESS_HEX "9D0E"
	#define MODBUS_INPUT_RESULTS_DT  "20x2"
	#define MODBUS_INPUT_BIGENDIAN   1
#endif
/*
****************************************************/

// supported modbus datatypes
enum registerDatatype
{
	DT_NONE  = 0,
	UINT16   = 1,
	INT16    = 2,
	UINT32   = 3,
	INT32    = 4,
	FLOAT16  = 5,
	FLOAT32  = 6,
	STRING16 = 7,
	STRING32 = 8,
	COIL     = 9
};

// Shall we run in debugmode and print some message on the outputchannels?  0 or 1
#define DEBUGMODE 0

// Inputchannels when using the textgenerator component
#ifdef CODECONFIG_BY_TEXTGENERATOR
	#define MODBUS_INPUT_IP_PORT     getinputtext(0)
	#define MODBUS_INPUT_ADDRESS_HEX getinputtext(1)
	#define MODBUS_INPUT_RESULTS_DT  getinputtext(2)
	#define MODBUS_INPUT_BIGENDIAN   getinput(0)
#endif

// Supported modbus functioncodes
#define MODBUS_FC_READCOILS            0x01
#define MODBUS_FC_READHOLDINGREGISTERS 0x03

// byte-size of message being sent to modbus-device
#define MODBUS_MSG_SIZE 12

// Payload bytes which can be ignored when getting the values from the response
#define MODBUS_RESPONSE_OFFSET 9

// Our own transaction id, as we like but unique
#define MODBUS_TRANSACTIONID 140

// Size of char-array holding the connectionstring for target device
#define MODBUS_CONNECTIONSTRING_SIZE 60

// Time spent in reading the responsestream/waiting for data, timeout in ms
#define MODBUS_READTIME_MS 1000

// Blocksize upper limit for reading response data
#define RD_BLOCK_SIZE 300

// Define how many Output channels are available before handing over
// the remaining results. In Loxone programs currently can address
// 13 output channels for number-values in maximum
#define MAX_OUTPUT_CHANNELS 13

// Maximum of displayable results over all components
#define MAX_OUTPUT_CHANNELS_TOTAL 2*MAX_OUTPUT_CHANNELS

// Maximum of sum of result datatypes to be defined in input component
#define MAX_DATATYPES_DEFINED_EXT 100

// Sleeptime before repeating request in milliseconds
#define REPEATER_SLEEP_MS 10000

// Definition of delimiters for datainput, defining
// datatypes and the number of results
#define DELIMITER_DT_BLOCKS ","
#define DELIMITER_DT_NUMBER "x"

#define STRING_NOPARAM      -99999

/*
	Convert characters/bytes to readable hex-string
	Caution: Don't use strlen or other string-oriented functions as
	byte-characters don't behave like normal alphabetical characters
	
	Due to efficiency reasons the parameter firstfill tells us with "0" that
	"dest" is used for the first time. Else Non-"0". 
	Otherwise we would have to use strlen() which can cause time delays when
	used excessively.
*/
void convertToReadableHex(char *bytes, int byteLen, char *dest, int firstfill)
{
	int  i;
	char tmpString[5];

	for (i=0; i<byteLen; i++)
	{
		if (i == 0 && firstfill == 0)
			sprintf(tmpString, "%02x", bytes[i]);
		else
			sprintf(tmpString," %02x", bytes[i]);

		strcat(dest, tmpString);
	}
}

/*
	Convert hex-String to bytes, hex-characters are handled 4bit-wise
	Be aware: For example two string-characters like 1C have to result
	in one 8bit byte-character
	
	To avoid free()-statements organize the memory allocation outside
	of this function and hand over "resultBytes"
	For the ease of use errorhandling is not priority...
	
	Returns the length of the characters created
*/
int convertHexToBytes(char *hexString, char *resultBytes)
{
	int hexlen, resultcounter, i;
    char bit4_1, bit4_2; 

	hexlen = strlen(hexString);
	resultcounter = 0;
	i = 0;
	
	do
	{
        resultBytes[resultcounter] = 0;
		
		// Unhandled errorcase
		if (i+1 > hexlen)
			break;
		
		bit4_1 = hexString[i]; 
        bit4_2 = hexString[i+1]; 

		// Ignore whitespaces
		if (bit4_1 == ' ')
		{
			i++;
			continue;
		}

		// Do the transformation: 1 Byte is 2x 4bits to be converted
		// Read pairwise
		if      (bit4_1 >= '0' && bit4_1 <= '9') bit4_1 = bit4_1 - '0';
		else if (bit4_1 >= 'a' && bit4_1 <='f')  bit4_1 = bit4_1 - 'a' + 10;
		else if (bit4_1 >= 'A' && bit4_1 <='F')  bit4_1 = bit4_1 - 'A' + 10;    

		if      (bit4_2 >= '0' && bit4_2 <= '9') bit4_2 = bit4_2 - '0';
		else if (bit4_2 >= 'a' && bit4_2 <='f')  bit4_2 = bit4_2 - 'a' + 10;
		else if (bit4_2 >= 'A' && bit4_2 <='F')  bit4_2 = bit4_2 - 'A' + 10;    
		
		// stick together the two 4bit values 
		resultBytes[resultcounter] = (bit4_1 << 4) | (bit4_2 & 0xF);

		i = i+2;
		resultcounter++;
    }
	while (i < hexlen);

	return resultcounter;
}

/*
	The bytes represent ascii-encoded values. Decode them...
*/
void decodeBytes2Ascii(char *src, char *dest, int len)
{
	char hlp[33], tmpString[35];
	init(hlp, 33, 0);
	
	int tmpLen = strlen(src);
	if (tmpLen < len)
		len = tmpLen;

	if (len == 0)
		return;
	
	strncpy(hlp, src, len);
	if (dest[0] == 0)
		sprintf(tmpString, "%s", hlp);
	else
		sprintf(tmpString, ", %s", hlp);

	strcat(dest, tmpString);
}

/*
	Swap all bytes according to their datatype
*/
void swapAllBytes(char *bytes, int *listDataTypes, int sizeListDataTypes, int littleendian)
{
	int i,
		result_type,
		offsetCnt=0;
	
	if (littleendian)
		return;
	
	for (i=0; i<sizeListDataTypes; i++)
	{
		result_type = listDataTypes[i];
		
		swapBytes(&bytes[offsetCnt], result_type);
		offsetCnt += getBytesizeByType(result_type);
	}
}

/*
	PicoC works little endian
	Supported values are arraylengths of 2 and 4
*/
void swapBytes(char *bytes, int result_type)
{
	// In these cases return unmodified
	if (result_type == STRING16  || 
		result_type == STRING32  || 
		result_type == COIL)
		return;
		
	// Maximum size possible
	char tmpBytes[4];
	int  len = 0;
	int  i=0;

	len = getBytesizeByType(result_type);
	
	for (i=0; i < len; i++) 
		tmpBytes[i] = bytes[i];
	
	if (len == 2)
	{
		bytes[0] = tmpBytes[1];
		bytes[1] = tmpBytes[0];		
	}
	else if (len == 4)
	{
		bytes[0] = tmpBytes[3];
		bytes[1] = tmpBytes[2];
		bytes[2] = tmpBytes[1];
		bytes[3] = tmpBytes[0];
	}
}

/*
	Helper function for retrieving bytesizes
*/
int getBytesizeByType(int type)
{
		if (type == UINT16      || type == INT16 || type == FLOAT16 || type == DT_NONE)
			return 2;
		else if (type == UINT32 || type == INT32 || type == FLOAT32)
			return 4;		
		else if (type == STRING16)
			return 16;		
		else if (type == STRING32)
			return 32;		
		else if (type == COIL)
			return 1;
		
		return 0;	// Unknown	
}

/*
	Helper function for retrieving bytesizes
*/
int getBytesizeByList(int *types, int sizeOfList, int countNone)
{
	int bytesize=0, 
		i;
	
	for (i=0; i<sizeOfList; i++)
	{
		if (types[i] == DT_NONE && !countNone)
			continue;

		bytesize += getBytesizeByType(types[i]);
	}
	
	// Bytesize of Coils has to be handled different
	if (types[0] == COIL)
		bytesize = (bytesize + 7) / 8;
	
	return bytesize;
}

/*
	calc float16 value
*/
float calc_float16(char *flt16_bits)
{ 
    char sign_bit;
    char exponent;
    unsigned int  significand;

    // IEEE 754 half precision
    sign_bit    =  (flt16_bits[0] & 0x80) >> 7;
    exponent    = ((flt16_bits[0] & 0x7C) >> 2);
    significand = ((flt16_bits[0] & 0x03) << 8) | (flt16_bits[1]);

	return (float)(pow(-1,sign_bit) * (1 + significand/pow(2,10)) * pow(2,(exponent - 31)));
}

/*
	calc float32 value
*/
float calc_float32(char *flt32_bits)
{ 
    char sign_bit;
    char exponent;
    unsigned int  significand;

    // IEEE 754 single precision
    sign_bit    =   (flt32_bits[1] & 0x80) >> 7;
    exponent    =  ((flt32_bits[1] & 0x7F) << 1)  | ((flt32_bits[0] & 0x80) >> 7);
    significand = (((flt32_bits[0] & 0x7F) << 16) |  (flt32_bits[3] << 8) | (flt32_bits[2]));

	return (float)(pow(-1,sign_bit) * (1 + significand/pow(2,23)) * pow(2,(exponent - 127)));
}

/*
	Helperfunctions for initialization
*/
void init      (char           *array, int size, int            value) { int x; for (x=0; x<size; x++) array[x] = value; }
void initFloat (float          *array, int size, float          value) { int x; for (x=0; x<size; x++) array[x] = value; }
void initUShort(unsigned short *array, int size, unsigned short value) { int x; for (x=0; x<size; x++) array[x] = value; }
void initSShort(signed   short *array, int size, signed   short value) { int x; for (x=0; x<size; x++) array[x] = value; }
void initUInt  (unsigned int   *array, int size, unsigned int   value) { int x; for (x=0; x<size; x++) array[x] = value; }
void initSInt  (signed   int   *array, int size, signed   int   value) { int x; for (x=0; x<size; x++) array[x] = value; }

/*
	Build connection string from an ip address including the port.
	No errorhandling is done as we assume a correct address
*/
void buildConnectionString(char *targetaddress, char *connectionString, int *modbusId)
{
	char ip[25], port[10], mbId[5];
	int  colon, comma;
	
	init(mbId, 5, 0);
	
	colon = strfind(targetaddress,":",0);
	comma = strfind(targetaddress,",",0);

	strncpy(ip, targetaddress, colon);

	if (comma == -1)
	{
		strncpy(port, &targetaddress[colon+1], strlen(targetaddress)-colon-1);
		*modbusId = 1;
	}
	else
	{
		strncpy(port, &targetaddress[colon+1], comma-colon-1);
		strncpy(mbId, &targetaddress[comma+1], strlen(targetaddress)-comma-1);
		*modbusId = batoi(mbId);
	}
	
	sprintf(connectionString,"/dev/tcp/%s/%s", ip, port);

	debugMsg(connectionString);
}

/*
The structure of a Modbus TCP message is:
Transaction Id(2 bytes), Protocol(2 bytes), Length(2 bytes), Unit Address(1 byte), Message(N bytes)

Where:
    The Transaction Id field identifies the transaction.
    The Protocol field is zero to indicate Modbus protocol.
    The Length field is the number of following bytes.
    The Unit Address field is the PLC Address encoded as single byte.
    The Message field is a Modbus PDU. The maximum length of the Message field is is 253 bytes.
    The maximum Modbus TCP message length is 260 bytes.
*/
void buildModbusRequest(char *modbusMsg, char *startAddress, int modbus_readregister_count, int functioncode, int modbusId)
{
	modbusMsg[0] = 0x00;
	modbusMsg[1] = MODBUS_TRANSACTIONID;

	modbusMsg[2] = 0x00;
	modbusMsg[3] = 0x00;

	modbusMsg[4] = 0x00;  // Bytelength of Unit-Id and PDU-Message hi
	modbusMsg[5] = 0x06;  // Bytelength of Unit-Id and PDU-Message lo

	// Beginning of PDU-Message
	modbusMsg[6] = modbusId;
	modbusMsg[7] = functioncode;

	// Set register startaddresses hi an low hex
	// modbusMsg[8] and [9] are filled up
	convertHexToBytes(startAddress, &modbusMsg[8]);

	modbusMsg[10] = 0x00;
	modbusMsg[11] = modbus_readregister_count;
}

/*
	Check for errors in receiving the modbus response
*/
int checkForErrors(int bytesReceived, char *modbusResp, int expected_bytesize)
{
	char errorMsg[100], error_code;

	// Check for Modbus-Error message. Means: First bit of byte 8 is set.
	// In that case the following byte holds the exception code
	error_code = bytesReceived > 8 && modbusResp[7] >= 129 && modbusResp[7] <= 134;
		
	if ( bytesReceived - MODBUS_RESPONSE_OFFSET < 0 || 
		 bytesReceived != expected_bytesize || 
		 error_code == 1 )
	{
		if (error_code == 0)
			sprintf(errorMsg,"%s", "Reading nok or no connection to device.");
		else
			sprintf(errorMsg,"%s Exceptioncode hex: %02x", "Reading nok.", modbusResp[8]);

		setoutputtext(0, errorMsg);
		setoutputtext(1, "");
		return 1;
	}
	
	return 0;
}

/*
	Get the datatype per outputchannel and store it in an array
	Keep your data clean as errorhandling is not good
	
	Returns the total number of results (not bytes) to read
*/
int parseDatatypes(char *dtString, int *listOfDatatypes, int *listOfDatatypesView, int maxOutputChannels)
{
	int  delim_block=0, delim_number=0,
		 dtSum=0, dtSumView=0, i, 
		 noResults, datatype;
	char szResults[3], szDatatype[2];

	do
	{
		delim_number = strfind(&dtString[delim_block], DELIMITER_DT_NUMBER, 0);
		strncpy(szResults,  &dtString[delim_block], delim_number);
		strncpy(szDatatype, &dtString[delim_block+delim_number+1], 1);

		noResults = batoi(szResults);
		datatype  = batoi(szDatatype);

		for (i=0; i<noResults; i++)
		{
			listOfDatatypes[dtSum] = datatype;
			dtSum++;			
			
			if (datatype != DT_NONE)
			{
				listOfDatatypesView[dtSumView] = datatype;
				dtSumView++;
			}
			
			// Have we reached the maximum number of displayable Results?
			// Avoid an overflow
			if (dtSumView == maxOutputChannels)
				break;
		}

		delim_block = strfind(dtString, DELIMITER_DT_BLOCKS, delim_block+1);

		// Have we reached the last block?
		if (delim_block == -1)
			break;

		delim_block++;
	}
	while (TRUE);
	
	return dtSum;
}

/*
	Print status and readable information
*/
void printReadingStatus(int clearError, char *results, char *addresses)
{
	char status[RD_BLOCK_SIZE];

	if (strlen(results) > 0)
		sprintf(status,"Reading ok, %s, %s", results, addresses);
	else
		sprintf(status,"%s, %s", "Reading ok", addresses);

	// Remove existing errormsg/refresh output channel
	if (clearError == 1)
		setoutputtext(0, "");	
	setoutputtext(1, status);
}

/*
	Hexify message for handover
	Only handover results to be displayed and already byte-swapped
*/
void msgMarshalling(char *hexOutput, int hexOutputSize, int *szListOfDatatypes, int *szListOfDatatypesView, int maxResults, char *modbusResponse, int nextStartingAddress)
{
	int   i, j, result_type, result_bytesize,
		  idxOffset, outputCnt, outputChannel;
	char  szResultHelper[3];
	short sNextStartingAddress;
	
	// Handover the remaining results as hex-string
	init(hexOutput, hexOutputSize, 0);

	// First hexify the nextStartingAddress (just a displaying issue in the next component)
	sNextStartingAddress = (short)nextStartingAddress;
	memcpy(szResultHelper, &sNextStartingAddress, 2);
	convertToReadableHex(szResultHelper, 2, hexOutput, 0);

	// Hexify the remaining result datatypes
	for (i=0; i<MAX_OUTPUT_CHANNELS; i++)
	{
		outputChannel = MAX_OUTPUT_CHANNELS+i;
		// Downcasting in sizes is ok here
		result_type = szListOfDatatypesView[outputChannel];
		szResultHelper[0] = (char)result_type;
		convertToReadableHex(szResultHelper, 1, hexOutput, 1);
	}

	// Hexify the remaining results themselves
	for (i=0; i<MAX_OUTPUT_CHANNELS; i++)
	{
		outputChannel = MAX_OUTPUT_CHANNELS+i;
		result_type   = szListOfDatatypesView[outputChannel];
		if (result_type == DT_NONE)
			return;

		// Putting the next for-iteration in a subfunction
		// leads to instability when redeploying on miniserver
		// so don't use ctrl_getResponseOffset()
		idxOffset = MODBUS_RESPONSE_OFFSET;
		outputCnt = 0;
		for (j=0; j<maxResults; j++)
		{
			if (outputCnt == outputChannel)
				break;
			
			if (szListOfDatatypes[j] != DT_NONE)
				outputCnt++;

			idxOffset += getBytesizeByType(szListOfDatatypes[j]);
		}

		result_bytesize = getBytesizeByType(result_type);
		convertToReadableHex(&modbusResponse[idxOffset], result_bytesize, hexOutput, 1);
	}
}

/*
	In case of debugmode print a message and have a short break to
	assure that the message can be read
*/
void debugMsg     (char *msg) { debugMsgParam(msg, STRING_NOPARAM); }
void debugMsgParam(char *msg, int val)
{
	if (!DEBUGMODE)
		return;

	char debugmsg[200];
	
	sprintf(debugmsg,"[DEBUG] %s", msg);
	messageParamTimed(debugmsg, val, 0);
}

void messageParam     (char *msg, int val)              { messageParamTimed       (msg, val, 1000); }
void messageParamTimed(char *msg, int val, int sleepMs) { messageParamTimedChannel(msg, val, sleepMs, 0); }
void messageParamTimedChannel(char *msg, int val, int sleepMs, int channel)
{
	char outMsg[200];
	
	if (val == STRING_NOPARAM)
		sprintf(outMsg,"%s", msg);
	else
		sprintf(outMsg,"%s %d", msg, val);

	setoutputtext(channel, outMsg);
	if (sleepMs > 0)
		sleep(sleepMs);
}

/*
	Returns the index offset in the response for the given output channel
	If we haven't enough data to be displayed -1 is returned.
*/
int ctrl_getResponseOffset(int *listOfDatatypes, int listMaxSize, int outputChannel)
{
	int i, result_size,
		outputCnt = 0,
		currentType, 
		resultIdx = MODBUS_RESPONSE_OFFSET;
	
	for (i=0; i<listMaxSize; i++)
	{
		if (outputCnt == outputChannel)
			return resultIdx;
		
		currentType = listOfDatatypes[i];
		if (currentType != DT_NONE)
			outputCnt++;

		result_size = getBytesizeByType(currentType);
		resultIdx += result_size;
	}
	
	return -1;
}

/*
	Count number of result valid to be printed on output channels
*/
int getPrintableResults(int *listOfDatatypesView, int maxListSize)
{
	int outputCnt=0;
	
	do
	{
		if (listOfDatatypesView[outputCnt] == DT_NONE)
			break;
		
		outputCnt++;
	}
	while (outputCnt < maxListSize);
	
	return outputCnt;
}

/*
	Print addresses of displayed registers in a string
	
	Returns the next real registeraddress to be displayed
*/
int getDisplayedRegisters(int *listOfDatatypes, int listMaxSize, char *dest, int maxAmount, unsigned short startAddress)
{
	int  i, 
		 result_type, result_size,
		 isOutputType, 
		 outputCnt=0;
	char tmpString[10];
	
	for (i=0; i<listMaxSize; i++)
	{
		result_type  = listOfDatatypes[i];
		isOutputType = (result_type != DT_NONE);
		
		if (isOutputType)
		{
			if (i==0)
				sprintf(tmpString,  "%d", startAddress);
			else
				sprintf(tmpString,", %d", startAddress);

			strcat(dest, tmpString);
			outputCnt++;
		}
		
		if (result_type == COIL)
			result_size  = getBytesizeByType(result_type);
		else
			result_size  = getBytesizeByType(result_type) / 2;
		
		startAddress+= result_size;

		if (outputCnt == maxAmount)
			break;
	}	

	return startAddress;
}

/************************************************************************
	
	Now lets do the main work

	Mainfunctionality encapsulated in a function
	Due to picoC issues a little awkward
*/
void main()
{
	char  modbusRequest[MODBUS_MSG_SIZE], modbusResponse[RD_BLOCK_SIZE],
		  connectionString[MODBUS_CONNECTIONSTRING_SIZE],
		  modbusStrings[RD_BLOCK_SIZE], hexOutput[RD_BLOCK_SIZE],		  
		  *startAddressChar, handledAddressChar[3], 
		  handledAddresses[RD_BLOCK_SIZE], szResultHelper[3];
	int   baseIndex, nBytesReceived, 
		  errorInLastIteration=0, errorInCurrentIteration,
		  no_resultvalues, no_printValues,
		  result_type, result_bytesize, 
		  modbus_resultregister_type_bytesize, 
		  channel, virtChannel, bReadCoils=0,
		  bLittle_endian, modbusId, i,
		  continuedAddress,
		  szListOfDatatypes[MAX_DATATYPES_DEFINED_EXT],
		  szListOfDatatypesView[MAX_DATATYPES_DEFINED_EXT];

	short   sNextStartingAddress;
	STREAM* pTcpStream;

	unsigned short startAddress;
	
	unsigned short us_resultValues[MAX_OUTPUT_CHANNELS];
	signed   short ss_resultValues[MAX_OUTPUT_CHANNELS];
	unsigned int   ui_resultValues[MAX_OUTPUT_CHANNELS];
	signed   int   si_resultValues[MAX_OUTPUT_CHANNELS];
	float          f_resultValues[MAX_OUTPUT_CHANNELS];
	
	initSInt(szListOfDatatypes,     MAX_DATATYPES_DEFINED_EXT, DT_NONE);
	initSInt(szListOfDatatypesView, MAX_DATATYPES_DEFINED_EXT, DT_NONE);

	bLittle_endian  = ! MODBUS_INPUT_BIGENDIAN;
	no_resultvalues = parseDatatypes(MODBUS_INPUT_RESULTS_DT, szListOfDatatypes, szListOfDatatypesView, MAX_OUTPUT_CHANNELS_TOTAL);

	if (no_resultvalues > 0 && szListOfDatatypes[0] == COIL)
		bReadCoils = 1;
	
	modbus_resultregister_type_bytesize = getBytesizeByList(szListOfDatatypes, no_resultvalues, 1);
	startAddressChar                    = MODBUS_INPUT_ADDRESS_HEX;

	convertHexToBytes(startAddressChar, &handledAddressChar[0]);
	startAddress = (handledAddressChar[1]) | (handledAddressChar[0] << 8);

	buildConnectionString(MODBUS_INPUT_IP_PORT, connectionString, &modbusId);

	// Coils are handled different
	if (bReadCoils)
	{
		buildModbusRequest(modbusRequest, startAddressChar, no_resultvalues, MODBUS_FC_READCOILS, modbusId);
		no_printValues = modbus_resultregister_type_bytesize;
	}
	else
	{
		buildModbusRequest(modbusRequest, startAddressChar, modbus_resultregister_type_bytesize/2, MODBUS_FC_READHOLDINGREGISTERS, modbusId);
		no_printValues = no_resultvalues;	
	}

	continuedAddress = getDisplayedRegisters(szListOfDatatypes, no_resultvalues, handledAddresses, MAX_OUTPUT_CHANNELS, startAddress);

	// Keep polling
	while (TRUE)
	{
		init(modbusStrings, RD_BLOCK_SIZE, 0);

		initUShort(us_resultValues, MAX_OUTPUT_CHANNELS, 0);
		initSShort(ss_resultValues, MAX_OUTPUT_CHANNELS, 0);
		initUInt(ui_resultValues,   MAX_OUTPUT_CHANNELS, 0);
		initSInt(si_resultValues,   MAX_OUTPUT_CHANNELS, 0);
		initFloat(f_resultValues,   MAX_OUTPUT_CHANNELS, 0.0);

		errorInCurrentIteration = 0;
		virtChannel             = 0;
		channel                 = 0;

		messageParamTimedChannel("Reading Modbus-TCP", STRING_NOPARAM, 0, 1);

		/****************/
		// TCP Request/Response. Putting this in a function leads to instability on 
		// the miniserver when deploying.
		pTcpStream = stream_create(connectionString, 0, 0);       // create tcp stream
		stream_write(pTcpStream, modbusRequest, MODBUS_MSG_SIZE); // write to stream
		stream_flush(pTcpStream);                                 // flush output stream buffer
		nBytesReceived = stream_read(pTcpStream, modbusResponse, RD_BLOCK_SIZE, MODBUS_READTIME_MS);
		stream_close(pTcpStream);
		/****************/

		if (checkForErrors(nBytesReceived, modbusResponse, MODBUS_RESPONSE_OFFSET+modbus_resultregister_type_bytesize))
		{
			errorInLastIteration = 1;
			sleep(REPEATER_SLEEP_MS);
			continue;
		}

		if (!bReadCoils)
			swapAllBytes(&modbusResponse[MODBUS_RESPONSE_OFFSET], szListOfDatatypes, no_resultvalues, bLittle_endian);

		messageParamTimedChannel("Processing received bytes: ", nBytesReceived, 0, 1);

		// Resulthandling
		// Buffers are big enough. So take the bytes from the responsestream and put them
		// into the result types/values	
		for (i=0; i<no_printValues && channel<MAX_OUTPUT_CHANNELS; i++)
		{
			result_type = szListOfDatatypesView[i];
			if (result_type == DT_NONE)
				break;
			
			baseIndex = ctrl_getResponseOffset(szListOfDatatypes, no_resultvalues, channel+virtChannel);
			if (baseIndex == -1)
			{
				errorInCurrentIteration = 1;
				break;
			}
				
			// Decode Bits/Bytes to values
			switch (result_type)
			{
				case UINT16:	// sizeof(short) == 2 == 16bit
					us_resultValues[channel] = (modbusResponse[baseIndex]) | (modbusResponse[baseIndex + 1] << 8);	
					break;

				case INT16:		// sizeof(short) == 2 == 16bit
					ss_resultValues[channel] = (modbusResponse[baseIndex]) | (modbusResponse[baseIndex + 1] << 8);
					break;

				case UINT32:	// sizeof(int)   == 4 == 32bit
					ui_resultValues[channel] = (modbusResponse[baseIndex] << 16) | (modbusResponse[baseIndex + 1] << 24) | (modbusResponse[baseIndex + 2]) | (modbusResponse[baseIndex + 3]  << 8);
					break;

				case INT32:		// sizeof(int)   == 4 == 32bit
					si_resultValues[channel] = (modbusResponse[baseIndex] << 16) | (modbusResponse[baseIndex + 1] << 24) | (modbusResponse[baseIndex + 2]) | (modbusResponse[baseIndex + 3]  << 8);
					break;
				
				case FLOAT16:	// sizeof(float) == 8 == 64bit
					f_resultValues[channel] = calc_float16(&modbusResponse[baseIndex]);
					break;
				
				case FLOAT32:	// sizeof(float) == 8 == 64bit
					f_resultValues[channel] = calc_float32(&modbusResponse[baseIndex]);
					break;

				case STRING16:
				case STRING32:
					result_bytesize = getBytesizeByType(result_type);
					decodeBytes2Ascii(&modbusResponse[baseIndex], modbusStrings, result_bytesize);
					virtChannel++;
					channel--;
					break;

				case COIL:	// In case of coils the number of results are coded in bits. Means: 8 coils  == 1 Byte
					setoutput(channel, modbusResponse[baseIndex]);	// sizeof(char)  == 1 == 8bit
					break;
			}
			
			channel++;
		}

		if (errorInCurrentIteration)
		{
			messageParamTimedChannel("Unexpected data error. Received bytes: ", nBytesReceived, 0, 1);
			sleep(REPEATER_SLEEP_MS);
			continue;
		}
		
		// Handover remaining results. Due to computing delays first do this.
		if (channel+virtChannel < getPrintableResults(szListOfDatatypesView, no_resultvalues) && !bReadCoils)
		{
			msgMarshalling(hexOutput, RD_BLOCK_SIZE, szListOfDatatypes, szListOfDatatypesView, no_resultvalues, modbusResponse, continuedAddress);
			setoutputtext(0, hexOutput);
		}

		// Print results on output
		virtChannel = 0;
		for (i=0; i<MAX_OUTPUT_CHANNELS; i++)
		{
			result_type = szListOfDatatypesView[i];
			if (result_type == DT_NONE)
				break;

			channel = i - virtChannel;

			// Display all values in a rush
			switch (result_type)
			{
				case UINT16:   setoutput(channel, us_resultValues[channel]); break;
				case INT16:	   setoutput(channel, ss_resultValues[channel]); break;
				case UINT32:   setoutput(channel, ui_resultValues[channel]); break;
				case INT32:	   setoutput(channel, si_resultValues[channel]); break;
				case FLOAT16:	
				case FLOAT32:  setoutput(channel, f_resultValues[channel]); break;
				case STRING16:
				case STRING32: virtChannel++; break;
			}			
		}

		printReadingStatus(errorInLastIteration, modbusStrings, handledAddresses);
	
		errorInLastIteration = 0;	
		sleep(REPEATER_SLEEP_MS);
	}
}

main();