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

	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 502
#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

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

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

// 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
*/
char* convertToReadableHex(char *bytes, int byteLen)
{
	char result[300];	
	int i=0;

	if (byteLen > 0)
		sprintf(result,"%02x",bytes[0]);

	for (i=1; i<byteLen; i++)
		sprintf(result,"%s %02x",result,bytes[i]);

	return result;
}

/*
	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];
	
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
	/*
		char *readableModbusRequest = convertToReadableHex(modbusRequest, MODBUS_MSG_SIZE);
		setoutputtext(0, readableModbusRequest); 
		free(readableModbusRequest);
	*/
	
	// 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");
	/*
		char *readableResult = convertToReadableHex(modbusResponse, nBytesReceived);
		setoutputtext(2, readableResult); 
		free(readableResult);
	*/
	}
	
	// Resulthandling
	// Buffers are big enough. So
	// take the bytes from the responsestream into the result types/values
	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 = 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] << 16) | (modbusResponse[baseIndex + 1] << 24) | (modbusResponse[baseIndex + 2] << 8) | (modbusResponse[baseIndex + 3]);
		else if (RESULT_TYPE == RESULT_DEFTYPE_INT32)
			si_resultValues[i] = (modbusResponse[baseIndex] << 16) | (modbusResponse[baseIndex + 1] << 24) | (modbusResponse[baseIndex + 2] << 8) | (modbusResponse[baseIndex + 3]);
		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_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);
}