#ifndef ModbusService
#define ModbusService

HardwareSerial SerialDriver(1);
#define RXD1 16
#define TXD1 17
#define DE_RE_PIN 4 // Pino para DE e RE
bool Debug = false;

class ModbusGetResponse {
    public:
        byte _address;
        byte _command_code;
        byte _num_registers;
        double _value;

        ModbusGetResponse() : _address(0), _command_code(0), _num_registers(0), _value(0.0) {}
};

void InicializarModbus(bool _debug = false) {
    SerialDriver.begin(9600, SERIAL_8N2, RXD1, TXD1);
    Serial.println("Modbus Inicializado");

    pinMode(DE_RE_PIN, OUTPUT);
    digitalWrite(DE_RE_PIN, LOW); // Inicialmente em modo de recepção

    Debug = _debug;
}

// Função para calcular CRC16
uint16_t calculateCRC(const uint8_t* data, uint8_t length) {
  uint16_t crc = 0xFFFF;

  for (uint8_t i = 0; i < length; i++) {
    crc ^= data[i];
    for (uint8_t j = 0; j < 8; j++) {
      if (crc & 0x0001) {
        crc >>= 1;
        crc ^= 0xA001;
      } else {
        crc >>= 1;
      }
    }
  }
  return crc;
}

// Função para construir o comando Modbus
void sendModbusCommand(uint8_t slaveAddress, uint8_t functionCode, uint16_t registerAddress, int value, uint8_t* response, uint8_t& length) {
  uint8_t command[8];

  command[0] = slaveAddress;
  command[1] = functionCode;
  command[2] = (registerAddress >> 8) & 0xFF; // Big endian
  command[3] = registerAddress & 0xFF;
  
  // Converter o valor inteiro para o formato adequado
  command[4] = (value >> 8) & 0xFF; // Big endian
  command[5] = value & 0xFF;

  uint16_t crc = calculateCRC(command, 6);
  command[6] = crc & 0xFF;
  command[7] = (crc >> 8) & 0xFF;

  // Alternar para modo de transmissão
  digitalWrite(DE_RE_PIN, HIGH);
  vTaskDelay(1); // Pequeno atraso para garantir que o modo de transmissão esteja ativo

  SerialDriver.write(command, 8);

  if (Debug) {
    // Imprimir o comando em formato hexadecimal
    Serial.print("Comando enviado: ");
    for (int i = 0; i < 8; i++) {
        if (command[i] < 0x10) {
        Serial.print("0"); // Adiciona um zero à esquerda para bytes menores que 0x10
        }
        Serial.print(command[i], HEX);
        Serial.print(" ");
    }
    Serial.println();
  }
  

  // Alternar para modo de recepção
  SerialDriver.flush(); // Garantir que todos os dados foram enviados
  digitalWrite(DE_RE_PIN, LOW);
  delay(1); // Pequeno atraso para garantir que o modo de recepção esteja ativo

  // Aguardar a resposta
  delay(100); // Ajuste o tempo de delay conforme necessário

  // Ler a resposta
  if (SerialDriver.available()) {
    length = SerialDriver.readBytes(response, SerialDriver.available());

    if (Debug) {
        // Imprimir a resposta em formato hexadecimal
        Serial.print("Resposta recebida: ");
        for (int i = 0; i < length; i++) {
        if (response[i] < 0x10) {
            Serial.print("0"); // Adiciona um zero à esquerda para bytes menores que 0x10
        }
        Serial.print(response[i], HEX);
        Serial.print(" ");
        }
        Serial.println();
    }
    
  } else {
    length = 0;
  }
}

#endif