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

};

struct ModResults {
  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, ""},
  {"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"}
};

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

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


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



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

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

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

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

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

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

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

          results[i].result[z] = 0;
          results[i].result[z + 1] = z + i;
        }
        results[i].result[3 + len] = 0xff;
        results[i].result[4 + len ] = 0xee;
        results[i].resultLength = len + 5;
      }
    }



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

    Serial.print(" Result: ");
    for (int j = 0; j < results[i].resultLength; ++j) {
      if (results[i].result[j] < 16 ) {
        Serial.print("0");
      }
      Serial.print(results[i].result[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);
}