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

	Andrej Konkow, 02/2026, v23
 
	...Pico-C is in somekind very special in syntax and possibilities...
******************************************************************/

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

// Holds the corresponding bytesizes for the above datatypes
// Careful!!! This map differs from the map in the main component!!
static int BYTESIZEMAP[10] = { 0, 2, 2, 4, 4, 2, 4, 16, 32, 1 };

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

// Size of String to be transferred to next component
#define TRANSFER_SIZE 500

// In Loxone programs currently can address
// 13 output channels for number-values in maximum
#define MAX_OUTPUT_CHANNELS 13

// Hexdata coming from the first component is encoded as:
// 1. Two bytes total bytes being handed over
// 2. Two bytes telling us the amount of results being sent
#define HEX_METADATA_OFFSET 4

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

// Sleeptime for displaying regular messages
#define MESSAGE_SLEEP_MS 1000

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

// Holds the data of a single register
struct registerDataValue
{
    int  datatype;           // one of "enum registerDatatype"
    int  registerAddress;    // Real address in the device
    char value[5];           // handed over value from main component
};

// Holds all the handed over data
struct registerData
{
    struct registerDataValue registerDataValues[MAX_DATATYPES_DEFINED_EXT];
    int    noOfResults;
};

// Keeping the configuration global makes things easier
static struct registerData DataValues;

// Map for converting asc hex-values efficiently
static char MapHex2Int[127] = 
{
     0,  0,  0,  0,  0,  0,  0,  0,  0,  0,
     0,  0,  0,  0,  0,  0,  0,  0,  0,  0,
     0,  0,  0,  0,  0,  0,  0,  0,  0,  0,
     0,  0,  0,  0,  0,  0,  0,  0,  0,  0,
     0,  0,  0,  0,  0,  0,  0,  0,  0,  1,     // values for 0-9
     2,  3,  4,  5,  6,  7,  8,  9,  0,  0,
     0,  0,  0,  0,  0, 10, 11, 12, 13, 14,     // values for A-F
    15,  0,  0,  0,  0,  0,  0,  0,  0,  0,
     0,  0,  0,  0,  0,  0,  0,  0,  0,  0,
     0,  0,  0,  0,  0,  0,  0, 10, 11, 12,     // values for a-f
    13, 14, 15,  0,  0,  0,  0,  0,  0,  0,
     0,  0,  0,  0,  0,  0,  0,  0,  0,  0,
     0,  0,  0,  0,  0,  0,  0
};

/*
    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 hasContent)
{
    int  i;

    for (i=0; i<byteLen; i++)
    {
        if (i == 0 && ! hasContent)
            sprintf(dest, "%02x", bytes[i]);
        else
            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 number of characters/bytes created
*/
int convertHexToBytes(char *hexString, char *resultBytes)
{
    int  hexlen, resultcounter=0, i=0;

    hexlen = strlen(hexString);

    for (i=0; i < hexlen; i+=2)
    {
        if (i+1 > hexlen)         // corrupt bytestring, unhandled errorcase
            break;

        if (hexString[i] == ' ')  // Ignore whitespaces
        { 
            i--; 
            continue; 
        }
        
        // stick together the two 4bit values
        resultBytes[resultcounter] = (MapHex2Int[hexString[i]] << 4) | (MapHex2Int[hexString[i+1]] & 0xF); 
        resultcounter++;
    }

    return resultcounter;
}

/*
	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 formatting output-value
*/
void padValue(char *retValue, int value)
{
	if (value <10)
		sprintf(retValue, "0%d", value);
	else
		sprintf(retValue, "%d", value);
}

/*
	Gets the current time and returns it formatted.
*/
char *getFormattedTime(char *buffer)
{
	int now  = getcurrenttime();
	int year = getyear(now, 1);

	char s_month[3], s_day[3];
	char s_hour[3], s_minute[3], s_second[3];

	padValue(s_month,  getmonth(now, 1));
	padValue(s_day,    getday(now, 1));
	padValue(s_hour,   gethour(now, 1));
	padValue(s_minute, getminute(now, 1));
	padValue(s_second, getsecond(now, 1));

	sprintf (buffer, "%s.%s.%d %s:%s:%s", s_day, s_month, year, s_hour, s_minute, s_second);

	return buffer;
}

