#include "Modbus.h"
#include "modbus_params.h"
#include "sdkconfig.h"
#include <driver/uart.h>
#include <esp_modbus_master.h>

static const char *TAG = "modbus";
static void *master_handle = NULL;

const mb_parameter_descriptor_t device_parameters[] = {{0x00, "voltage", "v", 0x01, MB_PARAM_INPUT, 0, 2}};

const uint16_t num_device_parameters = (sizeof(device_parameters) / sizeof(device_parameters[0]));

Modbus::Modbus() {
    // Initialize Modbus controller
    mb_communication_info_t comm;
    memset(&comm, 0, sizeof(mb_communication_info_t));

    comm.ser_opts.port = static_cast<uart_port_t>(CONFIG_MB_UART_PORT_NUM);
#if CONFIG_MB_COMM_MODE_ASCII
    comm.ser_opts.mode = MB_ASCII;
#elif CONFIG_MB_COMM_MODE_RTU
    comm.ser_opts.mode = MB_RTU;
#endif
    comm.ser_opts.baudrate = CONFIG_MB_UART_BAUD_RATE;
    comm.ser_opts.parity = MB_PARITY_NONE;
    comm.ser_opts.uid = 0;
    comm.ser_opts.response_tout_ms = 1000;
    comm.ser_opts.data_bits = UART_DATA_8_BITS;
    comm.ser_opts.stop_bits = UART_STOP_BITS_1;

    esp_err_t err = mbc_master_create_serial(&comm, &master_handle);

    // Set UART pin numbers
    err = uart_set_pin(static_cast<uart_port_t>(CONFIG_MB_UART_PORT_NUM), CONFIG_MB_UART_TXD, CONFIG_MB_UART_RXD,
                       UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE);

    err = mbc_master_start(master_handle);

    // Set driver mode to Half Duplex
    err = uart_set_mode(static_cast<uart_port_t>(CONFIG_MB_UART_PORT_NUM), UART_MODE_RS485_HALF_DUPLEX);

    vTaskDelay(5);
    err = mbc_master_set_descriptor(master_handle, &device_parameters[0], num_device_parameters);
    ESP_LOGI(TAG, "Modbus master stack initialized...");
}