/******************************************************************
    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, 02/2026, v26
 
    ...Pico-C is in somekind very special in syntax and possibilities...
******************************************************************/

// To run in debug mode and print some more messages activate the #define by removing the comment.
// Due to efficiency we will call the debug functions only in debugmode 
//#define DEBUGMODE 

// 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 };
static int DOBYTESWAP[15]    = { 0, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0,  0,  0, 0 };
static int PRINTBYTETYPE[15] = { 0, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0,  1,  1, 1 };
 
 
#define MODBUS_INPUT_IP_PORT     getinputtext(0)
#define MODBUS_INPUT_ADDRESS_HEX getinputtext(1)
#define MODBUS_INPUT_RESULTS_DT  getinputtext(2)
#define MODBUS_INPUT_BIGENDIAN   getinput(0)
#define REPEATER_SLEEP_MS        getinput(1)

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

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

// Time spent in reading the responsestream/waiting for data, timeout in ms
#define MODBUS_DEFAULT_READTIME_MS 1000

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

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

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

// 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
// 12 metainfobytes + (4 displaycomponents * 13 channels) * 14 payloadbytes = 740 (plus 10 for safety)
#define TRANSFER_DISPLAY_COMPONENTS 4
#define TRANSFER_SIZE               12 + (TRANSFER_DISPLAY_COMPONENTS * MAX_OUTPUT_CHANNELS * 14) + 10

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

// Definition of delimiters for datainput, defining
// datatypes and the number of results
#define DELIMITER_DT_BLOCKS ","
#define DELIMITER_DT_NUMBER "x"

// Pseudovalue for identfying a "no param"
#define STRING_NOPARAM      -99999

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


struct registerData
{
    int datatype;           // one of "enum registerDatatype"
    int idxResponse;        // Holds the index to the value in the responsestream;
    int registerAddress;    // Real address in the device
};

/*
    Holds information about the specified userdata
    in the textgenerator component
*/
struct dataConfig 
{
    struct registerData szRegisters      [MAX_DATATYPES_DEFINED_EXT];   // List of all user-defined datatypes
    int                 szRegisterIdxView[MAX_DATATYPES_DEFINED_EXT];   // Points to the Idx in szRegisters to be displayed
    
    int byteSizeListOfDatatypes;                             // Bytesize of list of all datatypes
    int no_resultvalues;                                     // Number of total values specified
    int no_printValues;                                      // Number of values to be printed

    char modbusRequest[MODBUS_MSG_SIZE];                    // Bytesequence for modbus request
    char connectionString[MODBUS_CONNECTIONSTRING_SIZE];    // tcp connection string
    int  modbusId;                                          // Modbus device id
    int  deviceLittleEndian;                                // Are we expecting the data in little endian format?
    int  modbus_readtime_ms;                                // Timeout waiting for response

    int readCoils;                                          // Are we in the mode of reading coils?
    int repeater_sleep_ms;                                  // sleep time between iterations

    char szStartAddress[10];                                // Starting address as specified hex-String
    char szPrintedAdresses[RD_BLOCK_SIZE];                  // Adresses printed on outputchannel (first component only)
    int  printHexStyle;                                     // Print displayed addresses in hex-notation or decimal
};

// Keeping the configuration global makes things easier
static struct dataConfig Config;

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

