// components/meter_manager/driver/meter_modbus/meter_dds665.c
// Driver Modbus RTU para DDS665.
// Suporta 1 ou 2 DDS665 no mesmo bus RS485:
//   GRID -> slave ID 1
//   EVSE -> slave ID 2
// Usa um único Modbus master e uma única task de leitura.
//
// Mapa DDS665 (FC=0x04, Input Registers, float32 IEEE-754):
//   0x0000 Voltage
//   0x0002 Current
//   0x0004 Active power (W)
//   0x0006 Power factor
//   0x0008 Total active energy (kWh)
//   0x000E Frequency (Hz)

#include "meter_dds665.h"

#include "modbus_params.h"
#include "mbcontroller.h"
#include "meter_events.h"

#include "esp_log.h"
#include "esp_timer.h"
#include "driver/uart.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/semphr.h"

#include <stddef.h>
#include <stdint.h>
#include <stdbool.h>
#include <string.h>
#include <math.h>

#define TAG "serial_mdb_dds665"

// ======= UART/Modbus config =======
#define MB_PORT_NUM 2
#define MB_DEV_SPEED 9600

// Ajustar conforme o hardware. GPIO2 para RTS/DE/RE deve ser evitado quando possível.
#define MB_UART_TXD 17
#define MB_UART_RXD 16
#define MB_UART_RTS 2

// ======= DDS665 no mesmo bus =======
#define DDS665_GRID_SLAVE_ID 1
#define DDS665_EVSE_SLAVE_ID 2

#define UPDATE_INTERVAL pdMS_TO_TICKS(3000)
#define POLL_INTERVAL pdMS_TO_TICKS(120)
#define DDS665_LOCK_TIMEOUT pdMS_TO_TICKS(10000)

/*
 * O DDS665 transmite cada float como dois registos Modbus:
 * exemplo 43 62 66 66 -> aproximadamente 226,4 V.
 *
 * Nesta versão do esp-modbus, PARAM_TYPE_FLOAT_CDAB é a transformação
 * que converte corretamente essa ordem para o float nativo do ESP32.
 * Validado em hardware com DDS665, 9600 8N1.
 */
#ifndef DDS665_FLOAT_PARAM_TYPE
#define DDS665_FLOAT_PARAM_TYPE PARAM_TYPE_FLOAT_CDAB
#endif

#define HOLD_OFFSET(field) ((uint16_t)(offsetof(holding_reg_params_t, field) + 1U))
#define OPTS(minimum, maximum, step_value) \
    {.opt1 = (minimum), .opt2 = (maximum), .opt3 = (step_value)}

// ======= Estado =======
static bool is_initialized = false;
static volatile bool task_should_run = false;
static TaskHandle_t meter_task = NULL;
static SemaphoreHandle_t dds665_lock = NULL;

// ======= CIDs locais por meter =======
enum
{
    CID_VOLTAGE = 0,
    CID_CURRENT,
    CID_ACTIVE_POWER_W,
    CID_POWER_FACTOR,
    CID_FREQUENCY,
    CID_TOTAL_ACTIVE_ENERGY_KWH,
    CID_COUNT
};

#define DDS665_REQUIRED_MASK \
    ((1UL << CID_VOLTAGE) |  \
     (1UL << CID_CURRENT) |  \
     (1UL << CID_ACTIVE_POWER_W))

typedef enum
{
    DDS665_SLOT_GRID = 0,
    DDS665_SLOT_EVSE,
    DDS665_MAX_INSTANCES
} dds665_slot_t;

typedef struct
{
    bool registered;
    const char *source; // "GRID" ou "EVSE"
    uint8_t slave_id;   // 1 ou 2
    uint16_t cid_base;  // atribuído em dds665_rebuild_descriptors_locked()
} dds665_instance_t;

static dds665_instance_t dds665_instances[DDS665_MAX_INSTANCES] = {
    [DDS665_SLOT_GRID] = {
        .registered = false,
        .source = "GRID",
        .slave_id = DDS665_GRID_SLAVE_ID,
        .cid_base = 0,
    },
    [DDS665_SLOT_EVSE] = {
        .registered = false,
        .source = "EVSE",
        .slave_id = DDS665_EVSE_SLAVE_ID,
        .cid_base = 0,
    },
};

