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

	Andrej Konkow, 02/2026, v26
 
	...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,
    UINT64   = 5,
    INT64    = 6,
    FLOAT16  = 7,
    FLOAT32  = 8,
    IGNORE16 = 9,
    IGNORE32 = 10,
    IGNORE64 = 11,
    STRING16 = 12,
    STRING32 = 13,
    COIL     = 14    
};

// Holds the corresponding bytesizes for the above datatypes
static int BYTESIZEMAP[15] = { 0, 2, 2, 4, 4, 8, 8, 2, 4, 2, 4, 8, 16, 32, 1 };

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

// 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(validation/integrity)
// 2. One byte holding the addressed output component
// 3. TRANSFER_DISPLAY_COMPONENTS bytes holding the starting reading index in 
//    the data stream for efficient reading, per component
// 4. Two bytes telling us the amount of results being sent
// 5. Two bytes telling us in which format addresses shall be printed hex/dec
#define HEX_METADATA_OFFSET 2 + 1 + (TRANSFER_DISPLAY_COMPONENTS*2) + 2 + 2

// Size of String to be transferred to next component
// Calulation:
// Per 16-Bit result to be transferred add 10
// Per 32-Bit result to be transferred add 14
// Example: Transfer of additional 13 16bit results means: plus 13*10
// Size of 500 means: 13 Byte Metainfo + 487 Byte payload
// Current sizing fits the transfer of 4*13 float32/4-byte results
// HEX_METADATA_OFFSET metainfobytes + (4 displaycomponents * 13 channels) * 14 payloadbytes = 740 (plus 10 for safety)
#define TRANSFER_DISPLAY_COMPONENTS 4
#define TRANSFER_SIZE               HEX_METADATA_OFFSET + (TRANSFER_DISPLAY_COMPONENTS * MAX_OUTPUT_CHANNELS * 14) + 10

// 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[9];           // handed over value from main component
};

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

    int    componentAddress;
    int    noOfResults;
    int    printHexStyle;
};

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

// Map for converting asc hex-values efficiently
// Map for converting asc hex-values efficiently
static char MapHex2Int[127];
void initMapHex2Int()
{
    int i;
    
    init(MapHex2Int, 127, 0);
    
    for (i=0; i<10; i++) MapHex2Int['0'+i] = i;     // values for 0-9
    for (i=0; i< 6; i++) MapHex2Int['A'+i] = 10+i;  // values for A-F
    for (i=0; i< 6; i++) MapHex2Int['a'+i] = 10+i;  // values for a-f  
}

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

    for (i=0; i<byteLen; i++)
    {
        if (i == 0 && ! hasContent)
            sprintf(dest, "%02x", bytes[i]);
        else if (printWhitespace)
            sprintf(dest,"%s %02x", dest, bytes[i]);
        else
            sprintf(dest,"%s%02x", dest, bytes[i]);
    }
}

/*
    Converts alphabetical characters to uppercase(for better readability)
*/
void convertToUppercase(char *array, int len)
{
    int  i;
    
    for (i=0; i<len; i++)
    {
        if (array[i] == 0)
            break;
        
        if (array[i] >= 'a' && array[i] <= 'z')
            array[i] -= 32;
    }
}

/*
	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 timeonly)
{
	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_hour,   gethour(now, 1));
	padValue(s_minute, getminute(now, 1));
	padValue(s_second, getsecond(now, 1));

    if (timeonly)
    {
        sprintf (buffer, "%s:%s:%s", s_hour, s_minute, s_second);
    }
    else
    {
        padValue(s_month,  getmonth(now, 1));
        padValue(s_day,    getday(now, 1));

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

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

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

	// Get index in stream which corresponds to my address
    // ignore integrity and address
    dataIndex = 2 + 1;
    // jump to corresponding index information
    dataIndex += (DataValues.componentAddress-1)*2;
    
	memcpy(numberOfResultsChar, &data[dataIndex], 2);
	resultIdx = (numberOfResultsChar[0]) | (numberOfResultsChar[1] << 8);
    resultIdx += HEX_METADATA_OFFSET;


    // How many results do we have to display?
    noResultsDisplay = MAX_OUTPUT_CHANNELS;
    if (DataValues.noOfResults - DataValues.componentAddress * MAX_OUTPUT_CHANNELS < 0)
        noResultsDisplay = DataValues.noOfResults % ((DataValues.componentAddress-1) * MAX_OUTPUT_CHANNELS);

    // now get the values (of our block only)
    for (i=0; i<noResultsDisplay; i++)
    {
        hlpRegisterDataValue = &DataValues.registerDataValues[i];
        
        hlpRegisterDataValue->datatype = data[resultIdx];
        resultIdx++;
        
        hlpRegisterDataValue->registerAddress = (data[resultIdx]) | (data[resultIdx+1] << 8);
        resultIdx+=2;
        
        for (j=0; j<BYTESIZEMAP[hlpRegisterDataValue->datatype]; j++)
            hlpRegisterDataValue->value[j] = data[resultIdx+j];   
        resultIdx+=BYTESIZEMAP[hlpRegisterDataValue->datatype];
    }
    
    return noResultsDisplay;
}

/*
    Check whether we have to handover the results to the next component
    modifies stream appropriately and returns 1 if handover has to be done
*/
int checkAndSetHandover(char *stream, int bytelen)
{
    int  numberOfResults, streamIndex;
    char numberOfResultsChar[2];
    
    // Ignore integrity
    streamIndex = 2;

	// Get the number of handed over results
	memcpy(numberOfResultsChar, &stream[streamIndex], 1);
	DataValues.componentAddress = (int)stream[streamIndex];
    streamIndex+=1;

    // ignore index bytes
    streamIndex+=TRANSFER_DISPLAY_COMPONENTS*2;
    
	// Get the number of handed over results
	memcpy(numberOfResultsChar, &stream[streamIndex], 2);
	DataValues.noOfResults = (numberOfResultsChar[0]) | (numberOfResultsChar[1] << 8);
    streamIndex+=2;

	// Print address in hexformat or decimal format?
    memcpy(numberOfResultsChar, &stream[streamIndex], 2);
	DataValues.printHexStyle = (numberOfResultsChar[0]) | (numberOfResultsChar[1] << 8);
    
    numberOfResults = DataValues.noOfResults - DataValues.componentAddress * MAX_OUTPUT_CHANNELS;
    if (numberOfResults > 0)
    {
        stream[2] = stream[2]+1;    // Address next component for displaying
        return 1;
    }
    
    return 0;
}

