// #include "uart_handler.h"
// #include "pca9685.h"
// #include "driver/uart.h"
// #include "esp_log.h"
// #include "freertos/FreeRTOS.h"
// #include "freertos/task.h"
// #include "freertos/queue.h"
// #include <string.h>
// #include <math.h>

// #define UART_BUF_SIZE 1024
// #define TAG "UART_HANDLER"
// #define MAX_SERVOS 32
// #define MAX_DEVICES 2

// // 设备配置 (在main.c中初始化)
// extern PCA9685_Device devices[MAX_DEVICES];
// // 当前角度状态
// static ServoAngle current_angles[MAX_SERVOS];
// static int angle_count = 0;
// // 舵机控制队列
// static QueueHandle_t servo_queue = NULL;

// // 获取当前角度
// static float get_angle(uint8_t addr, uint8_t channel) {
//     for(int i = 0; i < angle_count; i++) {
//         if(current_angles[i].addr == addr && 
//            current_angles[i].channel == channel) {
//             return current_angles[i].angle;
//         }
//     }
//     // 不存在则添加默认值
//     if(angle_count < MAX_SERVOS) {
//         current_angles[angle_count] = (ServoAngle){addr, channel, 90.0f};
//         return current_angles[angle_count++].angle;
//     }
//     return 90.0f; // 安全默认值
// }

// // 更新角度
// static void update_angle(uint8_t addr, uint8_t channel, float angle) {
//     for(int i = 0; i < angle_count; i++) {
//         if(current_angles[i].addr == addr && 
//            current_angles[i].channel == channel) {
//             current_angles[i].angle = angle;
//             return;
//         }
//     }
//     // 不存在则添加
//     if(angle_count < MAX_SERVOS) {
//         current_angles[angle_count++] = (ServoAngle){addr, channel, angle};
//     }
// }

// // 线性缓动执行
// static void move_servo_linear(uint8_t addr, uint8_t channel, float from, float to, uint16_t duration_ms) {
//     int steps = duration_ms / 20;
//     steps = steps < 1 ? 1 : steps;
    
//     for (int i = 0; i <= steps; ++i) {
//         float t = (float)i / steps;
//         float angle = from + (to - from) * t;
        
//         // 查找设备
//         for (int d = 0; d < MAX_DEVICES; d++) {
//             if (devices[d].addr == addr) {
//                 pca9685_set_angle(devices[d].port, addr, channel, angle);
//                 break;
//             }
//         }
//         vTaskDelay(20 / portTICK_PERIOD_MS);
//     }
//     update_angle(addr, channel, to);
// }

// // 舵机控制任务
// static void servo_control_task(void *param) {
//     ServoCommand cmd;
    
//     while (1) {
//         if (xQueueReceive(servo_queue, &cmd, portMAX_DELAY) == pdTRUE) {
//             float current = get_angle(cmd.addr, cmd.channel);
            
//             if (cmd.duration <= 1) {
//                 // 立即设置
//                 for (int d = 0; d < MAX_DEVICES; d++) {
//                     if (devices[d].addr == cmd.addr) {
//                         pca9685_set_angle(devices[d].port, cmd.addr, cmd.channel, cmd.angle);
//                         break;
//                     }
//                 }
//                 update_angle(cmd.addr, cmd.channel, cmd.angle);
//             } else {
//                 // 缓动控制
//                 move_servo_linear(cmd.addr, cmd.channel, current, cmd.angle, cmd.duration);
//             }
//         }
//     }
// }

// void uart_handler_init(uart_port_t uart_num) {
    
//     uart_config_t 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_DISABLE
//     };
//     uart_param_config(uart_num, &config);
//     esp_err_t ret = uart_driver_install(uart_num, UART_BUF_SIZE * 4, 0, 0, NULL, 0);
//     if (ret != ESP_OK) {
//         ESP_LOGE(TAG, "UART driver install failed: 0x%X", ret);
//     } else {
//         ESP_LOGI(TAG, "UART%d driver installed", uart_num);
//     }
    
//     // 创建舵机控制队列
//     servo_queue = xQueueCreate(20, sizeof(ServoCommand));
//     if (servo_queue == NULL) {
//         ESP_LOGE(TAG, "Failed to create servo queue");
//     } else {
//         ESP_LOGI(TAG, "Servo queue created");
//     }
    
//     // 创建舵机控制任务
//     BaseType_t task_ret = xTaskCreate(servo_control_task, "servo_ctrl", 4096, (void*)UART_NUM_0, 5, NULL);
//     if (task_ret != pdPASS) {
//         ESP_LOGE(TAG, "Failed to create servo_control_task");
//     } else {
//         ESP_LOGI(TAG, "servo_control_task created");
//     }
// }

