#include "modbus_rtu_slave.h"
#include "modbus_register_map.h"
#include "driver/uart.h"
#include "driver/gpio.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/queue.h"
#include <string.h>
#include "parameter.h"
#include "esp_log.h"
#include "system_freertos_define.h"

#define MODBUS_UART_NUM      UART_NUM_0
#define MODBUS_UART_TXD      43
#define MODBUS_UART_RXD      44
#define MODBUS_UART_RTS      UART_PIN_NO_CHANGE
#define MODBUS_UART_CTS      UART_PIN_NO_CHANGE
#define MODBUS_RS485_REDE    6   // GPIO6用于RS485收发控制

#define MODBUS_BUF_SIZE      512  // 增加缓冲区大小以支持更多寄存器

// 最大支持的寄存器操作数量
#define MAX_READ_REGISTERS   125  // 03功能码最大读取寄存器数：(512-5)/2 = 125个
#define MAX_WRITE_REGISTERS  125  // 10功能码最大写入寄存器数：(512-9)/2 = 125个

static const char *TAG = "MODBUS_RTU";

static uint8_t modbus_rx_buf[MODBUS_BUF_SIZE];
static uint8_t modbus_tx_buf[MODBUS_BUF_SIZE];

// 双任务架构变量
static TaskHandle_t modbus_rx_task_handle = NULL;
static TaskHandle_t modbus_tx_task_handle = NULL;
static QueueHandle_t uart_modbus_queue = NULL;
static QueueHandle_t modbus_tx_queue = NULL;

// 发送缓冲结构
typedef struct {
    uint8_t send_buff_length;
    uint8_t send_buff[MODBUS_BUF_SIZE];
} modbus_send_buff_struct;

static modbus_send_buff_struct current_send_buff = {0};

// 辅助函数声明
static void modbus_rx_task(void *pvParameters);
static void modbus_tx_task(void *pvParameters);
static void modbus_send_response_async(const uint8_t *data, int len);
static void reset_send_buffer(void);
static void setup_intelligent_rs485_timing(void);

// 将参数值转换为实际波特率
static uint32_t get_baud_rate_from_param(void) {
    uint16_t baud_param = GetBaud_rate();
    switch (baud_param) {
        case 1: return 4800;
        case 2: return 9600;
        case 3: return 19200;
        case 4: return 38400;
        default: 
            ESP_LOGW(TAG, "无效的波特率参数 %d，使用默认值9600", baud_param);
            return 9600;
    }
}

// CRC16标准Modbus计算（完整版本）
static uint16_t modbus_crc16(const uint8_t *buf, uint16_t len) {
    static const uint16_t crc_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 crc = 0xFFFF;
    for (uint16_t i = 0; i < len; i++) {
        uint8_t index = (crc ^ buf[i]) & 0xFF;
        crc = (crc >> 8) ^ crc_table[index];
    }
    return crc;
}

// 重置发送缓冲区
static void reset_send_buffer(void) {
    current_send_buff.send_buff_length = 0;
    memset(current_send_buff.send_buff, 0, MODBUS_BUF_SIZE);
}

// 异步发送Modbus响应
static void modbus_send_response_async(const uint8_t *data, int len) {
    if (len > MODBUS_BUF_SIZE) {
        ESP_LOGE(TAG, "发送数据长度超过缓冲区大小");
        return;
    }
    
    // 准备发送数据
    current_send_buff.send_buff_length = len;
    memcpy(current_send_buff.send_buff, data, len);
    
    // 发送事件到TX任务
    system_drv_event_type_t tx_event = {
        .type_id = RS485_SEND_EVENT_ID,
        .en = 1
    };
    
    if (modbus_tx_queue != NULL) {
        xQueueSend(modbus_tx_queue, &tx_event, 0);
    }
}

// 智能RS485时序控制
static void setup_intelligent_rs485_timing(void) {
    uint32_t baud_rate = get_baud_rate_from_param();
    uint8_t wait_loops;
    
    // 根据波特率动态调整等待时间
    if (baud_rate >= 9600) {
        wait_loops = 10;  // 高波特率，短等待
    } else {
        wait_loops = 20;  // 低波特率，长等待
    }
    
    // 设置发送完成检测循环次数
    // 这个值会在TX任务中使用
}

