#include <stdio.h>
#include <string.h>
#include "MODBUS_ESP.h"
#include "esp_log.h"
#include "RS-485-SP3485EEN.h"

#define TAG "MODBUS_ESP"

// CRC-16表（多项式0xA001）
static const uint16_t crc16_table[256] = {
    0x0000, 0xC0C1, 0xC181, 0x0140, 0xC301, 0x03C0, 0x0280, 0xC241,
    0xC601, 0x06C0, 0x0780, 0xC741, 0x0500, 0xC5C1, 0xC481, 0x0440,
    0xCC01, 0x0CC0, 0x0D80, 0xCD41, 0x0F00, 0xCFC1, 0xCE81, 0x0E40,
    0x0A00, 0xCAC1, 0xCB81, 0x0B40, 0xC901, 0x09C0, 0x0880, 0xC841,
    0xD801, 0x18C0, 0x1980, 0xD941, 0x1B00, 0xDBC1, 0xDA81, 0x1A40,
    0x1E00, 0xDEC1, 0xDF81, 0x1F40, 0xDD01, 0x1DC0, 0x1C80, 0xDC41,
    0x1400, 0xD4C1, 0xD581, 0x1540, 0xD701, 0x17C0, 0x1680, 0xD641,
    0xD201, 0x12C0, 0x1380, 0xD341, 0x1100, 0xD1C1, 0xD081, 0x1040,
    0xF001, 0x30C0, 0x3180, 0xF141, 0x3300, 0xF3C1, 0xF281, 0x3240,
    0x3600, 0xF6C1, 0xF781, 0x3740, 0xF501, 0x35C0, 0x3480, 0xF441,
    0x3C00, 0xFCC1, 0xFD81, 0x3D40, 0xFF01, 0x3FC0, 0x3E80, 0xFE41,
    0xFA01, 0x3AC0, 0x3B80, 0xFB41, 0x3900, 0xF9C1, 0xF881, 0x3840,
    0x2800, 0xE8C1, 0xE981, 0x2940, 0xEB01, 0x2BC0, 0x2A80, 0xEA41,
    0xEE01, 0x2EC0, 0x2F80, 0xEF41, 0x2D00, 0xEDC1, 0xEC81, 0x2C40,
    0xE401, 0x24C0, 0x2580, 0xE541, 0x2700, 0xE7C1, 0xE681, 0x2640,
    0x2200, 0xE2C1, 0xE381, 0x2340, 0xE101, 0x21C0, 0x2080, 0xE041,
    0xA001, 0x60C0, 0x6180, 0xA141, 0x6300, 0xA3C1, 0xA281, 0x6240,
    0x6600, 0xA6C1, 0xA781, 0x6740, 0xA501, 0x65C0, 0x6480, 0xA441,
    0x6C00, 0xACC1, 0xAD81, 0x6D40, 0xAF01, 0x6FC0, 0x6E80, 0xAE41,
    0xAA01, 0x6AC0, 0x6B80, 0xAB41, 0x6900, 0xA9C1, 0xA881, 0x6840,
    0x7800, 0xB8C1, 0xB981, 0x7940, 0xBB01, 0x7BC0, 0x7A80, 0xBA41,
    0xBE01, 0x7EC0, 0x7F80, 0xBF41, 0x7D00, 0xBDC1, 0xBC81, 0x7C40,
    0xB401, 0x74C0, 0x7580, 0xB541, 0x7700, 0xB7C1, 0xB681, 0x7640,
    0x7200, 0xB2C1, 0xB381, 0x7340, 0xB101, 0x71C0, 0x7080, 0xB041,
    0x5000, 0x90C1, 0x9181, 0x5140, 0x9301, 0x53C0, 0x5280, 0x9241,
    0x9601, 0x56C0, 0x5780, 0x9741, 0x5500, 0x95C1, 0x9481, 0x5440,
    0x9C01, 0x5CC0, 0x5D80, 0x9D41, 0x5F00, 0x9FC1, 0x9E81, 0x5E40,
    0x5A00, 0x9AC1, 0x9B81, 0x5B40, 0x9901, 0x59C0, 0x5880, 0x9841,
    0x8801, 0x48C0, 0x4980, 0x8941, 0x4B00, 0x8BC1, 0x8A81, 0x4A40,
    0x4E00, 0x8EC1, 0x8F81, 0x4F40, 0x8D01, 0x4DC0, 0x4C80, 0x8C41,
    0x4400, 0x84C1, 0x8581, 0x4540, 0x8701, 0x47C0, 0x4680, 0x8641,
    0x8201, 0x42C0, 0x4380, 0x8341, 0x4100, 0x81C1, 0x8081, 0x4040
};