// void uart_handler_task(void *param) {
//     uart_port_t uart_num = (uart_port_t)param;
//     uint8_t buf[UART_BUF_SIZE];

//     while (1) {
//         ESP_LOGI(TAG, "++++++++++++++++++++++++++++++++++++++++++++++++++++++++");
//         int len = uart_read_bytes(uart_num, buf, UART_BUF_SIZE, 0 / portTICK_PERIOD_MS);
//         if (len <= 0) continue;
//         ESP_LOGI("UART", "收到 %d 字节数据", len);
//         ESP_LOG_BUFFER_HEXDUMP("UART DATA", buf, len, ESP_LOG_INFO);

//         ESP_LOGI(TAG, "Received %d bytes", len);
//         esp_log_buffer_hex(TAG, buf, len);

//         // 寻找帧头 0xAA
//         int i = 0;
//         while (i < len && buf[i] != 0xAA) ++i;
//         if (i >= len - 1) continue;
        
//         int remaining = len - i;
//         uint8_t count = buf[i + 1];
//         if (remaining < 2 + count * 5) {
//             ESP_LOGW(TAG, "Incomplete frame, need %d bytes, have %d", 2+count*5, remaining);
//             continue;
//         }

//         // 计算校验和
//         uint8_t calc_checksum = 0;
//         for (int j = 0; j < 2 + count * 5; j++) {
//             calc_checksum ^= buf[i + j];
//         }
//         uint8_t packet_checksum = buf[i + 2 + count * 5]; // 最后一个字节

//         if (calc_checksum != packet_checksum) {
//             ESP_LOGE(TAG, "Checksum error: calc=0x%02X, packet=0x%02X", 
//                     calc_checksum, packet_checksum);
//             continue;
//         }
//         // 解析指令
//         for (int j = 0; j < count; ++j) {
//             int idx = i + 2 + j * 5;
//             ServoCommand cmd = {
//                 .addr     = buf[idx],
//                 .channel  = buf[idx + 1],
//                 .angle    = (float)buf[idx + 2],
//                 .duration = (uint16_t)(buf[idx + 3] | (buf[idx + 4] << 8))
//             };
            
//             // 发送到舵机控制队列
//             if (xQueueSend(servo_queue, &cmd, 10 / portTICK_PERIOD_MS) != pdTRUE) {
//                 ESP_LOGE(TAG, "Failed to send command to queue");
//             }
//         }
//     }
// }



// #include "uart_handler.h"
// #include "pca9685.h"
// #include "driver/uart.h"
// #include "esp_log.h"
// #include "freertos/FreeRTOS.h"
// #include "freertos/task.h"
// #include "freertos/queue.h"
// #include <string.h>
// #include <math.h>

// #define UART_BUF_SIZE 1024
// #define TAG "UART_HANDLER"
// #define MAX_SERVOS 32
// #define MAX_DEVICES 2

// extern PCA9685_Device devices[MAX_DEVICES];
// static ServoAngle current_angles[MAX_SERVOS];
// static int angle_count = 0;
// static QueueHandle_t servo_queue = NULL;

// static float get_angle(uint8_t addr, uint8_t channel) {
//     for(int i = 0; i < angle_count; i++) {
//         if(current_angles[i].addr == addr && current_angles[i].channel == channel) {
//             return current_angles[i].angle;
//         }
//     }
//     if(angle_count < MAX_SERVOS) {
//         current_angles[angle_count] = (ServoAngle){addr, channel, 90.0f};
//         return current_angles[angle_count++].angle;
//     }
//     return 90.0f;
// }

// static void update_angle(uint8_t addr, uint8_t channel, float angle) {
//     for(int i = 0; i < angle_count; i++) {
//         if(current_angles[i].addr == addr && current_angles[i].channel == channel) {
//             current_angles[i].angle = angle;
//             return;
//         }
//     }
//     if(angle_count < MAX_SERVOS) {
//         current_angles[angle_count++] = (ServoAngle){addr, channel, angle};
//     }
// }

// static void move_servo_linear(uint8_t addr, uint8_t channel, float from, float to, uint16_t duration_ms) {
//     int steps = duration_ms / 20;
//     steps = steps < 1 ? 1 : steps;
    
//     for (int i = 0; i <= steps; ++i) {
//         float t = (float)i / steps;
//         float angle = from + (to - from) * t;
        
//         for (int d = 0; d < MAX_DEVICES; d++) {
//             if (devices[d].addr == addr) {
//                 pca9685_set_angle(devices[d].port, addr, channel, angle);
//                 break;
//             }
//         }
//         vTaskDelay(20 / portTICK_PERIOD_MS);
//     }
//     update_angle(addr, channel, to);
// }

