#define LOG_LOCAL_LEVEL ESP_LOG_VERBOSE
#include "esp_log.h"

#include "serial_mdb.h"
#include "modbus_params.h" // for modbus parameters structures
#include "mbcontroller.h"
#include "sdkconfig.h"
#include "evse_api.h"
#include "loadbalancer.h"
#include "meter.h"

#define TXD_PIN (GPIO_NUM_17)
#define RXD_PIN (GPIO_NUM_16)

static const char *TAG = "serial_mdb";

static bool enabled = false;
static bool meterState = false;
static bool meterTest = false;

static TaskHandle_t serial_mdb_task = NULL;

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

#define MB_UART_TXD 17
#define MB_UART_RXD 16
#define MB_UART_RTS 5

// Note: Some pins on target chip cannot be assigned for UART communication.
// See UART documentation for selected board and target to configure pins using Kconfig.

// The number of parameters that intended to be used in the particular control process
#define MASTER_MAX_CIDS num_device_parameters

// Number of reading of parameters from slave
#define MASTER_MAX_RETRY 30

// Timeout to update cid over Modbus
#define UPDATE_CIDS_TIMEOUT_MS (3000)
#define UPDATE_CIDS_TIMEOUT_TICS (UPDATE_CIDS_TIMEOUT_MS / portTICK_PERIOD_MS)

// Timeout between polls
#define POLL_TIMEOUT_MS (500)
#define POLL_TIMEOUT_TICS (POLL_TIMEOUT_MS / portTICK_PERIOD_MS)

// Timeout between erros
#define ERROR_TIMEOUT_MS (1000)
#define ERROR_TIMEOUT_TICS (ERROR_TIMEOUT_MS / portTICK_PERIOD_MS)

// 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 STR(fieldname) ((const char *)(fieldname))
// Options can be used as bit masks or parameter limits
#define OPTS(min_val, max_val, step_val) \
    {                                    \
        .opt1 = min_val, .opt2 = max_val, .opt3 = step_val}

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

// Enumeration of all supported CIDs for device (used in parameter definition table)
enum
{
    CID_HOLD_DATA_0 = 0,
    CID_HOLD_DATA_1 = 1,
    CID_HOLD_DATA_2 = 2,
    CID_HOLD_DATA_3 = 3,
    CID_HOLD_DATA_4 = 4,
    CID_HOLD_DATA_5 = 5,
    CID_HOLD_DATA_6 = 6
};

#define SN 0x1000
#define METERID 0x1003
#define FW 0x1004

#define L1VOLTAGE 0x0100
// #define L2VOLTAGE 0x0010
// #define L3VOLTAGE 0x0012

#define L1CURRENT 0x0102
// #define L2CURRENT 0x0018
// #define L3CURRENT 0x001A

#define ACTIVEPOWER 0x0104
#define APPARENTPOWER 0x0106
#define REACTIVEPOWER 0x0108

#define TOTALFACTIVE 0x010E
#define TOTALRACTIVE 0x0118