uint16_t modbus_crc16(const uint8_t *data, size_t len)
{
    uint16_t crc = 0xFFFF;
    
    for (size_t i = 0; i < len; i++) {
        crc = (crc >> 8) ^ crc16_table[(crc ^ data[i]) & 0xFF];
    }
    
    return crc;
}

bool modbus_verify_crc(const uint8_t *data, size_t len)
{
    if (len < 3) {
        ESP_LOGE(TAG, "Frame too short for CRC verification");
        return false;
    }
    
    // 计算除最后2字节外的CRC
    uint16_t calculated_crc = modbus_crc16(data, len - 2);
    
    // 获取帧中的CRC（小端序）
    uint16_t frame_crc = ((uint16_t)data[len - 1] << 8) | data[len - 2];
    
    if (calculated_crc != frame_crc) {
        ESP_LOGE(TAG, "CRC mismatch: calculated=0x%04X, frame=0x%04X", calculated_crc, frame_crc);
        return false;
    }
    
    return true;
}

bool modbus_build_read_holding_req(uint8_t slave_addr, uint16_t start_addr, uint16_t reg_count,
                                   uint8_t *frame, size_t *frame_len)
{
    if (frame == NULL || frame_len == NULL) {
        ESP_LOGE(TAG, "Invalid parameters");
        return false;
    }
    
    // 参数检查
    if (reg_count == 0 || reg_count > 125) {
        ESP_LOGE(TAG, "Invalid register count: %d", reg_count);
        return false;
    }
    
    // 构建请求帧（8字节）
    frame[0] = slave_addr;                          // 从机地址
    frame[1] = MODBUS_FUNC_READ_HOLDING_REGISTERS;  // 功能码 0x03
    frame[2] = (start_addr >> 8) & 0xFF;            // 起始地址高字节
    frame[3] = start_addr & 0xFF;                   // 起始地址低字节
    frame[4] = (reg_count >> 8) & 0xFF;             // 寄存器数量高字节
    frame[5] = reg_count & 0xFF;                    // 寄存器数量低字节
    
    // 计算CRC
    uint16_t crc = modbus_crc16(frame, 6);
    frame[6] = crc & 0xFF;                          // CRC低字节
    frame[7] = (crc >> 8) & 0xFF;                   // CRC高字节
    
    *frame_len = 8;
    
    ESP_LOGI(TAG, "Built request: slave=%d, start_addr=0x%04X, reg_count=%d", 
             slave_addr, start_addr, reg_count);
    ESP_LOG_BUFFER_HEX(TAG, frame, 8);
    
    return true;
}

bool modbus_parse_response(const uint8_t *frame, size_t frame_len, modbus_response_t *response)
{
    if (frame == NULL || response == NULL) {
        ESP_LOGE(TAG, "Invalid parameters");
        return false;
    }
    
    // 初始化响应结构体
    memset(response, 0, sizeof(modbus_response_t));
    response->registers = NULL;
    
    // 验证CRC
    if (!modbus_verify_crc(frame, frame_len)) {
        ESP_LOGE(TAG, "CRC verification failed");
        return false;
    }
    
    // 解析基本字段
    response->slave_addr = frame[0];
    response->function_code = frame[1];
    
    ESP_LOGI(TAG, "Parsing response: slave=%d, func=0x%02X", response->slave_addr, response->function_code);
    
    // 检查是否为异常响应（功能码最高位置1）
    if (response->function_code & 0x80) {
        response->is_exception = true;
        response->exception_code = frame[2];
        ESP_LOGW(TAG, "Exception response: code=0x%02X", response->exception_code);
        return true;  // 异常响应也算解析成功
    }
    
    // 正常响应，仅处理功能码03
    if (response->function_code != MODBUS_FUNC_READ_HOLDING_REGISTERS) {
        ESP_LOGE(TAG, "Unsupported function code: 0x%02X", response->function_code);
        return false;
    }
    
    response->is_exception = false;
    
    // 检查最小长度（地址+功能码+字节数+CRC = 5字节）
    if (frame_len < 5) {
        ESP_LOGE(TAG, "Frame too short: %d", frame_len);
        return false;
    }
    
    // 获取字节数
    response->byte_count = frame[2];
    
    // 检查帧长度是否正确
    size_t expected_len = 3 + response->byte_count + 2;  // 3字节头部 + 数据 + 2字节CRC
    if (frame_len != expected_len) {
        ESP_LOGE(TAG, "Frame length mismatch: expected=%d, actual=%d", expected_len, frame_len);
        return false;
    }
    
    // 计算寄存器数量
    response->register_count = response->byte_count / 2;
    
    if (response->register_count == 0) {
        ESP_LOGW(TAG, "No registers in response");
        return true;
    }
    
    // 分配内存存储寄存器数据
    response->registers = malloc(response->register_count * sizeof(uint16_t));
    if (response->registers == NULL) {
        ESP_LOGE(TAG, "Failed to allocate memory for registers");
        return false;
    }
    
    // 解析寄存器数据（大端序）
    for (uint8_t i = 0; i < response->register_count; i++) {
        response->registers[i] = ((uint16_t)frame[3 + i * 2] << 8) | frame[3 + i * 2 + 1];
    }
    
    ESP_LOGI(TAG, "Parsed successfully: %d registers", response->register_count);
    
    return true;
}