// RS485收发控制
static void rs485_set_tx(void) {
    gpio_set_level(MODBUS_RS485_REDE, 1);
}
static void rs485_set_rx(void) {
    gpio_set_level(MODBUS_RS485_REDE, 0);
}

// 发送数据
static void modbus_uart_send(const uint8_t *data, int len) {
    rs485_set_tx();
    uart_write_bytes(MODBUS_UART_NUM, (const char *)data, len);
    uart_wait_tx_done(MODBUS_UART_NUM, pdMS_TO_TICKS(20));
    rs485_set_rx();
}

// 处理Modbus广播请求（仅支持写单个寄存器）
static void modbus_handle_broadcast_request(const uint8_t *req, int req_len) {
    if (req_len < (8- 2)) return; // 广播写单个寄存器最小长度：地址(1)+功能码(1)+寄存器地址(2)+数据(2)+CRC(2),CRC(2)未当做有效长度计算
    
    uint8_t slave_addr = req[0];
    uint8_t func_code  = req[1];
    uint16_t reg_addr  = (req[2] << 8) | req[3];
    
    // 只处理00广播地址且仅支持06功能码（写单个寄存器）
    if (slave_addr != 0x00 || func_code != 0x06) return;
    
    // 寄存器地址范围检查
    if (reg_addr >= rs485_register_count) {
        ESP_LOGW(TAG, "广播写入寄存器地址%d超出有效范围0~%d", reg_addr, rs485_register_count - 1);
        return;
    }
    
    int16_t val = (req[4] << 8) | req[5];
    uint16_t array_index = reg_addr; // 寄存器号直接对应数组索引
    
    ESP_LOGI(TAG, "收到广播写入命令：寄存器%d，值%d (0x%04X)", reg_addr, val, val & 0xFFFF);
    
    if (rs485_register_array[array_index].write_register_fun) {
        rs485_register_array[array_index].write_register_fun(val);
        ESP_LOGI(TAG, "广播写入成功：寄存器%d = %d", reg_addr, val);
    } else {
        ESP_LOGW(TAG, "寄存器%d无写入函数，广播写入失败", reg_addr);
    }
    
    // 广播不返回响应
}

