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

static const char *TAG = "modbus";

#define MB_PORT_NUM (CONFIG_MB_UART_PORT_NUM)   // Number of UART port used for Modbus connection
#define MB_DEV_SPEED (CONFIG_MB_UART_BAUD_RATE) // The communication speed of the UART

static void *master_handle = NULL;

// Enumeration of all supported CIDs for device (used in parameter definition table)
enum {
    CID_INP_DATA_0 = 0,
    CID_HOLD_DATA_0,
    CID_INP_DATA_1,
    CID_HOLD_DATA_1,
    CID_INP_DATA_2,
    CID_HOLD_DATA_2,
    CID_HOLD_TEST_REG,
    CID_RELAY_P1,
    CID_RELAY_P2,
    CID_DISCR_P1,
#if CONFIG_FMB_EXT_TYPE_SUPPORT
    CID_HOLD_U8_A,
    CID_HOLD_U8_B,
    CID_HOLD_U16_AB,
    CID_HOLD_U16_BA,
    CID_HOLD_UINT32_ABCD,
    CID_HOLD_UINT32_CDAB,
    CID_HOLD_UINT32_BADC,
    CID_HOLD_UINT32_DCBA,
    CID_HOLD_FLOAT_ABCD,
    CID_HOLD_FLOAT_CDAB,
    CID_HOLD_FLOAT_BADC,
    CID_HOLD_FLOAT_DCBA,
    CID_HOLD_DOUBLE_ABCDEFGH,
    CID_HOLD_DOUBLE_HGFEDCBA,
    CID_HOLD_DOUBLE_GHEFCDAB,
    CID_HOLD_DOUBLE_BADCFEHG,
#endif
    CID_COUNT
};

// Enumeration of modbus device addresses accessed by master device
enum {
    MB_DEVICE_ADDR1 = 1 // Only one slave device used for the test (add other slave addresses here)
};

#define STR(fieldname) ((const char *)(fieldname))

// The macro to get offset for parameter in the appropriate structure
#define HOLD_OFFSET(field) ((uint16_t)(offsetof(holding_reg_params_t, field) + 1))
#define INPUT_OFFSET(field) ((uint16_t)(offsetof(input_reg_params_t, field) + 1))
#define COIL_OFFSET(field) ((uint16_t)(offsetof(coil_reg_params_t, field) + 1))
// Discrete offset macro
#define DISCR_OFFSET(field) ((uint16_t)(offsetof(discrete_reg_params_t, field) + 1))

