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

// Some constants concerning supported modbus datatypes
#define RESULT_DEFTYPE_UINT16  0
#define RESULT_DEFTYPE_INT16   1
#define RESULT_DEFTYPE_UINT32  2
#define RESULT_DEFTYPE_INT32   3
#define RESULT_DEFTYPE_FLOAT16 4
#define RESULT_DEFTYPE_FLOAT32 5

/**********************************************************************
	Device settings
	Hardcoded here, of course also possible to be read over input channels
***********************************************************************/
#define MODBUS_IP "192.168.xx.xx"
#define MODBUS_PORT 1502
#define MODBUS_DEVICE_ID 1

/**********************************************************************
	Config start

	Hex calculation for example can be made here: https://bin-dez-hex-umrechner.de/
***********************************************************************/
// Start address for reading modbus register
// Solaredgetest 40206 (base 40000) --> 206 == 00 CE (Address taken from SunSpec Logging documentation)
// Solaredgetest 57732 == E1 84 (Address taken from Power Control documentation)
#define MODBUS_READREGISTER_STARTADDRESS_HI 0x00
//#define MODBUS_READREGISTER_STARTADDRESS_LO 0xCE
#define MODBUS_READREGISTER_STARTADDRESS_LO 0xBC

// Maximum is 125 registers to be read as once
#define NO_RESULTVALUES 25

// For the ease of use all results are assumed to be of the same type
// Choose one the DEFTYPE-types defined above
#define RESULT_TYPE   RESULT_DEFTYPE_INT16
/**********************************************************************
	Config end
***********************************************************************/

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

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

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

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

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

/*
	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
	
	The global character array "szResults" is used for returning the string
	The array is assumed to be sized big enough. This is because of the memory 
	behavior of picoC. Problems occur when handing over parameters being returned
	from subfunctions (allocated inside) to the next function.
*/
char* convertToReadableHex(char *bytes, int byteLen)
{
	int i=0;

	szResults = init(szResults, RD_BLOCK_SIZE, 0);
	
	if (byteLen > 0)
		sprintf(szResults,"%02x",bytes[0]);
	
	for (i=1; i<byteLen; i++)
		sprintf(szResults,"%s %02x",szResults,bytes[i]);

	return szResults;
}

/*
	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 = strlen(hexString);
	int resultcounter = 0;
	int i = 0;
    char bit4_1; 
    char bit4_2; 

	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;
}

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

/*
	PicoC works little endian
	Supported values are arraylengths of 2 and 4
*/
char *swapBytes(char *bytes, int len)
{
	// Maximum size possible
	char tmpBytes[4];
	int i=0;

	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[2];
		bytes[1] = tmpBytes[3];
		bytes[2] = tmpBytes[0];
		bytes[3] = tmpBytes[1];
	}

	return bytes;	
}

/*
	Helper function for retrieving bytesizes
*/
int getBytesizeByType(int type)
{
		if (type == RESULT_DEFTYPE_UINT16 || type == RESULT_DEFTYPE_INT16 || type == RESULT_DEFTYPE_FLOAT16)
			return 2;
		else if (type == RESULT_DEFTYPE_UINT32 || type == RESULT_DEFTYPE_INT32 || type == RESULT_DEFTYPE_FLOAT32)
			return 4;		

		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[0] & 0x80) >> 7;
    exponent    =  ((flt32_bits[0] & 0x7F) << 1)  | ((flt32_bits[1] & 0x80) >> 7);
    significand = (((flt32_bits[1] & 0x7F) << 16) |  (flt32_bits[2] << 8) | (flt32_bits[3]));

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