// ======= Mapa de registradores DDS665 (Input Registers; FC=0x04) =======
#define REG_VOLTAGE 0x0000U        // V, float32
#define REG_CURRENT 0x0002U        // A, float32
#define REG_ACTIVE_POWER_W 0x0004U // W, float32
#define REG_POWER_FACTOR 0x0006U   // cos(phi), float32
#define REG_E_ACTIVE_KWH 0x0008U   // kWh, float32
#define REG_FREQUENCY 0x000EU      // Hz, float32

// ======= Template de parâmetros para um DDS665 =======
static const mb_parameter_descriptor_t dds665_param_template[CID_COUNT] = {
    {CID_VOLTAGE, "Voltage", "V", 1,
     MB_PARAM_INPUT, REG_VOLTAGE, 2, HOLD_OFFSET(l1_voltage),
     DDS665_FLOAT_PARAM_TYPE, 4, OPTS(0, 300, 0.1), PAR_PERMS_READ},

    {CID_CURRENT, "Current", "A", 1,
     MB_PARAM_INPUT, REG_CURRENT, 2, HOLD_OFFSET(l1_current),
     DDS665_FLOAT_PARAM_TYPE, 4, OPTS(0, 100, 0.1), PAR_PERMS_READ},

    {CID_ACTIVE_POWER_W, "Active Power", "W", 1,
     MB_PARAM_INPUT, REG_ACTIVE_POWER_W, 2, HOLD_OFFSET(active_power),
     DDS665_FLOAT_PARAM_TYPE, 4,
     OPTS(-100000, 100000, 1),
     PAR_PERMS_READ},

    {CID_POWER_FACTOR, "Power Factor", "", 1,
     MB_PARAM_INPUT, REG_POWER_FACTOR, 2, HOLD_OFFSET(power_factor),
     DDS665_FLOAT_PARAM_TYPE, 4, OPTS(-1, 1, 0.001), PAR_PERMS_READ},

    {CID_FREQUENCY, "Frequency", "Hz", 1,
     MB_PARAM_INPUT, REG_FREQUENCY, 2, HOLD_OFFSET(frequency),
     DDS665_FLOAT_PARAM_TYPE, 4, OPTS(0, 100, 0.1), PAR_PERMS_READ},

    {CID_TOTAL_ACTIVE_ENERGY_KWH, "Total Active Energy", "kWh", 1,
     MB_PARAM_INPUT, REG_E_ACTIVE_KWH, 2, HOLD_OFFSET(active_energy),
     DDS665_FLOAT_PARAM_TYPE, 4, OPTS(0, 1000000, 0.01), PAR_PERMS_READ},
};

// O esp-modbus exige cid e param_key únicos no Data Dictionary.
static const char *const dds665_param_keys[DDS665_MAX_INSTANCES][CID_COUNT] = {
    [DDS665_SLOT_GRID] = {
        "GRID Voltage",
        "GRID Current",
        "GRID Active Power",
        "GRID Power Factor",
        "GRID Frequency",
        "GRID Total Active Energy",
    },
    [DDS665_SLOT_EVSE] = {
        "EVSE Voltage",
        "EVSE Current",
        "EVSE Active Power",
        "EVSE Power Factor",
        "EVSE Frequency",
        "EVSE Total Active Energy",
    },
};

static mb_parameter_descriptor_t device_parameters_dds665[DDS665_MAX_INSTANCES * CID_COUNT];
static uint16_t num_device_parameters_dds665 = 0;
static bool dds665_descriptor_dirty = true;

// ======= Ponteiro para buffer destino =======
static void *get_param_ptr(const mb_parameter_descriptor_t *param)
{
    if (!param || param->param_offset == 0)
        return NULL;

    return ((uint8_t *)&holding_reg_params + param->param_offset - 1U);
}

static bool dds665_has_registered_instance_locked(void)
{
    for (uint8_t i = 0; i < DDS665_MAX_INSTANCES; ++i)
    {
        if (dds665_instances[i].registered)
            return true;
    }

    return false;
}

