/******************************************************************
	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, 11/2025
 
	...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, values 0 or 1
#define CODECONFIG_BY_TEXTGENERATOR 1

// Following 4 lines can be ignored when using the textgenerator
#define MODBUS_INPUT_IP_PORT     "192.168.13.xx:1502"
#define MODBUS_INPUT_ADDRESS_HEX "9D0E"
#define MODBUS_INPUT_NO_RESULTS  20
#define MODBUS_INPUT_DATATYPE    INT16

// Specify whether the modbus-device being read delivers its data big endian(1) or not(0)
#define MODBUS_DEVICE_BIGENDIAN   1

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

// supported modbus datatypes
enum registerDatatype
{
	DT_NONE  = 0,
	UINT16,
	INT16,
	UINT32,
	INT32,
	FLOAT16,
	FLOAT32,
	STRING16,
	STRING32,
	COIL
};

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

// Mostly a default value but configurable
#define MODBUS_DEVICE_ID 1

// 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 40

// Time spent in reading the responsestream/waiting for data, timeout in ms
// This timeout is a critical one. Redeployments on the miniserver during streamreading
// results in a miniserver crash. To my knowledge there is no workaround but minimizing 
// this value to a minimum, but high enough dealing with your device. The stream reading
// implementation is provided by Loxone itself. As this error is existing for over 10 years
// lets hope the best for the next 10 years. :-)
#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

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

/*
	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
*/
void convertToReadableHex(char *bytes, int byteLen, char *dest)
{
	int i;
	
	if (byteLen > 0)
	{
		if (strlen(dest) == 0)
			sprintf(dest,"%02x", bytes[0]);
		else
			sprintf(dest,"%s %02x",dest, bytes[0]);
	}
	
	for (i=1; i<byteLen; i++)
		sprintf(dest,"%s %02x",dest,bytes[i]);
}

/*
	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[32];
	init(hlp, 32, 0);
	
	int tmpLen = strlen(src);
	if (tmpLen < len)
		len = tmpLen;

	if (len == 0)
		return;
	
	strncpy(hlp, src, len);
	if (strlen(dest) > 0)
		sprintf(dest,"%s, %s", dest, hlp);
	else
		sprintf(dest,"%s", hlp);
}

/*
	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 (!MODBUS_DEVICE_BIGENDIAN ||
		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)
			return 2;
		else if (type == UINT32 || type == INT32 || type == FLOAT32)
			return 4;		
		else if (type == STRING16)
			return 8;		
		else if (type == STRING32)
			return 16;		
		else if (type == COIL)
			return 1;
		
		return 0;	// Unknown	
}

/*
	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)));
}

/*
	Helperfunction for initialization
*/
char* init(char *array, int size, int value)
{
	int x;
	
	for (x=0; x<size; x++)
		array[x] = value;
	
	return array;
}