// 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_HOLD_DATA_0, STR("TOTALFACTIVE"), STR("kWh"), MB_DEVICE_ADDR1, MB_PARAM_HOLDING, TOTALFACTIVE, 2,
     HOLD_OFFSET(holding_data0), PARAM_TYPE_I32_CDAB, 4, OPTS(-100000, 100000, 1), PAR_PERMS_READ},

    {CID_HOLD_DATA_1, STR("TOTALRACTIVE"), STR("kWh"), MB_DEVICE_ADDR1, MB_PARAM_HOLDING, TOTALRACTIVE, 2,
     HOLD_OFFSET(holding_data1), PARAM_TYPE_I32_CDAB, 4, OPTS(-100000, 100000, 1), PAR_PERMS_READ},

    {CID_HOLD_DATA_2, STR("ACTIVEPOWER"), STR("W"), MB_DEVICE_ADDR1, MB_PARAM_HOLDING, ACTIVEPOWER, 2,
     HOLD_OFFSET(holding_data2), PARAM_TYPE_I32_CDAB, 4, OPTS(-100000, 100000, 1), PAR_PERMS_READ},

    {CID_HOLD_DATA_3, STR("APPARENTPOWER"), STR("W"), MB_DEVICE_ADDR1, MB_PARAM_HOLDING, APPARENTPOWER, 2,
     HOLD_OFFSET(holding_data3), PARAM_TYPE_I32_CDAB, 4, OPTS(-100000, 100000, 1), PAR_PERMS_READ},

    {CID_HOLD_DATA_4, STR("REACTIVEPOWER"), STR("W"), MB_DEVICE_ADDR1, MB_PARAM_HOLDING, REACTIVEPOWER, 2,
     HOLD_OFFSET(holding_data4), PARAM_TYPE_I32_CDAB, 4, OPTS(-100000, 100000, 1), PAR_PERMS_READ},

    {CID_HOLD_DATA_5, STR("L1CURRENT"), STR("A"), MB_DEVICE_ADDR1, MB_PARAM_HOLDING, L1CURRENT, 2,
     HOLD_OFFSET(holding_data5), PARAM_TYPE_I32_CDAB, 4, OPTS(-100000, 100000, 1), PAR_PERMS_READ},

    {CID_HOLD_DATA_6, STR("L1VOLTAGE"), STR("V"), MB_DEVICE_ADDR1, MB_PARAM_HOLDING, L1VOLTAGE, 2,
     HOLD_OFFSET(holding_data6), PARAM_TYPE_I32_CDAB, 4, OPTS(-100000, 100000, 1), PAR_PERMS_READ}
    /*

      {CID_HOLD_DATA_3, STR("L1VOLTAGE 4"), STR("V"), MB_DEVICE_ADDR1, MB_PARAM_HOLDING, L1VOLTAGE, 2,
       HOLD_OFFSET(holding_data3), PARAM_TYPE_I32_DCBA, 4, OPTS(-100000, 100000, 1), PAR_PERMS_READ},

      {CID_HOLD_DATA_4, STR("L1VOLTAGE 5"), STR("V"), MB_DEVICE_ADDR1, MB_PARAM_HOLDING, L1VOLTAGE, 2,
       HOLD_OFFSET(holding_data4), PARAM_TYPE_I32_CDAB, 4, OPTS(-100000, 100000, 1), PAR_PERMS_READ},

      {CID_HOLD_DATA_5, STR("L1CURRENT 2"), STR("A"), MB_DEVICE_ADDR1, MB_PARAM_HOLDING, L1CURRENT, 2,
       HOLD_OFFSET(holding_data5), PARAM_TYPE_FLOAT, 4, OPTS(-100000, 100000, 1), PAR_PERMS_READ}


             {CID_HOLD_DATA_2, STR("ID 2"), STR("ID 2"), MB_DEVICE_ADDR1, MB_PARAM_HOLDING, METERID, 2,
              HOLD_OFFSET(holding_data4), PARAM_TYPE_U8, 1, OPTS(0, 100000, 1), PAR_PERMS_READ},

             {CID_HOLD_DATA_3, STR("ID 3"), STR("FW 2"), MB_DEVICE_ADDR1, MB_PARAM_HOLDING, FW, 2,
              HOLD_OFFSET(holding_data5), PARAM_TYPE_U8, 1, OPTS(0, 100000, 1), PAR_PERMS_READ}


                {CID_HOLD_DATA_2, STR("ID 2"), STR("ID 2"), MB_DEVICE_ADDR1, MB_PARAM_HOLDING, METERID, 1,
                 HOLD_OFFSET(holding_data2), PARAM_TYPE_U16, 2, OPTS(0, 100000, 1), PAR_PERMS_READ},

                {CID_HOLD_DATA_3, STR("ID 3"), STR("FW 2"), MB_DEVICE_ADDR1, MB_PARAM_HOLDING, FW, 1,
                 HOLD_OFFSET(holding_data3), PARAM_TYPE_U16, 2, OPTS(0, 100000, 1), PAR_PERMS_READ},

                {CID_HOLD_DATA_4, STR("ID 4"), STR("ID 4"), MB_DEVICE_ADDR1, MB_PARAM_HOLDING, L1VOLTAGE, 2,
                 HOLD_OFFSET(holding_data4), PARAM_TYPE_U8, 1, OPTS(0, 100000, 1), PAR_PERMS_READ},

                {CID_HOLD_DATA_5, STR("ID 5"), STR("ID 5"), MB_DEVICE_ADDR1, MB_PARAM_HOLDING, L1VOLTAGE, 2,
                 HOLD_OFFSET(holding_data5), PARAM_TYPE_U16, 2, OPTS(0, 100000, 1), PAR_PERMS_READ},

                {CID_HOLD_DATA_6, STR("ID 6"), STR("ID 6"), MB_DEVICE_ADDR1, MB_PARAM_HOLDING, L1VOLTAGE, 2,
                 HOLD_OFFSET(holding_data6), PARAM_TYPE_U32, 4, OPTS(0, 100000, 1), PAR_PERMS_READ}




                {CID_HOLD_DATA_4, STR("ID 4"), STR("ID 3"), MB_DEVICE_ADDR1, MB_PARAM_HOLDING, METERID, 2,
                 HOLD_OFFSET(holding_data4), PARAM_TYPE_U8, 1, OPTS(0, 100000, 1), PAR_PERMS_READ},

                {CID_HOLD_DATA_5, STR("ID 5"), STR("FW 3"), MB_DEVICE_ADDR1, MB_PARAM_HOLDING, FW, 2,
                 HOLD_OFFSET(holding_data5), PARAM_TYPE_U8, 1, OPTS(0, 100000, 1), PAR_PERMS_READ},

                {CID_HOLD_DATA_6, STR("ID 6"), STR("ID 4"), MB_DEVICE_ADDR1, MB_PARAM_HOLDING, METERID, 2,
                 HOLD_OFFSET(holding_data6), PARAM_TYPE_U8, 2, OPTS(0, 100000, 1), PAR_PERMS_READ}


                {CID_HOLD_DATA_0, STR("L1"), STR("A"), MB_DEVICE_ADDR1, MB_PARAM_HOLDING, L1CURRENT, 2,
                HOLD_OFFSET(holding_data0), PARAM_TYPE_FLOAT, 4, OPTS(-1000, 1000, 0.1), PAR_PERMS_READ},


                {CID_HOLD_DATA_1, STR("L2"), STR("A"), MB_DEVICE_ADDR1, MB_PARAM_HOLDING, L2CURRENT, 2,
                HOLD_OFFSET(holding_data1), PARAM_TYPE_FLOAT, 4, OPTS(-1000, 1000, 0.1), PAR_PERMS_READ},

                {CID_HOLD_DATA_2, STR("L3"), STR("A"), MB_DEVICE_ADDR1, MB_PARAM_HOLDING, L3CURRENT, 2,
                HOLD_OFFSET(holding_data2), PARAM_TYPE_FLOAT, 4, OPTS(-1000, 1000, 0.1), PAR_PERMS_READ}
                */

};