#define TEST_INPUT_REG_START(field) (INPUT_OFFSET(field) >> 1)
#define TEST_INPUT_REG_SIZE(field) (sizeof(((input_reg_params_t *)0)->field) >> 1)
// Example Data (Object) Dictionary for Modbus parameters:
// The CID field in the table must be unique.
// Modbus Slave Addr field defines slave address of the device with correspond parameter.
// Modbus Reg Type - Type of Modbus register area (Holding register, Input Register and such).
// Reg Start field defines the start Modbus register number and Reg Size defines the number of registers for the
// characteristic accordingly. The Instance Offset defines offset in the appropriate parameter structure that will be
// used as instance to save parameter value. Data Type, Data Size specify type of the characteristic and its data size.
// Parameter Options field specifies the options that can be used to process parameter value (limits or masks).
// Access Mode - can be used to implement custom options for processing of characteristic (Read/Write restrictions,
// factory mode values and etc).
const mb_parameter_descriptor_t device_parameters[] = {
    // { CID, Param Name, Units, Modbus Slave Addr, Modbus Reg Type, Reg Start, Reg Size, Instance Offset, Data Type,
    // Data Size, Parameter Options, Access Mode}
    {CID_INP_DATA_0, STR("Data_channel_0"), STR("Volts"), MB_DEVICE_ADDR1, MB_PARAM_INPUT,
     TEST_INPUT_REG_START(input_data0), TEST_INPUT_REG_SIZE(input_data0), INPUT_OFFSET(input_data0), PARAM_TYPE_FLOAT,
     4, OPTS(TEST_TEMP_MIN, TEST_TEMP_MAX, 0), PAR_PERMS_READ_WRITE_TRIGGER},
    {CID_HOLD_DATA_0, STR("Humidity_1"), STR("%rH"), MB_DEVICE_ADDR1, MB_PARAM_HOLDING,
     TEST_HOLD_REG_START(holding_data0), TEST_HOLD_REG_SIZE(holding_data0), HOLD_OFFSET(holding_data0),
     PARAM_TYPE_FLOAT, 4, OPTS(TEST_HUMI_MIN, TEST_HUMI_MAX, 0), PAR_PERMS_READ_WRITE_TRIGGER},
    {CID_INP_DATA_1, STR("Temperature_1"), STR("C"), MB_DEVICE_ADDR1, MB_PARAM_INPUT, TEST_INPUT_REG_START(input_data1),
     TEST_INPUT_REG_SIZE(input_data1), INPUT_OFFSET(input_data1), PARAM_TYPE_FLOAT, 4,
     OPTS(TEST_TEMP_MIN, TEST_TEMP_MAX, 0), PAR_PERMS_READ_WRITE_TRIGGER},
    {CID_HOLD_DATA_1, STR("Humidity_2"), STR("%rH"), MB_DEVICE_ADDR1, MB_PARAM_HOLDING,
     TEST_HOLD_REG_START(holding_data1), TEST_HOLD_REG_SIZE(holding_data1), HOLD_OFFSET(holding_data1),
     PARAM_TYPE_FLOAT, 4, OPTS(TEST_HUMI_MIN, TEST_HUMI_MAX, 0), PAR_PERMS_READ_WRITE_TRIGGER},
    {CID_INP_DATA_2, STR("Temperature_2"), STR("C"), MB_DEVICE_ADDR1, MB_PARAM_INPUT, TEST_INPUT_REG_START(input_data2),
     TEST_INPUT_REG_SIZE(input_data2), INPUT_OFFSET(input_data2), PARAM_TYPE_FLOAT, 4,
     OPTS(TEST_TEMP_MIN, TEST_TEMP_MAX, 0), PAR_PERMS_READ_WRITE_TRIGGER},
    {CID_HOLD_DATA_2, STR("Humidity_3"), STR("%rH"), MB_DEVICE_ADDR1, MB_PARAM_HOLDING,
     TEST_HOLD_REG_START(holding_data2), TEST_HOLD_REG_SIZE(holding_data2), HOLD_OFFSET(holding_data2),
     PARAM_TYPE_FLOAT, 4, OPTS(TEST_HUMI_MIN, TEST_HUMI_MAX, 0), PAR_PERMS_READ_WRITE_TRIGGER},
    {CID_HOLD_TEST_REG, STR("Test_regs"), STR("__"), MB_DEVICE_ADDR1, MB_PARAM_HOLDING, TEST_HOLD_REG_START(test_regs),
     TEST_ARR_REG_SZ, HOLD_OFFSET(test_regs), PARAM_TYPE_ASCII, (TEST_ARR_REG_SZ * 2),
     OPTS(TEST_TEMP_MIN, TEST_TEMP_MAX, TEST_ASCII_BIN), PAR_PERMS_READ_WRITE_TRIGGER},
    {CID_RELAY_P1, STR("RelayP1"), STR("on/off"), MB_DEVICE_ADDR1, MB_PARAM_COIL, 2, 6, COIL_OFFSET(coils_port0),
     PARAM_TYPE_U8, 1, OPTS(0xAA, 0x15, 0), PAR_PERMS_READ_WRITE_TRIGGER},
    {CID_RELAY_P2, STR("RelayP2"), STR("on/off"), MB_DEVICE_ADDR1, MB_PARAM_COIL, 10, 6, COIL_OFFSET(coils_port1),
     PARAM_TYPE_U8, 1, OPTS(0x55, 0x2A, 0), PAR_PERMS_READ_WRITE_TRIGGER},
    {CID_DISCR_P1, STR("DiscreteInpP1"), STR("on/off"), MB_DEVICE_ADDR1, MB_PARAM_DISCRETE, 2, 7,
     DISCR_OFFSET(discrete_input_port1), PARAM_TYPE_U8, 1, OPTS(0xAA, 0x15, 0), PAR_PERMS_READ_WRITE_TRIGGER},
#if CONFIG_FMB_EXT_TYPE_SUPPORT
    {CID_HOLD_U8_A, STR("U8_A"), STR("__"), MB_DEVICE_ADDR1, MB_PARAM_HOLDING, TEST_HOLD_REG_START(holding_u8_a),
     TEST_HOLD_REG_SIZE(holding_u8_a), HOLD_OFFSET(holding_u8_a), PARAM_TYPE_U8_A,
     (TEST_HOLD_REG_SIZE(holding_u8_a) << 1), OPTS(CHAR_MIN, 0x0055, 0x0055), PAR_PERMS_READ_WRITE_TRIGGER},
    {CID_HOLD_U8_B, STR("U8_B"), STR("__"), MB_DEVICE_ADDR1, MB_PARAM_HOLDING, TEST_HOLD_REG_START(holding_u8_b),
     TEST_HOLD_REG_SIZE(holding_u8_b), HOLD_OFFSET(holding_u8_b), PARAM_TYPE_U8_B,
     (TEST_HOLD_REG_SIZE(holding_u8_b) << 1), OPTS(0, 0x5500, 0x5500), PAR_PERMS_READ_WRITE_TRIGGER},
    {CID_HOLD_U16_AB, STR("U16_AB"), STR("__"), MB_DEVICE_ADDR1, MB_PARAM_HOLDING, TEST_HOLD_REG_START(holding_u16_ab),
     TEST_HOLD_REG_SIZE(holding_u16_ab), HOLD_OFFSET(holding_u16_ab), PARAM_TYPE_U16_AB,
     (TEST_HOLD_REG_SIZE(holding_u16_ab) << 1), OPTS(0, TEST_VALUE, TEST_VALUE), PAR_PERMS_READ_WRITE_TRIGGER},
    {CID_HOLD_U16_BA, STR("U16_BA"), STR("__"), MB_DEVICE_ADDR1, MB_PARAM_HOLDING, TEST_HOLD_REG_START(holding_u16_ba),
     TEST_HOLD_REG_SIZE(holding_u16_ba), HOLD_OFFSET(holding_u16_ba), PARAM_TYPE_U16_BA,
     (TEST_HOLD_REG_SIZE(holding_u16_ab) << 1), OPTS(0, TEST_VALUE, TEST_VALUE), PAR_PERMS_READ_WRITE_TRIGGER},
    {CID_HOLD_UINT32_ABCD, STR("UINT32_ABCD"), STR("__"), MB_DEVICE_ADDR1, MB_PARAM_HOLDING,
     TEST_HOLD_REG_START(holding_uint32_abcd), TEST_HOLD_REG_SIZE(holding_uint32_abcd),
     HOLD_OFFSET(holding_uint32_abcd), PARAM_TYPE_U32_ABCD, (TEST_HOLD_REG_SIZE(holding_uint32_abcd) << 1),
     OPTS(0, TEST_VALUE, TEST_VALUE), PAR_PERMS_READ_WRITE_TRIGGER},
    {CID_HOLD_UINT32_CDAB, STR("UINT32_CDAB"), STR("__"), MB_DEVICE_ADDR1, MB_PARAM_HOLDING,
     TEST_HOLD_REG_START(holding_uint32_cdab), TEST_HOLD_REG_SIZE(holding_uint32_cdab),
     HOLD_OFFSET(holding_uint32_cdab), PARAM_TYPE_U32_CDAB, (TEST_HOLD_REG_SIZE(holding_uint32_cdab) << 1),
     OPTS(0, TEST_VALUE, TEST_VALUE), PAR_PERMS_READ_WRITE_TRIGGER},
    {CID_HOLD_UINT32_BADC, STR("UINT32_BADC"), STR("__"), MB_DEVICE_ADDR1, MB_PARAM_HOLDING,
     TEST_HOLD_REG_START(holding_uint32_badc), TEST_HOLD_REG_SIZE(holding_uint32_badc),
     HOLD_OFFSET(holding_uint32_badc), PARAM_TYPE_U32_BADC, (TEST_HOLD_REG_SIZE(holding_uint32_badc) << 1),
     OPTS(0, TEST_VALUE, TEST_VALUE), PAR_PERMS_READ_WRITE_TRIGGER},
    {CID_HOLD_UINT32_DCBA, STR("UINT32_DCBA"), STR("__"), MB_DEVICE_ADDR1, MB_PARAM_HOLDING,
     TEST_HOLD_REG_START(holding_uint32_dcba), TEST_HOLD_REG_SIZE(holding_uint32_dcba),
     HOLD_OFFSET(holding_uint32_dcba), PARAM_TYPE_U32_DCBA, (TEST_HOLD_REG_SIZE(holding_uint32_dcba) << 1),
     OPTS(0, TEST_VALUE, TEST_VALUE), PAR_PERMS_READ_WRITE_TRIGGER},
    {CID_HOLD_FLOAT_ABCD, STR("FLOAT_ABCD"), STR("__"), MB_DEVICE_ADDR1, MB_PARAM_HOLDING,
     TEST_HOLD_REG_START(holding_float_abcd), TEST_HOLD_REG_SIZE(holding_float_abcd), HOLD_OFFSET(holding_float_abcd),
     PARAM_TYPE_FLOAT_ABCD, (TEST_HOLD_REG_SIZE(holding_float_abcd) << 1), OPTS(0, TEST_VALUE, TEST_VALUE),
     PAR_PERMS_READ_WRITE_TRIGGER},
    {CID_HOLD_FLOAT_CDAB, STR("FLOAT_CDAB"), STR("__"), MB_DEVICE_ADDR1, MB_PARAM_HOLDING,
     TEST_HOLD_REG_START(holding_float_cdab), TEST_HOLD_REG_SIZE(holding_float_cdab), HOLD_OFFSET(holding_float_cdab),
     PARAM_TYPE_FLOAT_CDAB, (TEST_HOLD_REG_SIZE(holding_float_cdab) << 1), OPTS(0, TEST_VALUE, TEST_VALUE),
     PAR_PERMS_READ_WRITE_TRIGGER},
    {CID_HOLD_FLOAT_BADC, STR("FLOAT_BADC"), STR("__"), MB_DEVICE_ADDR1, MB_PARAM_HOLDING,
     TEST_HOLD_REG_START(holding_float_badc), TEST_HOLD_REG_SIZE(holding_float_badc), HOLD_OFFSET(holding_float_badc),
     PARAM_TYPE_FLOAT_BADC, (TEST_HOLD_REG_SIZE(holding_float_badc) << 1), OPTS(0, TEST_VALUE, TEST_VALUE),
     PAR_PERMS_READ_WRITE_TRIGGER},
    {CID_HOLD_FLOAT_DCBA, STR("FLOAT_DCBA"), STR("__"), MB_DEVICE_ADDR1, MB_PARAM_HOLDING,
     TEST_HOLD_REG_START(holding_float_dcba), TEST_HOLD_REG_SIZE(holding_float_dcba), HOLD_OFFSET(holding_float_dcba),
     PARAM_TYPE_FLOAT_DCBA, (TEST_HOLD_REG_SIZE(holding_float_dcba) << 1), OPTS(0, TEST_VALUE, TEST_VALUE),
     PAR_PERMS_READ_WRITE_TRIGGER},
    {CID_HOLD_DOUBLE_ABCDEFGH, STR("DOUBLE_ABCDEFGH"), STR("__"), MB_DEVICE_ADDR1, MB_PARAM_HOLDING,
     TEST_HOLD_REG_START(holding_double_abcdefgh), TEST_HOLD_REG_SIZE(holding_double_abcdefgh),
     HOLD_OFFSET(holding_double_abcdefgh), PARAM_TYPE_DOUBLE_ABCDEFGH,
     (TEST_HOLD_REG_SIZE(holding_double_abcdefgh) << 1), OPTS(0, TEST_VALUE, TEST_VALUE), PAR_PERMS_READ_WRITE_TRIGGER},
    {CID_HOLD_DOUBLE_HGFEDCBA, STR("DOUBLE_HGFEDCBA"), STR("__"), MB_DEVICE_ADDR1, MB_PARAM_HOLDING,
     TEST_HOLD_REG_START(holding_double_hgfedcba), TEST_HOLD_REG_SIZE(holding_double_hgfedcba),
     HOLD_OFFSET(holding_double_hgfedcba), PARAM_TYPE_DOUBLE_HGFEDCBA,
     (TEST_HOLD_REG_SIZE(holding_double_hgfedcba) << 1), OPTS(0, TEST_VALUE, TEST_VALUE), PAR_PERMS_READ_WRITE_TRIGGER},
    {CID_HOLD_DOUBLE_GHEFCDAB, STR("DOUBLE_GHEFCDAB"), STR("__"), MB_DEVICE_ADDR1, MB_PARAM_HOLDING,
     TEST_HOLD_REG_START(holding_double_ghefcdab), TEST_HOLD_REG_SIZE(holding_double_ghefcdab),
     HOLD_OFFSET(holding_double_ghefcdab), PARAM_TYPE_DOUBLE_GHEFCDAB,
     (TEST_HOLD_REG_SIZE(holding_double_ghefcdab) << 1), OPTS(0, TEST_VALUE, TEST_VALUE), PAR_PERMS_READ_WRITE_TRIGGER},
    {CID_HOLD_DOUBLE_BADCFEHG, STR("DOUBLE_BADCFEHG"), STR("__"), MB_DEVICE_ADDR1, MB_PARAM_HOLDING,
     TEST_HOLD_REG_START(holding_double_badcfehg), TEST_HOLD_REG_SIZE(holding_double_badcfehg),
     HOLD_OFFSET(holding_double_badcfehg), PARAM_TYPE_DOUBLE_BADCFEHG,
     (TEST_HOLD_REG_SIZE(holding_double_badcfehg) << 1), OPTS(0, TEST_VALUE, TEST_VALUE), PAR_PERMS_READ_WRITE_TRIGGER}
#endif
};

