/******************************************************************
	Example for reading modbus registers over TCP in Pico-C
	Not perfect but working
	Be careful: Don't connect two programs with each other. You
	will get stability problems in loxone when programs are
	restarted during redeployments. Use a textgenerator with
	impulses in between.

	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

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

// Set an upper limit for receiving values
#define NO_RESULTVALUES_UPPER_LIMIT 13

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

/*
	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 number of characters/bytes 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; Bytes not seem to match expectations
		if (i+1 > hexlen || hexlen > RD_BLOCK_SIZE)
			return 0;
		
		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)));
}

/*********************
	Now lets do the work
*********************/
int no_resultvalues;
int no_resultBytes;
int result_type;

char *remainingResultsHex;
char szRemainingResults[RD_BLOCK_SIZE];
char szDisplayMsg[100];

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;


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


// Keep polling
while (TRUE)
{
	result_type         = getinput(0);
	remainingResultsHex = getinputtext(0);
	
	modbus_resultregister_type_bytesize = getBytesizeByType(result_type);	

	no_resultBytes = convertHexToBytes(remainingResultsHex, szRemainingResults);
	free(remainingResultsHex);

	if (no_resultBytes == 0 || modbus_resultregister_type_bytesize == 0 || no_resultBytes % modbus_resultregister_type_bytesize != 0)
	{
		setoutputtext(0, "Just holding values. Continuing...");
		sleep(REPEATER_SLEEP_MS);
		continue;
	}

	no_resultvalues = no_resultBytes / modbus_resultregister_type_bytesize;

	sprintf(szDisplayMsg,"Converted %d bytes, %d values", no_resultBytes, no_resultvalues);
	setoutputtext(0, szDisplayMsg);

	// Resulthandling
	// Buffers are big enough. So
	// take the bytes from the input
	// Be aware: We have a maximum of 13 output channels
	for (i=0; i<no_resultvalues; i++)
	{
		f_resultValues[i]  = 0;
		si_resultValues[i] = 0;
		ss_resultValues[i] = 0;
		us_resultValues[i] = 0; 
		ui_resultValues[i] = 0;
		
		baseIndex = i * modbus_resultregister_type_bytesize;

		swapBytes(&szRemainingResults[baseIndex], modbus_resultregister_type_bytesize);

		
		// Decode Bits/Bytes to values
		if (result_type == RESULT_DEFTYPE_UINT16)
			us_resultValues[i] = (szRemainingResults[baseIndex]) | (szRemainingResults[baseIndex + 1] << 8);
		else if (result_type == RESULT_DEFTYPE_INT16)
			ss_resultValues[i] = (szRemainingResults[baseIndex]) | (szRemainingResults[baseIndex + 1] << 8);
		else if (result_type == RESULT_DEFTYPE_UINT32)
			ui_resultValues[i] = (szRemainingResults[baseIndex] << 8) | (szRemainingResults[baseIndex + 1] ) | (szRemainingResults[baseIndex + 2] << 24) | (szRemainingResults[baseIndex + 3]  << 16);
		else if (result_type == RESULT_DEFTYPE_INT32)
			si_resultValues[i] = (szRemainingResults[baseIndex] << 8) | (szRemainingResults[baseIndex + 1] ) | (szRemainingResults[baseIndex + 2] << 24) | (szRemainingResults[baseIndex + 3]  << 16);
		else if (result_type == RESULT_DEFTYPE_FLOAT16)
			f_resultValues[i] = calc_float16(&szRemainingResults[baseIndex]);
		else if (result_type == RESULT_DEFTYPE_FLOAT32)
			f_resultValues[i] = calc_float32(&szRemainingResults[baseIndex]);
	}
	
	// Print results on output
	for (i=0; i<no_resultvalues; i++)
	{
		if (result_type == RESULT_DEFTYPE_UINT16)
			setoutput(i, us_resultValues[i]);
		else if (result_type == RESULT_DEFTYPE_INT16)
			setoutput(i, ss_resultValues[i]);		
		else if (result_type == RESULT_DEFTYPE_UINT32)
			setoutput(i, ui_resultValues[i]);
		else if (result_type == RESULT_DEFTYPE_INT32)
			setoutput(i, si_resultValues[i]);
		else if (result_type == RESULT_DEFTYPE_FLOAT16 || result_type == RESULT_DEFTYPE_FLOAT32)
			setoutput(i, f_resultValues[i]);
		else
			setoutput(i, -1);	// Unknown		
	}
	
	sleep(REPEATER_SLEEP_MS);
}