/*
    Hexify message for handover
    Only handover results to be displayed and already byte-swapped
*/
void msgMarshalling(char *hexOutput, int hexOutputSize)
{ 
    int   i;
    char  szResultHelper[3];
    short shortHelper, totalBytesSent=0;
    
    struct registerDataValue *hlpRegisterDataValue;
    
    // First hexify the amount of results being sent
    shortHelper = (short)(DataValues.noOfResults - MAX_OUTPUT_CHANNELS);
    memcpy(szResultHelper, &shortHelper, 2);
    convertToReadableHex(szResultHelper, 2, hexOutput, 0);
    totalBytesSent+=2;
    
    // Hexify the remaining result datatypes
    for (i=MAX_OUTPUT_CHANNELS; i<DataValues.noOfResults; i++)
    {
        hlpRegisterDataValue = &DataValues.registerDataValues[i];
        
        // Hexify datatype
        szResultHelper[0] = (char)hlpRegisterDataValue->datatype;    // Downcasting in sizes is ok here
        convertToReadableHex(szResultHelper, 1, hexOutput, 1);
        totalBytesSent+=1;
            
        // Hexify address
        shortHelper = (short)hlpRegisterDataValue->registerAddress;
        memcpy(szResultHelper, &shortHelper, 2);
        convertToReadableHex(szResultHelper, 2, hexOutput, 1);
        totalBytesSent+=2;

        // Hexify value
        convertToReadableHex(hlpRegisterDataValue->value, BYTESIZEMAP[hlpRegisterDataValue->datatype], hexOutput, 1);
        totalBytesSent+=BYTESIZEMAP[hlpRegisterDataValue->datatype];
    }

    // Finalize the hexstring by putting the total bytes in front
    totalBytesSent+=2;
    memcpy(szResultHelper, &totalBytesSent, 2);
    sprintf(hexOutput, "%02x %02x %s", szResultHelper[0], szResultHelper[1], hexOutput);
}

/*
    Unhexify message from handover
    Results to be displayed already are byte-swapped if needed
*/
void msgUnmarshalling(char *data, int datalen)
{ 
    int   i, j, dataIndex=0;
    char  numberOfResultsChar[2];
    
    struct registerDataValue *hlpRegisterDataValue;

	// Get the number of handed over results
	memcpy(numberOfResultsChar, data, 2);
	DataValues.noOfResults = (numberOfResultsChar[0]) | (numberOfResultsChar[1] << 8);
    dataIndex+=2;

    for (i=0; i<DataValues.noOfResults; i++)
    {
        hlpRegisterDataValue = &DataValues.registerDataValues[i];
        
        hlpRegisterDataValue->datatype = data[dataIndex];
        dataIndex++;
        
        hlpRegisterDataValue->registerAddress = (data[dataIndex]) | (data[dataIndex+1] << 8);
        dataIndex+=2;
        
        for (j=0; j<BYTESIZEMAP[hlpRegisterDataValue->datatype]; j++)
            hlpRegisterDataValue->value[j] = data[dataIndex+j];   
        dataIndex+=BYTESIZEMAP[hlpRegisterDataValue->datatype];
    }
}

/*
    Print addresses in a char-array
*/
void printValue(char *dest, int value, int hasContent)
{
    if (! hasContent)
        sprintf(dest, "%d", value);
    else
        sprintf(dest,"%s, %d", dest, value);
}

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

	Mainfunctionality encapsulated in a function
	Due to picoC issues a little awkward