// Modbus master initialization
static esp_err_t master_init(void) {
    // Initialize Modbus controller
    mb_communication_info_t comm;
    comm.ser_opts.port = MB_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 = MB_DEV_SPEED;
    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);
    MB_RETURN_ON_FALSE((master_handle != NULL), ESP_ERR_INVALID_STATE, TAG, "mb controller initialization fail.");
    MB_RETURN_ON_FALSE((err == ESP_OK), ESP_ERR_INVALID_STATE, TAG, "mb controller initialization fail, returns(0x%x).",
                       (int)err);

    // Set UART pin numbers
    err = uart_set_pin(static_cast<uart_port_t>(MB_PORT_NUM), CONFIG_MB_UART_TXD, CONFIG_MB_UART_RXD,
                       CONFIG_MB_UART_RTS, UART_PIN_NO_CHANGE);
    MB_RETURN_ON_FALSE((err == ESP_OK), ESP_ERR_INVALID_STATE, TAG,
                       "mb serial set pin failure, uart_set_pin() returned (0x%x).", (int)err);

    err = mbc_master_start(master_handle);
    MB_RETURN_ON_FALSE((err == ESP_OK), ESP_ERR_INVALID_STATE, TAG, "mb controller start fail, returned (0x%x).",
                       (int)err);

    // Set driver mode to Half Duplex
    err = uart_set_mode(static_cast<uart_port_t>(MB_PORT_NUM), UART_MODE_RS485_HALF_DUPLEX);
    MB_RETURN_ON_FALSE((err == ESP_OK), ESP_ERR_INVALID_STATE, TAG,
                       "mb serial set mode failure, uart_set_mode() returned (0x%x).", (int)err);

    vTaskDelay(5);
    err = mbc_master_set_descriptor(master_handle, &device_parameters[0], num_device_parameters);
    MB_RETURN_ON_FALSE((err == ESP_OK), ESP_ERR_INVALID_STATE, TAG, "mb controller set descriptor fail, returns(0x%x).",
                       (int)err);
    ESP_LOGI(TAG, "Modbus master stack initialized...");
    return err;
}

Modbus::Modbus() {
    const uart_port_t uart_num = UART_NUM_2;
    uart_config_t uart_config = {
        .baud_rate = 115200,
        .data_bits = UART_DATA_8_BITS,
        .parity = UART_PARITY_DISABLE,
        .stop_bits = UART_STOP_BITS_1,
        .flow_ctrl = UART_HW_FLOWCTRL_CTS_RTS,
        .rx_flow_ctrl_thresh = 122,
    };
    // Configure UART parameters
    ESP_ERROR_CHECK(uart_param_config(uart_num, &uart_config));

    ESP_ERROR_CHECK(uart_set_pin(UART_NUM_2, 17, 16, 7, 8));

    // Setup UART buffered IO with event queue
    const int uart_buffer_size = (1024 * 2);
    QueueHandle_t uart_queue;
    // Install UART driver using an event queue here
    ESP_ERROR_CHECK(uart_driver_install(UART_NUM_2, uart_buffer_size, uart_buffer_size, 10, &uart_queue, 0));
}