// 处理Modbus请求
static void modbus_handle_request(const uint8_t *req, int req_len) {
    if (req_len < 4) return;
    uint8_t slave_addr = req[0];
    uint8_t func_code  = req[1];
    uint16_t reg_addr  = (req[2] << 8) | req[3];

    // 检查是否为广播地址
    if (slave_addr == 0x00) {
        printf("[MODBUS] Received broadcast request\r\n");
        printf("[MODBUS] req_len = %d\r\n", req_len);
        modbus_handle_broadcast_request(req, req_len);
        return;
    }

    // 动态从参数中获取地址（可根据需要修改）
    if (slave_addr != GetRS485_address()) return;

    uint8_t resp[MODBUS_BUF_SIZE] = {0};
    int resp_len = 0;

    if (func_code == 0x03) { // 读保持寄存器
        uint16_t reg_num = (req[4] << 8) | req[5];
        
        // 检查请求的寄存器数量是否超过最大限制
        if (reg_num > MAX_READ_REGISTERS) {
            ESP_LOGW(TAG, "请求读取寄存器数量%d超过最大限制%d", reg_num, MAX_READ_REGISTERS);
            return;
        }
        
        // 寄存器号从0开始，与数组索引直接对应
        if (reg_addr + reg_num > rs485_register_count) {
            ESP_LOGW(TAG, "请求读取寄存器范围%d~%d超出有效范围0~%d", 
                     reg_addr, reg_addr + reg_num - 1, rs485_register_count - 1);
            return;
        }
        resp[0] = slave_addr;
        resp[1] = func_code;
        resp[2] = reg_num * 2;
        for (uint16_t i = 0; i < reg_num; i++) {
            int16_t val = 0;
            uint16_t array_index = reg_addr + i; // 寄存器号直接对应数组索引
            // ESP_LOGI(TAG, "读取寄存器%d，数组索引%d", reg_addr + i, array_index);
            if (rs485_register_array[array_index].read_register_fun)
                val = rs485_register_array[array_index].read_register_fun();
            // ESP_LOGI(TAG, "寄存器%d返回值: %d (0x%04X)", reg_addr + i, val, val & 0xFFFF);
            resp[3 + i * 2] = (val >> 8) & 0xFF;
            resp[4 + i * 2] = val & 0xFF;
        }
        resp_len = 3 + reg_num * 2;
    } else if (func_code == 0x06) { // 写单个寄存器
        // 寄存器号从0开始，与数组索引直接对应
        if (reg_addr >= rs485_register_count) return;
        int16_t val = (req[4] << 8) | req[5];
        uint16_t array_index = reg_addr; // 寄存器号直接对应数组索引
        if (rs485_register_array[array_index].write_register_fun)
            rs485_register_array[array_index].write_register_fun(val);
        memcpy(resp, req, 6); // 回显
        resp_len = 6;
    } else if (func_code == 0x10) { // 写多个寄存器
        if (req_len < 9) return; // 最小数据包长度检查
        uint16_t reg_num = (req[4] << 8) | req[5]; // 寄存器数量
        uint8_t byte_count = req[6]; // 数据字节数
        
        // 检查请求的寄存器数量是否超过最大限制
        if (reg_num > MAX_WRITE_REGISTERS) {
            ESP_LOGW(TAG, "请求写入寄存器数量%d超过最大限制%d", reg_num, MAX_WRITE_REGISTERS);
            return;
        }
        
        // 数据包长度验证
        if (req_len < 7 + byte_count) return;
        if (byte_count != reg_num * 2) return; // 字节数应该等于寄存器数*2
        
        // 寄存器地址范围检查
        if (reg_addr + reg_num > rs485_register_count) {
            ESP_LOGW(TAG, "请求写入寄存器范围%d~%d超出有效范围0~%d", 
                     reg_addr, reg_addr + reg_num - 1, rs485_register_count - 1);
            return;
        }
        
        // 逐个写入寄存器
        for (uint16_t i = 0; i < reg_num; i++) {
            uint16_t array_index = reg_addr + i;
            if (rs485_register_array[array_index].write_register_fun) {
                int16_t val = (req[7 + i * 2] << 8) | req[7 + i * 2 + 1];
                ESP_LOGI(TAG, "写入寄存器%d，数组索引%d，值: %d (0x%04X)", 
                         reg_addr + i, array_index, val, val & 0xFFFF);
                rs485_register_array[array_index].write_register_fun(val);
            } else {
                ESP_LOGW(TAG, "寄存器%d无写入函数，跳过", reg_addr + i);
            }
        }
        
        // 构造响应 - 返回原始请求的前6个字节（地址、功能码、起始地址、寄存器数量）
        resp[0] = slave_addr;
        resp[1] = func_code;
        resp[2] = (reg_addr >> 8) & 0xFF;
        resp[3] = reg_addr & 0xFF;
        resp[4] = (reg_num >> 8) & 0xFF;
        resp[5] = reg_num & 0xFF;
        resp_len = 6;
    } else {
        // 不支持的功能码
        return;
    }
    // CRC
    uint16_t crc = modbus_crc16(resp, resp_len);
    resp[resp_len++] = crc & 0xFF;
    resp[resp_len++] = (crc >> 8) & 0xFF;
    
    // 使用异步发送替代同步发送
    modbus_send_response_async(resp, resp_len);
}