/*
	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)
{
	char ip[25], port[10];
	int  colon;
	
	colon = strfind(targetaddress,":",0);

	strncpy(ip,   targetaddress, colon);
	strncpy(port, &targetaddress[colon+1], strlen(targetaddress)-colon-1);
	sprintf(connectionString,"/dev/tcp/%s/%s", ip, port);
}

/*
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, int modbus_readregister_count, int functioncode)
{
	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] = MODBUS_DEVICE_ID;    // Unit address, see documentation of device, good guess if unknown: 1
	modbusMsg[7] = functioncode;

	// Set register startaddresses hi an low hex
	// modbusMsg[8] and [9] are filled up
	if (CODECONFIG_BY_TEXTGENERATOR)
		convertHexToBytes(getinputtext(1), &modbusMsg[8]);
	else
		convertHexToBytes(MODBUS_INPUT_ADDRESS_HEX, &modbusMsg[8]);

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

/*
	Execute Modbusrequest and read the results
*/
int doModbusRequest(char *connectionString, char *modbusRequest, int sizeModbusReq, char *modbusResp, int sizeModbusResp, int no_results, int result_type)
{
	int bytesReceived;
	int bytesWritten;
			
	// Request
	debugMsg("Start creating stream");
	STREAM*	pTcpStream = stream_create(connectionString,0,0); // create tcp stream
	
	if (pTcpStream == NULL)
	{
		debugMsg("Error in creating stream");
		return 0;
	}

	debugMsg("Finished creating stream, now writing");
	bytesWritten = stream_write(pTcpStream, modbusRequest, sizeModbusReq); // write to stream
	
	if (bytesWritten != sizeModbusReq)
	{
		debugMsgParam("Writing stream not successful. Written bytes: ", bytesWritten);
		stream_close(pTcpStream);
		return 0;
	}

	debugMsg("Finished writing stream, now flushing");
	stream_flush(pTcpStream); // flush output stream buffer
	/*
		char hexHelper[RD_BLOCK_SIZE];		
		init(hexHelper, RD_BLOCK_SIZE, 0);
		convertToReadableHex(modbusRequest, sizeModbusReq, hexHelper);
		setoutputtext(0, hexHelper);
	*/
	
	/* 
	   Response
	   Following line is the critical part: Redeployments on the miniserver during this action
	   results in a miniserver crash. To minimize risk: keep the MODBUS_READTIME_MS very low but
	   sufficient for your own device. Consult your device documentation.
	*/
	debugMsg("Start reading stream");
	bytesReceived = stream_read(pTcpStream,modbusResp,sizeModbusResp,MODBUS_READTIME_MS);
	debugMsgParam("Finished reading stream. Received bytes: ", bytesReceived);

	stream_close(pTcpStream);
	/*
		// Print results before bytes are swapped big/little endian
		char hexHelper[RD_BLOCK_SIZE];		
		init(hexHelper, RD_BLOCK_SIZE, 0);
		convertToReadableHex(modbusResp, bytesReceived, hexHelper);
		setoutputtext(2, hexHelper); 
	*/

	return bytesReceived;
}

/*
	Check for errors in receiving the modbus response
*/
int checkForErrors(int bytesReceived, char *modbusResp, int resultregister_type_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 - MODBUS_RESPONSE_OFFSET) % resultregister_type_bytesize) != 0 || 
		  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;
}

