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

// 第一组 I2C (I2C0)
#define I2C_PORT0 I2C_NUM_0
#define SDA_IO0 21
#define SCL_IO0 22
#define PCA_ADDR1 0x41

// 第二组 I2C (I2C1)
#define I2C_PORT1 I2C_NUM_1
#define SDA_IO1 32
#define SCL_IO1 33
#define PCA_ADDR2 0x40

// 设备列表 (在uart_handler.c中使用)
PCA9685_Device devices[2] = {
    {I2C_PORT0, PCA_ADDR1},  // 设备1
    {I2C_PORT1, PCA_ADDR2}   // 设备2
};

void app_main(void) {
    // 初始化 I2C0
    i2c_config_t conf0 = {
        .mode = I2C_MODE_MASTER,
        .sda_io_num = SDA_IO0,
        .scl_io_num = SCL_IO0,
        .sda_pullup_en = GPIO_PULLUP_ENABLE,
        .scl_pullup_en = GPIO_PULLUP_ENABLE,
        .master.clk_speed = 400000
    };
    ESP_ERROR_CHECK(i2c_param_config(I2C_PORT0, &conf0));
    ESP_ERROR_CHECK(i2c_driver_install(I2C_PORT0, conf0.mode, 0, 0, 0));
    
    // 初始化 I2C1
    i2c_config_t conf1 = {
        .mode = I2C_MODE_MASTER,
        .sda_io_num = SDA_IO1,
        .scl_io_num = SCL_IO1,
        .sda_pullup_en = GPIO_PULLUP_ENABLE,
        .scl_pullup_en = GPIO_PULLUP_ENABLE,
        .master.clk_speed = 400000
    };
    ESP_ERROR_CHECK(i2c_param_config(I2C_PORT1, &conf1));
    ESP_ERROR_CHECK(i2c_driver_install(I2C_PORT1, conf1.mode, 0, 0, 0));

    // 初始化两个 PCA9685

    pca9685_init(I2C_PORT0, PCA_ADDR1);
    vTaskDelay(10 / portTICK_PERIOD_MS); // 增加延迟
    pca9685_set_pwm_freq(I2C_PORT0, PCA_ADDR1, 50);
    

    pca9685_init(I2C_PORT1, PCA_ADDR2);
    vTaskDelay(10 / portTICK_PERIOD_MS); // 增加延迟
    pca9685_set_pwm_freq(I2C_PORT1, PCA_ADDR2, 50);
    
    // 初始化舵机到安全位置
    for (int ch = 0; ch < 16; ++ch) {
        pca9685_set_angle(I2C_PORT0, PCA_ADDR1, ch, 90);
        pca9685_set_angle(I2C_PORT1, PCA_ADDR2, ch, 90);
    }
    
    // 初始化UART

    uart_handler_init(UART_NUM_0);
    uart_flush_input(UART_NUM_0); // 清空串口接收缓冲，防止伪数据干扰
    vTaskDelay(10 / portTICK_PERIOD_MS);

    BaseType_t ret = xTaskCreate(uart_handler_task, "uart_task", 4096, (void*)UART_NUM_0, 10, NULL);
    if (ret != pdPASS) {
        ESP_LOGE("MAIN", "Failed to create uart_task! Error: %d", ret);
    } else {
        ESP_LOGI("MAIN", "uart_task created successfully");
    }

    // 主循环只保留延迟
    while (1) {
        vTaskDelay(0 / portTICK_PERIOD_MS);
    }
}