// 事件驱动的RX任务
static void modbus_rx_task(void *pvParameters) {
    uart_event_t event;
    ESP_LOGI(TAG, "Modbus RX任务已启动，监听UART%d，地址=%d", MODBUS_UART_NUM, GetRS485_address());
    
    while (1) {
        // 等待UART事件（事件驱动，无轮询延时）
        if (xQueueReceive(uart_modbus_queue, (void *)&event, (TickType_t)portMAX_DELAY)) {
            switch (event.type) {
                case UART_DATA: {
                    int len = uart_read_bytes(MODBUS_UART_NUM, modbus_rx_buf, MODBUS_BUF_SIZE, 
                                            pdMS_TO_TICKS(20));  // 短超时，因为数据已确认存在
                    
                    if (len >= 6) {  // Modbus最小帧长度
                        uint16_t crc_recv = (modbus_rx_buf[len-1] << 8) | modbus_rx_buf[len-2];
                        uint16_t crc_calc = modbus_crc16(modbus_rx_buf, len-2);
                        
                        if (crc_recv == crc_calc) {
                            // CRC校验通过，处理请求
                            modbus_handle_request(modbus_rx_buf, len-2);
                        } else {
                            ESP_LOGW(TAG, "CRC校验失败: 接收=0x%04X, 计算=0x%04X", crc_recv, crc_calc);
                        }
                    }
                    
                    // 清空接收缓冲区
                    memset(modbus_rx_buf, 0, MODBUS_BUF_SIZE);
                    break;
                }
                
                case UART_BUFFER_FULL: {
                    ESP_LOGW(TAG, "UART缓冲区满，清空并重置");
                    uart_flush_input(MODBUS_UART_NUM);
                    xQueueReset(uart_modbus_queue);
                    break;
                }
                
                case UART_FIFO_OVF: {
                    ESP_LOGW(TAG, "UART FIFO溢出，清空缓冲区");
                    uart_flush_input(MODBUS_UART_NUM);
                    break;
                }
                
                default:
                    break;
            }
        }
    }
}

// 智能TX任务（参考代码架构）
static void modbus_tx_task(void *pvParameters) {
    system_drv_event_type_t tx_event;
    uint32_t baud_rate = get_baud_rate_from_param();
    uint8_t wait_loops = (baud_rate >= 9600) ? 10 : 20;  // 智能等待时间
    
    ESP_LOGI(TAG, "Modbus TX任务已启动，波特率=%lu，等待循环=%d", 
             (unsigned long)baud_rate, wait_loops);
    
    while (1) {
        // 等待发送事件
        if (xQueueReceive(modbus_tx_queue, &tx_event, portMAX_DELAY) == pdTRUE) {
            if (tx_event.type_id == RS485_SEND_EVENT_ID && tx_event.en == 1) {
                
                // 切换到发送模式
                rs485_set_tx();
                vTaskDelay(pdMS_TO_TICKS(1));  // 短暂延时确保切换完成
                
                // 发送数据
                uart_write_bytes(MODBUS_UART_NUM, current_send_buff.send_buff, 
                               current_send_buff.send_buff_length);
                
                // 智能等待发送完成（参考代码的核心优化）
                bool tx_done = false;
                for (uint8_t i = 0; i < wait_loops; i++) {
                    if (uart_wait_tx_done(MODBUS_UART_NUM, pdMS_TO_TICKS(20)) == ESP_OK) {
                        tx_done = true;
                        break;
                    }
                    
                    // 如果是最后一次循环仍未完成，强制退出
                    if (i >= (wait_loops - 1)) {
                        ESP_LOGW(TAG, "发送完成等待超时");
                        break;
                    }
                }
                
                // 切换回接收模式
                rs485_set_rx();
                
                // 重置发送缓冲区
                reset_send_buffer();
                
                if (tx_done) {
                    // ESP_LOGD(TAG, "数据发送完成");
                } else {
                    ESP_LOGW(TAG, "发送完成检测超时");
                }
            }
        }
    }
}

// 兼容性：保留原有任务接口，内部调用RX任务
void modbus_rtu_slave_task(void *pvParameters) {
    modbus_rx_task(pvParameters);
}