// Calculate number of parameters in the table
const uint16_t num_device_parameters = (sizeof(device_parameters) / sizeof(device_parameters[0]));

// The function to get pointer to parameter storage (instance) according to parameter description table
static void *master_get_param_data(const mb_parameter_descriptor_t *param_descriptor)
{
    assert(param_descriptor != NULL);
    void *instance_ptr = NULL;
    if (param_descriptor->param_offset != 0)
    {
        switch (param_descriptor->mb_param_type)
        {
        case MB_PARAM_HOLDING:
            instance_ptr = ((void *)&holding_reg_params + param_descriptor->param_offset - 1);
            break;
        case MB_PARAM_INPUT:
            instance_ptr = ((void *)&input_reg_params + param_descriptor->param_offset - 1);
            break;
        case MB_PARAM_COIL:
            instance_ptr = ((void *)&coil_reg_params + param_descriptor->param_offset - 1);
            break;
        case MB_PARAM_DISCRETE:
            instance_ptr = ((void *)&discrete_reg_params + param_descriptor->param_offset - 1);
            break;
        default:
            instance_ptr = NULL;
            break;
        }
    }
    else
    {
        ESP_LOGE(TAG, "Wrong parameter offset for CID #%u", (unsigned)param_descriptor->cid);
        assert(instance_ptr != NULL);
    }
    return instance_ptr;
}

