#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

// Fila para armazenar mensagens Modbus
QueueHandle_t modbusQueue;
TaskHandle_t modbusTaskHandle = NULL;
// Definir prioridades
#define PRIORITY_HIGH 1
#define PRIORITY_LOW 0

HardwareSerial SerialDriver(1);
long ModbusBaudRate = 9600;
int RXD1 = 16;
int TXD1 = 17;
int DE_RE_PIN = 4;

bool Debug = false;

class ModbusGetResponse {
    public:
        byte _address;
        byte _command_code;
        byte _num_registers;
        double _value;
        bool _success = false;

        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;
    uint8_t priority;
    bool waitCallback;
};

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

void processModbusMessage(ModbusMessage& message) {
    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(pdMS_TO_TICKS(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);
    vTaskDelay(pdMS_TO_TICKS(1)); // Pequeno atraso para garantir que o modo de recepção esteja ativo

    *message.length = 0;

    if (message.waitCallback) {
        // Aguardar a resposta
        unsigned long startTime = millis();
        unsigned long timeout = 1000;
        while (SerialDriver.available() < 5 && (millis() - startTime) < timeout) {
            vTaskDelay(pdMS_TO_TICKS(50));
        }

        // 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 {
            Serial.println("Timeout ao aguardar resposta Modbus");
        }
    } else {
        vTaskDelay(pdMS_TO_TICKS(50));
    }

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

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) {
            // Processar mensagem de alta prioridade imediatamente
            if (message.priority == PRIORITY_HIGH) {
                // Processar mensagem de alta prioridade
                processModbusMessage(message);
            } else {
                // Verificar se há mensagens de alta prioridade na fila
                ModbusMessage highPriorityMessage;
                if (xQueuePeek(modbusQueue, &highPriorityMessage, 0) == pdPASS && highPriorityMessage.priority == PRIORITY_HIGH) {
                    // Recolocar a mensagem de baixa prioridade na fila
                    xQueueSendToBack(modbusQueue, &message, 0);
                } else {
                    // Processar mensagem de baixa prioridade
                    processModbusMessage(message);
                }
            }
        }
    }
}


// Função para inicializar o Modbus
void ConfigurarModbus(bool iniciar, int _rx, int _tx, int _md, long _baudRate = 9600, bool _debug = false) {
    // Se a comunicação serial já estiver inicializada, encerre-a
    if (SerialDriver) {
        SerialDriver.end();
    }

    // Se a fila já estiver criada, delete-a
    if (modbusQueue != NULL) {
        vQueueDelete(modbusQueue);
        modbusQueue = NULL;
    }

    // Se a tarefa já estiver criada, delete-a
    if (modbusTaskHandle != NULL) {
        vTaskDelete(modbusTaskHandle);
        modbusTaskHandle = NULL;
    }

    if (iniciar) {
        // Inicializar os novos pinos
        RXD1 = _rx;
        TXD1 = _tx;
        DE_RE_PIN = _md;
        ModbusBaudRate = _baudRate;

        // Inicializar a comunicação serial
        SerialDriver.begin(ModbusBaudRate, SERIAL_8N2, RXD1, TXD1);
        Serial.println("Modbus Inicializado");

        // Configurar o pino DE/RE
        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(100, sizeof(ModbusMessage));
        if (modbusQueue == NULL) {
            Serial.println("Erro ao criar a fila de mensagens Modbus");
            return; // Adicione um retorno para evitar continuar se a fila não for criada
        }

        // Criar a tarefa de processamento da fila
        if (xTaskCreate(processModbusQueue, "ProcessModbusQueue", 4096, NULL, 1, &modbusTaskHandle) != pdPASS) {
            Serial.println("Erro ao criar a tarefa de processamento da fila");
            return; // Adicione um retorno para evitar continuar se a tarefa não for criada
        }
    }
}

// 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, bool waitCallback) {
    ModbusMessage message;
    message.slaveAddress = slaveAddress;
    message.functionCode = functionCode;
    message.registerAddress = registerAddress;
    message.value = value;
    message.response = response;
    message.length = &length;
    message.priority = functionCode == 0x06 ? PRIORITY_HIGH : PRIORITY_LOW;
    message.waitCallback = waitCallback;
    message.responseSemaphore = xSemaphoreCreateBinary();

    if (message.responseSemaphore == NULL) {
        Serial.println("Erro ao criar semáforo");
        return; // Adicione um retorno para evitar continuar se o semáforo não for criado
    }

    // Adicionar a mensagem à fila
    if (xQueueSend(modbusQueue, &message, portMAX_DELAY) != pdPASS) {
        Serial.println("Erro ao adicionar mensagem à fila");
        vSemaphoreDelete(message.responseSemaphore);
        return; // Adicione um retorno para evitar continuar se a mensagem não for adicionada à fila
    }

    // Esperar até que a resposta esteja disponível
    if (xSemaphoreTake(message.responseSemaphore, portMAX_DELAY) != pdPASS) {
        Serial.println("Erro ao tomar semáforo");
    }
    vSemaphoreDelete(message.responseSemaphore);
}

#endif