// 初始化
void modbus_rtu_slave_init(void) {
    ESP_LOGI(TAG, "开始初始化Modbus RTU从机...");
    
    // 配置RS485收发控制引脚
    gpio_config_t io_conf = {
        .pin_bit_mask = 1ULL << MODBUS_RS485_REDE,
        .mode = GPIO_MODE_OUTPUT,
        .pull_up_en = 0,
        .pull_down_en = 0,
        .intr_type = GPIO_INTR_DISABLE
    };
    gpio_config(&io_conf);
    rs485_set_rx();
    ESP_LOGI(TAG, "GPIO%d 配置为RS485方向控制", MODBUS_RS485_REDE);

    // 配置UART
    uart_config_t uart_config = {
        .baud_rate = get_baud_rate_from_param(),
        .data_bits = UART_DATA_8_BITS,
        .parity    = UART_PARITY_DISABLE,
        .stop_bits = UART_STOP_BITS_1,
        .flow_ctrl = UART_HW_FLOWCTRL_DISABLE
    };
    // UART驱动安装将在后面与事件队列一起完成
    esp_err_t ret = uart_param_config(MODBUS_UART_NUM, &uart_config);
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "UART参数配置失败: %s", esp_err_to_name(ret));
        return;
    }
    
    ret = uart_set_pin(MODBUS_UART_NUM, MODBUS_UART_TXD, MODBUS_UART_RXD, MODBUS_UART_RTS, MODBUS_UART_CTS);
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "UART引脚设置失败: %s", esp_err_to_name(ret));
        return;
    }
    
    ESP_LOGI(TAG, "UART%d 配置完成：TXD=%d, RXD=%d, 波特率=%lu",
           MODBUS_UART_NUM, MODBUS_UART_TXD, MODBUS_UART_RXD, (unsigned long)get_baud_rate_from_param());

    // 创建UART事件队列（支持事件驱动）
    uart_driver_install(MODBUS_UART_NUM, MODBUS_BUF_SIZE * 2, 0, 20, &uart_modbus_queue, ESP_INTR_FLAG_SHARED);
    if (uart_modbus_queue == NULL) {
        ESP_LOGE(TAG, "UART事件队列创建失败");
        return;
    }

    // 创建TX任务事件队列
    modbus_tx_queue = xQueueCreate(10, sizeof(system_drv_event_type_t));
    if (modbus_tx_queue == NULL) {
        ESP_LOGE(TAG, "TX队列创建失败");
        return;
    }

    // 启动双任务架构
    // RX任务：最高优先级，处理接收
    BaseType_t rx_ret = xTaskCreate(modbus_rx_task, "modbus_rx_task", 4096, NULL, 
                                   RS485_TASK_RX_PRIORITY, &modbus_rx_task_handle);
    if (rx_ret != pdPASS) {
        ESP_LOGE(TAG, "Modbus RX任务创建失败");
        return;
    }

    // TX任务：次高优先级，处理发送
    BaseType_t tx_ret = xTaskCreate(modbus_tx_task, "modbus_tx_task", 2048, NULL, 
                                   RS485_TASK_TX_PRIORITY, &modbus_tx_task_handle);
    if (tx_ret != pdPASS) {
        ESP_LOGE(TAG, "Modbus TX任务创建失败");
        return;
    }

    ESP_LOGI(TAG, "Modbus双任务架构启动成功：RX优先级=%d, TX优先级=%d", 
            RS485_TASK_RX_PRIORITY, RS485_TASK_TX_PRIORITY);
}

// 动态重新配置UART波特率
void modbus_rtu_slave_reconfigure_baud_rate(void) {
    uint32_t new_baud_rate = get_baud_rate_from_param();
    
    ESP_LOGI(TAG, "开始重新配置UART波特率为%lu...", (unsigned long)new_baud_rate);
    
    // 停止当前UART
    uart_flush(MODBUS_UART_NUM);
    
    // 重新配置UART参数
    uart_config_t uart_config = {
        .baud_rate = new_baud_rate,
        .data_bits = UART_DATA_8_BITS,
        .parity    = UART_PARITY_DISABLE,
        .stop_bits = UART_STOP_BITS_1,
        .flow_ctrl = UART_HW_FLOWCTRL_DISABLE
    };
    
    esp_err_t ret = uart_param_config(MODBUS_UART_NUM, &uart_config);
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "UART波特率重新配置失败: %s", esp_err_to_name(ret));
        return;
    }
    
    ESP_LOGI(TAG, "UART%d 波特率已成功更新为%lu，立即生效", 
           MODBUS_UART_NUM, (unsigned long)new_baud_rate);
} 