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

};

struct modbusData {
  uint8_t msgLength; // Message length
  uint8_t data[60]; // The data
};


// 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, 10, "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"}
};

modbusData outmsg;
modbusData results[20] ;


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


void makeMsg(const SensorData& sd, modbusData& msg) {
  msg.data[0] = 0x1;
  msg.data[1] = 0x04;
  msg.data[2] = sd.hex >> 8;
  msg.data[3] = sd.hex & 0xff;
  msg.data[4] = 0;
  msg.data[5] = sd.length;
  //Now need to get the checksum
  msg.msgLength = 8 ; //Includes checksum
  int crc = calculateModbusCRC((const char*) msg.data, msg.msgLength - 2);
  msg.data[6] = crc & 0xff;
  msg.data[7] = crc >> 8;
}

// Function to send Modbus RTU queries
void sendModbusQueries(const SensorData sensorDataArray[], size_t arraySize) {

  for (size_t i = 0; i < arraySize; ++i) {

    makeMsg(sensorDataArray[i], outmsg);
    Serial.print("Query for ");
    Serial.print(sensorDataArray[i].operation);
    Serial.println(": ");


    for (int j = 0; j < outmsg.msgLength; ++j) {
      if (outmsg.data[j] < 16 ) {
        Serial.print("0");
      }
      Serial.print(outmsg.data[j], HEX);
      Serial.print(" ");
    }
    Serial.println("");

    Serial2.write(outmsg.data, outmsg.msgLength);

    // 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()) {
        results[i].data[index++] = Serial2.read();

        // Check for buffer overflow
        if (index >= sizeof(results[i].msgLength)) {
          break;
        }
      }

      // Store the length of the result
      results[i].msgLength = static_cast<uint8_t>(index);

      if (index == 0 ) {
        //Create dummy test data

        results[i].data[0] = 0x01;
        results[i].data[1] = 0x04;

        int len = sensorDataArray[i].length * 2;
        results[i].data[2] = len;

        int z = 0;
        for (z = 3; z < len + 3; z += 2) {
          //Create dummy result

          results[i].data[z] = 0;
          results[i].data[z + 1] = z + i;
        }
        results[i].msgLength = len + 5;
        int crc = calculateModbusCRC((const char*) results[i].data, results[i].msgLength - 2);

        results[i].data[3 + len] = crc & 0xff;
        results[i].data[4 + len ] = crc >> 8;

      }
    }



    Serial.print("Result Length: ");
    Serial.print(results[i].msgLength);

    Serial.print(" Result: ");
    for (int j = 0; j < results[i].msgLength; ++j) {
      if (results[i].data[j] < 16 ) {
        Serial.print("0");
      }
      Serial.print(results[i].data[j], HEX);
      Serial.print(" ");
    }
    Serial.println("\n");
  }
}

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

void loop() {
  // Your loop code here
  delay(1000);
}