// static void servo_control_task(void *param) {
//     ServoCommand cmd;
    
//     while (1) {
//         if (xQueueReceive(servo_queue, &cmd, portMAX_DELAY) == pdTRUE) {
//             float current[MAX_SERVOS];
//             int count = 0;

//             for (int i = 0; i < MAX_SERVOS; ++i) {
//                 if (current_angles[i].addr == cmd.addr) {
//                     current[count++] = current_angles[i].angle;
//                 }
//             }

//             if (count > 1) {
//                 pca9685_set_angles(devices[cmd.addr].port, cmd.addr, 0, count, current);
//             } else {
//                 pca9685_set_angle(devices[cmd.addr].port, cmd.addr, cmd.channel, cmd.angle);
//             }

//             update_angle(cmd.addr, cmd.channel, cmd.angle);
//         }
//     }
// }

// void uart_handler_init(uart_port_t uart_num) {
//     uart_config_t 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_DISABLE
//     };
//     uart_param_config(uart_num, &config);
//     esp_err_t ret = uart_driver_install(uart_num, UART_BUF_SIZE * 4, 0, 0, NULL, 0);
//     if (ret != ESP_OK) {
//         ESP_LOGE(TAG, "UART driver install failed: 0x%X", ret);
//     } else {
//         ESP_LOGI(TAG, "UART%d driver installed", uart_num);
//     }

//     servo_queue = xQueueCreate(20, sizeof(ServoCommand));
//     if (servo_queue == NULL) {
//         ESP_LOGE(TAG, "Failed to create servo queue");
//     } else {
//         ESP_LOGI(TAG, "Servo queue created");
//     }
    
//     BaseType_t task_ret = xTaskCreate(servo_control_task, "servo_ctrl", 4096, (void*)UART_NUM_0, 5, NULL);
//     if (task_ret != pdPASS) {
//         ESP_LOGE(TAG, "Failed to create servo_control_task");
//     } else {
//         ESP_LOGI(TAG, "servo_control_task created");
//     }
// }

// void uart_handler_task(void *param) {
//     uart_port_t uart_num = (uart_port_t)param;
//     uint8_t buf[UART_BUF_SIZE];

//     while (1) {
//         int len = uart_read_bytes(uart_num, buf, UART_BUF_SIZE, 0 / portTICK_PERIOD_MS);
//         if (len <= 0) continue;

//         int i = 0;
//         while (i < len && buf[i] != 0xAA) ++i;
//         if (i >= len - 1) continue;

//         int remaining = len - i;
//         uint8_t count = buf[i + 1];
//         if (remaining < 2 + count * 5) {
//             ESP_LOGW(TAG, "Incomplete frame, need %d bytes, have %d", 2 + count * 5, remaining);
//             continue;
//         }

//         uint8_t calc_checksum = 0;
//         for (int j = 0; j < 2 + count * 5; j++) {
//             calc_checksum ^= buf[i + j];
//         }
//         uint8_t packet_checksum = buf[i + 2 + count * 5];
//         if (calc_checksum != packet_checksum) {
//             ESP_LOGE(TAG, "Checksum error: calc=0x%02X, packet=0x%02X", 
//                     calc_checksum, packet_checksum);
//             continue;
//         }

//         for (int j = 0; j < count; ++j) {
//             int idx = i + 2 + j * 5;
//             ServoCommand cmd = {
//                 .addr     = buf[idx],
//                 .channel  = buf[idx + 1],
//                 .angle    = (float)buf[idx + 2],
//                 .duration = (uint16_t)(buf[idx + 3] | (buf[idx + 4] << 8))
//             };
            
//             if (xQueueSend(servo_queue, &cmd, 10 / portTICK_PERIOD_MS) != pdTRUE) {
//                 ESP_LOGE(TAG, "Failed to send command to queue");
//             }
//         }
//     }
// }
#include "uart_handler.h"
#include "pca9685.h"
#include "driver/uart.h"
#include "esp_log.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/queue.h"
#include <string.h>
#include <math.h>

#define UART_BUF_SIZE 1024
#define TAG "UART_HANDLER"
#define MAX_SERVOS   32
#define MAX_DEVICES  2
#define MAX_CHANNELS 16 // PCA9685 每个设备最多 16 通道

extern PCA9685_Device devices[MAX_DEVICES];
static QueueHandle_t servo_queue = NULL;

// 设备通道角度缓存
static float device_angles[MAX_DEVICES][MAX_CHANNELS];

// 根据 I2C 地址查设备索引
static int find_device_index(uint8_t addr) {
    for (int d = 0; d < MAX_DEVICES; ++d) {
        if (devices[d].addr == addr) return d;
    }
    return -1;
}