// Float - Mid-Little Endian (CDAB)
float ReverseFloat(const float inFloat)
{
    float retVal;
    char *floatToConvert = (char *)&inFloat;
    char *returnFloat = (char *)&retVal;

    // swap the bytes into a temporary buffer
    returnFloat[0] = floatToConvert[2];
    returnFloat[1] = floatToConvert[3];
    returnFloat[2] = floatToConvert[0];
    returnFloat[3] = floatToConvert[1];

    return retVal;
}

// Int - Mid-Little Endian (CDAB)
int ReverseInt(const int inFloat)
{
    int retVal;
    char *floatToConvert = (char *)&inFloat;
    char *returnFloat = (char *)&retVal;

    // swap the bytes into a temporary buffer
    returnFloat[0] = floatToConvert[2];
    returnFloat[1] = floatToConvert[3];
    returnFloat[2] = floatToConvert[0];
    returnFloat[3] = floatToConvert[1];

    return retVal;
}

static void serial_mdb_task_func(void *param)
{
    ESP_LOGI(TAG, "serial_mdb_task_func");
    esp_err_t err = ESP_OK;

    float maxcurrent = 0;
    float l1current = 0;
    float l2current = 0;
    float l3current = 0;
    int error_count = 0;

    bool alarm_state = false;
    const mb_parameter_descriptor_t *param_descriptor = NULL;

    ESP_LOGI(TAG, "Start modbus...");

    while (true)
    {

        // if ((evse_state_is_charging(evse_get_state()) && enabled) || (meterTest && enabled))
        {
            // Read all found characteristics from slave(s)
            for (uint16_t cid = 0; (err != ESP_ERR_NOT_FOUND) && cid < MASTER_MAX_CIDS; cid++)
            {
                // Get data from parameters description table
                // and use this information to fill the characteristics description table
                // and having all required fields in just one table
                err = mbc_master_get_cid_info(cid, &param_descriptor);
                if ((err != ESP_ERR_NOT_FOUND) && (param_descriptor != NULL))
                {
                    void *temp_data_ptr = master_get_param_data(param_descriptor);
                    uint8_t type = 0;

                    err = mbc_master_get_parameter(cid, (char *)param_descriptor->param_key,
                                                   (uint8_t *)temp_data_ptr, &type);

                    if (err == ESP_OK)
                    {

                        ESP_LOGI(TAG, "err == ESP_OK...");

                        error_count = 0;
                        meterState = true;

                        if ((param_descriptor->mb_param_type == MB_PARAM_HOLDING) ||
                            (param_descriptor->mb_param_type == MB_PARAM_INPUT))
                        {
                            int value = *(int *)temp_data_ptr;
                            // value = ReverseInt(value);

                            /*
                            switch (cid)
                            {
                            case 0:
                                // setMaxGridCurrent(grid_get_max_current() * 10);
                                maxcurrent = 0;
                                l1current = 0;
                                l2current = 0;
                                l3current = 0;

                                l1current = value;
                                break;
                            case 1:
                                l2current = value;
                                break;
                            case 2:
                                l3current = value;

                                maxcurrent = (l1current > l2current) ? l1current : l2current;
                                maxcurrent = (maxcurrent > l3current) ? maxcurrent : l3current;

                                // maxcurrent = (maxcurrent * 5) + 25;

                                // setLiveGridCurrent((int)maxcurrent * 10);
                                break;
                            default:
                                // code block
                            }*/

                            ESP_LOGI(TAG, "Characteristic #%u %s (%s) value = %d (0x%" PRIx32 ") read successful.",
                                     param_descriptor->cid,
                                     param_descriptor->param_key,
                                     param_descriptor->param_units,
                                     value,
                                     *(uint32_t *)temp_data_ptr);

                            if (((value > param_descriptor->param_opts.max) ||
                                 (value < param_descriptor->param_opts.min)))
                            {
                                alarm_state = true;
                                break;
                            }
                        }
                        else
                        {
                            uint8_t state = *(uint8_t *)temp_data_ptr;
                            const char *rw_str = (state & param_descriptor->param_opts.opt1) ? "ON" : "OFF";
                            if ((state & param_descriptor->param_opts.opt2) == param_descriptor->param_opts.opt2)
                            {
                                ESP_LOGI(TAG, "Characteristic 6 #%u %s (%s) value = %s (0x%" PRIx8 ") read successful.",
                                         param_descriptor->cid,
                                         param_descriptor->param_key,
                                         param_descriptor->param_units,
                                         (const char *)rw_str,
                                         *(uint8_t *)temp_data_ptr);
                            }
                            else
                            {
                                ESP_LOGE(TAG, "Characteristic 7 #%u %s (%s) value = %s (0x%" PRIx8 "), unexpected value.",
                                         param_descriptor->cid,
                                         param_descriptor->param_key,
                                         param_descriptor->param_units,
                                         (const char *)rw_str,
                                         *(uint8_t *)temp_data_ptr);
                                alarm_state = true;
                                break;
                            }
                            if (state & param_descriptor->param_opts.opt1)
                            {
                                alarm_state = true;
                                break;
                            }
                        }
                    }
                    else
                    {

                        if (error_count > 3 && !meterTest)
                        {
                            meterState = false;
                            vTaskDelay(ERROR_TIMEOUT_MS * error_count); // timeout between polls
                        }
                        else
                        {
                            error_count++;
                        }

                        ESP_LOGE(TAG, "Characteristic 8 #%u (%s) read fail, err = 0x%x (%s).",
                                 param_descriptor->cid,
                                 param_descriptor->param_key,
                                 (int)err,
                                 (char *)esp_err_to_name(err));
                    }

                    vTaskDelay(POLL_TIMEOUT_TICS); // timeout between polls
                }
            }
        }
        vTaskDelay(UPDATE_CIDS_TIMEOUT_TICS);
    }

    if (alarm_state)
    {
        ESP_LOGI(TAG, "Alarm triggered by cid #%u.", param_descriptor->cid);
    }
    else
    {
        ESP_LOGE(TAG, "Alarm is not triggered after %u retries.", MASTER_MAX_RETRY);
    }
    ESP_LOGI(TAG, "Destroy master...");
    ESP_ERROR_CHECK(mbc_master_destroy());

    /*
        while (true)
        {


            vTaskDelay(pdMS_TO_TICKS(1000));
        }
    */
}