/*
    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=0, i;

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

/*
    The bytes represent ascii-encoded values. Decode them...
*/
void decodeBytes2Ascii(char *src, char *dest, int len)
{
    char hlp[33], tmpString[35];
    init(hlp, 33, 0);
    
    int tmpLen = strlen(src);
    if (tmpLen < len)
        len = tmpLen;

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

/*
    Swap all bytes according to their datatype
*/
void swapAllBytes(char *bytes)
{
    int i,
        result_type, result_size,
        offsetCnt=0;
    
    struct registerData hlpRegisterData;
    
    if (Config.deviceLittleEndian || Config.readCoils)
        return;

    for (i=0; i<Config.no_resultvalues; i++)
    {
        hlpRegisterData = Config.szRegisters[i];
        result_type = hlpRegisterData.datatype;
        result_size = BYTESIZEMAP[result_type];
        
        if (DOBYTESWAP[result_type])
            swapBytes(&bytes[offsetCnt], result_size);

        offsetCnt += result_size;
    }
}

/*
    PicoC works little endian
    Supported values are arraylengths of 2 and 4
*/
void swapBytes(char *bytes, int result_size)
{                  
    char hlp0 = bytes[0],
         hlp1 = bytes[1];
    
    if (result_size == 2)
    {
        bytes[0] = bytes[1];
        bytes[1] = hlp0;    
    }
    else if (result_size == 4)
    {
        bytes[0] = bytes[3];
        bytes[1] = bytes[2];
        bytes[2] = hlp1;
        bytes[3] = hlp0;
    }
    else if (result_size == 8)
    {
        swapBytes(&bytes[0], 4);
        swapBytes(&bytes[4], 4);
    }
}

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

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

    strncpy(ip, targetaddress, colon);

    if (comma == -1)
    {
        strncpy(port, &targetaddress[colon+1], strlen(targetaddress)-colon-1);
        *modbusId = 1;
    }
    else
    {
        strncpy(port, &targetaddress[colon+1], comma-colon-1);
        strncpy(mbId, &targetaddress[comma+1], strlen(targetaddress)-comma-1);
        *modbusId = batoi(mbId);
    }
    
    sprintf(connectionString,"/dev/tcp/%s/%s", ip, port);

    #ifdef DEBUGMODE    debugMsg(connectionString);    #endif
}

/*
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()
{
    int functioncode              = MODBUS_FC_READHOLDINGREGISTERS,
        modbus_readregister_count = Config.byteSizeListOfDatatypes/2;

    if (Config.readCoils)
    {
        functioncode              = MODBUS_FC_READCOILS;
        modbus_readregister_count = Config.no_resultvalues;
    }
    
    
    Config.modbusRequest[0] = 0x00;
    Config.modbusRequest[1] = MODBUS_TRANSACTIONID;

    Config.modbusRequest[2] = 0x00;
    Config.modbusRequest[3] = 0x00;

    Config.modbusRequest[4] = 0x00;  // Bytelength of Unit-Id and PDU-Message hi
    Config.modbusRequest[5] = 0x06;  // Bytelength of Unit-Id and PDU-Message lo

    // Beginning of PDU-Message
    Config.modbusRequest[6] = Config.modbusId;
    Config.modbusRequest[7] = functioncode;

    // Set register startaddresses hi an low hex
    // modbusMsg[8] and [9] are filled up
    convertHexToBytes(Config.szStartAddress, &Config.modbusRequest[8]);

    Config.modbusRequest[10] = 0x00;
    Config.modbusRequest[11] = modbus_readregister_count;
}

/*
    Check for errors in receiving the modbus response
*/
int checkForErrors(int bytesReceived, char *modbusResp)
{
    int  errorCode=0, gotResponseError=0,
         expectedByteSize;
    char szErrorMsg[100];

    expectedByteSize = MODBUS_RESPONSE_OFFSET + Config.byteSizeListOfDatatypes;
    szErrorMsg[0]    = 0;

    if (bytesReceived > 8)
    {
        // Check for Modbus-Error message. Means: Highest bit of byte is set. The following byte 
        // then holds the exception code
        errorCode        = (int)modbusResp[7];
        gotResponseError = (errorCode & (1 << 7));
    }

    if ( bytesReceived == 0)
        sprintf(szErrorMsg,"%s", "Corrupt request or response. Received 0 bytes.");
    else if ( bytesReceived == -1)
        sprintf(szErrorMsg,"%s", "No connection to device.");
    else if (gotResponseError)
        sprintf(szErrorMsg,"Reading nok. Exceptioncode hex: %02x", modbusResp[8]);
    else if (bytesReceived != expectedByteSize)
        sprintf(szErrorMsg, "Received %d bytes don't match expected length of %d", bytesReceived, expectedByteSize);        

    if (szErrorMsg[0] != 0)
    {
        setoutputtext(0, szErrorMsg);
        setoutputtext(1, "");
        return 1;        
    }
    
    return 0;
}