// 线性缓动批量更新
static void move_servo_linear(uint8_t addr, uint8_t channel, float from, float to, uint16_t duration_ms) {
    int steps = duration_ms / 20;
    steps = steps < 1 ? 1 : steps;
    int idx = find_device_index(addr);
    if (idx < 0) return;

    for (int i = 0; i <= steps; ++i) {
        float t = (float)i / steps;
        float angle = from + (to - from) * t;
        device_angles[idx][channel] = angle;
        // 一次性批量更新所有通道，保证同步
        pca9685_set_angles(devices[idx].port, addr, 0, MAX_CHANNELS, device_angles[idx]);
        vTaskDelay(20 / portTICK_PERIOD_MS);
    }
}

// 舵机控制任务
static void servo_control_task(void *param) {
    ServoCommand cmd;
    while (1) {
        if (xQueueReceive(servo_queue, &cmd, portMAX_DELAY) == pdTRUE) {
            int idx = find_device_index(cmd.addr);
            if (idx < 0) continue;
            float from = device_angles[idx][cmd.channel];
            float to   = cmd.angle;

            if (cmd.duration <= 1) {
                // 立即批量更新
                device_angles[idx][cmd.channel] = to;
                pca9685_set_angles(devices[idx].port, cmd.addr, 0, MAX_CHANNELS, device_angles[idx]);
            } else {
                // 缓动控制
                move_servo_linear(cmd.addr, cmd.channel, from, to, cmd.duration);
                // 最终角度已在 move_servo_linear 内更新
            }
        }
    }
}

void uart_handler_init(uart_port_t uart_num) {
    uart_config_t 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_DISABLE
    };
    uart_param_config(uart_num, &config);
    esp_err_t ret = uart_driver_install(uart_num, UART_BUF_SIZE * 4, 0, 0, NULL, 0);
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "UART driver install failed: 0x%X", ret);
    } else {
        ESP_LOGI(TAG, "UART%d driver installed", uart_num);
    }

    // 初始化角度缓存为 90°
    for (int d = 0; d < MAX_DEVICES; ++d) {
        for (int c = 0; c < MAX_CHANNELS; ++c) {
            device_angles[d][c] = 90.0f;
        }
    }

    // 创建舵机控制队列
    servo_queue = xQueueCreate(20, sizeof(ServoCommand));
    if (!servo_queue) {
        ESP_LOGE(TAG, "Failed to create servo queue");
        return;
    }
    ESP_LOGI(TAG, "Servo queue created");

    // 启动舵机控制任务
    BaseType_t task_ret = xTaskCreate(servo_control_task, "servo_ctrl", 4096,
                                      NULL, configMAX_PRIORITIES - 1, NULL);
    if (task_ret != pdPASS) {
        ESP_LOGE(TAG, "Failed to create servo_control_task");
    } else {
        ESP_LOGI(TAG, "servo_control_task created");
    }
}

void uart_handler_task(void *param) {
    uart_port_t uart_num = (uart_port_t)param;
    uint8_t buf[UART_BUF_SIZE];

    while (1) {
        int len = uart_read_bytes(uart_num, buf, UART_BUF_SIZE, 10 / portTICK_PERIOD_MS);
        if (len <= 0) continue;

        // 找到帧头 0xAA
        int i = 0;
        while (i < len && buf[i] != 0xAA) ++i;
        if (i >= len - 1) continue;

        uint8_t count = buf[i + 1];
        int frame_len = 2 + count * 5 + 1;
        if (i + frame_len > len) {
            ESP_LOGW(TAG, "不完整帧，需要 %d 字节，实际 %d", frame_len, len - i);
            continue;
        }

        // 校验
        uint8_t chk = 0;
        for (int j = 0; j < frame_len - 1; ++j) chk ^= buf[i + j];
        if (chk != buf[i + frame_len - 1]) {
            ESP_LOGE(TAG, "校验失败，calc=0x%02X, packet=0x%02X", chk, buf[i + frame_len - 1]);
            continue;
        }

        // 解析并发送命令
        for (int j = 0; j < count; ++j) {
            int idx = i + 2 + j * 5;
            ServoCommand cmd = {
                .addr     = buf[idx],
                .channel  = buf[idx + 1],
                .angle    = (float)buf[idx + 2],
                .duration = (uint16_t)(buf[idx + 3] | (buf[idx + 4] << 8))
            };
            if (xQueueSend(servo_queue, &cmd, 10 / portTICK_PERIOD_MS) != pdTRUE) {
                ESP_LOGE(TAG, "发送命令到队列失败");
            }
        }
    }
}