void modbus_free_response(modbus_response_t *response)
{
    if (response != NULL && response->registers != NULL) {
        free(response->registers);
        response->registers = NULL;
    }
}

// ============================
// MODBUS轮询任务相关
// ============================

static TaskHandle_t poll_task_handle = NULL;
static modbus_poll_config_t current_config = {0};
static SemaphoreHandle_t config_mutex = NULL;

/**
 * @brief MODBUS轮询任务函数
 */
static void modbus_poll_task(void *arg)
{
    ESP_LOGI(TAG, "MODBUS poll task started");

    // 分配发送帧缓冲区
    uint8_t request_frame[8];
    size_t frame_len;

    while (1) {
        // 检查任务是否应该退出
        if (poll_task_handle == NULL) {
            break;
        }

        // 获取配置（互斥保护）
        xSemaphoreTake(config_mutex, portMAX_DELAY);
        bool enabled = current_config.enabled;
        int channel_num = current_config.channel_num;
        uint8_t slave_addr = current_config.slave_addr;
        uint16_t start_addr = current_config.start_addr;
        uint16_t reg_count = current_config.reg_count;
        uint32_t poll_interval_ms = current_config.poll_interval_ms;
        xSemaphoreGive(config_mutex);

        // 检查是否启用
        if (!enabled) {
            vTaskDelay(pdMS_TO_TICKS(100));
            continue;
        }

        // 验证通道号
        if (channel_num < 0 || channel_num >= NUM_CHANNELS) {
            ESP_LOGE(TAG, "Invalid channel number: %d", channel_num);
            vTaskDelay(pdMS_TO_TICKS(1000));
            continue;
        }

        // 构建MODBUS请求帧
        if (modbus_build_read_holding_req(slave_addr, start_addr, reg_count, request_frame, &frame_len)) {
            // 获取RS485通道
            rs485_channel_t *ch = &rs485_channels[channel_num];

            // 发送请求
            ESP_LOGI(TAG, "Polling: channel=%s, slave=%d, addr=0x%04X, count=%d",
                     ch->name, slave_addr, start_addr, reg_count);
            rs485_send(ch->uart_num, request_frame, frame_len);
        } else {
            ESP_LOGE(TAG, "Failed to build MODBUS request frame");
        }

        // 等待下一次轮询
        vTaskDelay(pdMS_TO_TICKS(poll_interval_ms));
    }

    ESP_LOGI(TAG, "MODBUS poll task exiting");
    poll_task_handle = NULL;
    vTaskDelete(NULL);
}