/*
    Decides whether the given characters specify an address in hex-format or not
*/
int isHexAddress(char *address)
{
    int addressLen = strlen(address);
    
    if (addressLen == 0)
        return 0;
    
    if (addressLen > 2 && 
        (address[0] == 'd' || address[0] == 'D') &&
        address[1] == ':')
        return 0;
    
    return 1;
}

/*
    Get the datatype per outputchannel and store it in an array
    Keep your data clean as errorhandling is not good
    
    Returns the total number of results (not bytes) to read
*/
void parseInput()
{
    int  delim_block=0, delim_number=0, time_colon=0,
         dtSumView=0, modbusId=0, i, 
         len_dtString, nextCharIdx, data_type_len,
         noResults, resultIdx, maxAvailableOutputChannels,
         datatype, currentAddress, offset=0;
    char szResults[10], szDatatype[2],
         handledAddressChar[3], szResultHelper[3],
         *ipPort, *dtString, *startAddressChar;
         
    struct registerData *hlpRegisterData;
    
    resultIdx        = MODBUS_RESPONSE_OFFSET;
    
    startAddressChar = MODBUS_INPUT_ADDRESS_HEX;
    dtString         = MODBUS_INPUT_RESULTS_DT;
    ipPort           = MODBUS_INPUT_IP_PORT;
    
    // Some initialization of config structure
    initInt(Config.szRegisterIdxView, MAX_DATATYPES_DEFINED_EXT, -1);
    maxAvailableOutputChannels       = (TRANSFER_DISPLAY_COMPONENTS + 1) * MAX_OUTPUT_CHANNELS;
    
    Config.no_resultvalues           = 0;
    Config.readCoils                 = 0;
    Config.byteSizeListOfDatatypes   = 0;
    Config.printHexStyle             = 0;
    Config.deviceLittleEndian        = ! MODBUS_INPUT_BIGENDIAN;
    Config.modbus_readtime_ms        = MODBUS_DEFAULT_READTIME_MS;
    Config.repeater_sleep_ms         = REPEATER_SLEEP_MS; 
    if (Config.repeater_sleep_ms <= 0)
        Config.repeater_sleep_ms = REPEATER_DEFAULT_SLEEP_MS;
    len_dtString                     = strlen(dtString);
    
    // Convert the specified address into a real value. By default assume a hex-char address
    strncpy(Config.szStartAddress,  startAddressChar, strlen(startAddressChar));
    if (isHexAddress(Config.szStartAddress))
    {
        if (Config.szStartAddress[1] == ':')    // Remove the specifier "h:"
        {
            for (i=0; i<8; i++)
                Config.szStartAddress[i] = Config.szStartAddress[i+2];
        }
        
        convertHexToBytes(&Config.szStartAddress[offset], &handledAddressChar[0]);
        currentAddress = (handledAddressChar[1]) | (handledAddressChar[0] << 8);
        Config.printHexStyle = 1;
    }
    else
    {
        currentAddress = batoi(&Config.szStartAddress[2]);  // Ignore specifier "d:" or "D:"
        memcpy(szResultHelper, &currentAddress, 2);
        swapBytes(szResultHelper, 2);
        convertToReadableHex(szResultHelper, 2, Config.szStartAddress, 0, 0);
    }

    // Now lets go to evaluating the datatypes
    do
    {
        time_colon = strfind(&dtString[delim_block], ":", 0);
        if (time_colon > 0)
        {
            strncpy(szResults, &dtString[0], time_colon);
            Config.modbus_readtime_ms = batoi(szResults);
            
            delim_block = time_colon+1;
        }

        delim_number = strfind(&dtString[delim_block], DELIMITER_DT_NUMBER, 0);
        strncpy(szResults,  &dtString[delim_block]               , delim_number);
        
        nextCharIdx = delim_block+delim_number+2;
        data_type_len = 1;
        if (nextCharIdx < len_dtString && dtString[nextCharIdx] >= '0' && dtString[nextCharIdx] <= '9')
            data_type_len = 2;
        
        strncpy(szDatatype, &dtString[delim_block+delim_number+1], data_type_len);

        noResults = batoi(szResults);
        datatype  = batoi(szDatatype);

        for (i=0; i<noResults; i++)
        {
            hlpRegisterData = &Config.szRegisters[Config.no_resultvalues];
            hlpRegisterData->datatype        = datatype;
            hlpRegisterData->idxResponse     = resultIdx;
            hlpRegisterData->registerAddress = currentAddress;

            resultIdx += BYTESIZEMAP[datatype];

            if (datatype == COIL)
                currentAddress += BYTESIZEMAP[datatype];
            else 
                currentAddress += BYTESIZEMAP[datatype] / 2;                
            
            if (PRINTBYTETYPE[datatype])
            {
                Config.szRegisterIdxView[dtSumView] = Config.no_resultvalues;
                dtSumView++;                
            }

            Config.no_resultvalues++;

            // hard cut at maximum output channels to avoid overflows
            if (dtSumView == maxAvailableOutputChannels)
                break;
        }

        delim_block = strfind(dtString, DELIMITER_DT_BLOCKS, delim_block+1);

        // Have we reached the last datatype block?
        if (delim_block == -1  || dtSumView == maxAvailableOutputChannels)
            break;

        delim_block++;
    }
    while (TRUE);
    
    Config.no_printValues          = dtSumView;
    Config.byteSizeListOfDatatypes = resultIdx - MODBUS_RESPONSE_OFFSET;
    
    // Post-handling: Calculate some values on the basis of the given data
    // Decide whether we are working on coils
    if (Config.no_resultvalues > 0)
    {
        hlpRegisterData = &Config.szRegisters[0];
        if (hlpRegisterData->datatype == COIL)
        {
            Config.readCoils = 1;
            
            Config.byteSizeListOfDatatypes = (Config.byteSizeListOfDatatypes + 7) / 8;
            Config.no_printValues          = Config.byteSizeListOfDatatypes;
        }
    }
    
    buildConnectionString(ipPort, Config.connectionString, &modbusId);
    Config.modbusId = modbusId;

    buildModbusRequest();

    for (i=0; i<MAX_OUTPUT_CHANNELS; i++)
    {
        resultIdx = Config.szRegisterIdxView[i];
        if (resultIdx == -1)
            break;
        
        hlpRegisterData = &Config.szRegisters[resultIdx];
        if (Config.printHexStyle)
        {
            memcpy(szResultHelper, &hlpRegisterData->registerAddress, 2);
            swapBytes(szResultHelper, 2);

            if (i > 0)
                sprintf(Config.szPrintedAdresses,"%s, ", Config.szPrintedAdresses);

            convertToReadableHex(szResultHelper, 2, Config.szPrintedAdresses, i, 0);
        }
        else
        {
            printValue(Config.szPrintedAdresses, hlpRegisterData->registerAddress, i);
        }
    }
    
    convertToUppercase(Config.szPrintedAdresses, RD_BLOCK_SIZE);
    
    free(startAddressChar);
    free(ipPort);
    free(dtString);
}

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

