#include "pca9685.h"
#include "esp_log.h"
#include "esp_err.h"
#include <math.h>

static const char *TAG = "PCA9685";

static esp_err_t write8(i2c_port_t i2c_num, uint8_t addr, uint8_t reg, uint8_t val) {
    i2c_cmd_handle_t cmd = i2c_cmd_link_create();
    i2c_master_start(cmd);
    i2c_master_write_byte(cmd, (addr << 1) | I2C_MASTER_WRITE, true);
    i2c_master_write_byte(cmd, reg, true);
    i2c_master_write_byte(cmd, val, true);
    i2c_master_stop(cmd);
    esp_err_t ret = i2c_master_cmd_begin(i2c_num, cmd, 1000 / portTICK_PERIOD_MS);
    i2c_cmd_link_delete(cmd);
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "write8 failed: reg=0x%02X val=0x%02X, err=0x%X", reg, val, ret);
    }
    return ret;
}

static uint8_t read8(i2c_port_t i2c_num, uint8_t addr, uint8_t reg) {
    uint8_t val = 0;
    i2c_cmd_handle_t cmd = i2c_cmd_link_create();
    
    // 写入寄存器地址
    i2c_master_start(cmd);
    i2c_master_write_byte(cmd, (addr << 1) | I2C_MASTER_WRITE, true);
    i2c_master_write_byte(cmd, reg, true);
    
    // 重复START条件
    i2c_master_start(cmd);
    i2c_master_write_byte(cmd, (addr << 1) | I2C_MASTER_READ, true);
    
    // 读取数据 (NACK最后一个字节)
    i2c_master_read_byte(cmd, &val, I2C_MASTER_LAST_NACK);
    i2c_master_stop(cmd);
    
    esp_err_t ret = i2c_master_cmd_begin(i2c_num, cmd, 1000 / portTICK_PERIOD_MS);
    i2c_cmd_link_delete(cmd);
    
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "read8 failed: reg=0x%02X, err=0x%X", reg, ret);
    }
    return val;
}

static esp_err_t set_pwm(i2c_port_t i2c_num, uint8_t addr, uint8_t channel, uint16_t on, uint16_t off) {
    i2c_cmd_handle_t cmd = i2c_cmd_link_create();
    i2c_master_start(cmd);
    i2c_master_write_byte(cmd, (addr << 1) | I2C_MASTER_WRITE, true);
    i2c_master_write_byte(cmd, LED0_ON_L + 4 * channel, true);
    i2c_master_write_byte(cmd, on & 0xFF, true);
    i2c_master_write_byte(cmd, on >> 8, true);
    i2c_master_write_byte(cmd, off & 0xFF, true);
    
    i2c_master_write_byte(cmd, off >> 8, true);
    i2c_master_stop(cmd);
    esp_err_t ret = i2c_master_cmd_begin(i2c_num, cmd, 1000 / portTICK_PERIOD_MS);
    i2c_cmd_link_delete(cmd);
    
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "set_pwm failed: ch=%d, err=0x%X", channel, ret);
    }
    return ret;
}

void pca9685_init(i2c_port_t i2c_num, uint8_t addr) {
    // 完整初始化序列
    write8(i2c_num, addr, MODE1, MODE1_SLEEP | MODE1_ALLCALL);
    vTaskDelay(50 / portTICK_PERIOD_MS);  // 延长延迟
    
    // 唤醒设备并启用自动增量
    write8(i2c_num, addr, MODE1, MODE1_AI | MODE1_ALLCALL);
    vTaskDelay(10 / portTICK_PERIOD_MS);
    
    // 关闭所有LED输出
    write8(i2c_num, addr, ALL_LED_OFF_H, 0x10);  // 设置所有LED_OFF_H为0x10
    
    ESP_LOGI(TAG, "PCA9685 at 0x%02X initialized", addr);
}

