#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];
  uint8_t result[256]; // Changed the type to byte array
  uint8_t resultLength; // New member to store the length of result
};


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

// 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; // Was 0x03 
  modbusMessage.startAddress = sensorData.address;
  modbusMessage.quantity = 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 to Serial2
void sendModbusQueriesToSerial2(const SensorData sensorDataArray[], size_t arraySize) {
  for (size_t i = 0; i < arraySize; ++i) {
    ModbusFrame modbusMessage = createModbusMessage(sensorDataArray[i]);

    Serial.print("Query for ");
    Serial.println(sensorDataArray[i].operation);

    Serial.print("\t");
    for (size_t j = 0; j < sizeof(ModbusFrame); ++j) {
      if (reinterpret_cast<uint8_t*>(&modbusMessage)[j] < 0x10) {
        Serial2.print("0");
      }
      Serial2.print(reinterpret_cast<uint8_t*>(&modbusMessage)[j], HEX);
      Serial2.print(" ");
    }

       // Send raw binary data to Serial2
    Serial2.write(reinterpret_cast<uint8_t*>(&modbusMessage), sizeof(ModbusFrame));

    // Process the response
    //getResult(sensorDataArray[i]);
    getResult(i);

    Serial.print("Result Length: ");
    Serial.print(sensorDataArray[i].resultLength);
    Serial.println();
    
    // Print Result
    Serial.print("Result: ");
    for (int j = 0; j < sensorDataArray[i].resultLength; ++j) {
      Serial.print(sensorDataArray[i].result[j], HEX);
      Serial.print(" ");
    }
    Serial.println();
  }
}

// Function to get result from Serial2
//void getResult(SensorData& sensorData) {
  void getResult(int i) {
  // Wait for up to 100ms for data to be available
  unsigned long startMillis = millis();
  while (millis() - startMillis < 100 && Serial2.available() == 0) {
    delay(1); // Allow other tasks to run
  }

  // Read incoming data while available
  int index = 0;
  while (Serial2.available()) {
    sensorDataArray[i].result[index++] = Serial2.read();
    
    // Check for buffer overflow
    if (index >= sizeof(sensorDataArray[i].result)) {
      break;
    }
  }

  // Store the length of the result
  sensorDataArray[i].resultLength = static_cast<uint8_t>(index);
}


void setup() {
  Serial.begin(115200); // Initialize Serial at 115200 baud
  Serial2.begin(9600); // Initialize Serial2 at 9600 baud
  sendModbusQueriesToSerial2(sensorDataArray, sizeof(sensorDataArray) / sizeof(sensorDataArray[0]));
}

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