static esp_err_t dds665_rebuild_descriptors_locked(void)
{
    num_device_parameters_dds665 = 0;
    dds665_descriptor_dirty = true;

    for (uint8_t inst_idx = 0; inst_idx < DDS665_MAX_INSTANCES; ++inst_idx)
    {
        dds665_instance_t *inst = &dds665_instances[inst_idx];
        if (!inst->registered)
            continue;

        // Mantém os CIDs contíguos no descriptor ativo.
        inst->cid_base = num_device_parameters_dds665;

        for (uint16_t local_cid = 0; local_cid < CID_COUNT; ++local_cid)
        {
            mb_parameter_descriptor_t *dst =
                &device_parameters_dds665[num_device_parameters_dds665++];

            *dst = dds665_param_template[local_cid];
            dst->cid = inst->cid_base + local_cid;
            dst->param_key = dds665_param_keys[inst_idx][local_cid];
            dst->mb_slave_addr = inst->slave_id;
        }
    }

    ESP_LOGI(TAG, "DDS665 descriptor table prepared: %u parameters",
             (unsigned)num_device_parameters_dds665);
    return ESP_OK;
}

static esp_err_t dds665_apply_descriptors_locked(void)
{
    if (num_device_parameters_dds665 == 0)
        return ESP_ERR_INVALID_STATE;

    if (!dds665_descriptor_dirty)
        return ESP_OK;

    esp_err_t err = mbc_master_set_descriptor(device_parameters_dds665,
                                              num_device_parameters_dds665);
    if (err != ESP_OK)
    {
        ESP_LOGE(TAG, "mbc_master_set_descriptor failed: %s",
                 esp_err_to_name(err));
        return err;
    }

    dds665_descriptor_dirty = false;
    ESP_LOGI(TAG, "DDS665 descriptor table applied: %u parameters",
             (unsigned)num_device_parameters_dds665);
    return ESP_OK;
}

static esp_err_t dds665_master_init_once(void)
{
    if (is_initialized)
        return ESP_OK;

    if (!dds665_lock)
    {
        dds665_lock = xSemaphoreCreateMutex();
        if (!dds665_lock)
            return ESP_ERR_NO_MEM;
    }

    ESP_LOGI(TAG, "meter_dds665 Modbus master init");

    mb_communication_info_t comm = {
        .port = MB_PORT_NUM,
        .mode = MB_MODE_RTU,
        .baudrate = MB_DEV_SPEED,
        .parity = UART_PARITY_DISABLE, // DDS665 testado: 9600 8N1
    };

    void *handler = NULL;
    esp_err_t err = mbc_master_init(MB_PORT_SERIAL_MASTER, &handler);
    if (err != ESP_OK)
        return err;

    err = mbc_master_setup(&comm);
    if (err != ESP_OK)
    {
        (void)mbc_master_destroy();
        return err;
    }

    err = uart_set_pin(MB_PORT_NUM, MB_UART_TXD, MB_UART_RXD,
                       MB_UART_RTS, UART_PIN_NO_CHANGE);
    if (err != ESP_OK)
    {
        (void)mbc_master_destroy();
        return err;
    }

    err = uart_set_word_length(MB_PORT_NUM, UART_DATA_8_BITS);
    if (err != ESP_OK)
    {
        (void)mbc_master_destroy();
        return err;
    }

    err = uart_set_hw_flow_ctrl(MB_PORT_NUM, UART_HW_FLOWCTRL_DISABLE, 0);
    if (err != ESP_OK)
    {
        (void)mbc_master_destroy();
        return err;
    }

    err = uart_set_stop_bits(MB_PORT_NUM, UART_STOP_BITS_1);
    if (err != ESP_OK)
    {
        (void)mbc_master_destroy();
        return err;
    }

    // O controlador Modbus deve arrancar antes da mudança para half-duplex.
    err = mbc_master_start();
    if (err != ESP_OK)
    {
        (void)mbc_master_destroy();
        return err;
    }

    err = uart_set_mode(MB_PORT_NUM, UART_MODE_RS485_HALF_DUPLEX);
    if (err != ESP_OK)
    {
        (void)mbc_master_destroy();
        return err;
    }

    esp_log_level_set("MB_CONTROLLER_MASTER", ESP_LOG_DEBUG);
    esp_log_level_set("MB_PORT_COMMON", ESP_LOG_DEBUG);
    esp_log_level_set("MB_SERIAL_MASTER", ESP_LOG_DEBUG);

    vTaskDelay(pdMS_TO_TICKS(5));

    is_initialized = true;
    return ESP_OK;
}

