#include <ModbusMaster.h>

// Modbus RTU frame structure
struct ModbusFrame {
  uint8_t slaveAddress;
  uint8_t functionCode;
  uint16_t startAddress;
  uint16_t quantity;
  uint16_t crc;
};

// Sensor data structure
struct SensorData {
  char operation[40];
  int address;
  int hex;
  int length;
  bool negative;
  bool datamode;
  bool datatype;
  int gain;
  char unit[3];
};

// Array to hold all instances of SensorData
SensorData sensorDataArray[] = {
  {"Product Mode", 33000, 0x80E8, 12, false, false, true, 1, ""},
  {"Total_Pv_Input_Power", 33057, 0x8121, 2, false, false, true, 1, "W"},
  {"Active_Power", 33079, 0x8137, 2, true, false, true, 1, "W"},
  {"Inverter_Temperature", 33093, 0x8145, 1, true, false, true, 0.1, "C"},
  {"Active_Power_Of_Electric_Meter", 33130, 0x816A, 2, true, false, true, 1, "W"},
  {"Battery_Current_Direction", 33135, 0x816F, 1, false, false, true, 1, ""},
  {"Soc_Value", 33139, 0x8173, 1, false, false, true, 1, "%"},
  {"Soh_Value", 33140, 0x8174, 1, false, false, true, 1, "%"},
  {"Load_Active_Power", 33147, 0x817B, 1, false, false, true, 1, "W"},
  {"Backup_Active_Power", 33148, 0x817C, 1, false, false, true, 1, "W"},
  {"Batteryp_Active_Power", 33149, 0x817D, 2, true, false, true, 1, "W"},
  {"Inv_Power", 33151, 0x817F, 2, true, false, true, 1, "W"},
};

int swapBytes(int value) {
  // Use bitwise operations to swap the bytes
  value = ((value >> 8) & 0xFF) | ((value << 8) & 0xFF00);
  return value;
}

// Function to calculate Modbus RTU CRC
uint16_t calculateModbusCRC(const char* data, int len) {
    uint16_t crc = 0xFFFF; // Initial value

    for (int i = 0; i < len; i++) {
        crc ^= static_cast<uint8_t>(data[i]); // XOR with current byte

        for (int j = 0; j < 8; j++) {
            if (crc & 0x0001) {
                crc = (crc >> 1) ^ 0xA001; // Right shift and XOR with polynomial
            } else {
                crc = crc >> 1; // Right shift
            }
        }
    }

    return crc;
}

// Function to create a Modbus RTU message from the SensorData structure
ModbusFrame createModbusMessage(const SensorData& sensorData) {
  ModbusFrame modbusMessage;

  modbusMessage.slaveAddress = 1; // Set the appropriate Modbus slave address
  modbusMessage.functionCode = 0x04; // 
  modbusMessage.startAddress = swapBytes(sensorData.address);
  modbusMessage.quantity = swapBytes(sensorData.length);

  // Calculate CRC (Cyclic Redundancy Check)
  modbusMessage.crc = calculateModbusCRC(reinterpret_cast<char*>(&modbusMessage), sizeof(ModbusFrame) - sizeof(uint16_t));

  return modbusMessage;
}

// Function to send Modbus RTU queries and print hex data to Serial port
void sendModbusQueries(const SensorData sensorDataArray[], size_t arraySize) {


  for (size_t i = 0; i < arraySize; ++i) {
    ModbusFrame modbusMessage = createModbusMessage(sensorDataArray[i]);

    // Print hex data to Serial port
    Serial.print("Query for ");
    Serial.print(sensorDataArray[i].operation);
    Serial.print(": ");
    
    for (size_t j = 0; j < sizeof(ModbusFrame); ++j) {
      if (reinterpret_cast<uint8_t*>(&modbusMessage)[j] < 0x10) {
        Serial.print("0");
      }
      Serial.print(reinterpret_cast<uint8_t*>(&modbusMessage)[j], HEX);
      Serial.print(" ");
    }

    Serial.println();
    delay(100); // Adjust the delay as needed
  }

}


void setup() {
    Serial.begin(115200);
  sendModbusQueries(sensorDataArray, sizeof(sensorDataArray) / sizeof(sensorDataArray[0]));
}

void loop() {
  // Your loop code here
}