*/
void main()
{
	int resultregister_type_total_bytesize,	// Byte-size of result registers
		result_type, result_bytesize,
		no_resultvalues, no_resultBytes,
		szListOfDatatypes[MAX_OUTPUT_CHANNELS],
		baseIndex, i, hexlen, event,
        integrity;
	char szRemainingResults[RD_BLOCK_SIZE],
         addresses[RD_BLOCK_SIZE], hexOutput[TRANSFER_SIZE], 
		 szDisplayMsg[100],
		 szCurrenttime_formatted[35],
		 integrityChar[2], 
		 *remainingResultsHex;
	
	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];
    
    struct registerDataValue *hlpRegisterDataValue;  
    
    
	// Keep polling
	while (TRUE)
	{
        event = getinputevent();

		if (! event)   
		{
			setoutputtext(0, "Waiting for data update...");
			sleep(REPEATER_SLEEP_MS);
			continue;
		}

		baseIndex                          = HEX_METADATA_OFFSET;
        hexOutput[0]                       = 0;
        resultregister_type_total_bytesize = 0;
		no_resultvalues                    = 0;
        no_resultBytes                     = 0;
        
		remainingResultsHex = getinputtext(0);
    
        // Due to instabilities in Loxone no sub-function convertHexToBytes is used
        hexlen = strlen(remainingResultsHex);
        for (i=0; i < hexlen; i+=2)
        {
            if (i+1 > hexlen)         // corrupt bytestring, unhandled errorcase
                break;

            if (remainingResultsHex[i] == ' ')  // Ignore whitespaces
            { 
                i--; 
                continue; 
            }
        
            // stick together the two 4bit values
            szRemainingResults[no_resultBytes] = (MapHex2Int[remainingResultsHex[i]] << 4) | (MapHex2Int[remainingResultsHex[i+1]] & 0xF); 
            no_resultBytes++;
        }
        // End convertHexToBytes
    
		free(remainingResultsHex);

		if ( no_resultBytes-HEX_METADATA_OFFSET <= 0 )
		{
			sprintf(szDisplayMsg,"Received %d bytes of corrupt data. Ignoring...", no_resultBytes);
			setoutputtext(0, szDisplayMsg);
			sleep(MESSAGE_SLEEP_MS);
			continue;
		}
        
        // First get the number of bytes of the message length for integrity check
		memcpy(integrityChar, szRemainingResults, 2);
		integrity = (integrityChar[0]) | (integrityChar[1] << 8);

		if ( no_resultBytes != integrity )
		{
			sprintf(szDisplayMsg,"Bytelength not ok. Got %d. Expecting: %d. Ignoring...", no_resultBytes, integrity);
			setoutputtext(0, szDisplayMsg);
			sleep(MESSAGE_SLEEP_MS);
			continue;
		}
        
        msgUnmarshalling(&szRemainingResults[2], no_resultBytes-2);

		// Resulthandling
		// Buffers are big enough. So
		// take the bytes from the input
		// Be aware: We have a maximum of 13 output channels
        // The rest will have to be handed over again
		for (i=0; i<MAX_OUTPUT_CHANNELS && i<DataValues.noOfResults; i++)
		{
            hlpRegisterDataValue = &DataValues.registerDataValues[i];
            
            printValue(addresses, hlpRegisterDataValue->registerAddress, i);

			result_type = hlpRegisterDataValue->datatype;
            
			// Decode Bits/Bytes to values
			switch (result_type)
			{
				case UINT16:	// sizeof(short) == 2 == 16bit
					us_resultValues[i] = (hlpRegisterDataValue->value[0]) | (hlpRegisterDataValue->value[1] << 8);	
					break;

				case INT16:		// sizeof(short) == 2 == 16bit
					ss_resultValues[i] = (hlpRegisterDataValue->value[0]) | (hlpRegisterDataValue->value[1] << 8);
					break;

				case UINT32:	// sizeof(int)   == 4 == 32bit
					ui_resultValues[i] = (hlpRegisterDataValue->value[0] << 16) | (hlpRegisterDataValue->value[1] << 24) | (hlpRegisterDataValue->value[2]) | (hlpRegisterDataValue->value[3]  << 8);
					break;

				case INT32:		// sizeof(int)   == 4 == 32bit
					si_resultValues[i] = (hlpRegisterDataValue->value[0] << 16) | (hlpRegisterDataValue->value[1] << 24) | (hlpRegisterDataValue->value[2]) | (hlpRegisterDataValue->value[3]  << 8);
					break;
				
				case FLOAT16:	// sizeof(float) == 8 == 64bit
					f_resultValues[i] = calc_float16(&hlpRegisterDataValue->value[0]);
					break;
				
				case FLOAT32:	// sizeof(float) == 8 == 64bit
					f_resultValues[i] = calc_float32(&hlpRegisterDataValue->value[0]);
					break;

				case DT_NONE:
					no_resultvalues--;
					break;					
			}	
			
			baseIndex += BYTESIZEMAP[result_type];			

			no_resultvalues++;
		}

		// Print results on output
		for (i=0; i<MAX_OUTPUT_CHANNELS && i<DataValues.noOfResults; i++)
		{
			hlpRegisterDataValue = &DataValues.registerDataValues[i];
			result_type = hlpRegisterDataValue->datatype;

			// Display all values in a rush
			switch (result_type)
			{
				case UINT16:   setoutput(i, us_resultValues[i]); break;
				case INT16:	   setoutput(i, ss_resultValues[i]); break;
				case UINT32:   setoutput(i, ui_resultValues[i]); break;
				case INT32:	   setoutput(i, si_resultValues[i]); break;
				case FLOAT16:	
				case FLOAT32:  setoutput(i, f_resultValues[i]);  break;
			}			
		}
		
        if (DataValues.noOfResults > MAX_OUTPUT_CHANNELS)
            msgMarshalling(hexOutput, TRANSFER_SIZE);
        
		setoutputtext(0, hexOutput);
		
		sprintf(szCurrenttime_formatted,"Last update: %s", getFormattedTime(szCurrenttime_formatted));
		setoutputtext(1, szCurrenttime_formatted);

		sprintf(szDisplayMsg,"%d results: %s", DataValues.noOfResults, addresses);
		setoutputtext(2, szDisplayMsg);

        // Wait a short time to assure the reading of the handed over hex string
        if (hexOutput[0] != 0)
            sleep(5 * REPEATER_SLEEP_MS);
        
		sleep(REPEATER_SLEEP_MS);
	}
}

main();