static esp_err_t dds665_register_instance(dds665_slot_t slot)
{
    if (slot >= DDS665_MAX_INSTANCES)
        return ESP_ERR_INVALID_ARG;

    esp_err_t err = dds665_master_init_once();
    if (err != ESP_OK)
        return err;

    if (xSemaphoreTake(dds665_lock, DDS665_LOCK_TIMEOUT) != pdTRUE)
        return ESP_ERR_TIMEOUT;

    if (dds665_instances[slot].registered)
    {
        xSemaphoreGive(dds665_lock);
        return ESP_OK;
    }

    dds665_instances[slot].registered = true;
    ESP_LOGI(TAG, "DDS665 %s registered on Modbus slave ID %u",
             dds665_instances[slot].source,
             (unsigned)dds665_instances[slot].slave_id);

    err = dds665_rebuild_descriptors_locked();
    xSemaphoreGive(dds665_lock);

    return err;
}

static void dds665_shutdown_if_idle(void)
{
    bool any_registered = false;

    if (dds665_lock &&
        xSemaphoreTake(dds665_lock, DDS665_LOCK_TIMEOUT) == pdTRUE)
    {
        any_registered = dds665_has_registered_instance_locked();
        xSemaphoreGive(dds665_lock);
    }

    if (any_registered || !is_initialized)
        return;

    ESP_LOGI(TAG, "No DDS665 instances registered; stopping shared Modbus master");

    task_should_run = false;
    for (int i = 0; i < 20 && meter_task != NULL; ++i)
        vTaskDelay(pdMS_TO_TICKS(10));

    if (meter_task != NULL)
    {
        ESP_LOGW(TAG, "DDS665 task did not exit in time; deleting it");
        vTaskDelete(meter_task);
        meter_task = NULL;
    }

    esp_err_t err = mbc_master_destroy();
    if (err != ESP_OK)
    {
        ESP_LOGW(TAG, "mbc_master_destroy() returned %s",
                 esp_err_to_name(err));
    }

    if (uart_is_driver_installed(MB_PORT_NUM))
        uart_driver_delete(MB_PORT_NUM);

    is_initialized = false;
    num_device_parameters_dds665 = 0;
    dds665_descriptor_dirty = true;
}

static void dds665_unregister_instance(dds665_slot_t slot)
{
    if (slot >= DDS665_MAX_INSTANCES)
        return;

    if (!is_initialized || !dds665_lock)
        return;

    if (xSemaphoreTake(dds665_lock, DDS665_LOCK_TIMEOUT) == pdTRUE)
    {
        if (dds665_instances[slot].registered)
        {
            ESP_LOGI(TAG, "DDS665 %s unregistered",
                     dds665_instances[slot].source);
            dds665_instances[slot].registered = false;

            (void)dds665_rebuild_descriptors_locked();

            // Se a outra instância continuar ativa, reaplica imediatamente
            // a tabela compactada para manter cid_base e slave ID coerentes.
            if (dds665_has_registered_instance_locked())
            {
                esp_err_t err = dds665_apply_descriptors_locked();
                if (err != ESP_OK)
                {
                    ESP_LOGE(TAG, "Failed to reapply DDS665 descriptors: %s",
                             esp_err_to_name(err));
                }
            }
        }

        xSemaphoreGive(dds665_lock);
    }

    dds665_shutdown_if_idle();
}

static bool dds665_is_valid_value(uint16_t local_cid, float value)
{
    if (!isfinite(value))
        return false;

    switch (local_cid)
    {
    case CID_VOLTAGE:
        return value >= 0.0f && value <= 300.0f;
    case CID_CURRENT:
        return value >= 0.0f && value <= 100.0f;
    case CID_ACTIVE_POWER_W:
        return value >= -100000.0f && value <= 100000.0f;
    case CID_POWER_FACTOR:
        return value >= -1.1f && value <= 1.1f;
    case CID_FREQUENCY:
        return value >= 0.0f && value <= 100.0f;
    case CID_TOTAL_ACTIVE_ENERGY_KWH:
        return value >= 0.0f && value <= 1000000.0f;
    default:
        return false;
    }
}