// Modbus master initialization
static esp_err_t master_init(void)
{
    // Initialize and start Modbus controller
    mb_communication_info_t comm = {
        //.slave_addr = 1,
        .port = MB_PORT_NUM,
        .mode = MB_MODE_RTU,
        .baudrate = MB_DEV_SPEED,
        .parity = UART_PARITY_DISABLE};
    void *master_handler = NULL;

    esp_err_t err = mbc_master_init(MB_PORT_SERIAL_MASTER, &master_handler);
    MB_RETURN_ON_FALSE((master_handler != 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);
    err = mbc_master_setup((void *)&comm);
    MB_RETURN_ON_FALSE((err == ESP_OK), ESP_ERR_INVALID_STATE, TAG,
                       "mb controller setup fail, returns(0x%x).", (int)err);

    // Set UART pin numbers
    err = uart_set_pin(MB_PORT_NUM, MB_UART_TXD, MB_UART_RXD,
                       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();
    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(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(&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;
}

/**
 * @brief Set meter model
 *
 */
void serial_mdb_set_model(bool _enabled)
{
    enabled = _enabled;
}

/**
 * @brief Set meter state
 *
 */
bool serial_mdb_get_meter_state()
{
    return meterState;
}

/**
 * @brief Set meter state
 *
 */
void serial_mdb_set_meter_test(bool _meterTest)
{
    meterTest = _meterTest;
}

void serial_mdb_start()
{

    ESP_LOGI(TAG, "Starting MDB Serial");

    enabled = meter_get_model() != ENERGY_METER_NONE;

    ESP_ERROR_CHECK(master_init());

    xTaskCreate(serial_mdb_task_func, "serial_mdb_task", 4 * 1024, NULL, 5, &serial_mdb_task);
}

void serial_mdb_stop(void)
{
    ESP_LOGI(TAG, "Stopping");

    if (serial_mdb_task)
    {
        vTaskDelete(serial_mdb_task);
        serial_mdb_task = NULL;
    }

    // if (port != -1)
    //{
    uart_driver_delete(MB_PORT_NUM);
    //   port = -1;
    //}
}