#include <Arduino.h>
#include <HardwareSerial.h>
#include <freertos/FreeRTOS.h>
#include <freertos/task.h>
#include <freertos/queue.h>
#include <freertos/semphr.h>

#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;
        bool _success;

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

// Estrutura para armazenar dados da mensagem Modbus
struct ModbusMessage {
    uint8_t slaveAddress;
    uint8_t functionCode;
    uint16_t registerAddress;
    int value;
    uint8_t* response;
    uint8_t* length;
    SemaphoreHandle_t responseSemaphore;
};

// Fila para armazenar mensagens Modbus
QueueHandle_t modbusQueue;

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

// Tarefa para processar a fila de mensagens Modbus
void processModbusQueue(void* pvParameters) {
    ModbusMessage message;

    while (true) {
        // Aguardar até que uma mensagem esteja disponível na fila
        if (xQueueReceive(modbusQueue, &message, portMAX_DELAY) == pdPASS) {
            uint8_t command[8];

            command[0] = message.slaveAddress;
            command[1] = message.functionCode;
            command[2] = (message.registerAddress >> 8) & 0xFF; // Big endian
            command[3] = message.registerAddress & 0xFF;
            
            // Converter o valor inteiro para o formato adequado
            command[4] = (message.value >> 8) & 0xFF; // Big endian
            command[5] = message.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()) {
                *message.length = SerialDriver.readBytes(message.response, SerialDriver.available());

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

            // Sinalizar que a resposta está disponível
            xSemaphoreGive(message.responseSemaphore);
        }
    }
}

// Função para inicializar o Modbus
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;

    // Criar a fila de mensagens Modbus
    modbusQueue = xQueueCreate(10, sizeof(ModbusMessage));
    if (modbusQueue == NULL) {
        Serial.println("Erro ao criar a fila de mensagens Modbus");
    }

    // Criar a tarefa de processamento da fila
    xTaskCreate(processModbusQueue, "ProcessModbusQueue", 4096, NULL, 1, NULL);
}


// 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) {
    ModbusMessage message;
    message.slaveAddress = slaveAddress;
    message.functionCode = functionCode;
    message.registerAddress = registerAddress;
    message.value = value;
    message.response = response;
    message.length = &length;
    message.responseSemaphore = xSemaphoreCreateBinary();

    // Adicionar a mensagem à fila
    if (xQueueSend(modbusQueue, &message, portMAX_DELAY) != pdPASS) {
        Serial.println("Erro ao adicionar mensagem à fila");
    }

    // Esperar até que a resposta esteja disponível
    xSemaphoreTake(message.responseSemaphore, portMAX_DELAY);
    vSemaphoreDelete(message.responseSemaphore);
}



#endif