// Function to calculate Modbus RTU CRC
int calculateModbusCRC(const char* data, int len) {
  int 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
      }
    }
  }
  // Display the calculated CRC
  Serial.print ("Modbus RTU CRC: " );
  Serial.println (crc, HEX);
  return crc;
}


// Function to send byte array with Modbus RTU CRC to Serial2
void sendToSerial2(const char* data, int len) {
  // Calculate Modbus CRC
  uint16_t crc = calculateModbusCRC(data, len);

  // Send data bytes
  for (int i = 0; i < len; i++) {
    byte b = data[i];
    Serial.print(i); Serial.print("=");
    Serial.println(b, HEX) ; //This omits leading zeros so hex 00 shows as 0 etc
    Serial2.print(b);
  }

  // Send CRC low byte
  Serial.print(crc & 0xFF, HEX);
Serial2.print(crc & 0xFF);
  // Send CRC high byte
  Serial.print((crc >> 8) & 0xFF, HEX);
  Serial2.print((crc >> 8) & 0xFF);
}

void test() {
  // Example usage

  //:01 04 80 E8 00 0C 59 FB
  /*000218-Tx:01 04 80 E8 00 0C 59 FB
    000219-Rx:01 04 18 00 F8 00 0C 00 0E 00 01 46 46 46 46 46 46 46 46 46 46 46 46 46 46 46 00
    85 A4
    000230-Tx:01 04 81 5B 00 0A 29 E2
    000231-Rx:01 04 14 00 00 00 04 00 00 00 01 00 00 00 04 06 08 00 00 00 00 00 00 EE EB
    000242-Tx:01 04 81 05 00 0A 48 30
    000243-Rx:01 04 14 00 00 00 71 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 71 21 EE
  */

  const char data[] = {0x01, 0x04, 0x80, 0xE8, 0x00, 0x0C};
  //expected Checksum :  0x59, 0xFB
  sendToSerial2( data, 6 );
}


void setup() {


  // put your setup code here, to run once:
  Serial.begin(115200);
  Serial.println("Hello, ESP32!");
  Serial2.begin(9600);
  delay(300);
  test();
}

void loop() {
  // put your main code here, to run repeatedly:
  delay(10); // this speeds up the simulation
}