/*
	Print status and readable information
*/
void printReadingStatus(int clearError, char *status, char *results)
{
	sprintf(status,"%s", "Reading ok");
	if (strlen(results) > 0)
		sprintf(status,"%s, %s", status, results);

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

/*
	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, -9999); }
void debugMsgParam(char *msg, int val)
{
	if (!DEBUGMODE)
		return;
	
	char debugmsg[150];

	if (val == -9999)
		sprintf(debugmsg,"[DEBUG] %s", msg);
	else
		sprintf(debugmsg,"[DEBUG] %s %d", msg, val);
	
	setoutputtext(0, debugmsg);
	sleep(1000);
}

/************************************************************************
	
	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],
		  okString[RD_BLOCK_SIZE],
		  modbusStrings[RD_BLOCK_SIZE], hexOutput[RD_BLOCK_SIZE];
	int   baseIndex, nBytesReceived, errorInLastIteration,
		  no_resultvalues, no_printValues,
		  result_type, modbus_resultregister_type_bytesize, 
		  channel, i;
	char  szResultHelper[2];
	
	unsigned short us_resultValue;
	signed   short ss_resultValue;
	unsigned int   ui_resultValue;
	signed   int   si_resultValue;

	if (CODECONFIG_BY_TEXTGENERATOR)
	{
		no_resultvalues = getinput(0);
		result_type     = getinput(1);

		buildConnectionString(getinputtext(0), connectionString);
	}
	else
	{
		no_resultvalues = MODBUS_INPUT_NO_RESULTS;
		result_type     = MODBUS_INPUT_DATATYPE;
		
		buildConnectionString(MODBUS_INPUT_IP_PORT, connectionString);
	}		
	
	// Byte-size of result registers
	// For the ease of use all resultsizes are assumed to be the same size
	modbus_resultregister_type_bytesize = getBytesizeByType(result_type);

	if (result_type == COIL)
	{
		buildModbusRequest(modbusRequest, no_resultvalues, MODBUS_FC_READCOILS);
		no_printValues = (no_resultvalues + 7) / 8;
	}
	else
	{
		buildModbusRequest(modbusRequest, no_resultvalues*(modbus_resultregister_type_bytesize/2), MODBUS_FC_READHOLDINGREGISTERS);
		no_printValues = no_resultvalues;	
	}

	// if we have more than MAX_OUTPUT_CHANNELS results (limited outputchannels...) 
	// then hand over results after MAX_OUTPUT_CHANNELS-1 outputs
	if (no_printValues > MAX_OUTPUT_CHANNELS)
		no_printValues = MAX_OUTPUT_CHANNELS;

	// Keep polling
	while (TRUE)
	{
		init(modbusStrings, RD_BLOCK_SIZE, 0);
		init(okString, 		RD_BLOCK_SIZE, 0);
		
		setoutputtext(1, "Reading Modbus-TCP. Do not interrupt!");
		nBytesReceived = doModbusRequest(connectionString, modbusRequest, MODBUS_MSG_SIZE, modbusResponse, RD_BLOCK_SIZE, no_resultvalues, result_type);
		setoutputtext(1, "Finished reading Modbus-TCP");
				
		if (checkForErrors(nBytesReceived, modbusResponse, modbus_resultregister_type_bytesize))
		{
			errorInLastIteration = 1;
			sleep(REPEATER_SLEEP_MS);
			continue;
		}

		// 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; i++)
		{
			baseIndex = MODBUS_RESPONSE_OFFSET + i * modbus_resultregister_type_bytesize;

			swapBytes(&modbusResponse[baseIndex], result_type);
			
			// Decode Bits/Bytes to values
			switch (result_type)
			{
				case UINT16:	// sizeof(short) == 2 == 16bit
					us_resultValue = (modbusResponse[baseIndex]) | (modbusResponse[baseIndex + 1] << 8);	
					setoutput(i, us_resultValue);
					break;

				case INT16:		// sizeof(short) == 2 == 16bit
					ss_resultValue = (modbusResponse[baseIndex]) | (modbusResponse[baseIndex + 1] << 8);
					setoutput(i, ss_resultValue);
					break;

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

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

				case STRING16:
					decodeBytes2Ascii(&modbusResponse[baseIndex], modbusStrings, modbus_resultregister_type_bytesize);
					break;
					
				case STRING32:
					decodeBytes2Ascii(&modbusResponse[baseIndex], modbusStrings, modbus_resultregister_type_bytesize);
					break;

				case COIL:	// In case of coils the number of results are coded in bits. Means: 8 coils  == 1 Byte
					setoutput(i, modbusResponse[baseIndex]);	// sizeof(char)  == 1 == 8bit
					break;
				
				default:
					setoutput(i, -11111111);	// Unknown
			}
		}
		
		printReadingStatus(errorInLastIteration, okString, modbusStrings);
		
		// Handover remaining results
		if (no_resultvalues > MAX_OUTPUT_CHANNELS && result_type != COIL)
		{
			// Handover the remaining results as hex-string
			init(hexOutput, RD_BLOCK_SIZE, 0);
			init(szResultHelper, 2, 0);

			// Hexify result datatype and then the remaining result data
			memcpy(szResultHelper, &result_type, 1);
			convertToReadableHex(szResultHelper, 1, hexOutput);

			convertToReadableHex(&modbusResponse[baseIndex + modbus_resultregister_type_bytesize], (no_resultvalues - MAX_OUTPUT_CHANNELS) * modbus_resultregister_type_bytesize, hexOutput);
			
			setoutputtext(0, hexOutput);
		}

		errorInLastIteration = 0;	
		sleep(REPEATER_SLEEP_MS);
	}
}

main();