/*
    Hexify message for handover
    Only handover results to be displayed and already byte-swapped
*/
void msgMarshalling(char *hexOutput, int hexOutputSize, char *modbusResponse)
{ 
    int   i, viewIndex, 
          displayComponentIdx[TRANSFER_DISPLAY_COMPONENTS], displayComponentCnt=0;
    char  szResultHelper[5], frontString[40];
    short shortHelper, totalBytesSent=0;
    
    struct registerData *hlpRegisterData;
    
    initInt(displayComponentIdx, TRANSFER_DISPLAY_COMPONENTS, 0);
    hexOutput[0]   = 0;
    frontString[0] = 0;
    
    // Hexify the remaining resultvalues, datatypes etc. Be aware: 1 Byte == 2 characters
    i=MAX_OUTPUT_CHANNELS;
    while (TRUE)
    {
        viewIndex = Config.szRegisterIdxView[i];
        if (viewIndex == -1)
            break;

        if (i % MAX_OUTPUT_CHANNELS == 0)
        {
            displayComponentIdx[displayComponentCnt] = (int)totalBytesSent;
            displayComponentCnt++;
        }
        
        hlpRegisterData = &Config.szRegisters[viewIndex];
        
        // Hexify datatype
        szResultHelper[0] = (char)hlpRegisterData->datatype;    // Downcasting in sizes is ok here
        convertToReadableHex(szResultHelper, 1, hexOutput, 1, 0);
        totalBytesSent+=1;
            
        // Hexify address
        shortHelper = (short)hlpRegisterData->registerAddress;
        memcpy(szResultHelper, &shortHelper, 2);
        convertToReadableHex(szResultHelper, 2, hexOutput, 1, 0);
        totalBytesSent+=2;

        // Hexify value
        convertToReadableHex(&modbusResponse[hlpRegisterData->idxResponse], BYTESIZEMAP[hlpRegisterData->datatype], hexOutput, 1, 0);
        totalBytesSent+=BYTESIZEMAP[hlpRegisterData->datatype];

        i++;
    }
    
    // Metainfo being put in the front of the hex-string
    // - Address of first component: 1
    szResultHelper[0] = 1;    // First component is addressed
    convertToReadableHex(szResultHelper, 1, frontString, 0, 0);
    totalBytesSent+=1;
  
    // - index information for TRANSFER_DISPLAY_COMPONENTS to read the results efficiently
    for (i=0; i<TRANSFER_DISPLAY_COMPONENTS; i++)
    {
        shortHelper = (short)displayComponentIdx[i];
        memcpy(szResultHelper, &shortHelper, 2);
        convertToReadableHex(szResultHelper, 2, frontString, 1, 0);
        totalBytesSent+=2;
    }

    // - hexify the amount of result values being sent
    shortHelper = (short)(Config.no_printValues - MAX_OUTPUT_CHANNELS);
    memcpy(szResultHelper, &shortHelper, 2);
    convertToReadableHex(szResultHelper, 2, frontString, 1, 0);
    totalBytesSent+=2;

    // - Information about printing in hex or dec
    shortHelper = (short)Config.printHexStyle;
    memcpy(szResultHelper, &shortHelper, 2);
    convertToReadableHex(szResultHelper, 2, frontString, 1, 0);
    totalBytesSent+=2;

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

/*
	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; }
void initInt  (int  *array, int size, int value) { int x; for (x=0; x<size; x++) array[x] = value; }

/*
    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, STRING_NOPARAM); }
void debugMsgParam(char *msg, int val)
{
    char debugmsg[TRANSFER_SIZE];
    
    sprintf(debugmsg,"[DEBUG] %s", msg);
    messageParamTimed(debugmsg, val, 1000);
}

void messageParam     (char *msg, int val)              { messageParamTimed       (msg, val, 1000); }
void messageParamTimed(char *msg, int val, int sleepMs) { messageParamTimedChannel(msg, val, sleepMs, 0); }
void messageParamTimedChannel(char *msg, int val, int sleepMs, int channel)
{
    if (val == STRING_NOPARAM)
    {
        setoutputtext(channel, msg);
    }
    else
    {
        char szOutMsg[RD_BLOCK_SIZE];

        sprintf(szOutMsg,"%s %d", msg, val);
        setoutputtext(channel, szOutMsg);
    }

    if (sleepMs > 0)
        sleep(sleepMs);
}

/*
    Print status and readable information
*/
void printReadingStatus(int clearError, char *results, int bytesReceived)
{
    char szStatus[RD_BLOCK_SIZE], szInfo[70];

    if (strlen(results) > 0)
        sprintf(szStatus,"Reading ok, %s, %s", results, Config.szPrintedAdresses);
    else
        sprintf(szStatus,"Reading ok, %s", Config.szPrintedAdresses);

 	sprintf(szInfo, "Printing %d values at %s", Config.no_printValues, getFormattedTime(szInfo, 1));
   
    // Remove existing errormsg/refresh output channel
    if (clearError == 1)
        setoutputtext(0, "");    
    setoutputtext(1, szStatus);
    setoutputtext(2, szInfo);
}

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

    Mainfunctionality encapsulated in a function
    Due to picoC issues a little awkward
*/
void main()
{
    char  modbusResponse[RD_BLOCK_SIZE],
          modbusStrings[RD_BLOCK_SIZE], hexOutput[TRANSFER_SIZE],          
          handledAddresses[RD_BLOCK_SIZE], 
          szResultHelper[3];
    int   baseIndex, nBytesReceived,
          errorInLastIteration=0, errorInCurrentIteration,
          result_type, result_bytesize,
          channel, virtChannel, i;
    
    
    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 registerData *hlpRegisterData;
    
    STREAM* pTcpStream;

    initMapHex2Int();
    parseInput();

    // Keep polling
    while (TRUE)
    {
        modbusStrings[0]        = 0;

        errorInCurrentIteration = 0;
        virtChannel             = 0;
        channel                 = 0;

        messageParamTimedChannel("Reading Modbus-TCP. Waiting ms: ", Config.modbus_readtime_ms, 0, 1);

        /****************/
        // TCP Request/Response. Putting this in a function leads to instability on 
        // the miniserver when deploying during stream_read()
        #ifdef DEBUGMODE    debugMsg("Creating stream");    #endif
        pTcpStream = stream_create(Config.connectionString, 0, 0);
        
        #ifdef DEBUGMODE        
            debugMsg("Request being sent is printed on Txt2");
            convertToReadableHex(Config.modbusRequest, MODBUS_MSG_SIZE, hexOutput, 0, 1);
            messageParamTimedChannel(hexOutput, STRING_NOPARAM, 4000, 2);
        #endif
        stream_write(pTcpStream, Config.modbusRequest, MODBUS_MSG_SIZE);
        
        #ifdef DEBUGMODE    debugMsg("Flushing stream");    #endif
        stream_flush(pTcpStream);

        #ifdef DEBUGMODE    debugMsg("Reading stream");    #endif
        nBytesReceived = stream_read(pTcpStream, modbusResponse, RD_BLOCK_SIZE, Config.modbus_readtime_ms);

        #ifdef DEBUGMODE        
            debugMsgParam("Received bytes on stream: ", nBytesReceived);
            convertToReadableHex(modbusResponse, nBytesReceived, hexOutput, 0, 1);
            debugMsg("Printing received bytes on Txt2"); 
            messageParamTimedChannel(hexOutput, STRING_NOPARAM, 4000, 2);
        #endif
        stream_close(pTcpStream);
        /****************/

        if (checkForErrors(nBytesReceived, modbusResponse))
        {
            errorInLastIteration = 1;
            sleep(Config.repeater_sleep_ms);
            continue;
        }

        swapAllBytes(&modbusResponse[MODBUS_RESPONSE_OFFSET]);

        // Resulthandling
        // Buffers are big enough. So take the bytes from the responsestream and put them
        // into the result types/values    
        for (i=0; i<Config.no_printValues && channel<MAX_OUTPUT_CHANNELS; i++)
        {
            hlpRegisterData = &Config.szRegisters[Config.szRegisterIdxView[i]];
            result_type = hlpRegisterData->datatype;
            baseIndex   = hlpRegisterData->idxResponse;
            
            channel = i-virtChannel;

            // Decode Bits/Bytes to values
            switch (result_type)
            {
                case UINT16:    // sizeof(short) == 2 == 16bit
                    us_resultValues[channel] = (modbusResponse[baseIndex]) | (modbusResponse[baseIndex + 1] << 8);    
                    break;

                case INT16:        // sizeof(short) == 2 == 16bit
                    ss_resultValues[channel] = (modbusResponse[baseIndex]) | (modbusResponse[baseIndex + 1] << 8);
                    break;

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

                case INT32:        // sizeof(int)   == 4 == 32bit
                    si_resultValues[channel] = (modbusResponse[baseIndex] << 16) | (modbusResponse[baseIndex + 1] << 24) | (modbusResponse[baseIndex + 2]) | (modbusResponse[baseIndex + 3]  << 8);
                    break;

                case UINT64:        // sizeof(long)   == 8 == 64bit
                    ul_resultValues[channel] = (modbusResponse[baseIndex]   << 16) | (modbusResponse[baseIndex + 1] << 24) | (modbusResponse[baseIndex + 2]) | (modbusResponse[baseIndex + 3]  << 8) |
                                               (modbusResponse[baseIndex+4] << 16) | (modbusResponse[baseIndex + 5] << 24) | (modbusResponse[baseIndex + 6]) | (modbusResponse[baseIndex + 7]  << 8);
                    break;

                case INT64:        // sizeof(long)   == 8 == 64bit
                    sl_resultValues[channel] = (modbusResponse[baseIndex]   << 16) | (modbusResponse[baseIndex + 1] << 24) | (modbusResponse[baseIndex + 2]) | (modbusResponse[baseIndex + 3]  << 8) |
                                               (modbusResponse[baseIndex+4] << 16) | (modbusResponse[baseIndex + 5] << 24) | (modbusResponse[baseIndex + 6]) | (modbusResponse[baseIndex + 7]  << 8);
                    break;
                
                case FLOAT16:    // sizeof(float) == 8 == 64bit
                    f_resultValues[channel] = calc_float16(&modbusResponse[baseIndex]);
                    break;
                
                case FLOAT32:    // sizeof(float) == 8 == 64bit
                    f_resultValues[channel] = calc_float32(&modbusResponse[baseIndex]);
                    break;
                    
                case STRING16:
                case STRING32:
                    result_bytesize = BYTESIZEMAP[result_type];
                    decodeBytes2Ascii(&modbusResponse[baseIndex], modbusStrings, result_bytesize);
                    virtChannel++;
                    channel--;
                    break;

                case COIL:    // In case of coils the number of results are coded in bits. Means: 8 coils  == 1 Byte
                    setoutput(channel, modbusResponse[baseIndex]);    // sizeof(char)  == 1 == 8bit
                    break;
            }
            
            channel++;
        }

        if (errorInCurrentIteration)
        {
            messageParamTimedChannel("Unexpected data error. Received bytes: ", nBytesReceived, 0, 1);
            sleep(Config.repeater_sleep_ms);
            continue;
        }

        // Handover remaining results. Due to computing delays first do this.
        if (channel+virtChannel < Config.no_printValues && !Config.readCoils)
        {
            msgMarshalling(hexOutput, TRANSFER_SIZE, modbusResponse);
            messageParamTimedChannel(hexOutput, STRING_NOPARAM, 0, 0);
        }

        // Print results on output
        virtChannel = 0;
        channel     = 0;
        for (i=0; i<Config.no_printValues && channel<MAX_OUTPUT_CHANNELS; i++)
        {
            hlpRegisterData = &Config.szRegisters[Config.szRegisterIdxView[i]];
            result_type     = hlpRegisterData->datatype;
            channel         = i-virtChannel;

            // Display all values in a rush
            switch (result_type)
            {
                case UINT16:   setoutput(channel, us_resultValues[channel]); break;
                case INT16:    setoutput(channel, ss_resultValues[channel]); break;
                case UINT32:   setoutput(channel, ui_resultValues[channel]); break;
                case INT32:    setoutput(channel, si_resultValues[channel]); break;
                case UINT64:   setoutput(channel, ul_resultValues[channel]); break;
                case INT64:    setoutput(channel, sl_resultValues[channel]); break;
                case FLOAT16:    
                case FLOAT32:  setoutput(channel, f_resultValues[channel]);  break;
                case STRING16:
                case STRING32: virtChannel++; break;
            }

            channel++;
        }

        printReadingStatus(errorInLastIteration, modbusStrings, nBytesReceived);

        errorInLastIteration = 0;    
        sleep(Config.repeater_sleep_ms);
    }
}

main();