BaseType_t modbus_start_poll_task(modbus_poll_config_t *config, UBaseType_t priority, uint32_t stack_size)
{
    if (config == NULL) {
        ESP_LOGE(TAG, "Invalid config parameter");
        return pdFALSE;
    }

    // 参数验证
    if (config->channel_num < 0 || config->channel_num >= NUM_CHANNELS) {
        ESP_LOGE(TAG, "Invalid channel number: %d", config->channel_num);
        return pdFALSE;
    }

    if (config->reg_count == 0 || config->reg_count > 125) {
        ESP_LOGE(TAG, "Invalid register count: %d", config->reg_count);
        return pdFALSE;
    }

    if (config->poll_interval_ms < 100) {
        ESP_LOGE(TAG, "Poll interval too short (minimum 100ms): %d", config->poll_interval_ms);
        return pdFALSE;
    }

    // 如果任务已存在，先停止
    if (poll_task_handle != NULL) {
        modbus_stop_poll_task();
        vTaskDelay(pdMS_TO_TICKS(100));
    }

    // 创建互斥量
    if (config_mutex == NULL) {
        config_mutex = xSemaphoreCreateMutex();
        if (config_mutex == NULL) {
            ESP_LOGE(TAG, "Failed to create config mutex");
            return pdFALSE;
        }
    }

    // 保存配置
    xSemaphoreTake(config_mutex, portMAX_DELAY);
    memcpy(&current_config, config, sizeof(modbus_poll_config_t));
    current_config.enabled = true;
    xSemaphoreGive(config_mutex);

    // 创建轮询任务
    BaseType_t ret = xTaskCreate(modbus_poll_task, "modbus_poll", stack_size, NULL, priority, &poll_task_handle);

    if (ret == pdPASS) {
        ESP_LOGI(TAG, "MODBUS poll task started successfully");
        ESP_LOGI(TAG, "Config: channel=%d, slave=%d, start_addr=0x%04X, reg_count=%d, interval=%dms",
                 config->channel_num, config->slave_addr, config->start_addr,
                 config->reg_count, config->poll_interval_ms);
    } else {
        ESP_LOGE(TAG, "Failed to create MODBUS poll task");
    }

    return ret;
}

void modbus_stop_poll_task(void)
{
    if (poll_task_handle != NULL) {
        ESP_LOGI(TAG, "Stopping MODBUS poll task...");

        // 禁用轮询
        xSemaphoreTake(config_mutex, portMAX_DELAY);
        current_config.enabled = false;
        xSemaphoreGive(config_mutex);

        // 等待任务退出
        vTaskDelay(pdMS_TO_TICKS(200));

        // 删除任务
        TaskHandle_t temp_handle = poll_task_handle;
        poll_task_handle = NULL;
        vTaskDelete(temp_handle);

        ESP_LOGI(TAG, "MODBUS poll task stopped");
    }
}

bool modbus_update_poll_config(modbus_poll_config_t *config)
{
    if (config == NULL) {
        ESP_LOGE(TAG, "Invalid config parameter");
        return false;
    }

    if (config->channel_num < 0 || config->channel_num >= NUM_CHANNELS) {
        ESP_LOGE(TAG, "Invalid channel number: %d", config->channel_num);
        return false;
    }

    if (config->reg_count == 0 || config->reg_count > 125) {
        ESP_LOGE(TAG, "Invalid register count: %d", config->reg_count);
        return false;
    }

    if (config->poll_interval_ms < 100) {
        ESP_LOGE(TAG, "Poll interval too short (minimum 100ms): %d", config->poll_interval_ms);
        return false;
    }

    // 如果互斥量还没创建，直接更新配置
    if (config_mutex == NULL) {
        memcpy(&current_config, config, sizeof(modbus_poll_config_t));
    } else {
        // 互斥保护
        xSemaphoreTake(config_mutex, portMAX_DELAY);
        memcpy(&current_config, config, sizeof(modbus_poll_config_t));
        xSemaphoreGive(config_mutex);
    }

    ESP_LOGI(TAG, "MODBUS poll config updated: channel=%d, slave=%d, start_addr=0x%04X, reg_count=%d, interval=%dms, enabled=%d",
             config->channel_num, config->slave_addr, config->start_addr,
             config->reg_count, config->poll_interval_ms, config->enabled);

    // 如果轮询任务还没有启动，自动启动
    if (poll_task_handle == NULL && config->enabled) {
        ESP_LOGI(TAG, "Auto-starting MODBUS poll task...");

        // 先创建互斥量（如果还没创建）
        if (config_mutex == NULL) {
            config_mutex = xSemaphoreCreateMutex();
            if (config_mutex == NULL) {
                ESP_LOGE(TAG, "Failed to create config mutex");
                return false;
            }
        }

        // 启动轮询任务
        BaseType_t ret = xTaskCreate(modbus_poll_task, "modbus_poll", 4096, NULL, 5, &poll_task_handle);
        if (ret != pdPASS) {
            ESP_LOGE(TAG, "Failed to auto-start MODBUS poll task");
            return false;
        }
        ESP_LOGI(TAG, "MODBUS poll task auto-started");
    }

    return true;
}

modbus_poll_config_t* modbus_get_current_config(void)
{
    return &current_config;
}