static void dds665_read_instance_locked(const dds665_instance_t *inst)
{
    esp_err_t err;
    const mb_parameter_descriptor_t *desc = NULL;

    float voltage[3] = {0.0f, 0.0f, 0.0f};
    float current[3] = {0.0f, 0.0f, 0.0f};
    int32_t watt[3] = {0, 0, 0};

    float pf = 0.0f;
    float hz = 0.0f;
    float e_kwh = 0.0f;

    uint32_t valid_mask = 0U;

    for (uint16_t local_cid = 0; local_cid < CID_COUNT; ++local_cid)
    {
        const uint16_t cid = inst->cid_base + local_cid;

        err = mbc_master_get_cid_info(cid, &desc);
        if (err != ESP_OK || !desc)
        {
            ESP_LOGE(TAG, "%s get_cid_info(%u) failed: %s",
                     inst->source, (unsigned)cid, esp_err_to_name(err));
            continue;
        }

        void *data_ptr = get_param_ptr(desc);
        if (!data_ptr)
        {
            ESP_LOGE(TAG, "%s CID %u (%s): null data_ptr",
                     inst->source, (unsigned)cid, desc->param_key);
            continue;
        }

        uint8_t type = 0;
        err = mbc_master_get_parameter(cid, (char *)desc->param_key,
                                       (uint8_t *)data_ptr, &type);
        if (err != ESP_OK)
        {
            ESP_LOGE(TAG, "%s CID %u (%s) read failed: %s",
                     inst->source, (unsigned)cid, desc->param_key,
                     esp_err_to_name(err));
            vTaskDelay(POLL_INTERVAL);
            continue;
        }

        uint8_t raw[sizeof(float)];
        memcpy(raw, data_ptr, sizeof(raw));
        ESP_LOGD(TAG,
                 "%s CID %u (%s) decoded bytes: %02X %02X %02X %02X",
                 inst->source, (unsigned)cid, desc->param_key,
                 raw[0], raw[1], raw[2], raw[3]);

        const float value = *(const float *)data_ptr;
        if (!dds665_is_valid_value(local_cid, value))
        {
            ESP_LOGW(TAG, "%s %s invalid value: %.6f",
                     inst->source, desc->param_key, (double)value);
            vTaskDelay(POLL_INTERVAL);
            continue;
        }

        ESP_LOGD(TAG, "%s %s: %.3f %s",
                 inst->source, desc->param_key, (double)value,
                 desc->param_units);

        valid_mask |= (1UL << local_cid);

        switch (local_cid)
        {
        case CID_VOLTAGE:
            voltage[0] = value;
            break;

        case CID_CURRENT:
            current[0] = value;
            break;

        case CID_ACTIVE_POWER_W:
            // Este DDS665 devolve potência ativa diretamente em watts.
            watt[0] = (int32_t)lrintf(value);
            break;

        case CID_POWER_FACTOR:
            pf = value;
            break;

        case CID_FREQUENCY:
            hz = value;
            break;

        case CID_TOTAL_ACTIVE_ENERGY_KWH:
            e_kwh = value;
            break;

        default:
            break;
        }

        vTaskDelay(POLL_INTERVAL);
    }

    if ((valid_mask & DDS665_REQUIRED_MASK) != DDS665_REQUIRED_MASK)
    {
        ESP_LOGW(TAG,
                 "%s incomplete DDS665 sample, mask=0x%02lX",
                 inst->source,
                 (unsigned long)valid_mask);
        return;
    }

    meter_event_data_t evt = {
        .source = inst->source,
        .frequency = hz,
        .power_factor = pf,
        .total_energy = e_kwh,
        .watt_total = watt[0],
        .timestamp_us = esp_timer_get_time(),
    };

    memcpy(evt.vrms, voltage, sizeof(evt.vrms));
    memcpy(evt.irms, current, sizeof(evt.irms));
    memcpy(evt.watt, watt, sizeof(evt.watt));

    ESP_LOGI(TAG,
             "%s DDS665 event: V=%.1fV I=%.3fA P=%ldW E=%.3fkWh Hz=%.2f PF=%.3f mask=0x%02lX",
             inst->source,
             (double)voltage[0],
             (double)current[0],
             (long)evt.watt_total,
             (double)e_kwh,
             (double)hz,
             (double)pf,
             (unsigned long)valid_mask);

    esp_err_t post_err = esp_event_post(METER_EVENT,
                                        METER_EVENT_DATA_READY,
                                        &evt,
                                        sizeof(evt),
                                        portMAX_DELAY);
    if (post_err != ESP_OK)
    {
        ESP_LOGW(TAG, "%s failed to post meter event: %s",
                 inst->source, esp_err_to_name(post_err));
    }
}