/*
    swap two bytes, length is assumed 2
*/
void swap(char *bytes)
{
    char tmpByte;

    tmpByte  = bytes[0];
    bytes[0] = bytes[1];
    bytes[1] = tmpByte;
}

/*
    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 no_resultBytes, noResultsDisplay,
		i, hexlen, event,
        integrity, handOver;
	char szRemainingResults[TRANSFER_SIZE],
         szAddresses[RD_BLOCK_SIZE], hexOutput[TRANSFER_SIZE], 
		 szDisplayMsg[100],
		 szCurrenttime_formatted[35],
		 integrityChar[2], szResultHelper[3],
		 *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];
    unsigned long  ul_resultValues[MAX_OUTPUT_CHANNELS];
    signed   long  sl_resultValues[MAX_OUTPUT_CHANNELS];
	float          f_resultValues[MAX_OUTPUT_CHANNELS];
    
    struct registerDataValue *hlpRegisterDataValue;  
    
    initMapHex2Int();
    
	// Keep polling
	while (TRUE)
	{
        event = getinputevent();

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

        hexOutput[0]        = 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 )
		{
            // The output might be irritating --> outcommented
			//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 )
		{
            // The output might be irritating --> outcommented
			//sprintf(szDisplayMsg,"Bytelength not ok. Got %d. Expecting: %d. Ignoring...", no_resultBytes, integrity);
			//setoutputtext(0, szDisplayMsg);
			sleep(MESSAGE_SLEEP_MS);
			continue;
		}
        
        handOver = checkAndSetHandover(szRemainingResults, no_resultBytes);
        if (handOver)
        {
            convertToReadableHex(szRemainingResults, no_resultBytes, hexOutput, 0, 0);
            setoutputtext(0, hexOutput);
        }

        noResultsDisplay = msgUnmarshalling(szRemainingResults, no_resultBytes);

		// 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<noResultsDisplay; i++)
		{
            hlpRegisterDataValue = &DataValues.registerDataValues[i];

            // Print address in hex/dec
            if (DataValues.printHexStyle)
            {
                memcpy(szResultHelper, &hlpRegisterDataValue->registerAddress, 2);
                swap(szResultHelper);

                if (i > 0)
                    sprintf(szAddresses,"%s, ", szAddresses);

                convertToReadableHex(szResultHelper, 2, szAddresses, i, 0);
            }
            else
            {
                printValue(szAddresses, hlpRegisterDataValue->registerAddress, i);
            }
            
			// Decode Bits/Bytes to values
			switch (hlpRegisterDataValue->datatype)
			{
				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 UINT64:    // sizeof(long)   == 8 == 64bit
                    ul_resultValues[i] = (hlpRegisterDataValue->value[0] << 16) | (hlpRegisterDataValue->value[1] << 24) | (hlpRegisterDataValue->value[2]) | (hlpRegisterDataValue->value[3]  << 8) |
                                         (hlpRegisterDataValue->value[4] << 16) | (hlpRegisterDataValue->value[5] << 24) | (hlpRegisterDataValue->value[6]) | (hlpRegisterDataValue->value[7]  << 8);
                    break;

                case INT64:     // sizeof(long)   == 8 == 64bit
                    sl_resultValues[i] = (hlpRegisterDataValue->value[0] << 16) | (hlpRegisterDataValue->value[1] << 24) | (hlpRegisterDataValue->value[2]) | (hlpRegisterDataValue->value[3]  << 8) |
                                         (hlpRegisterDataValue->value[4] << 16) | (hlpRegisterDataValue->value[5] << 24) | (hlpRegisterDataValue->value[6]) | (hlpRegisterDataValue->value[7]  << 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;
			}	
		}

		// Print results on output
		for (i=0; i<noResultsDisplay; i++)
		{
			hlpRegisterDataValue = &DataValues.registerDataValues[i];

			// Display all values in a rush
			switch (hlpRegisterDataValue->datatype)
			{
				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 UINT64:   setoutput(i, ul_resultValues[i]); break;
                case INT64:    setoutput(i, sl_resultValues[i]); break;
				case FLOAT16:	
				case FLOAT32:  setoutput(i, f_resultValues[i]);  break;
			}			
		}
				
		sprintf(szCurrenttime_formatted,"Last update: %s", getFormattedTime(szCurrenttime_formatted, 1));
		setoutputtext(1, szCurrenttime_formatted);

        convertToUppercase(szAddresses, RD_BLOCK_SIZE);
		sprintf(szDisplayMsg,"%d results: %s", noResultsDisplay, szAddresses);
		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();