/*
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)
{
	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] = 0x03;    			// Function code 3 = Read holding registers

	modbusMsg[8] = MODBUS_READREGISTER_STARTADDRESS_HI;   
	modbusMsg[9] = MODBUS_READREGISTER_STARTADDRESS_LO;

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

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

char modbusRequest[MODBUS_MSG_SIZE];
char modbusResponse[RD_BLOCK_SIZE];
char connectionString[40];

char szResults[RD_BLOCK_SIZE];	

int no_printValues = 0;
int output_offset = 0;

int nBytesReceived = 0;
int baseIndex = 0;
int i=0;

// Byte-size of result registers
// For the ease of use all resultsizes are assumed to be the same size
int modbus_resultregister_type_bytesize = getBytesizeByType(RESULT_TYPE);

// Expected number of values depending on sizes defined before
int modbus_readregister_count = NO_RESULTVALUES*(modbus_resultregister_type_bytesize/2);

// Declare resultarrays
signed   short ss_resultValues[NO_RESULTVALUES]; // sizeof(short) == 2 == 16bit
unsigned short us_resultValues[NO_RESULTVALUES]; 
signed   int   si_resultValues[NO_RESULTVALUES]; // sizeof(int)   == 4 == 32bit
unsigned int   ui_resultValues[NO_RESULTVALUES];	
float          f_resultValues[NO_RESULTVALUES];  // sizeof(float) == 8 == 64bit


sprintf(connectionString,"/dev/tcp/%s/%d", MODBUS_IP, MODBUS_PORT);
buildModbusRequest(modbusRequest, modbus_readregister_count);

// Keep polling
while (TRUE)
{
	// Request
	STREAM* 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
	/*
		szResults = convertToReadableHex(modbusRequest, MODBUS_MSG_SIZE);
		setoutputtext(0, szResults); 
	*/
	
	// Response
	nBytesReceived = stream_read(pTcpStream,modbusResponse,RD_BLOCK_SIZE,4000);
	stream_close(pTcpStream);

	// Any error or unexpected response
	if (nBytesReceived - MODBUS_RESPONSE_OFFSET < 0 || ((nBytesReceived - MODBUS_RESPONSE_OFFSET) % modbus_resultregister_type_bytesize) != 0)
	{
		setoutputtext(0, "Error at reading bytes or no connection to device");
		sleep(REPEATER_SLEEP_MS);
		continue;
	}
	else
	{
		setoutputtext(1, "Reading ok");
	/*
		szResults = convertToReadableHex(modbusResponse, nBytesReceived);
		setoutputtext(2, szResults); 
	*/
	}
	
	// Resulthandling
	// if we have more than MAX_OUTPUT_CHANNELS results (limited outputchannels...) 
	// then hand over after result MAX_OUTPUT_CHANNELS-1
	no_printValues = NO_RESULTVALUES;
	if (NO_RESULTVALUES > MAX_OUTPUT_CHANNELS)
	{
		no_printValues = MAX_OUTPUT_CHANNELS-1;
		output_offset  = 1;
	}

	// 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++)
	{
		f_resultValues[i]  = 0;
		si_resultValues[i] = 0;
		ss_resultValues[i] = 0;
		us_resultValues[i] = 0; 
		ui_resultValues[i] = 0;
		
		baseIndex = MODBUS_RESPONSE_OFFSET + i * modbus_resultregister_type_bytesize;

		swapBytes(&modbusResponse[baseIndex], modbus_resultregister_type_bytesize);
		
		// Decode Bits/Bytes to values
		if (RESULT_TYPE == RESULT_DEFTYPE_UINT16)
			us_resultValues[i] = (modbusResponse[baseIndex]) | (modbusResponse[baseIndex + 1] << 8);
		else if (RESULT_TYPE == RESULT_DEFTYPE_INT16)
			ss_resultValues[i] = (modbusResponse[baseIndex]) | (modbusResponse[baseIndex + 1] << 8);
		else if (RESULT_TYPE == RESULT_DEFTYPE_UINT32)
			ui_resultValues[i] = (modbusResponse[baseIndex] << 8) | (modbusResponse[baseIndex + 1] ) | (modbusResponse[baseIndex + 2] << 24) | (modbusResponse[baseIndex + 3]  << 16);
		else if (RESULT_TYPE == RESULT_DEFTYPE_INT32)
			si_resultValues[i] = (modbusResponse[baseIndex] << 8) | (modbusResponse[baseIndex + 1] ) | (modbusResponse[baseIndex + 2] << 24) | (modbusResponse[baseIndex + 3]  << 16);
		else if (RESULT_TYPE == RESULT_DEFTYPE_FLOAT16)
			f_resultValues[i] = calc_float16(&modbusResponse[baseIndex]);
		else if (RESULT_TYPE == RESULT_DEFTYPE_FLOAT32)
			f_resultValues[i] = calc_float32(&modbusResponse[baseIndex]);
	}
	
	// Print results on output
	for (i=0; i<no_printValues; i++)
	{
		if (RESULT_TYPE == RESULT_DEFTYPE_UINT16)
			setoutput(output_offset + i, us_resultValues[i]);
		else if (RESULT_TYPE == RESULT_DEFTYPE_INT16)
			setoutput(output_offset + i, ss_resultValues[i]);		
		else if (RESULT_TYPE == RESULT_DEFTYPE_UINT32)
			setoutput(output_offset + i, ui_resultValues[i]);
		else if (RESULT_TYPE == RESULT_DEFTYPE_INT32)
			setoutput(output_offset + i, si_resultValues[i]);
		else if (RESULT_TYPE == RESULT_DEFTYPE_FLOAT16 || RESULT_TYPE == RESULT_DEFTYPE_FLOAT32)
			setoutput(output_offset + i, f_resultValues[i]);
		else
			setoutput(output_offset + i, -1);	// Unknown		
	}
	
	// Handover remaining results
	if (NO_RESULTVALUES > MAX_OUTPUT_CHANNELS)
	{
		// Handover the datatype
		setoutput(0, RESULT_TYPE);

		// Handover the remaining results as hex-string
		setoutputtext(0, convertToReadableHex(&modbusResponse[baseIndex + modbus_resultregister_type_bytesize], (NO_RESULTVALUES - no_printValues) * modbus_resultregister_type_bytesize));
	}
	
	sleep(REPEATER_SLEEP_MS);
}