// ======= Tarefa de aquisição =======
static void serial_mdb_task(void *param)
{
    (void)param;

    while (task_should_run)
    {
        if (!is_initialized || !dds665_lock)
        {
            vTaskDelay(UPDATE_INTERVAL);
            continue;
        }

        bool had_instance = false;

        for (uint8_t slot = 0;
             slot < DDS665_MAX_INSTANCES && task_should_run;
             ++slot)
        {
            if (xSemaphoreTake(dds665_lock, DDS665_LOCK_TIMEOUT) != pdTRUE)
            {
                ESP_LOGW(TAG, "DDS665 task timeout waiting lock");
                continue;
            }

            dds665_instance_t inst = dds665_instances[slot];
            if (inst.registered)
            {
                had_instance = true;
                dds665_read_instance_locked(&inst);
            }

            xSemaphoreGive(dds665_lock);
        }

        vTaskDelay(had_instance ? UPDATE_INTERVAL : pdMS_TO_TICKS(500));
    }

    ESP_LOGI(TAG, "DDS665 task stopped");
    meter_task = NULL;
    vTaskDelete(NULL);
}

// ======= API pública =======
// Compatibilidade: chamada antiga inicializa DDS665 como GRID/ID 1.
esp_err_t meter_dds665_init(void)
{
    return meter_dds665_grid_init();
}

esp_err_t meter_dds665_grid_init(void)
{
    return dds665_register_instance(DDS665_SLOT_GRID);
}

esp_err_t meter_dds665_evse_init(void)
{
    return dds665_register_instance(DDS665_SLOT_EVSE);
}

esp_err_t meter_dds665_start(void)
{
    if (!is_initialized)
    {
        ESP_LOGE(TAG, "meter_dds665 not initialized");
        return ESP_ERR_INVALID_STATE;
    }

    if (!dds665_lock)
        return ESP_ERR_INVALID_STATE;

    if (xSemaphoreTake(dds665_lock, DDS665_LOCK_TIMEOUT) != pdTRUE)
        return ESP_ERR_TIMEOUT;

    const bool has_instance = dds665_has_registered_instance_locked();
    esp_err_t desc_err = ESP_OK;
    if (has_instance)
        desc_err = dds665_apply_descriptors_locked();

    xSemaphoreGive(dds665_lock);

    if (!has_instance)
    {
        ESP_LOGW(TAG, "meter_dds665 start ignored: no instances registered");
        return ESP_ERR_INVALID_STATE;
    }

    if (desc_err != ESP_OK)
        return desc_err;

    if (meter_task == NULL)
    {
        task_should_run = true;
        BaseType_t ok = xTaskCreate(serial_mdb_task,
                                    "meter_dds665_task",
                                    4096,
                                    NULL,
                                    3,
                                    &meter_task);
        if (ok != pdPASS)
        {
            task_should_run = false;
            meter_task = NULL;
            return ESP_ERR_NO_MEM;
        }

        ESP_LOGI(TAG, "meter_dds665 shared task started");
    }

    return ESP_OK;
}

esp_err_t meter_dds665_grid_start(void)
{
    return meter_dds665_start();
}

esp_err_t meter_dds665_evse_start(void)
{
    return meter_dds665_start();
}

void meter_dds665_stop(void)
{
    meter_dds665_grid_stop();
}

void meter_dds665_grid_stop(void)
{
    dds665_unregister_instance(DDS665_SLOT_GRID);
}

void meter_dds665_evse_stop(void)
{
    dds665_unregister_instance(DDS665_SLOT_EVSE);
}