void pca9685_set_pwm_freq(i2c_port_t i2c_num, uint8_t addr, float freq) {
    //待优化
    // 计算预分频值 (25MHz时钟)
    float prescaleval = 25000000.0;
    prescaleval /= 4096.0;    // 12位分辨率
    prescaleval /= freq;
    prescaleval -= 1.0;
    uint8_t prescale = (uint8_t)(prescaleval + 0.5);
    
    ESP_LOGI(TAG, "Setting PWM freq: %.1f Hz, prescale: %d", freq, prescale);

    // 进入睡眠模式设置预分频
    uint8_t oldmode = read8(i2c_num, addr, MODE1);
    uint8_t sleepmode = (oldmode & ~MODE1_RESTART) | MODE1_SLEEP;
    write8(i2c_num, addr, MODE1, sleepmode);
    vTaskDelay(5 / portTICK_PERIOD_MS);
    
    // 设置预分频寄存器
    write8(i2c_num, addr, PRESCALE, prescale);
    
    // 恢复模式并唤醒
    write8(i2c_num, addr, MODE1, oldmode);
    vTaskDelay(5 / portTICK_PERIOD_MS);
    
    // 触发重启
    write8(i2c_num, addr, MODE1, oldmode | MODE1_RESTART);
    vTaskDelay(10 / portTICK_PERIOD_MS);
}

void pca9685_set_angle(i2c_port_t i2c_num, uint8_t addr, uint8_t channel, float angle) {
    // 角度限幅
    angle = (angle < 0) ? 0 : (angle > 180) ? 180 : angle;
    
    // 计算脉冲宽度 (0.5ms - 2.5ms)
    float pulse_width_ms = 0.5f + (angle / 180.0f) * 2.0f;
    
    // 计算PWM值 (20ms周期)
    uint16_t pwm_value = (uint16_t)((pulse_width_ms / 20.0f) * 4096.0f);
    
    // 设置PWM (ON=0, OFF=pwm_value)
    esp_err_t ret = set_pwm(i2c_num, addr, channel, 0, pwm_value);
    
    if (ret == ESP_OK) {
        ESP_LOGI(TAG, "Set addr=0x%02X ch=%d: %d° -> PWM=%d", 
                addr, channel, (int)angle, pwm_value);
    } else {
        ESP_LOGE(TAG, "Failed to set angle on addr=0x%02X ch=%d", addr, channel);
    }
}

// 一次性更新多个通道的示例（在 pca9685.c 新增）
void pca9685_set_angles(i2c_port_t i2c_num, uint8_t addr,
                        uint8_t start_ch, uint8_t count,
                        const float *angles) {
    // 准备一大块 data：每通道 4 字节 ON_L,ON_H,OFF_L,OFF_H
    uint8_t data[4 * count];
    for (int i = 0; i < count; i++) {
        float a = angles[i];
        a = a < 0 ? 0 : a > 180 ? 180 : a;
        float pw = 0.5f + (a/180.0f)*2.0f;  // ms
        uint16_t pwm = (uint16_t)((pw/20.0f)*4096);
        data[4*i + 0] = 0;         // ON_L
        data[4*i + 1] = 0;         // ON_H
        data[4*i + 2] = pwm & 0xFF;// OFF_L
        data[4*i + 3] = pwm >> 8;  // OFF_H
    }

    i2c_cmd_handle_t cmd = i2c_cmd_link_create();
    i2c_master_start(cmd);
    // 写入寄存器地址，LED0_ON_L + 4*start_ch，自动递增模式
    i2c_master_write_byte(cmd, (addr<<1)|I2C_MASTER_WRITE, true);
    i2c_master_write_byte(cmd, LED0_ON_L + 4*start_ch, true);
    // 一次性写出所有 data
    i2c_master_write(cmd, data, sizeof(data), true);
    i2c_master_stop(cmd);
    i2c_master_cmd_begin(i2c_num, cmd, 1000/portTICK_PERIOD_MS);
    i2c_cmd_link_delete(cmd);
}